diff --git a/include/core/processor/processor_diff_drive.h b/include/core/processor/processor_diff_drive.h
index 60fbf8d295e0dd519e4d5a69cf4dc52e18349811..78ef6ca50c80e653f7143ebe72e310b859d7a18f 100644
--- a/include/core/processor/processor_diff_drive.h
+++ b/include/core/processor/processor_diff_drive.h
@@ -93,16 +93,19 @@ protected:
Motion& _second,
TimeStamp& _ts) override;
- virtual CaptureMotionPtr createCapture(const TimeStamp& _ts,
- const SensorBasePtr& _sensor,
- const VectorXs& _data,
- const MatrixXs& _data_cov,
- const FrameBasePtr& _frame_origin) 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 FactorBasePtr emplaceFactor(FeatureBasePtr _feature,
CaptureBasePtr _capture_origin) override;
- virtual FeatureBasePtr createFeature(CaptureMotionPtr _capture_motion) override;
+ virtual FeatureBasePtr emplaceFeature(CaptureMotionPtr _capture_motion) override;
public:
diff --git a/include/core/processor/processor_motion.h b/include/core/processor/processor_motion.h
index 56269e2041bc18e039dcdd6e64e228dc6aa35cc6..fe77c60535d7124c1c780bf6738a78743e293a1c 100644
--- a/include/core/processor/processor_motion.h
+++ b/include/core/processor/processor_motion.h
@@ -411,7 +411,7 @@ class ProcessorMotion : public ProcessorBase
*/
virtual Motion interpolate(const Motion& _ref1, const Motion& _ref2, const TimeStamp& _ts, Motion& _second);
- /** \brief create a CaptureMotion and add it to a Frame
+ /** \brief emplace a CaptureMotion
* \param _ts time stamp
* \param _sensor Sensor that produced the Capture
* \param _data a vector of motion data
@@ -419,32 +419,21 @@ 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 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);
-
- virtual CaptureMotionPtr createCapture(const TimeStamp& _ts,
- const SensorBasePtr& _sensor,
- const VectorXs& _data,
- const MatrixXs& _data_cov,
- const FrameBasePtr& _frame_origin) = 0;
-
- /** \brief create a feature corresponding to given capture and add the feature to this capture
+ 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) = 0;
+
+ /** \brief emplace a feature corresponding to given capture and add the feature to this capture
* \param _capture_motion: the parent capture
*/
- FeatureBasePtr emplaceFeature(CaptureMotionPtr _capture_own);
+ virtual FeatureBasePtr emplaceFeature(CaptureMotionPtr _capture_own) = 0;
- /** \brief create a feature corresponding to given capture
- * \param _capture_motion: the parent capture
- */
- virtual FeatureBasePtr createFeature(CaptureMotionPtr _capture_own) = 0;
-
- /** \brief create a factor and link it in the wolf tree
+ /** \brief emplace a factor and link it in the wolf tree
* \param _feature_motion: the parent feature
* \param _frame_origin: the frame constrained by this motion factor
*/
diff --git a/include/core/processor/processor_odom_2D.h b/include/core/processor/processor_odom_2D.h
index fdcf820856bb061e445273a72334b20698b9564b..37d6c9ee11c6f9c518ccbcdeb01bc9fa3f2c20a5 100644
--- a/include/core/processor/processor_odom_2D.h
+++ b/include/core/processor/processor_odom_2D.h
@@ -73,12 +73,15 @@ class ProcessorOdom2D : public ProcessorMotion
const TimeStamp& _ts,
Motion& _second) 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,
CaptureBasePtr _capture_origin) override;
diff --git a/include/core/processor/processor_odom_3D.h b/include/core/processor/processor_odom_3D.h
index b5fc2dedf099e74bfbd64e7b28ca313cdd1bd412..1c70a3b3385c82116609fa47d3897ca82d89f800 100644
--- a/include/core/processor/processor_odom_3D.h
+++ b/include/core/processor/processor_odom_3D.h
@@ -95,14 +95,19 @@ class ProcessorOdom3D : public ProcessorMotion
const TimeStamp& _ts,
Motion& _second) override;
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 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;
- virtual FeatureBasePtr createFeature(CaptureMotionPtr _capture_motion) override;
+ CaptureBasePtr _capture_origin) override;
protected:
ProcessorParamsOdom3DPtr params_odom_3D_;
diff --git a/src/processor/processor_diff_drive.cpp b/src/processor/processor_diff_drive.cpp
index 6c508a35a1f25693e7fa350945dab18b0c020cf2..b75dd17e468012c231fd9a8c2ded3fb60fca7588 100644
--- a/src/processor/processor_diff_drive.cpp
+++ b/src/processor/processor_diff_drive.cpp
@@ -195,41 +195,48 @@ Motion ProcessorDiffDrive::interpolate(const Motion& _ref,
}
-CaptureMotionPtr ProcessorDiffDrive::createCapture(const TimeStamp& _ts,
- const SensorBasePtr& _sensor,
- const VectorXs& _data,
- const MatrixXs& _data_cov,
- const FrameBasePtr& _frame_origin)
+CaptureMotionPtr ProcessorDiffDrive::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)
{
-
- StateBlockPtr i_ptr = _sensor->isIntrinsicDynamic()?
- std::make_shared<StateBlock>(3, false) : nullptr;
-
- return std::make_shared<CaptureWheelJointPosition>(_ts, _sensor, _data, _data_cov,
- _frame_origin, nullptr, nullptr, i_ptr);
+ StateBlockPtr i_ptr = _sensor->isIntrinsicDynamic()?
+ std::make_shared<StateBlock>(3, false) : nullptr;
+
+ auto cap_motion = std::static_pointer_cast<CaptureMotion>(CaptureBase::emplace<CaptureWheelJointPosition>(_frame_own,
+ _ts,
+ _sensor,
+ _data,
+ _data_cov,
+ _frame_origin,
+ nullptr,
+ nullptr,
+ i_ptr));
+ cap_motion->setCalibration(_calib);
+ cap_motion->setCalibrationPreint(_calib_preint);
+
+ return cap_motion;
}
FactorBasePtr ProcessorDiffDrive::emplaceFactor(FeatureBasePtr _feature,
CaptureBasePtr _capture_origin)
{
- // FactorOdom2DPtr fac_odom =
- // std::make_shared<FactorOdom2D>(_feature, _capture_origin->getFrame(),
- // shared_from_this());
- auto fac_odom = FactorBase::emplace<FactorOdom2D>(_feature, _feature, _capture_origin->getFrame(),
+ auto fac_odom = FactorBase::emplace<FactorOdom2D>(_feature, _feature, _capture_origin->getFrame(),
shared_from_this());
- // _feature->addFactor(fac_odom);
- // _capture_origin->getFrame()->addConstrainedBy(fac_odom);
-
- return fac_odom;
+ return fac_odom;
}
-FeatureBasePtr ProcessorDiffDrive::createFeature(CaptureMotionPtr _capture_motion)
+FeatureBasePtr ProcessorDiffDrive::emplaceFeature(CaptureMotionPtr _capture_motion)
{
- FeatureBasePtr key_feature_ptr = std::make_shared<FeatureDiffDrive>(
- _capture_motion->getBuffer().get().back().delta_integr_,
- _capture_motion->getBuffer().get().back().delta_integr_cov_,
- _capture_motion->getBuffer().getCalibrationPreint(),
- _capture_motion->getBuffer().get().back().jacobian_calib_);
+ FeatureBasePtr key_feature_ptr = FeatureBase::emplace<FeatureDiffDrive>(_capture_motion,
+ _capture_motion->getBuffer().get().back().delta_integr_,
+ _capture_motion->getBuffer().get().back().delta_integr_cov_,
+ _capture_motion->getBuffer().getCalibrationPreint(),
+ _capture_motion->getBuffer().get().back().jacobian_calib_);
WOLF_INFO(_capture_motion->getBuffer().getCalibrationPreint());
WOLF_INFO(_capture_motion->getBuffer().get().back().jacobian_calib_);
diff --git a/src/processor/processor_motion.cpp b/src/processor/processor_motion.cpp
index e3a115798c5e01cd3b3385aab38b1f499957f253..55d2c16b52f55c68d66a8494a87b7e7c906c5b64 100644
--- a/src/processor/processor_motion.cpp
+++ b/src/processor/processor_motion.cpp
@@ -612,38 +612,6 @@ CaptureMotionPtr ProcessorMotion::findCaptureContainingTimeStamp(const TimeStamp
return capture_motion;
}
-CaptureMotionPtr ProcessorMotion::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)
-{
- CaptureMotionPtr capture = createCapture(_ts,
- _sensor,
- _data,
- _data_cov,
- _frame_origin);
-
- capture->setCalibration(_calib);
- capture->setCalibrationPreint(_calib_preint);
-
- // add to wolf tree
- // _frame_own->addCapture(capture);
- capture->link(_frame_own);
- return capture;
-}
-
-FeatureBasePtr ProcessorMotion::emplaceFeature(CaptureMotionPtr _capture_motion)
-{
- FeatureBasePtr feature = createFeature(_capture_motion);
- // _capture_motion->addFeature(feature);
- feature->link(_capture_motion);
- return feature;
-}
-
PackKeyFramePtr ProcessorMotion::computeProcessingStep()
{
if (!getProblem()->priorIsSet())
diff --git a/src/processor/processor_odom_2D.cpp b/src/processor/processor_odom_2D.cpp
index 9b19d4e8c4eda8db40eac71574534f3419880634..38dccacd9bf4d2d1512de442f79734b30d4c02a6 100644
--- a/src/processor/processor_odom_2D.cpp
+++ b/src/processor/processor_odom_2D.cpp
@@ -144,33 +144,38 @@ bool ProcessorOdom2D::voteForKeyFrame()
return false;
}
-CaptureMotionPtr ProcessorOdom2D::createCapture(const TimeStamp& _ts, const SensorBasePtr& _sensor,
- const VectorXs& _data, const MatrixXs& _data_cov,
- const FrameBasePtr& _frame_origin)
+CaptureMotionPtr ProcessorOdom2D::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)
{
- CaptureOdom2DPtr capture_odom = std::make_shared<CaptureOdom2D>(_ts, _sensor, _data, _data_cov, _frame_origin);
- return capture_odom;
+ auto cap_motion = std::static_pointer_cast<CaptureMotion>(CaptureBase::emplace<CaptureOdom2D>(_frame_own, _ts, _sensor, _data, _data_cov, _frame_origin));
+ cap_motion->setCalibration(_calib);
+ cap_motion->setCalibrationPreint(_calib_preint);
+
+ return cap_motion;
}
FactorBasePtr ProcessorOdom2D::emplaceFactor(FeatureBasePtr _feature, CaptureBasePtr _capture_origin)
{
- // FactorOdom2DPtr fac_odom = std::make_shared<FactorOdom2D>(_feature, _capture_origin->getFrame(),
- // shared_from_this());
auto fac_odom = FactorBase::emplace<FactorOdom2D>(_feature, _feature, _capture_origin->getFrame(),
shared_from_this());
- // _feature->addFactor(fac_odom);
- // _capture_origin->getFrame()->addConstrainedBy(fac_odom);
return fac_odom;
}
-FeatureBasePtr ProcessorOdom2D::createFeature(CaptureMotionPtr _capture_motion)
+FeatureBasePtr ProcessorOdom2D::emplaceFeature(CaptureMotionPtr _capture_motion)
{
Eigen::MatrixXs covariance = _capture_motion->getBuffer().get().back().delta_integr_cov_;
makePosDef(covariance);
- FeatureBasePtr key_feature_ptr = std::make_shared<FeatureBase>("ODOM 2D",
- _capture_motion->getBuffer().get().back().delta_integr_,
- covariance);
+ FeatureBasePtr key_feature_ptr = FeatureBase::emplace<FeatureBase>(_capture_motion,
+ "ODOM 2D",
+ _capture_motion->getBuffer().get().back().delta_integr_,
+ covariance);
return key_feature_ptr;
}
diff --git a/src/processor/processor_odom_3D.cpp b/src/processor/processor_odom_3D.cpp
index 9c29e0f89fb6dba5d4495fd3a4d2b1241812cb68..76fc35d3bc166eb30e878ab2e61b4f4fbe2950e3 100644
--- a/src/processor/processor_odom_3D.cpp
+++ b/src/processor/processor_odom_3D.cpp
@@ -395,33 +395,38 @@ bool ProcessorOdom3D::voteForKeyFrame()
return false;
}
-CaptureMotionPtr ProcessorOdom3D::createCapture(const TimeStamp& _ts, const SensorBasePtr& _sensor,
- const VectorXs& _data, const MatrixXs& _data_cov,
- const FrameBasePtr& _frame_origin)
+CaptureMotionPtr ProcessorOdom3D::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)
{
- CaptureOdom3DPtr capture_odom = std::make_shared<CaptureOdom3D>(_ts, _sensor, _data, _data_cov, _frame_origin);
- return capture_odom;
+ auto cap_motion = std::static_pointer_cast<CaptureMotion>(CaptureBase::emplace<CaptureOdom3D>(_frame_own, _ts, _sensor, _data, _data_cov, _frame_origin));
+ cap_motion->setCalibration(_calib);
+ cap_motion->setCalibrationPreint(_calib_preint);
+
+ return cap_motion;
+}
+
+FeatureBasePtr ProcessorOdom3D::emplaceFeature(CaptureMotionPtr _capture_motion)
+{
+ FeatureBasePtr key_feature_ptr = FeatureBase::emplace<FeatureBase>(_capture_motion,
+ "ODOM 3D",
+ _capture_motion->getBuffer().get().back().delta_integr_,
+ _capture_motion->getBuffer().get().back().delta_integr_cov_);
+ return key_feature_ptr;
}
FactorBasePtr ProcessorOdom3D::emplaceFactor(FeatureBasePtr _feature_motion, CaptureBasePtr _capture_origin)
{
- // FactorOdom3DPtr fac_odom = std::make_shared<FactorOdom3D>(_feature_motion, _capture_origin->getFrame(),
- // shared_from_this());
auto fac_odom = FactorBase::emplace<FactorOdom3D>(_feature_motion, _feature_motion, _capture_origin->getFrame(),
shared_from_this());
- // _feature_motion->addFactor(fac_odom);
- // _capture_origin->getFrame()->addConstrainedBy(fac_odom);
return fac_odom;
}
-FeatureBasePtr ProcessorOdom3D::createFeature(CaptureMotionPtr _capture_motion)
-{
- FeatureBasePtr key_feature_ptr = std::make_shared<FeatureBase>(
- "ODOM 3D", _capture_motion->getBuffer().get().back().delta_integr_,
- _capture_motion->getBuffer().get().back().delta_integr_cov_);
- return key_feature_ptr;
-}
-
void ProcessorOdom3D::remap(const Eigen::VectorXs& _x1, const Eigen::VectorXs& _x2, Eigen::VectorXs& _x_out)
{
new (&p1_) Eigen::Map<const Eigen::Vector3s>(_x1.data());