diff --git a/src/processor/processor_odom_3D.cpp b/src/processor/processor_odom_3D.cpp index 4cb3ad75e7887de5624e87e567b04bc26cfd6735..5f1610c949c213796d2332e07e1023bb5f483d8b 100644 --- a/src/processor/processor_odom_3D.cpp +++ b/src/processor/processor_odom_3D.cpp @@ -114,16 +114,16 @@ void ProcessorOdom3D::deltaPlusDelta(const Eigen::VectorXs& _delta1, const Eigen */ // temporaries - Eigen::Matrix3s DR = dq1.matrix(); - Eigen::Matrix3s dR = dq2.matrix(); + Eigen::Matrix3s dR1 = dq1.matrix(); + Eigen::Matrix3s dR2 = dq2.matrix(); // fill Jacobians _jacobian1.setIdentity(); - _jacobian1.block<3, 3>(0, 3) = -DR * skew(dp2); // (Sola 16, ex. B.3.2 and Sec. 7.2.3) - _jacobian1.block<3, 3>(3, 3) = dR.transpose(); // (Sola 16, Sec. 7.2.3) + _jacobian1.block<3, 3>(0, 3) = -dR1 * skew(dp2); // (Sola 16, ex. B.3.2 and Sec. 7.2.3) + _jacobian1.block<3, 3>(3, 3) = dR2.transpose(); // (Sola 16, Sec. 7.2.3) _jacobian2.setIdentity(); - _jacobian2.block<3, 3>(0, 0) = DR; // (Sola 16, Sec. 7.2.3) + _jacobian2.block<3, 3>(0, 0) = dR1; // (Sola 16, Sec. 7.2.3) // perform composition here to avoid aliasing problems if _delta1 and _delta_plus_delta share the same storage dp_out = dp1 + dq1 * dp2;