diff --git a/include/core/capture/capture_base.h b/include/core/capture/capture_base.h
index 7864c46b9e03eb2b3609ca588d39e4ef1a06d975..f0670bfa64e6f3cacf460e22d312c8dd9e57fa80 100644
--- a/include/core/capture/capture_base.h
+++ b/include/core/capture/capture_base.h
@@ -19,6 +19,9 @@ namespace wolf{
//class CaptureBase
class CaptureBase : public NodeBase, public std::enable_shared_from_this<CaptureBase>
{
+ friend FeatureBase;
+ friend FactorBase;
+
private:
FrameBaseWPtr frame_ptr_;
FeatureBasePtrList feature_list_;
@@ -57,14 +60,12 @@ class CaptureBase : public NodeBase, public std::enable_shared_from_this<Capture
void setTimeStampToNow();
FrameBasePtr getFrame() const;
+ private:
void setFrame(const FrameBasePtr _frm_ptr);
- void unlinkFromFrame(){frame_ptr_.reset();}
+ public:
virtual void setProblem(ProblemPtr _problem) final;
-
- FeatureBasePtr addFeature(FeatureBasePtr _ft_ptr);
FeatureBasePtrList& getFeatureList();
- void addFeatureList(FeatureBasePtrList& _new_ft_list);
void getFactorList(FactorBasePtrList& _fac_list);
@@ -72,9 +73,12 @@ class CaptureBase : public NodeBase, public std::enable_shared_from_this<Capture
virtual void setSensor(const SensorBasePtr sensor_ptr);
// constrained by
+ private:
virtual FactorBasePtr addConstrainedBy(FactorBasePtr _fac_ptr);
+ virtual void removeConstrainedBy(FactorBasePtr _fac_ptr);
+ public:
unsigned int getHits() const;
- FactorBasePtrList& getConstrainedByList();
+ const FactorBasePtrList& getConstrainedByList() const;
// State blocks
const std::vector<StateBlockPtr>& getStateBlockVec() const;
@@ -106,6 +110,10 @@ class CaptureBase : public NodeBase, public std::enable_shared_from_this<Capture
protected:
SizeEigen computeCalibSize() const;
+ private:
+ FeatureBasePtr addFeature(FeatureBasePtr _ft_ptr);
+ void removeFeature(FeatureBasePtr _ft_ptr);
+
private:
void updateCalibSize();
};
@@ -132,13 +140,6 @@ inline SizeEigen CaptureBase::getCalibSize() const
return calib_size_;
}
-inline void CaptureBase::setProblem(ProblemPtr _problem)
-{
- NodeBase::setProblem(_problem);
- for (auto ft : feature_list_)
- ft->setProblem(_problem);
-}
-
inline void CaptureBase::updateCalibSize()
{
calib_size_ = computeCalibSize();
@@ -199,7 +200,7 @@ inline unsigned int CaptureBase::getHits() const
return constrained_by_list_.size();
}
-inline FactorBasePtrList& CaptureBase::getConstrainedByList()
+inline const FactorBasePtrList& CaptureBase::getConstrainedByList() const
{
return constrained_by_list_;
}
diff --git a/include/core/factor/factor_autodiff_distance_3D.h b/include/core/factor/factor_autodiff_distance_3D.h
index d4932885e58e941ad787c52a0428aa00ea6eec5e..e26c412812fd7c7e869a79c8b95085eac43ed633 100644
--- a/include/core/factor/factor_autodiff_distance_3D.h
+++ b/include/core/factor/factor_autodiff_distance_3D.h
@@ -34,7 +34,6 @@ class FactorAutodiffDistance3D : public FactorAutodiff<FactorAutodiffDistance3D,
_feat->getFrame()->getP(),
_frm_other->getP())
{
- setFeature(_feat);
}
virtual ~FactorAutodiffDistance3D() { /* nothing */ }
diff --git a/include/core/factor/factor_base.h b/include/core/factor/factor_base.h
index 884f622a8c00a55d20d6cc2022cb90f2f795ec3f..fbc07e89af8850d9e4b4228817b5bbb6e5abac53 100644
--- a/include/core/factor/factor_base.h
+++ b/include/core/factor/factor_base.h
@@ -20,8 +20,8 @@ namespace wolf {
*/
typedef enum
{
- FAC_INACTIVE = 0, ///< Factor established with a frame (odometry).
- FAC_ACTIVE = 1 ///< Factor established with absolute reference.
+ FAC_INACTIVE = 0, ///< Inactive factor, it is not included in the SLAM problem.
+ FAC_ACTIVE = 1 ///< Normal active factor, playing its role in the solution.
} FactorStatus;
/** \brief Enumeration of jacobian computation method
@@ -45,32 +45,36 @@ class FactorBase : public NodeBase, public std::enable_shared_from_this<FactorBa
protected:
unsigned int factor_id_;
- FactorStatus status_; ///< status of factor (types defined at wolf.h)
+ FactorStatus status_; ///< status of factor
bool apply_loss_function_; ///< flag for applying loss function to this factor
- FrameBaseWPtr frame_other_ptr_; ///< FrameBase pointer (for category FAC_FRAME)
+ FrameBaseWPtr frame_other_ptr_; ///< FrameBase pointer
CaptureBaseWPtr capture_other_ptr_; ///< CaptureBase pointer
- FeatureBaseWPtr feature_other_ptr_; ///< FeatureBase pointer (for category FAC_FEATURE)
- LandmarkBaseWPtr landmark_other_ptr_; ///< LandmarkBase pointer (for category FAC_LANDMARK)
+ FeatureBaseWPtr feature_other_ptr_; ///< FeatureBase pointer
+ LandmarkBaseWPtr landmark_other_ptr_; ///< LandmarkBase pointer
ProcessorBaseWPtr processor_ptr_; ///< ProcessorBase pointer
public:
- /** \brief Constructor of category FAC_ABSOLUTE
+ /** \brief Constructor not involving nodes of other frames, only feature, capture and/or frame of this factor
**/
FactorBase(const std::string& _tp,
- bool _apply_loss_function = false,
- FactorStatus _status = FAC_ACTIVE);
+ bool _apply_loss_function = false,
+ FactorStatus _status = FAC_ACTIVE);
- /** \brief Constructor valid for all categories (FRAME, FEATURE, LANDMARK)
+ /** \brief Default constructor.
+ *
+ * IMPORTANT: "other" means "of another branch of the wolf tree".
+ * You should only provide a non-nullptr in frame/capture/feature_other_ptr in case of a frame/capture/feature involved in this factor
+ * that does not located in the same branch.
**/
FactorBase(const std::string& _tp,
- const FrameBasePtr& _frame_other_ptr,
- const CaptureBasePtr& _capture_other_ptr,
- const FeatureBasePtr& _feature_other_ptr,
- const LandmarkBasePtr& _landmark_other_ptr,
- const ProcessorBasePtr& _processor_ptr = nullptr,
- bool _apply_loss_function = false,
- FactorStatus _status = FAC_ACTIVE);
+ const FrameBasePtr& _frame_other_ptr,
+ const CaptureBasePtr& _capture_other_ptr,
+ const FeatureBasePtr& _feature_other_ptr,
+ const LandmarkBasePtr& _landmark_other_ptr,
+ const ProcessorBasePtr& _processor_ptr = nullptr,
+ bool _apply_loss_function = false,
+ FactorStatus _status = FAC_ACTIVE);
virtual ~FactorBase() = default;
@@ -113,7 +117,6 @@ class FactorBase : public NodeBase, public std::enable_shared_from_this<FactorBa
/** \brief Returns a pointer to the feature constrained from
**/
FeatureBasePtr getFeature() const;
- void setFeature(const FeatureBasePtr _ft_ptr){feature_ptr_ = _ft_ptr;}
/** \brief Returns a pointer to its capture
**/
@@ -135,51 +138,43 @@ class FactorBase : public NodeBase, public std::enable_shared_from_this<FactorBa
*/
bool getApplyLossFunction();
- /** \brief Gets the apply loss function flag
- */
- void setApplyLossFunction(const bool _apply);
-
/** \brief Returns a pointer to the frame constrained to
**/
FrameBasePtr getFrameOther() const { return frame_other_ptr_.lock(); }
- void setFrameOther(FrameBasePtr _frm_o) { frame_other_ptr_ = _frm_o; }
- /** \brief Returns a pointer to the frame constrained to
+ /** \brief Returns a pointer to the capture constrained to
**/
CaptureBasePtr getCaptureOther() const { return capture_other_ptr_.lock(); }
- void setCaptureOther(CaptureBasePtr _cap_o) { capture_other_ptr_ = _cap_o; }
/** \brief Returns a pointer to the feature constrained to
**/
FeatureBasePtr getFeatureOther() const { return feature_other_ptr_.lock(); }
- void setFeatureOther(FeatureBasePtr _ftr_o) { feature_other_ptr_ = _ftr_o; }
/** \brief Returns a pointer to the landmark constrained to
**/
LandmarkBasePtr getLandmarkOther() const { return landmark_other_ptr_.lock(); }
- void setLandmarkOther(LandmarkBasePtr _lmk_o){ landmark_other_ptr_ = _lmk_o; }
+ public:
/**
* @brief getProcessor
* @return
*/
ProcessorBasePtr getProcessor() const;
+ void link(FeatureBasePtr ftr);
+ virtual void setProblem(ProblemPtr) final;
+ template<typename classType, typename... T>
+ static std::shared_ptr<FactorBase> emplace(FeatureBasePtr _oth_ptr, T&&... all);
+
+ private:
+
+ void setFeature(const FeatureBasePtr _ft_ptr){feature_ptr_ = _ft_ptr;}
+
/**
* @brief setProcessor
* @param _processor_ptr
*/
void setProcessor(const ProcessorBasePtr& _processor_ptr);
-
- void link(FeatureBasePtr);
- template<typename classType, typename... T>
- static std::shared_ptr<FactorBase> emplace(FeatureBasePtr _oth_ptr, T&&... all);
-
-// protected:
-// template<typename D>
-// void print(const std::string& name, const Eigen::MatrixBase<D>& mat) const; // Do nothing if input Scalar type is ceres::Jet
-// template<int R, int C>
-// void print(const std::string& name, const Eigen::Matrix<Scalar, R, C>& mat) const; // Normal print if Scalar type is wolf::Scalar
};
}
@@ -194,25 +189,6 @@ class FactorBase : public NodeBase, public std::enable_shared_from_this<FactorBa
namespace wolf{
-//template<typename D>
-//inline void FactorBase::print(const std::string& name, const Eigen::MatrixBase<D>& mat) const {} // Do nothing if input Scalar type is ceres::Jet
-//template<int R, int C>
-//inline void FactorBase::print(const std::string& name, const Eigen::Matrix<Scalar, R, C>& mat) const // Normal print if Scalar type is wolf::Scalar
-//{
-// if (mat.cols() == 1)
-// {
-// WOLF_TRACE(name, ": ", mat.transpose());
-// }
-// else if (mat.rows() == 1)
-// {
-// WOLF_TRACE(name, ": ", mat);
-// }
-// else
-// {
-// WOLF_TRACE(name, ":\n", mat);
-// }
-//}
-
template<typename classType, typename... T>
std::shared_ptr<FactorBase> FactorBase::emplace(FeatureBasePtr _ftr_ptr, T&&... all)
{
diff --git a/include/core/feature/feature_base.h b/include/core/feature/feature_base.h
index 72a2be8a46ba1f4e95b448570ac649d478a86054..2d3506fc96cea72a4db1ed6102edf60169e54682 100644
--- a/include/core/feature/feature_base.h
+++ b/include/core/feature/feature_base.h
@@ -18,6 +18,8 @@ namespace wolf {
//class FeatureBase
class FeatureBase : public NodeBase, public std::enable_shared_from_this<FeatureBase>
{
+ friend FactorBase;
+
private:
CaptureBaseWPtr capture_ptr_;
FactorBasePtrList factor_list_;
@@ -83,24 +85,30 @@ class FeatureBase : public NodeBase, public std::enable_shared_from_this<Feature
FrameBasePtr getFrame() const;
CaptureBasePtr getCapture() const;
- void setCapture(CaptureBasePtr _cap_ptr){capture_ptr_ = _cap_ptr;}
- FactorBasePtr addFactor(FactorBasePtr _co_ptr);
FactorBasePtrList& getFactorList();
- virtual FactorBasePtr addConstrainedBy(FactorBasePtr _fac_ptr);
unsigned int getHits() const;
- FactorBasePtrList& getConstrainedByList();
+ const FactorBasePtrList& getConstrainedByList() const;
// all factors
void getFactorList(FactorBasePtrList & _fac_list);
- void link(CaptureBasePtr);
+ void link(CaptureBasePtr cap_ptr);
template<typename classType, typename... T>
static std::shared_ptr<FeatureBase> emplace(CaptureBasePtr _cpt_ptr, T&&... all);
- private:
+
+ protected:
+
Eigen::MatrixXs computeSqrtUpper(const Eigen::MatrixXs& _M) const;
+
+ private:
+ void setCapture(CaptureBasePtr _cap_ptr){capture_ptr_ = _cap_ptr;}
+ FactorBasePtr addFactor(FactorBasePtr _co_ptr);
+ void removeFactor(FactorBasePtr _co_ptr);
+ virtual FactorBasePtr addConstrainedBy(FactorBasePtr _fac_ptr);
+ virtual void removeConstrainedBy(FactorBasePtr _fac_ptr);
};
}
@@ -124,7 +132,7 @@ inline unsigned int FeatureBase::getHits() const
return constrained_by_list_.size();
}
-inline FactorBasePtrList& FeatureBase::getConstrainedByList()
+inline const FactorBasePtrList& FeatureBase::getConstrainedByList() const
{
return constrained_by_list_;
}
diff --git a/include/core/frame/frame_base.h b/include/core/frame/frame_base.h
index 0fdb349c15db78a40fbe2b658c739c776a9b2c3d..23f3d0975565dfd2d9f123d4b78c291925bba810 100644
--- a/include/core/frame/frame_base.h
+++ b/include/core/frame/frame_base.h
@@ -30,6 +30,9 @@ typedef enum
//class FrameBase
class FrameBase : public NodeBase, public std::enable_shared_from_this<FrameBase>
{
+ friend CaptureBase;
+ friend FactorBase;
+
private:
TrajectoryBaseWPtr trajectory_ptr_;
CaptureBasePtrList capture_list_;
@@ -106,6 +109,7 @@ class FrameBase : public NodeBase, public std::enable_shared_from_this<FrameBase
void setP(const StateBlockPtr _p_ptr);
void setO(const StateBlockPtr _o_ptr);
void setV(const StateBlockPtr _v_ptr);
+ protected:
void registerNewStateBlocks();
void removeStateBlocks();
@@ -128,39 +132,33 @@ class FrameBase : public NodeBase, public std::enable_shared_from_this<FrameBase
virtual void setProblem(ProblemPtr _problem) final;
TrajectoryBasePtr getTrajectory() const;
- void setTrajectory(TrajectoryBasePtr _trj_ptr);
FrameBasePtr getPreviousFrame() const;
FrameBasePtr getNextFrame() const;
CaptureBasePtrList& getCaptureList();
- CaptureBasePtr addCapture(CaptureBasePtr _capt_ptr);
CaptureBasePtr getCaptureOf(const SensorBasePtr _sensor_ptr);
CaptureBasePtr getCaptureOf(const SensorBasePtr _sensor_ptr, const std::string& type);
CaptureBasePtrList getCapturesOf(const SensorBasePtr _sensor_ptr);
- void unlinkCapture(CaptureBasePtr _cap_ptr);
FactorBasePtr getFactorOf(const ProcessorBasePtr _processor_ptr);
FactorBasePtr getFactorOf(const ProcessorBasePtr _processor_ptr, const std::string& type);
void getFactorList(FactorBasePtrList& _fac_list);
- virtual FactorBasePtr addConstrainedBy(FactorBasePtr _fac_ptr);
unsigned int getHits() const;
- FactorBasePtrList& getConstrainedByList();
+ const FactorBasePtrList& getConstrainedByList() const;
void link(TrajectoryBasePtr);
template<typename classType, typename... T>
static std::shared_ptr<FrameBase> emplace(TrajectoryBasePtr _ptr, T&&... all);
- public:
- static FrameBasePtr create_PO_2D (const FrameType & _tp,
- const TimeStamp& _ts,
- const Eigen::VectorXs& _x = Eigen::VectorXs::Zero(3));
- static FrameBasePtr create_PO_3D (const FrameType & _tp,
- const TimeStamp& _ts,
- const Eigen::VectorXs& _x = Eigen::VectorXs::Zero(7));
- static FrameBasePtr create_POV_3D(const FrameType & _tp,
- const TimeStamp& _ts,
- const Eigen::VectorXs& _x = Eigen::VectorXs::Zero(10));
+ private:
+
+ CaptureBasePtr addCapture(CaptureBasePtr _capt_ptr);
+ void removeCapture(CaptureBasePtr _capt_ptr);
+ void setTrajectory(TrajectoryBasePtr _trj_ptr);
+ virtual FactorBasePtr addConstrainedBy(FactorBasePtr _fac_ptr);
+ virtual void removeConstrainedBy(FactorBasePtr _fac_ptr);
+
};
} // namespace wolf
@@ -283,14 +281,7 @@ inline unsigned int FrameBase::getHits() const
return constrained_by_list_.size();
}
-inline void FrameBase::setProblem(ProblemPtr _problem)
-{
- NodeBase::setProblem(_problem);
- for (auto cap : capture_list_)
- cap->setProblem(_problem);
-}
-
-inline FactorBasePtrList& FrameBase::getConstrainedByList()
+inline const FactorBasePtrList& FrameBase::getConstrainedByList() const
{
return constrained_by_list_;
}
diff --git a/include/core/hardware/hardware_base.h b/include/core/hardware/hardware_base.h
index 1b4149a6d32671492d1eee3c05e13dacb79e92fa..99a4179542d3857ed670b4dd95ac8607c1e588b5 100644
--- a/include/core/hardware/hardware_base.h
+++ b/include/core/hardware/hardware_base.h
@@ -16,6 +16,8 @@ namespace wolf {
//class HardwareBase
class HardwareBase : public NodeBase, public std::enable_shared_from_this<HardwareBase>
{
+ friend SensorBase;
+
private:
SensorBasePtrList sensor_list_;
@@ -23,8 +25,10 @@ class HardwareBase : public NodeBase, public std::enable_shared_from_this<Hardwa
HardwareBase();
virtual ~HardwareBase();
- virtual SensorBasePtr addSensor(SensorBasePtr _sensor_ptr);
SensorBasePtrList& getSensorList();
+
+ private:
+ virtual SensorBasePtr addSensor(SensorBasePtr _sensor_ptr);
};
} // namespace wolf
diff --git a/include/core/landmark/landmark_base.h b/include/core/landmark/landmark_base.h
index 373b20149b9202ca61fcfb1c1ea4d265e284f63f..57d294acfe4404cb16fc3abdcc15e5225d2053ac 100644
--- a/include/core/landmark/landmark_base.h
+++ b/include/core/landmark/landmark_base.h
@@ -22,6 +22,8 @@ namespace wolf {
//class LandmarkBase
class LandmarkBase : public NodeBase, public std::enable_shared_from_this<LandmarkBase>
{
+ friend FactorBase;
+
private:
MapBaseWPtr map_ptr_;
FactorBasePtrList constrained_by_list_;
@@ -68,12 +70,12 @@ class LandmarkBase : public NodeBase, public std::enable_shared_from_this<Landma
StateBlockPtr getO() const;
void setP(const StateBlockPtr _p_ptr);
void setO(const StateBlockPtr _o_ptr);
- virtual void registerNewStateBlocks();
Eigen::VectorXs getState() const;
void getState(Eigen::VectorXs& _state) const;
bool getCovariance(Eigen::MatrixXs& _cov) const;
protected:
+ virtual void registerNewStateBlocks();
virtual void removeStateBlocks();
// Descriptor
@@ -85,11 +87,9 @@ class LandmarkBase : public NodeBase, public std::enable_shared_from_this<Landma
// Navigate wolf tree
virtual void setProblem(ProblemPtr _problem) final;
- FactorBasePtr addConstrainedBy(FactorBasePtr _fac_ptr);
unsigned int getHits() const;
- FactorBasePtrList& getConstrainedByList();
+ const FactorBasePtrList& getConstrainedByList() const;
- void setMap(const MapBasePtr _map_ptr);
MapBasePtr getMap();
void link(MapBasePtr);
template<typename classType, typename... T>
@@ -100,6 +100,12 @@ class LandmarkBase : public NodeBase, public std::enable_shared_from_this<Landma
* These need to be set afterwards.
*/
static LandmarkBasePtr create(const YAML::Node& _node);
+
+ private:
+
+ void setMap(const MapBasePtr _map_ptr);
+ virtual FactorBasePtr addConstrainedBy(FactorBasePtr _fac_ptr);
+ virtual void removeConstrainedBy(FactorBasePtr _fac_ptr);
};
}
@@ -117,10 +123,6 @@ std::shared_ptr<LandmarkBase> LandmarkBase::emplace(MapBasePtr _map_ptr, T&&...
lmk->link(_map_ptr);
return lmk;
}
-inline void LandmarkBase::setProblem(ProblemPtr _problem)
-{
- NodeBase::setProblem(_problem);
-}
inline MapBasePtr LandmarkBase::getMap()
{
@@ -149,7 +151,7 @@ inline unsigned int LandmarkBase::getHits() const
return constrained_by_list_.size();
}
-inline FactorBasePtrList& LandmarkBase::getConstrainedByList()
+inline const FactorBasePtrList& LandmarkBase::getConstrainedByList() const
{
return constrained_by_list_;
}
diff --git a/include/core/map/map_base.h b/include/core/map/map_base.h
index 90ad785458a9a3a62f6882c7294945c19ceedcc8..d4d5f4fb7de8356cf53fe05283c53093842ebb6c 100644
--- a/include/core/map/map_base.h
+++ b/include/core/map/map_base.h
@@ -19,6 +19,8 @@ namespace wolf {
//class MapBase
class MapBase : public NodeBase, public std::enable_shared_from_this<MapBase>
{
+ friend LandmarkBase;
+
private:
LandmarkBasePtrList landmark_list_;
@@ -26,8 +28,11 @@ class MapBase : public NodeBase, public std::enable_shared_from_this<MapBase>
MapBase();
~MapBase();
+ private:
virtual LandmarkBasePtr addLandmark(LandmarkBasePtr _landmark_ptr);
- virtual void addLandmarkList(LandmarkBasePtrList& _landmark_list);
+ virtual void removeLandmark(LandmarkBasePtr _landmark_ptr);
+
+ public:
LandmarkBasePtrList& getLandmarkList();
void load(const std::string& _map_file_yaml);
diff --git a/include/core/problem/problem.h b/include/core/problem/problem.h
index 935a887be9a1bbbf467335d54692204e0b6f62b0..6aca54c5d7109822c26aede36da25ecbe58e5491 100644
--- a/include/core/problem/problem.h
+++ b/include/core/problem/problem.h
@@ -38,6 +38,8 @@ enum Notification
class Problem : public std::enable_shared_from_this<Problem>
{
friend SolverManager; // Enable SolverManager to acces protected functions (consumeXXXNotificationMap())
+ friend ProcessorBase;
+ friend ProcessorMotion;
protected:
HardwareBasePtr hardware_ptr_;
@@ -72,7 +74,6 @@ class Problem : public std::enable_shared_from_this<Problem>
// Hardware branch ------------------------------------
HardwareBasePtr getHardware();
- void addSensor(SensorBasePtr _sen_ptr);
/** \brief Factory method to install (create and add) sensors only from its properties
* \param _sen_type type of sensor
@@ -95,12 +96,12 @@ class Problem : public std::enable_shared_from_this<Problem>
const std::string& _unique_sensor_name, //
const Eigen::VectorXs& _extrinsics, //
const std::string& _intrinsics_filename);
- /**
- Custom installSensor using the parameters server
- */
- SensorBasePtr installSensor(const std::string& _sen_type, //
- const std::string& _unique_sensor_name,
- const paramsServer& _server);
+ /**
+ Custom installSensor using the parameters server
+ */
+ SensorBasePtr installSensor(const std::string& _sen_type, //
+ const std::string& _unique_sensor_name,
+ const paramsServer& _server);
/** \brief get a sensor pointer by its name
* \param _sensor_name The sensor name, as it was installed with installSensor()
*/
@@ -134,20 +135,23 @@ class Problem : public std::enable_shared_from_this<Problem>
const std::string& _corresponding_sensor_name, //
const std::string& _params_filename = "");
- /**
- Custom installProcessor to be used with parameters server
- */
- ProcessorBasePtr installProcessor(const std::string& _prc_type, //
- const std::string& _unique_processor_name, //
- const std::string& _corresponding_sensor_name, //
- const paramsServer& _server);
- /** \brief Set the processor motion
+ /**
+ Custom installProcessor to be used with parameters server
+ */
+ ProcessorBasePtr installProcessor(const std::string& _prc_type, //
+ const std::string& _unique_processor_name, //
+ const std::string& _corresponding_sensor_name, //
+ const paramsServer& _server);
+ protected:
+ /** \brief Set the processor motion
*
* Set the processor motion.
*/
void setProcessorMotion(ProcessorMotionPtr _processor_motion_ptr);
ProcessorMotionPtr setProcessorMotion(const std::string& _unique_processor_name);
void clearProcessorMotion();
+
+ public:
ProcessorMotionPtr& getProcessorMotion();
// Trajectory branch ----------------------------------
@@ -268,8 +272,6 @@ class Problem : public std::enable_shared_from_this<Problem>
// Map branch -----------------------------------------
MapBasePtr getMap();
- LandmarkBasePtr addLandmark(LandmarkBasePtr _lmk_ptr);
- void addLandmarkList(LandmarkBasePtrList& _lmk_list);
void loadMap(const std::string& _filename_dot_yaml);
void saveMap(const std::string& _filename_dot_yaml, //
const std::string& _map_name = "Map automatically saved by Wolf");
@@ -288,13 +290,9 @@ class Problem : public std::enable_shared_from_this<Problem>
// Solver management ----------------------------------------
- /** \brief Notifies a new state block to be added to the solver manager
- */
- StateBlockPtr addStateBlock(StateBlockPtr _state_ptr);
-
- /** \brief Notifies a state block to be removed from the solver manager
+ /** \brief Notifies a new/removed state block to update the solver manager
*/
- void removeStateBlock(StateBlockPtr _state_ptr);
+ StateBlockPtr notifyStateBlock(StateBlockPtr _state_ptr, Notification _notif);
/** \brief Returns the size of the map of state block notification
*/
@@ -304,13 +302,9 @@ class Problem : public std::enable_shared_from_this<Problem>
*/
bool getStateBlockNotification(const StateBlockPtr& sb_ptr, Notification& notif) const;
- /** \brief Notifies a new factor to be added to the solver manager
- */
- FactorBasePtr addFactor(FactorBasePtr _factor_ptr);
-
- /** \brief Notifies a factor to be removed from the solver manager
+ /** \brief Notifies a new/removed factor to update the solver manager
*/
- void removeFactor(FactorBasePtr _factor_ptr);
+ FactorBasePtr notifyFactor(FactorBasePtr _factor_ptr, Notification _notif);
/** \brief Returns the size of the map of factor notification
*/
diff --git a/include/core/processor/processor_base.h b/include/core/processor/processor_base.h
index 4ac6c161d079579783d13ae7a232c0f915a1170f..083c2c6f142dfe8df89fb18b77ae79180e2b92d4 100644
--- a/include/core/processor/processor_base.h
+++ b/include/core/processor/processor_base.h
@@ -266,8 +266,10 @@ class ProcessorBase : public NodeBase, public std::enable_shared_from_this<Proce
SensorBasePtr getSensor();
const SensorBasePtr getSensor() const;
+ private:
void setSensor(SensorBasePtr _sen_ptr){sensor_ptr_ = _sen_ptr;}
+ public:
virtual bool isMotion();
void setTimeTolerance(Scalar _time_tolerance);
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 d6cacdccdd701c40fb38b0db1349ab39f4580dd0..fe77c60535d7124c1c780bf6738a78743e293a1c 100644
--- a/include/core/processor/processor_motion.h
+++ b/include/core/processor/processor_motion.h
@@ -218,6 +218,8 @@ class ProcessorMotion : public ProcessorBase
*/
FrameBasePtr setOrigin(const Eigen::VectorXs& _x_origin, const TimeStamp& _ts_origin);
+ virtual void setProblem(ProblemPtr);
+
MotionBuffer& getBuffer();
const MotionBuffer& getBuffer() const;
@@ -409,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
@@ -417,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
- * \param _capture_motion: the parent capture
- */
- FeatureBasePtr emplaceFeature(CaptureMotionPtr _capture_own);
-
- /** \brief create a feature corresponding to given 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
*/
- virtual FeatureBasePtr createFeature(CaptureMotionPtr _capture_own) = 0;
+ virtual FeatureBasePtr emplaceFeature(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/include/core/processor/processor_tracker.h b/include/core/processor/processor_tracker.h
index 9f85d65f658b1790f886ae01300c7aeb550d5c5d..17de27c05b0e210b12f46a3d43ad89d6ab8510ef 100644
--- a/include/core/processor/processor_tracker.h
+++ b/include/core/processor/processor_tracker.h
@@ -103,8 +103,6 @@ class ProcessorTracker : public ProcessorBase
FeatureBasePtrList new_features_last_; ///< List of new features in \b last for landmark initialization and new key-frame creation.
FeatureBasePtrList new_features_incoming_; ///< list of the new features of \b last successfully tracked in \b incoming
- SizeStd number_of_tracks_;
-
public:
ProcessorTracker(const std::string& _type,
ProcessorParamsTrackerPtr _params_tracker);
@@ -210,18 +208,9 @@ class ProcessorTracker : public ProcessorBase
FeatureBasePtrList& getNewFeaturesListLast();
- const SizeStd& previousNumberOfTracks() const
- {
- return number_of_tracks_;
- }
-
- SizeStd& previousNumberOfTracks()
- {
- return number_of_tracks_;
+ std::string print(){
+ return this->params_tracker_->print();
}
- std::string print(){
- return this->params_tracker_->print();
- }
protected:
diff --git a/include/core/processor/processor_tracker_feature.h b/include/core/processor/processor_tracker_feature.h
index 055e2ddf87220a65289c09e20746dd4da990541d..a808a421da22a74ad417b6cdc38024f308ff59bf 100644
--- a/include/core/processor/processor_tracker_feature.h
+++ b/include/core/processor/processor_tracker_feature.h
@@ -113,19 +113,32 @@ class ProcessorTrackerFeature : public ProcessorTracker
*/
virtual unsigned int processKnown();
- /** \brief Track provided features from \b last to \b incoming
- * \param _features_last_in input list of features in \b last to track
- * \param _features_incoming_out returned list of features found in \b incoming
+ /** \brief Track provided features in \b _capture
+ * \param _features_in input list of features in \b last to track
+ * \param _capture the capture in which the _features_in should be searched
+ * \param _features_out returned list of features found in \b _capture
* \param _feature_correspondences returned map of correspondences: _feature_correspondences[feature_out_ptr] = feature_in_ptr
+ *
+ * IMPORTANT: The features in _features_out should be emplaced. Don't use `make_shared`, use `FeatureBase::emplace` instead.
+ * Then, they will be already linked to the _capture.
+ * If you detect all the features at once in preprocess(), you should create them (`make_shared()`) and only link the
+ * features that are returned in _features_out (`FeatureBase::link(_capture)`).
+ *
+ * \return the number of features tracked
*/
- virtual unsigned int trackFeatures(const FeatureBasePtrList& _features_last_in, FeatureBasePtrList& _features_incoming_out, FeatureMatchMap& _feature_correspondences) = 0;
+ virtual unsigned int trackFeatures(const FeatureBasePtrList& _features_in,
+ const CaptureBasePtr& _capture,
+ FeatureBasePtrList& _features_out,
+ FeatureMatchMap& _feature_correspondences) = 0;
/** \brief Correct the drift in incoming feature by re-comparing against the corresponding feature in origin.
* \param _origin_feature input feature in origin capture tracked
* \param _incoming_feature input/output feature in incoming capture to be corrected
* \return false if the the process discards the correspondence with origin's feature
*/
- virtual bool correctFeatureDrift(const FeatureBasePtr _origin_feature, const FeatureBasePtr _last_feature, FeatureBasePtr _incoming_feature) = 0;
+ virtual bool correctFeatureDrift(const FeatureBasePtr _origin_feature,
+ const FeatureBasePtr _last_feature,
+ FeatureBasePtr _incoming_feature) = 0;
/** \brief Vote for KeyFrame generation
*
@@ -147,27 +160,35 @@ class ProcessorTrackerFeature : public ProcessorTracker
/** \brief Detect new Features
* \param _max_features maximum number of features detected (-1: unlimited. 0: none)
- * \param _features_last_out The list of detected Features.
+ * \param _capture The capture in which the new features should be detected.
+ * \param _features_out The list of detected Features in _capture.
* \return The number of detected Features.
*
* This function detects Features that do not correspond to known Features/Landmarks in the system.
*
+ * IMPORTANT: The features in _features_out should be emplaced. Don't use `make_shared`, use `FeatureBase::emplace` instead.
+ * Then, they will be already linked to the _capture.
+ * If you detect all the features at once in preprocess(), you should create them (`make_shared()`) and only link the
+ * features that are returned in _features_out (`FeatureBase::link(_capture)`).
+ *
* The function is called in ProcessorTrackerFeature::processNew() to set the member new_features_last_,
* the list of newly detected features of the capture last_ptr_.
*/
- virtual unsigned int detectNewFeatures(const int& _max_new_features, FeatureBasePtrList& _features_last_out) = 0;
+ virtual unsigned int detectNewFeatures(const int& _max_new_features,
+ const CaptureBasePtr& _capture,
+ FeatureBasePtrList& _features_out) = 0;
- /** \brief Create a new factor and link it to the wolf tree
+ /** \brief Emplaces a new factor
* \param _feature_ptr pointer to the parent Feature
* \param _feature_other_ptr pointer to the other feature constrained.
*
* Implement this method in derived classes.
*
- * This function creates a factor of the appropriate type for the derived processor.
+ * This function emplaces a factor of the appropriate type for the derived processor.
*/
- virtual FactorBasePtr createFactor(FeatureBasePtr _feature_ptr, FeatureBasePtr _feature_other_ptr) = 0;
+ virtual FactorBasePtr emplaceFactor(FeatureBasePtr _feature_ptr, FeatureBasePtr _feature_other_ptr) = 0;
- /** \brief Establish factors between features in Captures \b last and \b origin
+ /** \brief Emplaces a new factor for each correspondence between a feature in Capture \b last and a feature in Capture \b origin
*/
virtual void establishFactors();
};
diff --git a/include/core/processor/processor_tracker_feature_dummy.h b/include/core/processor/processor_tracker_feature_dummy.h
index 150a6ffb0068b723c9f237918b81dc56116323e2..007207cecb59a4d5028bfe5f1b0e205a0b0726b8 100644
--- a/include/core/processor/processor_tracker_feature_dummy.h
+++ b/include/core/processor/processor_tracker_feature_dummy.h
@@ -45,12 +45,20 @@ class ProcessorTrackerFeatureDummy : public ProcessorTrackerFeature
ProcessorParamsTrackerFeatureDummyPtr params_tracker_feature_dummy_;
- /** \brief Track provided features from \b last to \b incoming
- * \param _features_last_in input list of features in \b last to track
- * \param _features_incoming_out returned list of features found in \b incoming
+ /** \brief Track provided features in \b _capture
+ * \param _features_in input list of features in \b last to track
+ * \param _capture the capture in which the _features_in should be searched
+ * \param _features_out returned list of features found in \b _capture
* \param _feature_correspondences returned map of correspondences: _feature_correspondences[feature_out_ptr] = feature_in_ptr
+ *
+ * IMPORTANT: The features in _features_out should be emplaced. Don't use `make_shared`, use `FeatureBase::emplace` instead.
+ * Then, they will be already linked to the _capture.
+ *
+ * \return the number of features tracked
*/
- virtual unsigned int trackFeatures(const FeatureBasePtrList& _features_last_in, FeatureBasePtrList& _features_incoming_out,
+ virtual unsigned int trackFeatures(const FeatureBasePtrList& _features_in,
+ const CaptureBasePtr& _capture,
+ FeatureBasePtrList& _features_out,
FeatureMatchMap& _feature_correspondences);
/** \brief Correct the drift in incoming feature by re-comparing against the corresponding feature in origin.
@@ -71,17 +79,28 @@ class ProcessorTrackerFeatureDummy : public ProcessorTrackerFeature
/** \brief Detect new Features
* \param _max_features maximum number of features detected (-1: unlimited. 0: none)
- * \param _features_last_out The list of detected Features.
+ * \param _capture The capture in which the new features should be detected.
+ * \param _features_out The list of detected Features in _capture.
* \return The number of detected Features.
*
* This function detects Features that do not correspond to known Features/Landmarks in the system.
*
+ * IMPORTANT: The features in _features_out should be emplaced. Don't use `make_shared`, use `FeatureBase::emplace` instead.
+ * Then, they will be already linked to the _capture.
+ *
* The function is called in ProcessorTrackerFeature::processNew() to set the member new_features_last_,
* the list of newly detected features of the capture last_ptr_.
*/
- virtual unsigned int detectNewFeatures(const int& _max_features, FeatureBasePtrList& _features_incoming_out);
-
- virtual FactorBasePtr createFactor(FeatureBasePtr _feature_ptr, FeatureBasePtr _feature_other_ptr);
+ virtual unsigned int detectNewFeatures(const int& _max_new_features,
+ const CaptureBasePtr& _capture,
+ FeatureBasePtrList& _features_out);
+ /** \brief Emplaces a new factor
+ * \param _feature_ptr pointer to the parent Feature
+ * \param _feature_other_ptr pointer to the other feature constrained.
+ *
+ * This function emplaces a factor of the appropriate type for the derived processor.
+ */
+ virtual FactorBasePtr emplaceFactor(FeatureBasePtr _feature_ptr, FeatureBasePtr _feature_other_ptr);
public:
diff --git a/include/core/processor/processor_tracker_landmark.h b/include/core/processor/processor_tracker_landmark.h
index 0e7e2e0a2b7bac5161caa2d7969766a2b57f2581..c11adecc7b16a9c0c2d367200af6ee9a55d46c64 100644
--- a/include/core/processor/processor_tracker_landmark.h
+++ b/include/core/processor/processor_tracker_landmark.h
@@ -104,15 +104,23 @@ class ProcessorTrackerLandmark : public ProcessorTracker
*/
virtual unsigned int processKnown();
- /** \brief Find provided landmarks in the incoming capture
- * \param _landmarks_in input list of landmarks to be found in incoming
- * \param _features_incoming_out returned list of incoming features corresponding to a landmark of _landmarks_in
+ /** \brief Find provided landmarks as features in the provided capture
+ * \param _landmarks_in input list of landmarks to be found
+ * \param _capture the capture in which the _landmarks_in should be searched
+ * \param _features_out returned list of features found in \b _capture corresponding to a landmark of _landmarks_in
* \param _feature_landmark_correspondences returned map of landmark correspondences: _feature_landmark_correspondences[_feature_out_ptr] = landmark_in_ptr
+ *
+ * IMPORTANT: The features in _features_out should be emplaced. Don't use `make_shared`, use `FeatureBase::emplace` instead.
+ * Then, they will be already linked to the _capture.
+ * If you detect all the features at once in preprocess(), you should create them (`make_shared()`) and only link the
+ * features that are returned in _features_out (`FeatureBase::link(_capture)`).
+ *
* \return the number of landmarks found
*/
- virtual unsigned int findLandmarks(const LandmarkBasePtrList& _landmarks_in,
- FeatureBasePtrList& _features_incoming_out,
- LandmarkMatchMap& _feature_landmark_correspondences) = 0;
+ virtual unsigned int findLandmarks(const LandmarkBasePtrList& _landmarks_in,
+ const CaptureBasePtr& _capture,
+ FeatureBasePtrList& _features_out,
+ LandmarkMatchMap& _feature_landmark_correspondences) = 0;
/** \brief Vote for KeyFrame generation
*
@@ -134,35 +142,43 @@ class ProcessorTrackerLandmark : public ProcessorTracker
/** \brief Detect new Features
* \param _max_features maximum number of features detected (-1: unlimited. 0: none)
- * \param _features_last_out The list of detected Features.
+ * \param _capture The capture in which the new features should be detected.
+ * \param _features_out The list of detected Features in _capture.
* \return The number of detected Features.
*
- * This function detects Features that do not correspond to known Landmarks in the system.
+ * This function detects Features that do not correspond to known Features/Landmarks in the system.
+ *
+ * IMPORTANT: The features in _features_out should be emplaced. Don't use `make_shared`, use `FeatureBase::emplace` instead.
+ * Then, they will be already linked to the _capture.
+ * If you detect all the features at once in preprocess(), you should create them (`make_shared()`) and only link the
+ * features that are returned in _features_out (`FeatureBase::link(_capture)`).
*
* The function is called in ProcessorTrackerLandmark::processNew() to set the member new_features_last_,
* the list of newly detected features of the capture last_ptr_.
*/
- virtual unsigned int detectNewFeatures(const int& _max_features, FeatureBasePtrList& _features_incoming_out) = 0;
+ virtual unsigned int detectNewFeatures(const int& _max_new_features,
+ const CaptureBasePtr& _capture,
+ FeatureBasePtrList& _features_out) = 0;
- /** \brief Creates a landmark for each of new_features_last_
+ /** \brief Emplaces a landmark for each new feature of new_features_last_
**/
- virtual void createNewLandmarks();
+ virtual void emplaceNewLandmarks();
- /** \brief Create one landmark
+ /** \brief Emplaces one landmark
*
* Implement in derived classes to build the type of landmark you need for this tracker.
*/
- virtual LandmarkBasePtr createLandmark(FeatureBasePtr _feature_ptr) = 0;
+ virtual LandmarkBasePtr emplaceLandmark(FeatureBasePtr _feature_ptr) = 0;
- /** \brief Create a new factor
+ /** \brief Emplaces a new factor
* \param _feature_ptr pointer to the Feature to constrain
* \param _landmark_ptr LandmarkBase pointer to the Landmark constrained.
*
* Implement this method in derived classes.
*/
- virtual FactorBasePtr createFactor(FeatureBasePtr _feature_ptr, LandmarkBasePtr _landmark_ptr) = 0;
+ virtual FactorBasePtr emplaceFactor(FeatureBasePtr _feature_ptr, LandmarkBasePtr _landmark_ptr) = 0;
- /** \brief Establish factors between features in Capture \b last and landmarks
+ /** \brief Emplaces a new factor for each correspondence between a feature in Capture \b last and a landmark
*/
virtual void establishFactors();
};
diff --git a/include/core/processor/processor_tracker_landmark_dummy.h b/include/core/processor/processor_tracker_landmark_dummy.h
index 804ecc9869789b9326d0b572c210884c8f9d15b3..7dce7d14e23e77df062ff2774630767d1d2d6b48 100644
--- a/include/core/processor/processor_tracker_landmark_dummy.h
+++ b/include/core/processor/processor_tracker_landmark_dummy.h
@@ -40,15 +40,20 @@ class ProcessorTrackerLandmarkDummy : public ProcessorTrackerLandmark
ProcessorParamsTrackerLandmarkDummyPtr params_tracker_landmark_dummy_;
- //virtual void preProcess() { }
- //virtual void postProcess();
-
- /** \brief Find provided landmarks in the incoming capture
- * \param _landmarks_in input list of landmarks to be found in incoming
- * \param _features_incoming_out returned list of incoming features corresponding to a landmark of _landmarks_in
+ /** \brief Find provided landmarks as features in the provided capture
+ * \param _landmarks_in input list of landmarks to be found
+ * \param _capture the capture in which the _landmarks_in should be searched
+ * \param _features_out returned list of features found in \b _capture corresponding to a landmark of _landmarks_in
* \param _feature_landmark_correspondences returned map of landmark correspondences: _feature_landmark_correspondences[_feature_out_ptr] = landmark_in_ptr
+ *
+ * IMPORTANT: The features in _features_out should be emplaced. Don't use `make_shared`, use `FeatureBase::emplace` instead.
+ * Then, they will be already linked to the _capture.
+ *
+ * \return the number of landmarks found
*/
- virtual unsigned int findLandmarks(const LandmarkBasePtrList& _landmarks_in, FeatureBasePtrList& _features_incoming_out,
+ virtual unsigned int findLandmarks(const LandmarkBasePtrList& _landmarks_in,
+ const CaptureBasePtr& _capture,
+ FeatureBasePtrList& _features_out,
LandmarkMatchMap& _feature_landmark_correspondences);
/** \brief Vote for KeyFrame generation
@@ -62,29 +67,35 @@ class ProcessorTrackerLandmarkDummy : public ProcessorTrackerLandmark
/** \brief Detect new Features
* \param _max_features maximum number of features detected (-1: unlimited. 0: none)
- * \param _features_last_out The list of detected Features.
+ * \param _capture The capture in which the new features should be detected.
+ * \param _features_out The list of detected Features in _capture.
* \return The number of detected Features.
*
* This function detects Features that do not correspond to known Features/Landmarks in the system.
*
+ * IMPORTANT: The features in _features_out should be emplaced. Don't use `make_shared`, use `FeatureBase::emplace` instead.
+ * Then, they will be already linked to the _capture.
+ *
* The function is called in ProcessorTrackerLandmark::processNew() to set the member new_features_last_,
* the list of newly detected features of the capture last_ptr_.
*/
- virtual unsigned int detectNewFeatures(const int& _max_features, FeatureBasePtrList& _features_incoming_out);
+ virtual unsigned int detectNewFeatures(const int& _max_new_features,
+ const CaptureBasePtr& _capture,
+ FeatureBasePtrList& _features_out);
- /** \brief Create one landmark
+ /** \brief Emplace one landmark
*
* Implement in derived classes to build the type of landmark you need for this tracker.
*/
- virtual LandmarkBasePtr createLandmark(FeatureBasePtr _feature_ptr);
+ virtual LandmarkBasePtr emplaceLandmark(FeatureBasePtr _feature_ptr);
- /** \brief Create a new factor
+ /** \brief Emplace a new factor
* \param _feature_ptr pointer to the Feature to constrain
* \param _landmark_ptr LandmarkBase pointer to the Landmark constrained.
*
* Implement this method in derived classes.
*/
- virtual FactorBasePtr createFactor(FeatureBasePtr _feature_ptr, LandmarkBasePtr _landmark_ptr);
+ virtual FactorBasePtr emplaceFactor(FeatureBasePtr _feature_ptr, LandmarkBasePtr _landmark_ptr);
};
} // namespace wolf
diff --git a/include/core/processor/track_matrix.h b/include/core/processor/track_matrix.h
index edc6f9092f96fe7835d80658b975eb127552a00c..2615ad411adbe9c32efea2bf543cb24657e5ed97 100644
--- a/include/core/processor/track_matrix.h
+++ b/include/core/processor/track_matrix.h
@@ -78,25 +78,25 @@ class TrackMatrix
TrackMatrix();
virtual ~TrackMatrix();
- // tracks across all Captures
- void newTrack (CaptureBasePtr _cap, FeatureBasePtr _ftr);
- void add (size_t _track_id, CaptureBasePtr _cap, FeatureBasePtr _ftr);
+ void newTrack (FeatureBasePtr _ftr);
+ void add (const SizeStd& _track_id, const FeatureBasePtr& _ftr);
+ void add (const FeatureBasePtr& _ftr_existing, const FeatureBasePtr& _ftr_new);
void remove (FeatureBasePtr _ftr);
- void remove (size_t _track_id);
+ void remove (const SizeStd& _track_id);
void remove (CaptureBasePtr _cap);
SizeStd numTracks ();
- SizeStd trackSize (size_t _track_id);
- Track track (size_t _track_id);
+ SizeStd trackSize (const SizeStd& _track_id);
+ Track track (const SizeStd& _track_id);
Snapshot snapshot (CaptureBasePtr _capture);
vector<FeatureBasePtr>
- trackAsVector(size_t _track_id);
+ trackAsVector(const SizeStd& _track_id);
list<FeatureBasePtr>
snapshotAsList(CaptureBasePtr _cap);
TrackMatches matches (CaptureBasePtr _cap_1, CaptureBasePtr _cap_2);
- FeatureBasePtr firstFeature(size_t _track_id);
- FeatureBasePtr lastFeature (size_t _track_id);
- FeatureBasePtr feature (size_t _track_id, CaptureBasePtr _cap);
- CaptureBasePtr firstCapture(size_t _track_id);
+ FeatureBasePtr firstFeature(const SizeStd& _track_id);
+ FeatureBasePtr lastFeature (const SizeStd& _track_id);
+ FeatureBasePtr feature (const SizeStd& _track_id, CaptureBasePtr _cap);
+ CaptureBasePtr firstCapture(const SizeStd& _track_id);
// tracks across captures that belong to keyframe
// SizeStd numKeyframeTracks();
@@ -108,9 +108,8 @@ class TrackMatrix
static SizeStd track_id_count_;
- // Along track: maps of Feature pointers indexed by time stamp.
// tracks across all Captures
- map<size_t, Track > tracks_; // map indexed by track_Id of ( maps indexed by TimeStamp of ( features ) )
+ map<SizeStd, Track > tracks_; // map indexed by track_Id of ( maps indexed by TimeStamp of ( features ) )
// // tracks across captures that belong to keyframe
// map<size_t, Track > tracks_kf_; // map indexed by track_Id of ( maps indexed by TimeStamp of ( features ) )
diff --git a/include/core/sensor/sensor_base.h b/include/core/sensor/sensor_base.h
index 9002ae8fe51a10c4fbb0bdbc39f4600d4e063fe1..99a227a4010dffb1bdcddc18c390b94e5dafc2e9 100644
--- a/include/core/sensor/sensor_base.h
+++ b/include/core/sensor/sensor_base.h
@@ -33,6 +33,9 @@ struct IntrinsicsBase: public ParamsBase
class SensorBase : public NodeBase, public std::enable_shared_from_this<SensorBase>
{
+ friend Problem;
+ friend ProcessorBase;
+
private:
HardwareBaseWPtr hardware_ptr_;
ProcessorBasePtrList processor_list_;
@@ -99,9 +102,12 @@ class SensorBase : public NodeBase, public std::enable_shared_from_this<SensorBa
virtual void setProblem(ProblemPtr _problem) final;
HardwareBasePtr getHardware();
+ private:
void setHardware(const HardwareBasePtr _hw_ptr);
-
ProcessorBasePtr addProcessor(ProcessorBasePtr _proc_ptr);
+ void removeProcessor(ProcessorBasePtr _proc_ptr);
+
+ public:
ProcessorBasePtrList& getProcessorList();
CaptureBasePtr lastKeyCapture(void);
@@ -132,7 +138,12 @@ class SensorBase : public NodeBase, public std::enable_shared_from_this<SensorBa
void setP(const StateBlockPtr _p_ptr);
void setO(const StateBlockPtr _o_ptr);
void setIntrinsic(const StateBlockPtr _intr_ptr);
+
+ protected:
void removeStateBlocks();
+ virtual void registerNewStateBlocks();
+
+ public:
void fix();
void unfix();
@@ -170,8 +181,6 @@ class SensorBase : public NodeBase, public std::enable_shared_from_this<SensorBa
SizeEigen getCalibSize() const;
Eigen::VectorXs getCalibration() const;
- virtual void registerNewStateBlocks();
-
bool isExtrinsicDynamic() const;
bool isIntrinsicDynamic() const;
bool hasCapture() const {return has_capture_;}
@@ -185,6 +194,7 @@ class SensorBase : public NodeBase, public std::enable_shared_from_this<SensorBa
Eigen::MatrixXs getNoiseCov();
void setExtrinsicDynamic(bool _extrinsic_dynamic);
void setIntrinsicDynamic(bool _intrinsic_dynamic);
+
void link(HardwareBasePtr);
template<typename classType, typename... T>
static std::shared_ptr<SensorBase> emplace(HardwareBasePtr _hwd_ptr, T&&... all);
diff --git a/include/core/trajectory/trajectory_base.h b/include/core/trajectory/trajectory_base.h
index 0ee9c37d53052a3da7f193b8a4e6715592d5628b..c41bf09830811009d415a748fbdca7a918a72f72 100644
--- a/include/core/trajectory/trajectory_base.h
+++ b/include/core/trajectory/trajectory_base.h
@@ -20,6 +20,8 @@ namespace wolf {
//class TrajectoryBase
class TrajectoryBase : public NodeBase, public std::enable_shared_from_this<TrajectoryBase>
{
+ friend FrameBase;
+
private:
std::list<FrameBasePtr> frame_list_;
@@ -36,7 +38,6 @@ class TrajectoryBase : public NodeBase, public std::enable_shared_from_this<Traj
std::string getFrameStructure() const;
// Frames
- FrameBasePtr addFrame(FrameBasePtr _frame_ptr);
FrameBasePtrList& getFrameList();
const FrameBasePtrList& getFrameList() const;
FrameBasePtr getLastFrame() const;
@@ -47,6 +48,11 @@ class TrajectoryBase : public NodeBase, public std::enable_shared_from_this<Traj
void sortFrame(FrameBasePtr _frm_ptr);
void updateLastFrames();
+ private:
+ FrameBasePtr addFrame(FrameBasePtr _frame_ptr);
+ void removeFrame(FrameBasePtr _frame_ptr);
+
+ public:
// factors
void getFactorList(FactorBasePtrList & _fac_list);
diff --git a/src/capture/capture_base.cpp b/src/capture/capture_base.cpp
index 78904aa8baac91d47d2300454b8a77b2f1160001..5f0ce68802c19668ce77ae1317062f0609f33399 100644
--- a/src/capture/capture_base.cpp
+++ b/src/capture/capture_base.cpp
@@ -102,16 +102,9 @@ FeatureBasePtr CaptureBase::addFeature(FeatureBasePtr _ft_ptr)
return _ft_ptr;
}
-void CaptureBase::addFeatureList(FeatureBasePtrList& _new_ft_list)
+void CaptureBase::removeFeature(FeatureBasePtr _ft_ptr)
{
- for (FeatureBasePtr feature_ptr : _new_ft_list)
- {
- feature_ptr->setCapture(shared_from_this());
- if (getProblem() != nullptr)
- feature_ptr->setProblem(getProblem());
- // feature_list_.push_back(feature_ptr);
- }
- feature_list_.splice(feature_list_.end(), _new_ft_list);
+ feature_list_.remove(_ft_ptr);
}
void CaptureBase::getFactorList(FactorBasePtrList& _fac_list)
@@ -123,10 +116,14 @@ void CaptureBase::getFactorList(FactorBasePtrList& _fac_list)
FactorBasePtr CaptureBase::addConstrainedBy(FactorBasePtr _fac_ptr)
{
constrained_by_list_.push_back(_fac_ptr);
- _fac_ptr->setCaptureOther(shared_from_this());
return _fac_ptr;
}
+void CaptureBase::removeConstrainedBy(FactorBasePtr _fac_ptr)
+{
+ constrained_by_list_.remove(_fac_ptr);
+}
+
StateBlockPtr CaptureBase::getStateBlock(unsigned int _i) const
{
if (getSensor())
@@ -165,7 +162,7 @@ void CaptureBase::removeStateBlocks()
{
if (getProblem() != nullptr)
{
- getProblem()->removeStateBlock(sbp);
+ getProblem()->notifyStateBlock(sbp, REMOVE);
}
setStateBlock(i, nullptr);
}
@@ -244,7 +241,7 @@ void CaptureBase::registerNewStateBlocks()
{
for (auto sbp : getStateBlockVec())
if (sbp != nullptr)
- getProblem()->addStateBlock(sbp);
+ getProblem()->notifyStateBlock(sbp,ADD);
}
}
@@ -294,12 +291,17 @@ void CaptureBase::setCalibration(const VectorXs& _calib)
void CaptureBase::link(FrameBasePtr _frm_ptr)
{
+ assert((this->getFrame() == nullptr || !this->getFrame()->isKey()) && "linking a capture already linked to a KF");
+
+ // unlink from previous non-key frame
+ if (this->getFrame())
+ this->getFrame()->removeCapture(shared_from_this());
+
if(_frm_ptr)
{
_frm_ptr->addCapture(shared_from_this());
this->setFrame(_frm_ptr);
this->setProblem(_frm_ptr->getProblem());
- this->registerNewStateBlocks();
}
else
{
@@ -307,5 +309,17 @@ void CaptureBase::link(FrameBasePtr _frm_ptr)
}
}
+void CaptureBase::setProblem(ProblemPtr _problem)
+{
+ if (_problem == nullptr || _problem == this->getProblem())
+ return;
+
+ NodeBase::setProblem(_problem);
+ this->registerNewStateBlocks();
+
+ for (auto ft : feature_list_)
+ ft->setProblem(_problem);
+}
+
} // namespace wolf
diff --git a/src/factor/factor_base.cpp b/src/factor/factor_base.cpp
index 44e1826d0c01432018fd83366e0b54b8233a54f6..adec4caf9e8554a4b95baa11dd3dcf5846c06eae 100644
--- a/src/factor/factor_base.cpp
+++ b/src/factor/factor_base.cpp
@@ -7,9 +7,9 @@ namespace wolf {
unsigned int FactorBase::factor_id_count_ = 0;
FactorBase::FactorBase(const std::string& _tp,
- bool _apply_loss_function,
- FactorStatus _status) :
- NodeBase("CONSTRAINT", _tp),
+ bool _apply_loss_function,
+ FactorStatus _status) :
+ NodeBase("FACTOR", _tp),
feature_ptr_(), // nullptr
factor_id_(++factor_id_count_),
status_(_status),
@@ -23,13 +23,14 @@ FactorBase::FactorBase(const std::string& _tp,
}
FactorBase::FactorBase(const std::string& _tp,
- const FrameBasePtr& _frame_other_ptr,
- const CaptureBasePtr& _capture_other_ptr,
- const FeatureBasePtr& _feature_other_ptr,
- const LandmarkBasePtr& _landmark_other_ptr,
- const ProcessorBasePtr& _processor_ptr,
- bool _apply_loss_function, FactorStatus _status) :
- NodeBase("CONSTRAINT", _tp),
+ const FrameBasePtr& _frame_other_ptr,
+ const CaptureBasePtr& _capture_other_ptr,
+ const FeatureBasePtr& _feature_other_ptr,
+ const LandmarkBasePtr& _landmark_other_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status) :
+ NodeBase("FACTOR", _tp),
feature_ptr_(),
factor_id_(++factor_id_count_),
status_(_status),
@@ -58,13 +59,13 @@ void FactorBase::remove()
}
// add factor to be removed from solver
if (getProblem() != nullptr)
- getProblem()->removeFactor(shared_from_this());
+ getProblem()->notifyFactor(shared_from_this(),REMOVE);
// remove other: {Frame, Capture, Feature, Landmark}
FrameBasePtr frm_o = frame_other_ptr_.lock();
if (frm_o)
{
- frm_o->getConstrainedByList().remove(shared_from_this());
+ frm_o->removeConstrainedBy(shared_from_this());
if (frm_o->getConstrainedByList().empty() && frm_o->getCaptureList().empty())
frm_o->remove();
}
@@ -72,7 +73,7 @@ void FactorBase::remove()
CaptureBasePtr cap_o = capture_other_ptr_.lock();
if (cap_o)
{
- cap_o->getConstrainedByList().remove(shared_from_this());
+ cap_o->removeConstrainedBy(shared_from_this());
if (cap_o->getConstrainedByList().empty() && cap_o->getFeatureList().empty())
cap_o->remove();
}
@@ -80,7 +81,7 @@ void FactorBase::remove()
FeatureBasePtr ftr_o = feature_other_ptr_.lock();
if (ftr_o)
{
- ftr_o->getConstrainedByList().remove(shared_from_this());
+ ftr_o->removeConstrainedBy(shared_from_this());
if (ftr_o->getConstrainedByList().empty() && ftr_o->getFactorList().empty())
ftr_o->remove();
}
@@ -88,7 +89,7 @@ void FactorBase::remove()
LandmarkBasePtr lmk_o = landmark_other_ptr_.lock();
if (lmk_o)
{
- lmk_o->getConstrainedByList().remove(shared_from_this());
+ lmk_o->removeConstrainedBy(shared_from_this());
if (lmk_o->getConstrainedByList().empty())
lmk_o->remove();
}
@@ -99,21 +100,25 @@ void FactorBase::remove()
const Eigen::VectorXs& FactorBase::getMeasurement() const
{
+ assert(getFeature() != nullptr && "calling getMeasurement before linking with a feature");
return getFeature()->getMeasurement();
}
const Eigen::MatrixXs& FactorBase::getMeasurementCovariance() const
{
+ assert(getFeature() != nullptr && "calling getMeasurementCovariance before linking with a feature");
return getFeature()->getMeasurementCovariance();
}
const Eigen::MatrixXs& FactorBase::getMeasurementSquareRootInformationUpper() const
{
+ assert(getFeature() != nullptr && "calling getMeasurementSquareRootInformationUpper before linking with a feature");
return getFeature()->getMeasurementSquareRootInformationUpper();
}
CaptureBasePtr FactorBase::getCapture() const
{
+ assert(getFeature() != nullptr && "calling getCapture before linking with a feature");
return getFeature()->getCapture();
}
@@ -124,47 +129,36 @@ void FactorBase::setStatus(FactorStatus _status)
else if (_status != status_)
{
if (_status == FAC_ACTIVE)
- getProblem()->addFactor(shared_from_this());
+ getProblem()->notifyFactor(shared_from_this(),ADD);
else if (_status == FAC_INACTIVE)
- getProblem()->removeFactor(shared_from_this());
+ getProblem()->notifyFactor(shared_from_this(),REMOVE);
}
status_ = _status;
}
-void FactorBase::setApplyLossFunction(const bool _apply)
-{
- if (apply_loss_function_ != _apply)
- {
- if (getProblem() == nullptr)
- std::cout << "factor not linked with Problem, apply loss function change not notified" << std::endl;
- else
- {
- FactorBasePtr this_c = shared_from_this();
- getProblem()->removeFactor(this_c);
- getProblem()->addFactor(this_c);
- }
- }
-}
+//void FactorBase::setApplyLossFunction(const bool _apply)
+//{
+// apply_loss_function_ = _apply;
+//}
+
void FactorBase::link(FeatureBasePtr _ftr_ptr)
{
+ assert(this->getFeature() == nullptr && "linking an already linked factor");
+
if(_ftr_ptr)
{
_ftr_ptr->addFactor(shared_from_this());
this->setFeature(_ftr_ptr);
this->setProblem(_ftr_ptr->getProblem());
// add factor to be added in solver
- if (this->getProblem() != nullptr)
- {
- if (this->getStatus() == FAC_ACTIVE)
- this->getProblem()->addFactor(shared_from_this());
- }
- else
- WOLF_WARN("ADDING CONSTRAINT ", this->id(), " TO FEATURE ", _ftr_ptr->id(), " NOT CONNECTED WITH PROBLEM.");
+ if (this->getProblem() == nullptr)
+ {
+ WOLF_WARN("ADDING FACTOR ", this->id(), " TO FEATURE ", _ftr_ptr->id(), " NOT CONNECTED WITH PROBLEM.");
+ }
}
else
- {
WOLF_WARN("Linking with nullptr");
- }
+
auto frame_other = this->frame_other_ptr_.lock();
if(frame_other != nullptr) frame_other->addConstrainedBy(shared_from_this());
auto capture_other = this->capture_other_ptr_.lock();
@@ -174,4 +168,15 @@ void FactorBase::link(FeatureBasePtr _ftr_ptr)
auto landmark_other = this->landmark_other_ptr_.lock();
if(landmark_other != nullptr) landmark_other->addConstrainedBy(shared_from_this());
}
+
+void FactorBase::setProblem(ProblemPtr _problem)
+{
+ if (_problem == nullptr || _problem == this->getProblem())
+ return;
+
+ NodeBase::setProblem(_problem);
+ if (this->getStatus() == FAC_ACTIVE)
+ this->getProblem()->notifyFactor(shared_from_this(),ADD);
+}
+
} // namespace wolf
diff --git a/src/feature/feature_base.cpp b/src/feature/feature_base.cpp
index dd86c7c8cb7eaa911baa731d67a413d76b3df1b4..2929eb17fc4bb44f7b6b6094c05590c62a9e6ceb 100644
--- a/src/feature/feature_base.cpp
+++ b/src/feature/feature_base.cpp
@@ -70,6 +70,11 @@ FactorBasePtr FeatureBase::addFactor(FactorBasePtr _co_ptr)
return _co_ptr;
}
+void FeatureBase::removeFactor(FactorBasePtr _co_ptr)
+{
+ factor_list_.remove(_co_ptr);
+}
+
FrameBasePtr FeatureBase::getFrame() const
{
return capture_ptr_.lock()->getFrame();
@@ -78,10 +83,14 @@ FrameBasePtr FeatureBase::getFrame() const
FactorBasePtr FeatureBase::addConstrainedBy(FactorBasePtr _fac_ptr)
{
constrained_by_list_.push_back(_fac_ptr);
- _fac_ptr->setFeatureOther(shared_from_this());
return _fac_ptr;
}
+void FeatureBase::removeConstrainedBy(FactorBasePtr _fac_ptr)
+{
+ constrained_by_list_.remove(_fac_ptr);
+}
+
FactorBasePtrList& FeatureBase::getFactorList()
{
return factor_list_;
@@ -148,6 +157,8 @@ Eigen::MatrixXs FeatureBase::computeSqrtUpper(const Eigen::MatrixXs & _info) con
void FeatureBase::link(CaptureBasePtr _cpt_ptr)
{
+ assert(this->getCapture() == nullptr && "linking an already linked feature");
+
if(_cpt_ptr)
{
_cpt_ptr->addFeature(shared_from_this());
@@ -159,5 +170,4 @@ void FeatureBase::link(CaptureBasePtr _cpt_ptr)
WOLF_WARN("Linking with nullptr");
}
}
-
} // namespace wolf
diff --git a/src/frame/frame_base.cpp b/src/frame/frame_base.cpp
index c8ac508f52c3f70504d3c3fe2137573e426a5fe0..0bdb038d95318698cccbd722e81c719c03d1d97b 100644
--- a/src/frame/frame_base.cpp
+++ b/src/frame/frame_base.cpp
@@ -135,7 +135,7 @@ void FrameBase::registerNewStateBlocks()
{
for (StateBlockPtr sbp : getStateBlockVec())
if (sbp != nullptr)
- getProblem()->addStateBlock(sbp);
+ getProblem()->notifyStateBlock(sbp,ADD);
}
}
@@ -148,7 +148,7 @@ void FrameBase::removeStateBlocks()
{
if (getProblem() != nullptr)
{
- getProblem()->removeStateBlock(sbp);
+ getProblem()->notifyStateBlock(sbp,REMOVE);
}
setStateBlock(i, nullptr);
}
@@ -343,6 +343,11 @@ CaptureBasePtr FrameBase::addCapture(CaptureBasePtr _capt_ptr)
return _capt_ptr;
}
+void FrameBase::removeCapture(CaptureBasePtr _capt_ptr)
+{
+ capture_list_.remove(_capt_ptr);
+}
+
CaptureBasePtr FrameBase::getCaptureOf(const SensorBasePtr _sensor_ptr, const std::string& type)
{
for (CaptureBasePtr capture_ptr : getCaptureList())
@@ -368,12 +373,6 @@ CaptureBasePtrList FrameBase::getCapturesOf(const SensorBasePtr _sensor_ptr)
return captures;
}
-void FrameBase::unlinkCapture(CaptureBasePtr _cap_ptr)
-{
- _cap_ptr->unlinkFromFrame();
- capture_list_.remove(_cap_ptr);
-}
-
FactorBasePtr FrameBase::getFactorOf(const ProcessorBasePtr _processor_ptr, const std::string& type)
{
for (const FactorBasePtr& constaint_ptr : getConstrainedByList())
@@ -399,66 +398,41 @@ void FrameBase::getFactorList(FactorBasePtrList& _fac_list)
FactorBasePtr FrameBase::addConstrainedBy(FactorBasePtr _fac_ptr)
{
constrained_by_list_.push_back(_fac_ptr);
- _fac_ptr->setFrameOther(shared_from_this());
return _fac_ptr;
}
-FrameBasePtr FrameBase::create_PO_2D(const FrameType & _tp,
- const TimeStamp& _ts,
- const Eigen::VectorXs& _x)
-{
- assert(_x.size() == 3 && "Wrong state vector size. Should be 3 for 2D!");
- StateBlockPtr p_ptr ( std::make_shared<StateBlock> (_x.head <2> ( ) ) );
- StateBlockPtr o_ptr ( std::make_shared<StateAngle> ((Scalar) _x(2) ) );
- StateBlockPtr v_ptr ( nullptr );
- FrameBasePtr f ( std::make_shared<FrameBase>(_tp, _ts, p_ptr, o_ptr, v_ptr) );
- f->setType("PO 2D");
- return f;
-}
-FrameBasePtr FrameBase::create_PO_3D(const FrameType & _tp,
- const TimeStamp& _ts,
- const Eigen::VectorXs& _x)
-{
- assert(_x.size() == 7 && "Wrong state vector size. Should be 7 for 3D!");
- StateBlockPtr p_ptr ( std::make_shared<StateBlock> (_x.head <3> ( ) ) );
- StateBlockPtr o_ptr ( std::make_shared<StateQuaternion> (_x.tail <4> ( ) ) );
- StateBlockPtr v_ptr ( nullptr );
- FrameBasePtr f ( std::make_shared<FrameBase>(_tp, _ts, p_ptr, o_ptr, v_ptr) );
- f->setType("PO 3D");
- return f;
-}
-FrameBasePtr FrameBase::create_POV_3D(const FrameType & _tp,
- const TimeStamp& _ts,
- const Eigen::VectorXs& _x)
-{
- assert(_x.size() == 10 && "Wrong state vector size. Should be 10 for 3D!");
- StateBlockPtr p_ptr ( std::make_shared<StateBlock> (_x.head <3> ( ) ) );
- StateBlockPtr o_ptr ( std::make_shared<StateQuaternion> (_x.segment <4> (3) ) );
- StateBlockPtr v_ptr ( std::make_shared<StateBlock> (_x.tail <3> ( ) ) );
- FrameBasePtr f ( std::make_shared<FrameBase>(_tp, _ts, p_ptr, o_ptr, v_ptr) );
- f->setType("POV 3D");
- return f;
-}
-void FrameBase::link(TrajectoryBasePtr _ptr)
-{
- if(_ptr)
+void FrameBase::removeConstrainedBy(FactorBasePtr _fac_ptr)
+{
+ constrained_by_list_.remove(_fac_ptr);
+}
+
+void FrameBase::link(TrajectoryBasePtr _trj_ptr)
+{
+ assert(this->getTrajectory() == nullptr && "linking an already linked frame");
+
+ if(_trj_ptr)
{
- _ptr->addFrame(shared_from_this());
- this->setTrajectory(_ptr);
- this->setProblem(_ptr->getProblem());
- if (this->isKey()) this->registerNewStateBlocks();
+ _trj_ptr->addFrame(shared_from_this());
+ this->setTrajectory(_trj_ptr);
+ this->setProblem(_trj_ptr->getProblem());
}
else
{
WOLF_WARN("Linking with a nullptr");
}
}
-} // namespace wolf
-#include "core/common/factory.h"
-namespace wolf
+void FrameBase::setProblem(ProblemPtr _problem)
{
-namespace{ const bool WOLF_UNUSED Frame_PO_2D_Registered = FrameFactory::get().registerCreator("PO 2D", FrameBase::create_PO_2D ); }
-namespace{ const bool WOLF_UNUSED Frame_PO_3D_Registered = FrameFactory::get().registerCreator("PO 3D", FrameBase::create_PO_3D ); }
-namespace{ const bool WOLF_UNUSED Frame_POV_3D_Registered = FrameFactory::get().registerCreator("POV 3D", FrameBase::create_POV_3D); }
+ if (_problem == nullptr || _problem == this->getProblem())
+ return;
+
+ NodeBase::setProblem(_problem);
+ if (this->isKey())
+ this->registerNewStateBlocks();
+
+ for (auto cap : capture_list_)
+ cap->setProblem(_problem);
+}
+
} // namespace wolf
diff --git a/src/landmark/landmark_base.cpp b/src/landmark/landmark_base.cpp
index 25dbf77343c40d83bc9a85960561fee198a4378b..cb3f7cb21d3c0172e99ee482018f48365206d15f 100644
--- a/src/landmark/landmark_base.cpp
+++ b/src/landmark/landmark_base.cpp
@@ -92,7 +92,7 @@ void LandmarkBase::registerNewStateBlocks()
{
for (auto sbp : getStateBlockVec())
if (sbp != nullptr)
- getProblem()->addStateBlock(sbp);
+ getProblem()->notifyStateBlock(sbp,ADD);
}
}
@@ -110,7 +110,7 @@ void LandmarkBase::removeStateBlocks()
{
if (getProblem() != nullptr)
{
- getProblem()->removeStateBlock(sbp);
+ getProblem()->notifyStateBlock(sbp,REMOVE);
}
setStateBlock(i, nullptr);
}
@@ -164,12 +164,13 @@ YAML::Node LandmarkBase::saveToYaml() const
void LandmarkBase::link(MapBasePtr _map_ptr)
{
+ assert(this->getMap() == nullptr && "linking an already linked landmark");
+
if(_map_ptr)
{
this->setMap(_map_ptr);
_map_ptr->addLandmark(shared_from_this());
this->setProblem(_map_ptr->getProblem());
- this->registerNewStateBlocks();
}
else
{
@@ -177,13 +178,26 @@ void LandmarkBase::link(MapBasePtr _map_ptr)
}
}
+void LandmarkBase::setProblem(ProblemPtr _problem)
+{
+ if (_problem == nullptr || _problem == this->getProblem())
+ return;
+
+ NodeBase::setProblem(_problem);
+ this->registerNewStateBlocks();
+}
+
FactorBasePtr LandmarkBase::addConstrainedBy(FactorBasePtr _fac_ptr)
{
constrained_by_list_.push_back(_fac_ptr);
- _fac_ptr->setLandmarkOther(shared_from_this());
return _fac_ptr;
}
+void LandmarkBase::removeConstrainedBy(FactorBasePtr _fac_ptr)
+{
+ constrained_by_list_.remove(_fac_ptr);
+}
+
LandmarkBasePtr LandmarkBase::create(const YAML::Node& _node)
{
unsigned int id = _node["id"] .as< unsigned int >();
diff --git a/src/map/map_base.cpp b/src/map/map_base.cpp
index bff3d2a105ce9c4dc44050d86bb8c25b3668d739..3a3d32f8ac073ba88160995f69af8e1c150426bb 100644
--- a/src/map/map_base.cpp
+++ b/src/map/map_base.cpp
@@ -33,17 +33,9 @@ LandmarkBasePtr MapBase::addLandmark(LandmarkBasePtr _landmark_ptr)
return _landmark_ptr;
}
-void MapBase::addLandmarkList(LandmarkBasePtrList& _landmark_list)
+void MapBase::removeLandmark(LandmarkBasePtr _landmark_ptr)
{
- //TEMPORARY FIX, should be made compliant with the new emplace methodology
- LandmarkBasePtrList lmk_list_copy = _landmark_list; //since _landmark_list will be empty after addDownNodeList()
- for (LandmarkBasePtr landmark_ptr : lmk_list_copy)
- {
- landmark_ptr->setMap(shared_from_this());
- landmark_ptr->setProblem(getProblem());
- landmark_ptr->registerNewStateBlocks();
- }
- landmark_list_.splice(landmark_list_.end(), _landmark_list);
+ landmark_list_.remove(_landmark_ptr);
}
void MapBase::load(const std::string& _map_file_dot_yaml)
diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index 24e38ddd14173751dc1de557223140371f5d62aa..b607d1b356e3772439e05bc62885f3bcd83a2de6 100644
--- a/src/problem/problem.cpp
+++ b/src/problem/problem.cpp
@@ -114,19 +114,13 @@ Problem::~Problem()
// WOLF_DEBUG("destructed -P");
}
-void Problem::addSensor(SensorBasePtr _sen_ptr)
-{
- // getHardware()->addSensor(_sen_ptr);
- _sen_ptr->link(getHardware());
-}
-
SensorBasePtr Problem::installSensor(const std::string& _sen_type, //
const std::string& _unique_sensor_name, //
const Eigen::VectorXs& _extrinsics, //
IntrinsicsBasePtr _intrinsics)
{
SensorBasePtr sen_ptr = SensorFactory::get().create(uppercase(_sen_type), _unique_sensor_name, _extrinsics, _intrinsics);
- addSensor(sen_ptr);
+ sen_ptr->link(getHardware());
return sen_ptr;
}
@@ -146,14 +140,16 @@ SensorBasePtr Problem::installSensor(const std::string& _sen_type, //
return installSensor(_sen_type, _unique_sensor_name, _extrinsics, IntrinsicsBasePtr());
}
- SensorBasePtr Problem::installSensor(const std::string& _sen_type, //
- const std::string& _unique_sensor_name, //
- const paramsServer& _server)
- {
- SensorBasePtr sen_ptr = AutoConfSensorFactory::get().create(uppercase(_sen_type), _unique_sensor_name, _server);
- addSensor(sen_ptr);
- return sen_ptr;
- }
+
+SensorBasePtr Problem::installSensor(const std::string& _sen_type, //
+ const std::string& _unique_sensor_name, //
+ const paramsServer& _server)
+{
+ SensorBasePtr sen_ptr = AutoConfSensorFactory::get().create(uppercase(_sen_type), _unique_sensor_name, _server);
+ sen_ptr->link(getHardware());
+ return sen_ptr;
+}
+
ProcessorBasePtr Problem::installProcessor(const std::string& _prc_type, //
const std::string& _unique_processor_name, //
SensorBasePtr _corresponding_sensor_ptr, //
@@ -168,7 +164,6 @@ ProcessorBasePtr Problem::installProcessor(const std::string& _prc_type, //
ProcessorBasePtr prc_ptr = ProcessorFactory::get().create(uppercase(_prc_type), _unique_processor_name, _prc_params, _corresponding_sensor_ptr);
prc_ptr->configure(_corresponding_sensor_ptr);
- // _corresponding_sensor_ptr->addProcessor(prc_ptr);
prc_ptr->link(_corresponding_sensor_ptr);
// setting the origin in all processor motion if origin already setted
@@ -211,7 +206,6 @@ ProcessorBasePtr Problem::installProcessor(const std::string& _prc_type, //
ProcessorBasePtr prc_ptr = AutoConfProcessorFactory::get().create(uppercase(_prc_type), _unique_processor_name, _server, sen_ptr);
prc_ptr->configure(sen_ptr);
prc_ptr->link(sen_ptr);
- // sen_ptr->addProcessor(prc_ptr);
// setting the origin in all processor motion if origin already setted
if (prc_ptr->isMotion() && prior_is_set_)
@@ -445,57 +439,30 @@ void Problem::auxFrameCallback(FrameBasePtr _frame_ptr, ProcessorBasePtr _proces
processor_motion_ptr_->keyFrameCallback(_frame_ptr, _time_tolerance);
}
-LandmarkBasePtr Problem::addLandmark(LandmarkBasePtr _lmk_ptr)
-{
- getMap()->addLandmark(_lmk_ptr);
- return _lmk_ptr;
-}
-
-void Problem::addLandmarkList(LandmarkBasePtrList& _lmk_list)
-{
- getMap()->addLandmarkList(_lmk_list);
-}
-
-StateBlockPtr Problem::addStateBlock(StateBlockPtr _state_ptr)
+StateBlockPtr Problem::notifyStateBlock(StateBlockPtr _state_ptr, Notification _noti)
{
std::lock_guard<std::mutex> lock(mut_state_block_notifications_);
//std::cout << "Problem::addStateBlockPtr " << _state_ptr.get() << std::endl;
- // Add add notification
- // Check if there is already a notification for this state block
- auto notification_it = state_block_notification_map_.find(_state_ptr);
- if (notification_it != state_block_notification_map_.end() && notification_it->second == ADD)
- {
- WOLF_WARN("There is already an ADD notification of this state block");
- }
- else
- state_block_notification_map_[_state_ptr] = ADD;
-
- return _state_ptr;
-}
-
-void Problem::removeStateBlock(StateBlockPtr _state_ptr)
-{
- std::lock_guard<std::mutex> lock(mut_state_block_notifications_);
- //std::cout << "Problem::removeStateBlockPtr " << _state_ptr.get() << std::endl;
-
// Check if there is already a notification for this state block
auto notification_it = state_block_notification_map_.find(_state_ptr);
+ // exsiting notification for this state block
if (notification_it != state_block_notification_map_.end())
{
- if (notification_it->second == REMOVE)
+ // duplicated notification
+ if ( notification_it->second == _noti)
{
- WOLF_WARN("There is already an REMOVE notification of this state block");
+ WOLF_WARN("This notification has been already notified");
}
- // Remove ADD notification
+ // opposite notification -> cancell out eachother
else
- {
state_block_notification_map_.erase(notification_it);
- }
}
- // Add REMOVE notification
+ // Add notification
else
- state_block_notification_map_[_state_ptr] = REMOVE;
+ state_block_notification_map_[_state_ptr] = _noti;
+
+ return _state_ptr;
}
bool Problem::getStateBlockNotification(const StateBlockPtr& sb_ptr, Notification& notif) const
@@ -508,45 +475,31 @@ bool Problem::getStateBlockNotification(const StateBlockPtr& sb_ptr, Notificatio
return true;
}
-FactorBasePtr Problem::addFactor(FactorBasePtr _factor_ptr)
+FactorBasePtr Problem::notifyFactor(FactorBasePtr _factor_ptr, Notification _noti)
{
std::lock_guard<std::mutex> lock(mut_factor_notifications_);
//std::cout << "Problem::addFactorPtr " << _factor_ptr->id() << std::endl;
- // Add ADD notification
- // Check if there is already a notification for this state block
- auto notification_it = factor_notification_map_.find(_factor_ptr);
- if (notification_it != factor_notification_map_.end() && notification_it->second == ADD)
- {
- WOLF_WARN("There is already an ADD notification of this factor");
- }
- // Add ADD notification (override in case of REMOVE)
- else
- factor_notification_map_[_factor_ptr] = ADD;
-
- return _factor_ptr;
-}
-
-void Problem::removeFactor(FactorBasePtr _factor_ptr)
-{
- std::lock_guard<std::mutex> lock(mut_factor_notifications_);
- //std::cout << "Problem::removeFactorPtr " << _factor_ptr->id() << std::endl;
-
- // Check if there is already a notification for this state block
+ // Check if there is already the same notification for this factor
auto notification_it = factor_notification_map_.find(_factor_ptr);
+ // exsiting notification for this factor
if (notification_it != factor_notification_map_.end())
{
- if (notification_it->second == REMOVE)
+ // duplicated notification
+ if (notification_it->second == _noti)
{
- WOLF_WARN("There is already an REMOVE notification of this state block");
+ WOLF_WARN("This notification has been already notified");
}
- // Remove ADD notification
+ // opposite notification -> cancell out eachother
else
factor_notification_map_.erase(notification_it);
+
}
- // Add REMOVE notification
+ // Add notification
else
- factor_notification_map_[_factor_ptr] = REMOVE;
+ factor_notification_map_[_factor_ptr] = _noti;
+
+ return _factor_ptr;
}
bool Problem::getFactorNotification(const FactorBasePtr& fac_ptr, Notification& notif) const
diff --git a/src/processor/processor_base.cpp b/src/processor/processor_base.cpp
index a70930c6b936ef898db10f22a01d8b8010cdc1c3..32c555dd44522ee092c34c5b3d372eeb34a0e36b 100644
--- a/src/processor/processor_base.cpp
+++ b/src/processor/processor_base.cpp
@@ -85,19 +85,22 @@ void ProcessorBase::remove()
sen->getProcessorList().remove(this_p);
}
}
- void ProcessorBase::link(SensorBasePtr _sen_ptr)
+
+void ProcessorBase::link(SensorBasePtr _sen_ptr)
+{
+ assert(this->getSensor() == nullptr && "linking an already linked processor");
+
+ if(_sen_ptr)
{
- if(_sen_ptr)
- {
- _sen_ptr->addProcessor(shared_from_this());
- this->setSensor(_sen_ptr);
- this->setProblem(_sen_ptr->getProblem());
- }
- else
- {
- WOLF_WARN("Linking with a nullptr");
- }
+ _sen_ptr->addProcessor(shared_from_this());
+ this->setSensor(_sen_ptr);
+ this->setProblem(_sen_ptr->getProblem());
}
+ else
+ {
+ WOLF_WARN("Linking with a nullptr");
+ }
+}
/////////////////////////////////////////////////////////////////////////////////////////
void BufferPackKeyFrame::add(const FrameBasePtr& _key_frame, const Scalar& _time_tolerance)
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 a33fcde2cac2a04d149f759e1e1ad08c574c041e..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())
@@ -684,4 +652,20 @@ PackKeyFramePtr ProcessorMotion::computeProcessingStep()
return pack;
}
+void ProcessorMotion::setProblem(ProblemPtr _problem)
+{
+ if (_problem == nullptr || _problem == this->getProblem())
+ return;
+
+ NodeBase::setProblem(_problem);
+
+ // set the origin
+ if (origin_ptr_ == nullptr && this->getProblem()->getLastKeyFrame() != nullptr)
+ this->setOrigin(this->getProblem()->getLastKeyFrame());
+
+ // set the main processor motion
+ if (this->getProblem()->getProcessorMotion() == nullptr)
+ this->getProblem()->setProcessorMotion(std::static_pointer_cast<ProcessorMotion>(shared_from_this()));
+};
+
}
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());
diff --git a/src/processor/processor_tracker.cpp b/src/processor/processor_tracker.cpp
index ad2c73b62f0c5a91601f59272aae2355b9b313d6..3cde80f33ac88d48ef30d56daf0a43a3ffe7c075 100644
--- a/src/processor/processor_tracker.cpp
+++ b/src/processor/processor_tracker.cpp
@@ -21,8 +21,7 @@ ProcessorTracker::ProcessorTracker(const std::string& _type,
processing_step_(FIRST_TIME_WITHOUT_PACK),
origin_ptr_(nullptr),
last_ptr_(nullptr),
- incoming_ptr_(nullptr),
- number_of_tracks_(0)
+ incoming_ptr_(nullptr)
{
//
}
@@ -55,7 +54,7 @@ void ProcessorTracker::process(CaptureBasePtr const _incoming_ptr)
PackKeyFramePtr pack = buffer_pack_kf_.selectPack( incoming_ptr_, params_tracker_->time_tolerance);
buffer_pack_kf_.removeUpTo( pack->key_frame->getTimeStamp() );
- WOLF_DEBUG( "PT ", getName(), ": KF" , pack->key_frame->id() , " callback unpacked with ts= " , pack->key_frame->getTimeStamp().get() );
+ WOLF_DEBUG( "PT ", getName(), " FIRST_TIME_WITH_PACK: KF" , pack->key_frame->id() , " callback unpacked with ts= " , pack->key_frame->getTimeStamp() );
// Append incoming to KF
// pack->key_frame->addCapture(incoming_ptr_);
@@ -76,6 +75,8 @@ void ProcessorTracker::process(CaptureBasePtr const _incoming_ptr)
}
case FIRST_TIME_WITHOUT_PACK :
{
+ WOLF_DEBUG( "PT ", getName(), " FIRST_TIME_WITHOUT_PACK" );
+
FrameBasePtr kfrm = getProblem()->emplaceFrame(KEY, incoming_ptr_->getTimeStamp());
incoming_ptr_->link(kfrm);
@@ -98,10 +99,12 @@ void ProcessorTracker::process(CaptureBasePtr const _incoming_ptr)
{
// No-break case only for debug. Next case will be executed too.
PackKeyFramePtr pack = buffer_pack_kf_.selectPack( incoming_ptr_, params_tracker_->time_tolerance);
- WOLF_DEBUG( "PT ", getName(), ": KF" , pack->key_frame->id() , " callback unpacked with ts= " , pack->key_frame->getTimeStamp().get() );
+ WOLF_DEBUG( "PT ", getName(), " SECOND_TIME_WITH_PACK: KF" , pack->key_frame->id() , " callback unpacked with ts= " , pack->key_frame->getTimeStamp() );
} // @suppress("No break at end of case")
case SECOND_TIME_WITHOUT_PACK :
{
+ WOLF_DEBUG( "PT ", getName(), " SECOND_TIME_WITHOUT_PACK" );
+
FrameBasePtr frm = getProblem()->emplaceFrame(NON_ESTIMATED, incoming_ptr_->getTimeStamp());
incoming_ptr_->link(frm);
@@ -127,16 +130,14 @@ void ProcessorTracker::process(CaptureBasePtr const _incoming_ptr)
PackKeyFramePtr pack = buffer_pack_kf_.selectPack( last_ptr_ , params_tracker_->time_tolerance);
buffer_pack_kf_.removeUpTo( pack->key_frame->getTimeStamp() );
- WOLF_DEBUG( "PT ", getName(), ": KF" , pack->key_frame->id() , " callback unpacked with ts= " , pack->key_frame->getTimeStamp().get() );
+ WOLF_DEBUG( "PT ", getName(), " RUNNING_WITH_PACK: KF" , pack->key_frame->id() , " callback unpacked with ts= " , pack->key_frame->getTimeStamp() );
processKnown();
// Capture last_ is added to the new keyframe
FrameBasePtr last_old_frame = last_ptr_->getFrame();
- last_old_frame->unlinkCapture(last_ptr_);
+ last_ptr_->link(pack->key_frame); // automatically calling last_ptr_->unlink();
last_old_frame->remove();
- // pack->key_frame->addCapture(last_ptr_);
- last_ptr_->link(pack->key_frame);
// Create new frame
FrameBasePtr frm = getProblem()->emplaceFrame(NON_ESTIMATED, incoming_ptr_->getTimeStamp());
@@ -158,19 +159,18 @@ void ProcessorTracker::process(CaptureBasePtr const _incoming_ptr)
}
case RUNNING_WITHOUT_PACK :
{
+ WOLF_DEBUG( "PT ", getName(), " RUNNING_WITHOUT_PACK" );
+
processKnown();
- // eventually add more features
- if (last_ptr_->getFeatureList().size() < params_tracker_->min_features_for_keyframe)
+ if (voteForKeyFrame() && permittedKeyFrame())
{
- //WOLF_TRACE("Adding more features...");
+ // process
processNew(params_tracker_->max_new_features);
- }
- if (voteForKeyFrame() && permittedKeyFrame())
- {
- // We create a KF
+ //TODO abort KF if last_ptr_->getFeatureList().size() < params_tracker_->min_features_for_keyframe
+ // We create a KF
// set KF on last
last_ptr_->getFrame()->setState(getProblem()->getState(last_ptr_->getTimeStamp()));
last_ptr_->getFrame()->setKey();
@@ -179,9 +179,6 @@ void ProcessorTracker::process(CaptureBasePtr const _incoming_ptr)
FrameBasePtr frm = getProblem()->emplaceFrame(NON_ESTIMATED, incoming_ptr_->getTimeStamp());
incoming_ptr_->link(frm);
- // process
- processNew(params_tracker_->max_new_features);
-
// Establish factors
establishFactors();
@@ -245,7 +242,6 @@ void ProcessorTracker::process(CaptureBasePtr const _incoming_ptr)
break;
}
- number_of_tracks_ = last_ptr_->getFeatureList().size();
postProcess();
}
diff --git a/src/processor/processor_tracker_feature.cpp b/src/processor/processor_tracker_feature.cpp
index fc923797005a5a5efd5e01c1e88473b6b595d82e..f27d3b9c1bc6f664e135b4ca6893b6a977e7d206 100644
--- a/src/processor/processor_tracker_feature.cpp
+++ b/src/processor/processor_tracker_feature.cpp
@@ -36,25 +36,30 @@ unsigned int ProcessorTrackerFeature::processNew(const int& _max_new_features)
matches_last_from_incoming_.clear();
// Populate the last Capture with new Features. The result is in new_features_last_.
- unsigned int n = detectNewFeatures(_max_new_features, new_features_last_);
+ unsigned int n = detectNewFeatures(_max_new_features, last_ptr_, new_features_last_);
+
+ // check all features have been emplaced
+ assert(std::all_of(new_features_last_.begin(), new_features_last_.end(), [](FeatureBasePtr f){return f->getCapture() != nullptr;}) &&
+ "any not linked feature returned by detectNewFeatures()");
+
+ // fill the track matrix
for (auto ftr : new_features_last_)
- track_matrix_.newTrack(last_ptr_, ftr);
+ track_matrix_.newTrack(ftr);
// Track new features from last to incoming. This will append new correspondences to matches_last_incoming
- trackFeatures(new_features_last_, new_features_incoming_, matches_last_from_incoming_);
+ trackFeatures(new_features_last_, incoming_ptr_, new_features_incoming_, matches_last_from_incoming_);
+
+ // check all features have been emplaced
+ assert(std::all_of(new_features_incoming_.begin(), new_features_incoming_.end(), [](FeatureBasePtr f){return f->getCapture() != nullptr;}) &&
+ "any not linked feature returned by trackFeatures()");
+
+ // fill the track matrix
for (auto ftr : new_features_incoming_)
{
- ftr->setProblem(this->getProblem());
- SizeStd trk_id_from_last = matches_last_from_incoming_[ftr]->feature_ptr_->trackId();
- track_matrix_.add(trk_id_from_last, incoming_ptr_, ftr);
+ assert(matches_last_from_incoming_.count(ftr) != 0);
+ track_matrix_.add(matches_last_from_incoming_[ftr]->feature_ptr_, ftr);
}
- // Append all new Features to the incoming Captures' list of Features
- incoming_ptr_->addFeatureList(new_features_incoming_);
-
- // Append all new Features to the last Captures' list of Features
- last_ptr_->addFeatureList(new_features_last_);
-
// return the number of new features detected in \b last
return n;
}
@@ -70,16 +75,23 @@ unsigned int ProcessorTrackerFeature::processKnown()
known_features_incoming_.clear();
if (!last_ptr_ || last_ptr_->getFeatureList().empty())
- {
return 0;
- }
// Track features from last_ptr_ to incoming_ptr_
- trackFeatures(last_ptr_->getFeatureList(), known_features_incoming_, matches_last_from_incoming_);
- for (auto match : matches_last_from_incoming_)
+ trackFeatures(last_ptr_->getFeatureList(),
+ incoming_ptr_,
+ known_features_incoming_,
+ matches_last_from_incoming_);
+
+ // check all features have been emplaced
+ assert(std::all_of(known_features_incoming_.begin(), known_features_incoming_.end(), [](FeatureBasePtr f){return f->getCapture() != nullptr;}) &&
+ "any not linked feature returned by trackFeatures()");
+
+ // fill the track matrix
+ for (auto ftr : known_features_incoming_)
{
- SizeStd trk_id_from_last = match.second->feature_ptr_->trackId();
- track_matrix_.add(trk_id_from_last, incoming_ptr_, match.first);
+ assert(matches_last_from_incoming_.count(ftr) != 0);
+ track_matrix_.add(matches_last_from_incoming_[ftr]->feature_ptr_, ftr);
}
// Check/correct incoming-origin correspondences
@@ -87,14 +99,10 @@ unsigned int ProcessorTrackerFeature::processKnown()
{
for (auto feature_in_incoming : known_features_incoming_)
{
- SizeStd track_id = feature_in_incoming->trackId();
+ SizeStd track_id = feature_in_incoming->trackId();
FeatureBasePtr feature_in_last = track_matrix_.feature(track_id, last_ptr_);
FeatureBasePtr feature_in_origin = track_matrix_.feature(track_id, origin_ptr_);
- if (correctFeatureDrift(feature_in_origin, feature_in_last, feature_in_incoming))
- {
- feature_in_incoming->setProblem(this->getProblem());
- }
- else
+ if (!correctFeatureDrift(feature_in_origin, feature_in_last, feature_in_incoming))
{
// Remove this feature from many places:
matches_last_from_incoming_ .erase (feature_in_incoming); // remove match
@@ -105,9 +113,6 @@ unsigned int ProcessorTrackerFeature::processKnown()
}
}
- // Add to wolf tree
- incoming_ptr_->addFeatureList(known_features_incoming_);
-
return matches_last_from_incoming_.size();
}
@@ -116,10 +121,6 @@ void ProcessorTrackerFeature::advanceDerived()
// Reset here the list of correspondences.
matches_last_from_incoming_.clear();
- // We set problem here because we could not determine Problem from incoming capture at the time of adding the features to incoming's feature list.
- for (auto ftr : incoming_ptr_->getFeatureList())
- ftr->setProblem(getProblem());
-
// // remove last from track matrix in case you want to have only KF in the track matrix
// track_matrix_.remove(last_ptr_);
}
@@ -129,17 +130,11 @@ void ProcessorTrackerFeature::resetDerived()
// Reset here the list of correspondences.
matches_last_from_incoming_.clear();
- // Update features according to the move above.
- TrackMatches matches_origin_last = track_matrix_.matches(origin_ptr_, last_ptr_);
-
- for (auto const & pair_trkid_pair : matches_origin_last)
- {
- FeatureBasePtr feature_in_origin = pair_trkid_pair.second.first;
- FeatureBasePtr feature_in_last = pair_trkid_pair.second.second;
- feature_in_last->setProblem(getProblem()); // Since these features were in incoming_, they had no Problem assigned.
-
- WOLF_DEBUG("Matches reset: track: ", pair_trkid_pair.first, " origin: ", feature_in_origin->id(), " last: ", feature_in_last->id());
- }
+ // Debug
+ //for (auto const & pair_trkid_pair : track_matrix_.matches(origin_ptr_, last_ptr_))
+ //{
+ // WOLF_DEBUG("Matches reset: track: ", pair_trkid_pair.first, " origin: ", pair_trkid_pair.second.first->id(), " last: ", pair_trkid_pair.second.second->id());
+ //}
}
void ProcessorTrackerFeature::establishFactors()
@@ -155,12 +150,13 @@ void ProcessorTrackerFeature::establishFactors()
FeatureBasePtr feature_in_last = pair_trkid_pair.second.second;
- auto fac_ptr = createFactor(feature_in_last, feature_in_origin);
- fac_ptr->link(feature_in_last);
+ auto fac_ptr = emplaceFactor(feature_in_last, feature_in_origin);
+
+ assert(fac_ptr->getFeature() != nullptr && "not emplaced factor returned by emplaceFactor()");
WOLF_DEBUG( "Factor: track: " , feature_in_last->trackId(),
- " origin: " , feature_in_origin->id() ,
- " from last: " , feature_in_last->id() );
+ " origin: " , feature_in_origin->id() ,
+ " from last: " , feature_in_last->id() );
}
}
diff --git a/src/processor/processor_tracker_feature_dummy.cpp b/src/processor/processor_tracker_feature_dummy.cpp
index 999f8620792dd83a7e34cb295ec4cf32049568ad..48838b3cda114b17cf0ce840b16983534fb7d258 100644
--- a/src/processor/processor_tracker_feature_dummy.cpp
+++ b/src/processor/processor_tracker_feature_dummy.cpp
@@ -11,14 +11,15 @@
namespace wolf
{
-unsigned int ProcessorTrackerFeatureDummy::trackFeatures(const FeatureBasePtrList& _features_last_in,
- FeatureBasePtrList& _features_incoming_out,
+unsigned int ProcessorTrackerFeatureDummy::trackFeatures(const FeatureBasePtrList& _features_in,
+ const CaptureBasePtr& _capture,
+ FeatureBasePtrList& _features_out,
FeatureMatchMap& _feature_correspondences)
{
- WOLF_INFO("tracking " , _features_last_in.size() , " features...");
+ WOLF_INFO("tracking " , _features_in.size() , " features...");
auto count = 0;
- for (auto feat_in : _features_last_in)
+ for (auto feat_in : _features_in)
{
if (count < params_tracker_feature_dummy_->n_tracks_lost) // lose first ntrackslost tracks
{
@@ -27,15 +28,18 @@ unsigned int ProcessorTrackerFeatureDummy::trackFeatures(const FeatureBasePtrLis
}
else
{
- FeatureBasePtr ftr(std::make_shared<FeatureBase>("POINT IMAGE", feat_in->getMeasurement(), feat_in->getMeasurementCovariance()));
- _features_incoming_out.push_back(ftr);
- _feature_correspondences[_features_incoming_out.back()] = std::make_shared<FeatureMatch>(FeatureMatch({feat_in,0}));
+ FeatureBasePtr ftr = FeatureBase::emplace<FeatureBase>(_capture,
+ "FEATURE DUMMY",
+ feat_in->getMeasurement(),
+ feat_in->getMeasurementCovariance());
+ _features_out.push_back(ftr);
+ _feature_correspondences[_features_out.back()] = std::make_shared<FeatureMatch>(FeatureMatch({feat_in,0}));
WOLF_INFO("track: " , feat_in->trackId() , " last: " , feat_in->id() , " inc: " , ftr->id() , " !" );
}
}
- return _features_incoming_out.size();
+ return _features_out.size();
}
bool ProcessorTrackerFeatureDummy::voteForKeyFrame()
@@ -49,42 +53,43 @@ bool ProcessorTrackerFeatureDummy::voteForKeyFrame()
return incoming_ptr_->getFeatureList().size() < params_tracker_feature_->min_features_for_keyframe;
}
-unsigned int ProcessorTrackerFeatureDummy::detectNewFeatures(const int& _max_features, FeatureBasePtrList& _features_incoming_out)
+unsigned int ProcessorTrackerFeatureDummy::detectNewFeatures(const int& _max_new_features,
+ const CaptureBasePtr& _capture,
+ FeatureBasePtrList& _features_out)
{
- unsigned int max_features = _max_features;
+ unsigned int max_features = _max_new_features;
if (max_features == -1)
{
max_features = 10;
WOLF_INFO("max_features unlimited, setting it to " , max_features);
}
- WOLF_INFO("Detecting " , _max_features , " new features..." );
+ WOLF_INFO("Detecting " , _max_new_features , " new features..." );
// detecting new features
for (unsigned int i = 0; i < max_features; i++)
{
- FeatureBasePtr ftr(std::make_shared<FeatureBase>("DUMMY FEATURE",
- Eigen::Vector1s::Ones(),
- Eigen::MatrixXs::Ones(1, 1)));
- _features_incoming_out.push_back(ftr);
+ FeatureBasePtr ftr = FeatureBase::emplace<FeatureBase>(_capture,
+ "DUMMY FEATURE",
+ Eigen::Vector1s::Ones(),
+ Eigen::MatrixXs::Ones(1, 1));
+ _features_out.push_back(ftr);
WOLF_INFO("feature " , ftr->id() , " detected!" );
}
- WOLF_INFO(_features_incoming_out.size() , " features detected!");
+ WOLF_INFO(_features_out.size() , " features detected!");
- return _features_incoming_out.size();
+ return _features_out.size();
}
-FactorBasePtr ProcessorTrackerFeatureDummy::createFactor(FeatureBasePtr _feature_ptr,
- FeatureBasePtr _feature_other_ptr)
+FactorBasePtr ProcessorTrackerFeatureDummy::emplaceFactor(FeatureBasePtr _feature_ptr,
+ FeatureBasePtr _feature_other_ptr)
{
- WOLF_INFO( "creating factor: track " , _feature_other_ptr->trackId() , " last feature " , _feature_ptr->id()
- , " with origin feature " , _feature_other_ptr->id() );
+ WOLF_INFO( "emplacing factor: track " , _feature_other_ptr->trackId() , " last feature " , _feature_ptr->id(),
+ " with origin feature " , _feature_other_ptr->id() );
- auto fac = std::make_shared<FactorFeatureDummy>(_feature_ptr, _feature_other_ptr, shared_from_this());
-
- return fac;
+ return FactorBase::emplace<FactorFeatureDummy>(_feature_ptr, _feature_ptr, _feature_other_ptr, shared_from_this());
}
ProcessorBasePtr ProcessorTrackerFeatureDummy::create(const std::string& _unique_name,
diff --git a/src/processor/processor_tracker_landmark.cpp b/src/processor/processor_tracker_landmark.cpp
index c55b21dc981c000ccf508b82ecf1f6842a1285c0..0d7c50c2a9e6ac8a3e91009a1ed361251a4104aa 100644
--- a/src/processor/processor_tracker_landmark.cpp
+++ b/src/processor/processor_tracker_landmark.cpp
@@ -23,50 +23,43 @@ ProcessorTrackerLandmark::ProcessorTrackerLandmark(const std::string& _type,
ProcessorTrackerLandmark::~ProcessorTrackerLandmark()
{
- // All is shared_ptr: no need to destruct explicitly
//
- // for ( auto match : matches_landmark_from_incoming_)
- // {
- // match.second.reset(); // : Should we just remove the entries? What about match.first?
- // }
- // for ( auto match : matches_landmark_from_last_)
- // {
- // match.second.reset(); // : Should we just remove the entries? What about match.first?
- // }
}
void ProcessorTrackerLandmark::advanceDerived()
{
- for (auto match : matches_landmark_from_last_)
- {
- match.second.reset(); // TODO: Should we just remove the entries? What about match.first?
- }
matches_landmark_from_last_ = std::move(matches_landmark_from_incoming_);
new_features_last_ = std::move(new_features_incoming_);
+
+ matches_landmark_from_incoming_.clear();
+ new_features_incoming_.clear();
+ new_landmarks_.clear();
}
void ProcessorTrackerLandmark::resetDerived()
{
- for (auto match : matches_landmark_from_last_)
- {
- match.second.reset(); // TODO: Should we just remove the entries? What about match.first?
- }
matches_landmark_from_last_ = std::move(matches_landmark_from_incoming_);
new_features_last_ = std::move(new_features_incoming_);
+
+ matches_landmark_from_incoming_.clear();
+ new_features_incoming_.clear();
+ new_landmarks_.clear();
}
unsigned int ProcessorTrackerLandmark::processKnown()
{
- // clear matches list
- matches_landmark_from_incoming_.clear();
+ assert(matches_landmark_from_incoming_.empty());
// Find landmarks in incoming_ptr_
FeatureBasePtrList known_features_list_incoming;
unsigned int n = findLandmarks(getProblem()->getMap()->getLandmarkList(),
+ incoming_ptr_,
known_features_list_incoming,
matches_landmark_from_incoming_);
- // Append found incoming features
- incoming_ptr_->addFeatureList(known_features_list_incoming);
+
+ // check all features have been emplaced
+ assert(std::all_of(known_features_list_incoming.begin(), known_features_list_incoming.end(), [](FeatureBasePtr f){return f->getCapture() != nullptr;}) &&
+ "any not linked feature returned by findLandmarks()");
return n;
}
@@ -76,54 +69,56 @@ unsigned int ProcessorTrackerLandmark::processNew(const int& _max_features)
/* Rationale: A keyFrame will be created using the last Capture.
*
* First, we work on this Capture to detect new Features,
- * eventually create Landmarks with them,
- * and in such case create the new Factors feature-landmark.
- * When done, we need to track these new Features to the incoming Capture.
- *
- * At the end, all new Features are appended to the lists of known Features in
- * the last and incoming Captures.
+ * eventually create Landmarks with them.
+ * When done, we need to find these new Landmarks in the incoming Capture.
*/
- // clear new lists
- new_features_last_.clear();
- new_features_incoming_.clear();
- new_landmarks_.clear();
+ // assuming cleared new lists
+ assert(new_features_last_.empty());
+ assert(new_features_incoming_.empty());
+ assert(new_landmarks_.empty());
// We first need to populate the \b last Capture with new Features
- unsigned int n = detectNewFeatures(_max_features, new_features_last_);
+ unsigned int n = detectNewFeatures(_max_features,
+ last_ptr_,
+ new_features_last_);
+
+ // check all features have been emplaced
+ assert(std::all_of(new_features_last_.begin(), new_features_last_.end(), [](FeatureBasePtr f){return f->getCapture() != nullptr;}) &&
+ "any not linked feature returned by detectNewFeatures()");
// create new landmarks with the new features discovered
- createNewLandmarks();
+ emplaceNewLandmarks();
// Find the new landmarks in incoming_ptr_ (if it's not nullptr)
if (incoming_ptr_ != nullptr)
{
- findLandmarks(new_landmarks_, new_features_incoming_, matches_landmark_from_incoming_);
-
- // Append all new Features to the incoming Capture's list of Features
- incoming_ptr_->addFeatureList(new_features_incoming_);
+ findLandmarks(new_landmarks_,
+ incoming_ptr_,
+ new_features_incoming_,
+ matches_landmark_from_incoming_);
+
+ // check all features have been emplaced
+ assert(std::all_of(new_features_incoming_.begin(), new_features_incoming_.end(), [](FeatureBasePtr f){return f->getCapture() != nullptr;}) &&
+ "any not linked feature returned by findLandmarks()");
}
- // Append all new Features to the last Capture's list of Features
- last_ptr_->addFeatureList(new_features_last_);
-
- // Append new landmarks to the map
- getProblem()->addLandmarkList(new_landmarks_);
-
// return the number of new features detected in \b last
return n;
}
-void ProcessorTrackerLandmark::createNewLandmarks()
+void ProcessorTrackerLandmark::emplaceNewLandmarks()
{
- // First, make sure the list is empty and will only contain new lmks
- new_landmarks_.clear();
+ // assuming empty list
+ assert(new_landmarks_.empty());
// Create a landmark for each new feature in last Capture:
for (auto new_feature_ptr : new_features_last_)
{
// create new landmark
- LandmarkBasePtr new_lmk_ptr = createLandmark(new_feature_ptr);
+ LandmarkBasePtr new_lmk_ptr = emplaceLandmark(new_feature_ptr);
+
+ assert(new_lmk_ptr->getMap() != nullptr && "not linked landmark returned by emplaceLandmark()");
new_landmarks_.push_back(new_lmk_ptr);
@@ -137,13 +132,11 @@ void ProcessorTrackerLandmark::establishFactors()
// Loop all features in last_ptr_
for (auto last_feature : last_ptr_->getFeatureList())
{
- auto lmk = matches_landmark_from_last_[last_feature]->landmark_ptr_;
- FactorBasePtr fac_ptr = createFactor(last_feature,
- lmk);
- if (fac_ptr != nullptr) // factor links
- {
- fac_ptr->link(last_feature);
- }
+ assert(matches_landmark_from_last_.count(last_feature) == 1);
+
+ FactorBasePtr fac_ptr = emplaceFactor(last_feature, matches_landmark_from_last_[last_feature]->landmark_ptr_);
+
+ assert(fac_ptr->getFeature() != nullptr && "not linked factor returned by emplaceFactor()");
}
}
diff --git a/src/processor/processor_tracker_landmark_dummy.cpp b/src/processor/processor_tracker_landmark_dummy.cpp
index 61540a8afec1b4c204e59b92fca645efde1cd4ca..422e778279f605a01fddbaa67b10baab5f4de92d 100644
--- a/src/processor/processor_tracker_landmark_dummy.cpp
+++ b/src/processor/processor_tracker_landmark_dummy.cpp
@@ -26,7 +26,8 @@ ProcessorTrackerLandmarkDummy::~ProcessorTrackerLandmarkDummy()
}
unsigned int ProcessorTrackerLandmarkDummy::findLandmarks(const LandmarkBasePtrList& _landmarks_in,
- FeatureBasePtrList& _features_incoming_out,
+ const CaptureBasePtr& _capture,
+ FeatureBasePtrList& _features_out,
LandmarkMatchMap& _feature_landmark_correspondences)
{
std::cout << "\tProcessorTrackerLandmarkDummy::findLandmarks" << std::endl;
@@ -40,10 +41,11 @@ unsigned int ProcessorTrackerLandmarkDummy::findLandmarks(const LandmarkBasePtrL
{
if (count < prev_found - params_tracker_landmark_dummy_->n_landmarks_lost) // lose last n_landmarks_lost landmarks
{
- FeatureBasePtr ftr(std::make_shared<FeatureBase>("DUMMY FEATURE",
- Eigen::Vector1s::Ones(),
- Eigen::MatrixXs::Ones(1, 1)));
- _features_incoming_out.push_back(ftr);
+ FeatureBasePtr ftr = FeatureBase::emplace<FeatureBase>(_capture,
+ "DUMMY FEATURE",
+ Eigen::Vector1s::Ones(),
+ Eigen::MatrixXs::Ones(1, 1));
+ _features_out.push_back(ftr);
_feature_landmark_correspondences[ftr] = std::make_shared<LandmarkMatch>(landmark_in_ptr, 1);
std::cout << "\t\tlandmark " << landmark_in_ptr->id() << " found!" << std::endl;
}
@@ -53,7 +55,7 @@ unsigned int ProcessorTrackerLandmarkDummy::findLandmarks(const LandmarkBasePtrL
}
count++;
}
- return _features_incoming_out.size();
+ return _features_out.size();
}
bool ProcessorTrackerLandmarkDummy::voteForKeyFrame()
@@ -64,7 +66,9 @@ bool ProcessorTrackerLandmarkDummy::voteForKeyFrame()
return vote;
}
-unsigned int ProcessorTrackerLandmarkDummy::detectNewFeatures(const int& _max_features, FeatureBasePtrList& _features_incoming_out)
+unsigned int ProcessorTrackerLandmarkDummy::detectNewFeatures(const int& _max_features,
+ const CaptureBasePtr& _capture,
+ FeatureBasePtrList& _features_out)
{
std::cout << "\tProcessorTrackerLandmarkDummy::detectNewFeatures" << std::endl;
@@ -80,31 +84,32 @@ unsigned int ProcessorTrackerLandmarkDummy::detectNewFeatures(const int& _max_fe
// detecting new features
for (unsigned int i = 0; i < max_features; i++)
{
- FeatureBasePtr ftr(std::make_shared<FeatureBase>("DUMMY FEATURE",
- Eigen::Vector1s::Ones(),
- Eigen::MatrixXs::Ones(1, 1)));
- _features_incoming_out.push_back(ftr);
+ FeatureBasePtr ftr = FeatureBase::emplace<FeatureBase>(_capture,
+ "DUMMY FEATURE",
+ Eigen::Vector1s::Ones(),
+ Eigen::MatrixXs::Ones(1, 1));
+ _features_out.push_back(ftr);
WOLF_INFO("feature " , ftr->id() , " detected!" );
}
- WOLF_INFO(_features_incoming_out.size() , " features detected!");
+ WOLF_INFO(_features_out.size() , " features detected!");
- return _features_incoming_out.size();
+ return _features_out.size();
}
-LandmarkBasePtr ProcessorTrackerLandmarkDummy::createLandmark(FeatureBasePtr _feature_ptr)
+LandmarkBasePtr ProcessorTrackerLandmarkDummy::emplaceLandmark(FeatureBasePtr _feature_ptr)
{
- //std::cout << "ProcessorTrackerLandmarkDummy::createLandmark" << std::endl;
- return std::make_shared<LandmarkBase>("BASE", std::make_shared<StateBlock>(2), std::make_shared<StateBlock>(1));
+ //std::cout << "ProcessorTrackerLandmarkDummy::emplaceLandmark" << std::endl;
+ return LandmarkBase::emplace<LandmarkBase>(getProblem()->getMap(), "BASE", std::make_shared<StateBlock>(2), std::make_shared<StateBlock>(1));
}
-FactorBasePtr ProcessorTrackerLandmarkDummy::createFactor(FeatureBasePtr _feature_ptr, LandmarkBasePtr _landmark_ptr)
+FactorBasePtr ProcessorTrackerLandmarkDummy::emplaceFactor(FeatureBasePtr _feature_ptr, LandmarkBasePtr _landmark_ptr)
{
- std::cout << "\tProcessorTrackerLandmarkDummy::createFactor" << std::endl;
+ std::cout << "\tProcessorTrackerLandmarkDummy::emplaceFactor" << std::endl;
std::cout << "\t\tfeature " << _feature_ptr->id() << std::endl;
std::cout << "\t\tlandmark "<< _landmark_ptr->id() << std::endl;
- return std::make_shared<FactorLandmarkDummy>(_feature_ptr, _landmark_ptr, shared_from_this());
+ return FactorBase::emplace<FactorLandmarkDummy>(_feature_ptr, _feature_ptr, _landmark_ptr, shared_from_this());
}
} //namespace wolf
diff --git a/src/processor/track_matrix.cpp b/src/processor/track_matrix.cpp
index 394fa88964d46e1a0d0e1632a058a124cb4ecfdc..b6c9cd176ffe3fd7468d60710c434ff29fe8e1db 100644
--- a/src/processor/track_matrix.cpp
+++ b/src/processor/track_matrix.cpp
@@ -22,7 +22,7 @@ TrackMatrix::~TrackMatrix()
//
}
-Track TrackMatrix::track(size_t _track_id)
+Track TrackMatrix::track(const SizeStd& _track_id)
{
if (tracks_.count(_track_id) > 0)
return tracks_.at(_track_id);
@@ -38,24 +38,28 @@ Snapshot TrackMatrix::snapshot(CaptureBasePtr _capture)
return Snapshot();
}
-void TrackMatrix::newTrack(CaptureBasePtr _cap, FeatureBasePtr _ftr)
+void TrackMatrix::newTrack(FeatureBasePtr _ftr)
{
track_id_count_ ++;
- add(track_id_count_, _cap, _ftr);
+ add(track_id_count_, _ftr);
}
-void TrackMatrix::add(size_t _track_id, CaptureBasePtr _cap, FeatureBasePtr _ftr)
+void TrackMatrix::add(const SizeStd& _track_id, const FeatureBasePtr& _ftr)
{
- assert( (_track_id > 0) && (_track_id <= track_id_count_) && "Provided track ID does not exist. Use newTrack() instead.");
+ assert(( tracks_.count(_track_id) != 0 || _track_id == track_id_count_) && "Provided track ID does not exist. Use newTrack() instead.");
+ assert( _ftr->getCapture() != nullptr && "adding a feature not linked to any capture");
_ftr->setTrackId(_track_id);
- if (_cap != _ftr->getCapture())
- _ftr->setCapture(_cap);
- tracks_[_track_id].emplace(_cap->getTimeStamp(), _ftr);
- snapshots_[_cap].emplace(_track_id, _ftr); // will create new snapshot if _cap_id is not present
+ tracks_[_track_id].emplace(_ftr->getCapture()->getTimeStamp(), _ftr);
+ snapshots_[_ftr->getCapture()].emplace(_track_id, _ftr); // will create new snapshot if _cap_id is not present
}
-void TrackMatrix::remove(size_t _track_id)
+void TrackMatrix::add(const FeatureBasePtr& _ftr_existing,const FeatureBasePtr& _ftr_new)
+{
+ add(_ftr_existing->trackId(), _ftr_new);
+}
+
+void TrackMatrix::remove(const SizeStd& _track_id)
{
// Remove track features from all Snapshots
if (tracks_.count(_track_id))
@@ -116,12 +120,12 @@ size_t TrackMatrix::numTracks()
return tracks_.size();
}
-size_t TrackMatrix::trackSize(size_t _track_id)
+size_t TrackMatrix::trackSize(const SizeStd& _track_id)
{
return track(_track_id).size();
}
-FeatureBasePtr TrackMatrix::firstFeature(size_t _track_id)
+FeatureBasePtr TrackMatrix::firstFeature(const SizeStd& _track_id)
{
if (tracks_.count(_track_id) > 0)
return tracks_.at(_track_id).begin()->second;
@@ -129,7 +133,7 @@ FeatureBasePtr TrackMatrix::firstFeature(size_t _track_id)
return nullptr;
}
-FeatureBasePtr TrackMatrix::lastFeature(size_t _track_id)
+FeatureBasePtr TrackMatrix::lastFeature(const SizeStd& _track_id)
{
if (tracks_.count(_track_id) > 0)
return tracks_.at(_track_id).rbegin()->second;
@@ -137,7 +141,7 @@ FeatureBasePtr TrackMatrix::lastFeature(size_t _track_id)
return nullptr;
}
-vector<FeatureBasePtr> TrackMatrix::trackAsVector(size_t _track_id)
+vector<FeatureBasePtr> TrackMatrix::trackAsVector(const SizeStd& _track_id)
{
vector<FeatureBasePtr> vec;
if (tracks_.count(_track_id))
@@ -182,7 +186,7 @@ TrackMatches TrackMatrix::matches(CaptureBasePtr _cap_1, CaptureBasePtr _cap_2)
return pairs;
}
-FeatureBasePtr TrackMatrix::feature(size_t _track_id, CaptureBasePtr _cap)
+FeatureBasePtr TrackMatrix::feature(const SizeStd& _track_id, CaptureBasePtr _cap)
{
if (snapshot(_cap).count(_track_id))
return snapshot(_cap).at(_track_id);
@@ -190,7 +194,7 @@ FeatureBasePtr TrackMatrix::feature(size_t _track_id, CaptureBasePtr _cap)
return nullptr;
}
-CaptureBasePtr TrackMatrix::firstCapture(size_t _track_id)
+CaptureBasePtr TrackMatrix::firstCapture(const SizeStd& _track_id)
{
return firstFeature(_track_id)->getCapture();
}
diff --git a/src/sensor/sensor_base.cpp b/src/sensor/sensor_base.cpp
index 3ad6e39bcd7a6b44013b48c4167e5bada37e1216..1be0920fb059789e99b16f3a84d11b3a33b49699 100644
--- a/src/sensor/sensor_base.cpp
+++ b/src/sensor/sensor_base.cpp
@@ -75,7 +75,7 @@ void SensorBase::removeStateBlocks()
{
if (getProblem() != nullptr && !extrinsic_dynamic_)
{
- getProblem()->removeStateBlock(sbp);
+ getProblem()->notifyStateBlock(sbp,REMOVE);
}
setStateBlockPtrStatic(i, nullptr);
}
@@ -171,14 +171,12 @@ void SensorBase::addPriorParameter(const unsigned int _i, const Eigen::VectorXs&
params_prior_map_[_i]->remove();
// create feature
- FeatureBasePtr ftr_prior = std::make_shared<FeatureBase>("ABSOLUTE",_x,_cov);
+ FeatureBasePtr ftr_prior = FeatureBase::emplace<FeatureBase>(nullptr, "ABSOLUTE",_x,_cov);//std::make_shared<FeatureBase>("ABSOLUTE",_x,_cov);
// set feature problem
ftr_prior->setProblem(getProblem());
// create & add factor absolute
- // ftr_prior->addFactor(std::make_shared<FactorQuaternionAbsolute>(_sb));
- // ftr_prior->addFactor(std::make_shared<FactorBlockAbsolute>(_sb, _start_idx, _size));
if (is_quaternion)
FactorBase::emplace<FactorQuaternionAbsolute>(ftr_prior, _sb);
else
@@ -198,13 +196,13 @@ void SensorBase::registerNewStateBlocks()
{
auto sbp = getStateBlockPtrStatic(i);
if (sbp != nullptr)
- getProblem()->addStateBlock(sbp);
+ getProblem()->notifyStateBlock(sbp,ADD);
}
if (i >= 2 && !isIntrinsicDynamic())
{
auto sbp = getStateBlockPtrStatic(i);
if (sbp != nullptr)
- getProblem()->addStateBlock(sbp);
+ getProblem()->notifyStateBlock(sbp,ADD);
}
}
}
@@ -339,6 +337,11 @@ ProcessorBasePtr SensorBase::addProcessor(ProcessorBasePtr _proc_ptr)
return _proc_ptr;
}
+void SensorBase::removeProcessor(ProcessorBasePtr _proc_ptr)
+{
+ processor_list_.remove(_proc_ptr);
+}
+
StateBlockPtr SensorBase::getStateBlock(unsigned int _i)
{
CaptureBasePtr cap;
@@ -406,6 +409,7 @@ void SensorBase::setProblem(ProblemPtr _problem)
void SensorBase::link(HardwareBasePtr _hw_ptr)
{
+ assert(this->getHardware() == nullptr && "linking an already linked sensor");
if(_hw_ptr)
{
this->setHardware(_hw_ptr);
diff --git a/src/solver/solver_manager.cpp b/src/solver/solver_manager.cpp
index e03be2d6ce6c5a7dac729efeef040a4d312ae11e..e7fcb6f56e2934b5c47c13cd0696013e476829bd 100644
--- a/src/solver/solver_manager.cpp
+++ b/src/solver/solver_manager.cpp
@@ -17,7 +17,7 @@ void SolverManager::update()
auto fac_notification_map = wolf_problem_->consumeFactorNotificationMap();
auto sb_notification_map = wolf_problem_->consumeStateBlockNotificationMap();
- // REMOVE CONSTRAINTS
+ // REMOVE FACTORS
for (auto fac_notification_it = fac_notification_map.begin();
fac_notification_it != fac_notification_map.end();
/* nothing, next is handled within the for */)
@@ -70,7 +70,7 @@ void SolverManager::update()
sb_notification_map.erase(sb_notification_map.begin());
}
- // ADD CONSTRAINTS
+ // ADD FACTORS
while (!fac_notification_map.empty())
{
assert(fac_notification_map.begin()->second == ADD && "unexpected factor notification value after all REMOVE have been processed, this should be ADD");
diff --git a/src/trajectory/trajectory_base.cpp b/src/trajectory/trajectory_base.cpp
index a2395ffae0c1512ca4157fc335bd02ed62855d6a..34921e3f468a0e4960f4b305978eb33ba01109f4 100644
--- a/src/trajectory/trajectory_base.cpp
+++ b/src/trajectory/trajectory_base.cpp
@@ -34,6 +34,16 @@ FrameBasePtr TrajectoryBase::addFrame(FrameBasePtr _frame_ptr)
return _frame_ptr;
}
+void TrajectoryBase::removeFrame(FrameBasePtr _frame_ptr)
+{
+ // add to list
+ frame_list_.remove(_frame_ptr);
+
+ // update last_estimated and last_key
+ if (_frame_ptr->isKeyOrAux())
+ updateLastFrames();
+}
+
void TrajectoryBase::getFactorList(FactorBasePtrList & _fac_list)
{
for(auto fr_ptr : getFrameList())
diff --git a/test/gtest_capture_base.cpp b/test/gtest_capture_base.cpp
index 633669c7b4a5fea47832bf22ab009cb3671cad7a..15be1ff455614ce95b116e865b1666a1a728d24b 100644
--- a/test/gtest_capture_base.cpp
+++ b/test/gtest_capture_base.cpp
@@ -82,28 +82,6 @@ TEST(CaptureBase, addFeature)
ASSERT_EQ(C->getFeatureList().front(), f);
}
-TEST(CaptureBase, addFeatureList)
-{
- CaptureBasePtr C(std::make_shared<CaptureBase>("DUMMY", 1.2)); // timestamp = 1.2
- // FeatureBasePtr f_first = C->addFeature(std::make_shared<FeatureBase>("DUMMY", Vector2s::Zero(), Matrix2s::Identity()));
- auto f_first = FeatureBase::emplace<FeatureBase>(C, "DUMMY", Vector2s::Zero(), Matrix2s::Identity());
- ASSERT_EQ(C->getFeatureList().size(), (unsigned int) 1);
-
- // make a list to add
- std::list<FeatureBasePtr> fl;
- for (int i = 0; i<3; i++)
- {
- fl.push_back(std::make_shared<FeatureBase>("DUMMY", Vector2s::Zero(), Matrix2s::Identity()));
- }
- FeatureBasePtr f_last = fl.back();
-
- C->addFeatureList((fl));
- ASSERT_EQ(C->getFeatureList().size(), (unsigned int) 4);
- ASSERT_EQ(fl.size(), (unsigned int) 0); // features have been moved, not copied // EDIT 02-2019: features have been copied
- ASSERT_EQ(C->getFeatureList().front(), f_first);
- ASSERT_EQ(C->getFeatureList().back(), f_last);
-}
-
TEST(CaptureBase, process)
{
SensorBasePtr S(std::make_shared<SensorBase>("DUMMY", nullptr, nullptr, nullptr, 2));
diff --git a/test/gtest_ceres_manager.cpp b/test/gtest_ceres_manager.cpp
index 28cd36ced23e26ab9921905c7227640e9bbf1134..1f508dbe5cb760e6c5360d4cab16d5f4eb378ac7 100644
--- a/test/gtest_ceres_manager.cpp
+++ b/test/gtest_ceres_manager.cpp
@@ -101,7 +101,7 @@ TEST(CeresManager, AddStateBlock)
StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
// add stateblock
- P->addStateBlock(sb_ptr);
+ P->notifyStateBlock(sb_ptr,ADD);
// update solver
ceres_manager_ptr->update();
@@ -124,13 +124,13 @@ TEST(CeresManager, DoubleAddStateBlock)
StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
// add stateblock
- P->addStateBlock(sb_ptr);
+ P->notifyStateBlock(sb_ptr,ADD);
// update solver
ceres_manager_ptr->update();
// add stateblock again
- P->addStateBlock(sb_ptr);
+ P->notifyStateBlock(sb_ptr,ADD);
// update solver
ceres_manager_ptr->update();
@@ -153,7 +153,7 @@ TEST(CeresManager, UpdateStateBlock)
StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
// add stateblock
- P->addStateBlock(sb_ptr);
+ P->notifyStateBlock(sb_ptr,ADD);
// update solver
ceres_manager_ptr->update();
@@ -184,7 +184,7 @@ TEST(CeresManager, AddUpdateStateBlock)
StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
// add stateblock
- P->addStateBlock(sb_ptr);
+ P->notifyStateBlock(sb_ptr,ADD);
// Fix state block
sb_ptr->fix();
@@ -211,7 +211,7 @@ TEST(CeresManager, RemoveStateBlock)
StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
// add stateblock
- P->addStateBlock(sb_ptr);
+ P->notifyStateBlock(sb_ptr,ADD);
// update solver
ceres_manager_ptr->update();
@@ -220,7 +220,7 @@ TEST(CeresManager, RemoveStateBlock)
ASSERT_TRUE(ceres_manager_ptr->isStateBlockRegisteredCeresManager(sb_ptr));
// remove state_block
- P->removeStateBlock(sb_ptr);
+ P->notifyStateBlock(sb_ptr,REMOVE);
// update solver
ceres_manager_ptr->update();
@@ -243,10 +243,10 @@ TEST(CeresManager, AddRemoveStateBlock)
StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
// add stateblock
- P->addStateBlock(sb_ptr);
+ P->notifyStateBlock(sb_ptr,ADD);
// remove state_block
- P->removeStateBlock(sb_ptr);
+ P->notifyStateBlock(sb_ptr,REMOVE);
// update solver
ceres_manager_ptr->update();
@@ -269,13 +269,13 @@ TEST(CeresManager, RemoveUpdateStateBlock)
StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
// add stateblock
- P->addStateBlock(sb_ptr);
+ P->notifyStateBlock(sb_ptr,ADD);
// update solver
ceres_manager_ptr->update();
// remove state_block
- P->removeStateBlock(sb_ptr);
+ P->notifyStateBlock(sb_ptr,REMOVE);
// update solver
ceres_manager_ptr->update();
@@ -294,16 +294,16 @@ TEST(CeresManager, DoubleRemoveStateBlock)
StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
// add stateblock
- P->addStateBlock(sb_ptr);
+ P->notifyStateBlock(sb_ptr,ADD);
// remove state_block
- P->removeStateBlock(sb_ptr);
+ P->notifyStateBlock(sb_ptr,REMOVE);
// update solver
ceres_manager_ptr->update();
// remove state_block
- P->removeStateBlock(sb_ptr);
+ P->notifyStateBlock(sb_ptr,REMOVE);
// update solver manager
ceres_manager_ptr->update();
@@ -346,7 +346,7 @@ TEST(CeresManager, DoubleAddFactor)
FactorPose2DPtr c = std::static_pointer_cast<FactorPose2D>(FactorBase::emplace<FactorPose2D>(f,f));
// add factor again
- P->addFactor(c);
+ P->notifyFactor(c,ADD);
// update solver
ceres_manager_ptr->update();
@@ -374,7 +374,7 @@ TEST(CeresManager, RemoveFactor)
ceres_manager_ptr->update();
// remove factor
- P->removeFactor(c);
+ P->notifyFactor(c,REMOVE);
// update solver
ceres_manager_ptr->update();
@@ -399,7 +399,7 @@ TEST(CeresManager, AddRemoveFactor)
FactorPose2DPtr c = std::static_pointer_cast<FactorPose2D>(FactorBase::emplace<FactorPose2D>(f,f));
// remove factor
- P->removeFactor(c);
+ P->notifyFactor(c,REMOVE);
ASSERT_TRUE(P->getFactorNotificationMapSize() == 0); // add+remove = empty
@@ -429,13 +429,13 @@ TEST(CeresManager, DoubleRemoveFactor)
ceres_manager_ptr->update();
// remove factor
- P->removeFactor(c);
+ P->notifyFactor(c,REMOVE);
// update solver
ceres_manager_ptr->update();
// remove factor
- P->removeFactor(c);
+ P->notifyFactor(c,REMOVE);
ASSERT_DEATH({
// update solver
@@ -463,7 +463,7 @@ TEST(CeresManager, AddStateBlockLocalParam)
sb_ptr->setLocalParametrization(local_param_ptr);
// add stateblock
- P->addStateBlock(sb_ptr);
+ P->notifyStateBlock(sb_ptr,ADD);
// update solver
ceres_manager_ptr->update();
@@ -490,7 +490,7 @@ TEST(CeresManager, RemoveLocalParam)
sb_ptr->setLocalParametrization(local_param_ptr);
// add stateblock
- P->addStateBlock(sb_ptr);
+ P->notifyStateBlock(sb_ptr,ADD);
// update solver
ceres_manager_ptr->update();
@@ -518,7 +518,7 @@ TEST(CeresManager, AddLocalParam)
StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
// add stateblock
- P->addStateBlock(sb_ptr);
+ P->notifyStateBlock(sb_ptr,ADD);
// update solver
ceres_manager_ptr->update();
diff --git a/test/gtest_frame_base.cpp b/test/gtest_frame_base.cpp
index 4f189f559920003b3a0b8a033a56cde340d997ef..f02418eb99ce6d49eeaaee923755314c85c8fcc4 100644
--- a/test/gtest_frame_base.cpp
+++ b/test/gtest_frame_base.cpp
@@ -133,10 +133,6 @@ TEST(FrameBase, LinksToTree)
F1->setKey();
ASSERT_TRUE(F1->isKey());
ASSERT_TRUE(F1->isKeyOrAux());
-
- // Unlink
- F1->unlinkCapture(C);
- ASSERT_TRUE(F1->getCaptureList().empty());
}
#include "core/state_block/state_quaternion.h"
diff --git a/test/gtest_problem.cpp b/test/gtest_problem.cpp
index d36a14461c139e60be272f5f54c883e77ff35999..7570f6c1870274ac8315b57468fff1da9e7b7be5 100644
--- a/test/gtest_problem.cpp
+++ b/test/gtest_problem.cpp
@@ -96,12 +96,7 @@ TEST(Problem, Processor)
// Sm->addProcessor(Pm);
auto Pm = std::static_pointer_cast<ProcessorMotion>(ProcessorBase::emplace<ProcessorOdom3D>(Sm, std::make_shared<ProcessorParamsOdom3D>()));
- // check motion processor IS NOT set by addSensor <-- using InstallProcessor it should, see test Installers
- ASSERT_FALSE(P->getProcessorMotion());
-
- // set processor motion
- P->setProcessorMotion(Pm);
- // re-check motion processor IS set by addSensor
+ // check motion processor IS NOT by emplace
ASSERT_EQ(P->getProcessorMotion(), Pm);
}
diff --git a/test/gtest_processor_loopclosure.cpp b/test/gtest_processor_loopclosure.cpp
index f2c99aac536fb2265ad100f00f41def2aa2abeae..a7fc6818d0b739682eb546923bab9cb40e9bef27 100644
--- a/test/gtest_processor_loopclosure.cpp
+++ b/test/gtest_processor_loopclosure.cpp
@@ -70,8 +70,7 @@ TEST(ProcessorLoopClosure, installProcessor)
std::make_shared<StateBlock>(Eigen::VectorXs::Zero(1)),
std::make_shared<StateBlock>(Eigen::VectorXs::Zero(2)), 2);
ProcessorParamsLoopClosurePtr params = std::make_shared<ProcessorParamsLoopClosure>();
- ProcessorLoopClosureDummyPtr proc_lc = std::make_shared<ProcessorLoopClosureDummy>(params, factor_created);
- proc_lc->link(sens_lc);
+ auto proc_lc = ProcessorBase::emplace<ProcessorLoopClosureDummy>(sens_lc, params, factor_created);
std::cout << "sensor & processor created and added to wolf problem" << std::endl;
// initialize
@@ -82,7 +81,7 @@ TEST(ProcessorLoopClosure, installProcessor)
// new KF
t += dt;
- FrameBasePtr kf = FrameBase::create_PO_2D (KEY, t, x); //KF2
+ FrameBasePtr kf = problem->emplaceFrame(KEY, x, t); //KF2
proc_lc->keyFrameCallback(kf, dt/2);
// new capture
diff --git a/test/gtest_processor_tracker_feature_dummy.cpp b/test/gtest_processor_tracker_feature_dummy.cpp
index 02e24d90b19dc6a9537ccfbb3a7434887ce6c48e..ea169fcf994feeb624f88e718e94265aa4fbdb10 100644
--- a/test/gtest_processor_tracker_feature_dummy.cpp
+++ b/test/gtest_processor_tracker_feature_dummy.cpp
@@ -25,13 +25,16 @@ class ProcessorTrackerFeatureDummyDummy : public ProcessorTrackerFeatureDummy
unsigned int callProcessNew(const int& _max_new_features){ return this->processNew(_max_new_features); }
- unsigned int callDetectNewFeatures(const int& _max_features, FeatureBasePtrList& _features_incoming_out){ return this->detectNewFeatures(_max_features, _features_incoming_out); }
+ unsigned int callDetectNewFeatures(const int& _max_features,
+ const CaptureBasePtr& _capture,
+ FeatureBasePtrList& _features_out){ return this->detectNewFeatures(_max_features, _capture, _features_out); }
- unsigned int callTrackFeatures(const FeatureBasePtrList& _features_last_in,
- FeatureBasePtrList& _features_incoming_out,
- FeatureMatchMap& _feature_correspondences){ return this->trackFeatures(_features_last_in, _features_incoming_out, _feature_correspondences); }
+ unsigned int callTrackFeatures(const FeatureBasePtrList& _features_in,
+ const CaptureBasePtr& _capture,
+ FeatureBasePtrList& _features_out,
+ FeatureMatchMap& _feature_correspondences){ return this->trackFeatures(_features_in, _capture, _features_out, _feature_correspondences); }
- FactorBasePtr callCreateFactor(FeatureBasePtr _feature_ptr, FeatureBasePtr _feature_other_ptr){ return this->createFactor(_feature_ptr, _feature_other_ptr); }
+ FactorBasePtr callEmplaceFactor(FeatureBasePtr _feature_ptr, FeatureBasePtr _feature_other_ptr){ return this->emplaceFactor(_feature_ptr, _feature_other_ptr); }
void callEstablishFactors(){ this->establishFactors();}
@@ -48,6 +51,20 @@ class ProcessorTrackerFeatureDummyDummy : public ProcessorTrackerFeatureDummy
};
};
+bool isFeatureLinked(FeatureBasePtr ftr, CaptureBasePtr cap)
+{
+ return ftr->getCapture() == cap &&
+ std::find(cap->getFeatureList().begin(), cap->getFeatureList().end(), ftr) != cap->getFeatureList().end();
+
+}
+
+bool isFactorLinked(FactorBasePtr fac, FeatureBasePtr ftr)
+{
+ return fac->getFeature() == ftr &&
+ std::find(ftr->getFactorList().begin(), ftr->getFactorList().end(), fac) != ftr->getFactorList().end();
+
+}
+
// Use the following in case you want to initialize tests with predefines variables or methods.
class ProcessorTrackerFeatureDummyTest : public testing::Test
{
@@ -57,6 +74,8 @@ class ProcessorTrackerFeatureDummyTest : public testing::Test
ProcessorParamsTrackerFeatureDummyPtr params;
ProcessorTrackerFeatureDummyDummyPtr processor;
+ virtual ~ProcessorTrackerFeatureDummyTest(){}
+
virtual void SetUp()
{
// Wolf problem
@@ -92,9 +111,14 @@ TEST_F(ProcessorTrackerFeatureDummyTest, callDetectNewFeatures)
FeatureBasePtrList feat_list;
// demo callDetectNewFeatures
- unsigned int n_feat = processor->callDetectNewFeatures(params->max_new_features, feat_list);
+ unsigned int n_feat = processor->callDetectNewFeatures(params->max_new_features, last_cap, feat_list);
ASSERT_EQ(n_feat, feat_list.size());
ASSERT_EQ(n_feat, params->max_new_features);
+
+ // check the features are emplaced
+ ASSERT_EQ(n_feat, last_cap->getFeatureList().size());
+ for (auto feat : feat_list)
+ ASSERT_TRUE(isFeatureLinked(feat, last_cap));
}
TEST_F(ProcessorTrackerFeatureDummyTest, trackFeatures)
@@ -106,7 +130,7 @@ TEST_F(ProcessorTrackerFeatureDummyTest, trackFeatures)
FeatureBasePtrList feat_list;
//fill feat_last list
- processor->callDetectNewFeatures(params->max_new_features, feat_list);
+ processor->callDetectNewFeatures(params->max_new_features, last_cap, feat_list);
// Put a capture on incoming_ptr_
CaptureBasePtr inc_cap = std::make_shared<CaptureVoid>(1, sensor);
@@ -115,9 +139,14 @@ TEST_F(ProcessorTrackerFeatureDummyTest, trackFeatures)
//test trackFeatures
FeatureBasePtrList feat_list_out;
FeatureMatchMap feat_correspondance;
- processor->callTrackFeatures(feat_list, feat_list_out, feat_correspondance);
+ processor->callTrackFeatures(feat_list, inc_cap, feat_list_out, feat_correspondance);
ASSERT_EQ(feat_list_out.size(), feat_correspondance.size());
ASSERT_EQ(feat_list.size(), feat_list_out.size()+1); // one of each 10 tracks is lost
+
+ // check the features are emplaced
+ ASSERT_EQ(inc_cap->getFeatureList().size(), feat_list_out.size());
+ for (auto feat : feat_list_out)
+ ASSERT_TRUE(isFeatureLinked(feat, inc_cap));
}
TEST_F(ProcessorTrackerFeatureDummyTest, processNew)
@@ -136,6 +165,12 @@ TEST_F(ProcessorTrackerFeatureDummyTest, processNew)
ASSERT_EQ(processor->getLast()->getFeatureList().size(), 10); // detected 10 features
ASSERT_EQ(processor->getIncoming()->getFeatureList().size(), 9); // 1 track lost
ASSERT_EQ(processor->getMatchesLastFromIncoming().size(), 9); // 1 track lost
+
+ // check the features are emplaced
+ for (auto feat : last_cap->getFeatureList())
+ ASSERT_TRUE(isFeatureLinked(feat, last_cap));
+ for (auto feat : inc_cap->getFeatureList())
+ ASSERT_TRUE(isFeatureLinked(feat, inc_cap));
}
TEST_F(ProcessorTrackerFeatureDummyTest, processKnown)
@@ -166,9 +201,17 @@ TEST_F(ProcessorTrackerFeatureDummyTest, processKnown)
ASSERT_EQ(n_tracked_feat, 13); // 1 track lost
ASSERT_EQ(processor->getIncoming()->getFeatureList().size(), 13); // 1 track lost
ASSERT_EQ(processor->getMatchesLastFromIncoming().size(), 13); // 1 track lost
+
+ // check the features are emplaced
+ for (auto feat : processor->getOrigin()->getFeatureList())
+ ASSERT_TRUE(isFeatureLinked(feat, processor->getOrigin()));
+ for (auto feat : processor->getLast()->getFeatureList())
+ ASSERT_TRUE(isFeatureLinked(feat, processor->getLast()));
+ for (auto feat : processor->getIncoming()->getFeatureList())
+ ASSERT_TRUE(isFeatureLinked(feat, processor->getIncoming()));
}
-TEST_F(ProcessorTrackerFeatureDummyTest, createFactor)
+TEST_F(ProcessorTrackerFeatureDummyTest, emplaceFactor)
{
FeatureBasePtr ftr(std::make_shared<FeatureBase>("DUMMY FEATURE",
Eigen::Vector1s::Ones(),
@@ -177,13 +220,15 @@ TEST_F(ProcessorTrackerFeatureDummyTest, createFactor)
Eigen::Vector1s::Ones(),
Eigen::MatrixXs::Ones(1, 1)));
- FactorBasePtr fac = processor->callCreateFactor(ftr, ftr_other);
- fac->link(ftr);
- ASSERT_EQ(fac->getFeature(),ftr);
+ FactorBasePtr fac = processor->callEmplaceFactor(ftr, ftr_other);
+
+ // check factor correctly emplaced
+ ASSERT_TRUE(isFactorLinked(fac, ftr));
ASSERT_EQ(fac->getFrameOther(),nullptr);
ASSERT_EQ(fac->getCaptureOther(),nullptr);
ASSERT_EQ(fac->getFeatureOther(),ftr_other);
ASSERT_EQ(fac->getLandmarkOther(),nullptr);
+ ASSERT_TRUE(std::find(ftr_other->getConstrainedByList().begin(),ftr_other->getConstrainedByList().end(), fac) != ftr_other->getConstrainedByList().end());
}
TEST_F(ProcessorTrackerFeatureDummyTest, establishFactors)
@@ -244,6 +289,7 @@ TEST_F(ProcessorTrackerFeatureDummyTest, process)
ASSERT_TRUE(problem->getTrajectory()->getLastKeyFrame() != nullptr);
ASSERT_EQ(problem->getTrajectory()->getLastKeyFrame()->id(), cap1->getFrame()->id());
+ ASSERT_TRUE(problem->check(0));
//2ND TIME
WOLF_DEBUG("Second time...");
@@ -252,6 +298,7 @@ TEST_F(ProcessorTrackerFeatureDummyTest, process)
ASSERT_EQ(processor->getOrigin()->getFeatureList().size(), params->max_new_features);
ASSERT_EQ(processor->getLast()->getFeatureList().size(), params->max_new_features-1);
+ ASSERT_TRUE(problem->check(0));
//3RD TIME
WOLF_DEBUG("Third time...");
@@ -259,6 +306,7 @@ TEST_F(ProcessorTrackerFeatureDummyTest, process)
cap3->process();
ASSERT_EQ(processor->getLast()->getFeatureList().size(), params->max_new_features-2);
+ ASSERT_TRUE(problem->check(0));
//4TH TIME
WOLF_DEBUG("Forth time...");
@@ -266,6 +314,7 @@ TEST_F(ProcessorTrackerFeatureDummyTest, process)
cap4->process();
ASSERT_EQ(processor->getLast()->getFeatureList().size(), params->max_new_features-3);
+ ASSERT_TRUE(problem->check(0));
//5TH TIME -> KF in cap4 (tracked features < 7 (params->min_features_for_keyframe))
WOLF_DEBUG("Fifth time...");
@@ -274,11 +323,13 @@ TEST_F(ProcessorTrackerFeatureDummyTest, process)
ASSERT_TRUE(cap4->getFrame()->isKey());
ASSERT_EQ(problem->getTrajectory()->getLastKeyFrame()->id(), cap4->getFrame()->id());
+ ASSERT_TRUE(problem->check(0));
// check factors
WOLF_DEBUG("checking factors...");
TrackMatrix track_matrix = processor->getTrackMatrix();
ASSERT_EQ(cap4->getFeatureList().size(), params->min_features_for_keyframe + params->max_new_features);
+ ASSERT_TRUE(problem->check(0));
unsigned int n_factors0 = 0;
unsigned int n_factors4 = 0;
for (auto feat : cap4->getFeatureList())
diff --git a/test/gtest_processor_tracker_landmark_dummy.cpp b/test/gtest_processor_tracker_landmark_dummy.cpp
index a5f3f5945b13320516d713ab30f535383f273d55..8ed266881c29e43a29836eac706ec0dd9c96f19e 100644
--- a/test/gtest_processor_tracker_landmark_dummy.cpp
+++ b/test/gtest_processor_tracker_landmark_dummy.cpp
@@ -26,19 +26,23 @@ class ProcessorTrackerLandmarkDummyDummy : public ProcessorTrackerLandmarkDummy
unsigned int callProcessNew(const int& _max_new_features){ return this->processNew(_max_new_features); }
unsigned int callDetectNewFeatures(const int& _max_features,
- FeatureBasePtrList& _features_incoming_out){ return this->detectNewFeatures(_max_features,
- _features_incoming_out); }
+ const CaptureBasePtr& _capture,
+ FeatureBasePtrList& _features_out){ return this->detectNewFeatures(_max_features,
+ _capture,
+ _features_out); }
unsigned int callFindLandmarks(const LandmarkBasePtrList& _landmarks_in,
- FeatureBasePtrList& _features_incoming_out,
+ const CaptureBasePtr& _capture,
+ FeatureBasePtrList& _features_out,
LandmarkMatchMap& _feature_landmark_correspondences){ return this->findLandmarks(_landmarks_in,
- _features_incoming_out,
+ _capture,
+ _features_out,
_feature_landmark_correspondences); }
- LandmarkBasePtr callCreateLandmark(FeatureBasePtr _feature_ptr){ return this->createLandmark(_feature_ptr); }
- void callCreateNewLandmarks(){ this->createNewLandmarks(); }
- FactorBasePtr callCreateFactor(FeatureBasePtr _feature_ptr,
- LandmarkBasePtr _landmark_ptr){ return this->createFactor(_feature_ptr,
+ LandmarkBasePtr callEmplaceLandmark(FeatureBasePtr _feature_ptr){ return this->emplaceLandmark(_feature_ptr); }
+ void callEmplaceNewLandmarks(){ this->emplaceNewLandmarks(); }
+ FactorBasePtr callEmplaceFactor(FeatureBasePtr _feature_ptr,
+ LandmarkBasePtr _landmark_ptr){ return this->emplaceFactor(_feature_ptr,
_landmark_ptr); }
void callEstablishFactors(){ this->establishFactors();}
@@ -58,6 +62,27 @@ class ProcessorTrackerLandmarkDummyDummy : public ProcessorTrackerLandmarkDummy
};
};
+bool isFeatureLinked(FeatureBasePtr ftr, CaptureBasePtr cap)
+{
+ return ftr->getCapture() == cap &&
+ std::find(cap->getFeatureList().begin(), cap->getFeatureList().end(), ftr) != cap->getFeatureList().end();
+
+}
+
+bool isFactorLinked(FactorBasePtr fac, FeatureBasePtr ftr)
+{
+ return fac->getFeature() == ftr &&
+ std::find(ftr->getFactorList().begin(), ftr->getFactorList().end(), fac) != ftr->getFactorList().end();
+
+}
+
+bool isLandmarkLinked(LandmarkBasePtr lmk, MapBasePtr map)
+{
+ return lmk->getMap() == map &&
+ std::find(map->getLandmarkList().begin(), map->getLandmarkList().end(), lmk) != map->getLandmarkList().end();
+
+}
+
// Use the following in case you want to initialize tests with predefines variables or methods.
class ProcessorTrackerLandmarkDummyTest : public testing::Test
{
@@ -67,6 +92,8 @@ class ProcessorTrackerLandmarkDummyTest : public testing::Test
ProcessorParamsTrackerLandmarkDummyPtr params;
ProcessorTrackerLandmarkDummyDummyPtr processor;
+ virtual ~ProcessorTrackerLandmarkDummyTest(){}
+
virtual void SetUp()
{
// Wolf problem
@@ -102,12 +129,17 @@ TEST_F(ProcessorTrackerLandmarkDummyTest, detectNewFeatures)
FeatureBasePtrList feat_list;
// demo callDetectNewFeatures
- unsigned int n_feat = processor->callDetectNewFeatures(params->max_new_features, feat_list);
+ unsigned int n_feat = processor->callDetectNewFeatures(params->max_new_features, last_cap, feat_list);
ASSERT_EQ(n_feat, feat_list.size()); // detected 10 features
ASSERT_EQ(n_feat, params->max_new_features); // detected 10 features
+
+ // check the features are emplaced
+ ASSERT_EQ(n_feat, last_cap->getFeatureList().size());
+ for (auto feat : feat_list)
+ ASSERT_TRUE(isFeatureLinked(feat, last_cap));
}
-TEST_F(ProcessorTrackerLandmarkDummyTest, createLandmark)
+TEST_F(ProcessorTrackerLandmarkDummyTest, emplaceLandmark)
{
// Put a capture on last_ptr_
CaptureBasePtr last_cap = std::make_shared<CaptureVoid>(0, sensor);
@@ -116,19 +148,18 @@ TEST_F(ProcessorTrackerLandmarkDummyTest, createLandmark)
FeatureBasePtrList feat_list;
// demo callDetectNewFeatures
- unsigned int n_feat = processor->callDetectNewFeatures(params->max_new_features, feat_list);
- ASSERT_EQ(n_feat, feat_list.size()); // detected 10 features
- ASSERT_EQ(n_feat, params->max_new_features); // detected 10 features
+ unsigned int n_feat = processor->callDetectNewFeatures(params->max_new_features, last_cap, feat_list);
for (auto ftr : feat_list)
{
- auto lmk = processor->callCreateLandmark(ftr);
- lmk->link(problem->getMap());
+ auto lmk = processor->callEmplaceLandmark(ftr);
+ // check that it is correctly emplaced
+ ASSERT_TRUE(isLandmarkLinked(lmk,problem->getMap()));
}
- ASSERT_EQ(problem->getMap()->getLandmarkList().size(),n_feat); // created 10 landmarks
+ ASSERT_EQ(problem->getMap()->getLandmarkList().size(),n_feat); // emplaced 10 landmarks
}
-TEST_F(ProcessorTrackerLandmarkDummyTest, createNewLandmarks)
+TEST_F(ProcessorTrackerLandmarkDummyTest, emplaceNewLandmarks)
{
// Put a capture on last_ptr_
CaptureBasePtr last_cap = std::make_shared<CaptureVoid>(0, sensor);
@@ -137,14 +168,16 @@ TEST_F(ProcessorTrackerLandmarkDummyTest, createNewLandmarks)
FeatureBasePtrList feat_list;
// test detectNewFeatures
- unsigned int n_feat = processor->callDetectNewFeatures(params->max_new_features, feat_list);
- ASSERT_EQ(n_feat, feat_list.size()); // detected 10 features
- ASSERT_EQ(n_feat, params->max_new_features); // detected 10 features
+ unsigned int n_feat = processor->callDetectNewFeatures(params->max_new_features, last_cap, feat_list);
// test createNewLandmarks
processor->setNewFeaturesLast(feat_list);
- processor->callCreateNewLandmarks();
- ASSERT_EQ(processor->getNewLandmarks().size(),n_feat); // created 10 landmarks
+ processor->callEmplaceNewLandmarks();
+ ASSERT_EQ(processor->getNewLandmarks().size(),n_feat); // emplaced 10 landmarks
+
+ // check that it is correctly emplaced
+ for (auto lmk : processor->getNewLandmarks())
+ ASSERT_TRUE(isLandmarkLinked(lmk,problem->getMap()));
}
TEST_F(ProcessorTrackerLandmarkDummyTest, findLandmarks)
@@ -156,22 +189,26 @@ TEST_F(ProcessorTrackerLandmarkDummyTest, findLandmarks)
FeatureBasePtrList feat_list;
// test detectNewFeatures
- unsigned int n_feat = processor->callDetectNewFeatures(params->max_new_features, feat_list);
- ASSERT_EQ(n_feat, feat_list.size()); // detected 10 features
- ASSERT_EQ(n_feat, params->max_new_features); // detected 10 features
+ processor->callDetectNewFeatures(params->max_new_features, last_cap, feat_list);
// test createNewLandmarks
processor->setNewFeaturesLast(feat_list);
- processor->callCreateNewLandmarks();
+ processor->callEmplaceNewLandmarks();
LandmarkBasePtrList new_landmarks = processor->getNewLandmarks();
- ASSERT_EQ(new_landmarks.size(),n_feat); // created 10 landmarks
//test findLandmarks
LandmarkMatchMap feature_landmark_correspondences;
FeatureBasePtrList feat_found;
- processor->callFindLandmarks(new_landmarks, feat_found, feature_landmark_correspondences);
+
+ CaptureBasePtr inc_cap = std::make_shared<CaptureVoid>(0, sensor);
+ processor->setInc(inc_cap);
+
+ processor->callFindLandmarks(new_landmarks, inc_cap, feat_found, feature_landmark_correspondences);
ASSERT_EQ(feature_landmark_correspondences.size(), feat_found.size());
ASSERT_EQ(feat_list.size(), feat_found.size()+1); // one of each 10 tracks is lost
+
+ for (auto feat : feat_found)
+ ASSERT_TRUE(isFeatureLinked(feat, inc_cap));
}
TEST_F(ProcessorTrackerLandmarkDummyTest, processNew)
@@ -184,7 +221,7 @@ TEST_F(ProcessorTrackerLandmarkDummyTest, processNew)
CaptureBasePtr inc_cap = std::make_shared<CaptureVoid>(1, sensor);
processor->setInc(inc_cap);
- auto n_new_feat = processor->callProcessNew(10); // detect 10 features
+ auto n_new_feat = processor->callProcessNew(10); // detect 10 features in last, create landmarks, find landmarks in incoming
ASSERT_EQ(n_new_feat, 10); // detected 10 features
ASSERT_EQ(processor->getLast()->getFeatureList().size(), 10); // detected 10 features
@@ -196,14 +233,15 @@ TEST_F(ProcessorTrackerLandmarkDummyTest, processNew)
TEST_F(ProcessorTrackerLandmarkDummyTest, processKnown)
{
+ // Put a capture on last_ptr_
+ CaptureBasePtr last_cap = std::make_shared<CaptureVoid>(0, sensor);
+ processor->setLast(last_cap);
+
// create 10 landmarks and link them to map
FeatureBasePtrList feat_list;
- processor->callDetectNewFeatures(params->max_new_features, feat_list);
- for (auto ftr : feat_list)
- {
- auto lmk = processor->callCreateLandmark(ftr);
- lmk->link(problem->getMap());
- }
+ processor->callDetectNewFeatures(params->max_new_features, last_cap, feat_list);
+ processor->setNewFeaturesLast(feat_list);
+ processor->callEmplaceNewLandmarks();
ASSERT_EQ(problem->getMap()->getLandmarkList().size(),10); // created 10 landmarks
// Put a capture on incoming_ptr_
@@ -217,7 +255,7 @@ TEST_F(ProcessorTrackerLandmarkDummyTest, processKnown)
ASSERT_EQ(processor->getMatchesLandmarkFromIncoming().size(), 9); // 1 of each 10 landmarks is not found
}
-TEST_F(ProcessorTrackerLandmarkDummyTest, createFactor)
+TEST_F(ProcessorTrackerLandmarkDummyTest, emplaceFactor)
{
FeatureBasePtr ftr(std::make_shared<FeatureBase>("DUMMY FEATURE",
Eigen::Vector1s::Ones(),
@@ -226,8 +264,7 @@ TEST_F(ProcessorTrackerLandmarkDummyTest, createFactor)
std::make_shared<StateBlock>(1),
std::make_shared<StateBlock>(1)));
- FactorBasePtr fac = processor->callCreateFactor(ftr, lmk);
- fac->link(ftr);
+ FactorBasePtr fac = processor->callEmplaceFactor(ftr, lmk);
ASSERT_EQ(fac->getFeature(),ftr);
ASSERT_EQ(fac->getFrameOther(),nullptr);
ASSERT_EQ(fac->getCaptureOther(),nullptr);
diff --git a/test/gtest_solver_manager.cpp b/test/gtest_solver_manager.cpp
index 815817a6eac6faabf045a438819d09ae2d2529be..de653851d9de3aa625421088b4fb076e9b586afb 100644
--- a/test/gtest_solver_manager.cpp
+++ b/test/gtest_solver_manager.cpp
@@ -126,7 +126,7 @@ TEST(SolverManager, AddStateBlock)
StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
// add stateblock
- P->addStateBlock(sb_ptr);
+ P->notifyStateBlock(sb_ptr,ADD);
// update solver
solver_manager_ptr->update();
@@ -145,13 +145,13 @@ TEST(SolverManager, DoubleAddStateBlock)
StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
// add stateblock
- P->addStateBlock(sb_ptr);
+ P->notifyStateBlock(sb_ptr,ADD);
// update solver
solver_manager_ptr->update();
// add stateblock again
- P->addStateBlock(sb_ptr);
+ P->notifyStateBlock(sb_ptr,ADD);
// update solver
solver_manager_ptr->update();
@@ -170,7 +170,7 @@ TEST(SolverManager, UpdateStateBlock)
StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
// add stateblock
- P->addStateBlock(sb_ptr);
+ P->notifyStateBlock(sb_ptr,ADD);
// check flags
ASSERT_FALSE(sb_ptr->stateUpdated());
@@ -218,7 +218,7 @@ TEST(SolverManager, AddUpdateStateBlock)
StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
// add stateblock
- P->addStateBlock(sb_ptr);
+ P->notifyStateBlock(sb_ptr,ADD);
// check flags
ASSERT_FALSE(sb_ptr->stateUpdated());
@@ -256,7 +256,7 @@ TEST(SolverManager, AddUpdateLocalParamStateBlock)
StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
// add stateblock
- P->addStateBlock(sb_ptr);
+ P->notifyStateBlock(sb_ptr,ADD);
// check flags
ASSERT_FALSE(sb_ptr->stateUpdated());
@@ -308,7 +308,7 @@ TEST(SolverManager, AddLocalParamRemoveLocalParamStateBlock)
ASSERT_TRUE(sb_ptr->localParamUpdated());
// add stateblock
- P->addStateBlock(sb_ptr);
+ P->notifyStateBlock(sb_ptr,ADD);
// update solver manager
solver_manager_ptr->update();
@@ -346,13 +346,13 @@ TEST(SolverManager, RemoveStateBlock)
StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
// add stateblock
- P->addStateBlock(sb_ptr);
+ P->notifyStateBlock(sb_ptr,ADD);
// update solver
solver_manager_ptr->update();
// remove state_block
- P->removeStateBlock(sb_ptr);
+ P->notifyStateBlock(sb_ptr,REMOVE);
// update solver
solver_manager_ptr->update();
@@ -371,10 +371,10 @@ TEST(SolverManager, AddRemoveStateBlock)
StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
// add stateblock
- P->addStateBlock(sb_ptr);
+ P->notifyStateBlock(sb_ptr,ADD);
// remove state_block
- P->removeStateBlock(sb_ptr);
+ P->notifyStateBlock(sb_ptr,REMOVE);
// update solver
solver_manager_ptr->update();
@@ -393,10 +393,10 @@ TEST(SolverManager, RemoveUpdateStateBlock)
StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
// add state_block
- P->addStateBlock(sb_ptr);
+ P->notifyStateBlock(sb_ptr,ADD);
// remove state_block
- P->removeStateBlock(sb_ptr);
+ P->notifyStateBlock(sb_ptr,REMOVE);
// update solver
solver_manager_ptr->update();
@@ -412,16 +412,16 @@ TEST(SolverManager, DoubleRemoveStateBlock)
StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
// add stateblock
- P->addStateBlock(sb_ptr);
+ P->notifyStateBlock(sb_ptr,ADD);
// remove state_block
- P->removeStateBlock(sb_ptr);
+ P->notifyStateBlock(sb_ptr,REMOVE);
// update solver
solver_manager_ptr->update();
// remove state_block
- P->removeStateBlock(sb_ptr);
+ P->notifyStateBlock(sb_ptr,REMOVE);
// update solver manager
solver_manager_ptr->update();
@@ -450,7 +450,7 @@ TEST(SolverManager, AddUpdatedStateBlock)
// == When an ADD is notified: a ADD notification should be stored ==
// add state_block
- P->addStateBlock(sb_ptr);
+ P->notifyStateBlock(sb_ptr,ADD);
Notification notif;
ASSERT_EQ(P->getStateBlockNotificationMapSize(),1);
@@ -465,35 +465,44 @@ TEST(SolverManager, AddUpdatedStateBlock)
// Fix --> FLAG
sb_ptr->unfix();
+ // Check flag has been set true
+ ASSERT_TRUE(sb_ptr->fixUpdated());
ASSERT_EQ(P->getStateBlockNotificationMapSize(),1); // No new notifications (fix and set state are flags in sb)
// == consume empties the notification map ==
solver_manager_ptr->update(); // it calls P->consumeStateBlockNotificationMap();
ASSERT_EQ(P->getStateBlockNotificationMapSize(),0);
+ // Check flags have been reset
+ ASSERT_FALSE(sb_ptr->fixUpdated());
+ ASSERT_FALSE(sb_ptr->stateUpdated());
// == When an REMOVE is notified: a REMOVE notification should be stored ==
// remove state_block
- P->removeStateBlock(sb_ptr);
+ P->notifyStateBlock(sb_ptr,REMOVE);
ASSERT_EQ(P->getStateBlockNotificationMapSize(),1);
ASSERT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
ASSERT_EQ(notif, REMOVE);
- // == ADD + REMOVE: notification map should be empty ==
- P->addStateBlock(sb_ptr);
- P->removeStateBlock(sb_ptr);
- ASSERT_TRUE(P->getStateBlockNotificationMapSize() == 0);
+ // == REMOVE + ADD: notification map should be empty ==
+ P->notifyStateBlock(sb_ptr,ADD);
+ ASSERT_EQ(P->getStateBlockNotificationMapSize(),0);
// == UPDATES should clear the list of notifications ==
// add state_block
- P->addStateBlock(sb_ptr);
+ P->notifyStateBlock(sb_ptr,ADD);
// update solver
solver_manager_ptr->update();
ASSERT_EQ(P->getStateBlockNotificationMapSize(),0); // After solver_manager->update, notifications should be empty
+
+ // == ADD + REMOVE: notification map should be empty ==
+ P->notifyStateBlock(sb_ptr,ADD);
+ P->notifyStateBlock(sb_ptr,REMOVE);
+ ASSERT_EQ(P->getStateBlockNotificationMapSize(),0);
}
TEST(SolverManager, AddFactor)
@@ -542,7 +551,7 @@ TEST(SolverManager, RemoveFactor)
solver_manager_ptr->update();
// add factor
- P->removeFactor(c);
+ P->notifyFactor(c,REMOVE);
// notification
Notification notif;
@@ -578,7 +587,7 @@ TEST(SolverManager, AddRemoveFactor)
ASSERT_EQ(notif, ADD);
// add factor
- P->removeFactor(c);
+ P->notifyFactor(c,REMOVE);
// notification
ASSERT_EQ(P->getFactorNotificationMapSize(), 0); // ADD+REMOVE cancels out
@@ -611,13 +620,13 @@ TEST(SolverManager, DoubleRemoveFactor)
solver_manager_ptr->update();
// remove factor
- P->removeFactor(c);
+ P->notifyFactor(c,REMOVE);
// update solver
solver_manager_ptr->update();
// remove factor
- P->removeFactor(c);
+ P->notifyFactor(c,REMOVE);
// update solver
solver_manager_ptr->update();
diff --git a/test/gtest_track_matrix.cpp b/test/gtest_track_matrix.cpp
index f2f08e8c2241624b8914fd0eb6dc2ebbf93bab2d..8d7cafc5f699abc2f0fa9d275f57d5b4b3677821 100644
--- a/test/gtest_track_matrix.cpp
+++ b/test/gtest_track_matrix.cpp
@@ -25,23 +25,29 @@ class TrackMatrixTest : public testing::Test
virtual void SetUp()
{
- F0 = std::make_shared<FrameBase>(0.0, nullptr);
- F1 = std::make_shared<FrameBase>(1.0, nullptr);
- F2 = std::make_shared<FrameBase>(2.0, nullptr);
- F3 = std::make_shared<FrameBase>(3.0, nullptr);
- F4 = std::make_shared<FrameBase>(4.0, nullptr);
-
- C0 = std::make_shared<CaptureBase>("BASE", 0.0);
- C1 = std::make_shared<CaptureBase>("BASE", 1.0);
- C2 = std::make_shared<CaptureBase>("BASE", 2.0);
- C3 = std::make_shared<CaptureBase>("BASE", 3.0);
- C4 = std::make_shared<CaptureBase>("BASE", 4.0);
-
- f0 = std::make_shared<FeatureBase>("BASE", m, m_cov);
- f1 = std::make_shared<FeatureBase>("BASE", m, m_cov);
- f2 = std::make_shared<FeatureBase>("BASE", m, m_cov);
- f3 = std::make_shared<FeatureBase>("BASE", m, m_cov);
- f4 = std::make_shared<FeatureBase>("BASE", m, m_cov);
+ // unlinked captures
+ // Some warnings will be thrown "linking with nullptr" for emplacing without providing frame pointer
+ C0 = CaptureBase::emplace<CaptureBase>(nullptr, "BASE", 0.0);
+ C1 = CaptureBase::emplace<CaptureBase>(nullptr, "BASE", 1.0);
+ C2 = CaptureBase::emplace<CaptureBase>(nullptr, "BASE", 2.0);
+ C3 = CaptureBase::emplace<CaptureBase>(nullptr, "BASE", 3.0);
+ C4 = CaptureBase::emplace<CaptureBase>(nullptr, "BASE", 4.0);
+
+ // unlinked frames
+ // Some warnings will be thrown "linking with nullptr" for emplacing without providing frame pointer
+ F0 = FrameBase::emplace<FrameBase>(nullptr, 0.0, nullptr);
+ F1 = FrameBase::emplace<FrameBase>(nullptr, 1.0, nullptr);
+ F2 = FrameBase::emplace<FrameBase>(nullptr, 2.0, nullptr);
+ F3 = FrameBase::emplace<FrameBase>(nullptr, 3.0, nullptr);
+ F4 = FrameBase::emplace<FrameBase>(nullptr, 4.0, nullptr);
+
+ // unlinked features
+ // Some warnings will be thrown "linking with nullptr" for emplacing without providing capture pointer
+ f0 = FeatureBase::emplace<FeatureBase>(nullptr, "BASE", m, m_cov);
+ f1 = FeatureBase::emplace<FeatureBase>(nullptr, "BASE", m, m_cov);
+ f2 = FeatureBase::emplace<FeatureBase>(nullptr, "BASE", m, m_cov);
+ f3 = FeatureBase::emplace<FeatureBase>(nullptr, "BASE", m, m_cov);
+ f4 = FeatureBase::emplace<FeatureBase>(nullptr, "BASE", m, m_cov);
// F0 and F4 are keyframes
F0->setKey();
@@ -58,26 +64,35 @@ class TrackMatrixTest : public testing::Test
TEST_F(TrackMatrixTest, newTrack)
{
- track_matrix.newTrack(C0, f0);
+ f0->link(C0);
+ f1->link(C1);
+ f2->link(C1);
+ f3->link(C1);
+
+ track_matrix.newTrack(f0);
ASSERT_EQ(track_matrix.numTracks(), (unsigned int) 1);
- track_matrix.newTrack(C0, f1);
+ track_matrix.newTrack(f1);
ASSERT_EQ(track_matrix.numTracks(), (unsigned int) 2);
- track_matrix.newTrack(C1, f2);
+ track_matrix.newTrack(f2);
ASSERT_EQ(track_matrix.numTracks(), (unsigned int) 3);
- track_matrix.newTrack(C1, f3);
+ track_matrix.newTrack(f3);
ASSERT_EQ(track_matrix.numTracks(), (unsigned int) 4);
}
TEST_F(TrackMatrixTest, add)
{
- track_matrix.newTrack(C0, f0);
+ f0->link(C0);
+ f1->link(C1);
+ f2->link(C2);
+
+ track_matrix.newTrack(f0);
ASSERT_EQ(track_matrix.numTracks(), (unsigned int) 1);
- track_matrix.add(f0->trackId(), C1, f1);
- /* KC0 C1 C2 snapshots
+ track_matrix.add(f0->trackId(), f1);
+ /* KC0 C1 snapshots
*
* f0---f1 trk 0
*/
@@ -85,7 +100,33 @@ TEST_F(TrackMatrixTest, add)
ASSERT_EQ(f1->trackId(), f0->trackId());
ASSERT_EQ(track_matrix.numTracks(), (unsigned int) 1);
- track_matrix.add(f0->trackId(), C2, f2);
+ track_matrix.add(f0->trackId(), f2);
+ /* KC0 C1 C2 snapshots
+ *
+ * f0---f1---f2 trk 0
+ */
+ ASSERT_EQ(track_matrix.trackSize(f2->trackId()), (unsigned int) 3);
+ ASSERT_EQ(f2->trackId(), f0->trackId());
+}
+
+TEST_F(TrackMatrixTest, add2)
+{
+ f0->link(C0);
+ f1->link(C1);
+ f2->link(C2);
+ f3->link(C1);
+
+ track_matrix.newTrack(f0);
+
+ track_matrix.add(f0, f1);
+ /* KC0 C1 snapshots
+ *
+ * f0---f1 trk 0
+ */
+ ASSERT_EQ(track_matrix.trackSize(f1->trackId()), (unsigned int) 2);
+ ASSERT_EQ(f1->trackId(), f0->trackId());
+
+ track_matrix.add(f1, f2);
/* KC0 C1 C2 snapshots
*
* f0---f1---f2 trk 0
@@ -94,13 +135,12 @@ TEST_F(TrackMatrixTest, add)
ASSERT_EQ(f2->trackId(), f0->trackId());
ASSERT_EQ(track_matrix.numTracks(), (unsigned int) 1);
- track_matrix.add(f0->trackId(), C2, f2);
+ track_matrix.newTrack(f3);
/* KC0 C1 C2 snapshots
*
* f0---f1---f2 trk 0
* f3 trk 1
*/
- track_matrix.add(1, C1, f3);
ASSERT_EQ(track_matrix.trackSize(f3->trackId()), (unsigned int) 1);
ASSERT_NE(f3->trackId(), f0->trackId());
ASSERT_EQ(track_matrix.numTracks(), (unsigned int) 2);
@@ -108,8 +148,12 @@ TEST_F(TrackMatrixTest, add)
TEST_F(TrackMatrixTest, numTracks)
{
- track_matrix.newTrack(C0, f0);
- track_matrix.add(f0->trackId(), C1, f1);
+ f0->link(C0);
+ f1->link(C1);
+ f2->link(C0);
+
+ track_matrix.newTrack(f0);
+ track_matrix.add(f0->trackId(), f1);
/* C0 C1 C2 snapshots
*
@@ -118,16 +162,20 @@ TEST_F(TrackMatrixTest, numTracks)
ASSERT_EQ(track_matrix.numTracks(), (unsigned int) 1);
- track_matrix.add(f0->trackId(), C0, f2);
+ track_matrix.add(f0->trackId(), f2);
ASSERT_EQ(track_matrix.numTracks(), (unsigned int) 1);
}
TEST_F(TrackMatrixTest, trackSize)
{
- track_matrix.newTrack(C0, f0);
- track_matrix.add(f0->trackId(), C1, f1);
- track_matrix.newTrack(C0, f2);
+ f0->link(C0);
+ f1->link(C1);
+ f2->link(C0);
+
+ track_matrix.newTrack(f0);
+ track_matrix.add(f0->trackId(), f1);
+ track_matrix.newTrack(f2);
/* C0 C1 C2 snapshots
*
@@ -149,9 +197,13 @@ TEST_F(TrackMatrixTest, first_last_Feature)
* f2 trk 1
*/
- track_matrix.newTrack(C0, f0);
- track_matrix.add(f0->trackId(), C1, f1);
- track_matrix.newTrack(C1, f2);
+ f0->link(C0);
+ f1->link(C1);
+ f2->link(C1);
+
+ track_matrix.newTrack(f0);
+ track_matrix.add(f0->trackId(), f1);
+ track_matrix.newTrack(f2);
ASSERT_EQ(track_matrix.firstFeature(f0->trackId()), f0);
ASSERT_EQ(track_matrix.lastFeature (f0->trackId()), f1);
@@ -162,9 +214,13 @@ TEST_F(TrackMatrixTest, first_last_Feature)
TEST_F(TrackMatrixTest, remove_ftr)
{
- track_matrix.newTrack(C0, f0);
- track_matrix.add(f0->trackId(), C1, f1);
- track_matrix.newTrack(C0, f2);
+ f0->link(C0);
+ f1->link(C1);
+ f2->link(C0);
+
+ track_matrix.newTrack(f0);
+ track_matrix.add(f0->trackId(), f1);
+ track_matrix.newTrack(f2);
/* C0 C1 C2 snapshots
*
@@ -207,9 +263,13 @@ TEST_F(TrackMatrixTest, remove_ftr)
TEST_F(TrackMatrixTest, remove_trk)
{
- track_matrix.newTrack(C0, f0);
- track_matrix.add(f0->trackId(), C1, f1);
- track_matrix.newTrack(C0, f2);
+ f0->link(C0);
+ f1->link(C1);
+ f2->link(C0);
+
+ track_matrix.newTrack(f0);
+ track_matrix.add(f0->trackId(), f1);
+ track_matrix.newTrack(f2);
/* C0 C1 C2 snapshots
*
@@ -230,9 +290,13 @@ TEST_F(TrackMatrixTest, remove_trk)
TEST_F(TrackMatrixTest, remove_snapshot)
{
- track_matrix.newTrack(C0, f0);
- track_matrix.add(f0->trackId(), C1, f1);
- track_matrix.newTrack(C0, f2);
+ f0->link(C0);
+ f1->link(C1);
+ f2->link(C0);
+
+ track_matrix.newTrack(f0);
+ track_matrix.add(f0->trackId(), f1);
+ track_matrix.newTrack(f2);
/* C0 C1 C2 snapshots
*
@@ -257,9 +321,13 @@ TEST_F(TrackMatrixTest, remove_snapshot)
TEST_F(TrackMatrixTest, track)
{
- track_matrix.newTrack(C0, f0);
- track_matrix.add(f0->trackId(), C1, f1);
- track_matrix.newTrack(C0, f2);
+ f0->link(C0);
+ f1->link(C1);
+ f2->link(C0);
+
+ track_matrix.newTrack(f0);
+ track_matrix.add(f0->trackId(), f1);
+ track_matrix.newTrack(f2);
/* C0 C1 C2 snapshots
*
@@ -281,9 +349,13 @@ TEST_F(TrackMatrixTest, track)
TEST_F(TrackMatrixTest, snapshot)
{
- track_matrix.newTrack(C0, f0);
- track_matrix.add(f0->trackId(), C1, f1);
- track_matrix.newTrack(C0, f2);
+ f0->link(C0);
+ f1->link(C1);
+ f2->link(C0);
+
+ track_matrix.newTrack(f0);
+ track_matrix.add(f0->trackId(), f1);
+ track_matrix.newTrack(f2);
/* C0 C1 C2 snapshots
*
@@ -305,9 +377,13 @@ TEST_F(TrackMatrixTest, snapshot)
TEST_F(TrackMatrixTest, trackAsVector)
{
- track_matrix.newTrack(C0, f0);
- track_matrix.add(f0->trackId(), C1, f1);
- track_matrix.newTrack(C0, f2);
+ f0->link(C0);
+ f1->link(C1);
+ f2->link(C0);
+
+ track_matrix.newTrack(f0);
+ track_matrix.add(f0->trackId(), f1);
+ track_matrix.newTrack(f2);
/* C0 C1 C2 snapshots
*
@@ -325,9 +401,13 @@ TEST_F(TrackMatrixTest, trackAsVector)
TEST_F(TrackMatrixTest, snapshotAsList)
{
- track_matrix.newTrack(C0, f0);
- track_matrix.add(f0->trackId(), C1, f1);
- track_matrix.newTrack(C0, f2);
+ f0->link(C0);
+ f1->link(C1);
+ f2->link(C0);
+
+ track_matrix.newTrack(f0);
+ track_matrix.add(f0->trackId(), f1);
+ track_matrix.newTrack(f2);
/* C0 C1 C2 snapshots
*
@@ -345,11 +425,17 @@ TEST_F(TrackMatrixTest, snapshotAsList)
TEST_F(TrackMatrixTest, matches)
{
- track_matrix.newTrack(C0, f0);
- track_matrix.add(f0->trackId(), C1, f1);
- track_matrix.add(f0->trackId(), C2, f2);
- track_matrix.newTrack(C0, f3);
- track_matrix.add(f3->trackId(), C1, f4);
+ f0->link(C0);
+ f1->link(C1);
+ f2->link(C2);
+ f3->link(C0);
+ f4->link(C1);
+
+ track_matrix.newTrack(f0);
+ track_matrix.add(f0->trackId(), f1);
+ track_matrix.add(f0->trackId(), f2);
+ track_matrix.newTrack(f3);
+ track_matrix.add(f3->trackId(), f4);
/* C0 C1 C2 C3 snapshots
*
@@ -371,11 +457,17 @@ TEST_F(TrackMatrixTest, matches)
TEST_F(TrackMatrixTest, trackAtKeyframes)
{
- track_matrix.newTrack(C0, f0);
- track_matrix.add(f0->trackId(), C1, f1);
- track_matrix.add(f0->trackId(), C2, f2);
- track_matrix.add(f0->trackId(), C4, f4);
- track_matrix.newTrack(C1, f3);
+ f0->link(C0);
+ f1->link(C1);
+ f2->link(C2);
+ f3->link(C1);
+ f4->link(C4);
+
+ track_matrix.newTrack(f0);
+ track_matrix.add(f0->trackId(), f1);
+ track_matrix.add(f0->trackId(), f2);
+ track_matrix.add(f0->trackId(), f4);
+ track_matrix.newTrack(f3);
/* KC0 C1 C2 C3 KC4 snapshots
*
* f0---f1---f2--------f4 trk 0
diff --git a/test/gtest_trajectory.cpp b/test/gtest_trajectory.cpp
index 2edffe94d62ca65ce10bce645eeb5d08d685676f..d31cb98496a85cc82a1436dd1ef8043809a89488 100644
--- a/test/gtest_trajectory.cpp
+++ b/test/gtest_trajectory.cpp
@@ -57,14 +57,10 @@ TEST(TrajectoryBase, ClosestKeyFrame)
// 1 2 3 4 time stamps
// --+-----+-----+-----+---> time
- FrameBasePtr F1 = std::make_shared<FrameBase>(KEY, 1, nullptr, nullptr);
- FrameBasePtr F2 = std::make_shared<FrameBase>(KEY, 2, nullptr, nullptr);
- FrameBasePtr F3 = std::make_shared<FrameBase>(AUXILIARY, 3, nullptr, nullptr);
- FrameBasePtr F4 = std::make_shared<FrameBase>(NON_ESTIMATED, 4, nullptr, nullptr);
- T->addFrame(F1);
- T->addFrame(F2);
- T->addFrame(F3);
- T->addFrame(F4);
+ FrameBasePtr F1 = P->emplaceFrame(KEY, Eigen::Vector3s::Zero(), 1);
+ FrameBasePtr F2 = P->emplaceFrame(KEY, Eigen::Vector3s::Zero(), 2);
+ FrameBasePtr F3 = P->emplaceFrame(AUXILIARY, Eigen::Vector3s::Zero(), 3);
+ FrameBasePtr F4 = P->emplaceFrame(NON_ESTIMATED, Eigen::Vector3s::Zero(), 4);
FrameBasePtr KF; // closest key-frame queried
@@ -98,9 +94,9 @@ TEST(TrajectoryBase, ClosestKeyOrAuxFrame)
// 1 2 3 time stamps
// --+-----+-----+---> time
- FrameBasePtr F1 = FrameBase::emplace<FrameBase>(T, KEY, 1, nullptr, nullptr);
- FrameBasePtr F2 = FrameBase::emplace<FrameBase>(T, AUXILIARY, 2, nullptr, nullptr);
- FrameBasePtr F3 = FrameBase::emplace<FrameBase>(T, NON_ESTIMATED, 3, nullptr, nullptr);
+ FrameBasePtr F1 = P->emplaceFrame(KEY, Eigen::Vector3s::Zero(), 1);
+ FrameBasePtr F2 = P->emplaceFrame(AUXILIARY, Eigen::Vector3s::Zero(), 2);
+ FrameBasePtr F3 = P->emplaceFrame(NON_ESTIMATED, Eigen::Vector3s::Zero(), 3);
FrameBasePtr KF; // closest key-frame queried
@@ -134,30 +130,24 @@ TEST(TrajectoryBase, Add_Remove_Frame)
// 1 2 3 time stamps
// --+-----+-----+---> time
- FrameBasePtr F1 = std::make_shared<FrameBase>(KEY, 1, make_shared<StateBlock>(2), make_shared<StateBlock>(1)); // 2 non-fixed
- FrameBasePtr F2 = std::make_shared<FrameBase>(KEY, 2, make_shared<StateBlock>(2), make_shared<StateBlock>(1, true)); // 1 fixed, 1 not
- FrameBasePtr F3 = std::make_shared<FrameBase>(NON_ESTIMATED, 3, make_shared<StateBlock>(2), make_shared<StateBlock>(1)); // non-key-frame
-
- // FrameBasePtr f1 = FrameBase::emplace<FrameBase>(T, KEY_FRAME, 1, make_shared<StateBlock>(2), make_shared<StateBlock>(1)); // 2 non-fixed
- // FrameBasePtr f2 = FrameBase::emplace<FrameBase>(T, KEY_FRAME, 2, make_shared<StateBlock>(2), make_shared<StateBlock>(1, true)); // 1 fixed, 1 not
- // FrameBasePtr f3 = FrameBase::emplace<FrameBase>(T, NON_KEY_FRAME, 3, make_shared<StateBlock>(2), make_shared<StateBlock>(1)); // non-key-frame
-
std::cout << __LINE__ << std::endl;
- // add frames and keyframes
- F1->link(T);
+ // add F1
+ FrameBasePtr F1 = P->emplaceFrame(KEY, Eigen::Vector3s::Zero(), 1); // 2 non-fixed
if (debug) P->print(2,0,0,0);
ASSERT_EQ(T->getFrameList(). size(), (SizeStd) 1);
ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd) 2);
std::cout << __LINE__ << std::endl;
- F2->link(T);
+ // add F2
+ FrameBasePtr F2 = P->emplaceFrame(KEY, Eigen::Vector3s::Zero(), 2); // 1 fixed, 1 not
if (debug) P->print(2,0,0,0);
ASSERT_EQ(T->getFrameList(). size(), (SizeStd) 2);
ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd) 4);
std::cout << __LINE__ << std::endl;
- F3->link(T);
+ // add F3
+ FrameBasePtr F3 = P->emplaceFrame(NON_ESTIMATED, Eigen::Vector3s::Zero(), 3);
if (debug) P->print(2,0,0,0);
ASSERT_EQ(T->getFrameList(). size(), (SizeStd) 3);
ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd) 4);
@@ -212,24 +202,20 @@ TEST(TrajectoryBase, KeyFramesAreSorted)
// 1 2 3 time stamps
// --+-----+-----+---> time
- FrameBasePtr F1 = std::make_shared<FrameBase>(KEY, 1, make_shared<StateBlock>(2), make_shared<StateBlock>(1)); // 2 non-fixed
- FrameBasePtr F2 = std::make_shared<FrameBase>(KEY, 2, make_shared<StateBlock>(2), make_shared<StateBlock>(1, true)); // 1 fixed, 1 not
- FrameBasePtr F3 = std::make_shared<FrameBase>(NON_ESTIMATED, 3, make_shared<StateBlock>(2), make_shared<StateBlock>(1)); // non-key-frame
-
// add frames and keyframes in random order --> keyframes must be sorted after that
- F2->link(T);
+ FrameBasePtr F2 = P->emplaceFrame(KEY, Eigen::Vector3s::Zero(), 2);
if (debug) P->print(2,0,0,0);
ASSERT_EQ(T->getLastFrame() ->id(), F2->id());
ASSERT_EQ(T->getLastKeyOrAuxFrame()->id(), F2->id());
ASSERT_EQ(T->getLastKeyFrame() ->id(), F2->id());
- F3->link(T);
+ FrameBasePtr F3 = P->emplaceFrame(NON_ESTIMATED, Eigen::Vector3s::Zero(), 3); // non-key-frame
if (debug) P->print(2,0,0,0);
ASSERT_EQ(T->getLastFrame() ->id(), F3->id());
ASSERT_EQ(T->getLastKeyOrAuxFrame()->id(), F2->id());
ASSERT_EQ(T->getLastKeyFrame() ->id(), F2->id());
- F1->link(T);
+ FrameBasePtr F1 = P->emplaceFrame(KEY, Eigen::Vector3s::Zero(), 1);
if (debug) P->print(2,0,0,0);
ASSERT_EQ(T->getLastFrame() ->id(), F3->id());
ASSERT_EQ(T->getLastKeyOrAuxFrame()->id(), F2->id());