Commit 9749c244 authored by Alejandro Lopez Gestoso's avatar Alejandro Lopez Gestoso
Browse files

Fixed bug when time persistance filter was disabled

parent 92cc12b6
......@@ -126,11 +126,9 @@ bool AdcTagLocalizationFilterAlgNode::check_tag_id(int _id)
void AdcTagLocalizationFilterAlgNode::prepare_feature_candidates()
{
//Remove not detected candidates
if (!this->config_.time_persistance_filter_en)
return;
for (auto it = this->candidates_.begin(); it != this->candidates_.end(); ++it)
{
if (it->detected)
if (it->detected && this->config_.time_persistance_filter_en)
it->detected = false;
else
{
......@@ -145,7 +143,7 @@ bool AdcTagLocalizationFilterAlgNode::update_feature_candidates(double _r, doubl
{
//Check if is a new candidate or an already seen candidate
if (!this->config_.time_persistance_filter_en)
return false;
return true;
for (auto it = this->candidates_.begin(); it != this->candidates_.end(); ++it)
{
double yaw, pitch, roll, in_orientation, cand_orientation;
......@@ -210,8 +208,8 @@ void AdcTagLocalizationFilterAlgNode::publish_visualization_msg()
this->visualization_msg_.markers[i].pose.orientation.z = this->features_msg_.features[i].pose.orientation.z;
this->visualization_msg_.markers[i].pose.orientation.w = this->features_msg_.features[i].pose.orientation.w;
this->visualization_msg_.markers[i].scale.x = 0.05;
this->visualization_msg_.markers[i].scale.y = 0.05;
this->visualization_msg_.markers[i].scale.x = 0.2;
this->visualization_msg_.markers[i].scale.y = 0.2;
this->visualization_msg_.markers[i].scale.z = 0.02;
this->visualization_msg_.markers[i].color.a = 1.0;
......
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