Merge branch '2-adapt-to-emplace-api' into 'devel'

Resolve "Adapt to emplace API"

Closes #2

See merge request !3

include/IMU/processor/processor_IMU.h

@@ -62,14 +62,17 @@ class ProcessorIMU : public ProcessorMotion{
                                    Motion& _motion,
                                    TimeStamp& _ts) override;
         virtual bool voteForKeyFrame() override;
-        virtual CaptureMotionPtr createCapture(const TimeStamp& _ts,
-                                               const SensorBasePtr& _sensor,
-                                               const VectorXs& _data,
-                                               const MatrixXs& _data_cov,
-                                               const FrameBasePtr& _frame_origin) override;
-        virtual FeatureBasePtr createFeature(CaptureMotionPtr _capture_motion) override;
+        virtual CaptureMotionPtr emplaceCapture(const FrameBasePtr& _frame_own,
+                                                const SensorBasePtr& _sensor,
+                                                const TimeStamp& _ts,
+                                                const VectorXs& _data,
+                                                const MatrixXs& _data_cov,
+                                                const VectorXs& _calib,
+                                                const VectorXs& _calib_preint,
+                                                const FrameBasePtr& _frame_origin) override;
+        virtual FeatureBasePtr emplaceFeature(CaptureMotionPtr _capture_motion) override;
         virtual FactorBasePtr emplaceFactor(FeatureBasePtr _feature_motion,
-                                                    CaptureBasePtr _capture_origin) override;
+                                            CaptureBasePtr _capture_origin) override;
 
     protected:
         ProcessorParamsIMUPtr params_motion_IMU_;

src/feature/feature_IMU.cpp

