diff --git a/gtsam/navigation/tests/testPoseVelocityBias.cpp b/gtsam/navigation/tests/testPoseVelocityBias.cpp index cc2a47498..0b897bc6e 100644 --- a/gtsam/navigation/tests/testPoseVelocityBias.cpp +++ b/gtsam/navigation/tests/testPoseVelocityBias.cpp @@ -31,7 +31,7 @@ using namespace gtsam; Vector9 error(const PoseVelocityBias& pvb1, const PoseVelocityBias& pvb2) { Matrix3 R1 = pvb1.pose.rotation().matrix(); // Ri.transpose() translate the error from the global frame into pose1's frame - const Vector3 fp = R1.transpose() * (pvb2.pose.translation() - pvb1.pose.translation()).vector(); + const Vector3 fp = R1.transpose() * (pvb2.pose.translation() - pvb1.pose.translation()); const Vector3 fv = R1.transpose() * (pvb2.velocity - pvb1.velocity); const Rot3 R1BetweenR2 = pvb1.pose.rotation().between(pvb2.pose.rotation()); const Vector3 fR = Rot3::Logmap(R1BetweenR2);