diff --git a/include/laser/capture/capture_laser_2d.h b/include/laser/capture/capture_laser_2d.h
index 579f4475b3e2bd3a59fd41c4fb3fb9dc24db883a..fca364b7234505008a311b93ebbe60007ed4c4a6 100644
--- a/include/laser/capture/capture_laser_2d.h
+++ b/include/laser/capture/capture_laser_2d.h
@@ -47,6 +47,7 @@ class CaptureLaser2d : public CaptureBase
CaptureLaser2d(const TimeStamp& _ts, SensorBasePtr _sensor_ptr, const std::vector<float>& _ranges);
~CaptureLaser2d() override = default;
+ const laserscanutils::LaserScan& getScan() const;
laserscanutils::LaserScan& getScan();
void setSensor(const SensorBasePtr sensor_ptr) override;
@@ -57,6 +58,11 @@ class CaptureLaser2d : public CaptureBase
};
+inline const laserscanutils::LaserScan& CaptureLaser2d::getScan() const
+{
+ return scan_;
+}
+
inline laserscanutils::LaserScan& CaptureLaser2d::getScan()
{
return scan_;
diff --git a/include/laser/factor/factor_point_2d.h b/include/laser/factor/factor_point_2d.h
index 30b4884d08fa0874c97722f47f0804b3b0e3b3c4..15d6ee7185659a0e5068388748140d09993ce02b 100644
--- a/include/laser/factor/factor_point_2d.h
+++ b/include/laser/factor/factor_point_2d.h
@@ -88,6 +88,10 @@ class FactorPoint2d: public FactorAutodiff<FactorPoint2d, 2,2,1,2,1,2>
* @brief getLandmarkPtr
* @return
*/
+ LandmarkPolyline2dConstPtr getLandmark() const
+ {
+ return std::static_pointer_cast<const LandmarkPolyline2d>(getLandmarkOther());
+ }
LandmarkPolyline2dPtr getLandmark()
{
return std::static_pointer_cast<LandmarkPolyline2d>(getLandmarkOther());
@@ -97,7 +101,7 @@ class FactorPoint2d: public FactorAutodiff<FactorPoint2d, 2,2,1,2,1,2>
* @brief getLandmarkPointId
* @return
*/
- int getLandmarkPointId()
+ int getLandmarkPointId() const
{
return landmark_point_id_;
}
@@ -106,7 +110,7 @@ class FactorPoint2d: public FactorAutodiff<FactorPoint2d, 2,2,1,2,1,2>
* @brief getFeaturePointId
* @return
*/
- unsigned int getFeaturePointId()
+ unsigned int getFeaturePointId() const
{
return feature_point_id_;
}
@@ -115,6 +119,10 @@ class FactorPoint2d: public FactorAutodiff<FactorPoint2d, 2,2,1,2,1,2>
* @brief getLandmarkPointPtr
* @return
*/
+ StateBlockConstPtr getLandmarkPoint() const
+ {
+ return point_state_ptr_;
+ }
StateBlockPtr getLandmarkPoint()
{
return point_state_ptr_;
diff --git a/include/laser/factor/factor_point_to_line_2d.h b/include/laser/factor/factor_point_to_line_2d.h
index cc975d802a098f3dc610bb5b185e5184e489c69c..f2035e32e3f84b8ff44c1bae60b95ecb6636f4e4 100644
--- a/include/laser/factor/factor_point_to_line_2d.h
+++ b/include/laser/factor/factor_point_to_line_2d.h
@@ -36,8 +36,8 @@ WOLF_PTR_TYPEDEFS(FactorPointToLine2d);
class FactorPointToLine2d: public FactorAutodiff<FactorPointToLine2d, 1,2,1,2,1,2,2>
{
protected:
- int landmark_point_id_;
- int landmark_point_aux_id_;
+ int landmark_point_id_;
+ int landmark_point_aux_id_;
unsigned int feature_point_id_;
StateBlockPtr point_state_ptr_;
StateBlockPtr point_aux_state_ptr_;
@@ -91,31 +91,45 @@ class FactorPointToLine2d: public FactorAutodiff<FactorPointToLine2d, 1,2,1,2,1,
~FactorPointToLine2d() override = default;
+ LandmarkPolyline2dConstPtr getLandmark() const
+ {
+ return std::static_pointer_cast<const LandmarkPolyline2d>( getLandmarkOther() );
+ }
LandmarkPolyline2dPtr getLandmark()
{
return std::static_pointer_cast<LandmarkPolyline2d>( getLandmarkOther() );
}
- int getLandmarkPointId()
+ int getLandmarkPointId() const
{
return landmark_point_id_;
}
- int getLandmarkPointAuxId()
+ int getLandmarkPointAuxId() const
{
return landmark_point_aux_id_;
}
- unsigned int getFeaturePointId()
+ unsigned int getFeaturePointId() const
{
return feature_point_id_;
}
+ StateBlockConstPtr getLandmarkPoint() const
+ {
+ return point_state_ptr_;
+ }
+
StateBlockPtr getLandmarkPoint()
{
return point_state_ptr_;
}
+ StateBlockConstPtr getLandmarkPointAux() const
+ {
+ return point_state_ptr_;
+ }
+
StateBlockPtr getLandmarkPointAux()
{
return point_state_ptr_;
diff --git a/include/laser/feature/feature_polyline_2d.h b/include/laser/feature/feature_polyline_2d.h
index b889c49b827b3f76f197b9f4fa7f801285cecf82..c5cd36350ff9757a9dcbc058ae65d2292e8ada64 100644
--- a/include/laser/feature/feature_polyline_2d.h
+++ b/include/laser/feature/feature_polyline_2d.h
@@ -55,13 +55,8 @@ class FeaturePolyline2d : public FeatureBase
int getNPoints() const;
};
-//<<<<<<< HEAD
inline FeaturePolyline2d::FeaturePolyline2d(const Eigen::MatrixXd& _points, const Eigen::MatrixXd& _points_cov, const bool& _first_defined, const bool& _last_defined) :
FeatureBase("FeaturePolyline2d", Eigen::Vector1d::Zero(), Eigen::Vector1d::Ones()),
-//=======
-//inline FeaturePolyline2d::FeaturePolyline2d(const Eigen::MatrixXd& _points, const Eigen::MatrixXd& _points_cov, const bool& _first_defined, const bool& _last_defined) :
-// FeatureBase("POLYLINE 2d", Eigen::Vector1d::Zero(), Eigen::Vector1d::Ones()),
-//>>>>>>> 8-replace-scalar-to-double
points_(_points),
points_cov_(_points_cov),
first_defined_(_first_defined),
diff --git a/include/laser/feature/feature_scene_falko.h b/include/laser/feature/feature_scene_falko.h
index c571db55da8177dadbd4e63818317ce2271a76d3..69d449fe653d7b3003d61b4230f553ba2bdf4b90 100644
--- a/include/laser/feature/feature_scene_falko.h
+++ b/include/laser/feature/feature_scene_falko.h
@@ -48,29 +48,11 @@ template <typename D> class FeatureSceneFalko : public FeatureBase
std::shared_ptr<laserscanutils::SceneFalko<D>> scene_;
};
-WOLF_PTR_TYPEDEFS(FeatureSceneFalkoBsc);
+typedef FeatureSceneFalko<laserscanutils::bsc> FeatureSceneFalkoBsc;
+WOLF_DECLARED_PTR_TYPEDEFS(FeatureSceneFalkoBsc);
-class FeatureSceneFalkoBsc : public FeatureSceneFalko<laserscanutils::bsc>
-{
- public:
- FeatureSceneFalkoBsc(const std::shared_ptr<laserscanutils::SceneFalko<laserscanutils::bsc>> _falko_scene)
- : FeatureSceneFalko(_falko_scene)
- {
- }
- ~FeatureSceneFalkoBsc();
-};
-
-WOLF_PTR_TYPEDEFS(FeatureSceneFalkoCgh);
-
-class FeatureSceneFalkoCgh : public FeatureSceneFalko<laserscanutils::cgh>
-{
- public:
- FeatureSceneFalkoCgh(const std::shared_ptr<laserscanutils::SceneFalko<laserscanutils::cgh>> _falko_scene)
- : FeatureSceneFalko(_falko_scene)
- {
- }
- ~FeatureSceneFalkoCgh() override{};
-};
+typedef FeatureSceneFalko<laserscanutils::cgh> FeatureSceneFalkoCgh;
+WOLF_DECLARED_PTR_TYPEDEFS(FeatureSceneFalkoCgh);
} // namespace wolf
diff --git a/include/laser/landmark/landmark_polyline_2d.h b/include/laser/landmark/landmark_polyline_2d.h
index 4d1574cf3bb765c674435ba65cd2afb1c632b584..4d930a8edcc460bf117d9f4fd38fdde6c1bb4da0 100644
--- a/include/laser/landmark/landmark_polyline_2d.h
+++ b/include/laser/landmark/landmark_polyline_2d.h
@@ -39,6 +39,12 @@
namespace wolf
{
+static std::vector<char> available_chars({'0','1','2','3','4','5','6','7','8','9',
+ 'a','b','c','d','e','f','g','h','i','j','k','l',
+ 'm','n','r','s','t','u','v','w','x','y','z',
+ 'A','B','C','D','E','F','G','H','I','J','K','L',
+ 'M','N','R','S','T','U','V','W','X','Y','Z'});
+
//landmark classification
typedef enum
{
@@ -92,7 +98,7 @@ WOLF_LIST_TYPEDEFS(LandmarkPolyline2d);
class LandmarkPolyline2d : public LandmarkBase
{
protected:
- std::map<int,StateBlockPtr> point_state_ptr_map_; ///< polyline points state blocks
+ std::map<int,StateBlockPtr> point_state_map_; ///< polyline points state blocks
bool first_defined_; ///< Wether the first point is an extreme of a line or the line may continue
bool last_defined_; ///< Wether the last point is an extreme of a line or the line may continue
bool closed_; ///< Wether the polyline is closed or not
@@ -101,19 +107,29 @@ class LandmarkPolyline2d : public LandmarkBase
LandmarkPolyline2dPtr merged_in_lmk_;
private:
-
- StateBlockPtr& firstStateBlock();
- StateBlockPtr& lastStateBlock();
+ char idToChar(int id) const;
+ StateBlockPtr firstStateBlock();
+ StateBlockPtr lastStateBlock();
public:
- LandmarkPolyline2d(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, const Eigen::MatrixXd& _points, const bool _first_defined, const bool _last_defined, unsigned int _first_id = 0);
- LandmarkPolyline2d(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, const Eigen::MatrixXd& _points, const bool _first_defined, const bool _last_defined, unsigned int _first_id, PolylineRectangularClass _class);
+ LandmarkPolyline2d(StateBlockPtr _p_ptr,
+ StateBlockPtr _o_ptr,
+ const Eigen::MatrixXd& _points,
+ const bool _first_defined,
+ const bool _last_defined,
+ unsigned int _first_id = 0);
+ LandmarkPolyline2d(StateBlockPtr _p_ptr,
+ StateBlockPtr _o_ptr,
+ const Eigen::MatrixXd& _points,
+ const bool _first_defined,
+ const bool _last_defined,
+ unsigned int _first_id,
+ PolylineRectangularClass _class);
~LandmarkPolyline2d() override;
/** \brief Gets a const reference to the point state block pointer vector
**/
- std::map<int,StateBlockPtr>& getPointStatePtrMap();
-
+ std::map<int,StateBlockPtr>& getPointStateMap();
/** \brief Gets wether the first/last points are defined or not
**/
@@ -126,7 +142,8 @@ class LandmarkPolyline2d : public LandmarkBase
/** \brief Get the landmark pointer in which was merged (nullptr in case of no merged)
**/
- LandmarkPolyline2dPtr getMergedInLandmark() const;
+ LandmarkPolyline2dConstPtr getMergedInLandmark() const;
+ LandmarkPolyline2dPtr getMergedInLandmark();
/** \brief Set the landmark pointer in which was merged
**/
@@ -134,7 +151,7 @@ class LandmarkPolyline2d : public LandmarkBase
/** \brief Gets whether the given state block point is defined or not (assumes the state block is in the landmark)
**/
- bool isDefined(StateBlockPtr _state_block) const;
+ bool isDefined(StateBlockConstPtr _state_block) const;
/** \brief Sets the first/last extreme point
**/
@@ -152,18 +169,15 @@ class LandmarkPolyline2d : public LandmarkBase
int getPrevValidId(const int& i) const;
int getVersion() const;
- const Eigen::VectorXd getPointVector(int _i) const;
+ Eigen::VectorXd getPointVector(int _i) const;
Eigen::MatrixXd computePointsGlobal() const;
Eigen::MatrixXd computePointsGlobal(const int& _from_id, const int& _to_id) const;
Eigen::MatrixXd computePointsCovGlobal() const;
+ StateBlockConstPtr getPointStateBlock(int _i) const;
StateBlockPtr getPointStateBlock(int _i);
- /** \brief Gets a vector of all state blocks pointers
- **/
- virtual std::vector<StateBlockPtr> getPointsStateBlockVector() const;
-
/** \brief Adds a new point to the landmark
* \param _point: the point to be added
* \param _extreme: if its extreme or not
@@ -219,11 +233,6 @@ class LandmarkPolyline2d : public LandmarkBase
**/
PolylineRectangularClass getClassification() const;
- /** \brief Adds all stateBlocks of the frame to the wolfProblem list of new stateBlocks
- **/
- virtual void registerNewStateBlocks() const;
- virtual void removeStateBlocks();
-
/** Factory method to create new landmarks from YAML nodes
*/
static LandmarkBasePtr create(const YAML::Node& _lmk_node);
@@ -240,19 +249,19 @@ class LandmarkPolyline2d : public LandmarkBase
// STL
#include <iterator>
-inline StateBlockPtr& LandmarkPolyline2d::firstStateBlock()
+inline StateBlockPtr LandmarkPolyline2d::firstStateBlock()
{
- return point_state_ptr_map_.begin()->second;
+ return point_state_map_.begin()->second;
}
-inline StateBlockPtr& LandmarkPolyline2d::lastStateBlock()
+inline StateBlockPtr LandmarkPolyline2d::lastStateBlock()
{
- return point_state_ptr_map_.rbegin()->second;
+ return point_state_map_.rbegin()->second;
}
-inline std::map<int,StateBlockPtr>& LandmarkPolyline2d::getPointStatePtrMap()
+inline std::map<int,StateBlockPtr>& LandmarkPolyline2d::getPointStateMap()
{
- return point_state_ptr_map_;
+ return point_state_map_;
}
inline bool LandmarkPolyline2d::isFirstDefined() const
@@ -267,7 +276,7 @@ inline bool LandmarkPolyline2d::isLastDefined() const
inline bool LandmarkPolyline2d::isValidId(const int& i) const
{
- return point_state_ptr_map_.count(i) != 0;
+ return point_state_map_.count(i) != 0;
};
inline bool LandmarkPolyline2d::isClosed() const
@@ -275,7 +284,15 @@ inline bool LandmarkPolyline2d::isClosed() const
return closed_;
}
-inline LandmarkPolyline2dPtr LandmarkPolyline2d::getMergedInLandmark() const
+inline LandmarkPolyline2dConstPtr LandmarkPolyline2d::getMergedInLandmark() const
+{
+ // recursive call
+ if (merged_in_lmk_ != nullptr && merged_in_lmk_->getMergedInLandmark() != nullptr)
+ return merged_in_lmk_->getMergedInLandmark();
+ return merged_in_lmk_;
+}
+
+inline LandmarkPolyline2dPtr LandmarkPolyline2d::getMergedInLandmark()
{
// recursive call
if (merged_in_lmk_ != nullptr && merged_in_lmk_->getMergedInLandmark() != nullptr)
@@ -283,12 +300,12 @@ inline LandmarkPolyline2dPtr LandmarkPolyline2d::getMergedInLandmark() const
return merged_in_lmk_;
}
-inline bool LandmarkPolyline2d::isDefined(StateBlockPtr _state_block) const
+inline bool LandmarkPolyline2d::isDefined(StateBlockConstPtr _state_block) const
{
- if (_state_block == point_state_ptr_map_.begin()->second)
+ if (_state_block == point_state_map_.begin()->second)
return first_defined_;
- if (_state_block == point_state_ptr_map_.rbegin()->second)
+ if (_state_block == point_state_map_.rbegin()->second)
return last_defined_;
return true;
@@ -296,18 +313,18 @@ inline bool LandmarkPolyline2d::isDefined(StateBlockPtr _state_block) const
inline int LandmarkPolyline2d::getNPoints() const
{
- return (int)point_state_ptr_map_.size();
+ return (int)point_state_map_.size();
}
inline int LandmarkPolyline2d::getNDefinedPoints() const
{
- return (int)point_state_ptr_map_.size() - (first_defined_ ? 0 : 1) - (last_defined_ ? 0 : 1);
+ return (int)point_state_map_.size() - (first_defined_ ? 0 : 1) - (last_defined_ ? 0 : 1);
}
inline std::vector<int> LandmarkPolyline2d::getValidIds() const
{
std::vector<int> valid_ids;
- for (auto st_pair : point_state_ptr_map_)
+ for (auto st_pair : point_state_map_)
valid_ids.push_back(st_pair.first);
return valid_ids;
@@ -316,17 +333,17 @@ inline std::vector<int> LandmarkPolyline2d::getValidIds() const
inline int LandmarkPolyline2d::id2idx(int id) const
{
assert(isValidId(id));
- return std::distance(point_state_ptr_map_.begin(),point_state_ptr_map_.find(id));
+ return std::distance(point_state_map_.begin(),point_state_map_.find(id));
}
inline int LandmarkPolyline2d::getFirstId() const
{
- return point_state_ptr_map_.begin()->first;
+ return point_state_map_.begin()->first;
}
inline int LandmarkPolyline2d::getLastId() const
{
- return point_state_ptr_map_.rbegin()->first;
+ return point_state_map_.rbegin()->first;
}
@@ -337,7 +354,7 @@ inline int LandmarkPolyline2d::getNextValidId(const int& i) const
if (i == getLastId() && closed_)
return getFirstId();
- return std::next(point_state_ptr_map_.find(i))->first;
+ return std::next(point_state_map_.find(i))->first;
}
inline int LandmarkPolyline2d::getPrevValidId(const int& i) const
@@ -347,7 +364,7 @@ inline int LandmarkPolyline2d::getPrevValidId(const int& i) const
if (i == getFirstId() && closed_)
return getLastId();
- return std::prev(point_state_ptr_map_.find(i))->first;
+ return std::prev(point_state_map_.find(i))->first;
}
inline int LandmarkPolyline2d::getVersion() const
@@ -355,15 +372,6 @@ inline int LandmarkPolyline2d::getVersion() const
return version_;
}
-inline std::vector<StateBlockPtr> LandmarkPolyline2d::getPointsStateBlockVector() const
-{
- std::vector<StateBlockPtr> state_block_vec;
- for (auto st_pair : point_state_ptr_map_)
- state_block_vec.push_back(st_pair.second);
-
- return state_block_vec;
-}
-
inline void LandmarkPolyline2d::classify(PolylineRectangularClass _class)
{
classification_ = _class;
diff --git a/include/laser/processor/processor_loop_closure_falko.h b/include/laser/processor/processor_loop_closure_falko.h
index 755b702fa89231af99f6b43330a6393e40bb1e7a..d05b1f5d083aee9bf1fb05bf1abfd5492d6bda28 100644
--- a/include/laser/processor/processor_loop_closure_falko.h
+++ b/include/laser/processor/processor_loop_closure_falko.h
@@ -604,7 +604,7 @@ class ProcessorLoopClosureFalko : public ProcessorLoopClosure
int init_outData_;
};
-WOLF_STRUCT_PTR_TYPEDEFS(ProcessorLoopClosureFalkoNnBsc);
+WOLF_PTR_TYPEDEFS(ProcessorLoopClosureFalkoNnBsc);
/** \brief A class that implements the loop closure with BSC descriptors and NN matcher
**/
@@ -621,7 +621,7 @@ class ProcessorLoopClosureFalkoNnBsc
~ProcessorLoopClosureFalkoNnBsc() = default;
};
-WOLF_STRUCT_PTR_TYPEDEFS(ProcessorLoopClosureFalkoAhtBsc);
+WOLF_PTR_TYPEDEFS(ProcessorLoopClosureFalkoAhtBsc);
/** \brief A class that implements the loop closure with BSC descriptors and AHT matcher
**/
@@ -638,7 +638,7 @@ class ProcessorLoopClosureFalkoAhtBsc : public ProcessorLoopClosureFalko<lasersc
~ProcessorLoopClosureFalkoAhtBsc() = default;
};
-WOLF_STRUCT_PTR_TYPEDEFS(ProcessorLoopClosureFalkoNnCgh);
+WOLF_PTR_TYPEDEFS(ProcessorLoopClosureFalkoNnCgh);
/** \brief A class that implements the loop closure with CGH descriptors and NN matcher
**/
@@ -655,7 +655,7 @@ class ProcessorLoopClosureFalkoNnCgh : public ProcessorLoopClosureFalko<lasersca
~ProcessorLoopClosureFalkoNnCgh() = default;
};
-WOLF_STRUCT_PTR_TYPEDEFS(ProcessorLoopClosureFalkoAhtCgh);
+WOLF_PTR_TYPEDEFS(ProcessorLoopClosureFalkoAhtCgh);
/** \brief A class that implements the loop closure with CGH descriptors and AHT matcher
**/
diff --git a/include/laser/processor/processor_loop_closure_icp.h b/include/laser/processor/processor_loop_closure_icp.h
index 5741d1110f42beca9cf2c473917bde618d164119..f2324d07117dfc842aefd4c02befd2c9f7f47cbc 100644
--- a/include/laser/processor/processor_loop_closure_icp.h
+++ b/include/laser/processor/processor_loop_closure_icp.h
@@ -138,7 +138,7 @@ class ProcessorLoopClosureIcp : public ProcessorLoopClosure
*/
std::map<double,MatchLoopClosurePtr> findLoopClosures(CaptureBasePtr _capture) override;
- MatchLoopClosurePtr matchScans(CaptureLaser2dPtr cap_ref, CaptureLaser2dPtr cap_target) const;
+ MatchLoopClosurePtr matchScans(CaptureLaser2dPtr cap_ref, CaptureLaser2dPtr cap_target);
/** \brief validates a loop closure
*/
@@ -150,7 +150,7 @@ class ProcessorLoopClosureIcp : public ProcessorLoopClosure
/** \brief returns a list containing the loop closure frame candidates list
*/
- FrameBasePtrList generateSearchList(CaptureBasePtr) const;
+ FrameBasePtrList generateSearchList(CaptureBasePtr);
};
}
diff --git a/src/landmark/landmark_polyline_2d.cpp b/src/landmark/landmark_polyline_2d.cpp
index bdd6566a85dcf025d6053ce62cfc7b47ab2e98a4..7be128506e5bd3451cb5399bf68b3e9d0c22139e 100644
--- a/src/landmark/landmark_polyline_2d.cpp
+++ b/src/landmark/landmark_polyline_2d.cpp
@@ -44,31 +44,25 @@ LandmarkPolyline2d::LandmarkPolyline2d(StateBlockPtr _p_ptr, StateBlockPtr _o_pt
{
}
-//<<<<<<< HEAD
LandmarkPolyline2d::LandmarkPolyline2d(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, const Eigen::MatrixXd& _points, const bool _first_defined, const bool _last_defined, unsigned int _first_id, PolylineRectangularClass _class) :
LandmarkBase("LandmarkPolyline2d", _p_ptr, _o_ptr), first_defined_(_first_defined), last_defined_(_last_defined), closed_(false), classification_(_class), version_(0), merged_in_lmk_(nullptr)
-//=======
-//LandmarkPolyline2d::LandmarkPolyline2d(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, const Eigen::MatrixXd& _points, const bool _first_defined, const bool _last_defined, unsigned int _first_id, PolylineRectangularClass _class) :
-// LandmarkBase("POLYLINE 2d", _p_ptr, _o_ptr), first_defined_(_first_defined), last_defined_(_last_defined), closed_(false), classification_(_class), version_(0), merged_in_lmk_(nullptr)
-//>>>>>>> 8-replace-scalar-to-double
{
//std::cout << "LandmarkPolyline2d::LandmarkPolyline2d" << std::endl;
assert(_points.cols() >= 2 && "2 points at least needed.");
for (auto i = 0; i < _points.cols(); i++)
- point_state_ptr_map_[i+_first_id] = std::make_shared<StateBlock>(_points.col(i).head<2>());
+ point_state_map_[i+_first_id] = std::make_shared<StateBlock>(_points.col(i).head<2>());
if (!first_defined_)
- firstStateBlock()->setLocalParametrization(std::make_shared<LocalParametrizationPolylineExtreme>(std::next(point_state_ptr_map_.begin())->second));
+ firstStateBlock()->setLocalParametrization(std::make_shared<LocalParametrizationPolylineExtreme>(std::next(point_state_map_.begin())->second));
if (!last_defined_)
- lastStateBlock()->setLocalParametrization(std::make_shared<LocalParametrizationPolylineExtreme>(std::next(point_state_ptr_map_.rbegin())->second));
+ lastStateBlock()->setLocalParametrization(std::make_shared<LocalParametrizationPolylineExtreme>(std::next(point_state_map_.rbegin())->second));
//std::cout << "LandmarkPolyline2d " << id() << " created. First: " << getFirstId() << " last: "<< getLastId() << std::endl;
}
LandmarkPolyline2d::~LandmarkPolyline2d()
{
- removeStateBlocks();
}
void LandmarkPolyline2d::setFirst(const Eigen::VectorXd& _point, bool _defined)
@@ -91,16 +85,16 @@ void LandmarkPolyline2d::setLast(const Eigen::VectorXd& _point, bool _defined)
defineLast();
}
-const Eigen::VectorXd LandmarkPolyline2d::getPointVector(int _i) const
+Eigen::VectorXd LandmarkPolyline2d::getPointVector(int _i) const
{
- assert(point_state_ptr_map_.find(_i) != point_state_ptr_map_.end());
- return point_state_ptr_map_.at(_i)->getState();
+ assert(point_state_map_.find(_i) != point_state_map_.end());
+ return point_state_map_.at(_i)->getState();
}
StateBlockPtr LandmarkPolyline2d::getPointStateBlock(int _i)
{
- assert(point_state_ptr_map_.find(_i) != point_state_ptr_map_.end());
- return point_state_ptr_map_.at(_i);
+ assert(point_state_map_.find(_i) != point_state_map_.end());
+ return point_state_map_.at(_i);
}
Eigen::MatrixXd LandmarkPolyline2d::computePointsGlobal() const
@@ -155,8 +149,6 @@ Eigen::MatrixXd LandmarkPolyline2d::computePointsGlobal(const int& _from_id, con
return points_global;
}
-
-
Eigen::MatrixXd LandmarkPolyline2d::computePointsCovGlobal() const
{
Eigen::MatrixXd points_cov_global = Eigen::MatrixXd::Zero(2,2*getNPoints());
@@ -173,6 +165,17 @@ Eigen::MatrixXd LandmarkPolyline2d::computePointsCovGlobal() const
return points_cov_global;
}
+char LandmarkPolyline2d::idToChar(int id) const
+{
+ if (id < 0)
+ id += available_chars.size();
+
+ if (id < 0 or id >= available_chars.size())
+ throw std::runtime_error("Too many points in LandmarkPolyline2d");
+
+ return available_chars[id];
+}
+
void LandmarkPolyline2d::addPoint(const Eigen::VectorXd& _point, const bool& _defined, const bool& _back)
{
assert(!closed_ && "adding point to a closed polyline!");
@@ -192,10 +195,9 @@ void LandmarkPolyline2d::addPoint(const Eigen::VectorXd& _point, const bool& _de
(!_defined ?
std::make_shared<LocalParametrizationPolylineExtreme>(lastStateBlock()) :
nullptr));
- point_state_ptr_map_[getLastId()+1]= new_sb_ptr;
-
- if (getProblem())
- getProblem()->notifyStateBlock(new_sb_ptr,ADD);
+ auto point_id = getLastId()+1;
+ point_state_map_[point_id]= new_sb_ptr;
+ HasStateBlocks::addStateBlock(idToChar(point_id),new_sb_ptr,getProblem());
last_defined_ = _defined;
}
@@ -206,10 +208,9 @@ void LandmarkPolyline2d::addPoint(const Eigen::VectorXd& _point, const bool& _de
(!_defined ?
std::make_shared<LocalParametrizationPolylineExtreme>(firstStateBlock()) :
nullptr));
- point_state_ptr_map_[getFirstId()-1] = new_sb_ptr;
-
- if (getProblem())
- getProblem()->notifyStateBlock(new_sb_ptr,ADD);
+ auto point_id = getLastId()-1;
+ point_state_map_[point_id] = new_sb_ptr;
+ HasStateBlocks::addStateBlock(idToChar(point_id),new_sb_ptr,getProblem());
first_defined_ = _defined;
}
@@ -241,10 +242,9 @@ void LandmarkPolyline2d::addPoints(const Eigen::MatrixXd& _points, const unsigne
(i == _points.cols()-1 && !_defined ?
std::make_shared<LocalParametrizationPolylineExtreme>(lastStateBlock()) :
nullptr));
- point_state_ptr_map_[getLastId()+1] = new_sb_ptr;
-
- if (getProblem())
- getProblem()->notifyStateBlock(new_sb_ptr,ADD);
+ auto point_id = getLastId()+1;
+ point_state_map_[point_id]= new_sb_ptr;
+ HasStateBlocks::addStateBlock(idToChar(point_id),new_sb_ptr,getProblem());
}
last_defined_ = _defined;
}
@@ -257,10 +257,9 @@ void LandmarkPolyline2d::addPoints(const Eigen::MatrixXd& _points, const unsigne
(i == 0 && !_defined ?
std::make_shared<LocalParametrizationPolylineExtreme>(firstStateBlock()) :
nullptr));
- point_state_ptr_map_[getFirstId()-1] = new_sb_ptr;
-
- if (getProblem())
- getProblem()->notifyStateBlock(new_sb_ptr,ADD);
+ auto point_id = getLastId()-1;
+ point_state_map_[point_id] = new_sb_ptr;
+ HasStateBlocks::addStateBlock(idToChar(point_id),new_sb_ptr,getProblem());
}
first_defined_ = _defined;
}
@@ -481,16 +480,13 @@ void LandmarkPolyline2d::setClosed()
void LandmarkPolyline2d::mergePoints(int _remove_id, int _remain_id)
{
- assert(point_state_ptr_map_.find(_remove_id) != point_state_ptr_map_.end() && "removing an inexistent point");
- assert(point_state_ptr_map_.find(_remain_id) != point_state_ptr_map_.end() && "merging an inexistent point");
+ assert(point_state_map_.find(_remove_id) != point_state_map_.end() && "removing an inexistent point");
+ assert(point_state_map_.find(_remain_id) != point_state_map_.end() && "merging an inexistent point");
assert(!(_remain_id == getLastId() && !last_defined_) && "in merging points, the remaining point must be defined");
assert(!(_remain_id == getFirstId() && !first_defined_) && "in merging points, the remaining point must be defined");
//std::cout << "merge points: remove " << _remove_id << " and keep " << _remain_id << " (ids: " << getFirstId() << " to " << getLastId() << ")" << std::endl;
- StateBlockPtr remove_state = getPointStateBlock(_remove_id);
- //std::cout << "state block to remove " << remove_state->getState().transpose() << std::endl;
-
// Change factors from remove_state to remain_state
FactorBasePtrList old_factors_list = getConstrainedByList();
//std::cout << "changing " << old_factors_list.size() << " factors." << std::endl;
@@ -565,12 +561,11 @@ void LandmarkPolyline2d::mergePoints(int _remove_id, int _remain_id)
last_defined_ = true;
// Remove remove_state
- if (getProblem() != nullptr)
- getProblem()->notifyStateBlock(remove_state, REMOVE);
+ HasStateBlocks::removeStateBlock(idToChar(_remove_id));
//std::cout << "state removed " << std::endl;
// remove element from deque
- point_state_ptr_map_.erase(_remove_id);
+ point_state_map_.erase(_remove_id);
//std::cout << "state removed from point vector " << std::endl;
// new version
@@ -774,7 +769,7 @@ bool LandmarkPolyline2d::tryClassify(const double& _dist_tol, std::vector<Polyli
getPointStateBlock(points_ids[3])->setState(Eigen::Vector2d( classification_.L / 2,-classification_.W / 2));
}
- for (auto st_pair : point_state_ptr_map_)
+ for (auto st_pair : point_state_map_)
st_pair.second->fix();
return true;
@@ -933,27 +928,6 @@ void LandmarkPolyline2d::mergeLandmark(const LandmarkPolyline2dPtr _merged_lmk,
//std::cout << "\tLandmark deleted\n";
}
-void LandmarkPolyline2d::registerNewStateBlocks() const
-{
- LandmarkBase::registerNewStateBlocks(getProblem());
- if (getProblem())
- for (auto state_it : point_state_ptr_map_)
- getProblem()->notifyStateBlock(state_it.second, ADD);
-}
-
-void LandmarkPolyline2d::removeStateBlocks()
-{
- auto sbp_pair = point_state_ptr_map_.begin();
- while (sbp_pair != point_state_ptr_map_.end())
- {
- if (getProblem())
- getProblem()->notifyStateBlock(sbp_pair->second, REMOVE);
-
- sbp_pair = point_state_ptr_map_.erase(sbp_pair);
- }
- LandmarkBase::removeStateBlocks(getProblem());
-}
-
// static
LandmarkBasePtr LandmarkPolyline2d::create(const YAML::Node& _lmk_node)
{
@@ -989,7 +963,7 @@ LandmarkBasePtr LandmarkPolyline2d::create(const YAML::Node& _lmk_node)
lmk_ptr->setId(id);
// fix all points
- for (auto st_pair : lmk_ptr->getPointStatePtrMap())
+ for (auto st_pair : lmk_ptr->getPointStateMap())
st_pair.second->fix();
return lmk_ptr;
@@ -1008,8 +982,8 @@ YAML::Node LandmarkPolyline2d::saveToYaml() const
node["classification_L"] = classification_.L;
node["classification_W"] = classification_.W;
- for (auto st : getPointsStateBlockVector())
- node["points"].push_back(st->getState());
+ for (auto st_id : getValidIds())
+ node["points"].push_back(getPointVector(st_id));
return node;
}
diff --git a/src/processor/processor_loop_closure_falko.cpp b/src/processor/processor_loop_closure_falko.cpp
index 92ba29fa13ecaecfb684287e56426c4102ecc467..91259910eba66e2baf445fa5a01a26d7b3ff0db5 100644
--- a/src/processor/processor_loop_closure_falko.cpp
+++ b/src/processor/processor_loop_closure_falko.cpp
@@ -20,6 +20,7 @@
//
//--------LICENSE_END--------
#include "laser/processor/processor_loop_closure_falko.h"
+
// Register in the FactorySensor
#include "core/processor/factory_processor.h"
namespace wolf {
diff --git a/src/processor/processor_loop_closure_icp.cpp b/src/processor/processor_loop_closure_icp.cpp
index a57559753b2cd4cf8503ab1f10a422ba18b52dfc..09d35ccdf3fcc2eb75ced42a97da1d4844b798db 100644
--- a/src/processor/processor_loop_closure_icp.cpp
+++ b/src/processor/processor_loop_closure_icp.cpp
@@ -113,7 +113,7 @@ std::map<double,MatchLoopClosurePtr> ProcessorLoopClosureIcp::findLoopClosures(C
return match_lc_map;
}
-MatchLoopClosurePtr ProcessorLoopClosureIcp::matchScans(CaptureLaser2dPtr cap_ref, CaptureLaser2dPtr cap_tar) const
+MatchLoopClosurePtr ProcessorLoopClosureIcp::matchScans(CaptureLaser2dPtr cap_ref, CaptureLaser2dPtr cap_tar)
{
auto frm_tar = cap_tar->getFrame();
auto frm_ref = cap_ref->getFrame();
@@ -212,18 +212,18 @@ void ProcessorLoopClosureIcp::emplaceFactors(MatchLoopClosurePtr match)
last_loop_closure_ = match;
}
-FrameBasePtrList ProcessorLoopClosureIcp::generateSearchList(CaptureBasePtr _cap) const
+FrameBasePtrList ProcessorLoopClosureIcp::generateSearchList(CaptureBasePtr _cap)
{
assert(_cap->getFrame() and _cap->getFrame()->getTrajectory());
- int N = getProblem()->getTrajectory()->getFrameMap().size() - 1 - params_loop_closure_icp_->recent_frames_ignored;
+ int N = getProblem()->getTrajectory()->size() - 1 - params_loop_closure_icp_->recent_frames_ignored;
WOLF_DEBUG("ProcessorLoopClosureIcp::generateSearchList N = ", N);
if (N <= 0)
return FrameBasePtrList();
- auto map_begin = getProblem()->getTrajectory()->getFrameMap().begin();
+ auto first_frame = getProblem()->getTrajectory()->getFirstFrame();
//std::vector<int> idxs;
std::vector<bool> done(N, false);
@@ -241,10 +241,10 @@ FrameBasePtrList ProcessorLoopClosureIcp::generateSearchList(CaptureBasePtr _cap
// initialize with first: 0
done.at(0) = true;
- if (not map_begin->second->getCapturesOfType<CaptureLaser2d>().empty())
+ if (not first_frame->getCapturesOfType<CaptureLaser2d>().empty())
{
//idxs.push_back(0);
- frame_list.push_back(map_begin->second);
+ frame_list.push_back(first_frame);
}
// continue with tree
@@ -262,7 +262,7 @@ FrameBasePtrList ProcessorLoopClosureIcp::generateSearchList(CaptureBasePtr _cap
{
done.at(idx) = true;
checked++;
- auto frame_idx = std::next(map_begin, idx)->second;
+ auto frame_idx = first_frame->getNextFrame(idx);
assert(frame_idx);
assert(frame_idx != _cap->getFrame());
if (not frame_idx->getCapturesOfType<CaptureLaser2d>().empty())
@@ -297,7 +297,7 @@ FrameBasePtrList ProcessorLoopClosureIcp::generateSearchList(CaptureBasePtr _cap
checked++;
// consider adding
- auto frame_idx = std::next(map_begin, idx)->second;
+ auto frame_idx = first_frame->getNextFrame(idx);
assert(frame_idx);
assert(frame_idx != _cap->getFrame());
if (not frame_idx->getCapturesOfType<CaptureLaser2d>().empty())
diff --git a/src/processor/processor_tracker_feature_polyline_2d.cpp b/src/processor/processor_tracker_feature_polyline_2d.cpp
index be4e0ad864a4d5ec1aafd8d9b71aa6f5bf90f3a8..d5a6c8ce3958b1fada4636b17c136818231ffc30 100644
--- a/src/processor/processor_tracker_feature_polyline_2d.cpp
+++ b/src/processor/processor_tracker_feature_polyline_2d.cpp
@@ -622,7 +622,7 @@ LandmarkBasePtr ProcessorTrackerFeaturePolyline2d::emplaceLandmark(FeatureBasePt
//std::cout << "Robot global pose: " << t_world_robot_.transpose() << std::endl;
//std::cout << "Sensor global pose: " << t_world_sensor_.transpose() << std::endl;
assert(_feature_ptr->getType() == "POLYLINE 2d");
- assert(std::find(last_ptr_->getFeatureList().begin(),last_ptr_->getFeatureList().end(),_feature_ptr) != last_ptr_->getFeatureList().end() && "creating a landmark for a feature which is not in last capture");
+ assert(last_ptr_->hasFeature(_feature_ptr) && "creating a landmark for a feature which is not in last capture");
FeaturePolyline2dPtr polyline_ptr = std::static_pointer_cast<FeaturePolyline2d>(_feature_ptr);
diff --git a/test/gtest_landmark_polyline.cpp b/test/gtest_landmark_polyline.cpp
index d84c18d1ac15c24aac47545cae3513561b823aca..93dd5c8fc27e83ea3685b9ab4bfd2f742c9e234f 100644
--- a/test/gtest_landmark_polyline.cpp
+++ b/test/gtest_landmark_polyline.cpp
@@ -143,7 +143,7 @@ TEST_F(LandmarkPolylineTest, Classify)
// test points fixed and center unfixed
ASSERT_FALSE(lmk_ptr->getP()->isFixed());
ASSERT_FALSE(lmk_ptr->getO()->isFixed());
- for (auto st_pair : lmk_ptr->getPointStatePtrMap())
+ for (auto st_pair : lmk_ptr->getPointStateMap())
ASSERT_TRUE(st_pair.second->isFixed());
// test points are in the same place (compoping with object pose)
diff --git a/test/gtest_processor_odom_icp.cpp b/test/gtest_processor_odom_icp.cpp
index b0f24d0d3bbedd22b82cda213ec0499925ea2669..57c7b06ba718af26849a25c57d594283b6459ce5 100644
--- a/test/gtest_processor_odom_icp.cpp
+++ b/test/gtest_processor_odom_icp.cpp
@@ -199,15 +199,15 @@ TEST_F(ProcessorOdomIcp_Test, solve)
t += 1.0;
}
- for (auto F : *problem->getTrajectory())
- F->perturb(0.5);
+ for (auto F_pair : problem->getTrajectory()->getFrameMap())
+ F_pair.second->perturb(0.5);
std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
WOLF_TRACE(report);
- for (auto F : *problem->getTrajectory())
+ for (auto F_pair : problem->getTrajectory()->getFrameMap())
{
- ASSERT_MATRIX_APPROX(F->getState().vector("PO") , x0.vector("PO") , 1e-6);
+ ASSERT_MATRIX_APPROX(F_pair.second->getState().vector("PO") , x0.vector("PO") , 1e-6);
}
}
@@ -242,15 +242,15 @@ TEST_F(ProcessorOdomIcp_Test, solve_3D)
t += 1.0;
}
- for (auto F : *problem->getTrajectory())
- F->perturb(0.5);
+ for (auto F_pair : problem->getTrajectory()->getFrameMap())
+ F_pair.second->perturb(0.5);
- std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
+ std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
WOLF_TRACE(report);
- for (auto F : *problem->getTrajectory())
+ for (auto F_pair : problem->getTrajectory()->getFrameMap())
{
- ASSERT_MATRIX_APPROX(F->getState().vector("PO") , x0.vector("PO") , 1e-6);
+ ASSERT_MATRIX_APPROX(F_pair.second->getState().vector("PO") , x0.vector("PO") , 1e-6);
}
}