Merge pull request #919 from RainerKuemmerle/pymem-merge

Pymem merge

Author: RainerKuemmerle

g2o/types/icp/edge_gicp.cpp

@@ -28,8 +28,44 @@
 
 #include <Eigen/Geometry>
 
+#include "g2o/core/eigen_types.h"
+
 namespace g2o {
 
+namespace {
+constexpr double kYSquaredBnd{0.1};
+
+void makeRot(Matrix3& R, const Vector3& normal) {
+  Vector3 y(0., 1., 0.);
+  R.row(2) = normal;
+  y = y - normal(1) * normal;
+  const double ysquarednorm = y.squaredNorm();
+  if (ysquarednorm >= kYSquaredBnd) {
+    y /= std::sqrt(ysquarednorm);
+    R.row(1) = y;
+    R.row(0) = normal.cross(R.row(1));
+  } else {
+    Vector3 x(-1., 0., 0.);
+    x = x + normal(0) * normal;
+    x.normalize();
+    R.row(0) = x;
+    R.row(1) = -normal.cross(R.row(0));
+  }
+}
+
+Matrix3 prec(double e, const Matrix3& R) {
+  Matrix3 prec_mat;
+  prec_mat << e, 0, 0, 0, e, 0, 0, 0, 1;
+  return R.transpose() * prec_mat * R;
+}
+
+Matrix3 cov(double e, const Matrix3& R) {
+  Matrix3 cov_mat;
+  cov_mat << 1, 0, 0, 0, 1, 0, 0, 0, e;
+  return R.transpose() * cov_mat * R;
+}
+}  // namespace
+
 EdgeGICP::EdgeGICP() {
   pos0.setZero();
   pos1.setZero();
@@ -40,55 +76,28 @@ EdgeGICP::EdgeGICP() {
   R1.setIdentity();
 }
 
-void EdgeGICP::makeRot0() {
-  Vector3 y;
-  y << 0, 1, 0;
-  R0.row(2) = normal0;
-  y = y - normal0(1) * normal0;
-  y.normalize();  // need to check if y is close to 0
-  R0.row(1) = y;
-  R0.row(0) = normal0.cross(R0.row(1));
-  //      cout << normal.transpose() << endl;
-  //      cout << R0 << endl << endl;
-  //      cout << R0*R0.transpose() << endl << endl;
-}
+void EdgeGICP::makeRot0() { makeRot(R0, normal0); }
 
-void EdgeGICP::makeRot1() {
-  Vector3 y;
-  y << 0, 1, 0;
-  R1.row(2) = normal1;
-  y = y - normal1(1) * normal1;
-  y.normalize();  // need to check if y is close to 0
-  R1.row(1) = y;
-  R1.row(0) = normal1.cross(R1.row(1));
-}
+void EdgeGICP::makeRot1() { makeRot(R1, normal1); }
 
 Matrix3 EdgeGICP::prec0(double e) {
   makeRot0();
-  Matrix3 prec;
-  prec << e, 0, 0, 0, e, 0, 0, 0, 1;
-  return R0.transpose() * prec * R0;
+  return prec(e, R0);
 }
 
 Matrix3 EdgeGICP::prec1(double e) {
   makeRot1();
-  Matrix3 prec;
-  prec << e, 0, 0, 0, e, 0, 0, 0, 1;
-  return R1.transpose() * prec * R1;
+  return prec(e, R1);
 }
 
 Matrix3 EdgeGICP::cov0(double e) {
   makeRot0();
-  Matrix3 cov;
-  cov << 1, 0, 0, 0, 1, 0, 0, 0, e;
-  return R0.transpose() * cov * R0;
+  return cov(e, R0);
 }
 
 Matrix3 EdgeGICP::cov1(double e) {
   makeRot1();
-  Matrix3 cov;
-  cov << 1, 0, 0, 0, 1, 0, 0, 0, e;
-  return R1.transpose() * cov * R1;
+  return cov(e, R1);
 }
 
 }  // namespace g2o

unit_test/CMakeLists.txt

@@ -38,6 +38,7 @@ add_subdirectory(slam3d)
 add_subdirectory(sba)
 add_subdirectory(sim3)
 add_subdirectory(solver)
+add_subdirectory(icp)
 
 if (TARGET simulator_lib)
   add_subdirectory(simulator)

unit_test/icp/CMakeLists.txt

@@ -0,0 +1,5 @@
+add_executable(unittest_icp
+  icp_rotation.cpp
+)
+target_link_libraries(unittest_icp types_icp)
+create_test(unittest_icp)

unit_test/icp/icp_rotation.cpp

@@ -0,0 +1,69 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2014 R. Kuemmerle, G. Grisetti, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+//   this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+//   notice, this list of conditions and the following disclaimer in the
+//   documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include "g2o/types/icp/types_icp.h"
+#include "gtest/gtest.h"
+
+using namespace g2o;
+
+void checkRotationMatrix(const Matrix3& R) {
+  constexpr double tol = 1e-14;
+  // check norms of basis vectors
+  EXPECT_NEAR(R.row(0).norm(), 1, tol);
+  EXPECT_NEAR(R.row(1).norm(), 1, tol);
+  EXPECT_NEAR(R.row(2).norm(), 1, tol);
+  // check orthogonality
+  EXPECT_NEAR(R.row(0).dot(R.row(1)), 0, tol);
+  EXPECT_NEAR(R.row(1).dot(R.row(2)), 0, tol);
+  EXPECT_NEAR(R.row(2).dot(R.row(0)), 0, tol);
+  // check that basis is left-handed
+  EXPECT_NEAR(R.determinant(), -1.0, tol);
+}
+
+/*
+ * ROTATION MATRIX Tests
+ */
+TEST(IcpRotation, RotationMatrix) {
+  constexpr size_t thetaPoints = 100;
+  constexpr size_t phiPoints = 100;
+  EdgeGICP edge;
+  for (size_t n = 0; n < thetaPoints; n++) {
+    const double theta = M_PI * n / thetaPoints;
+    for (size_t k = 0; k < phiPoints; k++) {
+      const double phi = 2.0L * M_PI * k / phiPoints;
+      const Vector3 normal(std::sin(theta) * std::cos(phi), std::cos(theta),
+                           std::sin(theta) * std::sin(phi));
+      edge.normal0 = normal;
+      edge.normal1 = normal;
+
+      edge.makeRot0();
+      edge.makeRot1();
+
+      checkRotationMatrix(edge.R0);
+      checkRotationMatrix(edge.R1);
+    }
+  }
+}