From 8ec2a3e3ec1c8c9461ba4a5a2be754df21a6b6bc Mon Sep 17 00:00:00 2001
From: joanvallve <jvallve@iri.upc.edu>
Date: Mon, 12 Aug 2019 10:34:40 +0200
Subject: [PATCH] points of polyline in state_block_vector_
---
include/laser/landmark/landmark_polyline_2D.h | 4 +-
src/landmark/landmark_polyline_2D.cpp | 87 +++++++++++--------
2 files changed, 54 insertions(+), 37 deletions(-)
diff --git a/include/laser/landmark/landmark_polyline_2D.h b/include/laser/landmark/landmark_polyline_2D.h
index cac3d358e..a40e68472 100644
--- a/include/laser/landmark/landmark_polyline_2D.h
+++ b/include/laser/landmark/landmark_polyline_2D.h
@@ -200,8 +200,8 @@ class LandmarkPolyline2D : public LandmarkBase
/** \brief Adds all stateBlocks of the frame to the wolfProblem list of new stateBlocks
**/
- virtual void registerNewStateBlocks();
- virtual void removeStateBlocks();
+// virtual void registerNewStateBlocks();
+// virtual void removeStateBlocks();
/** Factory method to create new landmarks from YAML nodes
*/
diff --git a/src/landmark/landmark_polyline_2D.cpp b/src/landmark/landmark_polyline_2D.cpp
index eb1d75fb5..1265a9a03 100644
--- a/src/landmark/landmark_polyline_2D.cpp
+++ b/src/landmark/landmark_polyline_2D.cpp
@@ -24,13 +24,23 @@ LandmarkPolyline2D::LandmarkPolyline2D(StateBlockPtr _p_ptr, StateBlockPtr _o_pt
}
LandmarkPolyline2D::LandmarkPolyline2D(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, const Eigen::MatrixXs& _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)
+ 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)
{
//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>());
+ {
+ auto new_sb = std::make_shared<StateBlock>(_points.col(i).head<2>());
+ point_state_ptr_map_[i+_first_id] = new_sb;
+ getStateBlockVec().push_back(new_sb);
+ }
if (!first_defined_)
firstStateBlock()->setLocalParametrization(std::make_shared<LocalParametrizationPolylineExtreme>(std::next(point_state_ptr_map_.begin())->second));
@@ -129,8 +139,6 @@ Eigen::MatrixXs LandmarkPolyline2D::computePointsGlobal(const int& _from_id, con
return points_global;
}
-
-
Eigen::MatrixXs LandmarkPolyline2D::computePointsCovGlobal() const
{
Eigen::MatrixXs points_cov_global = Eigen::MatrixXs::Zero(2,2*getNPoints());
@@ -167,6 +175,7 @@ void LandmarkPolyline2D::addPoint(const Eigen::VectorXs& _point, const bool& _de
std::make_shared<LocalParametrizationPolylineExtreme>(lastStateBlock()) :
nullptr));
point_state_ptr_map_[getLastId()+1]= new_sb_ptr;
+ getStateBlockVec().push_back(new_sb_ptr);
if (getProblem())
getProblem()->notifyStateBlock(new_sb_ptr,ADD);
@@ -181,6 +190,7 @@ void LandmarkPolyline2D::addPoint(const Eigen::VectorXs& _point, const bool& _de
std::make_shared<LocalParametrizationPolylineExtreme>(firstStateBlock()) :
nullptr));
point_state_ptr_map_[getFirstId()-1] = new_sb_ptr;
+ getStateBlockVec().push_back(new_sb_ptr);
if (getProblem())
getProblem()->notifyStateBlock(new_sb_ptr,ADD);
@@ -216,6 +226,7 @@ void LandmarkPolyline2D::addPoints(const Eigen::MatrixXs& _points, const unsigne
std::make_shared<LocalParametrizationPolylineExtreme>(lastStateBlock()) :
nullptr));
point_state_ptr_map_[getLastId()+1] = new_sb_ptr;
+ getStateBlockVec().push_back(new_sb_ptr);
if (getProblem())
getProblem()->notifyStateBlock(new_sb_ptr,ADD);
@@ -232,6 +243,7 @@ void LandmarkPolyline2D::addPoints(const Eigen::MatrixXs& _points, const unsigne
std::make_shared<LocalParametrizationPolylineExtreme>(firstStateBlock()) :
nullptr));
point_state_ptr_map_[getFirstId()-1] = new_sb_ptr;
+ getStateBlockVec().push_back(new_sb_ptr);
if (getProblem())
getProblem()->notifyStateBlock(new_sb_ptr,ADD);
@@ -291,7 +303,7 @@ bool LandmarkPolyline2D::tryClose(const Scalar& _dist_tol)
return false;
}
- WOLF_TRACE("tryClose landmark ",id()," point ids: ");
+ WOLF_DEBUG("tryClose landmark ",id()," point ids: ");
for (auto id : getValidIds())
std::cout << id << " ";
std::cout << std::endl;
@@ -311,7 +323,7 @@ bool LandmarkPolyline2D::tryClose(const Scalar& _dist_tol)
// last_with_id is the point overlapped with the last defined point, it CAN'T be overlapped with with_point_id since it's already overlapped with first defined point
while (with_point_id > last_with_id)
{
- WOLF_TRACE("first_point_id: ",first_point_id,"\nwith_point_id: ",with_point_id,"\nlast_with_id: ", last_with_id);
+ WOLF_DEBUG("first_point_id: ",first_point_id,"\nwith_point_id: ",with_point_id,"\nlast_with_id: ", last_with_id);
if ((getPointVector(with_point_id)-getPointVector(first_point_id)).squaredNorm() < _dist_tol*_dist_tol)
{
found = true;
@@ -325,7 +337,7 @@ bool LandmarkPolyline2D::tryClose(const Scalar& _dist_tol)
// IF MATCH FOUND CHECK THE WHOLE OVERLAPPING
if (found)
{
- WOLF_TRACE("good point match first: ", first_point_id, " with: ", with_point_id);
+ WOLF_DEBUG("good point match first: ", first_point_id, " with: ", with_point_id);
// COMPUTE OVERLAPPING POINTS
// if first not defined -> add previous overlapping point
@@ -368,12 +380,12 @@ bool LandmarkPolyline2D::tryClose(const Scalar& _dist_tol)
if (sq_dist > _dist_tol*_dist_tol)
{
- WOLF_TRACE("\nBad overlapping: ", sq_dist,
+ WOLF_DEBUG("\nBad overlapping: ", sq_dist,
"\n\tpoint\t", overlapped_ids[i].first, " (", getPointVector(overlapped_ids[i].first).transpose(), ")", (overlapped_ids[i].first == getFirstId() && !first_defined_ ? "NOT DEFINED" : ""),
"\n\twith \t", overlapped_ids[i].second," (", getPointVector(overlapped_ids[i].second).transpose(),")", (overlapped_ids[i].second == getLastId() && !last_defined_ ? "NOT DEFINED" : ""));
return false;
}
- WOLF_TRACE("\nGood overlapping: ", sq_dist,
+ WOLF_DEBUG("\nGood overlapping: ", sq_dist,
"\n\tpoint\t", overlapped_ids[i].first, " (", getPointVector(overlapped_ids[i].first).transpose(), ")", (overlapped_ids[i].first == getFirstId() && !first_defined_ ? "NOT DEFINED" : ""),
"\n\twith \t", overlapped_ids[i].second," (", getPointVector(overlapped_ids[i].second).transpose(),")", (overlapped_ids[i].second == getLastId() && !last_defined_ ? "NOT DEFINED" : ""));
}
@@ -384,7 +396,7 @@ bool LandmarkPolyline2D::tryClose(const Scalar& _dist_tol)
// Trying to close overlapping both not defined extremes
if (overlapped_ids.empty() && !first_defined_ && !last_defined_)
{
- WOLF_TRACE("No overlapping found with defined points, trying with not defined extremes");
+ WOLF_DEBUG("No overlapping found with defined points, trying with not defined extremes");
// Check for both not defined extremes that the distance to the segment of first and last defined points is below the threshold
Eigen::Vector2s first_not_def = getPointVector(getFirstId());
@@ -395,7 +407,7 @@ bool LandmarkPolyline2D::tryClose(const Scalar& _dist_tol)
if(sqDistPoint2Segment(first_not_def, last_def, first_def) < _dist_tol*_dist_tol &&
sqDistPoint2Segment(last_not_def, last_def, first_def) < _dist_tol*_dist_tol)
{
- WOLF_TRACE("The two not defined extremes close to the segment defined first_defined and last_defined:",
+ WOLF_DEBUG("The two not defined extremes close to the segment defined first_defined and last_defined:",
"\n\tfirst point: ", first_not_def.transpose(),
"\n\tlast point: ", last_not_def.transpose(),
"\n\tfirst defined point: ", first_def.transpose(),
@@ -409,7 +421,7 @@ bool LandmarkPolyline2D::tryClose(const Scalar& _dist_tol)
if (last_proj_2_first_def < first_proj_2_first_def)
{
- WOLF_TRACE("The not defined extremes are overlapped",
+ WOLF_DEBUG("The not defined extremes are overlapped",
"\n\tlast_proj_2_first_def: ", last_proj_2_first_def,
"\n\tfirst_proj_2_first_def: ", first_proj_2_first_def);
overlapped_ids.push_back(std::pair<int,int>(getFirstId(), getLastId()));
@@ -420,7 +432,7 @@ bool LandmarkPolyline2D::tryClose(const Scalar& _dist_tol)
// IF ANY OVERLAPPING -> CLOSE AND MERGE OVERLAPPED POINTS
if (!overlapped_ids.empty())
{
- WOLF_TRACE("All overlappings OK, merging ", overlapped_ids.size(), " points");
+ WOLF_DEBUG("All overlappings OK, merging ", overlapped_ids.size(), " points");
// merge points
while (!overlapped_ids.empty())
@@ -435,7 +447,7 @@ bool LandmarkPolyline2D::tryClose(const Scalar& _dist_tol)
// Close
setClosed();
- WOLF_TRACE("Landmark ",id()," was closed.");
+ WOLF_DEBUG("Landmark ",id()," was closed.");
return true;
}
@@ -543,8 +555,13 @@ void LandmarkPolyline2D::mergePoints(int _remove_id, int _remain_id)
getProblem()->notifyStateBlock(remove_state, REMOVE);
//std::cout << "state removed " << std::endl;
- // remove element from deque
+ // remove stateblock from map
point_state_ptr_map_.erase(_remove_id);
+ // remove state block from state_block_vec
+ auto sb_it = std::find(getStateBlockVec().begin(), getStateBlockVec().end(), remove_state);
+ assert(sb_it != getStateBlockVec().end());
+ getStateBlockVec().erase(sb_it);
+
//std::cout << "state removed from point vector " << std::endl;
// new version
@@ -907,26 +924,26 @@ void LandmarkPolyline2D::mergeLandmark(const LandmarkPolyline2DPtr _merged_lmk,
//std::cout << "\tLandmark deleted\n";
}
-void LandmarkPolyline2D::registerNewStateBlocks()
-{
- LandmarkBase::registerNewStateBlocks();
- 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();
-}
+//void LandmarkPolyline2D::registerNewStateBlocks()
+//{
+// LandmarkBase::registerNewStateBlocks();
+// 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();
+//}
// static
LandmarkBasePtr LandmarkPolyline2D::create(const YAML::Node& _lmk_node)
--
GitLab