refactor(ekf_localizer): isolate mahalanobis functions and add tests (a…

…utowarefoundation#1877)

* refactor(ekf_localizer): isolate mahalanobis functions and add tests

localization/ekf_localizer/CMakeLists.txt

@@ -15,6 +15,7 @@ ament_auto_find_build_dependencies()
 
 ament_auto_add_library(ekf_localizer_lib SHARED
   src/ekf_localizer.cpp
+  src/mahalanobis.cpp
   src/measurement.cpp
   src/state_transition.cpp
 )
@@ -41,6 +42,7 @@ if(BUILD_TESTING)
   find_package(ament_cmake_gtest REQUIRED)
 
   set(TEST_FILES
+    test/test_mahalanobis.cpp
     test/test_measurement.cpp
     test/test_state_transition.cpp)
 

localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp

@@ -266,18 +266,6 @@ class EKFLocalizer : public rclcpp::Node
    */
   void measurementUpdateTwist(const geometry_msgs::msg::TwistWithCovarianceStamped & twist);
 
-  /**
-   * @brief check whether a measurement value falls within the mahalanobis distance threshold
-   * @param dist_max mahalanobis distance threshold
-   * @param estimated current estimated state
-   * @param measured measured state
-   * @param estimated_cov current estimation covariance
-   * @return whether it falls within the mahalanobis distance threshold
-   */
-  bool mahalanobisGate(
-    const double & dist_max, const Eigen::MatrixXd & estimated, const Eigen::MatrixXd & measured,
-    const Eigen::MatrixXd & estimated_cov) const;
-
   /**
    * @brief get transform from frame_id
    */

localization/ekf_localizer/include/ekf_localizer/mahalanobis.hpp

@@ -0,0 +1,28 @@
+// Copyright 2022 Autoware Foundation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef EKF_LOCALIZER__MAHALANOBIS_HPP_
+#define EKF_LOCALIZER__MAHALANOBIS_HPP_
+
+#include <Eigen/Core>
+#include <Eigen/Dense>
+
+double squaredMahalanobis(
+  const Eigen::VectorXd & x, const Eigen::VectorXd & y, const Eigen::MatrixXd & C);
+
+bool mahalanobisGate(
+  const double & dist_max, const Eigen::MatrixXd & x, const Eigen::MatrixXd & obj_x,
+  const Eigen::MatrixXd & cov);
+
+#endif  // EKF_LOCALIZER__MAHALANOBIS_HPP_

localization/ekf_localizer/src/ekf_localizer.cpp

@@ -14,6 +14,7 @@
 
 #include "ekf_localizer/ekf_localizer.hpp"
 
+#include "ekf_localizer/mahalanobis.hpp"
 #include "ekf_localizer/matrix_types.hpp"
 #include "ekf_localizer/measurement.hpp"
 #include "ekf_localizer/state_index.hpp"
@@ -603,24 +604,6 @@ void EKFLocalizer::measurementUpdateTwist(
   DEBUG_PRINT_MAT((X_result - X_curr).transpose());
 }
 
-/*
- * mahalanobisGate
- */
-bool EKFLocalizer::mahalanobisGate(
-  const double & dist_max, const Eigen::MatrixXd & x, const Eigen::MatrixXd & obj_x,
-  const Eigen::MatrixXd & cov) const
-{
-  Eigen::MatrixXd mahalanobis_squared = (x - obj_x).transpose() * cov.inverse() * (x - obj_x);
-  DEBUG_INFO(
-    get_logger(), "measurement update: mahalanobis = %f, gate limit = %f",
-    std::sqrt(mahalanobis_squared(0)), dist_max);
-  if (mahalanobis_squared(0) > dist_max * dist_max) {
-    return false;
-  }
-
-  return true;
-}
-
 /*
  * publishEstimateResult
  */

localization/ekf_localizer/src/mahalanobis.cpp

@@ -0,0 +1,29 @@
+// Copyright 2022 Autoware Foundation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "ekf_localizer/mahalanobis.hpp"
+
+double squaredMahalanobis(
+  const Eigen::VectorXd & x, const Eigen::VectorXd & y, const Eigen::MatrixXd & C)
+{
+  const Eigen::VectorXd d = x - y;
+  return d.dot(C.inverse() * d);
+}
+
+bool mahalanobisGate(
+  const double & dist_max, const Eigen::MatrixXd & x, const Eigen::MatrixXd & obj_x,
+  const Eigen::MatrixXd & cov)
+{
+  return squaredMahalanobis(x, obj_x, cov) <= dist_max * dist_max;
+}

localization/ekf_localizer/test/test_mahalanobis.cpp

@@ -0,0 +1,52 @@
+// Copyright 2022 Autoware Foundation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "ekf_localizer/mahalanobis.hpp"
+
+#include <gtest/gtest.h>
+
+constexpr double tolerance = 1e-8;
+
+TEST(MahalanobisGate, SquaredMahalanobis)
+{
+  {
+    Eigen::Vector2d x(0, 1);
+    Eigen::Vector2d y(3, 2);
+    Eigen::Matrix2d C;
+    C << 10, 0, 0, 10;
+
+    EXPECT_NEAR(squaredMahalanobis(x, y, C), 1.0, tolerance);
+  }
+
+  {
+    Eigen::Vector2d x(4, 1);
+    Eigen::Vector2d y(1, 5);
+    Eigen::Matrix2d C;
+    C << 5, 0, 0, 5;
+
+    EXPECT_NEAR(squaredMahalanobis(x, y, C), 5.0, tolerance);
+  }
+}
+
+TEST(MahalanobisGate, MahalanobisGate)
+{
+  Eigen::Vector2d x(0, 1);
+  Eigen::Vector2d y(3, 2);
+  Eigen::Matrix2d C;
+  C << 10, 0, 0, 10;
+
+  EXPECT_FALSE(mahalanobisGate(0.99, x, y, C));
+  EXPECT_FALSE(mahalanobisGate(1.00, x, y, C));
+  EXPECT_TRUE(mahalanobisGate(1.01, x, y, C));
+}