diff --git a/src/capture_base.h b/src/capture_base.h
index cbc052bb8765f002ac6e6fe49ba14bc986e3bfef..dca78846c8f83d0cfa13e155d7c7bc1c3f9f7360 100644
--- a/src/capture_base.h
+++ b/src/capture_base.h
@@ -22,6 +22,7 @@ class CaptureBase : public NodeBase, public std::enable_shared_from_this<Capture
private:
FrameBaseWPtr frame_ptr_;
FeatureBaseList feature_list_;
+ ConstraintBaseList constrained_by_list_;
SensorBaseWPtr sensor_ptr_; ///< Pointer to sensor
// Deal with sensors with dynamic extrinsics (check dynamic_extrinsic_ in SensorBase)
std::vector<StateBlockPtr> state_block_vec_; ///< vector of state blocks, in the order P, O, intrinsic.
@@ -47,6 +48,9 @@ class CaptureBase : public NodeBase, public std::enable_shared_from_this<Capture
virtual ~CaptureBase();
void remove();
+ // Type
+ virtual bool isMotion() const { return false; }
+
unsigned int id();
TimeStamp getTimeStamp() const;
void setTimeStamp(const TimeStamp& _ts);
@@ -67,6 +71,11 @@ class CaptureBase : public NodeBase, public std::enable_shared_from_this<Capture
SensorBasePtr getSensorPtr() const;
virtual void setSensorPtr(const SensorBasePtr sensor_ptr);
+ // constrained by
+ virtual ConstraintBasePtr addConstrainedBy(ConstraintBasePtr _ctr_ptr);
+ unsigned int getHits() const;
+ ConstraintBaseList& getConstrainedByList();
+
// State blocks
const std::vector<StateBlockPtr>& getStateBlockVec() const;
std::vector<StateBlockPtr>& getStateBlockVec();
@@ -217,6 +226,24 @@ inline void CaptureBase::getConstraintList(ConstraintBaseList& _ctr_list)
f_ptr->getConstraintList(_ctr_list);
}
+inline ConstraintBasePtr CaptureBase::addConstrainedBy(ConstraintBasePtr _ctr_ptr)
+{
+ constrained_by_list_.push_back(_ctr_ptr);
+ _ctr_ptr->setCaptureOtherPtr( shared_from_this() );
+ return _ctr_ptr;
+}
+
+inline unsigned int CaptureBase::getHits() const
+{
+ return constrained_by_list_.size();
+}
+
+inline ConstraintBaseList& CaptureBase::getConstrainedByList()
+{
+ return constrained_by_list_;
+}
+
+
inline TimeStamp CaptureBase::getTimeStamp() const
{
return time_stamp_;
diff --git a/src/capture_motion.h b/src/capture_motion.h
index 68c920792c5fcdb7718a3c0161087bbaac284834..65b7fd4f33553ab89492ba1d8429fc585f1c62a4 100644
--- a/src/capture_motion.h
+++ b/src/capture_motion.h
@@ -63,6 +63,9 @@ class CaptureMotion : public CaptureBase
virtual ~CaptureMotion();
+ // Type
+ virtual bool isMotion() const override { return true; }
+
// Data
const Eigen::VectorXs& getData() const;
const Eigen::MatrixXs& getDataCovariance() const;
diff --git a/src/capture_odom_3D.cpp b/src/capture_odom_3D.cpp
index 666f3e58f39f6d82b857203e6c9a207513da668e..232f088ffa9f639e089a5c154342217a5eb80d52 100644
--- a/src/capture_odom_3D.cpp
+++ b/src/capture_odom_3D.cpp
@@ -38,7 +38,7 @@ Eigen::VectorXs CaptureOdom3D::correctDelta(const VectorXs& _delta, const Vector
{
VectorXs delta(7);
delta.head(3) = _delta.head(3) + _delta_error.head(3);
- delta.tail(4) = (Quaternions(_delta.head(4).data()) * exp_q(_delta_error.tail(3))).coeffs();
+ delta.tail(4) = (Quaternions(_delta.data()+3) * exp_q(_delta_error.tail(3))).coeffs();
return delta;
}
diff --git a/src/constraint_imu.h b/src/constraint_imu.h
index 197ba15d36fd288d2a39c9fd3f69d5e9595b9e01..7948045a05c805c54c4130f4f2d43edd00c953d1 100644
--- a/src/constraint_imu.h
+++ b/src/constraint_imu.h
@@ -139,8 +139,39 @@ class ConstraintIMU : public ConstraintAutodiff<ConstraintIMU, 15, 3, 4, 3, 6, 3
*/
const Eigen::Matrix3s sqrt_A_r_dt_inv;
const Eigen::Matrix3s sqrt_W_r_dt_inv;
+
+
+ private:
+ template<typename D>
+ void print(const std::string& name, const Eigen::MatrixBase<D>& mat) const;
+// template<typename T, int R, int C>
+// void print(const std::string& name, const Eigen::Matrix<T, R, C>& mat) const;
+ template<int R, int C>
+ void print(const std::string& name, const Matrix<Scalar, R, C>& mat) const;
};
+template<typename D>
+inline void ConstraintIMU::print(const std::string& name, const Eigen::MatrixBase<D>& mat) const {}
+//template<typename T, int R, int C>
+//inline void ConstraintIMU::print(const std::string& name, const Eigen::Matrix<T, R, C>& mat) const {}
+template<int R, int C>
+inline void ConstraintIMU::print(const std::string& name, const Matrix<Scalar, R, C>& mat) const
+{
+ if (mat.cols() == 1)
+ {
+ WOLF_TRACE(name, ": ", mat.transpose());
+ }
+ else if (mat.rows() == 1)
+ {
+ WOLF_TRACE(name, ": ", mat);
+ }
+ else
+ {
+ WOLF_TRACE(name, ":\n", mat);
+ }
+}
+
+
inline ConstraintIMU::ConstraintIMU(const FeatureIMUPtr& _ftr_ptr,
const CaptureIMUPtr& _cap_origin_ptr,
const ProcessorBasePtr& _processor_ptr,
@@ -163,9 +194,9 @@ inline ConstraintIMU::ConstraintIMU(const FeatureIMUPtr& _ftr_ptr,
_ftr_ptr->getFramePtr()->getOPtr(),
_ftr_ptr->getFramePtr()->getVPtr(),
_ftr_ptr->getCapturePtr()->getSensorIntrinsicPtr()),
- dp_preint_(_ftr_ptr->getDpPreint()), // dp, dv, dq at preintegration time
- dq_preint_(_ftr_ptr->getDqPreint()),
- dv_preint_(_ftr_ptr->getDvPreint()),
+ dp_preint_(_ftr_ptr->getMeasurement().head<3>()), // dp, dv, dq at preintegration time
+ dq_preint_(_ftr_ptr->getMeasurement().data()+3),
+ dv_preint_(_ftr_ptr->getMeasurement().tail<3>()),
acc_bias_preint_(_ftr_ptr->getAccBiasPreint()), // state biases at preintegration time
gyro_bias_preint_(_ftr_ptr->getGyroBiasPreint()),
dDp_dab_(_ftr_ptr->getJacobianBias().block(0,0,3,3)), // Jacs of dp dv dq wrt biases
@@ -180,8 +211,39 @@ inline ConstraintIMU::ConstraintIMU(const FeatureIMUPtr& _ftr_ptr,
sqrt_W_r_dt_inv((Eigen::Matrix3s::Identity() * wb_rate_stdev_ * sqrt(dt_)).inverse())
{
setType("IMU");
+
+// WOLF_TRACE("Constr IMU (f", _ftr_ptr->id(),
+// " C", _ftr_ptr->getCapturePtr()->id(),
+// " F", _ftr_ptr->getCapturePtr()->getFramePtr()->id(),
+// ") (Co", _cap_origin_ptr->id(),
+// " Fo", _cap_origin_ptr->getFramePtr()->id(), ")");
+//
+// WOLF_TRACE("dt: ", dt_);
+//
+// WOLF_TRACE("delta preint: ", std::static_pointer_cast<CaptureMotion>(_ftr_ptr->getCapturePtr())->getDeltaPreint().transpose());
+//// WOLF_TRACE("Dp preint : ", dp_preint_.transpose()); // OK
+//// WOLF_TRACE("Dq preint : ", dq_preint_.coeffs().transpose()); // OK
+//// WOLF_TRACE("Dv preint : ", dv_preint_.transpose()); // OK
+//
+// WOLF_TRACE("bias: ", std::static_pointer_cast<CaptureMotion>(_ftr_ptr->getCapturePtr())->getCalibrationPreint().transpose());
+//// WOLF_TRACE("bias acc : ", acc_bias_preint_.transpose()); // OK
+//// WOLF_TRACE("bias gyro: ", gyro_bias_preint_.transpose()); // OK
+//
+// WOLF_TRACE("Jac bias : \n", std::static_pointer_cast<CaptureMotion>(_ftr_ptr->getCapturePtr())->getJacobianCalib());
+//// WOLF_TRACE("jac Dp_ab: \n", dDp_dab_); // OK
+//// WOLF_TRACE("jac Dv_ab: \n", dDv_dab_); // OK
+//// WOLF_TRACE("jac Dp_wb: \n", dDp_dwb_); // OK
+//// WOLF_TRACE("jac Dq_wb: \n", dDq_dwb_); // OK
+//// WOLF_TRACE("jac Dv_wb: \n", dDv_dwb_); // OK
+//
+// WOLF_TRACE("Omega_delta.sqrt: \n", _ftr_ptr->getMeasurementSquareRootInformationUpper());
+// WOLF_TRACE("Omega_acc.sqrt: \n", sqrt_A_r_dt_inv);
+// WOLF_TRACE("Omega_gyro.sqrt: \n", sqrt_W_r_dt_inv);
+
}
+
+
template<typename T>
inline bool ConstraintIMU::operator ()(const T* const _p1,
const T* const _q1,
@@ -228,6 +290,32 @@ inline bool ConstraintIMU::residual(const Eigen::MatrixBase<D1> & _p1,
const Eigen::MatrixBase<D1> & _wb2,
Eigen::MatrixBase<D3> & _res) const
{
+
+ /* Two methods are possible (select with #define below this note)
+ *
+ * Common computations:
+ * D_exp = between(x1,x2,dt) // Predict delta from states
+ * step = J * (b - b_preint) // compute the delta correction step due to bias change
+ *
+ * Method 1:
+ * corr = plus(D_preint, step) // correct the pre-integrated delta with correction step due to bias change
+ * err = diff(D_exp, D_corr) // compare against expected delta
+ * res = W.sqrt * err
+ *
+ * results in:
+ * res = W.sqrt * ( diff ( D_exp, (D_preint * retract(J * (b - b_preint) ) ) )
+ *
+ * Method 2:
+ * diff = diff(D_preint, D_exp) // compare pre-integrated against expected delta
+ * err = diff - step // the difference should be the correction step due to bias change
+ * res = W.sqrt * err
+ *
+ * results in :
+ * res = W.sqrt * ( diff ( D_preint , D_exp ) ) - J * (b - b_preint)
+ */
+#define METHOD_1
+
+
//needed typedefs
typedef typename D2::Scalar T;
@@ -240,56 +328,139 @@ inline bool ConstraintIMU::residual(const Eigen::MatrixBase<D1> & _p1,
imu::betweenStates(_p1, _q1, _v1, _p2, _q2, _v2, (T)dt_, dp_exp, dq_exp, dv_exp);
+
// 2. Corrected integrated delta: delta_corr = delta_preint (+) J_bias * (bias_current - bias_preint)
+
+ // 2.a. Compute the delta step in tangent space: step = J_bias * (bias - bias_preint)
+ Eigen::Matrix<T, 9, 1> d_step;
+
+ d_step.block(0,0,3,1) = dDp_dab_.cast<T>() * (_ab1 - acc_bias_preint_ .cast<T>()) + dDp_dwb_.cast<T>() * (_wb1 - gyro_bias_preint_.cast<T>());
+ d_step.block(3,0,3,1) = dDq_dwb_.cast<T>() * (_wb1 - gyro_bias_preint_.cast<T>());
+ d_step.block(6,0,3,1) = dDv_dab_.cast<T>() * (_ab1 - acc_bias_preint_ .cast<T>()) + dDv_dwb_.cast<T>() * (_wb1 - gyro_bias_preint_.cast<T>());
+
+#ifdef METHOD_1 // method 1
+ Eigen::Matrix<T, 3, 1> dp_step = d_step.block(0,0,3,1);
+ Eigen::Matrix<T, 3, 1> do_step = d_step.block(3,0,3,1);
+ Eigen::Matrix<T, 3, 1> dv_step = d_step.block(6,0,3,1);
+
+ // 2.b. Add the correction step to the preintegrated delta: delta_cor = delta_preint (+) step
Eigen::Matrix<T,3,1> dp_correct;
Eigen::Quaternion<T> dq_correct;
Eigen::Matrix<T,3,1> dv_correct;
- imu::plus(dp_preint_.cast<T>(),
- dq_preint_.cast<T>(),
- dv_preint_.cast<T>(),
- dDp_dab_.cast<T>() * (_ab1 - acc_bias_preint_ .cast<T>()) + dDp_dwb_.cast<T>() * (_wb1 - gyro_bias_preint_.cast<T>()),
- dDq_dwb_.cast<T>() * (_wb1 - gyro_bias_preint_.cast<T>()),
- dDv_dab_.cast<T>() * (_ab1 - acc_bias_preint_ .cast<T>()) + dDv_dwb_.cast<T>() * (_wb1 - gyro_bias_preint_.cast<T>()),
- dp_correct,
- dq_correct,
- dv_correct);
-
- // 3. Delta error in minimal form: d_min = log(delta_pred (-) delta_corr)
+ imu::plus(dp_preint_.cast<T>(), dq_preint_.cast<T>(), dv_preint_.cast<T>(),
+ dp_step, do_step, dv_step,
+ dp_correct, dq_correct, dv_correct);
+
+
+ // 3. Delta error in minimal form: d_min = lift(delta_pred (-) delta_corr)
// Note the Dt here is zero because it's the delta-time between the same time stamps!
- Eigen::Matrix<T, 9, 1> dpov_error;
- Eigen::Map<Eigen::Matrix<T, 3, 1> > dp_error(dpov_error.data() );
- Eigen::Map<Eigen::Matrix<T, 3, 1> > do_error(dpov_error.data() + 3);
- Eigen::Map<Eigen::Matrix<T, 3, 1> > dv_error(dpov_error.data() + 6);
+ Eigen::Matrix<T, 9, 1> d_error;
+ Eigen::Map<Eigen::Matrix<T, 3, 1> > dp_error(d_error.data() );
+ Eigen::Map<Eigen::Matrix<T, 3, 1> > do_error(d_error.data() + 3);
+ Eigen::Map<Eigen::Matrix<T, 3, 1> > dv_error(d_error.data() + 6);
+
+ Eigen::Matrix<T, 3, 3> J_do_dq1, J_do_dq2;
+ Eigen::Matrix<T, 9, 9> J_err_corr;
+//#define WITH_JAC
+#ifdef WITH_JAC
+ imu::diff(dp_exp, dq_exp, dv_exp, dp_correct, dq_correct, dv_correct, dp_error, do_error, dv_error, J_do_dq1, J_do_dq2);
+
+ J_err_corr.setIdentity();
+ J_err_corr.block(3,3,3,3) = J_do_dq2;
+
+ // 4. Residuals are the weighted errors
+ _res.head(9) = J_err_corr.inverse().transpose() * getMeasurementSquareRootInformationTransposed().cast<T>() * d_error;
+#else
imu::diff(dp_exp, dq_exp, dv_exp, dp_correct, dq_correct, dv_correct, dp_error, do_error, dv_error);
+ _res.head(9) = getMeasurementSquareRootInformationTransposed().cast<T>() * d_error;
+#endif
+
+#else // method 2
+
+ Eigen::Matrix<T, 9, 1> d_diff;
+ Eigen::Map<Eigen::Matrix<T, 3, 1> > dp_diff(d_diff.data() );
+ Eigen::Map<Eigen::Matrix<T, 3, 1> > do_diff(d_diff.data() + 3);
+ Eigen::Map<Eigen::Matrix<T, 3, 1> > dv_diff(d_diff.data() + 6);
+
+ imu::diff(dp_preint_.cast<T>(), dq_preint_.cast<T>(), dv_preint_.cast<T>(),
+ dp_exp, dq_exp, dv_exp,
+ dp_diff, do_diff, dv_diff);
+
+ Eigen::Matrix<T, 9, 1> d_error;
+ d_error << d_diff - d_step;
+
+ // 4. Residuals are the weighted errors
+ _res.head(9) = getMeasurementSquareRootInformationTransposed().cast<T>() * d_error;
+
+#endif
// Errors between biases
Eigen::Matrix<T,3,1> ab_error(_ab1 - _ab2); // KF1.bias - KF2.bias
Eigen::Matrix<T,3,1> wb_error(_wb1 - _wb2);
// 4. Residuals are the weighted errors
- _res.head(9) = getMeasurementSquareRootInformationTransposed().cast<T>() * dpov_error;
_res.segment(9,3) = sqrt_A_r_dt_inv.cast<T>() * ab_error;
_res.tail(3) = sqrt_W_r_dt_inv.cast<T>() * wb_error;
+
+ /////////////////////////////////////////////////////////////////////////////////////////////
+ /////////////////////////////////////////////////////////////////////////////////////////////
+#if 0
+ // print values -----------------------
+ Matrix<T, 10, 1> x1; x1 << _p1 , _q1.coeffs(), _v1;
+ Matrix<T, 10, 1> x2; x2 << _p2 , _q2.coeffs(), _v2;
+ print("x1 ", x1);
+ print("x2 ", x2);
+ Matrix<T, 6, 1> bp; bp << acc_bias_preint_.cast<T>(), gyro_bias_preint_.cast<T>();
+ Matrix<T, 6, 1> b1; b1 << _ab1, _wb1;
+ Matrix<T, 6, 1> b2; b2 << _ab2, _wb2;
+ print("bp ", bp);
+ print("b1 ", b1);
+ print("b2 ", b2);
+ Matrix<T, 10, 1> exp; exp << dp_exp , dq_exp.coeffs(), dv_exp;
+ print("D exp", exp);
+ Matrix<T, 10, 1> pre; pre << dp_preint_.cast<T>() , dq_preint_.cast<T>().coeffs(), dv_preint_.cast<T>();
+ print("D preint", pre);
+ Matrix<T, 9, 6> J_b_r0; J_b_r0.setZero();
+ J_b_r0.block(0,0,3,3) = dDp_dab_.cast<T>();
+ J_b_r0.block(0,3,3,3) = dDp_dwb_.cast<T>();
+ J_b_r0.block(3,3,3,3) = dDq_dwb_.cast<T>();
+ J_b_r0.block(6,0,3,3) = dDv_dab_.cast<T>();
+ J_b_r0.block(6,3,3,3) = dDv_dwb_.cast<T>();
+ print("J bias", J_b_r0);
+ print("D step", d_step);
+#ifndef METHOD_1
+ Matrix<T, 9, 1> dif; dif << dp_diff , do_diff, dv_diff;
+ print("D diff", dif);
+#endif
+ print("D err", d_error);
+ WOLF_TRACE("-----------------------------------------")
+#endif
+ /////////////////////////////////////////////////////////////////////////////////////////////
+ /////////////////////////////////////////////////////////////////////////////////////////////
+
+
+
return true;
}
+
inline Eigen::VectorXs ConstraintIMU::expectation() const
{
FrameBasePtr frm_current = getFeaturePtr()->getFramePtr();
FrameBasePtr frm_previous = getFrameOtherPtr();
//get information on current_frame in the ConstraintIMU
- const Eigen::Vector3s frame_current_pos = frm_current->getPPtr()->getState();
- const Eigen::Quaternions frame_current_ori(frm_current->getOPtr()->getState().data());
- const Eigen::Vector3s frame_current_vel = frm_current->getVPtr()->getState();
+ const Eigen::Vector3s frame_current_pos = (frm_current->getPPtr()->getState());
+ const Eigen::Quaternions frame_current_ori (frm_current->getOPtr()->getState().data());
+ const Eigen::Vector3s frame_current_vel = (frm_current->getVPtr()->getState());
// get info on previous frame in the ConstraintIMU
- const Eigen::Vector3s frame_previous_pos = (frm_previous->getPPtr()->getState());
+ const Eigen::Vector3s frame_previous_pos = (frm_previous->getPPtr()->getState());
const Eigen::Quaternions frame_previous_ori (frm_previous->getOPtr()->getState().data());
- const Eigen::Vector3s frame_previous_vel = (frm_previous->getVPtr()->getState());
+ const Eigen::Vector3s frame_previous_vel = (frm_previous->getVPtr()->getState());
// Define results vector and Map bits over it
Eigen::Matrix<wolf::Scalar, 10, 1> exp;
@@ -326,6 +497,8 @@ inline JacobianMethod ConstraintIMU::getJacobianMethod() const
}
+
+
} // namespace wolf
#endif
diff --git a/src/constraint_odom_3D.h b/src/constraint_odom_3D.h
index e1b6ac1e9ad1a253088e075eef689ea1ae416d9b..851433fab2fffb002f41663cd2a4319f007fcb77 100644
--- a/src/constraint_odom_3D.h
+++ b/src/constraint_odom_3D.h
@@ -93,6 +93,14 @@ inline ConstraintOdom3D::ConstraintOdom3D(const FeatureBasePtr& _ftr_current_ptr
_frame_past_ptr->getOPtr()) // past frame Q
{
setType("ODOM 3D");
+
+// WOLF_TRACE("Constr ODOM 3D (f", _ftr_current_ptr->id(),
+// " F", _ftr_current_ptr->getCapturePtr()->getFramePtr()->id(),
+// ") (Fo", _frame_past_ptr->id(), ")");
+//
+// WOLF_TRACE("delta preint: ", _ftr_current_ptr->getMeasurement().transpose());
+//
+// WOLF_TRACE("Omega_delta.sqrt: \n", _ftr_current_ptr->getMeasurementSquareRootInformationUpper());
//
}
diff --git a/src/feature_imu.cpp b/src/feature_imu.cpp
index d7ecf3b917dd2800f6bd2aba6d69ff1141607b93..b657039367682802ddfe7c285efa59e48a8c6b3a 100644
--- a/src/feature_imu.cpp
+++ b/src/feature_imu.cpp
@@ -2,39 +2,27 @@
namespace wolf {
-FeatureIMU::FeatureIMU(const Eigen::VectorXs& _delta_preintegrated, const Eigen::MatrixXs& _delta_preintegrated_covariance) :
- FeatureBase("IMU", _delta_preintegrated, _delta_preintegrated_covariance),
- dp_preint_(_delta_preintegrated.head<3>()),dv_preint_(_delta_preintegrated.tail<3>()), dq_preint_(_delta_preintegrated.segment<4>(3))
-{
- //std::cout << "New FeatureIMU: measurement " << _measurement.transpose() << std::endl << "covariance" << std::endl << _meas_covariance << std::endl;
-}
-
-FeatureIMU::FeatureIMU(const Eigen::VectorXs& _delta_preintegrated, const Eigen::MatrixXs& _delta_preintegrated_covariance, const Eigen::Vector3s& _acc_bias, const Eigen::Vector3s& _gyro_bias, const Eigen::Matrix<wolf::Scalar,9,6>& _dD_db_jacobians) :
+FeatureIMU::FeatureIMU(const Eigen::VectorXs& _delta_preintegrated,
+ const Eigen::MatrixXs& _delta_preintegrated_covariance,
+ const Eigen::Vector6s& _bias,
+ const Eigen::Matrix<wolf::Scalar,9,6>& _dD_db_jacobians,
+ CaptureMotionPtr _cap_imu_ptr) :
FeatureBase("IMU", _delta_preintegrated, _delta_preintegrated_covariance),
- dp_preint_(_delta_preintegrated.head<3>()),dv_preint_(_delta_preintegrated.tail<3>()), dq_preint_(_delta_preintegrated.segment<4>(3)),
- acc_bias_preint_(_acc_bias), gyro_bias_preint_(_gyro_bias),
+ acc_bias_preint_(_bias.head<3>()),
+ gyro_bias_preint_(_bias.tail<3>()),
jacobian_bias_(_dD_db_jacobians)
{
- //std::cout << "New FeatureIMU: measurement " << _measurement.transpose() << std::endl << "covariance" << std::endl << _meas_covariance << std::endl;
+ if (_cap_imu_ptr)
+ this->setCapturePtr(_cap_imu_ptr);
}
-FeatureIMU::FeatureIMU(const Eigen::VectorXs& _delta_preintegrated,
- const Eigen::MatrixXs& _delta_preintegrated_covariance,
- const wolf::CaptureIMUPtr _cap_imu_ptr,
- const Eigen::Matrix<wolf::Scalar,9,6>& _dD_db_jacobians):
- FeatureBase("IMU", _delta_preintegrated, _delta_preintegrated_covariance),
- jacobian_bias_(_dD_db_jacobians)
+FeatureIMU::FeatureIMU(CaptureMotionPtr _cap_imu_ptr):
+ FeatureBase("IMU", _cap_imu_ptr->getDeltaPreint(), _cap_imu_ptr->getDeltaPreintCov()),
+ acc_bias_preint_ (_cap_imu_ptr->getCalibrationPreint().head<3>()),
+ gyro_bias_preint_(_cap_imu_ptr->getCalibrationPreint().tail<3>()),
+ jacobian_bias_(_cap_imu_ptr->getJacobianCalib())
{
- //TODO : SIZE ASSERTIONS : make sure _delta_preintegrated and _delta_preintegrated_covariance sizes are correct !
-
this->setCapturePtr(_cap_imu_ptr);
-
- dp_preint_ = _delta_preintegrated.head<3>();
- dv_preint_ = _delta_preintegrated.tail<3>();
- dq_preint_ = _delta_preintegrated.segment<4>(3);
-
- acc_bias_preint_ = _cap_imu_ptr->getCalibrationPreint().head(3);
- gyro_bias_preint_ = _cap_imu_ptr->getCalibrationPreint().tail(3);
}
FeatureIMU::~FeatureIMU()
diff --git a/src/feature_imu.h b/src/feature_imu.h
index 72b99eb47319ba08294e0e29ee14a3746d3b9c04..83a3e8c4dae7488f23e397a3f8e5de41a32eb87d 100644
--- a/src/feature_imu.h
+++ b/src/feature_imu.h
@@ -14,61 +14,39 @@ namespace wolf {
//WOLF_PTR_TYPEDEFS(CaptureIMU);
WOLF_PTR_TYPEDEFS(FeatureIMU);
-//class FeatureIMU
class FeatureIMU : public FeatureBase
{
public:
- /** \brief Constructor from capture pointer and measure
- *
- * \param _measurement the measurement
- * \param _meas_covariance the noise of the measurement
- *
- */
- FeatureIMU(const Eigen::VectorXs& _delta_preintegrated, const Eigen::MatrixXs& _delta_preintegrated_covariance);
- /** \brief Constructor from capture pointer and measure
+ /** \brief Constructor from and measures
*
* \param _measurement the measurement
* \param _meas_covariance the noise of the measurement
* \param _dD_db_jacobians Jacobians of preintegrated delta wrt IMU biases
* \param acc_bias accelerometer bias of origin frame
* \param gyro_bias gyroscope bias of origin frame
- *
+ * \param _cap_imu_ptr pointer to parent CaptureMotion
*/
FeatureIMU(const Eigen::VectorXs& _delta_preintegrated,
const Eigen::MatrixXs& _delta_preintegrated_covariance,
- const Eigen::Vector3s& _acc_bias,
- const Eigen::Vector3s& _gyro_bias,
- const Eigen::Matrix<wolf::Scalar,9,6>& _dD_db_jacobians);
+ const Eigen::Vector6s& _bias,
+ const Eigen::Matrix<wolf::Scalar,9,6>& _dD_db_jacobians,
+ CaptureMotionPtr _cap_imu_ptr = nullptr);
- /** \brief Constructor from capture pointer and measure
- *
- * \param _measurement the measurement
- * \param _meas_covariance the noise of the measurement
- * \param _dD_db_jacobians Jacobians of preintegrated delta wrt IMU biases
- * \param _cap_imu_ptr is the CaptureIMUPtr pointing to desired capture containing the origin frame
+ /** \brief Constructor from capture pointer
*
+ * \param _cap_imu_ptr pointer to parent CaptureMotion
*/
- FeatureIMU(const Eigen::VectorXs& _delta_preintegrated,
- const Eigen::MatrixXs& _delta_preintegrated_covariance,
- const wolf::CaptureIMUPtr _cap_imu_ptr,
- const Eigen::Matrix<wolf::Scalar,9,6>& _dD_db_jacobians);
+ FeatureIMU(CaptureMotionPtr _cap_imu_ptr);
virtual ~FeatureIMU();
- const Eigen::Vector3s& getDpPreint() const;
- const Eigen::Quaternions& getDqPreint() const;
- const Eigen::Vector3s& getDvPreint() const;
const Eigen::Vector3s& getAccBiasPreint() const;
const Eigen::Vector3s& getGyroBiasPreint() const;
const Eigen::Matrix<Scalar, 9, 6>& getJacobianBias() const;
private:
- /// Preintegrated delta
- Eigen::Vector3s dp_preint_;
- Eigen::Vector3s dv_preint_;
- Eigen::Quaternions dq_preint_;
// Used biases
Eigen::Vector3s acc_bias_preint_; ///< Acceleration bias used for delta preintegration
@@ -76,22 +54,10 @@ class FeatureIMU : public FeatureBase
Eigen::Matrix<Scalar, 9, 6> jacobian_bias_;
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
};
-inline const Eigen::Vector3s& FeatureIMU::getDpPreint() const
-{
- return dp_preint_;
-}
-
-inline const Eigen::Quaternions& FeatureIMU::getDqPreint() const
-{
- return dq_preint_;
-}
-
-inline const Eigen::Vector3s& FeatureIMU::getDvPreint() const
-{
- return dv_preint_;
-}
inline const Eigen::Vector3s& FeatureIMU::getAccBiasPreint() const
{
diff --git a/src/imu_tools.h b/src/imu_tools.h
index 0e2ceff648556386d85cdeee3530137e7ccbcf6b..86900a8d6b80622366700201ab1e269015926576 100644
--- a/src/imu_tools.h
+++ b/src/imu_tools.h
@@ -79,6 +79,24 @@ inline void inverse(const MatrixBase<D1>& d,
inverse(dp, dq, dv, dt, idp, idq, idv);
}
+template<typename D1, typename D2, class T, typename D3>
+inline void inverse(const MatrixBase<D1>& d,
+ T dt,
+ MatrixBase<D2>& id,
+ MatrixBase<D3>& Jac)
+{
+ MatrixSizeCheck<10, 1>::check(d);
+ MatrixSizeCheck<10, 1>::check(id);
+ MatrixSizeCheck<9, 9>::check(Jac);
+
+ Map<const Quaternion<typename D1::Scalar> > dq ( & d( 3 ) );
+
+ inverse(d, dt, id);
+
+ Jac.setIdentity();
+ Jac.block<3,3>(3,3) = -q2R(dq); // d(R.tr) / dR = - R.tr
+}
+
template<typename D, class T>
inline Matrix<typename D::Scalar, 10, 1> inverse(const MatrixBase<D>& d,
T dt)
@@ -215,6 +233,7 @@ inline void between(const MatrixBase<D1>& d1,
between(dp1, dq1, dv1, dp2, dq2, dv2, dt, diff_p, diff_q, diff_v);
}
+
template<typename D1, typename D2, class T>
inline Matrix<typename D1::Scalar, 10, 1> between(const MatrixBase<D1>& d1,
const MatrixBase<D2>& d2,
@@ -400,6 +419,18 @@ inline void diff(const MatrixBase<D1>& dp1, const QuaternionBase<D2>& dq1, const
diff_v = dv2 - dv1;
}
+template<typename D1, typename D2, typename D3, typename D4, typename D5, typename D6, typename D7, typename D8, typename D9, typename D10, typename D11>
+inline void diff(const MatrixBase<D1>& dp1, const QuaternionBase<D2>& dq1, const MatrixBase<D3>& dv1,
+ const MatrixBase<D4>& dp2, const QuaternionBase<D5>& dq2, const MatrixBase<D6>& dv2,
+ MatrixBase<D7>& diff_p, MatrixBase<D8>& diff_o, MatrixBase<D9>& diff_v ,
+ MatrixBase<D10>& J_do_dq1, MatrixBase<D11>& J_do_dq2)
+{
+ diff(dp1, dq1, dv1, dp2, dq2, dv2, diff_p, diff_o, diff_v);
+
+ J_do_dq1 = - jac_SO3_left_inv(diff_o);
+ J_do_dq2 = jac_SO3_right_inv(diff_o);
+}
+
template<typename D1, typename D2, typename D3>
inline void diff(const MatrixBase<D1>& d1,
@@ -419,6 +450,46 @@ inline void diff(const MatrixBase<D1>& d1,
diff(dp1, dq1, dv1, dp2, dq2, dv2, diff_p, diff_o, diff_v);
}
+template<typename D1, typename D2, typename D3, typename D4, typename D5>
+inline void diff(const MatrixBase<D1>& d1,
+ const MatrixBase<D2>& d2,
+ MatrixBase<D3>& dif,
+ MatrixBase<D4>& J_diff_d1,
+ MatrixBase<D5>& J_diff_d2)
+{
+ Map<const Matrix<typename D1::Scalar, 3, 1> > dp1 ( & d1(0) );
+ Map<const Quaternion<typename D1::Scalar> > dq1 ( & d1(3) );
+ Map<const Matrix<typename D1::Scalar, 3, 1> > dv1 ( & d1(7) );
+ Map<const Matrix<typename D2::Scalar, 3, 1> > dp2 ( & d2(0) );
+ Map<const Quaternion<typename D2::Scalar> > dq2 ( & d2(3) );
+ Map<const Matrix<typename D2::Scalar, 3, 1> > dv2 ( & d2(7) );
+ Map<Matrix<typename D3::Scalar, 3, 1> > diff_p ( & dif(0) );
+ Map<Matrix<typename D3::Scalar, 3, 1> > diff_o ( & dif(3) );
+ Map<Matrix<typename D3::Scalar, 3, 1> > diff_v ( & dif(6) );
+
+ Matrix<typename D4::Scalar, 3, 3> J_do_dq1, J_do_dq2;
+
+ diff(dp1, dq1, dv1, dp2, dq2, dv2, diff_p, diff_o, diff_v, J_do_dq1, J_do_dq2);
+
+
+ /* d = diff(d1, d2) is
+ * dp = dp2 - dp1
+ * do = Log(dq1.conj * dq2)
+ * dv = dv2 - dv1
+ *
+ * With trivial Jacobians for dp and dv, and:
+ * J_do_dq1 = - J_l_inv(theta)
+ * J_do_dq2 = J_r_inv(theta)
+ */
+
+ J_diff_d1 = - Matrix<typename D4::Scalar, 9, 9>::Identity();// d(p2 - p1) / d(p1) = - Identity
+ J_diff_d1.block(3,3,3,3) = J_do_dq1; // d(R1.tr*R2) / d(R1) = - J_l_inv(theta)
+
+ J_diff_d2.setIdentity(); // d(R1.tr*R2) / d(R2) = Identity
+ J_diff_d2.block(3,3,3,3) = J_do_dq2; // d(R1.tr*R2) / d(R1) = J_r_inv(theta)
+
+}
+
template<typename D1, typename D2>
inline Matrix<typename D1::Scalar, 9, 1> diff(const MatrixBase<D1>& d1,
const MatrixBase<D2>& d2)
@@ -429,6 +500,64 @@ inline Matrix<typename D1::Scalar, 9, 1> diff(const MatrixBase<D1>& d1,
}
+template<typename D1, typename D2, typename D3, typename D4, typename D5>
+inline void body2delta(const MatrixBase<D1>& a,
+ const MatrixBase<D2>& w,
+ const typename D1::Scalar& dt,
+ MatrixBase<D3>& dp,
+ QuaternionBase<D4>& dq,
+ MatrixBase<D5>& dv)
+{
+ MatrixSizeCheck<3,1>::check(a);
+ MatrixSizeCheck<3,1>::check(w);
+ MatrixSizeCheck<3,1>::check(dp);
+ MatrixSizeCheck<3,1>::check(dv);
+
+ dp = 0.5 * a * dt * dt;
+ dq = exp_q(w * dt);
+ dv = a * dt;
+}
+
+template<typename D1>
+inline Matrix<typename D1::Scalar, 10, 1> body2delta(const MatrixBase<D1>& body,
+ const typename D1::Scalar& dt)
+{
+ MatrixSizeCheck<6,1>::check(body);
+
+ typedef typename D1::Scalar T;
+
+ Matrix<T, 10, 1> delta;
+
+ Map< Matrix<T, 3, 1>> dp ( & delta(0) );
+ Map< Quaternion<T>> dq ( & delta(3) );
+ Map< Matrix<T, 3, 1>> dv ( & delta(7) );
+
+ body2delta(body.block(0,0,3,1), body.block(3,0,3,1), dt, dp, dq, dv);
+
+ return delta;
+}
+
+template<typename D1, typename D2, typename D3>
+inline void body2delta(const MatrixBase<D1>& body,
+ const typename D1::Scalar& dt,
+ MatrixBase<D2>& delta,
+ MatrixBase<D3>& jac_body)
+{
+ MatrixSizeCheck<6,1>::check(body);
+ MatrixSizeCheck<9,6>::check(jac_body);
+
+ typedef typename D1::Scalar T;
+
+ delta = body2delta(body, dt);
+
+ Matrix<T, 3, 1> w = body.block(3,0,3,1);
+
+ jac_body.setZero();
+ jac_body.block(0,0,3,3) = 0.5 * dt * dt * Matrix<T, 3, 3>::Identity();
+ jac_body.block(3,3,3,3) = dt * jac_SO3_right(w * dt);
+ jac_body.block(6,0,3,3) = dt * Matrix<T, 3, 3>::Identity();
+}
+
} // namespace imu
} // namespace wolf
diff --git a/src/logging.h b/src/logging.h
index eea75fcc522654ab8d302634c432e267249be632..ea437b0942df836954adcfe68fe2b54017c3e04f 100644
--- a/src/logging.h
+++ b/src/logging.h
@@ -109,7 +109,8 @@ inline Logger::Logger(const std::string& name) :
// [thread num][hour:minutes:seconds.nanoseconds][log type] #log-content
//set_pattern("[%t][%H:%M:%S.%F][%l] %v");
// [log type][MM/DD/YY - hour:minutes:seconds.nanoseconds] #log-content
- set_pattern("[%l][%x - %H:%M:%S.%F] %v");
+// set_pattern("[%l][%x - %H:%M:%S.%F] %v");
+ set_pattern("[%l][%H:%M:%S] %v");
else
// Logging pattern is :
// [logger name][thread num][hour:minutes:seconds.nanoseconds][log type] #log-content
@@ -137,7 +138,8 @@ inline Logger::Logger(std::string&& name) :
// [thread num][hour:minutes:seconds.nanoseconds][log type] #log-content
//set_pattern("[%t][%H:%M:%S.%F][%l] %v");
// [log type][MM/DD/YY - hour:minutes:seconds.nanoseconds] #log-content
- set_pattern("[%l][%x - %H:%M:%S.%F] %v");
+// set_pattern("[%l][%x - %H:%M:%S.%F] %v");
+ set_pattern("[%l][%H:%M:%S] %v");
else
// Logging pattern is :
// [logger name][thread num][hour:minutes:seconds.nanoseconds][log type] #log-content
diff --git a/src/problem.cpp b/src/problem.cpp
index 0a0f6d37e04ab664789e1463213dcbc71d9884a1..fc92b0f7c20e7ad1b1c829216615c6ed80f5fc22 100644
--- a/src/problem.cpp
+++ b/src/problem.cpp
@@ -690,7 +690,8 @@ void Problem::print(int depth, bool constr_by, bool metric, bool state_blocks)
// Sensors
for (auto S : getHardwarePtr()->getSensorList())
{
- cout << " S" << S->id();
+ cout << " S" << S->id() << " " << S->getType();
+ cout << (S->isExtrinsicDynamic() ? " [Dyn," : " [Sta,") << (S->isIntrinsicDynamic() ? "Dyn]" : "Sta]");
if (depth < 2)
cout << " -- " << S->getProcessorList().size() << "p";
cout << endl;
@@ -709,7 +710,7 @@ void Problem::print(int depth, bool constr_by, bool metric, bool state_blocks)
{
if (p->isMotion())
{
- std::cout << " pm" << p->id() << std::endl;
+ std::cout << " pm" << p->id() << " " << p->getType() << endl;
ProcessorMotionPtr pm = std::static_pointer_cast<ProcessorMotion>(p);
if (pm->getOriginPtr())
cout << " o: C" << pm->getOriginPtr()->id() << " - F"
@@ -724,7 +725,7 @@ void Problem::print(int depth, bool constr_by, bool metric, bool state_blocks)
{
try
{
- cout << " pt" << p->id() << endl;
+ cout << " pt" << p->id() << " " << p->getType() << endl;
ProcessorTrackerPtr pt = std::static_pointer_cast<ProcessorTracker>(p);
if (pt->getOriginPtr())
cout << " o: C" << pt->getOriginPtr()->id() << " - F"
@@ -761,7 +762,7 @@ void Problem::print(int depth, bool constr_by, bool metric, bool state_blocks)
cout << endl;
if (metric)
{
- cout << (F->isFixed() ? " Fixed" : " Estim") << ", ts=" << std::setprecision(5)
+ cout << (F->isFixed() ? " Fix" : " Est") << ", ts=" << std::setprecision(5)
<< F->getTimeStamp().get();
cout << ",\t x = ( " << std::setprecision(2) << F->getState().transpose() << ")";
cout << endl;
@@ -779,35 +780,68 @@ void Problem::print(int depth, bool constr_by, bool metric, bool state_blocks)
// Captures
for (auto C : F->getCaptureList())
{
- cout << " C" << C->id();
+ cout << " C" << (C->isMotion() ? "M" : "") << C->id() << " " << C->getType();
if (C->getSensorPtr()) cout << " -> S" << C->getSensorPtr()->id();
else cout << " -> S-";
- cout << " [" << C->getType() << ", ";
+ cout << " [";
if(C->getSensorPtr() != nullptr)
+ cout << (C->getSensorPtr()->isExtrinsicDynamic() ? "Dyn, ": "Sta, ");
+
+ if(C->getSensorPtr() != nullptr)
+ cout << (C->getSensorPtr()->isIntrinsicDynamic() ? "Dyn]" : "Sta]");
+
+ cout << ((depth < 3) ? " -- " + std::to_string(C->getFeatureList().size()) + "f" : "");
+ if (constr_by)
{
- if(C->getSensorPtr()->isExtrinsicDynamic()) cout << "Dyn, ";
- else cout << "Sta, ";
+ cout << " <-- ";
+ for (auto cby : C->getConstrainedByList())
+ cout << "c" << cby->id() << " \t";
}
+ cout << endl;
- if(C->getSensorPtr() != nullptr)
+ if (state_blocks)
{
- if(C->getSensorPtr()->isIntrinsicDynamic()) cout << "Dyn | ";
- else cout << "Sta | ";
+ for(auto sb : C->getStateBlockVec())
+ {
+ cout << " sb: ";
+ if(sb != nullptr)
+ {
+ cout << (sb->isFixed() ? "Fix" : "Est");
+ if (metric)
+ cout << std::setprecision(3) << " (" << sb->getState().transpose() << ")";
+ }
+ else cout << "nullptr ";
+ cout << endl;
+ }
}
- for(auto cpt_sb : C->getStateBlockVec())
- if(cpt_sb != nullptr) cout << std::setprecision(3) << cpt_sb->getState().transpose() << " ";
- else cout << "nullptr ";
- cout << " ]";
+ if (C->isMotion() && metric)
+ {
+ try
+ {
+ CaptureMotionPtr CM = std::static_pointer_cast<CaptureMotion>(C);
+ if ( CM->getCalibSize() > 0 && ! CM->getBuffer().get().empty())
+ {
+ cout << " buffer size : " << CM->getBuffer().get().size() << endl;
+ cout << " delta preint : (" << CM->getDeltaPreint().transpose() << ")" << endl;
+ cout << " calib preint : (" << CM->getCalibrationPreint().transpose() << ")" << endl;
+ cout << " jacob preint : (" << CM->getJacobianCalib().row(0) << ")" << endl;
+ cout << " calib current: (" << CM->getCalibration().transpose() << ")" << endl;
+ cout << " delta correct: (" << CM->getDeltaCorrected(CM->getCalibration()).transpose() << ")" << endl;
+ }
+ }
+ catch (std::runtime_error& e)
+ {
+ }
+ }
- cout << ((depth < 3) ? " -- " + std::to_string(C->getFeatureList().size()) + "f" : "") << endl;
if (depth >= 3)
{
// Features
for (auto f : C->getFeatureList())
{
- cout << " f" << f->id() << ((depth < 4) ? " -- " + std::to_string(f->getConstraintList().size()) + "c " : "");
+ cout << " f" << f->id() << " " << f->getType() << ((depth < 4) ? " -- " + std::to_string(f->getConstraintList().size()) + "c " : "");
if (constr_by)
{
cout << " <--\t";
@@ -828,6 +862,8 @@ void Problem::print(int depth, bool constr_by, bool metric, bool state_blocks)
cout << " A";
if (c->getFrameOtherPtr() != nullptr)
cout << " F" << c->getFrameOtherPtr()->id();
+ if (c->getCaptureOtherPtr() != nullptr)
+ cout << " C" << c->getCaptureOtherPtr()->id();
if (c->getFeatureOtherPtr() != nullptr)
cout << " f" << c->getFeatureOtherPtr()->id();
if (c->getLandmarkOtherPtr() != nullptr)
@@ -847,7 +883,7 @@ void Problem::print(int depth, bool constr_by, bool metric, bool state_blocks)
// Landmarks
for (auto L : getMapPtr()->getLandmarkList())
{
- cout << " L" << L->id();
+ cout << " L" << L->id() << " " << L->getType();
if (constr_by)
{
cout << "\t<-- ";
@@ -993,6 +1029,17 @@ bool Problem::check(int verbose_level)
cout << endl;
cout << " -> P @ " << C->getProblem().get() << endl;
cout << " -> F" << C->getFramePtr()->id() << " @ " << C->getFramePtr().get() << endl;
+ for (auto sb : C->getStateBlockVec())
+ {
+ cout << " sb @ " << sb.get();
+ if (sb)
+ {
+ auto lp = sb->getLocalParametrizationPtr();
+ if (lp)
+ cout << " (lp @ " << lp.get() << ")";
+ }
+ cout << endl;
+ }
}
// check problem and frame pointers
is_consistent = is_consistent && (C->getProblem().get() == P_raw);
@@ -1025,10 +1072,11 @@ bool Problem::check(int verbose_level)
cout << " c" << c->id() << " @ " << c.get();
auto Fo = c->getFrameOtherPtr();
+ auto Co = c->getCaptureOtherPtr();
auto fo = c->getFeatureOtherPtr();
auto Lo = c->getLandmarkOtherPtr();
- if ( !Fo && !fo && !Lo ) // case ABSOLUTE:
+ if ( !Fo && !Co && !fo && !Lo ) // case ABSOLUTE:
{
if (verbose_level > 0)
cout << " --> Abs." << endl;
@@ -1052,6 +1100,24 @@ bool Problem::check(int verbose_level)
is_consistent = is_consistent && found;
}
+ // find constrained_by pointer in constrained capture
+ if ( Co ) // case CAPTURE:
+ {
+ if (verbose_level > 0)
+ cout << " --> C" << Co->id() << " <- ";
+ bool found = false;
+ for (auto cby : Co->getConstrainedByList())
+ {
+ if (verbose_level > 0)
+ cout << " c" << cby->id();
+ found = found || (c == cby);
+ }
+ if (verbose_level > 0)
+ cout << endl;
+ // check constrained_by pointer in constrained frame
+ is_consistent = is_consistent && found;
+ }
+
// find constrained_by pointer in constrained feature
if ( fo ) // case FEATURE:
{
@@ -1113,23 +1179,32 @@ bool Problem::check(int verbose_level)
}
// find in own Frame
found = found || (std::find(F->getStateBlockVec().begin(), F->getStateBlockVec().end(), sb) != F->getStateBlockVec().end());
+ // find in own Capture
+ found = found || (std::find(C->getStateBlockVec().begin(), C->getStateBlockVec().end(), sb) != C->getStateBlockVec().end());
// find in own Sensor
if (S)
found = found || (std::find(S->getStateBlockVec().begin(), S->getStateBlockVec().end(), sb) != S->getStateBlockVec().end());
// find in constrained Frame
if (Fo)
found = found || (std::find(Fo->getStateBlockVec().begin(), Fo->getStateBlockVec().end(), sb) != Fo->getStateBlockVec().end());
+ // find in constrained Capture
+ if (Co)
+ found = found || (std::find(Co->getStateBlockVec().begin(), Co->getStateBlockVec().end(), sb) != Co->getStateBlockVec().end());
+ // find in constrained Feature
if (fo)
{
// find in constrained feature's Frame
FrameBasePtr foF = fo->getFramePtr();
found = found || (std::find(foF->getStateBlockVec().begin(), foF->getStateBlockVec().end(), sb) != foF->getStateBlockVec().end());
+ // find in constrained feature's Capture
+ CaptureBasePtr foC = fo->getCapturePtr();
+ found = found || (std::find(foC->getStateBlockVec().begin(), foC->getStateBlockVec().end(), sb) != foC->getStateBlockVec().end());
// find in constrained feature's Sensor
SensorBasePtr foS = fo->getCapturePtr()->getSensorPtr();
found = found || (std::find(foS->getStateBlockVec().begin(), foS->getStateBlockVec().end(), sb) != foS->getStateBlockVec().end());
}
+ // find in constrained landmark
if (Lo)
- // find in constrained landmark
found = found || (std::find(Lo->getStateBlockVec().begin(), Lo->getStateBlockVec().end(), sb) != Lo->getStateBlockVec().end());
if (verbose_level > 0)
{
@@ -1205,4 +1280,20 @@ bool Problem::check(int verbose_level)
return is_consistent;
}
+void Problem::print(const std::string& depth, bool constr_by, bool metric, bool state_blocks)
+{
+ if (depth.compare("T") == 0)
+ print(0, constr_by, metric, state_blocks);
+ else if (depth.compare("F") == 0)
+ print(1, constr_by, metric, state_blocks);
+ else if (depth.compare("C") == 0)
+ print(2, constr_by, metric, state_blocks);
+ else if (depth.compare("f") == 0)
+ print(3, constr_by, metric, state_blocks);
+ else if (depth.compare("c") == 0)
+ print(4, constr_by, metric, state_blocks);
+ else
+ print(0, constr_by, metric, state_blocks);
+}
+
} // namespace wolf
diff --git a/src/problem.h b/src/problem.h
index d7ab78f3b1ae955886bf3588113dbdd06bfd9343..d46e1f6d6785c7f487e966a7ac9b24ae46e1017e 100644
--- a/src/problem.h
+++ b/src/problem.h
@@ -298,6 +298,7 @@ class Problem : public std::enable_shared_from_this<Problem>
* \param state_blocks : show state blocks
*/
void print(int depth = 4, bool constr_by = false, bool metric = true, bool state_blocks = false);
+ void print(const std::string& depth, bool constr_by = false, bool metric = true, bool state_blocks = false);
bool check(int verbose_level = 0);
};
diff --git a/src/processor_imu.cpp b/src/processor_imu.cpp
index 4f35ccebfab7fa59105fa1c40045055d45cab351..d6a1ce78cd0acd30b81502597a8e3c3e0794e036 100644
--- a/src/processor_imu.cpp
+++ b/src/processor_imu.cpp
@@ -86,6 +86,7 @@ Motion ProcessorIMU::interpolate(const Motion& _motion_ref, Motion& _motion_seco
* Covariances receive linear interpolation
* Q_ret = (ts - t_ref) / dt * Q_sec
*/
+ /*
// resolve out-of-bounds time stamp as if the time stamp was exactly on the bounds
if (_ts <= _motion_ref.ts_) // behave as if _ts == _motion_ref.ts_
{
@@ -97,7 +98,7 @@ Motion ProcessorIMU::interpolate(const Motion& _motion_ref, Motion& _motion_seco
motion_int.delta_cov_ . setZero();
return motion_int;
}
- if (_motion_second.ts_ <= _ts) // behave as if _ts == _motion_second.ts_
+ if (_ts >= _motion_second.ts_) // behave as if _ts == _motion_second.ts_
{
// return original second motion. Second motion becomes null motion
Motion motion_int ( _motion_second );
@@ -173,6 +174,10 @@ Motion ProcessorIMU::interpolate(const Motion& _motion_ref, Motion& _motion_seco
//_motion.delta_integr_cov_ = _motion.delta_integr_cov_; // trivial, just leave the code commented
return motion_int;
+ */
+
+ return _motion_ref;
+
}
CaptureMotionPtr ProcessorIMU::createCapture(const TimeStamp& _ts,
@@ -194,8 +199,7 @@ FeatureBasePtr ProcessorIMU::createFeature(CaptureMotionPtr _capture_motion)
FeatureIMUPtr key_feature_ptr = std::make_shared<FeatureIMU>(
_capture_motion->getBuffer().get().back().delta_integr_,
_capture_motion->getBuffer().get().back().delta_integr_cov_,
- _capture_motion->getBuffer().getCalibrationPreint().head(3),
- _capture_motion->getBuffer().getCalibrationPreint().tail(3),
+ _capture_motion->getBuffer().getCalibrationPreint(),
_capture_motion->getBuffer().get().back().jacobian_calib_);
return key_feature_ptr;
}
@@ -205,11 +209,120 @@ ConstraintBasePtr ProcessorIMU::emplaceConstraint(FeatureBasePtr _feature_motion
CaptureIMUPtr cap_imu = std::static_pointer_cast<CaptureIMU>(_capture_origin);
FeatureIMUPtr ftr_imu = std::static_pointer_cast<FeatureIMU>(_feature_motion);
ConstraintIMUPtr ctr_imu = std::make_shared<ConstraintIMU>(ftr_imu, cap_imu, shared_from_this());
+
+ // link ot wolf tree
_feature_motion->addConstraint(ctr_imu);
- cap_imu->getFramePtr()->addConstrainedBy(ctr_imu);
+ _capture_origin->addConstrainedBy(ctr_imu);
+ _capture_origin->getFramePtr()->addConstrainedBy(ctr_imu);
+
return ctr_imu;
}
+void ProcessorIMU::computeCurrentDelta(const Eigen::VectorXs& _data,
+ const Eigen::MatrixXs& _data_cov,
+ const Eigen::VectorXs& _calib,
+ const Scalar _dt,
+ Eigen::VectorXs& _delta,
+ Eigen::MatrixXs& _delta_cov,
+ Eigen::MatrixXs& _jac_delta_calib)
+{
+ assert(_data.size() == data_size_ && "Wrong data size!");
+
+ using namespace Eigen;
+ Matrix<Scalar, 9, 6> jac_data;
+
+ /*
+ * We have the following computing pipeline:
+ * Input: data, calib, dt
+ * Output: delta, delta_cov, jac_calib
+ *
+ * We do:
+ * body = data - calib
+ * delta = body2delta(body, dt) --> jac_body
+ * jac_data = jac_body
+ * jac_calib = - jac_body
+ * delta_cov <-- propagate data_cov using jac_data
+ *
+ * where body2delta(.) produces a delta from body=(a,w) as follows:
+ * dp = 1/2 * a * dt^2
+ * dq = exp(w * dt)
+ * dv = a * dt
+ */
+
+ // create delta
+ imu::body2delta(_data - _calib, _dt, _delta, jac_data); // Jacobians tested in imu_tools
+
+ // compute delta_cov
+ _delta_cov = jac_data * _data_cov * jac_data.transpose();
+
+ // compute jacobian_calib
+ _jac_delta_calib = - jac_data;
+
+}
+
+void ProcessorIMU::deltaPlusDelta(const Eigen::VectorXs& _delta_preint,
+ const Eigen::VectorXs& _delta,
+ const Scalar _dt,
+ Eigen::VectorXs& _delta_preint_plus_delta)
+{
+ /* MATHS according to Sola-16
+ * Dp' = Dp + Dv*dt + 1/2*Dq*(a-a_b)*dt^2 = Dp + Dv*dt + Dq*dp if dp = 1/2*(a-a_b)*dt^2
+ * Dv' = Dv + Dq*(a-a_b)*dt = Dv + Dq*dv if dv = (a-a_b)*dt
+ * Dq' = Dq * exp((w-w_b)*dt) = Dq * dq if dq = exp((w-w_b)*dt)
+ *
+ * where (dp, dq, dv) need to be computed in data2delta(), and Dq*dx =is_equivalent_to= Dq*dx*Dq'.
+ */
+ _delta_preint_plus_delta = imu::compose(_delta_preint, _delta, _dt);
+}
+
+void ProcessorIMU::statePlusDelta(const Eigen::VectorXs& _x,
+ const Eigen::VectorXs& _delta,
+ const Scalar _dt,
+ Eigen::VectorXs& _x_plus_delta)
+{
+ // assert(_x.size() == 10 && "Wrong _x vector size");
+ // assert(_delta.size() == 10 && "Wrong _delta vector size");
+ // assert(_x_plus_delta.size() == 10 && "Wrong _x_plus_delta vector size");
+ assert(_dt >= 0 && "Time interval _Dt is negative!");
+ VectorXs x(_x.head(10));
+ VectorXs x_plus_delta(10);
+ x_plus_delta = imu::composeOverState(x, _delta, _dt);
+ _x_plus_delta.head(10) = x_plus_delta;
+}
+
+void ProcessorIMU::deltaPlusDelta(const Eigen::VectorXs& _delta_preint,
+ const Eigen::VectorXs& _delta,
+ const Scalar _dt,
+ Eigen::VectorXs& _delta_preint_plus_delta,
+ Eigen::MatrixXs& _jacobian_delta_preint,
+ Eigen::MatrixXs& _jacobian_delta)
+{
+ /*
+ * Expression of the delta integration step, D' = D (+) d:
+ *
+ * Dp' = Dp + Dv*dt + Dq*dp
+ * Dv' = Dv + Dq*dv
+ * Dq' = Dq * dq
+ *
+ * Jacobians for covariance propagation.
+ *
+ * a. With respect to Delta, gives _jacobian_delta_preint = D_D as:
+ *
+ * D_D = [ I -DR*skew(dp) I*dt
+ * 0 dR.tr 0
+ * 0 -DR*skew(dv) I ] // See Sola-16
+ *
+ * b. With respect to delta, gives _jacobian_delta = D_d as:
+ *
+ * D_d = [ DR 0 0
+ * 0 I 0
+ * 0 0 DR ] // See Sola-16
+ *
+ * Note: covariance propagation, i.e., P+ = D_D * P * D_D' + D_d * M * D_d', is done in ProcessorMotion.
+ */
+ imu::compose(_delta_preint, _delta, _dt, _delta_preint_plus_delta, _jacobian_delta_preint, _jacobian_delta); // jacobians tested in imu_tools
+}
+
} // namespace wolf
diff --git a/src/processor_imu.h b/src/processor_imu.h
index b4f8f1e662ba32a3235902a8c5fc8cb21961e250..f46dcc423bd9038da49db1bf09ad2c59e7f79010 100644
--- a/src/processor_imu.h
+++ b/src/processor_imu.h
@@ -110,150 +110,6 @@ class ProcessorIMU : public ProcessorMotion{
namespace wolf{
-inline void ProcessorIMU::computeCurrentDelta(const Eigen::VectorXs& _data,
- const Eigen::MatrixXs& _data_cov,
- const Eigen::VectorXs& _calib,
- const Scalar _dt,
- Eigen::VectorXs& _delta,
- Eigen::MatrixXs& _delta_cov,
- Eigen::MatrixXs& _jacobian_calib)
-{
- assert(_data.size() == data_size_ && "Wrong data size!");
-
- using namespace Eigen;
-
- // acc and gyro measurements corrected with the estimated bias, times dt
- Vector3s a_dt = (_data.head(3) - _calib.head(3)) * _dt;
- Vector3s w_dt = (_data.tail(3) - _calib.tail(3)) * _dt;
-
- /* create delta
- *
- * MATHS of delta creation -- Sola-16
- * dp = 1/2 * (a-a_b) * dt^2 = 1/2 * dv * dt
- * dv = (a-a_b) * dt
- * dq = exp((w-w_b)*dt)
- */
- Vector3s delta_p = a_dt * _dt / 2;
- Quaternions delta_q = v2q(w_dt);
- Vector3s delta_v = a_dt;
- _delta << delta_p , delta_q.coeffs() , delta_v;
-
- /* Compute jacobian of delta wrt data
- *
- * MATHS : jacobian dd_dn, of delta wrt noise
- * substituting a and w respectively by (a+a_n) and (w+w_n) (measurement noise is additive)
- * an wn
- * dp [ 0.5*I*dt*dt 0 ]
- * dd_dn = do [ 0 Jr*dt ]
- * dv [ I*dt 0 ] // see Sola-16
- */
-
- // we go the sparse way:
- Matrix3s ddv_dan = Matrix3s::Identity() * _dt;
- Matrix3s ddp_dan = ddv_dan * _dt / 2;
- Matrix3s ddo_dwn = jac_SO3_right(w_dt) * _dt;
-
- /* Covariance computation
- *
- * Covariance is sparse:
- * [ Cpp 0 Cpv
- * COV = 0 Coo 0
- * Cpv' 0 Cvv ]
- *
- * where Cpp, Cpv, Coo and Cvv are computed below
- */
- _delta_cov.block<3,3>(0,0).noalias() = ddp_dan*_data_cov.block<3,3>(0,0)*ddp_dan.transpose(); // Cpp = ddp_dan * Caa * ddp_dan'
- _delta_cov.block<3,3>(0,6).noalias() = ddp_dan*_data_cov.block<3,3>(0,0)*ddv_dan.transpose(); // Cpv = ddp_dan * Caa * ddv_dan'
- _delta_cov.block<3,3>(3,3).noalias() = ddo_dwn*_data_cov.block<3,3>(3,3)*ddo_dwn.transpose(); // Coo = ddo_dwn * Cww * ddo_dwn'
- _delta_cov.block<3,3>(6,0) = _delta_cov.block<3,3>(0,6).transpose(); // Cvp = Cpv'
- _delta_cov.block<3,3>(6,6).noalias() = ddv_dan*_data_cov.block<3,3>(0,0)*ddv_dan.transpose(); // Cvv = ddv_dan * Caa * ddv_dan'
-
-
- /* Jacobians of delta wrt calibration parameters -- bias
- *
- * We know that d_(meas - bias)/d_bias = -I
- * so d_delta/d_bias = - d_delta/d_meas
- * we assign only the non-null ones
- */
- _jacobian_calib.setZero(delta_cov_size_,calib_size_); // can be commented usually, more sure this way
- _jacobian_calib.block(0,0,3,3) = - ddp_dan;
- _jacobian_calib.block(3,3,3,3) = - ddo_dwn;
- _jacobian_calib.block(6,0,3,3) = - ddv_dan;
-
-}
-
-inline void ProcessorIMU::deltaPlusDelta(const Eigen::VectorXs& _delta_preint,
- const Eigen::VectorXs& _delta,
- const Scalar _dt,
- Eigen::VectorXs& _delta_preint_plus_delta,
- Eigen::MatrixXs& _jacobian_delta_preint,
- Eigen::MatrixXs& _jacobian_delta)
-{
-
- /*
- * Expression of the delta integration step, D' = D (+) d:
- *
- * Dp' = Dp + Dv*dt + Dq*dp
- * Dv' = Dv + Dq*dv
- * Dq' = Dq * dq
- */
-
- /*/////////////////////////////////////////////////////////
- * Note. Jacobians for covariance propagation.
- *
- * 1.a. With respect to Delta, gives _jacobian_delta_preint = D_D as:
- *
- * D_D = [ I -DR*skew(dp) I*dt
- * 0 dR.tr 0
- * 0 -DR*skew(dv) I ] // See Sola-16
- *
- * 1.b. With respect to delta, gives _jacobian_delta = D_d as:
- *
- * D_d = [ DR 0 0
- * 0 I 0
- * 0 0 DR ] // See Sola-16
- *
- * Note: covariance propagation, i.e., P+ = D_D * P * D_D' + D_d * M * D_d', is done in ProcessorMotion.
- */
-
- imu::compose(_delta_preint, _delta, _dt, _delta_preint_plus_delta, _jacobian_delta_preint, _jacobian_delta);
-}
-
-inline void ProcessorIMU::deltaPlusDelta(const Eigen::VectorXs& _delta_preint,
- const Eigen::VectorXs& _delta,
- const Scalar _dt,
- Eigen::VectorXs& _delta_preint_plus_delta)
-{
-
- /* MATHS according to Sola-16
- * Dp' = Dp + Dv*dt + 1/2*Dq*(a-a_b)*dt^2 = Dp + Dv*dt + Dq*dp if dp = 1/2*(a-a_b)*dt^2
- * Dv' = Dv + Dq*(a-a_b)*dt = Dv + Dq*dv if dv = (a-a_b)*dt
- * Dq' = Dq * exp((w-w_b)*dt) = Dq * dq if dq = exp((w-w_b)*dt)
- *
- * where (dp, dq, dv) need to be computed in data2delta(), and Dq*dx =is_equivalent_to= Dq*dx*Dq'.
- */
-
- imu::compose(_delta_preint, _delta, _delta_preint_plus_delta);
-
-}
-
-inline void ProcessorIMU::statePlusDelta(const Eigen::VectorXs& _x,
- const Eigen::VectorXs& _delta,
- const Scalar _dt,
- Eigen::VectorXs& _x_plus_delta)
-{
-// assert(_x.size() == 10 && "Wrong _x vector size");
-// assert(_delta.size() == 10 && "Wrong _delta vector size");
-// assert(_x_plus_delta.size() == 10 && "Wrong _x_plus_delta vector size");
- assert(_dt >= 0 && "Time interval _Dt is negative!");
-
- VectorXs x( _x.head(10) );
- VectorXs x_plus_delta(10);
-
- x_plus_delta = imu::composeOverState(x, _delta, _dt);
- _x_plus_delta.head(10) = x_plus_delta;
-}
-
inline Eigen::VectorXs ProcessorIMU::deltaZero() const
{
return (Eigen::VectorXs(10) << 0,0,0, 0,0,0,1, 0,0,0 ).finished(); // p, q, v
diff --git a/src/processor_imu_UnitTester.cpp b/src/processor_imu_UnitTester.cpp
index fa1b49c060514de69ea33b0413ca7c1131ac244d..4716356e6415f3ed3b803c6ebb68cdd3a8b5036e 100644
--- a/src/processor_imu_UnitTester.cpp
+++ b/src/processor_imu_UnitTester.cpp
@@ -9,13 +9,5 @@ ProcessorIMU_UnitTester::ProcessorIMU_UnitTester() :
ProcessorIMU_UnitTester::~ProcessorIMU_UnitTester(){}
-ProcessorBasePtr ProcessorIMU_UnitTester::create(const std::string& _unique_name, const ProcessorParamsBasePtr _params, const SensorBasePtr)
-{
- ProcessorIMUPtr prc_ptr = std::make_shared<ProcessorIMU>();
- prc_ptr->setName(_unique_name);
- return prc_ptr;
-}
-
-
} // namespace wolf
diff --git a/src/processor_imu_UnitTester.h b/src/processor_imu_UnitTester.h
index b0a714ee18216b349888fcae8fef8e8b78652e25..2c70cec4ac2570229dc4169072e9ad9586829990 100644
--- a/src/processor_imu_UnitTester.h
+++ b/src/processor_imu_UnitTester.h
@@ -9,9 +9,23 @@
namespace wolf {
struct IMU_jac_bias{ //struct used for checking jacobians by finite difference
- IMU_jac_bias(Eigen::Matrix<Eigen::VectorXs,6,1> _Deltas_noisy_vect, Eigen::VectorXs _Delta0 , Eigen::Matrix3s _dDp_dab, Eigen::Matrix3s _dDv_dab,
- Eigen::Matrix3s _dDp_dwb, Eigen::Matrix3s _dDv_dwb, Eigen::Matrix3s _dDq_dwb) : Deltas_noisy_vect_(_Deltas_noisy_vect), Delta0_(_Delta0) ,
- dDp_dab_(_dDp_dab), dDv_dab_(_dDv_dab), dDp_dwb_(_dDp_dwb), dDv_dwb_(_dDv_dwb), dDq_dwb_(_dDq_dwb){}
+ IMU_jac_bias(Eigen::Matrix<Eigen::VectorXs,6,1> _Deltas_noisy_vect,
+ Eigen::VectorXs _Delta0 ,
+ Eigen::Matrix3s _dDp_dab,
+ Eigen::Matrix3s _dDv_dab,
+ Eigen::Matrix3s _dDp_dwb,
+ Eigen::Matrix3s _dDv_dwb,
+ Eigen::Matrix3s _dDq_dwb) :
+ Deltas_noisy_vect_(_Deltas_noisy_vect),
+ Delta0_(_Delta0) ,
+ dDp_dab_(_dDp_dab),
+ dDv_dab_(_dDv_dab),
+ dDp_dwb_(_dDp_dwb),
+ dDv_dwb_(_dDv_dwb),
+ dDq_dwb_(_dDq_dwb)
+ {
+ //
+ }
IMU_jac_bias(){
@@ -30,7 +44,7 @@ namespace wolf {
IMU_jac_bias(IMU_jac_bias const & toCopy){
Deltas_noisy_vect_ = toCopy.Deltas_noisy_vect_;
- Delta0_ = toCopy.Delta0_;
+ Delta0_ = toCopy.Delta0_;
dDp_dab_ = toCopy.dDp_dab_;
dDv_dab_ = toCopy.dDv_dab_;
dDp_dwb_ = toCopy.dDp_dwb_;
@@ -56,7 +70,7 @@ namespace wolf {
void copyfrom(IMU_jac_bias const& right){
Deltas_noisy_vect_ = right.Deltas_noisy_vect_;
- Delta0_ = right.Delta0_;
+ Delta0_ = right.Delta0_;
dDp_dab_ = right.dDp_dab_;
dDv_dab_ = right.dDv_dab_;
dDp_dwb_ = right.dDp_dwb_;
@@ -67,10 +81,21 @@ namespace wolf {
struct IMU_jac_deltas{
- IMU_jac_deltas(Eigen::VectorXs _Delta0, Eigen::VectorXs _delta0, Eigen::Matrix<Eigen::VectorXs,9,1> _Delta_noisy_vect, Eigen::Matrix<Eigen::VectorXs,9,1> _delta_noisy_vect,
- Eigen::MatrixXs _jacobian_delta_preint, Eigen::MatrixXs _jacobian_delta ) :
- Delta0_(_Delta0), delta0_(_delta0), Delta_noisy_vect_(_Delta_noisy_vect), delta_noisy_vect_(_delta_noisy_vect),
- jacobian_delta_preint_(_jacobian_delta_preint), jacobian_delta_(_jacobian_delta) {}
+ IMU_jac_deltas(Eigen::VectorXs _Delta0,
+ Eigen::VectorXs _delta0,
+ Eigen::Matrix<Eigen::VectorXs,9,1> _Delta_noisy_vect,
+ Eigen::Matrix<Eigen::VectorXs,9,1> _delta_noisy_vect,
+ Eigen::MatrixXs _jacobian_delta_preint,
+ Eigen::MatrixXs _jacobian_delta ) :
+ Delta0_(_Delta0),
+ delta0_(_delta0),
+ Delta_noisy_vect_(_Delta_noisy_vect),
+ delta_noisy_vect_(_delta_noisy_vect),
+ jacobian_delta_preint_(_jacobian_delta_preint),
+ jacobian_delta_(_jacobian_delta)
+ {
+ //
+ }
IMU_jac_deltas(){
for (int i=0; i<9; i++){
@@ -114,12 +139,12 @@ namespace wolf {
public:
void copyfrom(IMU_jac_deltas const& right){
- Delta_noisy_vect_ = right.Delta_noisy_vect_;
- delta_noisy_vect_ = right.delta_noisy_vect_;
- Delta0_ = right.Delta0_;
- delta0_ = right.delta0_;
- jacobian_delta_preint_ = right.jacobian_delta_preint_;
- jacobian_delta_ = right.jacobian_delta_;
+ Delta_noisy_vect_ = right.Delta_noisy_vect_;
+ delta_noisy_vect_ = right.delta_noisy_vect_;
+ Delta0_ = right.Delta0_;
+ delta0_ = right.delta0_;
+ jacobian_delta_preint_ = right.jacobian_delta_preint_;
+ jacobian_delta_ = right.jacobian_delta_;
}
};
@@ -137,7 +162,10 @@ namespace wolf {
_dt : time interval between 2 IMU measurements
da_b : bias noise to add - scalar because adding the same noise to each component of bias (abx, aby, abz, wbx, wby, wbz) one by one.
*/
- IMU_jac_bias finite_diff_ab(const Scalar _dt, Eigen::Vector6s& _data, const wolf::Scalar& da_b, const Eigen::Matrix<wolf::Scalar,10,1>& _delta_preint0);
+ IMU_jac_bias finite_diff_ab(const Scalar _dt,
+ Eigen::Vector6s& _data,
+ const wolf::Scalar& da_b,
+ const Eigen::Matrix<wolf::Scalar,10,1>& _delta_preint0);
/* params :
_data : input data vector (size 6 : ax,ay,az,wx,wy,wz)
@@ -145,10 +173,16 @@ namespace wolf {
_Delta_noise : noise to add to Delta_preint (D1 in D = D1 + d), vector 9 because rotation expressed as a vector (R2v(q.matrix()))
_delta_noise : noise to add to instantaneous delta (d in D = D1 + d), vector 9 because rotation expressed as a vector (R2v(q.matrix()))
*/
- IMU_jac_deltas finite_diff_noise(const Scalar& _dt, Eigen::Vector6s& _data, const Eigen::Matrix<wolf::Scalar,9,1>& _Delta_noise, const Eigen::Matrix<wolf::Scalar,9,1>& _delta_noise, const Eigen::Matrix<wolf::Scalar,10,1>& _Delta0);
+ IMU_jac_deltas finite_diff_noise(const Scalar& _dt,
+ Eigen::Vector6s& _data,
+ const Eigen::Matrix<wolf::Scalar,9,1>& _Delta_noise,
+ const Eigen::Matrix<wolf::Scalar,9,1>& _delta_noise,
+ const Eigen::Matrix<wolf::Scalar,10,1>& _Delta0);
public:
- static ProcessorBasePtr create(const std::string& _unique_name, const ProcessorParamsBasePtr _params, const SensorBasePtr = nullptr);
+ static ProcessorBasePtr create(const std::string& _unique_name,
+ const ProcessorParamsBasePtr _params,
+ const SensorBasePtr = nullptr);
public:
// Maps quat, to be used as temporary
@@ -170,7 +204,10 @@ namespace wolf {
namespace wolf{
//Functions to test jacobians with finite difference method
-inline IMU_jac_bias ProcessorIMU_UnitTester::finite_diff_ab(const Scalar _dt, Eigen::Vector6s& _data, const wolf::Scalar& da_b, const Eigen::Matrix<wolf::Scalar,10,1>& _delta_preint0)
+inline IMU_jac_bias ProcessorIMU_UnitTester::finite_diff_ab(const Scalar _dt,
+ Eigen::Vector6s& _data,
+ const wolf::Scalar& da_b,
+ const Eigen::Matrix<wolf::Scalar,10,1>& _delta_preint0)
{
//TODO : need to use a reset function here to make sure jacobians have not been used before --> reset everything
///Define all the needed variables
@@ -225,7 +262,7 @@ inline IMU_jac_bias ProcessorIMU_UnitTester::finite_diff_ab(const Scalar _dt, Ei
_data = data0;
_data(i) = _data(i) - da_b; //- because a = a_m − a_b + a_n, in out case, a = a_m − a_b - da_b + a_n
//data2delta
- computeCurrentDelta(_data, data_cov, bias, _dt,delta_,delta_cov_,jacobian_delta_calib_);
+ computeCurrentDelta(_data, data_cov, bias, _dt, delta_, delta_cov_, jacobian_delta_calib_);
deltaPlusDelta(_delta_preint0, delta_, _dt, delta_preint_plus_delta0, jacobian_delta_preint, jacobian_delta);
Deltas_noisy_vect(i) = delta_preint_plus_delta0; //preintegrated deltas affected by added bias noise
}
diff --git a/src/processor_motion.cpp b/src/processor_motion.cpp
index b33f06087a6e0d98b00da00a274e3d65139f9a26..f00fbfb36d3d3b55d1f49259622dd07589ee0427 100644
--- a/src/processor_motion.cpp
+++ b/src/processor_motion.cpp
@@ -48,7 +48,6 @@ void ProcessorMotion::process(CaptureBasePtr _incoming_ptr)
if (status_ == IDLE)
{
-// std::cout << "PM: IDLE" << std::endl;
TimeStamp t0 = _incoming_ptr->getTimeStamp();
if (origin_ptr_ == nullptr)
@@ -69,7 +68,6 @@ void ProcessorMotion::process(CaptureBasePtr _incoming_ptr)
}
}
status_ = RUNNING;
-// std::cout << "PM: RUNNING" << std::endl;
}
incoming_ptr_ = getIncomingCaptureMotion(_incoming_ptr);
@@ -106,17 +104,21 @@ void ProcessorMotion::process(CaptureBasePtr _incoming_ptr)
getCurrentState(),
getCurrentTimeStamp());
// create a new capture
- CaptureMotionPtr new_capture_ptr = emplaceCapture(key_frame_ptr->getTimeStamp(),
+ CaptureMotionPtr new_capture_ptr = emplaceCapture(new_frame_ptr,
getSensorPtr(),
+ key_frame_ptr->getTimeStamp(),
Eigen::VectorXs::Zero(data_size_),
Eigen::MatrixXs::Zero(data_size_, data_size_),
- new_frame_ptr,
+ last_ptr_->getCalibration(),
+ last_ptr_->getCalibration(),
key_frame_ptr);
// reset the new buffer
+ new_capture_ptr->getBuffer().get().clear();
new_capture_ptr->getBuffer().get().push_back( motionZero(key_frame_ptr->getTimeStamp()) ) ;
-
// reset integrals
+ delta_ = deltaZero();
+ delta_cov_.setZero();
delta_integrated_ = deltaZero();
delta_integrated_cov_.setZero();
jacobian_calib_.setZero();
@@ -183,51 +185,49 @@ void ProcessorMotion::setOrigin(FrameBasePtr _origin_frame)
&& "ProcessorMotion::setOrigin: origin frame must be in the trajectory.");
assert(_origin_frame->isKey() && "ProcessorMotion::setOrigin: origin frame must be KEY FRAME.");
+ // -------- ORIGIN ---------
// emplace (empty) origin Capture
- origin_ptr_ = emplaceCapture(_origin_frame->getTimeStamp(),
+ origin_ptr_ = emplaceCapture(_origin_frame,
getSensorPtr(),
+ _origin_frame->getTimeStamp(),
Eigen::VectorXs::Zero(data_size_),
Eigen::MatrixXs::Zero(data_size_, data_size_),
- _origin_frame,
+ getSensorPtr()->getCalibration(),
+ getSensorPtr()->getCalibration(),
nullptr);
+ // ---------- LAST ----------
// Make non-key-frame for last Capture
FrameBasePtr new_frame_ptr = getProblem()->emplaceFrame(NON_KEY_FRAME,
_origin_frame->getState(),
_origin_frame->getTimeStamp());
// emplace (emtpy) last Capture
- last_ptr_ = emplaceCapture(_origin_frame->getTimeStamp(),
+ last_ptr_ = emplaceCapture(new_frame_ptr,
getSensorPtr(),
+ _origin_frame->getTimeStamp(),
Eigen::VectorXs::Zero(data_size_),
Eigen::MatrixXs::Zero(data_size_, data_size_),
- new_frame_ptr,
+ getSensorPtr()->getCalibration(),
+ getSensorPtr()->getCalibration(),
_origin_frame);
- /* Status:
- * KF --- F ---
- * o l i
- */
+ // clear and reset buffer
+ getBuffer().get().clear();
+ getBuffer().get().push_back(motionZero(_origin_frame->getTimeStamp()));
- // Reset deltas
+ // Reset integrals
delta_ = deltaZero();
- delta_integrated_ = deltaZero();
delta_cov_.setZero();
+ delta_integrated_ = deltaZero();
delta_integrated_cov_.setZero();
jacobian_calib_.setZero();
- // clear and reset buffer
- getBuffer().get().clear();
- getBuffer().setCalibrationPreint(origin_ptr_->getCalibration());
- getBuffer().get().push_back(motionZero(_origin_frame->getTimeStamp()));
-
// Reset derived things
resetDerived();
}
bool ProcessorMotion::keyFrameCallback(FrameBasePtr _new_keyframe, const Scalar& _time_tol_other)
{
-// std::cout << "PM: KF" << _new_keyframe->id() << " callback received at ts= " << _new_keyframe->getTimeStamp().get() << std::endl;
-// std::cout << " while last ts= " << last_ptr_->getTimeStamp().get() << std::endl;
assert(_new_keyframe->getTrajectoryPtr() != nullptr
&& "ProcessorMotion::keyFrameCallback: key frame must be in the trajectory.");
@@ -235,26 +235,27 @@ bool ProcessorMotion::keyFrameCallback(FrameBasePtr _new_keyframe, const Scalar&
// get keyframe's time stamp
TimeStamp new_ts = _new_keyframe->getTimeStamp();
- // find capture whose buffer is affected by the new keyframe
+ // find the capture whose buffer is affected by the new keyframe
CaptureMotionPtr existing_capture = findCaptureContainingTimeStamp(new_ts);
assert(existing_capture != nullptr
&& "ProcessorMotion::keyFrameCallback: no motion capture containing the required TimeStamp found");
// Find the frame acting as the capture's origin
- FrameBasePtr new_keyframe_origin = existing_capture->getOriginFramePtr();
+ FrameBasePtr keyframe_origin = existing_capture->getOriginFramePtr();
// emplace a new motion capture to the new keyframe
- CaptureMotionPtr new_capture = emplaceCapture(new_ts,
+ CaptureMotionPtr new_capture = emplaceCapture(_new_keyframe,
getSensorPtr(),
+ new_ts,
Eigen::VectorXs::Zero(data_size_),
- Eigen::MatrixXs::Zero(data_size_, data_size_),
- _new_keyframe,
- new_keyframe_origin);
+ existing_capture->getDataCovariance(),
+ existing_capture->getCalibration(),
+ existing_capture->getCalibration(),
+ keyframe_origin);
// split the buffer
- // and give the old buffer to the key_capture
+ // and give the part of the buffer before the new keyframe to the key_capture
existing_capture->getBuffer().split(new_ts, new_capture->getBuffer());
- new_capture->getBuffer().setCalibrationPreint(new_capture->getCalibration());
// interpolate individual delta
if (!existing_capture->getBuffer().get().empty() && new_capture->getBuffer().get().back().ts_ != new_ts)
@@ -271,7 +272,7 @@ bool ProcessorMotion::keyFrameCallback(FrameBasePtr _new_keyframe, const Scalar&
FeatureBasePtr new_feature = emplaceFeature(new_capture);
// create motion constraint and add it to the feature, and constrain to the other capture (origin)
- emplaceConstraint(new_feature, new_keyframe_origin->getCaptureOf(getSensorPtr())); // XXX it was new_keyframe_origin
+ emplaceConstraint(new_feature, keyframe_origin->getCaptureOf(getSensorPtr()) ); // XXX it was new_keyframe_origin
@@ -332,7 +333,10 @@ void ProcessorMotion::integrateOneStep()
// integrate Jacobian wrt calib
if (calib_size_ > 0)
+ {
jacobian_calib_ = jacobian_delta_preint_ * getBuffer().get().back().jacobian_calib_ + jacobian_delta_ * jacobian_delta_calib_;
+ // WOLF_TRACE("jac calib: ", jacobian_calib_.row(0));
+ }
// Integrate covariance
delta_integrated_cov_ = jacobian_delta_preint_ * getBuffer().get().back().delta_integr_cov_ * jacobian_delta_preint_.transpose()
@@ -356,6 +360,8 @@ void ProcessorMotion::reintegrateBuffer(CaptureMotionPtr _capture_ptr)
// start with empty motion
_capture_ptr->getBuffer().get().push_front(motionZero(_capture_ptr->getOriginFramePtr()->getTimeStamp()));
+ VectorXs calib = _capture_ptr->getCalibrationPreint();
+
// Iterate all the buffer
auto motion_it = _capture_ptr->getBuffer().get().begin();
auto prev_motion_it = motion_it;
@@ -366,8 +372,6 @@ void ProcessorMotion::reintegrateBuffer(CaptureMotionPtr _capture_ptr)
const Scalar dt = motion_it->ts_ - prev_motion_it->ts_;
// re-convert data to delta with the new calibration parameters
- VectorXs calib = _capture_ptr->getBuffer().getCalibrationPreint();
-
computeCurrentDelta(motion_it->data_,
motion_it->data_cov_,
calib,
@@ -430,11 +434,13 @@ CaptureMotionPtr ProcessorMotion::getCaptureMotionContainingTimeStamp(const Time
return capture_motion;
}
-CaptureMotionPtr ProcessorMotion::emplaceCapture(const TimeStamp& _ts,
+CaptureMotionPtr ProcessorMotion::emplaceCapture(const FrameBasePtr& _frame_own,
const SensorBasePtr& _sensor,
+ const TimeStamp& _ts,
const VectorXs& _data,
const MatrixXs& _data_cov,
- const FrameBasePtr& _frame_own,
+ const VectorXs& _calib,
+ const VectorXs& _calib_preint,
const FrameBasePtr& _frame_origin)
{
CaptureMotionPtr capture = createCapture(_ts,
@@ -443,16 +449,8 @@ CaptureMotionPtr ProcessorMotion::emplaceCapture(const TimeStamp& _ts,
_data_cov,
_frame_origin);
- // Only assign calibration params to this Capture if these parameters are dynamic
- if (getSensorPtr()->isExtrinsicDynamic() || getSensorPtr()->isIntrinsicDynamic())
- {
- if (origin_ptr_)
- // get calib params from origin capture
- capture->setCalibration(origin_ptr_->getCalibration());
- else
- // get calib params from sensor
- capture->setCalibration(getSensorPtr()->getCalibration());
- }
+ capture->setCalibration(_calib);
+ capture->setCalibrationPreint(_calib_preint);
// add to wolf tree
_frame_own->addCapture(capture);
diff --git a/src/processor_motion.h b/src/processor_motion.h
index 5e88ba9a070b98674a0d625f83dc4bb508ae94b6..51e2fdb491f19d3291a170a6cf7306f069491f13 100644
--- a/src/processor_motion.h
+++ b/src/processor_motion.h
@@ -371,11 +371,13 @@ class ProcessorMotion : public ProcessorBase
* \param _frame_own frame owning the Capture to create
* \param _frame_origin frame acting as the origin of motions for the MorionBuffer of the created MotionCapture
*/
- CaptureMotionPtr emplaceCapture(const TimeStamp& _ts,
+ CaptureMotionPtr emplaceCapture(const FrameBasePtr& _frame_own,
const SensorBasePtr& _sensor,
+ const TimeStamp& _ts,
const VectorXs& _data,
const MatrixXs& _data_cov,
- const FrameBasePtr& _frame_own,
+ const VectorXs& _calib,
+ const VectorXs& _calib_preint,
const FrameBasePtr& _frame_origin);
virtual CaptureMotionPtr createCapture(const TimeStamp& _ts,
diff --git a/src/processor_odom_3D.cpp b/src/processor_odom_3D.cpp
index 8a1eee69cbc863434d6269784c0a74692d053528..8dcfa2b61e8c279d400c4f5d093d83d933bb2639 100644
--- a/src/processor_odom_3D.cpp
+++ b/src/processor_odom_3D.cpp
@@ -12,6 +12,8 @@ ProcessorOdom3D::ProcessorOdom3D(ProcessorOdom3DParamsPtr _params, SensorOdom3DP
q1_(nullptr), q2_(nullptr), q_out_(nullptr)
{
setup(_sensor_ptr);
+ delta_ = deltaZero();
+ delta_integrated_ = deltaZero();
jacobian_delta_preint_.setZero(delta_cov_size_, delta_cov_size_);
jacobian_delta_.setZero(delta_cov_size_, delta_cov_size_);
}
@@ -293,14 +295,14 @@ bool ProcessorOdom3D::voteForKeyFrame()
return true;
}
// distance traveled
- Scalar dist = delta_integrated_.head(3).norm();
+ Scalar dist = getMotion().delta_integr_.head(3).norm();
if (dist > dist_traveled_)
{
WOLF_DEBUG( "PM: vote: distance traveled" );
return true;
}
// angle turned
- Scalar angle = 2.0 * acos(delta_integrated_(6));
+ Scalar angle = q2v(Quaternions(getMotion().delta_integr_.data()+3)).norm(); // 2.0 * acos(getMotion().delta_integr_(6));
if (angle > angle_turned_)
{
WOLF_DEBUG( "PM: vote: angle turned" );
diff --git a/src/rotations.h b/src/rotations.h
index caa57f0d442e7a0c3afc66b1315a16a6ba8aebe0..35776c25c9b5121cfe81f1b22b216b5fbfef9efd 100644
--- a/src/rotations.h
+++ b/src/rotations.h
@@ -71,9 +71,9 @@ inline Eigen::Matrix<typename Derived::Scalar, 3, 3> skew(const Eigen::MatrixBas
Eigen::Matrix<T, 3, 3> sk;
- sk << 0.0 , -_v(2), +_v(1),
- +_v(2), 0.0 , -_v(0),
- -_v(1), +_v(0), 0.0;
+ sk << (T)0.0 , -_v(2), +_v(1),
+ +_v(2), (T)0.0, -_v(0),
+ -_v(1), +_v(0), (T)0.0;
return sk;
}
@@ -354,7 +354,7 @@ inline Eigen::Matrix<typename Derived::Scalar, 3, 3> jac_SO3_right_inv(const Eig
return Eigen::Matrix<T, 3, 3>::Identity() + (T)0.5 * W; // Small angle approximation
T theta = sqrt(theta2); // rotation angle
Eigen::Matrix<T, 3, 3> M;
- M.noalias() = ((T)1 / theta2 - (1 + cos(theta)) / ((T)2 * theta * sin(theta))) * (W * W);
+ M.noalias() = ((T)1.0 / theta2 - ((T)1.0 + cos(theta)) / ((T)2.0 * theta * sin(theta))) * (W * W);
return Eigen::Matrix<T, 3, 3>::Identity() + (T)0.5 * W + M; //is this really more optimized?
}
@@ -426,28 +426,62 @@ inline Eigen::Matrix<typename Derived::Scalar, 3, 3> jac_SO3_left_inv(const Eige
return Eigen::Matrix<T, 3, 3>::Identity() + (T)0.5 * W; // Small angle approximation
T theta = sqrt(theta2); // rotation angle
Eigen::Matrix<T, 3, 3> M;
- M.noalias() = ((T)1 / theta2 - (1 + cos(theta)) / ((T)2 * theta * sin(theta))) * (W * W);
+ M.noalias() = ((T)1.0 / theta2 - ((T)1.0 + cos(theta)) / ((T)2.0 * theta * sin(theta))) * (W * W);
return Eigen::Matrix<T, 3, 3>::Identity() - (T)0.5 * W + M; //is this really more optimized?
}
+template<typename D1, typename D2, typename D3, typename D4, typename D5>
+inline void compose(const Eigen::QuaternionBase<D1>& _q1,
+ const Eigen::QuaternionBase<D2>& _q2,
+ Eigen::QuaternionBase<D3>& _q_comp,
+ Eigen::MatrixBase<D4>& _J_comp_q1,
+ Eigen::MatrixBase<D5>& _J_comp_q2)
+{
+ MatrixSizeCheck<3, 3>::check(_J_comp_q1);
+ MatrixSizeCheck<3, 3>::check(_J_comp_q2);
+
+ _q_comp = _q1 * _q2;
+
+ _J_comp_q1 = q2R(_q2.conjugate()); // R2.tr
+ _J_comp_q2 . setIdentity();
+}
+
+template<typename D1, typename D2, typename D3, typename D4, typename D5>
+inline void between(const Eigen::QuaternionBase<D1>& _q1,
+ const Eigen::QuaternionBase<D2>& _q2,
+ Eigen::QuaternionBase<D3>& _q_between,
+ Eigen::MatrixBase<D4>& _J_between_q1,
+ Eigen::MatrixBase<D5>& _J_between_q2)
+{
+ MatrixSizeCheck<3, 3>::check(_J_between_q1);
+ MatrixSizeCheck<3, 3>::check(_J_between_q2);
+
+ _q_between = _q1.conjugate() * _q2;
+
+ _J_between_q1 = -q2R(_q2.conjugate()*_q1); // - R2.tr * R1
+ _J_between_q2 . setIdentity();
+}
+
template<typename T>
inline Eigen::Matrix<T, 3, 3> matrixRollPitchYaw(const T roll,
const T pitch,
const T yaw)
{
- const Eigen::AngleAxis<T> ax = Eigen::AngleAxis<T>(roll, Eigen::Matrix<T, 3, 1>::UnitX());
- const Eigen::AngleAxis<T> ay = Eigen::AngleAxis<T>(pitch, Eigen::Matrix<T, 3, 1>::UnitY());
- const Eigen::AngleAxis<T> az = Eigen::AngleAxis<T>(yaw, Eigen::Matrix<T, 3, 1>::UnitZ());
+ const Eigen::AngleAxis<T> ax = Eigen::AngleAxis<T>(roll, Eigen::Matrix<T, 3, 1>::UnitX());
+ const Eigen::AngleAxis<T> ay = Eigen::AngleAxis<T>(pitch, Eigen::Matrix<T, 3, 1>::UnitY());
+ const Eigen::AngleAxis<T> az = Eigen::AngleAxis<T>(yaw, Eigen::Matrix<T, 3, 1>::UnitZ());
- return (az * ay * ax).toRotationMatrix().matrix();
+ return (az * ay * ax).toRotationMatrix().matrix();
}
template <typename Derived>
inline typename Eigen::MatrixBase<Derived>::Scalar
-getYaw(const Eigen::MatrixBase<Derived>& r)
+getYaw(const Eigen::MatrixBase<Derived>& R)
{
- using std::atan2;
- return atan2( r(1, 0), r(0, 0) );
+ MatrixSizeCheck<3, 3>::check(R);
+
+ using std::atan2;
+ return atan2( R(1, 0), R(0, 0) );
}
} // namespace wolf
diff --git a/src/sensor_base.cpp b/src/sensor_base.cpp
index d78c08c1f45fc1fdc3e313882ba3ccc473bb3078..fc88b489e679f4f22acb159600c2fddb20974c9e 100644
--- a/src/sensor_base.cpp
+++ b/src/sensor_base.cpp
@@ -25,6 +25,8 @@ SensorBase::SensorBase(const std::string& _type,
noise_std_(_noise_size),
noise_cov_(_noise_size, _noise_size)
{
+ noise_std_.setZero();
+ noise_cov_.setZero();
state_block_vec_[0] = _p_ptr;
state_block_vec_[1] = _o_ptr;
state_block_vec_[2] = _intr_ptr;
@@ -53,9 +55,7 @@ SensorBase::SensorBase(const std::string& _type,
state_block_vec_[0] = _p_ptr;
state_block_vec_[1] = _o_ptr;
state_block_vec_[2] = _intr_ptr;
- noise_cov_.setZero();
- for (unsigned int i = 0; i < _noise_std.size(); i++)
- noise_cov_(i, i) = noise_std_(i) * noise_std_(i);
+ setNoiseStd(_noise_std);
updateCalibSize();
}
@@ -203,11 +203,19 @@ void SensorBase::registerNewStateBlocks()
}
}
-void SensorBase::setNoise(const Eigen::VectorXs& _noise_std) {
- noise_std_ = _noise_std;
- noise_cov_.setZero();
- for (unsigned int i=0; i<_noise_std.size(); i++)
- noise_cov_(i,i) = _noise_std(i) * _noise_std(i);
+void SensorBase::setNoiseStd(const Eigen::VectorXs& _noise_std) {
+ noise_std_ = _noise_std;
+ noise_cov_.setZero();
+ for (unsigned int i=0; i<_noise_std.size(); i++)
+ {
+ Scalar var_i = _noise_std(i) * _noise_std(i);
+ noise_cov_(i,i) = var_i;
+ }
+}
+
+void SensorBase::setNoiseCov(const Eigen::MatrixXs& _noise_cov) {
+ noise_std_ = _noise_cov.diagonal().array().sqrt();
+ noise_cov_ = _noise_cov;
}
CaptureBasePtr SensorBase::lastCapture(const TimeStamp& _ts)
diff --git a/src/sensor_base.h b/src/sensor_base.h
index 0ee9736e74902f726d6a9ce29e67ea04d6f03e80..07eb80a0db04a61df97557ae7e0013be853713cf 100644
--- a/src/sensor_base.h
+++ b/src/sensor_base.h
@@ -145,7 +145,8 @@ class SensorBase : public NodeBase, public std::enable_shared_from_this<SensorBa
bool extrinsicsInCaptures() const { return extrinsic_dynamic_ && has_capture_; }
bool intrinsicsInCaptures() const { return intrinsic_dynamic_ && has_capture_; }
- void setNoise(const Eigen::VectorXs & _noise_std);
+ void setNoiseStd(const Eigen::VectorXs & _noise_std);
+ void setNoiseCov(const Eigen::MatrixXs & _noise_std);
Eigen::VectorXs getNoiseStd();
Eigen::MatrixXs getNoiseCov();
diff --git a/src/sensor_odom_3D.cpp b/src/sensor_odom_3D.cpp
index e65d2eb907feb4c07801152a76ee2998b98ee4e3..21acafbbb484f81cbedf94376e370e6504fa9b5b 100644
--- a/src/sensor_odom_3D.cpp
+++ b/src/sensor_odom_3D.cpp
@@ -21,6 +21,7 @@ SensorOdom3D::SensorOdom3D(StateBlockPtr _p_ptr, StateQuaternionPtr _o_ptr, Intr
min_rot_var_(params->min_rot_var)
{
noise_cov_ = (Eigen::Vector6s() << min_disp_var_, min_disp_var_, min_disp_var_, min_rot_var_, min_rot_var_, min_rot_var_).finished().asDiagonal();
+ setNoiseCov(noise_cov_); // sets also noise_std_
}
SensorOdom3D::~SensorOdom3D()
diff --git a/src/test/gtest_constraint_imu.cpp b/src/test/gtest_constraint_imu.cpp
index 2b3dcc789431ee0ead33f650a8f66544d542bc73..33702a760048897b6781b6b4bd4a429429248d38 100644
--- a/src/test/gtest_constraint_imu.cpp
+++ b/src/test/gtest_constraint_imu.cpp
@@ -101,7 +101,7 @@ class ConstraintIMU_biasTest_Static_NullBias : public testing::Test
Eigen::Vector6s data_imu;
data_imu << -wolf::gravity(), 0,0,0;
- wolf::CaptureIMUPtr imu_ptr = std::make_shared<CaptureIMU>(t, sen_imu, data_imu, Eigen::Matrix6s::Identity(), origin_bias); //set data on IMU (measure only gravity here)
+ wolf::CaptureIMUPtr imu_ptr = std::make_shared<CaptureIMU>(t, sen_imu, data_imu, sen_imu->getNoiseCov(), origin_bias); //set data on IMU (measure only gravity here)
for(unsigned int i = 0; i < 1000; i++) //integrate during 1 second
{
@@ -170,7 +170,7 @@ class ConstraintIMU_biasTest_Static_NonNullAccBias : public testing::Test
x_origin.resize(10);
x_origin << 0,0,0, 0,0,0,1, 0,0,0;
t.set(0);
- origin_bias << 0.002, 0.005, 0.1, 0,0,0;
+ origin_bias << 0.02, 0.05, 0.1, 0,0,0;
expected_final_state = x_origin; //null bias + static
@@ -185,7 +185,7 @@ class ConstraintIMU_biasTest_Static_NonNullAccBias : public testing::Test
data_imu << -wolf::gravity(), 0,0,0;
data_imu = data_imu + origin_bias;
- wolf::CaptureIMUPtr imu_ptr = std::make_shared<CaptureIMU>(t, sen_imu, data_imu, Eigen::Matrix6s::Identity(), origin_bias); //set data on IMU (measure only gravity here)
+ wolf::CaptureIMUPtr imu_ptr = std::make_shared<CaptureIMU>(t, sen_imu, data_imu, sen_imu->getNoiseCov(), origin_bias); //set data on IMU (measure only gravity here)
for(unsigned int i = 0; i < 1000; i++) //integrate during 1 second
{
@@ -236,9 +236,9 @@ class ConstraintIMU_biasTest_Static_NonNullGyroBias : public testing::Test
// CERES WRAPPER
ceres::Solver::Options ceres_options;
- ceres_options.minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;ceres::LINE_SEARCH
- ceres_options.max_line_search_step_contraction = 1e-3;
- ceres_options.max_num_iterations = 1e4;
+// ceres_options.minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;ceres::LINE_SEARCH
+// ceres_options.max_line_search_step_contraction = 1e-3;
+// ceres_options.max_num_iterations = 1e4;
ceres_manager_wolf_diff = new CeresManager(wolf_problem_ptr_, ceres_options);
// SENSOR + PROCESSOR IMU
@@ -254,9 +254,7 @@ class ConstraintIMU_biasTest_Static_NonNullGyroBias : public testing::Test
x_origin.resize(10);
x_origin << 0,0,0, 0,0,0,1, 0,0,0;
t.set(0);
- origin_bias << 0, 0, 0, 0.07,-0.035,-0.1;
- origin_bias *= 1e-2;
- //origin_bias << 0.002, 0.005, 0.1, 0.07,-0.035,-0.1;
+ origin_bias << 0, 0, 0, 0.0002, 0.0005, 0.001;
expected_final_state = x_origin; //null bias + static,
@@ -271,7 +269,7 @@ class ConstraintIMU_biasTest_Static_NonNullGyroBias : public testing::Test
data_imu << -wolf::gravity(), 0,0,0;
data_imu = data_imu + origin_bias;
- wolf::CaptureIMUPtr imu_ptr = std::make_shared<CaptureIMU>(t, sen_imu, data_imu, Eigen::Matrix6s::Identity() );//, origin_bias); //set data on IMU (measure only gravity here)
+ wolf::CaptureIMUPtr imu_ptr = std::make_shared<CaptureIMU>(t, sen_imu, data_imu, sen_imu->getNoiseCov() );//, origin_bias); //set data on IMU (measure only gravity here)
for(unsigned int i = 0; i < 1000; i++) //integrate during 1 second
{
@@ -357,7 +355,7 @@ class ConstraintIMU_biasTest_Static_NonNullBias : public testing::Test
data_imu << -wolf::gravity(), 0,0,0;
data_imu = data_imu + origin_bias;
- wolf::CaptureIMUPtr imu_ptr = std::make_shared<CaptureIMU>(t, sen_imu, data_imu, Eigen::Matrix6s::Identity(), Eigen::Vector6s::Zero()); //set data on IMU (measure only gravity here)
+ wolf::CaptureIMUPtr imu_ptr = std::make_shared<CaptureIMU>(t, sen_imu, data_imu, sen_imu->getNoiseCov(), Eigen::Vector6s::Zero()); //set data on IMU (measure only gravity here)
for(unsigned int i = 0; i < 1000; i++) //integrate during 1 second
{
@@ -445,7 +443,7 @@ class ConstraintIMU_biasTest_Move_NullBias : public testing::Test
//===================================================== PROCESS DATA
// PROCESS DATA
- wolf::CaptureIMUPtr imu_ptr = std::make_shared<CaptureIMU>(t, sen_imu, data_imu, Eigen::Matrix6s::Identity(), Eigen::Vector6s::Zero());
+ wolf::CaptureIMUPtr imu_ptr = std::make_shared<CaptureIMU>(t, sen_imu, data_imu, sen_imu->getNoiseCov(), Eigen::Vector6s::Zero());
for(unsigned int i = 0; i < 1000; i++) //integrate during 1 second
{
@@ -528,7 +526,7 @@ class ConstraintIMU_biasTest_Move_NonNullBias : public testing::Test
//===================================================== PROCESS DATA
// PROCESS DATA
- wolf::CaptureIMUPtr imu_ptr = std::make_shared<CaptureIMU>(t, sen_imu, data_imu, Eigen::Matrix6s::Identity(), Eigen::Vector6s::Zero());
+ wolf::CaptureIMUPtr imu_ptr = std::make_shared<CaptureIMU>(t, sen_imu, data_imu, sen_imu->getNoiseCov(), Eigen::Vector6s::Zero());
for(unsigned int i = 0; i < 1000; i++) //integrate during 1 second
{
@@ -617,7 +615,7 @@ class ConstraintIMU_biasTest_Move_NonNullBiasRotCst : public testing::Test
//===================================================== END{INITIALIZATION}
//===================================================== PROCESS DATA
- wolf::CaptureIMUPtr imu_ptr = std::make_shared<CaptureIMU>(t, sen_imu, data_imu, Eigen::Matrix6s::Identity(), Eigen::Vector6s::Zero());
+ wolf::CaptureIMUPtr imu_ptr = std::make_shared<CaptureIMU>(t, sen_imu, data_imu, sen_imu->getNoiseCov(), Eigen::Vector6s::Zero());
for(unsigned int i = 0; i < 1000; i++) //integrate during 1 second
{
@@ -716,7 +714,7 @@ class ConstraintIMU_biasTest_Move_NonNullBiasRotAndVCst : public testing::Test
//===================================================== PROCESS DATA
// PROCESS DATA
- wolf::CaptureIMUPtr imu_ptr = std::make_shared<CaptureIMU>(t, sen_imu, data_imu, Eigen::Matrix6s::Identity(), Eigen::Vector6s::Zero());
+ wolf::CaptureIMUPtr imu_ptr = std::make_shared<CaptureIMU>(t, sen_imu, data_imu, sen_imu->getNoiseCov(), Eigen::Vector6s::Zero());
for(unsigned int i = 0; i < 1000; i++) //integrate during 1 second
{
@@ -790,10 +788,9 @@ class ConstraintIMU_biasTest_Move_NonNullBiasRot : public testing::Test
expected_final_state.resize(10);
x_origin.resize(10);
x_origin << 0,0,0, 0,0,0,1, 0,0,0;
- origin_bias << 0.0015, 0.004, -0.002, 0.005, -0.0074, -0.003;
- origin_bias *= 0.01;
+ origin_bias << 0.01, 0.02, 0.003, 0.002, 0.005, 0.01;
t.set(0);
- Eigen::Quaternions current_quatState(Eigen::Quaternions::Identity());
+ Eigen::Quaternions quat_current(Eigen::Quaternions::Identity());
//set origin of the problem
origin_KF = processor_ptr_imu->setOrigin(x_origin, t);
@@ -807,20 +804,20 @@ class ConstraintIMU_biasTest_Move_NonNullBiasRot : public testing::Test
Scalar dt(0.001);
TimeStamp ts(0);
- wolf::CaptureIMUPtr imu_ptr = std::make_shared<CaptureIMU>(ts, sen_imu, data_imu, 0.01*Eigen::Matrix6s::Identity(), Eigen::Vector6s::Zero());
+ wolf::CaptureIMUPtr imu_ptr = std::make_shared<CaptureIMU>(ts, sen_imu, data_imu, sen_imu->getNoiseCov(), Eigen::Vector6s::Zero());
while( ts.get() < 1 )
{
// PROCESS IMU DATA
// Time and data variables
- ts.set(ts.get() + dt);
+ ts += dt;
- rateOfTurn = Eigen::Vector3s::Random()*10; //to have rate of turn > 0 deg/s
- data_imu.tail(3) = rateOfTurn* M_PI/180.0;
- data_imu.head(3) = current_quatState.conjugate() * (- wolf::gravity()); //gravity measured, we have no other translation movement
+ rateOfTurn << .1, .2, .3; //to have rate of turn > 0 deg/s
+ data_imu.head(3) = quat_current.conjugate() * (- wolf::gravity()); //gravity measured, we have no other translation movement
+ data_imu.tail(3) = rateOfTurn;
//compute current orientaton taking this measure into account
- current_quatState = current_quatState * wolf::v2q(data_imu.tail(3)*dt);
+ quat_current = quat_current * wolf::v2q(rateOfTurn*dt);
//set timestamp, add bias, set data and process
imu_ptr->setTimeStamp(ts);
@@ -829,7 +826,7 @@ class ConstraintIMU_biasTest_Move_NonNullBiasRot : public testing::Test
sen_imu->process(imu_ptr);
}
- expected_final_state << 0,0,0, current_quatState.x(), current_quatState.y(), current_quatState.z(), current_quatState.w(), 0,0,0;
+ expected_final_state << 0,0,0, quat_current.x(), quat_current.y(), quat_current.z(), quat_current.w(), 0,0,0;
last_KF = wolf_problem_ptr_->getTrajectoryPtr()->closestKeyFrameToTimeStamp(ts);
last_KF->setState(expected_final_state);
@@ -845,15 +842,18 @@ class ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRot : public testing::Test
{
public:
wolf::TimeStamp t;
- SensorIMUPtr sen_imu;
- SensorOdom3DPtr sen_odom3D;
- ProblemPtr wolf_problem_ptr_;
- CeresManagerPtr ceres_manager_wolf_diff;
- ProcessorBasePtr processor_ptr_;
- ProcessorIMUPtr processor_ptr_imu;
- ProcessorOdom3DPtr processor_ptr_odom3D;
+ ProblemPtr problem;
+ CeresManagerPtr ceres_manager;
+ SensorBasePtr sensor;
+ SensorIMUPtr sensor_imu;
+ SensorOdom3DPtr sensor_odo;
+ ProcessorBasePtr processor;
+ ProcessorIMUPtr processor_imu;
+ ProcessorOdom3DPtr processor_odo;
FrameBasePtr origin_KF;
FrameBasePtr last_KF;
+ CaptureIMUPtr capture_imu;
+ CaptureOdom3DPtr capture_odo;
Eigen::Vector6s origin_bias;
Eigen::VectorXs expected_final_state;
Eigen::VectorXs x_origin;
@@ -868,111 +868,156 @@ class ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRot : public testing::Test
//===================================================== SETTING PROBLEM
// WOLF PROBLEM
- wolf_problem_ptr_ = Problem::create("POV 3D");
+ problem = Problem::create("POV 3D");
// CERES WRAPPER
ceres::Solver::Options ceres_options;
ceres_options.minimizer_type = ceres::TRUST_REGION;
-// ceres_options.max_line_search_step_contraction = 1e-3;
-// ceres_options.max_num_iterations = 1e4;
- ceres_manager_wolf_diff = std::make_shared<CeresManager>(wolf_problem_ptr_, ceres_options);
+ ceres_manager = std::make_shared<CeresManager>(problem, ceres_options);
// SENSOR + PROCESSOR IMU
- SensorBasePtr sen0_ptr = wolf_problem_ptr_->installSensor("IMU", "Main IMU", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/src/examples/sensor_imu.yaml");
- processor_ptr_ = wolf_problem_ptr_->installProcessor("IMU", "IMU pre-integrator", "Main IMU", wolf_root + "/src/examples/processor_imu_t6.yaml");
- sen_imu = std::static_pointer_cast<SensorIMU>(sen0_ptr);
- processor_ptr_imu = std::static_pointer_cast<ProcessorIMU>(processor_ptr_);
+ sensor = problem->installSensor("IMU", "Main IMU", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/src/examples/sensor_imu.yaml");
+ sensor_imu = std::static_pointer_cast<SensorIMU>(sensor);
+ processor = problem->installProcessor("IMU", "IMU pre-integrator", "Main IMU", wolf_root + "/src/examples/processor_imu_t6.yaml");
+ processor_imu = std::static_pointer_cast<ProcessorIMU>(processor);
// SENSOR + PROCESSOR ODOM 3D
- SensorBasePtr sen1_ptr = wolf_problem_ptr_->installSensor("ODOM 3D", "odom", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/src/examples/sensor_odom_3D_HQ.yaml");
+ sensor = problem->installSensor("ODOM 3D", "odom", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/src/examples/sensor_odom_3D_HQ.yaml");
+ sensor_odo = std::static_pointer_cast<SensorOdom3D>(sensor);
+
+ sensor_imu->setNoiseStd((sensor_imu->getNoiseStd()/10.0).eval());
+ sensor_odo->setNoiseStd((sensor_odo->getNoiseStd()/10.0).eval());
+ WOLF_TRACE("IMU cov: ", sensor_imu->getNoiseCov().diagonal().transpose());
+ WOLF_TRACE("ODO cov: ", sensor_odo->getNoiseCov().diagonal().transpose());
+
ProcessorOdom3DParamsPtr prc_odom3D_params = std::make_shared<ProcessorOdom3DParams>();
- prc_odom3D_params->max_time_span = 0.9999;
- prc_odom3D_params->max_buff_length = 1000000000; //make it very high so that this condition will not pass
- prc_odom3D_params->dist_traveled = 1000000000;
- prc_odom3D_params->angle_turned = 1000000000;
+ prc_odom3D_params->max_time_span = 0.0099;
+ prc_odom3D_params->max_buff_length = 1000; //make it very high so that this condition will not pass
+ prc_odom3D_params->dist_traveled = 1000;
+ prc_odom3D_params->angle_turned = 1000;
- ProcessorBasePtr processor_ptr_odom = wolf_problem_ptr_->installProcessor("ODOM 3D", "odom", sen1_ptr, prc_odom3D_params);
- sen_odom3D = std::static_pointer_cast<SensorOdom3D>(sen1_ptr);
- processor_ptr_odom3D = std::static_pointer_cast<ProcessorOdom3D>(processor_ptr_odom);
+ processor = problem->installProcessor("ODOM 3D", "odom", sensor_odo, prc_odom3D_params);
+ processor_odo = std::static_pointer_cast<ProcessorOdom3D>(processor);
- WOLF_TRACE("IMU noise cov: ", sen0_ptr->getNoiseCov());
- WOLF_TRACE("ODO noise cov: ", sen1_ptr->getNoiseCov());
-
//===================================================== END{SETTING PROBLEM}
//===================================================== INITIALIZATION
- expected_final_state.resize(10);
x_origin.resize(10);
x_origin << 0,0,0, 0,0,0,1, 0,0,0;
origin_bias << 0.0015, 0.004, -0.002, 0.005, -0.0074, -0.003;
+// origin_bias /= 100;
t.set(0);
- Eigen::Quaternions odom_quat(Eigen::Quaternions::Identity());
- Eigen::Quaternions current_quatState(Eigen::Quaternions::Identity());
//set origin of the problem
- origin_KF = processor_ptr_imu->setOrigin(x_origin, t);
- processor_ptr_odom3D->setOrigin(origin_KF);
+ origin_KF = processor_imu->setOrigin(x_origin, t);
+ processor_odo->setOrigin(origin_KF);
//===================================================== END{INITIALIZATION}
//===================================================== PROCESS DATA
// PROCESS DATA
- Eigen::Vector6s data_imu(Eigen::Vector6s::Zero()), data_odom3D(Eigen::Vector6s::Zero());
+ Eigen::Vector6s data_imu(Eigen::Vector6s::Zero()),
+ data_odo(Eigen::Vector6s::Zero());
Eigen::Vector3s rateOfTurn(Eigen::Vector3s::Zero()); //deg/s
- Scalar dt_imu(0.001), dt_odo(1.0);
TimeStamp t_imu(0.0), t_odo(0.0);
- wolf::CaptureIMUPtr imu_ptr = std::make_shared<CaptureIMU> (t_imu, sen_imu, data_imu, Eigen::Matrix6s::Identity()*1e-6, Eigen::Vector6s::Zero());
- wolf::CaptureOdom3DPtr odo_ptr = std::make_shared<CaptureOdom3D>(t_odo, sen_odom3D, data_odom3D, nullptr);
- sen_odom3D->process(odo_ptr);
- //first odometry data will be processed at this timestamp
- t_odo += dt_odo;
+ Scalar dt_imu(0.001), dt_odo(.01);
- //when we find a IMU timestamp corresponding with this odometry timestamp then we process odometry measurement
+ capture_imu = std::make_shared<CaptureIMU> (t_imu, sensor_imu, data_imu, sensor_imu->getNoiseCov(), sensor_imu->getCalibration(), nullptr);
+
+ capture_odo = std::make_shared<CaptureOdom3D>(t_odo, sensor_odo, data_odo, sensor_odo->getNoiseCov(), nullptr);
+ sensor_odo->process(capture_odo);
+ t_odo += dt_odo; //first odometry data will be processed at this timestamp
+
+ // ground truth for quaternion:
+ Eigen::Quaternions quat_odo(Eigen::Quaternions::Identity());
+ Eigen::Quaternions quat_imu(Eigen::Quaternions::Identity());
+
+ WOLF_TRACE("last delta preint: ", processor_imu->getLastPtr()->getDeltaPreint().transpose());
+ WOLF_TRACE("last jacoob bias : ", processor_imu->getLastPtr()->getJacobianCalib().row(0));
for(unsigned int i = 1; i<=1000; i++)
{
+
// PROCESS IMU DATA
// Time and data variables
- t_imu.set(i*dt_imu);
+ t_imu += dt_imu;
- rateOfTurn = Eigen::Vector3s::Random()*10; //to have rate of turn > 0.99 deg/s
- data_imu.tail<3>() = rateOfTurn* M_PI/180.0;
- data_imu.head<3>() = current_quatState.conjugate() * (- wolf::gravity()); //gravity measured, we have no other translation movement
+// rateOfTurn = Eigen::Vector3s::Random()*10; //to have rate of turn > 0.99 deg/s
+ rateOfTurn << 5, 10, 15; // deg/s
+ data_imu.tail<3>() = rateOfTurn * M_PI/180.0;
+ data_imu.head<3>() = quat_imu.conjugate() * (- wolf::gravity()); //gravity measured, we have no other translation movement
//compute odometry + current orientaton taking this measure into account
- odom_quat = odom_quat * wolf::v2q(data_imu.tail(3)*dt_imu);
- current_quatState = current_quatState * wolf::v2q(data_imu.tail(3)*dt_imu);
+ quat_odo = quat_odo * wolf::v2q(data_imu.tail(3)*dt_imu);
+ quat_imu = quat_imu * wolf::v2q(data_imu.tail(3)*dt_imu);
//set timestamp, add bias, set data and process
- imu_ptr->setTimeStamp(t_imu);
+ capture_imu->setTimeStamp(t_imu);
data_imu = data_imu + origin_bias;
- imu_ptr->setData(data_imu);
- sen_imu->process(imu_ptr);
+ capture_imu->setData(data_imu);
+ sensor_imu->process(capture_imu);
+
+ WOLF_TRACE("last delta preint: ", processor_imu->getLastPtr()->getDeltaPreint().transpose());
+ WOLF_TRACE("last jacoob bias : ", processor_imu->getLastPtr()->getJacobianCalib().row(0));
+ //when we find a IMU timestamp corresponding with this odometry timestamp then we process odometry measurement
if(t_imu.get() >= t_odo.get())
{
+ WOLF_TRACE("====== create ODOM KF ========");
+// WOLF_TRACE("Jac calib: ", processor_imu->getLastPtr()->getJacobianCalib().row(0));
+// WOLF_TRACE("last calib: ", processor_imu->getLastPtr()->getCalibration().transpose());
+// WOLF_TRACE("last calib preint: ", processor_imu->getLastPtr()->getCalibrationPreint().transpose());
+
// PROCESS ODOM 3D DATA
- data_odom3D.head(3) << 0,0,0;
- data_odom3D.tail(3) = q2v(odom_quat);
- odo_ptr->setTimeStamp(t_odo);
- odo_ptr->setData(data_odom3D);
- sen_odom3D->process(odo_ptr);
+ data_odo.head(3) << 0,0,0;
+ data_odo.tail(3) = q2v(quat_odo);
+ capture_odo->setTimeStamp(t_odo);
+ capture_odo->setData(data_odo);
+
+// WOLF_TRACE("Jac calib: ", processor_imu->getLastPtr()->getJacobianCalib().row(0));
+// WOLF_TRACE("last calib: ", processor_imu->getLastPtr()->getCalibration().transpose());
+// WOLF_TRACE("last calib preint: ", processor_imu->getLastPtr()->getCalibrationPreint().transpose());
+
+ sensor_odo->process(capture_odo);
+
+// WOLF_TRACE("Jac calib: ", std::static_pointer_cast<CaptureMotion>(processor_imu->getOriginPtr())->getJacobianCalib().row(0));
+// WOLF_TRACE("orig calib: ", processor_imu->getOriginPtr()->getCalibration().transpose());
+// WOLF_TRACE("orig calib preint: ", std::static_pointer_cast<CaptureMotion>(processor_imu->getOriginPtr())->getCalibrationPreint().transpose());
//prepare next odometry measurement
- odom_quat = Eigen::Quaternions::Identity(); //set to identity to have next odom relative to this last KF
+ quat_odo = Eigen::Quaternions::Identity(); //set to identity to have next odom relative to this last KF
t_odo += dt_odo;
+ break;
}
}
- expected_final_state << 0,0,0, current_quatState.x(), current_quatState.y(), current_quatState.z(), current_quatState.w(), 0,0,0;
- last_KF = wolf_problem_ptr_->getTrajectoryPtr()->closestKeyFrameToTimeStamp(t_imu);
+ expected_final_state.resize(10);
+ expected_final_state << 0,0,0, quat_imu.x(), quat_imu.y(), quat_imu.z(), quat_imu.w(), 0,0,0;
+
+ last_KF = problem->getTrajectoryPtr()->closestKeyFrameToTimeStamp(t_imu);
last_KF->setState(expected_final_state);
//===================================================== END{PROCESS DATA}
origin_KF->unfix();
last_KF->unfix();
+
+ CaptureBasePtr origin_CB = origin_KF->getCaptureOf(sensor_imu);
+ CaptureMotionPtr last_CM = std::static_pointer_cast<CaptureMotion>(last_KF ->getCaptureOf(sensor_imu));
+
+// WOLF_TRACE("KF1 calib : ", origin_CB->getCalibration().transpose());
+// WOLF_TRACE("KF2 calib pre: ", last_CM ->getCalibrationPreint().transpose());
+// WOLF_TRACE("KF2 calib : ", last_CM ->getCalibration().transpose());
+// WOLF_TRACE("KF2 delta pre: ", last_CM ->getDeltaPreint().transpose());
+// WOLF_TRACE("KF2 delta cor: ", last_CM ->getDeltaCorrected(origin_CB->getCalibration()).transpose());
+// WOLF_TRACE("KF2 jacob : ", last_CM ->getJacobianCalib().row(0));
+
+
+ // ==================================================== show problem status
+
+ problem->print(4,1,1,1);
+
}
virtual void TearDown(){}
@@ -1058,8 +1103,8 @@ class ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRotY : public testing::Test
Scalar dt(0.0010), dt_odom(1.0);
TimeStamp ts(0.0), t_odom(0.0);
- wolf::CaptureIMUPtr imu_ptr = std::make_shared<CaptureIMU>(ts, sen_imu, data_imu, Eigen::Matrix6s::Identity(), Eigen::Vector6s::Zero());
- wolf::CaptureMotionPtr mot_ptr = std::make_shared<CaptureMotion>(t, sen_odom3D, data_odom3D, 7, 6, nullptr);
+ wolf::CaptureIMUPtr imu_ptr = std::make_shared<CaptureIMU>(ts, sen_imu, data_imu, sen_imu->getNoiseCov(), Eigen::Vector6s::Zero());
+ wolf::CaptureMotionPtr mot_ptr = std::make_shared<CaptureMotion>(t, sen_odom3D, data_odom3D, sen_odom3D->getNoiseCov(), 7, 6, nullptr);
sen_odom3D->process(mot_ptr);
//first odometry data will be processed at this timestamp
t_odom.set(t_odom.get() + dt_odom);
@@ -1190,7 +1235,7 @@ class ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRotXY : public testing::Test
Scalar dt(0.0010), dt_odom(1.0);
TimeStamp ts(0.0), t_odom(1.0);
- wolf::CaptureIMUPtr imu_ptr = std::make_shared<CaptureIMU>(ts, sen_imu, data_imu, Eigen::Matrix6s::Identity(), Eigen::Vector6s::Zero());
+ wolf::CaptureIMUPtr imu_ptr = std::make_shared<CaptureIMU>(ts, sen_imu, data_imu, sen_imu->getNoiseCov(), Eigen::Vector6s::Zero());
wolf::CaptureMotionPtr mot_ptr = std::make_shared<CaptureMotion>(t, sen_odom3D, data_odom3D, 7, 6, nullptr);
sen_odom3D->process(mot_ptr);
//first odometry data will be processed at this timestamp
@@ -1536,12 +1581,8 @@ TEST_F(ConstraintIMU_biasTest_Static_NonNullGyroBias,VarB1B2_InvarP1Q1V1P2Q2V2_i
last_KF->getOPtr()->fix();
last_KF->getVPtr()->fix();
- WOLF_DEBUG("bias before solving : ",origin_KF->getCaptureOf(sen_imu)->getCalibration().transpose());
-
std::string report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport;
- WOLF_DEBUG("bias after solving : ",origin_KF->getCaptureOf(sen_imu)->getCalibration().transpose());
-
//Only biases are unfixed
ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
@@ -1587,12 +1628,8 @@ TEST_F(ConstraintIMU_biasTest_Static_NonNullGyroBias,VarB1B2_InvarP1Q1V1P2Q2V2_E
origin_KF->getCaptureOf(sen_imu)->setCalibration(perturbed_bias);
last_KF->getCaptureOf(sen_imu)->setCalibration(origin_bias);
- WOLF_DEBUG("bias before solving : ",origin_KF->getCaptureOf(sen_imu)->getCalibration().transpose());
-
report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport;
- WOLF_DEBUG("bias after solving : ",origin_KF->getCaptureOf(sen_imu)->getCalibration().transpose());
-
//Only biases are unfixed
ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
@@ -2134,26 +2171,26 @@ TEST_F(ConstraintIMU_biasTest_Move_NonNullBiasRotAndVCst, VarB1B2V1P2V2_InvarP1Q
}
-TEST_F(ConstraintIMU_biasTest_Move_NonNullBiasRot, VarB1B2V1P2V2_InvarP1Q1Q2_initOK)
-{
- //prepare problem for solving
- origin_KF->getPPtr()->fix();
- origin_KF->getOPtr()->fix();
- origin_KF->getVPtr()->unfix();
-
- last_KF->getPPtr()->unfix();
- last_KF->getOPtr()->fix();
- last_KF->getVPtr()->unfix();
-
- last_KF->setState(expected_final_state);
-
- std::string report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
-
- //Only biases are unfixed
- ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
- ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
-
-}
+//TEST_F(ConstraintIMU_biasTest_Move_NonNullBiasRot, VarB1B2V1P2V2_InvarP1Q1Q2_initOK)
+//{
+// //prepare problem for solving
+// origin_KF->getPPtr()->fix();
+// origin_KF->getOPtr()->fix();
+// origin_KF->getVPtr()->unfix();
+//
+// last_KF->getPPtr()->unfix();
+// last_KF->getOPtr()->fix();
+// last_KF->getVPtr()->unfix();
+//
+// last_KF->setState(expected_final_state);
+//
+// std::string report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
+//
+// //Only biases are unfixed
+// ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-3)
+// ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-3)
+//
+//}
TEST_F(ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2_InvarP1Q1V1P2Q2V2_initOK)
{
@@ -2180,20 +2217,20 @@ TEST_F(ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2_InvarP1Q1V1P2Q2
ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
- Eigen::Matrix<wolf::Scalar, 16, 1> cov_stdev, actual_state(last_KF->getState());
- Eigen::MatrixXs covX(16,16);
+ Eigen::Matrix<wolf::Scalar, 10, 1> cov_stdev, actual_state(last_KF->getState());
+ Eigen::MatrixXs covX(10,10);
//get data from covariance blocks
wolf_problem_ptr_->getFrameCovariance(last_KF, covX);
- for(int i = 0; i<16; i++)
+ for(int i = 0; i<10; i++)
cov_stdev(i) = ( covX(i,i)? 2*sqrt(covX(i,i)):0); //if diagonal value is 0 then store 0 else store 2*sqrt(diag_value)
/*TEST_COUT << "2*std : " << cov_stdev.transpose();
TEST_COUT << "expect : " << expected_final_state.transpose(); //expected final state
TEST_COUT << "estim : " << last_KF->getState().transpose(); //estimated final state*/
- for(unsigned int i = 0; i<16; i++)
+ for(unsigned int i = 0; i<10; i++)
assert((expected_final_state(i) <= actual_state(i) + cov_stdev(i)) && (expected_final_state(i) >= actual_state(i) - cov_stdev(i)));
if(cov_stdev.tail(6).maxCoeff()>=1)
@@ -2226,20 +2263,20 @@ TEST_F(ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2V2_InvarP1Q1V1P2
ASSERT_MATRIX_APPROX(last_KF->getVPtr()->getState(), expected_final_state.segment(7,3), wolf::Constants::EPS*100)
- Eigen::Matrix<wolf::Scalar, 16, 1> cov_stdev, actual_state(last_KF->getState());
- Eigen::MatrixXs covX(16,16);
+ Eigen::Matrix<wolf::Scalar, 10, 1> cov_stdev, actual_state(last_KF->getState());
+ Eigen::MatrixXs covX(10,10);
//get data from covariance blocks
wolf_problem_ptr_->getFrameCovariance(last_KF, covX);
- for(int i = 0; i<16; i++)
+ for(int i = 0; i<10; i++)
cov_stdev(i) = ( covX(i,i)? 2*sqrt(covX(i,i)):0); //if diagonal value is 0 then store 0 else store 2*sqrt(diag_value)
/*TEST_COUT << "2*std : " << cov_stdev.transpose();
TEST_COUT << "expect : " << expected_final_state.transpose(); //expected final state
TEST_COUT << "estim : " << last_KF->getState().transpose(); //estimated final state*/
- for(unsigned int i = 0; i<16; i++)
+ for(unsigned int i = 0; i<10; i++)
assert((expected_final_state(i) <= actual_state(i) + cov_stdev(i)) && (expected_final_state(i) >= actual_state(i) - cov_stdev(i)));
if(cov_stdev.tail(6).maxCoeff()>=1)
@@ -2393,20 +2430,20 @@ TEST_F(ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2P2Q2V2_InvarP1Q1
Eigen::Map<const Eigen::Quaternions> estimatedLastQuat(last_KF->getOPtr()->getState().data()), expectedLastQuat(expected_final_state.segment(3,4).data());
ASSERT_QUATERNION_APPROX(estimatedLastQuat, expectedLastQuat, wolf::Constants::EPS*100)
- Eigen::Matrix<wolf::Scalar, 16, 1> cov_stdev, actual_state(last_KF->getState());
- Eigen::MatrixXs covX(16,16);
+ Eigen::Matrix<wolf::Scalar, 10, 1> cov_stdev, actual_state(last_KF->getState());
+ Eigen::MatrixXs covX(10,10);
//get data from covariance blocks
wolf_problem_ptr_->getFrameCovariance(last_KF, covX);
- for(int i = 0; i<16; i++)
+ for(int i = 0; i<10; i++)
cov_stdev(i) = ( covX(i,i)? 2*sqrt(covX(i,i)):0); //if diagonal value is 0 then store 0 else store 2*sqrt(diag_value)
/*TEST_COUT << "2*std : " << cov_stdev.transpose();
TEST_COUT << "expect : " << expected_final_state.transpose(); //expected final state
TEST_COUT << "estim : " << last_KF->getState().transpose(); //estimated final state*/
- for(unsigned int i = 0; i<16; i++)
+ for(unsigned int i = 0; i<10; i++)
assert((expected_final_state(i) <= actual_state(i) + cov_stdev(i)) && (expected_final_state(i) >= actual_state(i) - cov_stdev(i)));
if(cov_stdev.tail(6).maxCoeff()>=1)
@@ -2451,20 +2488,20 @@ TEST_F(ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarQ1B1B2P2Q2_InvarP1V1
Eigen::Map<const Eigen::Quaternions> estimatedLastQuat(last_KF->getOPtr()->getState().data()), expectedLastQuat(expected_final_state.segment(3,4).data());
ASSERT_QUATERNION_APPROX(estimatedLastQuat, expectedLastQuat, wolf::Constants::EPS*100)
- Eigen::Matrix<wolf::Scalar, 16, 1> cov_stdev, actual_state(last_KF->getState());
- Eigen::MatrixXs covX(16,16);
+ Eigen::Matrix<wolf::Scalar, 10, 1> cov_stdev, actual_state(last_KF->getState());
+ Eigen::MatrixXs covX(10,10);
//get data from covariance blocks
wolf_problem_ptr_->getFrameCovariance(last_KF, covX);
- for(int i = 0; i<16; i++)
+ for(int i = 0; i<10; i++)
cov_stdev(i) = ( covX(i,i)? 2*sqrt(covX(i,i)):0); //if diagonal value is 0 then store 0 else store 2*sqrt(diag_value)
/*TEST_COUT << "2*std : " << cov_stdev.transpose();
TEST_COUT << "expect : " << expected_final_state.transpose(); //expected final state
TEST_COUT << "estim : " << last_KF->getState().transpose(); //estimated final state*/
- for(unsigned int i = 0; i<16; i++)
+ for(unsigned int i = 0; i<10; i++)
assert((expected_final_state(i) <= actual_state(i) + cov_stdev(i)) && (expected_final_state(i) >= actual_state(i) - cov_stdev(i)));
if(cov_stdev.tail(6).maxCoeff()>=1)
@@ -2509,20 +2546,20 @@ TEST_F(ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRotXY, VarQ1B1B2P2Q2_InvarP1V
Eigen::Map<const Eigen::Quaternions> estimatedLastQuat(last_KF->getOPtr()->getState().data()), expectedLastQuat(expected_final_state.segment(3,4).data());
ASSERT_QUATERNION_APPROX(estimatedLastQuat, expectedLastQuat, wolf::Constants::EPS*100)
- Eigen::Matrix<wolf::Scalar, 16, 1> cov_stdev, actual_state(last_KF->getState());
- Eigen::MatrixXs covX(16,16);
+ Eigen::Matrix<wolf::Scalar, 10, 1> cov_stdev, actual_state(last_KF->getState());
+ Eigen::MatrixXs covX(10,10);
//get data from covariance blocks
wolf_problem_ptr_->getFrameCovariance(last_KF, covX);
- for(int i = 0; i<16; i++)
+ for(int i = 0; i<10; i++)
cov_stdev(i) = ( covX(i,i)? 2*sqrt(covX(i,i)):0); //if diagonal value is 0 then store 0 else store 2*sqrt(diag_value)
/*TEST_COUT << "2*std : " << cov_stdev.transpose();
TEST_COUT << "expect : " << expected_final_state.transpose(); //expected final state
TEST_COUT << "estim : " << last_KF->getState().transpose(); //estimated final state*/
- for(unsigned int i = 0; i<16; i++)
+ for(unsigned int i = 0; i<10; i++)
assert((expected_final_state(i) <= actual_state(i) + cov_stdev(i)) && (expected_final_state(i) >= actual_state(i) - cov_stdev(i)));
if(cov_stdev.tail(6).maxCoeff()>=1)
@@ -2544,30 +2581,36 @@ TEST_F(ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2_InvarP1Q1V1P2Q2V
//perturbation of origin bias
Eigen::Vector6s random_err(Eigen::Vector6s::Random() * 0.001);
- Eigen::Vector6s bias = origin_KF->getCaptureOf(sen_imu)->getCalibration();
- origin_KF->getCaptureOf(sen_imu)->setCalibration(bias + random_err);
+ Eigen::Vector6s bias = origin_KF->getCaptureOf(sensor_imu)->getCalibration();
+ origin_KF->getCaptureOf(sensor_imu)->setCalibration(bias + random_err);
- std::string report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
- ceres_manager_wolf_diff->computeCovariances(ALL);
+// WOLF_TRACE("real bias : ", origin_bias.transpose());
+// WOLF_TRACE("origin bias : ", origin_KF->getCaptureOf(sensor_imu)->getCalibration().transpose());
+// WOLF_TRACE("last bias : ", last_KF->getCaptureOf(sensor_imu)->getCalibration().transpose());
+// WOLF_TRACE("jacob bias : ", std::static_pointer_cast<CaptureIMU>(last_KF->getCaptureOf(sensor_imu))->getJacobianCalib().row(0));
+
+ std::string report = ceres_manager->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
+ std::cout << report << std::endl;
+ ceres_manager->computeCovariances(ALL);
//Only biases are unfixed
- ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
- ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
+ ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
+ ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
- Eigen::Matrix<wolf::Scalar, 16, 1> cov_stdev, actual_state(last_KF->getState());
- Eigen::MatrixXs covX(16,16);
+ Eigen::Matrix<wolf::Scalar, 10, 1> cov_stdev, actual_state(last_KF->getState());
+ Eigen::MatrixXs covX(10,10);
//get data from covariance blocks
- wolf_problem_ptr_->getFrameCovariance(last_KF, covX);
+ problem->getFrameCovariance(last_KF, covX);
- for(int i = 0; i<16; i++)
+ for(int i = 0; i<10; i++)
cov_stdev(i) = ( covX(i,i)? 2*sqrt(covX(i,i)):0); //if diagonal value is 0 then store 0 else store 2*sqrt(diag_value)
/*TEST_COUT << "2*std : " << cov_stdev.transpose();
TEST_COUT << "expect : " << expected_final_state.transpose(); //expected final state
TEST_COUT << "estim : " << last_KF->getState().transpose(); //estimated final state*/
- for(unsigned int i = 0; i<16; i++)
+ for(unsigned int i = 0; i<10; i++)
assert((expected_final_state(i) <= actual_state(i) + cov_stdev(i)) && (expected_final_state(i) >= actual_state(i) - cov_stdev(i)));
if(cov_stdev.tail(6).maxCoeff()>=1)
@@ -2589,31 +2632,31 @@ TEST_F(ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2V2_InvarP1Q1V1P2Q
//perturbation of origin bias
Eigen::Vector6s random_err(Eigen::Vector6s::Random() * 0.001);
- Eigen::Vector6s bias = origin_KF->getCaptureOf(sen_imu)->getCalibration();
- origin_KF->getCaptureOf(sen_imu)->setCalibration(bias + random_err);
+ Eigen::Vector6s bias = origin_KF->getCaptureOf(sensor_imu)->getCalibration();
+ origin_KF->getCaptureOf(sensor_imu)->setCalibration(bias + random_err);
- std::string report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
- ceres_manager_wolf_diff->computeCovariances(ALL);
+ std::string report = ceres_manager->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
+ ceres_manager->computeCovariances(ALL);
- ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
- ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
+ ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
+ ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
ASSERT_MATRIX_APPROX(last_KF->getVPtr()->getState(), expected_final_state.segment(7,3), wolf::Constants::EPS*100)
- Eigen::Matrix<wolf::Scalar, 16, 1> cov_stdev, actual_state(last_KF->getState());
- Eigen::MatrixXs covX(16,16);
+ Eigen::Matrix<wolf::Scalar, 10, 1> cov_stdev, actual_state(last_KF->getState());
+ Eigen::MatrixXs covX(10,10);
//get data from covariance blocks
- wolf_problem_ptr_->getFrameCovariance(last_KF, covX);
+ problem->getFrameCovariance(last_KF, covX);
- for(int i = 0; i<16; i++)
+ for(int i = 0; i<10; i++)
cov_stdev(i) = ( covX(i,i)? 2*sqrt(covX(i,i)):0); //if diagonal value is 0 then store 0 else store 2*sqrt(diag_value)
/*TEST_COUT << "2*std : " << cov_stdev.transpose();
TEST_COUT << "expect : " << expected_final_state.transpose(); //expected final state
TEST_COUT << "estim : " << last_KF->getState().transpose(); //estimated final state*/
- for(unsigned int i = 0; i<16; i++)
+ for(unsigned int i = 0; i<10; i++)
assert((expected_final_state(i) <= actual_state(i) + cov_stdev(i)) && (expected_final_state(i) >= actual_state(i) - cov_stdev(i)));
if(cov_stdev.tail(6).maxCoeff()>=1)
@@ -2635,14 +2678,14 @@ TEST_F(ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2V1V2_InvarP1Q1P2Q
//perturbation of origin bias
Eigen::Vector6s random_err(Eigen::Vector6s::Random() * 0.00001);
- Eigen::Vector6s bias = origin_KF->getCaptureOf(sen_imu)->getCalibration();
- origin_KF->getCaptureOf(sen_imu)->setCalibration(bias + random_err);
+ Eigen::Vector6s bias = origin_KF->getCaptureOf(sensor_imu)->getCalibration();
+ origin_KF->getCaptureOf(sensor_imu)->setCalibration(bias + random_err);
- std::string report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
+ std::string report = ceres_manager->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
ASSERT_MATRIX_APPROX(origin_KF->getVPtr()->getState(), x_origin.segment(7,3), wolf::Constants::EPS*1000)
- ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
- ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
+ ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
+ ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
ASSERT_MATRIX_APPROX(last_KF->getVPtr()->getState(), expected_final_state.segment(7,3), wolf::Constants::EPS*1000)
}
@@ -2662,14 +2705,14 @@ TEST_F(ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2V1Q2V2_InvarP1Q1P
//perturbation of origin bias
Eigen::Vector6s random_err(Eigen::Vector6s::Random() * 0.00001);
- Eigen::Vector6s bias = origin_KF->getCaptureOf(sen_imu)->getCalibration();
- origin_KF->getCaptureOf(sen_imu)->setCalibration(bias + random_err);
+ Eigen::Vector6s bias = origin_KF->getCaptureOf(sensor_imu)->getCalibration();
+ origin_KF->getCaptureOf(sensor_imu)->setCalibration(bias + random_err);
- std::string report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
+ std::string report = ceres_manager->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
ASSERT_MATRIX_APPROX(origin_KF->getVPtr()->getState(), x_origin.segment(7,3), wolf::Constants::EPS*1000)
- ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
- ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
+ ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
+ ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
ASSERT_MATRIX_APPROX(last_KF->getVPtr()->getState(), expected_final_state.segment(7,3), wolf::Constants::EPS*1000)
Eigen::Map<const Eigen::Quaternions> estimatedLastQuat(last_KF->getOPtr()->getState().data()), expectedLastQuat(expected_final_state.segment(3,4).data());
@@ -2691,14 +2734,14 @@ TEST_F(ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2V1P2V2_InvarP1Q1Q
//perturbation of origin bias
Eigen::Vector6s random_err(Eigen::Vector6s::Random() * 0.00001);
- Eigen::Vector6s bias = origin_KF->getCaptureOf(sen_imu)->getCalibration();
- origin_KF->getCaptureOf(sen_imu)->setCalibration(bias + random_err);
+ Eigen::Vector6s bias = origin_KF->getCaptureOf(sensor_imu)->getCalibration();
+ origin_KF->getCaptureOf(sensor_imu)->setCalibration(bias + random_err);
- std::string report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
+ std::string report = ceres_manager->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
ASSERT_MATRIX_APPROX(origin_KF->getVPtr()->getState(), x_origin.segment(7,3), wolf::Constants::EPS*1000)
- ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
- ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
+ ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
+ ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
ASSERT_MATRIX_APPROX(last_KF->getVPtr()->getState(), expected_final_state.segment(7,3), wolf::Constants::EPS*1000)
}
@@ -2718,14 +2761,14 @@ TEST_F(ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2V1P2Q2V2_InvarP1Q
//perturbation of origin bias
Eigen::Vector6s random_err(Eigen::Vector6s::Random() * 0.0001);
- Eigen::Vector6s bias = origin_KF->getCaptureOf(sen_imu)->getCalibration();
- origin_KF->getCaptureOf(sen_imu)->setCalibration(bias + random_err);
+ Eigen::Vector6s bias = origin_KF->getCaptureOf(sensor_imu)->getCalibration();
+ origin_KF->getCaptureOf(sensor_imu)->setCalibration(bias + random_err);
- std::string report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
+ std::string report = ceres_manager->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
ASSERT_MATRIX_APPROX(origin_KF->getVPtr()->getState(), x_origin.segment(7,3), wolf::Constants::EPS*10000)
- ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
- ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
+ ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
+ ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
ASSERT_MATRIX_APPROX(last_KF->getVPtr()->getState(), expected_final_state.segment(7,3), wolf::Constants::EPS*10000)
Eigen::Map<const Eigen::Quaternions> estimatedLastQuat(last_KF->getOPtr()->getState().data()), expectedLastQuat(expected_final_state.segment(3,4).data());
@@ -2747,39 +2790,39 @@ TEST_F(ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2P2Q2V2_InvarP1Q1V
//perturbation of origin bias
Eigen::Vector6s random_err(Eigen::Vector6s::Random() * 0.00001);
- Eigen::Vector6s bias = origin_KF->getCaptureOf(sen_imu)->getCalibration();
- origin_KF->getCaptureOf(sen_imu)->setCalibration(bias + random_err);
+ Eigen::Vector6s bias = origin_KF->getCaptureOf(sensor_imu)->getCalibration();
+ origin_KF->getCaptureOf(sensor_imu)->setCalibration(bias + random_err);
- std::string report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
- ceres_manager_wolf_diff->computeCovariances(ALL);
+ std::string report = ceres_manager->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
+ ceres_manager->computeCovariances(ALL);
ASSERT_MATRIX_APPROX(origin_KF->getVPtr()->getState(), x_origin.segment(7,3), wolf::Constants::EPS*1000)
- ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
- ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
+ ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
+ ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
ASSERT_MATRIX_APPROX(last_KF->getPPtr()->getState(), expected_final_state.head(3), wolf::Constants::EPS*100)
ASSERT_MATRIX_APPROX(last_KF->getVPtr()->getState(), expected_final_state.segment(7,3), wolf::Constants::EPS*1000)
Eigen::Map<const Eigen::Quaternions> estimatedLastQuat(last_KF->getOPtr()->getState().data()), expectedLastQuat(expected_final_state.segment(3,4).data());
ASSERT_QUATERNION_APPROX(estimatedLastQuat, expectedLastQuat, wolf::Constants::EPS*100)
- Eigen::Matrix<wolf::Scalar, 16, 1> cov_stdev, actual_state(last_KF->getState());
- Eigen::MatrixXs covX(16,16);
+ Eigen::Matrix<wolf::Scalar, 10, 1> cov_stdev, actual_state(last_KF->getState());
+ Eigen::MatrixXs covX(10,10);
//get data from covariance blocks
- wolf_problem_ptr_->getFrameCovariance(last_KF, covX);
+ problem->getFrameCovariance(last_KF, covX);
- for(int i = 0; i<16; i++)
+ for(int i = 0; i<10; i++)
cov_stdev(i) = ( covX(i,i)? 2*sqrt(covX(i,i)):0); //if diagonal value is 0 then store 0 else store 2*sqrt(diag_value)
/*TEST_COUT << "2*std : " << cov_stdev.transpose();
TEST_COUT << "expect : " << expected_final_state.transpose(); //expected final state
TEST_COUT << "estim : " << last_KF->getState().transpose(); //estimated final state*/
- for(unsigned int i = 0; i<16; i++)
+ for(unsigned int i = 0; i<10; i++)
assert((expected_final_state(i) <= actual_state(i) + cov_stdev(i)) && (expected_final_state(i) >= actual_state(i) - cov_stdev(i)));
- if(cov_stdev.tail(6).maxCoeff()>=1)
- WOLF_WARN("Big 2*stdev on one or more biases! Max coeff :", cov_stdev.tail(6).maxCoeff())
+// if(cov_stdev.tail(6).maxCoeff()>=1)
+// WOLF_WARN("Big 2*stdev on one or more biases! Max coeff :", cov_stdev.tail(6).maxCoeff())
}
//Tests related to noise
@@ -2787,8 +2830,12 @@ TEST_F(ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2P2Q2V2_InvarP1Q1V
int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);
- //::testing::GTEST_FLAG(filter) = "ConstraintIMU_biasTest_Move_NonNullBiasRot.*:ConstraintIMU_biasTest_Static_NullBias.*:ConstraintIMU_biasTest_Static_NonNullAccBias.*:ConstraintIMU_biasTest_Static_NonNullGyroBias.*";
- ::testing::GTEST_FLAG(filter) = "ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRot.*";
+ ::testing::GTEST_FLAG(filter) = "ConstraintIMU_biasTest_Move_NonNullBiasRot.*:ConstraintIMU_biasTest_Static_NullBias.*:ConstraintIMU_biasTest_Static_NonNullAccBias.*:ConstraintIMU_biasTest_Static_NonNullGyroBias.*";
+// ::testing::GTEST_FLAG(filter) = "ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRotY.VarB1B2V1V2_InvarP1Q1P2Q2_initOK";
+// ::testing::GTEST_FLAG(filter) = "ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRot.VarB1B2_InvarP1Q1V1P2Q2V2_initOK";
+// ::testing::GTEST_FLAG(filter) = "ConstraintIMU_biasTest_Move_NonNullBiasRot.VarB1B2V1P2V2_InvarP1Q1Q2_initOK";
+
+
return RUN_ALL_TESTS();
}
diff --git a/src/test/gtest_feature_imu.cpp b/src/test/gtest_feature_imu.cpp
index 0c0ed288631dfe3c39ccfeaefba62280252934ce..bf0956ac15fbeb34f65fbcd227d88382637a7093 100644
--- a/src/test/gtest_feature_imu.cpp
+++ b/src/test/gtest_feature_imu.cpp
@@ -14,7 +14,7 @@ class FeatureIMU_test : public testing::Test
{
public: //These can be accessed in fixtures
- wolf::ProblemPtr wolf_problem_ptr_;
+ wolf::ProblemPtr problem;
wolf::TimeStamp ts;
wolf::CaptureIMUPtr imu_ptr;
Eigen::VectorXs state_vec;
@@ -35,12 +35,13 @@ class FeatureIMU_test : public testing::Test
using std::static_pointer_cast;
// Wolf problem
- wolf_problem_ptr_ = Problem::create("POV 3D");
+ problem = Problem::create("POV 3D");
Eigen::VectorXs IMU_extrinsics(7);
IMU_extrinsics << 0,0,0, 0,0,0,1; // IMU pose in the robot
IntrinsicsIMUPtr sen_imu_params = std::make_shared<IntrinsicsIMU>();
- SensorBasePtr sensor_ptr = wolf_problem_ptr_->installSensor("IMU", "Main IMU", IMU_extrinsics, sen_imu_params);
- processor_ptr_ = wolf_problem_ptr_->installProcessor("IMU", "IMU pre-integrator", "Main IMU", "");
+ SensorBasePtr sensor_ptr = problem->installSensor("IMU", "Main IMU", IMU_extrinsics, sen_imu_params);
+ ProcessorIMUParamsPtr prc_imu_params = std::make_shared<ProcessorIMUParams>();
+ processor_ptr_ = problem->installProcessor("IMU", "IMU pre-integrator", sensor_ptr, prc_imu_params);
// Time and data variables
TimeStamp t;
@@ -51,7 +52,7 @@ class FeatureIMU_test : public testing::Test
// Set the origin
Eigen::VectorXs x0(10);
x0 << 0,0,0, 0,0,0,1, 0,0,0;
- origin_frame = wolf_problem_ptr_->getProcessorMotionPtr()->setOrigin(x0, t); //create a keyframe at origin
+ origin_frame = problem->getProcessorMotionPtr()->setOrigin(x0, t); //create a keyframe at origin
// Create one capture to store the IMU data arriving from (sensor / callback / file / etc.)
// give the capture a big covariance, otherwise it will be so small that it won't pass following assertions
@@ -67,27 +68,25 @@ class FeatureIMU_test : public testing::Test
imu_ptr->setData(data_);
imu_ptr->setTimeStamp(t);
// process data in capture
- WOLF_TRACE("D_ini: ", wolf_problem_ptr_->getProcessorMotionPtr()->getMotion().delta_integr_.transpose());
sensor_ptr->process(imu_ptr);
- WOLF_TRACE("D_pre: ", wolf_problem_ptr_->getProcessorMotionPtr()->getMotion().delta_integr_.transpose());
//create Frame
- ts = wolf_problem_ptr_->getProcessorMotionPtr()->getBuffer().get().back().ts_;
- state_vec = wolf_problem_ptr_->getProcessorMotionPtr()->getCurrentState();
+ ts = problem->getProcessorMotionPtr()->getBuffer().get().back().ts_;
+ state_vec = problem->getProcessorMotionPtr()->getCurrentState();
last_frame = std::make_shared<FrameBase>(KEY_FRAME, ts, std::make_shared<StateBlock>(state_vec.head(3)), std::make_shared<StateBlock>(state_vec.segment(3,4)), std::make_shared<StateBlock>(state_vec.head(3)));
- wolf_problem_ptr_->getTrajectoryPtr()->addFrame(last_frame);
+ problem->getTrajectoryPtr()->addFrame(last_frame);
//create a feature
- delta_preint = wolf_problem_ptr_->getProcessorMotionPtr()->getMotion().delta_integr_;
- delta_preint_cov = wolf_problem_ptr_->getProcessorMotionPtr()->getMotion().delta_integr_cov_;
- //feat_imu = std::make_shared<FeatureIMU>(delta_preint, delta_preint_cov);
- WOLF_TRACE("D_pre: ", delta_preint.transpose());
- dD_db_jacobians = std::static_pointer_cast<wolf::ProcessorIMU>(wolf_problem_ptr_->getProcessorMotionPtr())->getLastPtr()->getJacobianCalib();
- feat_imu = std::make_shared<FeatureIMU>(delta_preint, delta_preint_cov, imu_ptr, dD_db_jacobians);
+ delta_preint = problem->getProcessorMotionPtr()->getMotion().delta_integr_;
+ delta_preint_cov = problem->getProcessorMotionPtr()->getMotion().delta_integr_cov_;
+ VectorXs calib_preint = problem->getProcessorMotionPtr()->getBuffer().getCalibrationPreint();
+ dD_db_jacobians = problem->getProcessorMotionPtr()->getMotion().jacobian_calib_;
+ feat_imu = std::make_shared<FeatureIMU>(delta_preint,
+ delta_preint_cov,
+ calib_preint,
+ dD_db_jacobians,
+ imu_ptr);
feat_imu->setCapturePtr(imu_ptr); //associate the feature to a capture
- WOLF_TRACE("P_pre: ", feat_imu->getDpPreint().transpose());
- WOLF_TRACE("Q_pre: ", feat_imu->getDqPreint().coeffs().transpose());
- WOLF_TRACE("V_pre: ", feat_imu->getDvPreint().transpose());
}
@@ -115,8 +114,9 @@ TEST_F(FeatureIMU_test, check_frame)
left_frame->getTimeStamp(t);
origin_frame->getTimeStamp(ts);
- ASSERT_EQ(t,ts) << "t != ts \t t=" << t << "\t ts=" << ts << std::endl;
+ ASSERT_NEAR(t.get(), ts.get(), wolf::Constants::EPS_SMALL) << "t != ts \t t=" << t << "\t ts=" << ts << std::endl;
ASSERT_TRUE(origin_frame->isKey());
+ ASSERT_TRUE(last_frame->isKey());
ASSERT_TRUE(left_frame->isKey());
wolf::StateBlockPtr origin_pptr, origin_optr, origin_vptr, left_pptr, left_optr, left_vptr;
@@ -142,11 +142,7 @@ TEST_F(FeatureIMU_test, access_members)
Eigen::VectorXs delta(10);
//dx = 0.5*2*0.1^2 = 0.01; dvx = 2*0.1 = 0.2; dz = 0.5*9.8*0.1^2 = 0.049; dvz = 9.8*0.1 = 0.98
delta << 0.01,0,0.049, 0,0,0,1, 0.2,0,0.98;
- ASSERT_MATRIX_APPROX(feat_imu->getDpPreint(), delta.head<3>(), wolf::Constants::EPS_SMALL);
- ASSERT_MATRIX_APPROX(feat_imu->getDvPreint(), delta.tail<3>(), wolf::Constants::EPS);
-
- Eigen::Map<const Eigen::Quaternions> delta_quat(delta.segment<4>(3).data());
- ASSERT_QUATERNION_APPROX(feat_imu->getDqPreint(), delta_quat, wolf::Constants::EPS_SMALL);
+ ASSERT_MATRIX_APPROX(feat_imu->getMeasurement().transpose(), delta.transpose(), wolf::Constants::EPS);
}
//TEST_F(FeatureIMU_test, addConstraint)
diff --git a/src/test/gtest_imu_tools.cpp b/src/test/gtest_imu_tools.cpp
index b44ddfebb8052a49957c2130bc2b9a6beced5cf4..e53214a6bbee5c0dc2ae5ebfe1a40c3fba4bbdc5 100644
--- a/src/test/gtest_imu_tools.cpp
+++ b/src/test/gtest_imu_tools.cpp
@@ -148,6 +148,22 @@ TEST(IMU_tools, lift_retract)
ASSERT_MATRIX_APPROX(delta_from_d, delta, 1e-10);
}
+TEST(IMU_tools, plus)
+{
+ VectorXs d1(10), d2(10), d3(10);
+ VectorXs err(9);
+ Vector4s qv = (Vector4s() << 3, 4, 5, 6).finished().normalized();
+ d1 << 0, 1, 2, qv, 7, 8, 9;
+ err << 0.01, 0.02, 0.03, 0.04, 0.05, 0.06, 0.07, 0.08, 0.09;
+
+ d3.head(3) = d1.head(3) + err.head(3);
+ d3.segment(3,4) = (Quaternions(qv.data()) * exp_q(err.segment(3,3))).coeffs();
+ d3.tail(3) = d1.tail(3) + err.tail(3);
+
+ plus(d1, err, d2);
+ ASSERT_MATRIX_APPROX(diff(d3, d2), VectorXs::Zero(9), 1e-10);
+}
+
TEST(IMU_tools, diff)
{
VectorXs d1(10), d2(10);
@@ -159,6 +175,15 @@ TEST(IMU_tools, diff)
diff(d1, d2, err);
ASSERT_MATRIX_APPROX(err, VectorXs::Zero(9), 1e-10);
ASSERT_MATRIX_APPROX(diff(d1, d2), VectorXs::Zero(9), 1e-10);
+
+ VectorXs d3(10);
+ d3.setRandom(); d3.segment(3,4).normalize();
+ err.head(3) = d3.head(3) - d1.head(3);
+ err.segment(3,3) = log_q(Quaternions(d1.data()+3).conjugate()*Quaternions(d3.data()+3));
+ err.tail(3) = d3.tail(3) - d1.tail(3);
+
+ ASSERT_MATRIX_APPROX(err, diff(d1, d3), 1e-10);
+
}
TEST(IMU_tools, compose_jacobians)
@@ -201,9 +226,330 @@ TEST(IMU_tools, compose_jacobians)
ASSERT_MATRIX_APPROX(J2_a, J2_n, 1e-4);
}
+TEST(IMU_tools, diff_jacobians)
+{
+ VectorXs d1(10), d2(10), d3(9), d1_pert(10), d2_pert(10), d3_pert(9); // deltas
+ VectorXs t1(9), t2(9); // tangents
+ Matrix<Scalar, 9, 9> J1_a, J2_a, J1_n, J2_n;
+ Vector4s qv1, qv2;
+ Scalar dx = 1e-6;
+ qv1 = (Vector4s() << 3, 4, 5, 6).finished().normalized();
+ d1 << 0, 1, 2, qv1, 7, 8, 9;
+ qv2 = (Vector4s() << 1, 2, 3, 4).finished().normalized();
+ d2 << 9, 8, 7, qv2, 2, 1, 0;
+
+ // analytical jacobians
+ diff(d1, d2, d3, J1_a, J2_a);
+
+ // numerical jacobians
+ for (unsigned int i = 0; i<9; i++)
+ {
+ t1 . setZero();
+ t1(i) = dx;
+
+ // Jac wrt first delta
+ d1_pert = plus(d1, t1); // (d1 + t1)
+ d3_pert = diff(d1_pert, d2); // d2 - (d1 + t1)
+ t2 = d3_pert - d3; // { d2 - (d1 + t1) } - { d2 - d1 }
+ J1_n.col(i) = t2/dx; // [ { d2 - (d1 + t1) } - { d2 - d1 } ] / dx
+
+ // Jac wrt second delta
+ d2_pert = plus(d2, t1); // (d2 + t1)
+ d3_pert = diff(d1, d2_pert); // (d2 + t1) - d1
+ t2 = d3_pert - d3; // { (d2 + t1) - d1 } - { d2 - d1 }
+ J2_n.col(i) = t2/dx; // [ { (d2 + t1) - d1 } - { d2 - d1 } ] / dx
+ }
+
+ // check that numerical and analytical match
+ ASSERT_MATRIX_APPROX(J1_a, J1_n, 1e-4);
+ ASSERT_MATRIX_APPROX(J2_a, J2_n, 1e-4);
+}
+
+TEST(IMU_tools, body2delta_jacobians)
+{
+ VectorXs delta(10), delta_pert(10); // deltas
+ VectorXs body(6), pert(6);
+ VectorXs tang(9); // tangents
+ Matrix<Scalar, 9, 6> J_a, J_n; // analytic and numerical jacs
+ Vector4s qv;;
+ Scalar dt = 0.1;
+ Scalar dx = 1e-6;
+ qv = (Vector4s() << 3, 4, 5, 6).finished().normalized();
+ delta << 0, 1, 2, qv, 7, 8, 9;
+ body << 1, 2, 3, 3, 2, 1;
+
+ // analytical jacobians
+ body2delta(body, dt, delta, J_a);
+
+ // numerical jacobians
+ for (unsigned int i = 0; i<6; i++)
+ {
+ pert . setZero();
+ pert(i) = dx;
+
+ // Jac wrt first delta
+ delta_pert = body2delta(body + pert, dt); // delta(body + pert)
+ tang = diff(delta, delta_pert); // delta(body + pert) -- delta(body)
+ J_n.col(i) = tang/dx; // ( delta(body + pert) -- delta(body) ) / dx
+ }
+
+ // check that numerical and analytical match
+ ASSERT_MATRIX_APPROX(J_a, J_n, 1e-4);
+}
+
+/* Create IMU data from body motion
+ * Input:
+ * motion: [ax, ay, az, wx, wy, wz] the motion in body frame
+ * q: the current orientation wrt horizontal
+ * bias: the bias of the IMU
+ * Output:
+ * return: the data vector as created by the IMU (with motion, gravity, and bias)
+ */
+VectorXs motion2data(const VectorXs& body, const Quaternions& q, const VectorXs& bias)
+{
+ VectorXs data(6);
+ data = body; // start with body motion
+ data.head(3) -= q.conjugate()*gravity(); // add -g
+ data = data + bias; // add bias
+ return data;
+}
+
+/* Integrate acc and angVel motion, obtain Delta_preintegrated
+ * Input:
+ * N: number of steps
+ * q0: initial orientaiton
+ * motion: [ax, ay, az, wx, wy, wz] as the true magnitudes in body brame
+ * bias_real: the real bias of the IMU
+ * bias_preint: the bias used for Delta pre-integration
+ * Output:
+ * return: the preintegrated delta
+ */
+VectorXs integrateDelta(int N, const Quaternions& q0, const VectorXs& motion, const VectorXs& bias_real, const VectorXs& bias_preint, Scalar dt)
+{
+ VectorXs data(6);
+ VectorXs body(6);
+ VectorXs delta(10);
+ VectorXs Delta(10), Delta_plus(10);
+ Delta << 0,0,0, 0,0,0,1, 0,0,0;
+ Quaternions q(q0);
+ for (int n = 0; n<N; n++)
+ {
+ data = motion2data(motion, q, bias_real);
+ q = q*exp_q(motion.tail(3)*dt);
+ body = data - bias_preint;
+ delta = body2delta(body, dt);
+ Delta_plus = compose(Delta, delta, dt);
+ Delta = Delta_plus;
+ }
+ return Delta;
+}
+
+/* Integrate acc and angVel motion, obtain Delta_preintegrated
+ * Input:
+ * N: number of steps
+ * q0: initial orientaiton
+ * motion: [ax, ay, az, wx, wy, wz] as the true magnitudes in body brame
+ * bias_real: the real bias of the IMU
+ * bias_preint: the bias used for Delta pre-integration
+ * Output:
+ * J_D_b: the Jacobian of the preintegrated delta wrt the bias
+ * return: the preintegrated delta
+ */
+VectorXs integrateDelta(int N, const Quaternions& q0, const VectorXs& motion, const VectorXs& bias_real, const VectorXs& bias_preint, Scalar dt, Matrix<Scalar, 9, 6>& J_D_b)
+{
+ VectorXs data(6);
+ VectorXs body(6);
+ VectorXs delta(10);
+ Matrix<Scalar, 9, 6> J_d_d, J_d_b;
+ Matrix<Scalar, 9, 9> J_D_D, J_D_d;
+ VectorXs Delta(10), Delta_plus(10);
+ Quaternions q;
+
+ Delta << 0,0,0, 0,0,0,1, 0,0,0;
+ J_D_b.setZero();
+ q = q0;
+ for (int n = 0; n<N; n++)
+ {
+ // Simulate data
+ data = motion2data(motion, q, bias_real);
+ q = q*exp_q(motion.tail(3)*dt);
+ // Motion::integrateOneStep()
+ { // IMU::computeCurrentDelta
+ body = data - bias_preint;
+ body2delta(body, dt, delta, J_d_d);
+ J_d_b = - J_d_d;
+ }
+ { // IMU::deltaPlusDelta
+ compose(Delta, delta, dt, Delta_plus, J_D_D, J_D_d);
+ }
+ // Motion:: jac calib
+ J_D_b = J_D_D*J_D_b + J_D_d*J_d_b;
+ // Motion:: buffer
+ Delta = Delta_plus;
+ }
+ return Delta;
+}
+
+TEST(IMU_tools, integral_jacobian_bias)
+{
+ VectorXs Delta(10), Delta_pert(10); // deltas
+ VectorXs bias_real(6), pert(6), bias_pert(6), bias_preint(6);
+ VectorXs body(6), data(6), motion(6);
+ VectorXs tang(9); // tangents
+ Matrix<Scalar, 9, 6> J_a, J_n; // analytic and numerical jacs
+ Scalar dt = 0.001;
+ Scalar dx = 1e-4;
+ Quaternions q0(3, 4, 5, 6); q0.normalize();
+ motion << .1, .2, .3, .3, .2, .1;
+ bias_real << .002, .004, .001, .003, .002, .001;
+ bias_preint << .004, .005, .006, .001, .002, .003;
+
+ int N = 500; // # steps of integration
+
+ // Analytical Jacobians
+ Delta = integrateDelta(N, q0, motion, bias_real, bias_preint, dt, J_a);
+
+ // numerical jacobians
+ for (unsigned int i = 0; i<6; i++)
+ {
+ pert . setZero();
+ pert(i) = dx;
+
+ bias_pert = bias_preint + pert;
+ Delta_pert = integrateDelta(N, q0, motion, bias_real, bias_pert, dt);
+ tang = diff(Delta, Delta_pert); // Delta(bias + pert) -- Delta(bias)
+ J_n.col(i) = tang/dx; // ( Delta(bias + pert) -- Delta(bias) ) / dx
+ }
+
+ // check that numerical and analytical match
+ ASSERT_MATRIX_APPROX(J_a, J_n, 1e-4);
+}
+
+TEST(IMU_tools, delta_correction)
+{
+ VectorXs Delta(10), Delta_preint(10), Delta_corr; // deltas
+ VectorXs bias(6), pert(6), bias_preint(6);
+ VectorXs body(6), motion(6);
+ VectorXs tang(9); // tangents
+ Matrix<Scalar, 9, 6> J_b; // analytic and numerical jacs
+ Vector4s qv;;
+ Scalar dt = 0.001;
+ Quaternions q0(3, 4, 5, 6); q0.normalize();
+ motion << 1, 2, 3, 3, 2, 1; motion *= .1;
+
+ int N = 1000; // # steps of integration
+
+ // preintegration with correct bias
+ bias << .004, .005, .006, .001, .002, .003;
+ Delta = integrateDelta(N, q0, motion, bias, bias, dt);
+
+ // preintegration with wrong bias
+ pert << .002, -.001, .003, -.0003, .0002, -.0001;
+ bias_preint = bias + pert;
+ Delta_preint = integrateDelta(N, q0, motion, bias, bias_preint, dt, J_b);
+
+ // correct perturbated
+ Vector9s step = J_b*(bias-bias_preint);
+ Delta_corr = plus(Delta_preint, step);
+
+ // Corrected delta should match real delta
+ ASSERT_MATRIX_APPROX(Delta, Delta_corr, 1e-5);
+
+ // diff between real and corrected should be zero
+ ASSERT_MATRIX_APPROX(diff(Delta, Delta_corr), Vector9s::Zero(), 1e-5);
+
+ // diff between preint and corrected deltas should be the jacobian-computed step
+ ASSERT_MATRIX_APPROX(diff(Delta_preint, Delta_corr), step, 1e-5);
+}
+
+TEST(IMU_tools, full_delta_residual)
+{
+ VectorXs x1(10), x2(10);
+ VectorXs Delta(10), Delta_preint(10), Delta_corr(10);
+ VectorXs Delta_real(9), Delta_err(9), Delta_diff(10), Delta_exp(10);
+ VectorXs bias(6), pert(6), bias_preint(6), bias_null(6);
+ VectorXs body(6), motion(6);
+ VectorXs tang(9); // tangents
+ Matrix<Scalar, 9, 6> J_b; // analytic and numerical jacs
+ Scalar dt = 0.001;
+ Quaternions q0; q0.setIdentity();
+
+ x1 << 0,0,0, 0,0,0,1, 0,0,0;
+ motion << .3, .2, .1, .1, .2, .3; // acc and gyro
+// motion << .3, .2, .1, .0, .0, .0; // only acc
+// motion << .0, .0, .0, .1, .2, .3; // only gyro
+ bias << 0.01, 0.02, 0.003, 0.002, 0.005, 0.01;
+ bias_null << 0, 0, 0, 0, 0, 0;
+// bias_preint << 0.003, 0.002, 0.001, 0.001, 0.002, 0.003;
+ bias_preint = bias_null;
+
+ int N = 1000; // # steps of integration
+
+ // preintegration with no bias
+ Delta_real = integrateDelta(N, q0, motion, bias_null, bias_null, dt);
+
+ // final state
+ x2 = composeOverState(x1, Delta_real, 1000*dt);
+
+ // preintegration with the correct bias
+ Delta = integrateDelta(N, q0, motion, bias, bias, dt);
+
+ ASSERT_MATRIX_APPROX(Delta, Delta_real, 1e-4);
+
+ // preintegration with wrong bias
+ Delta_preint = integrateDelta(N, q0, motion, bias, bias_preint, dt, J_b);
+
+ // compute correction step
+ Vector9s step = J_b*(bias-bias_preint);
+
+ // correct preintegrated delta
+ Delta_corr = plus(Delta_preint, step);
+
+ // Corrected delta should match real delta
+ ASSERT_MATRIX_APPROX(Delta_real, Delta_corr, 1e-3);
+
+ // diff between real and corrected should be zero
+ ASSERT_MATRIX_APPROX(diff(Delta_real, Delta_corr), Vector9s::Zero(), 1e-3);
+
+ // expected delta
+ Delta_exp = betweenStates(x1, x2, N*dt);
+
+ ASSERT_MATRIX_APPROX(Delta_exp, Delta_corr, 1e-3);
+
+ // compute diff between expected and corrected
+ Matrix<Scalar, 9, 9> J_err_exp, J_err_corr;
+ diff(Delta_exp, Delta_corr, Delta_err, J_err_exp, J_err_corr);
+
+ ASSERT_MATRIX_APPROX(Delta_err, Vector9s::Zero(), 1e-3);
+
+ // compute error between expected and preintegrated
+ Delta_err = diff(Delta_preint, Delta_exp);
+
+ // diff between preint and corrected deltas should be the jacobian-computed step
+ ASSERT_MATRIX_APPROX(diff(Delta_preint, Delta_corr), step, 1e-3);
+ /* This implies:
+ * - Since D_corr is expected to be similar to D_exp
+ * step ~ diff(Delta_preint, Delta_exp)
+ * - the residual can be computed with a regular '-' :
+ * residual = diff(D_preint, D_exp) - J_bias * (bias - bias_preint)
+ */
+
+// WOLF_TRACE("Delta real: ", Delta_real.transpose());
+// WOLF_TRACE("x2: ", x2.transpose());
+// WOLF_TRACE("Delta: ", Delta.transpose());
+// WOLF_TRACE("Delta pre: ", Delta_preint.transpose());
+// WOLF_TRACE("Jac bias: \n", J_b);
+// WOLF_TRACE("Delta step: ", step.transpose());
+// WOLF_TRACE("Delta cor: ", Delta_corr.transpose());
+// WOLF_TRACE("Delta exp: ", Delta_exp.transpose());
+// WOLF_TRACE("Delta err: ", Delta_err.transpose());
+// WOLF_TRACE("Delta err exp-pre: ", Delta_err.transpose());
+}
+
int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);
+// ::testing::GTEST_FLAG(filter) = "IMU_tools.delta_correction";
return RUN_ALL_TESTS();
}
diff --git a/src/test/gtest_rotation.cpp b/src/test/gtest_rotation.cpp
index c9028feb65b51be8f7823ae29cb841b6a6b93640..a241b462b6f7a0cc5d344741f32bb33ea888bed9 100644
--- a/src/test/gtest_rotation.cpp
+++ b/src/test/gtest_rotation.cpp
@@ -583,7 +583,7 @@ TEST(rotations, q2R_R2q)
ASSERT_MATRIX_APPROX(R, qq_R.matrix(), wolf::Constants::EPS);
}
-TEST(rotations, Jr)
+TEST(Jacobians, Jr)
{
Vector3s theta; theta.setRandom();
Vector3s dtheta; dtheta.setRandom(); dtheta *= 1e-4;
@@ -595,7 +595,7 @@ TEST(rotations, Jr)
ASSERT_MATRIX_APPROX(exp_R(theta+dtheta), (exp_R(theta) * exp_R(Jr*dtheta)), 1e-8);
}
-TEST(rotations, Jl)
+TEST(Jacobians, Jl)
{
Vector3s theta; theta.setRandom();
Vector3s dtheta; dtheta.setRandom(); dtheta *= 1e-4;
@@ -605,9 +605,15 @@ TEST(rotations, Jl)
Matrix3s Jl = jac_SO3_left(theta);
ASSERT_QUATERNION_APPROX(exp_q(theta+dtheta), exp_q(Jl*dtheta) * exp_q(theta), 1e-8);
ASSERT_MATRIX_APPROX(exp_R(theta+dtheta), (exp_R(Jl*dtheta) * exp_R(theta)), 1e-8);
+
+ // Jl = Jr.tr
+ ASSERT_MATRIX_APPROX(Jl, jac_SO3_right(theta).transpose(), 1e-8);
+
+ // Jl = R*Jr
+ ASSERT_MATRIX_APPROX(Jl, exp_R(theta)*jac_SO3_right(theta), 1e-8);
}
-TEST(rotations, Jr_inv)
+TEST(Jacobians, Jr_inv)
{
Vector3s theta; theta.setRandom();
Vector3s dtheta; dtheta.setRandom(); dtheta *= 1e-4;
@@ -621,7 +627,7 @@ TEST(rotations, Jr_inv)
ASSERT_MATRIX_APPROX(log_R(R * exp_R(dtheta)), log_R(R) + Jr_inv*dtheta, 1e-8);
}
-TEST(rotations, Jl_inv)
+TEST(Jacobians, Jl_inv)
{
Vector3s theta; theta.setRandom();
Vector3s dtheta; dtheta.setRandom(); dtheta *= 1e-4;
@@ -635,6 +641,78 @@ TEST(rotations, Jl_inv)
ASSERT_MATRIX_APPROX(log_R(exp_R(dtheta) * R), log_R(R) + Jl_inv*dtheta, 1e-8);
}
+TEST(Jacobians, compose)
+{
+
+ Vector3s th1(.1,.2,.3), th2(.3,.1,.2);
+ Quaternions q1(exp_q(th1));
+ Quaternions q2(exp_q(th2));
+ Quaternions qc;
+ Matrix3s J1a, J2a, J1n, J2n;
+
+ // composition and analytic Jacobians
+ wolf::compose(q1, q2, qc, J1a, J2a);
+
+ // Numeric Jacobians
+ Scalar dx = 1e-6;
+ Vector3s pert;
+ Quaternions q1_pert, q2_pert, qc_pert;
+ for (int i = 0; i<3; i++)
+ {
+ pert.setZero();
+ pert(i) = dx;
+
+ // Jac wrt q1
+ q1_pert = q1*exp_q(pert);
+ qc_pert = q1_pert * q2;
+ J1n.col(i) = log_q(qc.conjugate()*qc_pert) / dx;
+
+ // Jac wrt q2
+ q2_pert = q2*exp_q(pert);
+ qc_pert = q1 * q2_pert;
+ J2n.col(i) = log_q(qc.conjugate()*qc_pert) / dx;
+ }
+
+ ASSERT_MATRIX_APPROX(J1a, J1n, 1e-5);
+ ASSERT_MATRIX_APPROX(J2a, J2n, 1e-5);
+}
+
+TEST(Jacobians, between)
+{
+
+ Vector3s th1(.1,.2,.3), th2(.3,.1,.2);
+ Quaternions q1(exp_q(th1));
+ Quaternions q2(exp_q(th2));
+ Quaternions qc;
+ Matrix3s J1a, J2a, J1n, J2n;
+
+ // composition and analytic Jacobians
+ wolf::between(q1, q2, qc, J1a, J2a);
+
+ // Numeric Jacobians
+ Scalar dx = 1e-6;
+ Vector3s pert;
+ Quaternions q1_pert, q2_pert, qc_pert;
+ for (int i = 0; i<3; i++)
+ {
+ pert.setZero();
+ pert(i) = dx;
+
+ // Jac wrt q1
+ q1_pert = q1*exp_q(pert);
+ qc_pert = q1_pert.conjugate() * q2;
+ J1n.col(i) = log_q(qc.conjugate()*qc_pert) / dx;
+
+ // Jac wrt q2
+ q2_pert = q2*exp_q(pert);
+ qc_pert = q1.conjugate() * q2_pert;
+ J2n.col(i) = log_q(qc.conjugate()*qc_pert) / dx;
+ }
+
+ ASSERT_MATRIX_APPROX(J1a, J1n, 1e-5);
+ ASSERT_MATRIX_APPROX(J2a, J2n, 1e-5);
+}
+
int main(int argc, char **argv)
{