Branch data     Line data    Source code

       1                 :            : // Copyright 2021 Apex.AI, Inc.
       2                 :            : //
       3                 :            : // Licensed under the Apache License, Version 2.0 (the "License");
       4                 :            : // you may not use this file except in compliance with the License.
       5                 :            : // You may obtain a copy of the License at
       6                 :            : //
       7                 :            : //     http://www.apache.org/licenses/LICENSE-2.0
       8                 :            : //
       9                 :            : // Unless required by applicable law or agreed to in writing, software
      10                 :            : // distributed under the License is distributed on an "AS IS" BASIS,
      11                 :            : // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
      12                 :            : // See the License for the specific language governing permissions and
      13                 :            : // limitations under the License.
      14                 :            : //
      15                 :            : // Co-developed by Tier IV, Inc. and Apex.AI, Inc.
      16                 :            : 
      17                 :            : #ifndef HELPER_FUNCTIONS__MAHALANOBIS_DISTANCE_HPP_
      18                 :            : #define HELPER_FUNCTIONS__MAHALANOBIS_DISTANCE_HPP_
      19                 :            : 
      20                 :            : #include <Eigen/Cholesky>
      21                 :            : 
      22                 :            : namespace autoware
      23                 :            : {
      24                 :            : namespace common
      25                 :            : {
      26                 :            : namespace helper_functions
      27                 :            : {
      28                 :            : /// \brief Calculate square of mahalanobis distance
      29                 :            : /// \tparam T Type of elements in the matrix
      30                 :            : /// \tparam kNumOfStates Number of states
      31                 :            : /// \param sample Single column matrix containing sample whose distance needs to be computed
      32                 :            : /// \param mean Single column matrix containing mean of samples received so far
      33                 :            : /// \param covariance_factor Covariance matrix
      34                 :            : /// \return Square of mahalanobis distance
      35                 :            : template<typename T, std::int32_t kNumOfStates>
      36                 :          2 : types::float32_t calculate_squared_mahalanobis_distance(
      37                 :            :   const Eigen::Matrix<T, kNumOfStates, 1> & sample,
      38                 :            :   const Eigen::Matrix<T, kNumOfStates, 1> & mean,
      39                 :            :   const Eigen::Matrix<T, kNumOfStates, kNumOfStates> & covariance_factor)
      40                 :            : {
      41                 :            :   using Vector = Eigen::Matrix<T, kNumOfStates, 1>;
      42                 :            :   // This is equivalent to the squared Mahalanobis distance of the form: diff.T * C.inv() * diff
      43                 :            :   // Instead of the covariance matrix C we have its lower-triangular factor L, such that C = L * L.T
      44                 :            :   // squared_mahalanobis_distance = diff.T * C.inv() * diff
      45                 :            :   // = diff.T * (L * L.T).inv() * diff
      46                 :            :   // = diff.T * L.T.inv() * L.inv() * diff
      47                 :            :   // = (L.inv() * diff).T * (L.inv() * diff)
      48                 :            :   // this allows us to efficiently find the squared Mahalanobis distance using (L.inv() * diff),
      49                 :            :   // which can be found as a solution to: L * x = diff.
      50   [ +  -  +  - ]:          2 :   const Vector diff = sample - mean;
      51   [ +  -  +  -  :          2 :   const Vector x = covariance_factor.ldlt().solve(diff);
                   +  - ]
      52   [ +  -  +  -  :          2 :   return x.transpose() * x;
                   +  - ]
      53                 :            : }
      54                 :            : 
      55                 :            : /// \brief Calculate mahalanobis distance
      56                 :            : /// \tparam T Type of elements in the matrix
      57                 :            : /// \tparam kNumOfStates Number of states
      58                 :            : /// \param sample Single column matrix containing sample whose distance needs to be computed
      59                 :            : /// \param mean Single column matrix containing mean of samples received so far
      60                 :            : /// \param covariance_factor Covariance matrix
      61                 :            : /// \return Mahalanobis distance
      62                 :            : template<typename T, std::int32_t kNumOfStates>
      63                 :          2 : types::float32_t calculate_mahalanobis_distance(
      64                 :            :   const Eigen::Matrix<T, kNumOfStates, 1> & sample,
      65                 :            :   const Eigen::Matrix<T, kNumOfStates, 1> & mean,
      66                 :            :   const Eigen::Matrix<T, kNumOfStates, kNumOfStates> & covariance_factor
      67                 :            : )
      68                 :            : {
      69                 :          2 :   return sqrtf(calculate_squared_mahalanobis_distance(sample, mean, covariance_factor));
      70                 :            : }
      71                 :            : }  // namespace helper_functions
      72                 :            : }  // namespace common
      73                 :            : }  // namespace autoware
      74                 :            : 
      75                 :            : #endif  // HELPER_FUNCTIONS__MAHALANOBIS_DISTANCE_HPP_