Commit 2ab0e421 authored by Alejandro Lopez Gestoso's avatar Alejandro Lopez Gestoso
Browse files

Fixed bug on mahalanobis distance

parent 4174d41f
......@@ -32,6 +32,10 @@
#include <sstream>
#include <iostream>
#include <fstream>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/TransformStamped.h>
#include <tf2_ros/transform_listener.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include "estimated_pose_residual_2D.h"
#include "odom_residual_2D.h"
......@@ -112,10 +116,12 @@ class AdcLandmarksSlamSolverAlgorithm
* \brief Function to load landmarks map from a txt file.
*
* \param _landmarks_file The route to the landmarks file to be loaded.
*
* \param _fixed If landamrks are fixed.
*
* \return If success.
*/
bool load_landmarks(const std::string& _landmarks_file);
bool load_landmarks(const std::string& _landmarks_file, bool _fixed);
/**
* \brief Function to add a landmark.
......@@ -187,17 +193,19 @@ class AdcLandmarksSlamSolverAlgorithm
/**
* \brief Function to calculate the mahalanobis distance between a landmark and a feature.
*
* \param _feature_pose The feature X and Y coordenates and its YAW orientation.
* \param _feature_pose The feature local X and Y coordenates and its YAW orientation.
*
* \param _feature_r Feature range.
*
* \param _feature_th Feature angle from the robot.
*
* \param _source_frame The frame of the sensor.
*
* \param _tf_sensor_map The transformation from sensor to map.
*
* \return The landmark key if detected or -1.0.
*/
double is_a_mapped_landmark(Eigen::Vector3d _feature_pose, 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, geometry_msgs::TransformStamped& _tf_sensor_map);
/**
* \brief Function to solve the problem.
......
......@@ -213,10 +213,14 @@ class AdcLandmarksSlamSolverAlgNode : public algorithm_base::IriBaseAlgorithm<Ad
* \param _feature The feature to check.
*
* \param _front Flag to select front or rear features.
*
* \param _feature_pose The feature local X and Y coordenates and its YAW orientation.
*
* \param _tf_sensor_map The transformation from sensor to map.
*
* \return If is a landmark candidate have been seen enough times (is a landmark).
*/
bool is_a_new_landmark(FeatureInfo &_feature, bool _front);
bool is_a_new_landmark(FeatureInfo &_feature, bool _front, Eigen::Vector3d _feature_pose, geometry_msgs::TransformStamped& _tf_sensor_map);
/**
* \brief Function to update the frame data with the detected landmarks' information.
......
......@@ -51,7 +51,7 @@ void AdcLandmarksSlamSolverAlgorithm::config_update(Config& config, uint32_t lev
}
// AdcLandmarksSlamSolverAlgorithm Public API
bool AdcLandmarksSlamSolverAlgorithm::load_landmarks(const std::string& _landmarks_file)
bool AdcLandmarksSlamSolverAlgorithm::load_landmarks(const std::string& _landmarks_file, bool _fixed)
{
std::ifstream landmarks_file;
landmarks_file.open(_landmarks_file.c_str());
......@@ -71,8 +71,7 @@ bool AdcLandmarksSlamSolverAlgorithm::load_landmarks(const std::string& _landmar
l.pose(2) = l3;
l.type = t;
l.source_frame_id = "";
l.fixed = this->config_.landmarks_pos_fixed;
std::cout << (this->config_.landmarks_pos_fixed? "fixed true": "Fixed false");
l.fixed = _fixed;
this->landmarks_[id] = l;
}
// std::cout << "landmarks size = " << (int) this->global_landmarks.size() << std::endl;
......@@ -156,7 +155,7 @@ double AdcLandmarksSlamSolverAlgorithm::mahalanobis_distance(Eigen::Vector2d _fe
return std::sqrt(dist2);
}
double AdcLandmarksSlamSolverAlgorithm::is_a_mapped_landmark(Eigen::Vector3d _feature_pose, 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, geometry_msgs::TransformStamped& _tf_sensor_map)
{
double min_dist = 99999999999.0;
std::map<double, LandmarkInfo>::iterator min_dist_land = this->landmarks_.end();
......@@ -165,11 +164,29 @@ double AdcLandmarksSlamSolverAlgorithm::is_a_mapped_landmark(Eigen::Vector3d _fe
if (_type == it->second.type)
{
Eigen::Vector2d landmark_pos, feature_pos;
landmark_pos << it->second.pose(0), it->second.pose(1);
feature_pos << _feature_pose(0), _feature_pose(1);
geometry_msgs::PoseStamped local_land, global_land;
global_land.header.stamp = _tf_sensor_map.header.stamp;
global_land.header.frame_id = this->config_.global_frame;
global_land.pose.position.z = 0.0;
global_land.pose.position.x = it->second.pose(0);
global_land.pose.position.y = it->second.pose(1);
tf2::Quaternion q;
q.setRPY(0.0, 0.0, it->second.pose(2));
global_land.pose.orientation = tf2::toMsg(q);
tf2::doTransform(global_land, local_land, _tf_sensor_map);
double land_yaw, pitch, roll;
q = tf2::Quaternion(local_land.pose.orientation.x, local_land.pose.orientation.y, local_land.pose.orientation.z, local_land.pose.orientation.w);
tf2::Matrix3x3(q).getEulerYPR(land_yaw, pitch, roll);
landmark_pos << local_land.pose.position.x, local_land.pose.position.y;
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 && ((!this->config_.landmarks_filter_orientation_en) || (std::fabs(it->second.pose(2) -_feature_pose(2)) < this->config_.landmarks_orientation_th)))
if (dist <= this->config_.landmark_mahalanobis_dist && ((!this->config_.landmarks_filter_orientation_en) || (std::fabs(land_yaw -_feature_pose(2)) < this->config_.landmarks_orientation_th)))
{
Eigen::Vector2d d = landmark_pos - feature_pos;
if (d.norm() < min_dist)
......
......@@ -545,12 +545,12 @@ bool AdcLandmarksSlamSolverAlgNode::check_landmarks(bool _update_landmarks, bool
{
///Transform local front_features to global
//get transform from fixed_frame to sensor frame
geometry_msgs::TransformStamped tf_map_sensor_msg;
geometry_msgs::TransformStamped tf_map_sensor_msg, tf_sensor_map_msg;
geometry_msgs::PoseStamped local_feature, global_feature;
if ((_front && this->front_features_frame_ == "") || (!_front && this->rear_features_frame_ == ""))
{
ROS_WARN_STREAM("AdcLandmarksSlamSolverAlgNode::check_landmarks -> " << (_front? "front ": "rear ") << "features frame id empty; AR tag filter not initialized.");
ROS_DEBUG_STREAM("AdcLandmarksSlamSolverAlgNode::check_landmarks -> " << (_front? "front ": "rear ") << "features frame id empty; AR tag filter not initialized.");
return false;
}
......@@ -562,6 +562,10 @@ bool AdcLandmarksSlamSolverAlgNode::check_landmarks(bool _update_landmarks, bool
tf_map_sensor_msg.header.frame_id = this->config_.global_frame;
tf_map_sensor_msg.header.stamp = this->features_stamp_;//front/rear
tf_map_sensor_msg.child_frame_id = (_front? this->front_features_frame_: this->rear_features_frame_);
tf_sensor_map_msg.header.frame_id = (_front? this->front_features_frame_: this->rear_features_frame_);
tf_sensor_map_msg.header.stamp = this->features_stamp_;//front/rear
tf_sensor_map_msg.child_frame_id = this->config_.global_frame;
if(!this->config_.amcl_localization)
{
......@@ -581,7 +585,7 @@ bool AdcLandmarksSlamSolverAlgNode::check_landmarks(bool _update_landmarks, bool
tf2::Transform tf_odom_sensor;
// tf2::transformMsgToTF2(tf_odom_sensor_msg.transform, tf_odom_sensor);
tf_odom_sensor = tf2::Transform(tf2::Quaternion(tf_odom_sensor_msg.transform.rotation.x, tf_odom_sensor_msg.transform.rotation.y, tf_odom_sensor_msg.transform.rotation.z, tf_odom_sensor_msg.transform.rotation.w), tf2::Vector3(tf_odom_sensor_msg.transform.translation.x, tf_odom_sensor_msg.transform.translation.y, tf_odom_sensor_msg.transform.translation.z));
tf2::Transform tf_map_sensor;
tf2::Transform tf_map_sensor, tf_sensor_map;
tf_map_sensor = tf_map_odom * tf_odom_sensor;
// tf2::transformTF2ToMsg(tf_map_sensor, tf_map_sensor_msg.transform);
......@@ -592,15 +596,31 @@ bool AdcLandmarksSlamSolverAlgNode::check_landmarks(bool _update_landmarks, bool
tf_map_sensor_msg.transform.rotation.y = tf_map_sensor.getRotation().y();
tf_map_sensor_msg.transform.rotation.z = tf_map_sensor.getRotation().z();
tf_map_sensor_msg.transform.rotation.w = tf_map_sensor.getRotation().w();
tf_sensor_map = tf_map_sensor.inverse();
tf_sensor_map_msg.transform.translation.x = tf_sensor_map.getOrigin().x();
tf_sensor_map_msg.transform.translation.y = tf_sensor_map.getOrigin().y();
tf_sensor_map_msg.transform.translation.z = tf_sensor_map.getOrigin().z();
tf_sensor_map_msg.transform.rotation.x = tf_sensor_map.getRotation().x();
tf_sensor_map_msg.transform.rotation.y = tf_sensor_map.getRotation().y();
tf_sensor_map_msg.transform.rotation.z = tf_sensor_map.getRotation().z();
tf_sensor_map_msg.transform.rotation.w = tf_sensor_map.getRotation().w();
}
else
{
if (!this->tf2_buffer.canTransform(this->config_.global_frame, tf_map_sensor_msg.child_frame_id, tf_map_sensor_msg.header.stamp, ros::Duration(this->config_.tf_timeout)))
if (!this->tf2_buffer.canTransform(tf_map_sensor_msg.header.frame_id, tf_map_sensor_msg.child_frame_id, tf_map_sensor_msg.header.stamp, ros::Duration(this->config_.tf_timeout)))
{
ROS_WARN_STREAM("AdcLandmarksSlamSolverAlgNode:check_landmarks -> Can't transform from " << this->config_.global_frame << " to " << tf_map_sensor_msg.child_frame_id << " at " << tf_map_sensor_msg.header.stamp);
ROS_WARN_STREAM("AdcLandmarksSlamSolverAlgNode:check_landmarks -> Can't transform from " << tf_map_sensor_msg.header.frame_id << " to " << tf_map_sensor_msg.child_frame_id << " at " << tf_map_sensor_msg.header.stamp);
return false;
}
tf_map_sensor_msg = this->tf2_buffer.lookupTransform(this->config_.global_frame, tf_map_sensor_msg.child_frame_id, tf_map_sensor_msg.header.stamp);
tf_map_sensor_msg = this->tf2_buffer.lookupTransform(tf_map_sensor_msg.header.frame_id, tf_map_sensor_msg.child_frame_id, tf_map_sensor_msg.header.stamp);
if (!this->tf2_buffer.canTransform(tf_sensor_map_msg.header.frame_id, tf_sensor_map_msg.child_frame_id, tf_sensor_map_msg.header.stamp, ros::Duration(this->config_.tf_timeout)))
{
ROS_WARN_STREAM("AdcLandmarksSlamSolverAlgNode:check_landmarks -> Can't transform from " << tf_sensor_map_msg.header.frame_id << " to " << tf_sensor_map_msg.child_frame_id << " at " << tf_sensor_map_msg.header.stamp);
return false;
}
tf_sensor_map_msg = this->tf2_buffer.lookupTransform(tf_sensor_map_msg.header.frame_id, tf_sensor_map_msg.child_frame_id, tf_sensor_map_msg.header.stamp);
}
local_feature.header.stamp = this->features_stamp_;//front/rear
......@@ -613,11 +633,15 @@ bool AdcLandmarksSlamSolverAlgNode::check_landmarks(bool _update_landmarks, bool
//Check if is a mapped landmark
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;
double yaw, pitch, roll, yaw_local;
tf2::Matrix3x3(q).getEulerYPR(yaw, pitch, roll);
Eigen::Vector3d feature_pose;
Eigen::Vector3d feature_pose, feature_pose_local;
feature_pose << global_feature.pose.position.x, global_feature.pose.position.y, yaw;
(_front? this->front_features_: this->rear_features_)[i].landmark_key = this->alg_.is_a_mapped_landmark(feature_pose, (_front? this->front_features_: this->rear_features_)[i].r, (_front? this->front_features_: this->rear_features_)[i].th, (_front? this->front_features_: this->rear_features_)[i].type, (_front? this->front_features_frame_: this->rear_features_frame_));
q = tf2::Quaternion(local_feature.pose.orientation.x, local_feature.pose.orientation.y, local_feature.pose.orientation.z, local_feature.pose.orientation.w);
tf2::Matrix3x3(q).getEulerYPR(yaw_local, pitch, roll);
feature_pose_local << local_feature.pose.position.x, local_feature.pose.position.y, yaw_local;
(_front? this->front_features_: this->rear_features_)[i].landmark_key = this->alg_.is_a_mapped_landmark(feature_pose_local, (_front? this->front_features_: this->rear_features_)[i].r, (_front? this->front_features_: this->rear_features_)[i].th, (_front? this->front_features_: this->rear_features_)[i].type, (_front? this->front_features_frame_: this->rear_features_frame_), tf_sensor_map_msg);
//if it isn't a mapped landmark, update them if enabled
if (_update_landmarks && ((_front? this->front_features_: this->rear_features_)[i].landmark_key < 0.0))
......@@ -625,7 +649,8 @@ bool AdcLandmarksSlamSolverAlgNode::check_landmarks(bool _update_landmarks, bool
(_front? this->front_features_: this->rear_features_)[i].pose(0) = global_feature.pose.position.x;
(_front? this->front_features_: this->rear_features_)[i].pose(1) = global_feature.pose.position.y;
(_front? this->front_features_: this->rear_features_)[i].pose(2) = yaw;
if (is_a_new_landmark((_front? this->front_features_: this->rear_features_)[i], _front))
if (is_a_new_landmark((_front? this->front_features_: this->rear_features_)[i], _front, feature_pose_local, tf_sensor_map_msg))
{
LandmarkInfo landmark;
landmark.pose = (_front? this->front_features_: this->rear_features_)[i].pose;
......@@ -664,7 +689,7 @@ void AdcLandmarksSlamSolverAlgNode::prepare_landmark_candidates(bool _front)
}
}
bool AdcLandmarksSlamSolverAlgNode::is_a_new_landmark(FeatureInfo &_feature, bool _front)
bool AdcLandmarksSlamSolverAlgNode::is_a_new_landmark(FeatureInfo &_feature, bool _front, Eigen::Vector3d _feature_pose, geometry_msgs::TransformStamped& _tf_sensor_map)
{
//Check if a landmark candidate has been seen enough times
if (!this->config_.landmarks_candidates_filter_en)
......@@ -676,10 +701,27 @@ bool AdcLandmarksSlamSolverAlgNode::is_a_new_landmark(FeatureInfo &_feature, boo
if(_feature.type == it->type)
{
Eigen::Vector2d feature_pos, landmark_pos;
feature_pos << _feature.pose(0), _feature.pose(1);
landmark_pos << it->pose(0), it->pose(1);
feature_pos << _feature_pose(0), _feature_pose(1);
geometry_msgs::PoseStamped local_land, global_land;
global_land.header.stamp = _tf_sensor_map.header.stamp;
global_land.header.frame_id = this->config_.global_frame;
global_land.pose.position.z = 0.0;
global_land.pose.position.x = it->pose(0);
global_land.pose.position.y = it->pose(1);
tf2::Quaternion q;
q.setRPY(0.0, 0.0, it->pose(2));
global_land.pose.orientation = tf2::toMsg(q);
tf2::doTransform(global_land, local_land, _tf_sensor_map);
double land_yaw, pitch, roll;
q = tf2::Quaternion(local_land.pose.orientation.x, local_land.pose.orientation.y, local_land.pose.orientation.z, local_land.pose.orientation.w);
tf2::Matrix3x3(q).getEulerYPR(land_yaw, pitch, roll);
landmark_pos << local_land.pose.position.x, local_land.pose.position.y;
double dist = this->alg_.mahalanobis_distance(feature_pos, _feature.r, _feature.th, landmark_pos);
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)))
if (dist <= this->config_.landmark_mahalanobis_dist && ((!this->config_.landmarks_filter_orientation_en) || (std::fabs(land_yaw -_feature_pose(2)) < this->config_.landmarks_orientation_th)))
{
Eigen::Vector2d d = landmark_pos - feature_pos;
if (d.norm() < min_dist)
......@@ -825,7 +867,7 @@ bool AdcLandmarksSlamSolverAlgNode::get_tf_map_odom(ros::Time _t, Eigen::Vector3
void AdcLandmarksSlamSolverAlgNode::load_landmarks(void)
{
ROS_INFO_STREAM("AdcLandmarksSlamSolverAlgNode::load_landmarks -> Loading from file " << this->config_.landmarks_map_file);
if (!this->alg_.load_landmarks(this->config_.landmarks_map_file))
if (!this->alg_.load_landmarks(this->config_.landmarks_map_file, this->config_.landmarks_pos_fixed))
ROS_ERROR_STREAM("AdcLandmarksSlamSolverAlgNode::load_landmarks -> File" << this->config_.landmarks_map_file << " can't be opened");
}
......
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