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

Added orientation and id filter at time_persistance filter

parent 385677d4
......@@ -6,7 +6,7 @@ find_package(catkin REQUIRED)
# ********************************************************************
# Add catkin additional components here
# ********************************************************************
find_package(catkin REQUIRED COMPONENTS iri_base_algorithm visualization_msgs iri_adc_msgs ar_track_alvar_msgs)
find_package(catkin REQUIRED COMPONENTS iri_base_algorithm visualization_msgs iri_adc_msgs ar_track_alvar_msgs tf2_ros)
## System dependencies are found with CMake's conventions
# find_package(Boost REQUIRED COMPONENTS system)
......@@ -61,7 +61,7 @@ catkin_package(
# ********************************************************************
# Add ROS and IRI ROS run time dependencies
# ********************************************************************
CATKIN_DEPENDS iri_base_algorithm visualization_msgs iri_adc_msgs ar_track_alvar_msgs
CATKIN_DEPENDS iri_base_algorithm visualization_msgs iri_adc_msgs ar_track_alvar_msgs tf2_ros
# ********************************************************************
# Add system and labrobotica run time dependencies here
# ********************************************************************
......
......@@ -54,9 +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_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_min_detections", int_t, 0, "The number of detections on a row to trust it", 5, 1, 15)
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)
exit(gen.generate(PACKAGE, "AdcTagLocalizationFilterAlgorithm", "AdcTagLocalizationFilter"))
\ No newline at end of file
......@@ -11,6 +11,8 @@ 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
......@@ -30,6 +30,7 @@
#include <Eigen/Eigen>
#include <iostream>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
// [publisher subscriber headers]
#include <visualization_msgs/MarkerArray.h>
......@@ -64,11 +65,10 @@ typedef struct {
class AdcTagLocalizationFilterAlgNode : public algorithm_base::IriBaseAlgorithm<AdcTagLocalizationFilterAlgorithm>
{
private:
bool new_data; ///< Boolean to know if new data is available.
std::vector<int> ids_enabled; ///< Vector with the Tag ids enabled.
std::list<Feature_candidate> candidates; ///< List with the tag candidates to be features.
ros::Time stamp; ///< Last msg time stamp.
std::string source_frame_id; ///< Source frame id to publish with this frame id when no marker is detected.
bool new_data_; ///< Boolean to know if new data is available.
std::vector<int> ids_enabled_; ///< Vector with the Tag ids enabled.
std::list<Feature_candidate> candidates_; ///< List with the tag candidates to be features.
std::string source_frame_id_; ///< Source frame id to publish with this frame id when no marker is detected.
// [publisher attributes]
ros::Publisher tag_features_publisher_;
......@@ -105,24 +105,24 @@ class AdcTagLocalizationFilterAlgNode : public algorithm_base::IriBaseAlgorithm<
/**
* \brief Function to apply range, confidence and id filters.
*
* \param range The detection distance.
* \param _range The detection distance.
*
* \param confidence The detection confidence.
* \param _confidence The detection confidence.
*
* \param id The Tag id.
* \param _id The Tag id.
*
* \return If is a Tag.
*/
bool is_a_tag(double range, int confidence, int id);
bool is_a_tag(double _range, int _confidence, int _id);
/**
* \brief Function to filter tag by ids if enabled
*
* \param id The Tag id.
* \param _id The Tag id.
*
* \return If is id enabled.
* \return If its id is enabled.
*/
bool check_tag_id(int id);
bool check_tag_id(int _id);
/**
* \brief Function to remove feature candidates not detected and reinit its variables.
......@@ -132,13 +132,17 @@ class AdcTagLocalizationFilterAlgNode : public algorithm_base::IriBaseAlgorithm<
/**
* \brief Function to update features candidates applying time persistance filter if enabled.
*
* \param r The detection distance.
* \param _r The detection distance.
*
* \param theta The detection theta.
* \param _theta The detection theta.
*
* \param pose The detection pose.
* \param _id The detection id.
*
* \param _pose The detection pose.
*
* \return If it is a new candidate.
*/
bool update_feature_candidates(double r, double theta, const geometry_msgs::Pose &pose);
bool update_feature_candidates(double _r, double _theta, int _id, const geometry_msgs::Pose &_pose);
/**
* \brief Function to update features msg.
......
......@@ -56,6 +56,7 @@
<depend>iri_adc_msgs</depend>
<depend>ar_track_alvar_msgs</depend>
<depend>ar_track_alvar</depend>
<depend>tf2_ros</depend>
<!-- The export tag contains other, unspecified, tags -->
......
......@@ -21,11 +21,11 @@ AdcTagLocalizationFilterAlgNode::AdcTagLocalizationFilterAlgNode(void) :
//Parse ids enabled
std::vector<int>().swap(this->ids_enabled);
std::vector<int>().swap(this->ids_enabled_);
std::string ids;
if(!this->private_node_handle_.getParam("ids_enabled", ids))
if(!this->private_node_handle_.getParam("ids_enabled_", ids))
{
ROS_WARN("AdcTagLocalizationFilterAlgNode::AdcTagLocalizationFilterAlgNode: No ids_enabled param found.");
ROS_WARN("AdcTagLocalizationFilterAlgNode::AdcTagLocalizationFilterAlgNode: No ids_enabled_ param found.");
}
else
{
......@@ -35,16 +35,16 @@ AdcTagLocalizationFilterAlgNode::AdcTagLocalizationFilterAlgNode(void) :
std::string substr;
getline(ss, substr, ',');
int id = std::stoi(substr.c_str(), NULL);
this->ids_enabled.push_back(id);
this->ids_enabled_.push_back(id);
}
}
std::cout << "Ids enabled: ";
for (unsigned int i = 0; i < this->ids_enabled.size(); i++)
std::cout << this->ids_enabled[i] << ", ";
for (unsigned int i = 0; i < this->ids_enabled_.size(); i++)
std::cout << this->ids_enabled_[i] << ", ";
std::cout << std::endl;
std::list<Feature_candidate>().swap(this->candidates);
this->new_data = false;
this->source_frame_id = "";
std::list<Feature_candidate>().swap(this->candidates_);
this->new_data_ = false;
this->source_frame_id_ = "";
// [init services]
......@@ -60,10 +60,10 @@ AdcTagLocalizationFilterAlgNode::~AdcTagLocalizationFilterAlgNode(void)
// [free dynamic memory]
pthread_mutex_destroy(&this->ar_tag_detections_mutex_);
std::vector<int>().swap(this->ids_enabled);
std::list<Feature_candidate>().swap(this->candidates);
this->new_data = false;
this->source_frame_id = "";
std::vector<int>().swap(this->ids_enabled_);
std::list<Feature_candidate>().swap(this->candidates_);
this->new_data_ = false;
this->source_frame_id_ = "";
}
void AdcTagLocalizationFilterAlgNode::mainNodeThread(void)
......@@ -73,13 +73,13 @@ void AdcTagLocalizationFilterAlgNode::mainNodeThread(void)
this->ar_tag_detections_mutex_enter();
ROS_DEBUG("AdcTagLocalizationFilterAlgNode::mainNodeThread");
if (this->new_data)
if (this->new_data_)
{
update_features_msg();
this->features_publisher_.publish(this->features_msg_);
if (this->config_.publish_visualization)
publish_visualization_msg();
this->new_data = false;
this->new_data_ = false;
}
// [fill msg structures]
// Initialize the topic message structure
......@@ -104,21 +104,21 @@ void AdcTagLocalizationFilterAlgNode::mainNodeThread(void)
// this->alg_.unlock();
}
bool AdcTagLocalizationFilterAlgNode::is_a_tag(double range, int confidence, int id)
bool AdcTagLocalizationFilterAlgNode::is_a_tag(double _range, int _confidence, int _id)
{
//Apply enabled filters
return ((!this->config_.range_filter_en || (range < this->config_.detection_max_range)) &&
(!this->config_.confidence_filter_en || (confidence < this->config_.detection_min_confidence)) &&
check_tag_id(id));
return ((!this->config_.range_filter_en || (_range < this->config_.detection_max_range)) &&
(!this->config_.confidence_filter_en || (_confidence < this->config_.detection_min_confidence)) &&
check_tag_id(_id));
}
bool AdcTagLocalizationFilterAlgNode::check_tag_id(int id)
bool AdcTagLocalizationFilterAlgNode::check_tag_id(int _id)
{
//Check tag id if enabled
if (!this->config_.id_filter_en)
return true;
for (unsigned int i = 0; i < this->ids_enabled.size(); i++)
if (id == this->ids_enabled[i])
for (unsigned int i = 0; i < this->ids_enabled_.size(); i++)
if (_id == this->ids_enabled_[i])
return true;
return false;
}
......@@ -128,35 +128,42 @@ 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)
for (auto it = this->candidates_.begin(); it != this->candidates_.end(); ++it)
{
if (it->detected)
it->detected = false;
else
{
it = this->candidates.erase(it);
if (it == this->candidates.end())
it = this->candidates_.erase(it);
if (it == this->candidates_.end())
break;
}
}
}
bool AdcTagLocalizationFilterAlgNode::update_feature_candidates(double r, double theta, const geometry_msgs::Pose &pose)
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)
return false;
for (auto it = this->candidates.begin(); it != this->candidates.end(); ++it)
for (auto it = this->candidates_.begin(); it != this->candidates_.end(); ++it)
{
if ((std::fabs(it->th - theta) <= this->config_.time_persistance_alpha_window)
&& (std::fabs(it->r - r) <= this->config_.time_persistance_range_window))
double yaw, pitch, roll, in_orientation, cand_orientation;
tf2::Matrix3x3(tf2::Quaternion(_pose.orientation.x, _pose.orientation.y,_pose.orientation.z, _pose.orientation.w)).getEulerYPR(yaw, pitch, roll);
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)) &&
check_tag_id(_id))
{
it->detected = true;
it->count++;
//update feature candidate position
it->r = r;
it->th = theta;
it->pose = pose;
it->r = _r;
it->th = _theta;
it->pose = _pose;
return false;
}
}
......@@ -166,7 +173,7 @@ bool AdcTagLocalizationFilterAlgNode::update_feature_candidates(double r, double
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)
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))
{
......@@ -226,14 +233,14 @@ void AdcTagLocalizationFilterAlgNode::ar_tag_detections_callback(const ar_track_
this->ar_tag_detections_mutex_enter();
if (msg->markers.size() == 0)
{
this->features_msg_.header.frame_id = this->source_frame_id;
this->features_msg_.header.frame_id = this->source_frame_id_;
this->features_msg_.header.stamp = ros::Time::now();
}
else
{
this->features_msg_.header.frame_id = msg->markers[0].header.frame_id;
this->features_msg_.header.stamp = msg->markers[0].header.stamp;
this->source_frame_id = msg->markers[0].header.frame_id;
this->source_frame_id_ = msg->markers[0].header.frame_id;
}
prepare_feature_candidates();
......@@ -250,7 +257,7 @@ void AdcTagLocalizationFilterAlgNode::ar_tag_detections_callback(const ar_track_
theta -= 2*M_PI;
while (theta < -M_PI)
theta += 2*M_PI;
if (update_feature_candidates(dist, theta, msg->markers[i].pose.pose))
if (update_feature_candidates(dist, theta, msg->markers[i].id, msg->markers[i].pose.pose))
{
Feature_candidate cand;
cand.r = dist;
......@@ -261,11 +268,11 @@ void AdcTagLocalizationFilterAlgNode::ar_tag_detections_callback(const ar_track_
cand.pose.orientation = msg->markers[i].pose.pose.orientation;
cand.id = msg->markers[i].id;
cand.confidence = msg->markers[i].confidence;
this->candidates.push_back(cand);
this->candidates_.push_back(cand);
}
}
}
this->new_data = true;
this->new_data_ = true;
//std::cout << msg->data << std::endl;
//unlock previously blocked shared variables
......@@ -303,7 +310,7 @@ void AdcTagLocalizationFilterAlgNode::node_config_update(Config &config, uint32_
std::string substr;
getline(ss, substr, ',');
int id = std::stoi(substr.c_str(), NULL);
this->ids_enabled.push_back(id);
this->ids_enabled_.push_back(id);
}
this->config_=config;
this->ar_tag_detections_mutex_exit();
......
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