diff --git a/include/core/factor/factor_base.h b/include/core/factor/factor_base.h index c039c319445c82d5b8909a46ba2fa695da88b118..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,23 +45,27 @@ 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); - /** \brief Constructor valid for all categories (FRAME, CAPTURE; 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, @@ -134,10 +138,6 @@ 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(); } @@ -161,12 +161,6 @@ class FactorBase : public NodeBase, public std::enable_shared_from_this<FactorBa */ ProcessorBasePtr getProcessor() const; - /** - * @brief setProcessor - * @param _processor_ptr - */ - void setProcessor(const ProcessorBasePtr& _processor_ptr); - void link(FeatureBasePtr ftr); virtual void setProblem(ProblemPtr) final; template<typename classType, typename... T> @@ -175,6 +169,12 @@ class FactorBase : public NodeBase, public std::enable_shared_from_this<FactorBa private: void setFeature(const FeatureBasePtr _ft_ptr){feature_ptr_ = _ft_ptr;} + + /** + * @brief setProcessor + * @param _processor_ptr + */ + void setProcessor(const ProcessorBasePtr& _processor_ptr); }; } diff --git a/include/core/processor/processor_tracker_feature.h b/include/core/processor/processor_tracker_feature.h index ed87f3ea76c252c8ba9f1a91bf1432f9365f1709..d3034cc601dafd1da5331a8e48996a4954d53ef6 100644 --- a/include/core/processor/processor_tracker_feature.h +++ b/include/core/processor/processor_tracker_feature.h @@ -119,7 +119,10 @@ class ProcessorTrackerFeature : public ProcessorTracker * \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. It means, they have to be already linked to the _capture + * 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 either emplace them (`FeatureBase::emplace()`) and remove the not returned features in _features_out (`FeatureBase::remove()`), + * or create them (`make_shared()`) and link all the returned features in _features_out (`FeatureBase::link(_capture)`). * * \return the number of features tracked */ @@ -163,7 +166,10 @@ class ProcessorTrackerFeature : public ProcessorTracker * * This function detects Features that do not correspond to known Features/Landmarks in the system. * - * IMPORTANT: The features in _features_out should be emplaced. It means, they have to be already linked to the _capture + * 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 either emplace them (`FeatureBase::emplace()`) and remove the not returned features in _features_out (`FeatureBase::remove()`), + * or create them (`make_shared()`) and link all the returned features 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_. diff --git a/include/core/processor/processor_tracker_feature_dummy.h b/include/core/processor/processor_tracker_feature_dummy.h index c3142d422480e0cbb9d59151badfde3437f23310..007207cecb59a4d5028bfe5f1b0e205a0b0726b8 100644 --- a/include/core/processor/processor_tracker_feature_dummy.h +++ b/include/core/processor/processor_tracker_feature_dummy.h @@ -51,7 +51,8 @@ class ProcessorTrackerFeatureDummy : public ProcessorTrackerFeature * \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. It means, they have to be already linked to the _capture + * 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 */ @@ -84,7 +85,8 @@ class ProcessorTrackerFeatureDummy : public ProcessorTrackerFeature * * This function detects Features that do not correspond to known Features/Landmarks in the system. * - * IMPORTANT: The features in _features_out should be emplaced. It means, they have to be already linked to the _capture + * 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_. diff --git a/include/core/processor/processor_tracker_landmark.h b/include/core/processor/processor_tracker_landmark.h index 8349111e509b1f7813e1ad147ac9ab612c50427c..b74ce24d5b4d90888631856a058143ade8af9fad 100644 --- a/include/core/processor/processor_tracker_landmark.h +++ b/include/core/processor/processor_tracker_landmark.h @@ -110,7 +110,10 @@ class ProcessorTrackerLandmark : public ProcessorTracker * \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. It means, they have to be already linked to the _capture + * 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 either emplace them (`FeatureBase::emplace()`) and remove the not returned features in _features_out (`FeatureBase::remove()`), + * or create them (`make_shared()`) and link all the returned features in _features_out (`FeatureBase::link(_capture)`). * * \return the number of landmarks found */ @@ -145,7 +148,10 @@ class ProcessorTrackerLandmark : public ProcessorTracker * * This function detects Features that do not correspond to known Features/Landmarks in the system. * - * IMPORTANT: The features in _features_out should be emplaced. It means, they have to be already linked to the _capture + * 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 either emplace them (`FeatureBase::emplace()`) and remove the not returned features in _features_out (`FeatureBase::remove()`), + * or create them (`make_shared()`) and link all the returned features 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_. diff --git a/include/core/processor/processor_tracker_landmark_dummy.h b/include/core/processor/processor_tracker_landmark_dummy.h index 9bbcf66b5b7096565d14a5457bab4a5427f4f56b..7dce7d14e23e77df062ff2774630767d1d2d6b48 100644 --- a/include/core/processor/processor_tracker_landmark_dummy.h +++ b/include/core/processor/processor_tracker_landmark_dummy.h @@ -46,7 +46,8 @@ class ProcessorTrackerLandmarkDummy : public ProcessorTrackerLandmark * \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. It means, they have to be already linked to the _capture + * 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 */ @@ -72,7 +73,8 @@ class ProcessorTrackerLandmarkDummy : public ProcessorTrackerLandmark * * This function detects Features that do not correspond to known Features/Landmarks in the system. * - * IMPORTANT: The features in _features_out should be emplaced. It means, they have to be already linked to the _capture + * 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_. diff --git a/src/factor/factor_base.cpp b/src/factor/factor_base.cpp index 82cb9a01f7bec72ca26fb4dbed907e40bc992dc7..adec4caf9e8554a4b95baa11dd3dcf5846c06eae 100644 --- a/src/factor/factor_base.cpp +++ b/src/factor/factor_base.cpp @@ -7,8 +7,8 @@ namespace wolf { unsigned int FactorBase::factor_id_count_ = 0; FactorBase::FactorBase(const std::string& _tp, - bool _apply_loss_function, - FactorStatus _status) : + bool _apply_loss_function, + FactorStatus _status) : NodeBase("FACTOR", _tp), feature_ptr_(), // nullptr factor_id_(++factor_id_count_), @@ -23,12 +23,13 @@ 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) : + 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_),