Handle the case when normal is close to y axis (#917)

* fix EdgeGICP::makeRot functions for case when y is close to 0
* improve numerical stability if a normal is close to [0,1,0]
* add unit test

g2o/types/icp/types_icp.h

@@ -84,12 +84,19 @@ class G2O_TYPES_ICP_API EdgeGICP {
     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;
+    double ysquarednorm = y.squaredNorm();
+    if (ysquarednorm >= ySquaredBnd) {
+      y /= std::sqrt(ysquarednorm);
+      R0.row(1) = y;
+      R0.row(0) = normal0.cross(R0.row(1));
+    } else {
+      Vector3 x;
+      x << -1, 0, 0;
+      x = x + normal0(0) * normal0;
+      x.normalize();
+      R0.row(0) = x;
+      R0.row(1) = -normal0.cross(R0.row(0));
+    }
   }
 
   // set up rotation matrix for pos1
@@ -98,9 +105,19 @@ class G2O_TYPES_ICP_API EdgeGICP {
     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));
+    double ysquarednorm = y.squaredNorm();
+    if (y.squaredNorm() >= ySquaredBnd) {
+      y /= std::sqrt(ysquarednorm);
+      R1.row(1) = y;
+      R1.row(0) = normal1.cross(R1.row(1));
+    } else {
+      Vector3 x;
+      x << -1, 0, 0;
+      x = x + normal1(0) * normal1;
+      x.normalize();
+      R1.row(0) = x;
+      R1.row(1) = -normal1.cross(R1.row(0));
+    }
   }
 
   // returns a precision matrix for point-plane
@@ -134,6 +151,9 @@ class G2O_TYPES_ICP_API EdgeGICP {
     cov << 1, 0, 0, 0, 1, 0, 0, 0, e;
     return R1.transpose() * cov * R1;
   }
+
+  // this parameter is used by makeRot0 and makeRot1
+  static constexpr double ySquaredBnd{0.1};
 };
 
 // 3D rigid constraint

unit_test/CMakeLists.txt

@@ -26,3 +26,4 @@ add_subdirectory(slam3d_addons)
 add_subdirectory(sim3)
 add_subdirectory(sba)
 add_subdirectory(solver)
+add_subdirectory(icp)

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);
+    }
+  }
+}