diff --git a/gtsam/geometry/Pose2.cpp b/gtsam/geometry/Pose2.cpp index 8e5d58047..a2dfc9f18 100644 --- a/gtsam/geometry/Pose2.cpp +++ b/gtsam/geometry/Pose2.cpp @@ -150,17 +150,16 @@ Matrix3 Pose2::ExpmapDerivative(const Vector3& v) { */ double sZalpha = sin(alpha)/alpha, c_1Zalpha = (cos(alpha)-1)/alpha; double v1Zalpha = v[0]/alpha, v2Zalpha = v[1]/alpha; - J = (Matrix(3,3) << - sZalpha, -c_1Zalpha, v1Zalpha + v2Zalpha*c_1Zalpha - v1Zalpha*sZalpha, - c_1Zalpha, sZalpha, -v1Zalpha*c_1Zalpha + v2Zalpha - v2Zalpha*sZalpha, - 0, 0, 1).finished(); + J << sZalpha, -c_1Zalpha, v1Zalpha + v2Zalpha*c_1Zalpha - v1Zalpha*sZalpha, + c_1Zalpha, sZalpha, -v1Zalpha*c_1Zalpha + v2Zalpha - v2Zalpha*sZalpha, + 0, 0, 1; } else { // Thanks to Krunal: Apply L'Hospital rule to several times to // compute the limits when alpha -> 0 - J = (Matrix(3,3) << 1,0,-0.5*v[1], + J << 1,0,-0.5*v[1], 0,1, 0.5*v[0], - 0,0, 1).finished(); + 0,0, 1; } return J; @@ -175,15 +174,14 @@ Matrix3 Pose2::LogmapDerivative(const Vector3& v) { double halfCotHalfAlpha = 0.5*sin(alpha)/(1-cos(alpha)); double v1 = v[0], v2 = v[1]; - J = (Matrix(3,3) << - alpha*halfCotHalfAlpha, -0.5*alpha, v1*alphaInv - v1*halfCotHalfAlpha + 0.5*v2, - 0.5*alpha, alpha*halfCotHalfAlpha, v2*alphaInv - 0.5*v1 - v2*halfCotHalfAlpha, - 0, 0, 1).finished(); + J << alpha*halfCotHalfAlpha, -0.5*alpha, v1*alphaInv - v1*halfCotHalfAlpha + 0.5*v2, + 0.5*alpha, alpha*halfCotHalfAlpha, v2*alphaInv - 0.5*v1 - v2*halfCotHalfAlpha, + 0, 0, 1; } else { - J = (Matrix(3,3) << 1,0, 0.5*v[1], + J << 1,0, 0.5*v[1], 0,1, -0.5*v[0], - 0,0, 1).finished(); + 0,0, 1; } return J; }