tests use number_t

.travis.yml

@@ -37,7 +37,7 @@ before_script:
 
 script:
   - make
-  - ctest
+  - ctest --extra-verbose
 
 # right now only build the master branch
 branches:

g2o/core/eigen_types.h

@@ -63,10 +63,12 @@ namespace g2o {
   typedef Eigen::Matrix<float,4,4,Eigen::ColMajor>                                Matrix4F;
   typedef Eigen::Matrix<float,Eigen::Dynamic,Eigen::Dynamic,Eigen::ColMajor>      MatrixXF;
 
-  typedef Eigen::Matrix<number_t,2,2,Eigen::ColMajor>                             Matrix2;
-  typedef Eigen::Matrix<number_t,3,3,Eigen::ColMajor>                             Matrix3;
-  typedef Eigen::Matrix<number_t,4,4,Eigen::ColMajor>                             Matrix4;
-  typedef Eigen::Matrix<number_t,Eigen::Dynamic,Eigen::Dynamic,Eigen::ColMajor>   MatrixX;
+  template<int N>
+  using MatrixN = Eigen::Matrix<number_t, N, N, Eigen::ColMajor>;
+  using Matrix2 = MatrixN<2>;
+  using Matrix3 = MatrixN<3>;
+  using Matrix4 = MatrixN<4>;
+  using MatrixX = MatrixN<Eigen::Dynamic>;
 
   typedef Eigen::Transform<number_t,2,Eigen::Isometry,Eigen::ColMajor>            Isometry2;
   typedef Eigen::Transform<number_t,3,Eigen::Isometry,Eigen::ColMajor>            Isometry3;

unit_test/general/CMakeLists.txt

@@ -1,5 +1,4 @@
 add_executable(unittest_general
-  ../test_helper/test_main.cpp
   graph_operations.cpp
   clear_and_redo.cpp
 )

unit_test/general/clear_and_redo.cpp

@@ -52,40 +52,40 @@ TEST(General, ClearAndRedo)
   {
     // Add vertices
     g2o::VertexSE3* v0 = new g2o::VertexSE3;
-    v0->setEstimate(Eigen::Transform<double,3,1>(Eigen::Translation<double, 3>(0,0,0)));
+    v0->setEstimate(Eigen::Transform<number_t,3,1>(Eigen::Translation<number_t, 3>(0,0,0)));
     v0->setId(0);
     mOptimizer.addVertex(v0);
 
     g2o::VertexSE3* v1 = new g2o::VertexSE3;
-    v1->setEstimate(Eigen::Transform<double,3,1>(Eigen::Translation<double, 3>(0,0,0)));
+    v1->setEstimate(Eigen::Transform<number_t,3,1>(Eigen::Translation<number_t, 3>(0,0,0)));
     v1->setId(1);
     mOptimizer.addVertex(v1);
 
     g2o::VertexSE3* v2 = new g2o::VertexSE3;
-    v2->setEstimate(Eigen::Transform<double,3,1>(Eigen::Translation<double, 3>(0,0,0)));
+    v2->setEstimate(Eigen::Transform<number_t,3,1>(Eigen::Translation<number_t, 3>(0,0,0)));
     v2->setId(2);
     mOptimizer.addVertex(v2);
 
     // Add edges
     g2o::EdgeSE3* e1 = new g2o::EdgeSE3();
     e1->vertices()[0] = mOptimizer.vertex(0);
     e1->vertices()[1] = mOptimizer.vertex(1);
-    e1->setMeasurement(Eigen::Isometry3d(Eigen::Translation<double, 3>(1,0,0)));
-    e1->setInformation(Eigen::Matrix<double,6,6>::Identity());
+    e1->setMeasurement(g2o::Isometry3(Eigen::Translation<number_t, 3>(1,0,0)));
+    e1->setInformation(g2o::MatrixN<6>::Identity());
     mOptimizer.addEdge(e1);
 
     g2o::EdgeSE3* e2 = new g2o::EdgeSE3();
     e2->vertices()[0] = mOptimizer.vertex(1);
     e2->vertices()[1] = mOptimizer.vertex(2);
-    e2->setMeasurement(Eigen::Isometry3d(Eigen::Translation<double, 3>(0,1,0)));
-    e2->setInformation(Eigen::Matrix<double,6,6>::Identity());
+    e2->setMeasurement(g2o::Isometry3(Eigen::Translation<number_t, 3>(0,1,0)));
+    e2->setInformation(g2o::MatrixN<6>::Identity());
     mOptimizer.addEdge(e2);
 
     g2o::EdgeSE3* e3 = new g2o::EdgeSE3();
     e3->vertices()[0] = mOptimizer.vertex(2);
     e3->vertices()[1] = mOptimizer.vertex(0);
-    e3->setMeasurement(Eigen::Isometry3d(Eigen::Translation<double, 3>(-0.8, -0.7, 0.1)));
-    e3->setInformation(Eigen::Matrix<double,6,6>::Identity());
+    e3->setMeasurement(g2o::Isometry3(Eigen::Translation<number_t, 3>(-0.8, -0.7, 0.1)));
+    e3->setInformation(g2o::MatrixN<6>::Identity());
     mOptimizer.addEdge(e3);
 
     v0->setFixed(true);

unit_test/slam2d/CMakeLists.txt

@@ -1,5 +1,4 @@
 add_executable(unittest_slam2d
-  ../test_helper/test_main.cpp
   mappings_se2.cpp
   jacobians_slam2d.cpp
   odom_convert_sclam2d.cpp

unit_test/slam2d/mappings_se2.cpp

@@ -33,8 +33,24 @@ using namespace g2o;
 
 TEST(MappingsSlam2D, SE2)
 {
-  SE2 s1(0., 1., 2.);
+  // constructor from 3 values
+  SE2 s1(cst(0.), cst(1.), cst(2.));
   EXPECT_DOUBLE_EQ(0., s1[0]);
   EXPECT_DOUBLE_EQ(1., s1[1]);
   EXPECT_DOUBLE_EQ(2., s1[2]);
+
+  // constructor from vector
+  Vector3 vec2 = Vector3(cst(0.1), cst(0.2), cst(0.3));
+  SE2 s2(vec2);
+  EXPECT_DOUBLE_EQ(0.1, s2[0]);
+  EXPECT_DOUBLE_EQ(0.2, s2[1]);
+  EXPECT_DOUBLE_EQ(0.3, s2[2]);
+  EXPECT_DOUBLE_EQ(0., (vec2.head<2>() - s2.translation()).norm());
+
+  // constructor from Isometry
+  Rotation2D rot3 = Rotation2D(cst(1.));
+  SE2 s3 = SE2(Isometry2(rot3));
+  EXPECT_DOUBLE_EQ(0., s3.translation()(0));
+  EXPECT_DOUBLE_EQ(0., s3.translation()(1));
+  EXPECT_DOUBLE_EQ(0.1, s3.rotation().angle());
 }

unit_test/slam3d/CMakeLists.txt

@@ -1,5 +1,4 @@
 add_executable(unittest_slam3d
-  ../test_helper/test_main.cpp
   jacobians_slam3d.cpp
   mappings_slam3d.cpp
   orthogonal_matrix.cpp

unit_test/slam3d/mappings_slam3d.cpp

@@ -33,72 +33,71 @@ using namespace std;
 
 TEST(MappingsSlam3D, EulerConversion)
 {
-  Eigen::Vector3d eulerAngles(.1,.2,.3);
-  Eigen::Matrix3d m1 = g2o::internal::fromEuler(eulerAngles);
-  Eigen::Vector3d eulerAnglesFromMatrix = g2o::internal::toEuler(m1);
+  g2o::Vector3 eulerAngles(.1,.2,.3);
+  g2o::Matrix3 m1 = g2o::internal::fromEuler(eulerAngles);
+  g2o::Vector3 eulerAnglesFromMatrix = g2o::internal::toEuler(m1);
   for (int i = 0; i < 3; ++i)
     EXPECT_DOUBLE_EQ(eulerAngles(i), eulerAnglesFromMatrix(i));
 }
 
 TEST(MappingsSlam3D, QuaternionConversion)
 {
-  Eigen::Vector3d eulerAngles(.1,.2,.3);
-  Eigen::Matrix3d m1 = g2o::internal::fromEuler(eulerAngles);
-
-  Eigen::Vector3d q = g2o::internal::toCompactQuaternion(m1);
-  Eigen::Matrix3d m2 = g2o::internal::fromCompactQuaternion(q);
+  g2o::Vector3 eulerAngles(.1,.2,.3);
+  g2o::Matrix3 m1 = g2o::internal::fromEuler(eulerAngles);
+  g2o::Vector3 q = g2o::internal::toCompactQuaternion(m1);
+  g2o::Matrix3 m2 = g2o::internal::fromCompactQuaternion(q);
   for (int r = 0; r < 3; ++r)
     for (int c = 0; c < 3; ++c)
       EXPECT_DOUBLE_EQ(m1(r,c), m2(r,c));
 }
 
 TEST(MappingsSlam3D, ET)
 {
-  Eigen::Vector3d eulerAngles(.1,.2,.3);
-  Eigen::Matrix<double,6,1> et;
-  Eigen::Vector3d t(1.,2.,3.);
+  g2o::Vector3 eulerAngles(.1,.2,.3);
+  g2o::Vector6 et;
+  g2o::Vector3 t(1.,2.,3.);
   et.block<3,1>(0,0) = t;
   et.block<3,1>(3,0) = eulerAngles;
-  Eigen::Matrix3d m1 = g2o::internal::fromEuler(eulerAngles);
+  g2o::Matrix3 m1 = g2o::internal::fromEuler(eulerAngles);
 
-  Eigen::Isometry3d i1 = g2o::internal::fromVectorET(et);
+  g2o::Isometry3 i1 = g2o::internal::fromVectorET(et);
   for (int r = 0; r < 3; ++r)
     EXPECT_EQ(t(r), i1.translation()(r));
 
   EXPECT_NEAR(0., (i1.linear() - m1).array().abs().maxCoeff(), 1e-6);
 
-  Eigen::Matrix<double,6,1> et2 = g2o::internal::toVectorET(i1);
+  g2o::Vector6 et2 = g2o::internal::toVectorET(i1);
   EXPECT_NEAR(0., (et - et2).array().abs().maxCoeff(), 1e-6);
 }
 
 TEST(MappingsSlam3D, MQT)
 {
-  Eigen::Vector3d eulerAngles(.1,.2,.3);
-  Eigen::Matrix<double,6,1> et;
-  Eigen::Vector3d t(1.,2.,3.);
+  g2o::Vector3 eulerAngles(.1,.2,.3);
+  g2o::Vector6 et;
+  g2o::Vector3 t(1.,2.,3.);
   et.block<3,1>(0,0) = t;
   et.block<3,1>(3,0) = eulerAngles;
-  Eigen::Isometry3d i1 = g2o::internal::fromVectorET(et);
+  g2o::Isometry3 i1 = g2o::internal::fromVectorET(et);
 
-  Eigen::Matrix<double,6,1> qt1 = g2o::internal::toVectorMQT(i1);
+  g2o::Vector6 qt1 = g2o::internal::toVectorMQT(i1);
 
-  Eigen::Isometry3d i2 = g2o::internal::fromVectorMQT(qt1);
+  g2o::Isometry3 i2 = g2o::internal::fromVectorMQT(qt1);
   EXPECT_NEAR(0., (i1.linear() - i2.linear()).array().abs().maxCoeff(), 1e-6);
   EXPECT_NEAR(0., (i1.translation() - i2.translation()).array().abs().maxCoeff(), 1e-6);
 }
 
 TEST(MappingsSlam3D, QT)
 {
-  Eigen::Vector3d eulerAngles(.1,.2,.3);
-  Eigen::Matrix<double,6,1> et;
-  Eigen::Vector3d t(1.,2.,3.);
+  g2o::Vector3 eulerAngles(.1,.2,.3);
+  g2o::Vector6 et;
+  g2o::Vector3 t(1.,2.,3.);
   et.block<3,1>(0,0) = t;
   et.block<3,1>(3,0) = eulerAngles;
-  Eigen::Isometry3d i1 = g2o::internal::fromVectorET(et);
+  g2o::Isometry3 i1 = g2o::internal::fromVectorET(et);
 
-  Eigen::Matrix<double,7,1> qt2 = g2o::internal::toVectorQT(i1);
+  g2o::Vector7 qt2 = g2o::internal::toVectorQT(i1);
 
-  Eigen::Isometry3d i2 = g2o::internal::fromVectorQT(qt2);
+  g2o::Isometry3 i2 = g2o::internal::fromVectorQT(qt2);
   EXPECT_NEAR(0., (i1.linear() - i2.linear()).array().abs().maxCoeff(), 1e-6);
   EXPECT_NEAR(0., (i1.translation() - i2.translation()).array().abs().maxCoeff(), 1e-6);
 }

unit_test/slam3d/orthogonal_matrix.cpp

@@ -32,39 +32,39 @@ using namespace std;
 
 TEST(Slam3D, OrthogonalMatrix)
 {
-  Eigen::Matrix3d shouldBeIdentity;
-  Eigen::Matrix3d R = Eigen::Matrix3d::Identity();
-  Eigen::Matrix3d rot = (Eigen::Matrix3d)Eigen::AngleAxisd(0.01, Eigen::Vector3d::UnitZ());
-  rot = rot * (Eigen::Matrix3d)Eigen::AngleAxisd(0.01, Eigen::Vector3d::UnitX());
+  g2o::Matrix3 shouldBeIdentity;
+  g2o::Matrix3 R = g2o::Matrix3::Identity();
+  g2o::Matrix3 rot = (g2o::Matrix3)g2o::AngleAxis(0.01, g2o::Vector3::UnitZ());
+  rot = rot * (g2o::Matrix3)g2o::AngleAxis(0.01, g2o::Vector3::UnitX());
 
   shouldBeIdentity = R * R.transpose();
-  double initialDifference = (shouldBeIdentity - Eigen::Matrix3d::Identity()).array().abs().maxCoeff();
+  number_t initialDifference = (shouldBeIdentity - g2o::Matrix3::Identity()).array().abs().maxCoeff();
   EXPECT_DOUBLE_EQ(0., initialDifference);
 
   //introduce numerical inaccuracies
   for (int i = 0; i < 10000; ++i)
     R = R * rot;
   shouldBeIdentity = R * R.transpose();
-  double afterMultDifference = (shouldBeIdentity - Eigen::Matrix3d::Identity()).array().abs().maxCoeff();
+  number_t afterMultDifference = (shouldBeIdentity - g2o::Matrix3::Identity()).array().abs().maxCoeff();
   EXPECT_GE(afterMultDifference, initialDifference);
 
-  double inaccurateDet = R.determinant();
-  Eigen::Matrix3d approxSolution = R;
+  number_t inaccurateDet = R.determinant();
+  g2o::Matrix3 approxSolution = R;
   g2o::internal::approximateNearestOrthogonalMatrix(approxSolution);
   g2o::internal::nearestOrthogonalMatrix(R);
 
   EXPECT_LE(std::abs(R.determinant() - 1.), std::abs(inaccurateDet - 1.));
   EXPECT_NEAR(1.0, R.determinant(), 1e-7);
   shouldBeIdentity = R * R.transpose();
-  double nearestDifference = (shouldBeIdentity - Eigen::Matrix3d::Identity()).array().abs().maxCoeff();
+  number_t nearestDifference = (shouldBeIdentity - g2o::Matrix3::Identity()).array().abs().maxCoeff();
   EXPECT_NEAR(0., nearestDifference, 1e-7);
 
   // norm of the comluns
   for (int i = 0; i < 3; ++i)
     EXPECT_NEAR(1.0, R.col(i).norm(), 1e-7);
 
   shouldBeIdentity = approxSolution * approxSolution.transpose();
-  double approxNearestDifference = (shouldBeIdentity - Eigen::Matrix3d::Identity()).array().abs().maxCoeff();
+  number_t approxNearestDifference = (shouldBeIdentity - Eigen::Matrix3d::Identity()).array().abs().maxCoeff();
   EXPECT_NEAR(0., approxNearestDifference, 1e-6);
   EXPECT_LE(std::abs(R.determinant() - 1.), std::abs(approxSolution.determinant() - 1.));
   EXPECT_NEAR(1.0, approxSolution.determinant(), 1e-6);

unit_test/test_helper/evaluate_jacobian.h

@@ -50,8 +50,8 @@ void evaluateJacobianUnary(EdgeType& e, JacobianWorkspace& jacobianWorkspace, Ja
     typename EdgeType::VertexXiType>::linearizeOplus();
 
   // compare the Jacobians
-  double* n = numericJacobianWorkspace.workspaceForVertex(0);
-  double* a = jacobianWorkspace.workspaceForVertex(0);
+  number_t* n = numericJacobianWorkspace.workspaceForVertex(0);
+  number_t* a = jacobianWorkspace.workspaceForVertex(0);
   int numElems = EdgeType::Dimension;
   numElems *= EdgeType::VertexXiType::Dimension;
   for (int j = 0; j < numElems; ++j) {
@@ -74,8 +74,8 @@ void evaluateJacobian(EdgeType& e, JacobianWorkspace& jacobianWorkspace, Jacobia
 
     // compare the two Jacobians
     for (int i = 0; i < 2; ++i) {
-      double* n = numericJacobianWorkspace.workspaceForVertex(i);
-      double* a = jacobianWorkspace.workspaceForVertex(i);
+      number_t* n = numericJacobianWorkspace.workspaceForVertex(i);
+      number_t* a = jacobianWorkspace.workspaceForVertex(i);
       int numElems = EdgeType::Dimension;
       if (i == 0)
         numElems *= EdgeType::VertexXiType::Dimension;