diff --git a/src/processor/processor_imu_3d.cpp b/src/processor/processor_imu_3d.cpp index e31b359ffdf328b264ea4b46ee50dfc1d2be477f..086c6a434f3e7e429b432c4194330cda880c8d95 100644 --- a/src/processor/processor_imu_3d.cpp +++ b/src/processor/processor_imu_3d.cpp @@ -99,13 +99,13 @@ bool ProcessorImu3d::voteForKeyFrame() const return false; } -CaptureMotionPtr ProcessorImu3d::emplaceCapture(const FrameBasePtr &_frame_own, - const SensorBasePtr &_sensor, - const TimeStamp &_ts, - const VectorXd &_data, - const MatrixXd &_data_cov, - const VectorXd &_calib, - const VectorXd &_calib_preint, +CaptureMotionPtr ProcessorImu3d::emplaceCapture(const FrameBasePtr & _frame_own, + const SensorBasePtr & _sensor, + const TimeStamp & _ts, + const VectorXd & _data, + const MatrixXd & _data_cov, + const VectorXd & _calib, + const VectorXd & _calib_preint, const CaptureBasePtr &_capture_origin) { auto cap_motion = std::static_pointer_cast<CaptureMotion>( @@ -177,9 +177,9 @@ void ProcessorImu3d::computeCurrentDelta(const Eigen::VectorXd &_data, const Eigen::MatrixXd &_data_cov, const Eigen::VectorXd &_calib, const double _dt, - Eigen::VectorXd &_delta, - Eigen::MatrixXd &_delta_cov, - Eigen::MatrixXd &_jac_delta_calib) const + Eigen::VectorXd & _delta, + Eigen::MatrixXd & _delta_cov, + Eigen::MatrixXd & _jac_delta_calib) const { assert(_data.size() == data_size_ && "Wrong data size!"); @@ -218,7 +218,7 @@ void ProcessorImu3d::computeCurrentDelta(const Eigen::VectorXd &_data, void ProcessorImu3d::deltaPlusDelta(const Eigen::VectorXd &_delta_preint, const Eigen::VectorXd &_delta, const double _dt, - Eigen::VectorXd &_delta_preint_plus_delta) const + Eigen::VectorXd & _delta_preint_plus_delta) const { /* MATHS according to Sola-16 * Dp' = Dp + Dv*dt + 1/2*Dq*(a-a_b)*dt^2 = Dp + Dv*dt + Dq*dp if dp = @@ -235,7 +235,7 @@ void ProcessorImu3d::deltaPlusDelta(const Eigen::VectorXd &_delta_preint, void ProcessorImu3d::statePlusDelta(const VectorComposite &_x, const Eigen::VectorXd &_delta, const double _dt, - VectorComposite &_x_plus_delta) const + VectorComposite & _x_plus_delta) const { assert(_delta.size() == 10 && "Wrong _delta vector size"); assert(_dt >= 0 && "Time interval _dt is negative!"); @@ -255,9 +255,9 @@ void ProcessorImu3d::statePlusDelta(const VectorComposite &_x, void ProcessorImu3d::deltaPlusDelta(const Eigen::VectorXd &_delta_preint, const Eigen::VectorXd &_delta, const double _dt, - Eigen::VectorXd &_delta_preint_plus_delta, - Eigen::MatrixXd &_jacobian_delta_preint, - Eigen::MatrixXd &_jacobian_delta) const + Eigen::VectorXd & _delta_preint_plus_delta, + Eigen::MatrixXd & _jacobian_delta_preint, + Eigen::MatrixXd & _jacobian_delta) const { /* * Expression of the delta integration step, D' = D (+) d: @@ -532,8 +532,8 @@ bool ProcessorImu3d::recomputeStates() const const auto &cap_origin = cap->getOriginCapture(); const auto &frm_origin = cap_origin->getFrame(); const auto &delta = VectorComposite({{'P', ftr->getMeasurement().head<3>()}, - {'O', ftr->getMeasurement().segment<4>(3)}, - {'V', ftr->getMeasurement().tail<3>()}}); + {'O', ftr->getMeasurement().segment<4>(3)}, + {'V', ftr->getMeasurement().tail<3>()}}); const auto &x_origin = frm_origin->getState(); auto dt = cap->getTimeStamp() - cap_origin->getTimeStamp(); auto x = imu::composeOverState(x_origin, delta, dt);