Skip to content
GitLab
Commit 1009d0c1 authored by Alejandro Lopez Gestoso's avatar Alejandro Lopez Gestoso
Browse files

Added orientation filter and matching

parent 1d551be5
Hide whitespace changes
Inline Side-by-side
cfg/AdcLandmarksSlamSolver.cfg
View file @ 1009d0c1
......@@ -57,8 +57,10 @@ gen.add("output_files_folder", str_t, 0, "Output
gen.add("err_msg_rate", double_t, 0, "Rate to publish err messages", 0.5, 0.1, 1.0)
landmarks.add("landmarks_candidates_filter_en", bool_t, 0, "Boolean to filter landmarks checking if it's a feature on the followings scans before adding it as landmark", False)
landmarks.add("landmarks_filter_orientation_en", bool_t, 0, "Boolean to add orientation filter to time persistance filter", False)
landmarks.add("landmark_mahalanobis_dist", double_t, 0, "Mahalonibis distance parameter", 2.0, 0.01, 20.0)
landmarks.add("landmarks_min_detections", int_t, 0, "The number of detections on a row to trust it as a landmark", 3, 1, 30)
landmarks.add("landmarks_orientation_th", double_t, 0, "Maximum orientation diff from the last detection on following inputs", 0.4, 0.01, 0.8)
ceres.add("update_problem_rate", double_t, 0, "Rate to update the ceres problem", 1.0, 0.00001, 10)
ceres.add("update_problem_distance", double_t, 0, "Distance from last robot state to update the ceres problem", 1.0, 0.1, 100000)
......
include/adc_landmarks_slam_solver_alg.h
View file @ 1009d0c1
......@@ -186,7 +186,7 @@ class AdcLandmarksSlamSolverAlgorithm
/**
* \brief Function to calculate the mahalanobis distance between a landmark and a feature.
*
* \param _feature_pos The feature X and Y coordenates.
* \param _feature_pose The feature X and Y coordenates and its YAW orientation.
*
* \param _feature_r Feature range.
*
......@@ -196,7 +196,7 @@ class AdcLandmarksSlamSolverAlgorithm
*
* \return The landmark key if detected or -1.0.
*/
double is_a_mapped_landmark(Eigen::Vector2d _feature_pos, double _feature_r, double _feature_th, int _type, const std::string& _source_frame);
double is_a_mapped_landmark(Eigen::Vector3d _feature_pose, double _feature_r, double _feature_th, int _type, const std::string& _source_frame);
/**
* \brief Function to solve the problem.
......
src/adc_landmarks_slam_solver_alg.cpp
View file @ 1009d0c1
......@@ -148,7 +148,7 @@ double AdcLandmarksSlamSolverAlgorithm::mahalanobis_distance(Eigen::Vector2d _fe
return std::sqrt(dist2);
}
double AdcLandmarksSlamSolverAlgorithm::is_a_mapped_landmark(Eigen::Vector2d _feature_pos, double _feature_r, double _feature_th, int _type, const std::string& _source_frame)
double AdcLandmarksSlamSolverAlgorithm::is_a_mapped_landmark(Eigen::Vector3d _feature_pose, double _feature_r, double _feature_th, int _type, const std::string& _source_frame)
{
double min_dist = 99999999999.0;
std::map<double, LandmarkInfo>::iterator min_dist_land = this->landmarks_.end();
......@@ -156,13 +156,14 @@ double AdcLandmarksSlamSolverAlgorithm::is_a_mapped_landmark(Eigen::Vector2d _fe
{
if (_type == it->second.type)
{
Eigen::Vector2d landmark_pos;
Eigen::Vector2d landmark_pos, feature_pos;
landmark_pos << it->second.pose(0), it->second.pose(1);
double dist = mahalanobis_distance(_feature_pos, _feature_r, _feature_th, landmark_pos);
feature_pos << _feature_pose(0), _feature_pose(1);
double dist = mahalanobis_distance(feature_pos, _feature_r, _feature_th, landmark_pos);
// std::cout << "check dist " << dist << std::endl;
if (dist <= this->config_.landmark_mahalanobis_dist)
if (dist <= this->config_.landmark_mahalanobis_dist && ((!this->config_.landmarks_filter_orientation_en) || (std::fabs(it->second.pose(2) -_feature_pose(2)) < this->config_.landmarks_orientation_th)))
{
Eigen::Vector2d d = landmark_pos - _feature_pos;
Eigen::Vector2d d = landmark_pos - feature_pos;
if (d.norm() < min_dist)
{
min_dist = d.norm();
......
src/adc_landmarks_slam_solver_alg_node.cpp
View file @ 1009d0c1
......@@ -576,7 +576,6 @@ bool AdcLandmarksSlamSolverAlgNode::check_landmarks(bool _update_landmarks)
local_feature.header.stamp = this->features_stamp_;
local_feature.header.frame_id = this->features_frame_;
for (unsigned int i = 0; i < this->features_.size(); i++)
{
//Get global feature
......@@ -584,18 +583,18 @@ bool AdcLandmarksSlamSolverAlgNode::check_landmarks(bool _update_landmarks)
tf2::doTransform(local_feature, global_feature, tf_map_sensor_msg);
//Check if is a mapped landmark
Eigen::Vector2d feature_pos;
feature_pos << global_feature.pose.position.x, global_feature.pose.position.y;
this->features_[i].landmark_key = this->alg_.is_a_mapped_landmark(feature_pos, this->features_[i].r, this->features_[i].th, this->features_[i].type, this->features_frame_);
tf2::Quaternion q = tf2::Quaternion(global_feature.pose.orientation.x, global_feature.pose.orientation.y, global_feature.pose.orientation.z, global_feature.pose.orientation.w);
double yaw, pitch, roll;
tf2::Matrix3x3(q).getEulerYPR(yaw, pitch, roll);
Eigen::Vector3d feature_pose;
feature_pose << global_feature.pose.position.x, global_feature.pose.position.y, yaw;
this->features_[i].landmark_key = this->alg_.is_a_mapped_landmark(feature_pose, this->features_[i].r, this->features_[i].th, this->features_[i].type, this->features_frame_);
//if it isn't a mapped landmark, update them if enabled
if (_update_landmarks && (this->features_[i].landmark_key < 0.0))
{
this->features_[i].pose(0) = global_feature.pose.position.x;
this->features_[i].pose(1) = global_feature.pose.position.y;
tf2::Quaternion q = tf2::Quaternion(global_feature.pose.orientation.x, global_feature.pose.orientation.y, global_feature.pose.orientation.z, global_feature.pose.orientation.w);
double yaw, pitch, roll;
tf2::Matrix3x3(q).getEulerYPR(yaw, pitch, roll);
this->features_[i].pose(2) = yaw;
if (is_a_new_landmark(this->features_[i]))
{
......@@ -650,7 +649,7 @@ bool AdcLandmarksSlamSolverAlgNode::is_a_new_landmark(FeatureInfo &_feature)
feature_pos << _feature.pose(0), _feature.pose(1);
landmark_pos << it->pose(0), it->pose(1);
double dist = this->alg_.mahalanobis_distance(feature_pos, _feature.r, _feature.th, landmark_pos);
if (dist <= this->config_.landmark_mahalanobis_dist)
if (dist <= this->config_.landmark_mahalanobis_dist && ((!this->config_.landmarks_filter_orientation_en) || (std::fabs(it->pose(2) -_feature.pose(2)) < this->config_.landmarks_orientation_th)))
{
Eigen::Vector2d d = landmark_pos - feature_pos;
if (d.norm() < min_dist)
......@@ -811,7 +810,7 @@ void AdcLandmarksSlamSolverAlgNode::publish_landmarks_markers()
this->landmarks_MarkerArray_msg_.markers[i].id = i;
this->landmarks_MarkerArray_msg_.markers[i].ns = "landmarks";
// this->landmarks_MarkerArray_msg_.markers[i].type = visualization_msgs::Marker::TEXT_VIEW_FACING;
this->landmarks_MarkerArray_msg_.markers[i].type = visualization_msgs::Marker::CYLINDER;
this->landmarks_MarkerArray_msg_.markers[i].type = visualization_msgs::Marker::CUBE;
this->landmarks_MarkerArray_msg_.markers[i].action = visualization_msgs::Marker::ADD;
// this->landmarks_MarkerArray_msg_.markers[i].lifetime = ros::Duration(0.1);
......@@ -819,13 +818,15 @@ void AdcLandmarksSlamSolverAlgNode::publish_landmarks_markers()
this->landmarks_MarkerArray_msg_.markers[i].pose.position.y = it->second.pose(1);
this->landmarks_MarkerArray_msg_.markers[i].pose.position.z = 0.2;
this->landmarks_MarkerArray_msg_.markers[i].pose.orientation.x = 0.0;
this->landmarks_MarkerArray_msg_.markers[i].pose.orientation.y = 0.0;
this->landmarks_MarkerArray_msg_.markers[i].pose.orientation.z = 0.0;
this->landmarks_MarkerArray_msg_.markers[i].pose.orientation.w = 1.0;
this->landmarks_MarkerArray_msg_.markers[i].scale.x = 0.1;
this->landmarks_MarkerArray_msg_.markers[i].scale.y = 0.1;
tf2::Quaternion q;
q.setRPY(0.0, 0.0, it->second.pose(2));
this->landmarks_MarkerArray_msg_.markers[i].pose.orientation.x = q.x();
this->landmarks_MarkerArray_msg_.markers[i].pose.orientation.y = q.y();
this->landmarks_MarkerArray_msg_.markers[i].pose.orientation.z = q.z();
this->landmarks_MarkerArray_msg_.markers[i].pose.orientation.w = q.w();
this->landmarks_MarkerArray_msg_.markers[i].scale.x = 0.05;
this->landmarks_MarkerArray_msg_.markers[i].scale.y = 0.02;
this->landmarks_MarkerArray_msg_.markers[i].scale.z = 0.4;
this->landmarks_MarkerArray_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