diff --git a/src/constraint_imu.h b/src/constraint_imu.h index 8956a3257d2152a4bebbbac0b4a4adb133f40daa..ae08626c3c6e59c77745bd0bae22d479bae94a1b 100644 --- a/src/constraint_imu.h +++ b/src/constraint_imu.h @@ -21,6 +21,11 @@ class ConstraintIMU : public ConstraintSparse<9, 3, 4, 3, 3, 3, 3, 4, 3> virtual ~ConstraintIMU(); + /* \brief : compute the residual from the state blocks being iterated by the solver. + -> computes the expected measurement + -> compares the actual measurement with the expected one + -> weights the result with the covariance of the noise (residual = sqrt_info_matrix * err;) + */ template<typename T> bool operator ()(const T* const _p1, const T* const _o1, const T* const _v1, const T* const _b_a1, const T* _b_g1, const T* const _p2, const T* const _o2, const T* const _v2, @@ -44,6 +49,10 @@ class ConstraintIMU : public ConstraintSparse<9, 3, 4, 3, 3, 3, 3, 4, 3> void expectation(const Eigen::MatrixBase<D1> & _p1, const Eigen::QuaternionBase<D1> & _q1, const Eigen::MatrixBase<D1> & _v1, const Eigen::MatrixBase<D1> & _ab, const Eigen::MatrixBase<D1> & _wb, const Eigen::MatrixBase<D1> & _p2, const Eigen::MatrixBase<D1> & _q2, const Eigen::MatrixBase<D1> & _v2, const Eigen::Matrix<D1,10,1> & _result) const; + /* \brief : return the expected value given the state blocks in the wolf tree + current frame data is taken from constraintIMU object. + IMU frame is taken from wolf tree + */ Eigen::VectorXs expectation() const { Eigen::Matrix<wolf::Scalar, 10, 1> exp; @@ -60,8 +69,7 @@ class ConstraintIMU : public ConstraintSparse<9, 3, 4, 3, 3, 3, 3, 4, 3> const Eigen::VectorXs frame_imu_ori = (frm_imu->getOPtr()->getVector()); const Eigen::VectorXs frame_imu_vel = (frm_imu->getVPtr()->getVector()); - //expectation(frame_current_pos, frame_current_ori, frame_current_vel, frame_current_ab, frame_current_wb, - // frame_imu_pos, frame_imu_ori, frame_imu_vel, exp); + expectation(frame_current_pos, frame_current_ori, frame_current_vel, frame_current_ab, frame_current_wb, frame_imu_pos, frame_imu_ori, frame_imu_vel, exp); std::cout << frame_current_pos << std::endl; std::cout << frame_current_ori.w() << std::endl; std::cout << frame_current_vel << std::endl; @@ -202,30 +210,6 @@ inline void ConstraintIMU::expectation(const Eigen::MatrixBase<D1> & _p1, const const_cast< Eigen::MatrixBase<D1> > (_result).tail(3) = dv_predict; } -/*Eigen::VectorXs ConstraintIMU::expectation(void) const -{ - Eigen::VectorXs exp(10); - FrameBasePtr frm_current = getFeaturePtr()->getCapturePtr()->getFramePtr(); - FrameBasePtr frm_imu = getFrameOtherPtr(); - //LandmarkBasePtr lmk = getLandmarkOtherPtr(); - const Scalar * const frame_current_pos = frm_current->getPPtr()->getVector().data(); - const Scalar * const frame_current_ori = frm_current->getOPtr()->getVector().data(); - const Scalar * const frame_current_vel = frm_current->getVPtr()->getVector().data(); - const Scalar * const frame_imu_pos = frm_imu->getPPtr()->getVector().data(); - const Scalar * const frame_imu_ori = frm_imu->getOPtr()->getVector().data(); - const Scalar * const frame_imu_vel = frm_imu->getVPtr()->getVector().data(); - //const Scalar * const lmk_pos_hmg = lmk->getPPtr()->getVector().data(); - //expectation(frame_current_pos, frame_current_ori, frame_current_vel, - // frame_imu_pos, frame_imu_ori, frame_imu_vel, exp.data()); - std::cout << frame_current_pos << std::endl; - std::cout << frame_current_ori << std::endl; - std::cout << frame_current_vel << std::endl; - std::cout << frame_imu_pos << std::endl; - std::cout << frame_imu_ori << std::endl; - std::cout << frame_imu_vel << std::endl; - return exp; -}*/ - inline JacobianMethod ConstraintIMU::getJacobianMethod() const { return JAC_AUTO;