Commit 1d551be5 authored by Alejandro Lopez Gestoso's avatar Alejandro Lopez Gestoso
Browse files

Improved closest one landmark's matching

parent b5a51194
......@@ -150,7 +150,7 @@ double AdcLandmarksSlamSolverAlgorithm::mahalanobis_distance(Eigen::Vector2d _fe
double AdcLandmarksSlamSolverAlgorithm::is_a_mapped_landmark(Eigen::Vector2d _feature_pos, double _feature_r, double _feature_th, int _type, const std::string& _source_frame)
{
double min_dist = this->config_.landmark_mahalanobis_dist + 1.0;
double min_dist = 99999999999.0;
std::map<double, LandmarkInfo>::iterator min_dist_land = this->landmarks_.end();
for (std::map<double, LandmarkInfo>::iterator it = this->landmarks_.begin(); it != this->landmarks_.end(); ++it)
{
......@@ -162,9 +162,10 @@ double AdcLandmarksSlamSolverAlgorithm::is_a_mapped_landmark(Eigen::Vector2d _fe
// std::cout << "check dist " << dist << std::endl;
if (dist <= this->config_.landmark_mahalanobis_dist)
{
if (dist < min_dist)
Eigen::Vector2d d = landmark_pos - _feature_pos;
if (d.norm() < min_dist)
{
min_dist = dist;
min_dist = d.norm();
min_dist_land = it;
}
}
......
......@@ -640,7 +640,7 @@ bool AdcLandmarksSlamSolverAlgNode::is_a_new_landmark(FeatureInfo &_feature)
//Check if a landmark candidate has been seen enough times
if (!this->config_.landmarks_candidates_filter_en)
return true;
double min_dist = this->config_.landmark_mahalanobis_dist + 1.0;
double min_dist = 999999999.0;
std::list<LandmarkCandidate>::iterator min_dist_land = this->landmarks_candidates_.end();
for (auto it = this->landmarks_candidates_.begin(); it != this->landmarks_candidates_.end(); ++it)
{
......@@ -652,9 +652,10 @@ bool AdcLandmarksSlamSolverAlgNode::is_a_new_landmark(FeatureInfo &_feature)
double dist = this->alg_.mahalanobis_distance(feature_pos, _feature.r, _feature.th, landmark_pos);
if (dist <= this->config_.landmark_mahalanobis_dist)
{
if (dist < min_dist)
Eigen::Vector2d d = landmark_pos - feature_pos;
if (d.norm() < min_dist)
{
min_dist = dist;
min_dist = d.norm();
min_dist_land = it;
}
}
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment