diff --git a/src/problem.cpp b/src/problem.cpp
index fc92b0f7c20e7ad1b1c829216615c6ed80f5fc22..513cc64c2c4628b26eb6cda6eb63d55a08164063 100644
--- a/src/problem.cpp
+++ b/src/problem.cpp
@@ -235,13 +235,6 @@ Eigen::VectorXs Problem::getCurrentState()
return state;
}
-Eigen::VectorXs Problem::getCurrentStateAndStamp(TimeStamp& ts)
-{
- Eigen::VectorXs state(getFrameStructureSize());
- getCurrentStateAndStamp(state, ts);
- return state;
-}
-
void Problem::getCurrentState(Eigen::VectorXs& state)
{
assert(state.size() == getFrameStructureSize() && "Problem::getCurrentState: bad state size");
@@ -260,7 +253,10 @@ void Problem::getCurrentStateAndStamp(Eigen::VectorXs& state, TimeStamp& ts)
assert(state.size() == getFrameStructureSize() && "Problem::getCurrentState: bad state size");
if (processor_motion_ptr_ != nullptr)
- processor_motion_ptr_->getCurrentStateAndStamp(state, ts);
+ {
+ processor_motion_ptr_->getCurrentState(state);
+ processor_motion_ptr_->getCurrentTimeStamp(ts);
+ }
else if (trajectory_ptr_->getLastKeyFramePtr() != nullptr)
{
trajectory_ptr_->getLastKeyFramePtr()->getTimeStamp(ts);
@@ -300,39 +296,12 @@ Eigen::VectorXs Problem::getState(const TimeStamp& _ts)
Size Problem::getFrameStructureSize() const
{
return state_size_;
-// if (trajectory_ptr_->getFrameStructure() == "PO 2D")
-// return 3;
-// if (trajectory_ptr_->getFrameStructure() == "PO 3D")
-// return 7;
-// if (trajectory_ptr_->getFrameStructure() == "POV 3D")
-// return 10;
-// throw std::runtime_error(
-// "Problem::getFrameStructureSize(): Unknown frame structure. Add appropriate frame structure to the switch statement.");
}
void Problem::getFrameStructureSize(Size& _x_size, Size& _cov_size) const
{
_x_size = state_size_;
_cov_size = state_cov_size_;
-
-// if (trajectory_ptr_->getFrameStructure() == "PO 2D")
-// {
-// _x_size = 3;
-// _cov_size = 3;
-// }
-// else if (trajectory_ptr_->getFrameStructure() == "PO 3D")
-// {
-// _x_size = 7;
-// _cov_size = 6;
-// }
-// else if (trajectory_ptr_->getFrameStructure() == "POV 3D")
-// {
-// _x_size = 10;
-// _cov_size = 9;
-// }
-// else
-// throw std::runtime_error(
-// "Problem::getFrameStructureSize(): Unknown frame structure. Add appropriate frame structure to the switch statement.");
}
Eigen::VectorXs Problem::zeroState()
@@ -573,9 +542,9 @@ Eigen::MatrixXs Problem::getFrameCovariance(FrameBasePtr _frame_ptr)
for (const auto& sb : _frame_ptr->getStateBlockVec())
if (sb)
sz += sb->getSize();
+
Eigen::MatrixXs covariance(sz, sz);
-// Eigen::MatrixXs covariance = Eigen::MatrixXs::Zero(_frame_ptr->getPPtr()->getSize()+_frame_ptr->getOPtr()->getSize(), _frame_ptr->getPPtr()->getSize()+_frame_ptr->getOPtr()->getSize());
getFrameCovariance(_frame_ptr, covariance);
return covariance;
}
diff --git a/src/problem.h b/src/problem.h
index d46e1f6d6785c7f487e966a7ac9b24ae46e1017e..86c56553641ee955bf95cbb0a2bbb07f2dc8f4cc 100644
--- a/src/problem.h
+++ b/src/problem.h
@@ -203,7 +203,6 @@ class Problem : public std::enable_shared_from_this<Problem>
// State getters
Eigen::VectorXs getCurrentState();
void getCurrentState(Eigen::VectorXs& state);
- Eigen::VectorXs getCurrentStateAndStamp(TimeStamp& _ts);
void getCurrentStateAndStamp(Eigen::VectorXs& state, TimeStamp& _ts);
Eigen::VectorXs getState(const TimeStamp& _ts);
void getState(const TimeStamp& _ts, Eigen::VectorXs& state);
diff --git a/src/processor_imu.cpp b/src/processor_imu.cpp
index 2bbf52756946875b517ebe8eb129ab13f4277549..a2d526cbd7c1fb4f6a01b96bcf650302ef97ca84 100644
--- a/src/processor_imu.cpp
+++ b/src/processor_imu.cpp
@@ -1,4 +1,5 @@
#include "processor_imu.h"
+#include "imu_tools.h"
namespace wolf {
diff --git a/src/processor_imu.h b/src/processor_imu.h
index f46dcc423bd9038da49db1bf09ad2c59e7f79010..47decd316b1f420611629e390d783c63c64a337c 100644
--- a/src/processor_imu.h
+++ b/src/processor_imu.h
@@ -106,6 +106,7 @@ class ProcessorIMU : public ProcessorMotion{
#include "constraint_imu.h"
#include "state_block.h"
#include "rotations.h"
+#include "imu_tools.h"
namespace wolf{
diff --git a/src/processor_motion.cpp b/src/processor_motion.cpp
index f00fbfb36d3d3b55d1f49259622dd07589ee0427..0e10ad72a2af54de99ed6dab4c286032800e3afb 100644
--- a/src/processor_motion.cpp
+++ b/src/processor_motion.cpp
@@ -70,7 +70,7 @@ void ProcessorMotion::process(CaptureBasePtr _incoming_ptr)
status_ = RUNNING;
}
- incoming_ptr_ = getIncomingCaptureMotion(_incoming_ptr);
+ incoming_ptr_ = std::static_pointer_cast<CaptureMotion>(_incoming_ptr);
/// @todo Anything else to do ?
if (incoming_ptr_ == nullptr) return;
@@ -402,11 +402,6 @@ void ProcessorMotion::reintegrateBuffer(CaptureMotionPtr _capture_ptr)
}
}
-CaptureMotionPtr ProcessorMotion::getIncomingCaptureMotion(CaptureBasePtr& _incoming_ptr)
-{
- return std::static_pointer_cast<CaptureMotion>(_incoming_ptr);
-}
-
CaptureMotionPtr ProcessorMotion::getCaptureMotionContainingTimeStamp(const TimeStamp& _ts)
{
// We need to search in previous keyframes for the capture containing a motion buffer with the queried time stamp
diff --git a/src/processor_motion.h b/src/processor_motion.h
index 51e2fdb491f19d3291a170a6cf7306f069491f13..412b36f64d6013b2c0a2efa613a8a4c82c7c0a19 100644
--- a/src/processor_motion.h
+++ b/src/processor_motion.h
@@ -131,17 +131,13 @@ class ProcessorMotion : public ProcessorBase
* \param _x the returned state vector
*/
void getCurrentState(Eigen::VectorXs& _x);
+ void getCurrentTimeStamp(TimeStamp& _ts){ _ts = getBuffer().get().back().ts_; }
- /** \brief Get a constant reference to the state integrated so far
+ /** \brief Get the state integrated so far
* \return the state vector
*/
Eigen::VectorXs getCurrentState();
-
- /** \brief Fill a reference to the state integrated so far and its stamp
- * \param _x the returned state vector
- * \param _ts the returned stamp
- */
- void getCurrentStateAndStamp(Eigen::VectorXs& _x, TimeStamp& _ts);
+ wolf::TimeStamp getCurrentTimeStamp();
/** \brief Fill the state corresponding to the provided time-stamp
* \param _ts the time stamp
@@ -225,14 +221,6 @@ class ProcessorMotion : public ProcessorBase
*/
virtual void postProcess() { };
- /**
- * @brief Get the incoming CaptureBasePtr and returns a CaptureMotionPtr out of it.
- * If not overloaded, the base class calls
- * std::static_pointer_cast<CaptureMotion>(_incoming_ptr)
- * @return CaptureMotionPtr.
- */
- virtual CaptureMotionPtr getIncomingCaptureMotion(CaptureBasePtr& _incoming_ptr);
-
// These are the pure virtual functions doing the mathematics
protected:
@@ -291,12 +279,12 @@ class ProcessorMotion : public ProcessorBase
* where `F_data = d_f/d_data` is the Jacobian of `f()`.
*/
virtual void 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) = 0;
+ const Eigen::MatrixXs& _data_cov,
+ const Eigen::VectorXs& _calib,
+ const Scalar _dt,
+ Eigen::VectorXs& _delta,
+ Eigen::MatrixXs& _delta_cov,
+ Eigen::MatrixXs& _jacobian_calib) = 0;
/** \brief composes a delta-state on top of another delta-state
* \param _delta1 the first delta-state
@@ -433,8 +421,6 @@ class ProcessorMotion : public ProcessorBase
Eigen::MatrixXs jacobian_calib_; ///< jacobian of delta preintegration wrt calibration params
Eigen::MatrixXs jacobian_delta_calib_; ///< jacobian of delta wrt calib params
- private:
- wolf::TimeStamp getCurrentTimeStamp();
};
}
@@ -509,12 +495,6 @@ inline void ProcessorMotion::getCurrentState(Eigen::VectorXs& _x)
statePlusDelta(origin_ptr_->getFramePtr()->getState(), last_ptr_->getDeltaCorrected(origin_ptr_->getCalibration()), Dt, _x);
}
-inline void ProcessorMotion::getCurrentStateAndStamp(Eigen::VectorXs& _x, TimeStamp& _ts)
-{
- getCurrentState(_x);
- _ts = getCurrentTimeStamp();
-}
-
inline const Eigen::MatrixXs ProcessorMotion::getCurrentDeltaPreintCov()
{
return getBuffer().get().back().delta_integr_cov_;
diff --git a/src/test/gtest_constraint_imu.cpp b/src/test/gtest_constraint_imu.cpp
index 33702a760048897b6781b6b4bd4a429429248d38..0d5cf8b78e753a540240a514f49bc48da44f5c9d 100644
--- a/src/test/gtest_constraint_imu.cpp
+++ b/src/test/gtest_constraint_imu.cpp
@@ -110,6 +110,7 @@ class ConstraintIMU_biasTest_Static_NullBias : public testing::Test
// process data in capture
sen_imu->process(imu_ptr);
+
}
KF1 = wolf_problem_ptr_->getTrajectoryPtr()->closestKeyFrameToTimeStamp(t);
@@ -824,6 +825,7 @@ class ConstraintIMU_biasTest_Move_NonNullBiasRot : public testing::Test
data_imu = data_imu + origin_bias;
imu_ptr->setData(data_imu);
sen_imu->process(imu_ptr);
+
}
expected_final_state << 0,0,0, quat_current.x(), quat_current.y(), quat_current.z(), quat_current.w(), 0,0,0;
@@ -1100,6 +1102,7 @@ class ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRotY : public testing::Test
Eigen::Vector6s data_imu(Eigen::Vector6s::Zero()), data_odom3D(Eigen::Vector6s::Zero());
Eigen::Vector3s rateOfTurn; //deg/s
rateOfTurn << 0,90,0;
+ VectorXs D_cor(10);
Scalar dt(0.0010), dt_odom(1.0);
TimeStamp ts(0.0), t_odom(0.0);
@@ -1129,8 +1132,12 @@ class ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRotY : public testing::Test
imu_ptr->setData(data_imu);
sen_imu->process(imu_ptr);
+ D_cor = processor_ptr_imu->getLastPtr()->getDeltaCorrected(origin_bias);
+
if(ts.get() >= t_odom.get())
{
+ WOLF_TRACE("X_preint(t) : ", wolf_problem_ptr_->getState(ts).transpose());
+
// PROCESS ODOM 3D DATA
data_odom3D.head(3) << 0,0,0;
data_odom3D.tail(3) = q2v(odom_quat);
@@ -1148,6 +1155,9 @@ class ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRotY : public testing::Test
last_KF = wolf_problem_ptr_->getTrajectoryPtr()->closestKeyFrameToTimeStamp(ts);
last_KF->setState(expected_final_state);
+ WOLF_TRACE("X_correct(t) : ", imu::composeOverState(x_origin, D_cor, ts - t).transpose());
+ WOLF_TRACE("X_true(t) : ", expected_final_state.transpose());
+
//===================================================== END{PROCESS DATA}
origin_KF->unfix();
last_KF->unfix();