Skip to content
Snippets Groups Projects
Commit b8d19c9c authored by Joan Solà Ortega's avatar Joan Solà Ortega
Browse files

Update processor_odom_3D.cpp

parent 9f55add5
No related branches found
No related tags found
1 merge request!301Resolve "Work on const / non-const in wolf base classes"
Pipeline #4289 passed
...@@ -114,16 +114,16 @@ void ProcessorOdom3D::deltaPlusDelta(const Eigen::VectorXs& _delta1, const Eigen ...@@ -114,16 +114,16 @@ void ProcessorOdom3D::deltaPlusDelta(const Eigen::VectorXs& _delta1, const Eigen
*/ */
// temporaries // temporaries
Eigen::Matrix3s DR = dq1.matrix(); Eigen::Matrix3s dR1 = dq1.matrix();
Eigen::Matrix3s dR = dq2.matrix(); Eigen::Matrix3s dR2 = dq2.matrix();
// fill Jacobians // fill Jacobians
_jacobian1.setIdentity(); _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>(0, 3) = -dR1 * 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>(3, 3) = dR2.transpose(); // (Sola 16, Sec. 7.2.3)
_jacobian2.setIdentity(); _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 // perform composition here to avoid aliasing problems if _delta1 and _delta_plus_delta share the same storage
dp_out = dp1 + dq1 * dp2; dp_out = dp1 + dq1 * dp2;
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment