diff --git a/src/processor_imu.cpp b/src/processor_imu.cpp index 4943314918e82395e790612fcf03c9ebd7b2cd0f..052e8ff8bb26345f1a422958fa4dd476ccf91b29 100644 --- a/src/processor_imu.cpp +++ b/src/processor_imu.cpp @@ -234,7 +234,7 @@ void ProcessorIMU::computeCurrentDelta(const Eigen::VectorXs& _data, assert(_data.size() == data_size_ && "Wrong data size!"); using namespace Eigen; - Matrix<Scalar, 9, 6> jac_data; + Matrix<Scalar, 9, 6> jac_delta_data; /* * We have the following computing pipeline: @@ -255,13 +255,13 @@ void ProcessorIMU::computeCurrentDelta(const Eigen::VectorXs& _data, */ // create delta - imu::body2delta(_data - _calib, _dt, _delta, jac_data); // Jacobians tested in imu_tools + imu::body2delta(_data - _calib, _dt, _delta, jac_delta_data); // Jacobians tested in imu_tools // compute delta_cov - _delta_cov = jac_data * _data_cov * jac_data.transpose(); + _delta_cov = jac_delta_data * _data_cov * jac_delta_data.transpose(); // compute jacobian_calib - _jac_delta_calib = - jac_data; + _jac_delta_calib = - jac_delta_data; } diff --git a/src/processor_motion.cpp b/src/processor_motion.cpp index 9b5ac64ebf90d8760725271d6d21128303d1bcae..a8e94f2432955dc03d0068f8f9f46a4079d7cb78 100644 --- a/src/processor_motion.cpp +++ b/src/processor_motion.cpp @@ -296,7 +296,7 @@ bool ProcessorMotion::keyFrameCallback(FrameBasePtr _new_keyframe, const Scalar& // reintegrateBuffer(new_capture); // create motion constraint and add it to the feature, and constrain to the other capture (origin) - emplaceConstraint(new_feature, keyframe_origin->getCaptureOf(getSensorPtr()) ); // XXX it was new_keyframe_origin + emplaceConstraint(new_feature, keyframe_origin->getCaptureOf(getSensorPtr()) );