solve div by zero error

ethz-asl · Nov 19, 2018 · da8744b · da8744b
1 parent cc28da3
commit da8744b
Showing 1 changed file with 9 additions and 3 deletions.
diff --git a/include/lidar_align/transform.h b/include/lidar_align/transform.h
@@ -46,9 +46,15 @@ class Transform {
   }
 
   static Transform exp(const Vector6& vector) {
-    return Transform(vector.head<3>(), Rotation(Eigen::AngleAxisf(
-                                           vector.tail<3>().norm(),
-                                           vector.tail<3>().normalized())));
+    Rotation rotation;
+    constexpr float kEpsilon = 1e-8;
+    const float norm = vector.tail<3>().norm();
+    if (norm < kEpsilon) {
+      return Transform(vector.head<3>(), Rotation::Identity());
+    } else {
+      return Transform(vector.head<3>(), Rotation(Eigen::AngleAxisf(
+                                             norm, vector.tail<3>() / norm)));
+    }
   }
 
   Vector6 log() const {