Fixed bug on mahalanobis distance

include/adc_landmarks_slam_solver_alg.h

@@ -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.

include/adc_landmarks_slam_solver_alg_node.h

@@ -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.

src/adc_landmarks_slam_solver_alg.cpp

@@ -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)

src/adc_landmarks_slam_solver_alg_node.cpp

@@ -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");
 }