Commit 324ea85e authored by Alejandro Lopez Gestoso's avatar Alejandro Lopez Gestoso
Browse files

Changed persistance by persistence

parent 37b38937
......@@ -5,7 +5,7 @@ This node is intended to filter [ar_track_alvar](http://wiki.ros.org/ar_track_al
- **Range filter**: Defines a maximum distance to take into account an ar tag detection.
- **Confidence filter**: Defines a minimum confidence to take into account an ar tag detection. *NOTE: the ar_track_alvar node doesn't fill this parameter*.
- **ID filter**: It filters any ar tag detection with a different ID than the enabled ones.
- **Time persistance filter**: Filter that checks that an ar tag detection is detected a minimum number of times on the same region. The region is defined with a range and angular window from the last detection. An orientation filter can be enabled to check that it has a similar orientation on each detection.
- **Time persistence filter**: Filter that checks that an ar tag detection is detected a minimum number of times on the same region. The region is defined with a range and angular window from the last detection. An orientation filter can be enabled to check that it has a similar orientation on each detection.
# ROS Interface
### Topic publishers
......@@ -27,13 +27,13 @@ This node is intended to filter [ar_track_alvar](http://wiki.ros.org/ar_track_al
##### ID filter
- ~**id_filter_en** (Bool; default: False) Boolean to filter detections by tag id.
- ~**ids_enabled** (String) Comma separated ids enabled.
##### Time persistance filter
- ~**time_persistance_filter_en** (Bool; default: False) Boolean to filter detections by time persistance.
- ~**time_persistance_filter_orientation_en** (Bool; default: False) Boolean to add orientation filter to time persistance filter.
- ~**time_persistance_alpha_window** (Double; default: 0.035; min: 0.01; max: 0.07) Angle window to search around the last detection on following inputs.
- ~**time_persistance_range_window** (Double; default: 0.1; min: 0.01; max: 0.3) Range window to search around the last detection on following inputs.
- ~**time_persistance_orientation_th** (Double; default: 0.3; min: 0.01; max: 0.8) Maximum orientation diff from the last detection on following inputs.
- ~**time_persistance_min_detections** (Integer; default: 5; min: 1; max: 15) Min confidence to take into account a detection.
##### Time persistence filter
- ~**time_persistence_filter_en** (Bool; default: False) Boolean to filter detections by time persistence.
- ~**time_persistence_filter_orientation_en** (Bool; default: False) Boolean to add orientation filter to time persistence filter.
- ~**time_persistence_alpha_window** (Double; default: 0.035; min: 0.01; max: 0.07) Angle window to search around the last detection on following inputs.
- ~**time_persistence_range_window** (Double; default: 0.1; min: 0.01; max: 0.3) Range window to search around the last detection on following inputs.
- ~**time_persistence_orientation_th** (Double; default: 0.3; min: 0.01; max: 0.8) Maximum orientation diff from the last detection on following inputs.
- ~**time_persistence_min_detections** (Integer; default: 5; min: 1; max: 15) Min confidence to take into account a detection.
## Installation
......
......@@ -54,11 +54,11 @@ confidence_filter.add("detection_min_confidence", int_t, 0,
id_filter.add("id_filter_en", bool_t, 0, "Boolean to filter detections by tag id", False)
id_filter.add("ids_enabled", str_t, 0, "Comma separated ids enabled", "")
time_persistence_filter.add("time_persistance_filter_en", bool_t, 0, "Boolean to filter detections by time persistance", False)
time_persistence_filter.add("time_persistance_filter_orientation_en", bool_t, 0, "Boolean to add orientation filter to time persistance filter", False)
time_persistence_filter.add("time_persistance_alpha_window", double_t, 0, "Angle window to search around the last detection on following inputs", 0.035, 0.01, 0.07)
time_persistence_filter.add("time_persistance_range_window", double_t, 0, "Range window to search around the last detection on following inputs", 0.1, 0.01, 0.3)
time_persistence_filter.add("time_persistance_orientation_th", double_t, 0, "Maximum orientation diff from the last detection on following inputs", 0.3, 0.01, 0.8)
time_persistence_filter.add("time_persistance_min_detections", int_t, 0, "The number of detections on a row to trust it", 5, 1, 15)
time_persistence_filter.add("time_persistence_filter_en", bool_t, 0, "Boolean to filter detections by time persistence", False)
time_persistence_filter.add("time_persistence_filter_orientation_en", bool_t, 0, "Boolean to add orientation filter to time persistence filter", False)
time_persistence_filter.add("time_persistence_alpha_window", double_t, 0, "Angle window to search around the last detection on following inputs", 0.035, 0.01, 0.07)
time_persistence_filter.add("time_persistence_range_window", double_t, 0, "Range window to search around the last detection on following inputs", 0.1, 0.01, 0.3)
time_persistence_filter.add("time_persistence_orientation_th", double_t, 0, "Maximum orientation diff from the last detection on following inputs", 0.3, 0.01, 0.8)
time_persistence_filter.add("time_persistence_min_detections", int_t, 0, "The number of detections on a row to trust it", 5, 1, 15)
exit(gen.generate(PACKAGE, "AdcTagLocalizationFilterAlgorithm", "AdcTagLocalizationFilter"))
\ No newline at end of file
......@@ -10,9 +10,9 @@ detection_min_confidence: 126
id_filter_en: True
ids_enabled: "0, 1"
time_persistance_filter_en: True
time_persistance_filter_orientation_en: True
time_persistance_alpha_window: 0.1
time_persistance_range_window: 0.2
time_persistance_orientation_th: 0.4
time_persistance_min_detections: 4
time_persistence_filter_en: True
time_persistence_filter_orientation_en: True
time_persistence_alpha_window: 0.1
time_persistence_range_window: 0.2
time_persistence_orientation_th: 0.4
time_persistence_min_detections: 4
......@@ -130,7 +130,7 @@ class AdcTagLocalizationFilterAlgNode : public algorithm_base::IriBaseAlgorithm<
void prepare_feature_candidates();
/**
* \brief Function to update features candidates applying time persistance filter if enabled.
* \brief Function to update features candidates applying time persistence filter if enabled.
*
* \param _r The detection distance.
*
......
......@@ -128,7 +128,7 @@ void AdcTagLocalizationFilterAlgNode::prepare_feature_candidates()
//Remove not detected candidates
for (auto it = this->candidates_.begin(); it != this->candidates_.end(); ++it)
{
if (it->detected && this->config_.time_persistance_filter_en)
if (it->detected && this->config_.time_persistence_filter_en)
it->detected = false;
else
{
......@@ -142,7 +142,7 @@ void AdcTagLocalizationFilterAlgNode::prepare_feature_candidates()
bool AdcTagLocalizationFilterAlgNode::update_feature_candidates(double _r, double _theta, int _id, const geometry_msgs::Pose &_pose)
{
//Check if is a new candidate or an already seen candidate
if (!this->config_.time_persistance_filter_en)
if (!this->config_.time_persistence_filter_en)
return true;
for (auto it = this->candidates_.begin(); it != this->candidates_.end(); ++it)
{
......@@ -151,9 +151,9 @@ bool AdcTagLocalizationFilterAlgNode::update_feature_candidates(double _r, doubl
in_orientation = pitch;
tf2::Matrix3x3(tf2::Quaternion(it->pose.orientation.x, it->pose.orientation.y,it->pose.orientation.z, it->pose.orientation.w)).getEulerYPR(yaw, pitch, roll);
cand_orientation = pitch;
if ((std::fabs(it->th - _theta) <= this->config_.time_persistance_alpha_window) &&
(std::fabs(it->r - _r) <= this->config_.time_persistance_range_window) &&
(!this->config_.time_persistance_filter_orientation_en || (std::fabs(in_orientation - cand_orientation) < this->config_.time_persistance_orientation_th)) &&
if ((std::fabs(it->th - _theta) <= this->config_.time_persistence_alpha_window) &&
(std::fabs(it->r - _r) <= this->config_.time_persistence_range_window) &&
(!this->config_.time_persistence_filter_orientation_en || (std::fabs(in_orientation - cand_orientation) < this->config_.time_persistence_orientation_th)) &&
check_tag_id(_id))
{
it->detected = true;
......@@ -173,7 +173,7 @@ void AdcTagLocalizationFilterAlgNode::update_features_msg()
std::vector<iri_adc_msgs::feature>().swap(this->features_msg_.features);
for (auto it = this->candidates_.begin(); it != this->candidates_.end(); ++it)
{
if (!this->config_.time_persistance_filter_en || (it->count >= this->config_.time_persistance_min_detections))
if (!this->config_.time_persistence_filter_en || (it->count >= this->config_.time_persistence_min_detections))
{
iri_adc_msgs::feature feature;
feature.pose = it->pose;
......
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