1 // g2o - General Graph Optimization
2 // Copyright (C) 2020 R. Kuemmerle, G. Grisetti, W. Burgard
3 // All rights reserved.
4 //
5 // Redistribution and use in source and binary forms, with or without
6 // modification, are permitted provided that the following conditions are
7 // met:
8 //
9 // * Redistributions of source code must retain the above copyright notice,
10 // this list of conditions and the following disclaimer.
11 // * Redistributions in binary form must reproduce the above copyright
12 // notice, this list of conditions and the following disclaimer in the
13 // documentation and/or other materials provided with the distribution.
14 //
15 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
16 // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
17 // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
18 // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
19 // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
20 // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
21 // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
22 // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
23 // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
24 // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26
27 #include <sstream>
28
29 #include "g2o/types/slam3d_addons/edge_plane.h"
30 #include "g2o/types/slam3d_addons/edge_se3_calib.h"
31 #include "g2o/types/slam3d_addons/edge_se3_line.h"
32 #include "g2o/types/slam3d_addons/edge_se3_plane_calib.h"
33 #include "g2o/types/slam3d_addons/vertex_line3d.h"
34 #include "g2o/types/slam3d_addons/vertex_plane.h"
35 #include "g2o/types/slam3d_addons/vertex_se3_euler.h"
36 #include "gtest/gtest.h"
37 #include "unit_test/test_helper/io.h"
38 #include "unit_test/test_helper/random_state.h"
39
40 using namespace std;
41 using namespace g2o;
42
43 struct RandomPlane3D {
createRandomPlane3D44 static Plane3D create() { return Plane3D(Vector4::Random()); }
isApproxRandomPlane3D45 static bool isApprox(const Plane3D& a, const Plane3D& b) { return a.coeffs().isApprox(b.coeffs(), 1e-5); }
46 };
47
48 struct RandomLine3D {
createRandomLine3D49 static Line3D create() { return Line3D(Vector6::Random()); }
isApproxRandomLine3D50 static bool isApprox(const Line3D& a, const Line3D& b) { return a.isApprox(b, 1e-5); }
51 };
52
53 class IoSlam3dAddonsParam : public ::testing::Test {
54 protected:
SetUp()55 void SetUp() override {
56 graph.reset(new g2o::OptimizableGraph);
57
58 // setting up parameters for tests
59 paramOffset = new ParameterSE3Offset();
60 paramOffset->setId(63);
61 graph->addParameter(paramOffset);
62
63 // setting up some vertices
64 int numVertices = 0;
65 pose = new VertexSE3;
66 pose->setId(numVertices++);
67 graph->addVertex(pose);
68 line = new VertexLine3D;
69 line->setId(numVertices++);
70 graph->addVertex(line);
71 }
72
73 std::shared_ptr<g2o::OptimizableGraph> graph;
74 ParameterSE3Offset* paramOffset = nullptr;
75 VertexSE3* pose = nullptr;
76 VertexLine3D* line = nullptr;
77 };
78
TEST(IoSlam3dAddOns,ReadWriteVertexSE3Euler)79 TEST(IoSlam3dAddOns, ReadWriteVertexSE3Euler) {
80 readWriteVectorBasedVertex<VertexSE3Euler, internal::RandomIsometry3>();
81 }
82
TEST(IoSlam3dAddOns,ReadWriteVertexPlane)83 TEST(IoSlam3dAddOns, ReadWriteVertexPlane) {
84 // setting up the vertex for output
85 VertexPlane* outputVertex = new VertexPlane();
86 outputVertex->color = Vector3::Random();
87 // IO Test
88 readWriteVectorBasedVertex<VertexPlane, RandomPlane3D>(outputVertex);
89 }
90
TEST(IoSlam3dAddOns,ReadWriteVertexLine3D)91 TEST(IoSlam3dAddOns, ReadWriteVertexLine3D) { readWriteVectorBasedVertex<VertexLine3D, RandomLine3D>(); }
92
TEST(IoSlam3dAddOns,ReadWriteEdgeSE3Calib)93 TEST(IoSlam3dAddOns, ReadWriteEdgeSE3Calib) { readWriteVectorBasedEdge<EdgeSE3Calib, internal::RandomIsometry3>(); }
94
TEST_F(IoSlam3dAddonsParam,ReadWriteEdgeSE3Line3D)95 TEST_F(IoSlam3dAddonsParam, ReadWriteEdgeSE3Line3D) {
96 // prepare edge
97 EdgeSE3Line3D* outputEdge = new EdgeSE3Line3D();
98 outputEdge->setParameterId(0, paramOffset->id());
99 outputEdge->setVertex(0, pose);
100 outputEdge->setVertex(1, line);
101 ASSERT_TRUE(graph->addEdge(outputEdge));
102 // test IO
103 readWriteVectorBasedEdge<EdgeSE3Line3D, RandomLine3D>(outputEdge);
104 }
105
TEST(IoSlam3dAddOns,ReadWriteEdgeSE3PlaneSensorCalib)106 TEST(IoSlam3dAddOns, ReadWriteEdgeSE3PlaneSensorCalib) { readWriteVectorBasedEdge<EdgeSE3PlaneSensorCalib, RandomPlane3D>(); }
107