@@ -12,8 +12,6 @@ FeatureIMU::FeatureIMU(const Eigen::VectorXs& _delta_preintegrated,
     gyro_bias_preint_(_bias.tail<3>()),
     jacobian_bias_(_dD_db_jacobians)
 {
-    if (_cap_imu_ptr)
-        this->setCapture(_cap_imu_ptr);
 }
 
 FeatureIMU::FeatureIMU(CaptureMotionPtr _cap_imu_ptr):
@@ -22,7 +20,6 @@ FeatureIMU::FeatureIMU(CaptureMotionPtr _cap_imu_ptr):
         gyro_bias_preint_(_cap_imu_ptr->getCalibrationPreint().tail<3>()),
         jacobian_bias_(_cap_imu_ptr->getJacobianCalib())
 {
-    this->setCapture(_cap_imu_ptr);
 }
 
 FeatureIMU::~FeatureIMU()

src/processor/processor_IMU.cpp

@@ -185,27 +185,34 @@ Motion ProcessorIMU::interpolate(const Motion& _motion_ref, Motion& _motion_seco
 
 }
 
-CaptureMotionPtr ProcessorIMU::createCapture(const TimeStamp& _ts,
-                                             const SensorBasePtr& _sensor,
-                                             const VectorXs& _data,
-                                             const MatrixXs& _data_cov,
-                                             const FrameBasePtr& _frame_origin)
+CaptureMotionPtr ProcessorIMU::emplaceCapture(const FrameBasePtr& _frame_own,
+                                              const SensorBasePtr& _sensor,
+                                              const TimeStamp& _ts,
+                                              const VectorXs& _data,
+                                              const MatrixXs& _data_cov,
+                                              const VectorXs& _calib,
+                                              const VectorXs& _calib_preint,
+                                              const FrameBasePtr& _frame_origin)
 {
-    CaptureIMUPtr capture_imu = std::make_shared<CaptureIMU>(_ts,
-                                                             _sensor,
-                                                             _data,
-                                                             _data_cov,
-                                                             _frame_origin);
-    return capture_imu;
+    auto cap_motion = std::static_pointer_cast<CaptureMotion>(CaptureBase::emplace<CaptureIMU>(_frame_own,
+                                                                                               _ts,
+                                                                                               _sensor,
+                                                                                               _data,
+                                                                                               _data_cov,
+                                                                                               _frame_origin));
+    cap_motion->setCalibration(_calib);
+    cap_motion->setCalibrationPreint(_calib_preint);
+
+    return cap_motion;
 }
 
-FeatureBasePtr ProcessorIMU::createFeature(CaptureMotionPtr _capture_motion)
+FeatureBasePtr ProcessorIMU::emplaceFeature(CaptureMotionPtr _capture_motion)
 {
-    FeatureIMUPtr feature = std::make_shared<FeatureIMU>(
-            _capture_motion->getBuffer().get().back().delta_integr_,
-            _capture_motion->getBuffer().get().back().delta_integr_cov_ + unmeasured_perturbation_cov_,
-            _capture_motion->getBuffer().getCalibrationPreint(),
-            _capture_motion->getBuffer().get().back().jacobian_calib_);
+    auto feature = FeatureBase::emplace<FeatureIMU>(_capture_motion,
+                                                    _capture_motion->getBuffer().get().back().delta_integr_,
+                                                    _capture_motion->getBuffer().get().back().delta_integr_cov_ + unmeasured_perturbation_cov_,
+                                                    _capture_motion->getBuffer().getCalibrationPreint(),
+                                                    _capture_motion->getBuffer().get().back().jacobian_calib_);
     return feature;
 }
 
@@ -213,13 +220,8 @@ FactorBasePtr ProcessorIMU::emplaceFactor(FeatureBasePtr _feature_motion, Captur
 {
     CaptureIMUPtr cap_imu = std::static_pointer_cast<CaptureIMU>(_capture_origin);
     FeatureIMUPtr ftr_imu = std::static_pointer_cast<FeatureIMU>(_feature_motion);
-    // FactorIMUPtr fac_imu = std::make_shared<FactorIMU>(ftr_imu, cap_imu, shared_from_this());
-    auto fac_imu = FactorBase::emplace<FactorIMU>(_feature_motion, ftr_imu, cap_imu, shared_from_this());
 
-    // link ot wolf tree
-    // _feature_motion->addFactor(fac_imu);
-    // _capture_origin->addConstrainedBy(fac_imu);
-    // _capture_origin->getFrame()->addConstrainedBy(fac_imu);
+    auto fac_imu = FactorBase::emplace<FactorIMU>(_feature_motion, ftr_imu, cap_imu, shared_from_this());
 
     return fac_imu;
 }

test/gtest_feature_IMU.cpp

@@ -58,10 +58,15 @@ class FeatureIMU_test : public testing::Test
         MatrixXs P0; P0.setIdentity(9,9);
         origin_frame = problem->setPrior(x0, P0, 0.0, 0.01);
 
-    // Create one capture to store the IMU data arriving from (sensor / callback / file / etc.)
+    // Emplace 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
-        imu_ptr = std::make_shared<CaptureIMU>(t, sensor_ptr, data_, Eigen::Matrix6s::Identity(), Eigen::Vector6s::Zero());
-        imu_ptr->setFrame(origin_frame); //to get ptr to Frm in processorIMU and then get biases
+        imu_ptr = std::static_pointer_cast<CaptureIMU>(
+                CaptureBase::emplace<CaptureIMU>(origin_frame,
+                                                 t,
+                                                 sensor_ptr,
+                                                 data_,
+                                                 Eigen::Matrix6s::Identity(),
+                                                 Eigen::Vector6s::Zero()) );
 
     //process data
         data_ << 2, 0, 9.8, 0, 0, 0;
@@ -74,23 +79,23 @@ class FeatureIMU_test : public testing::Test
     // process data in capture
         sensor_ptr->process(imu_ptr);
 
-    //create Frame
+    //emplace Frame
         ts = problem->getProcessorMotion()->getBuffer().get().back().ts_;
         state_vec = problem->getProcessorMotion()->getCurrentState();
         last_frame = problem->emplaceFrame(KEY, state_vec, ts);
 
-    //create a feature
+    //emplace a feature
         delta_preint = problem->getProcessorMotion()->getMotion().delta_integr_;
         delta_preint_cov = problem->getProcessorMotion()->getMotion().delta_integr_cov_ + MatrixXs::Identity(9,9)*1e-08;
         VectorXs calib_preint = problem->getProcessorMotion()->getBuffer().getCalibrationPreint();
         dD_db_jacobians = problem->getProcessorMotion()->getMotion().jacobian_calib_;
-        feat_imu = std::make_shared<FeatureIMU>(delta_preint,
-                                                delta_preint_cov,
-                                                calib_preint,
-                                                dD_db_jacobians,
-                                                imu_ptr);
-        feat_imu->setCapture(imu_ptr); //associate the feature to a capture
-
+        feat_imu = std::static_pointer_cast<FeatureIMU>(
+                FeatureBase::emplace<FeatureIMU>(imu_ptr,
+                                                 delta_preint,
+                                                 delta_preint_cov,
+                                                 calib_preint,
+                                                 dD_db_jacobians,
+                                                 imu_ptr) );
     }
 
     virtual void TearDown()