Skip to content
Snippets Groups Projects
Commit cc6585d6 authored by Dinesh Atchuthan's avatar Dinesh Atchuthan
Browse files

correct expectation() function

parent fb45e53c
No related branches found
No related tags found
No related merge requests found
...@@ -7,6 +7,8 @@ ...@@ -7,6 +7,8 @@
#include "frame_imu.h" #include "frame_imu.h"
#include "rotations.h" #include "rotations.h"
//Eigen include
namespace wolf { namespace wolf {
...@@ -45,9 +47,9 @@ class ConstraintIMU : public ConstraintSparse<9, 3, 4, 3, 3, 3, 3, 4, 3> ...@@ -45,9 +47,9 @@ class ConstraintIMU : public ConstraintSparse<9, 3, 4, 3, 3, 3, 3, 4, 3>
* Vector3s _v2 : velocity in current frame * Vector3s _v2 : velocity in current frame
* Matrix<10,1, wolf::Scalar> _expectation : to retrieve resulting expectation (PVQ) * Matrix<10,1, wolf::Scalar> _expectation : to retrieve resulting expectation (PVQ)
*/ */
template<typename D1> template<typename D1, typename D2, typename D3>
  • @AtDinesh Did you look at how we proceeded in ConstraintAHP.h? We create the method expectation(void) which gathers all the pointers, and calls a method expectation(const T* const p, ...etc...), that is exactly the same method we call from operator() (const T* const, ...etc...).

    The ultimate goal is not having to code again the same math for the expectation.

    Is your approach better than ours? Could we discuss this? If yours is better, then I'll change ConstraintAHP !

  • Please register or sign in to reply
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, void expectation(const Eigen::MatrixBase<D1> & _p1, const Eigen::QuaternionBase<D2> & _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; const Eigen::MatrixBase<D1> & _p2, const Eigen::QuaternionBase<D2> & _q2, const Eigen::MatrixBase<D1> & _v2, const Eigen::MatrixBase<D3> & _result) const;
/* \brief : return the expected value given the state blocks in the wolf tree /* \brief : return the expected value given the state blocks in the wolf tree
current frame data is taken from constraintIMU object. current frame data is taken from constraintIMU object.
...@@ -65,11 +67,13 @@ class ConstraintIMU : public ConstraintSparse<9, 3, 4, 3, 3, 3, 3, 4, 3> ...@@ -65,11 +67,13 @@ class ConstraintIMU : public ConstraintSparse<9, 3, 4, 3, 3, 3, 3, 4, 3>
const Eigen::Vector3s frame_current_vel = dv_preint_; const Eigen::Vector3s frame_current_vel = dv_preint_;
const Eigen::Vector3s frame_current_ab = acc_bias_preint_; const Eigen::Vector3s frame_current_ab = acc_bias_preint_;
const Eigen::Vector3s frame_current_wb = gyro_bias_preint_; const Eigen::Vector3s frame_current_wb = gyro_bias_preint_;
const Eigen::VectorXs frame_imu_pos = (frm_imu->getPPtr()->getVector()); const Eigen::Vector3s frame_imu_pos = (frm_imu->getPPtr()->getVector());
const Eigen::VectorXs frame_imu_ori = (frm_imu->getOPtr()->getVector()); const Eigen::Vector4s frame_imu_ori = (frm_imu->getOPtr()->getVector());
const Eigen::VectorXs frame_imu_vel = (frm_imu->getVPtr()->getVector()); const Eigen::Vector3s frame_imu_vel = (frm_imu->getVPtr()->getVector());
Eigen::Quaternions frame_imu_ori_q(frame_imu_ori);
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_q, frame_imu_vel, exp);
std::cout << frame_current_pos << std::endl; std::cout << frame_current_pos << std::endl;
std::cout << frame_current_ori.w() << std::endl; std::cout << frame_current_ori.w() << std::endl;
std::cout << frame_current_vel << std::endl; std::cout << frame_current_vel << std::endl;
...@@ -182,32 +186,29 @@ inline bool ConstraintIMU::operator ()(const T* const _p1, const T* const _q1, c ...@@ -182,32 +186,29 @@ inline bool ConstraintIMU::operator ()(const T* const _p1, const T* const _q1, c
return true; return true;
} }
template<typename D1> template<typename D1, typename D2, typename D3>
inline void ConstraintIMU::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, inline void ConstraintIMU::expectation(const Eigen::MatrixBase<D1> & _p1, const Eigen::QuaternionBase<D2> & _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 const Eigen::MatrixBase<D1> & _p2, const Eigen::QuaternionBase<D2> & _q2, const Eigen::MatrixBase<D1> & _v2, const Eigen::MatrixBase<D3> & _result) const
{ {
// MAPS //needed typedefs
typedef typename D2::Vector3 Vector3Map;
Eigen::Map<const Eigen::Matrix<D1,3,1> > p1(_p1); typedef typename D2::Scalar DataType;
Eigen::Map<const Eigen::Quaternion<D1> > q1(_q1); typedef Eigen::Map <Eigen::Matrix<DataType,4,1> > ConstVector4Map;
Eigen::Map<const Eigen::Matrix<D1,3,1> > v1(_v1);
Eigen::Map<const Eigen::Matrix<D1,3,1> > ab(_ab); //instead of maps we use static_asserts from eigen to detect size at compile time
Eigen::Map<const Eigen::Matrix<D1,3,1> > wb(_wb); //check entry sizes
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(D1, 3)
Eigen::Map<const Eigen::Matrix<D1,3,1> > p2(_p2); EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(D3, 10)
Eigen::Map<const Eigen::Quaternion<D1> > q2(_q2);
Eigen::Map<const Eigen::Matrix<D1,3,1> > v2(_v2);
//Eigen::Map<Eigen::Matrix<D1,10,1> > result(_result);
// Predict delta: d_pred = x2 (-) x1 // Predict delta: d_pred = x2 (-) x1
Eigen::Matrix<D1,3,1> dp_predict = q1.conjugate() * ( p2 - p1 - v1 * (D1)dt_ - (D1)0.5 * g_.cast<D1>() * (D1)dt_2_ ); Vector3Map dp_predict = (_q1.conjugate() * ( _p2 - _p1 - _v1 * (DataType)dt_ - (DataType)0.5 * g_.cast<DataType>() * (DataType)dt_2_ ));
Eigen::Matrix<D1,3,1> dv_predict = q1.conjugate() * ( v2 - v1 - g_.cast<D1>() * (D1)dt_ ); Vector3Map dv_predict (_q1.conjugate() * ( _v2 - _v1 - g_.cast<DataType>() * (DataType)dt_ ));
Eigen::Quaternion<D1> dq_predict = q1.conjugate() * q2; Eigen::Quaternion<DataType> dq_predict (_q1.conjugate() * _q2);
ConstVector4Map dq_vec4(dq_predict.coeffs().data());
const_cast< Eigen::MatrixBase<D1> > (_result).head(3) = dp_predict;
const_cast< Eigen::MatrixBase<D1> > (_result).segment(3,4) = dq_predict; const_cast< Eigen::MatrixBase<D3>& > (_result).head(3) = dp_predict;
const_cast< Eigen::MatrixBase<D1> > (_result).tail(3) = dv_predict; const_cast< Eigen::MatrixBase<D3>& > (_result).segment(3,4) = dq_vec4;
const_cast< Eigen::MatrixBase<D3>& > (_result).tail(3) = dv_predict;
} }
inline JacobianMethod ConstraintIMU::getJacobianMethod() const inline JacobianMethod ConstraintIMU::getJacobianMethod() const
......
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