Skip to content
Snippets Groups Projects
Commit 8ec2a3e3 authored by Joan Vallvé Navarro's avatar Joan Vallvé Navarro
Browse files

points of polyline in state_block_vector_

parent 44669449
No related branches found
No related tags found
No related merge requests found
......@@ -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
*/
......
......@@ -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)
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment