diff --git a/include/core/processor/processor_motion.h b/include/core/processor/processor_motion.h
index 1f62a82a39dfcf5ae2de6183d341051d5785b45c..6a0f15a90885f5438c18c2cf01ed714c488ba599 100644
--- a/include/core/processor/processor_motion.h
+++ b/include/core/processor/processor_motion.h
@@ -247,11 +247,11 @@ class ProcessorMotion : public ProcessorBase, public IsMotion
double updateDt();
void integrateOneStep();
- void reintegrateBuffer(CaptureMotionPtr _capture_ptr);
+ void reintegrateBuffer(CaptureMotionPtr _capture_ptr) const;
void splitBuffer(const wolf::CaptureMotionPtr& capture_source,
TimeStamp ts_split,
const FrameBasePtr& keyframe_target,
- const wolf::CaptureMotionPtr& capture_target);
+ const wolf::CaptureMotionPtr& capture_target) const;
/** Pre-process incoming Capture
*
@@ -416,14 +416,26 @@ class ProcessorMotion : public ProcessorBase, public IsMotion
virtual Eigen::VectorXd correctDelta(const Eigen::VectorXd& delta_preint,
const Eigen::VectorXd& delta_step) const = 0;
+ /** \brief merge two consecutive capture motions into the second one
+ * Merge two consecutive capture motions into the second one.
+ * The first capture motion is not removed.
+ * If the second capture has feature and factor emplaced, they are replaced by a new ones.
+ * @param cap_prev : the first capture motion to be merged (input)
+ * @param cap_target : the second capture motion (modified)
+ */
+ void mergeCaptures(CaptureMotionConstPtr cap_prev,
+ CaptureMotionPtr cap_target);
+
protected:
/** \brief emplace a CaptureMotion
- * \param _ts time stamp
+ * \param _frame_own frame owning the Capture to create
* \param _sensor Sensor that produced the Capture
+ * \param _ts time stamp
* \param _data a vector of motion data
- * \param _sata_cov covariances matrix of the motion data data
- * \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
+ * \param _data_cov covariances matrix of the motion data data
+ * \param _calib calibration vector
+ * \param _calib_preint calibration vector used during pre-integration
+ * \param _capture_origin_ptr capture acting as the origin of motions for the MorionBuffer of the created MotionCapture
*/
virtual CaptureMotionPtr emplaceCapture(const FrameBasePtr& _frame_own,
const SensorBasePtr& _sensor,
@@ -491,18 +503,19 @@ class ProcessorMotion : public ProcessorBase, public IsMotion
protected:
// helpers to avoid allocation
- double dt_; ///< Time step
- Eigen::VectorXd x_; ///< current state
- Eigen::VectorXd delta_; ///< current delta
- Eigen::MatrixXd delta_cov_; ///< current delta covariance
- Eigen::VectorXd delta_integrated_; ///< integrated delta
- Eigen::MatrixXd delta_integrated_cov_; ///< integrated delta covariance
- Eigen::VectorXd calib_preint_; ///< calibration vector used during pre-integration
- Eigen::MatrixXd jacobian_delta_preint_; ///< jacobian of delta composition w.r.t previous delta integrated
- Eigen::MatrixXd jacobian_delta_; ///< jacobian of delta composition w.r.t current delta
- Eigen::MatrixXd jacobian_calib_; ///< jacobian of delta preintegration wrt calibration params
- Eigen::MatrixXd jacobian_delta_calib_; ///< jacobian of delta wrt calib params
- Eigen::MatrixXd unmeasured_perturbation_cov_; ///< Covariance of unmeasured DoF to avoid singularity
+ mutable double dt_; ///< Time step
+ mutable Eigen::VectorXd x_; ///< current state
+ mutable Eigen::VectorXd delta_; ///< current delta
+ mutable Eigen::MatrixXd delta_cov_; ///< current delta covariance
+ mutable Eigen::VectorXd delta_integrated_; ///< integrated delta
+ mutable Eigen::MatrixXd delta_integrated_cov_; ///< integrated delta covariance
+ mutable Eigen::VectorXd calib_preint_; ///< calibration vector used during pre-integration
+ mutable Eigen::MatrixXd jacobian_delta_preint_; ///< jacobian of delta composition w.r.t previous delta integrated
+ mutable Eigen::MatrixXd jacobian_delta_; ///< jacobian of delta composition w.r.t current delta
+ mutable Eigen::MatrixXd jacobian_calib_; ///< jacobian of delta preintegration wrt calibration params
+ mutable Eigen::MatrixXd jacobian_delta_calib_; ///< jacobian of delta wrt calib params
+
+ Eigen::MatrixXd unmeasured_perturbation_cov_; ///< Covariance of unmeasured DoF to avoid singularity
};
}
diff --git a/src/processor/processor_motion.cpp b/src/processor/processor_motion.cpp
index f56b8c80a344e864f16be3ca2f8f006a0996052f..3a770b6f10dac4cfd05494725674b32c1f783eea 100644
--- a/src/processor/processor_motion.cpp
+++ b/src/processor/processor_motion.cpp
@@ -54,10 +54,46 @@ ProcessorMotion::~ProcessorMotion()
// std::cout << "destructed -p-Mot" << id() << std::endl;
}
-void ProcessorMotion::splitBuffer(const wolf::CaptureMotionPtr& _capture_source,
+
+void ProcessorMotion::mergeCaptures(CaptureMotionConstPtr cap_prev,
+ CaptureMotionPtr cap_target)
+{
+ assert(cap_prev == cap_target->getOriginCapture() && "merging not consecutive capture motions");
+
+ // add prev buffer (discarding the first zero motion)
+ cap_target->getBuffer().pop_front();
+ cap_target->getBuffer().insert(cap_target->getBuffer().begin(),
+ cap_prev->getBuffer().begin(),
+ cap_prev->getBuffer().end());
+ cap_target->getBuffer().print();
+
+ // change origin
+ cap_target->setOriginCapture(cap_prev->getOriginCapture());
+
+ // change calibration params for preintegration from origin
+ cap_target->setCalibrationPreint(getCalibration(cap_prev->getOriginCapture()));
+
+ // reintegrate buffer
+ reintegrateBuffer(cap_target);
+
+ // replace existing feature and factor (if they exist)
+ if (!cap_target->getFeatureList().empty())
+ {
+ // remove feature and factor (automatically)
+ cap_target->getFeatureList().back()->remove();
+
+ assert(cap_target->getFeatureList().empty());// there should be one feature only
+
+ // emplace new feature and factor
+ auto new_feature = emplaceFeature(cap_target);
+ emplaceFactor(new_feature, cap_prev->getOriginCapture());
+ }
+}
+
+void ProcessorMotion::splitBuffer(const CaptureMotionPtr& _capture_source,
TimeStamp _ts_split,
const FrameBasePtr& _keyframe_target,
- const wolf::CaptureMotionPtr& _capture_target)
+ const CaptureMotionPtr& _capture_target) const
{
/** we are doing this:
*
@@ -732,7 +768,7 @@ void ProcessorMotion::integrateOneStep()
statePlusDelta(odometry_, delta_, dt_, odometry_);
}
-void ProcessorMotion::reintegrateBuffer(CaptureMotionPtr _capture_ptr)
+void ProcessorMotion::reintegrateBuffer(CaptureMotionPtr _capture_ptr) const
{
VectorXd calib = _capture_ptr->getCalibrationPreint();
diff --git a/test/gtest_processor_motion.cpp b/test/gtest_processor_motion.cpp
index 743b0fcc56a4088ceb87073a6941200fd74ccd33..d905bcad555b88e4cd92afed94771c2e7f46546d 100644
--- a/test/gtest_processor_motion.cpp
+++ b/test/gtest_processor_motion.cpp
@@ -289,6 +289,83 @@ TEST_F(ProcessorMotion_test, IntegrateCircleFixPrior)
ASSERT_MATRIX_APPROX(problem->getState().vector("PO"), (Vector3d()<<0,0,0).finished(), 1e-8);
}
+TEST_F(ProcessorMotion_test, mergeCaptures)
+{
+ // Prior
+ Vector3d x0; x0 << 0, 0, 0;
+ Matrix3d P0; P0.setIdentity();
+
+ data << 1, 2*M_PI/10; // advance in circle
+ data_cov.setIdentity();
+ TimeStamp t(0.0);
+
+ for (int i = 0; i<10; i++) // one full turn exactly
+ {
+ t += dt;
+ capture->setTimeStamp(t);
+ capture->setData(data);
+ capture->setDataCovariance(data_cov);
+ processor->captureCallback(capture);
+ WOLF_DEBUG("t: ", t, " x: ", problem->getState().vector("PO").transpose());
+ }
+
+ SensorBasePtr S = processor->getSensor();
+
+ TimeStamp t_target = 8.5;
+ FrameBasePtr F_target = problem->emplaceKeyFrame(t_target);
+ CaptureMotionPtr C_source = std::dynamic_pointer_cast<CaptureMotion>(processor->getLast());
+ CaptureMotionPtr C_target = CaptureBase::emplace<CaptureMotion>(F_target,
+ "ODOM 2d",
+ t_target,
+ sensor,
+ data,
+ nullptr);
+
+ // copy initial buffer
+ MotionBuffer initial_buffer = C_source->getBuffer();
+
+ processor->splitBuffer(C_source,
+ t_target,
+ F_target,
+ C_target);
+
+ C_target->getBuffer().print(1,1,1,0);
+ C_source->getBuffer().print(1,1,1,0);
+
+ // merge captures
+ processor->mergeCaptures(C_target, C_source);
+
+ C_target->getBuffer().print(1,1,1,0);
+ C_source->getBuffer().print(1,1,1,0);
+
+ // Check that merged buffer is the initial one (before splitting)
+ EXPECT_EQ(C_source->getBuffer().size(), initial_buffer.size());
+
+ auto init_it = initial_buffer.begin();
+ auto res_it = C_source->getBuffer().begin();
+ while (init_it != initial_buffer.end())
+ {
+ EXPECT_EQ(init_it->data_size_, init_it->data_size_);
+ EXPECT_EQ(init_it->delta_size_, init_it->delta_size_);
+ EXPECT_EQ(init_it->cov_size_, init_it->cov_size_);
+ EXPECT_EQ(init_it->calib_size_, init_it->calib_size_);
+ EXPECT_EQ(init_it->ts_, init_it->ts_);
+
+ EXPECT_MATRIX_APPROX(init_it->data_ ,init_it->data_, 1e-12);
+ EXPECT_MATRIX_APPROX(init_it->data_cov_,init_it->data_cov_, 1e-12);
+ EXPECT_MATRIX_APPROX(init_it->delta_, init_it->delta_, 1e-12);
+ EXPECT_MATRIX_APPROX(init_it->delta_cov_, init_it->delta_cov_, 1e-12);
+ EXPECT_MATRIX_APPROX(init_it->delta_integr_, init_it->delta_integr_, 1e-12);
+ EXPECT_MATRIX_APPROX(init_it->delta_integr_cov_, init_it->delta_integr_cov_, 1e-12);
+ EXPECT_MATRIX_APPROX(init_it->jacobian_delta_, init_it->jacobian_delta_, 1e-12);
+ EXPECT_MATRIX_APPROX(init_it->jacobian_delta_integr_, init_it->jacobian_delta_integr_, 1e-12);
+ EXPECT_MATRIX_APPROX(init_it->jacobian_calib_, init_it->jacobian_calib_, 1e-12);
+
+ init_it++;
+ res_it++;
+ }
+}
+
TEST_F(ProcessorMotion_test, splitBufferAutoPrior)
{
// Prior