add mahalanobis gate (#9)

* add mahalanobis gate

* fix format

* rename function

* fix calc mahalanobis_distance

include/ndt_localizer/ndt_odom_integrator.h

@@ -42,6 +42,8 @@ class NDTOdomIntegrator
   void predict_between_timestamps(const ros::Time begin_stamp, const ros::Time end_stamp);
   void update_by_ndt_pose(const Eigen::VectorXd& pose);
   Eigen::Matrix3d get_rotation_matrix(double roll, double pitch, double yaw);
+  bool is_mahalanobis_distance_gate(const double mahalanobis_distance_threshold, const Eigen::VectorXd& ndt_pose,
+                           const Eigen::VectorXd& last_pose, const Eigen::MatrixXd& cov);
   void publish_map_to_odom_tf(const ros::Time& stamp,
                               const geometry_msgs::Pose& pose);
   void process(void);
@@ -66,6 +68,7 @@ class NDTOdomIntegrator
   double sigma_ndt_;
   bool enable_odom_tf_;
   bool enable_tf_;
+  double mahalanobis_distance_threshold_;
 
   unsigned int state_dim_;
   unsigned int position_dim_;

src/ndt_odom_integrator.cpp

@@ -43,6 +43,7 @@ NDTOdomIntegrator::NDTOdomIntegrator(void)
   local_nh_.param<bool>("enable_odom_tf", enable_odom_tf_, true);
   local_nh_.param<bool>("enable_tf", enable_tf_, true);
   local_nh_.param<int>("queue_capacity", queue_capacity_, 1000);
+  local_nh_.param<double>("mahalanobis_distance_threshold", mahalanobis_distance_threshold_, 1.5);
 
   ROS_INFO_STREAM("init_sigma_position: " << init_sigma_position_);
   ROS_INFO_STREAM("init_sigma_orientation: " << init_sigma_orientation_);
@@ -58,6 +59,7 @@ NDTOdomIntegrator::NDTOdomIntegrator(void)
   ROS_INFO_STREAM("enable_odom_tf: " << enable_odom_tf_);
   ROS_INFO_STREAM("enable_tf: " << enable_tf_);
   ROS_INFO_STREAM("queue_capacity: " << queue_capacity_);
+  ROS_INFO_STREAM("mahalanobis_distance_threshold: " << mahalanobis_distance_threshold_);
 
   tf_ = std::make_shared<tf2_ros::Buffer>();
   tf_->setUsingDedicatedThread(true);
@@ -111,7 +113,10 @@ void NDTOdomIntegrator::ndt_pose_callback(
   if (last_pose_stamp_ <= received_pose_stamp)
   {
     predict_between_timestamps(last_pose_stamp_, received_pose_stamp);
-    update_by_ndt_pose(received_pose);
+
+    if (is_mahalanobis_distance_gate(mahalanobis_distance_threshold_, received_pose, x_, p_))
+      update_by_ndt_pose(received_pose);
+
     last_pose_ = x_;
     last_covariance_ = p_;
     last_pose_stamp_ = msg->header.stamp;
@@ -502,6 +507,25 @@ NDTOdomIntegrator::get_rotation_matrix(double roll, double pitch, double yaw)
   return rot;
 }
 
+bool NDTOdomIntegrator::is_mahalanobis_distance_gate(
+                        const double mahalanobis_distance_threshold, const Eigen::VectorXd& ndt_pose,
+                        const Eigen::VectorXd& last_pose, const Eigen::MatrixXd& cov)
+{
+  const double mahalanobis_distance =
+                  std::sqrt((ndt_pose - last_pose).transpose() * cov.inverse() * (ndt_pose - last_pose));
+
+  if (mahalanobis_distance > mahalanobis_distance_threshold)
+  {
+    ROS_ERROR_STREAM("Mahalanobis_distance distance is over the threshold: " << mahalanobis_distance);
+    return false;
+  }
+  else
+  {
+    // ROS_WARN_STREAM("Mahalanobis_distance distance is under the threshold: " << mahalanobis_distance);
+    return true;
+  }
+}
+
 void NDTOdomIntegrator::publish_map_to_odom_tf(
     const ros::Time& stamp, const geometry_msgs::Pose& pose)
 {