Commit b5a51194 authored by Alejandro Lopez Gestoso's avatar Alejandro Lopez Gestoso
Browse files

Now, when matching landmarks it takes the closest one

parent 9501d7e2
......@@ -150,6 +150,8 @@ 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;
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)
{
if (_type == it->second.type)
......@@ -160,12 +162,20 @@ double AdcLandmarksSlamSolverAlgorithm::is_a_mapped_landmark(Eigen::Vector2d _fe
// std::cout << "check dist " << dist << std::endl;
if (dist <= this->config_.landmark_mahalanobis_dist)
{
it->second.detected = true;
it->second.source_frame_id = _source_frame;
return it->first;
if (dist < min_dist)
{
min_dist = dist;
min_dist_land = it;
}
}
}
}
if (min_dist_land != this->landmarks_.end())
{
min_dist_land->second.detected = true;
min_dist_land->second.source_frame_id = _source_frame;
return min_dist_land->first;
}
return -1.0;
}
......
......@@ -256,7 +256,7 @@ void AdcLandmarksSlamSolverAlgNode::mainNodeThread(void)
{
if (!this->waiting_for_feature_)
this->update_event_t_ = this->features_stamp_;
if (this->features_.size() == 0 && (this->features_stamp_ - this->update_event_t_ < ros::Duration(this->config_.wait_feature_detected_timeout)))
if (this->features_.size() == 0 && ((this->features_stamp_ - this->update_event_t_) < ros::Duration(this->config_.wait_feature_detected_timeout)))
this->waiting_for_feature_ = true;
else
{
......@@ -640,6 +640,8 @@ 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;
std::list<LandmarkCandidate>::iterator min_dist_land = this->landmarks_candidates_.end();
for (auto it = this->landmarks_candidates_.begin(); it != this->landmarks_candidates_.end(); ++it)
{
if(_feature.type == it->type)
......@@ -650,18 +652,26 @@ 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)
{
it->detected = true;
it->source_frame_id = this->features_frame_;
it->count++;
if (it->count >= this->config_.landmarks_min_detections)
if (dist < min_dist)
{
this->landmarks_candidates_.erase(it);
return true;
min_dist = dist;
min_dist_land = it;
}
return false;
}
}
}
if (min_dist_land != this->landmarks_candidates_.end())
{
min_dist_land->detected = true;
min_dist_land->source_frame_id = this->features_frame_;
min_dist_land->count++;
if (min_dist_land->count >= this->config_.landmarks_min_detections)
{
this->landmarks_candidates_.erase(min_dist_land);
return true;
}
return false;
}
LandmarkCandidate cand;
cand.pose = _feature.pose;
cand.count = 1;
......
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