diff --git a/cpp/Pose3.cpp b/cpp/Pose3.cpp index baa970b51..bc6fb5179 100644 --- a/cpp/Pose3.cpp +++ b/cpp/Pose3.cpp @@ -36,7 +36,7 @@ namespace gtsam { Vector u = vector_range(d, range(3,6)); double t = norm_2(w); if (t < 1e-5) - return Pose3(expmap (w), expmap (u)); + return Pose3(Rot3(), expmap (u)); else { Matrix W = skewSymmetric(w/t); Matrix A = eye(3, 3) + ((1 - cos(t)) / t) * W + ((t - sin(t)) / t) * (W * W);