LCOV - code coverage report
Current view: top level - src/localization/ndt/src - ndt_voxel.cpp (source / functions) Hit Total Coverage
Test: lcov.total.filtered Lines: 55 59 93.2 %
Date: 2023-03-03 05:44:19 Functions: 14 14 100.0 %
Legend: Lines: hit not hit | Branches: + taken - not taken # not executed Branches: 22 36 61.1 %

           Branch data     Line data    Source code
       1                 :            : // Copyright 2019 the Autoware Foundation
       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                 :            : #include <ndt/ndt_voxel.hpp>
      18                 :            : #include <ndt/utils.hpp>
      19                 :            : #include <Eigen/LU>
      20                 :            : #include <limits>
      21                 :            : #include "common/types.hpp"
      22                 :            : 
      23                 :            : using autoware::common::types::bool8_t;
      24                 :            : 
      25                 :            : namespace autoware
      26                 :            : {
      27                 :            : namespace localization
      28                 :            : {
      29                 :            : namespace ndt
      30                 :            : {
      31                 :            : constexpr uint32_t DynamicNDTVoxel::NUM_POINT_THRESHOLD;
      32                 :            : 
      33                 :      18728 : DynamicNDTVoxel::DynamicNDTVoxel()
      34                 :            : {
      35                 :            :   // default constructors use uninitialized values.
      36                 :            :   m_centroid.setZero();
      37                 :            :   m_M2.setZero();
      38                 :            :   m_covariance.setZero();
      39                 :      18728 : }
      40                 :            : 
      41                 :    5103060 : void DynamicNDTVoxel::add_observation(const Point & pt)
      42                 :            : {
      43                 :    5103060 :   const auto last_count = m_num_points++;
      44                 :            : 
      45                 :            :   const auto last_centroid = m_centroid;
      46                 :            :   const auto last_delta = pt - last_centroid;
      47                 :    5103060 :   m_centroid = last_centroid + (last_delta / m_num_points);
      48                 :            : 
      49                 :    5103060 :   const auto current_delta = pt - m_centroid;
      50                 :    5103060 :   m_M2 += last_delta * current_delta.transpose();
      51                 :            : 
      52         [ +  + ]:    5103060 :   if (usable()) {
      53                 :            :     // TODO(yunus.caliskan): Apply numerical stability enhancing steps described in:
      54                 :            :     // http://www.diva-portal.org/smash/get/diva2:276162/FULLTEXT02.pdf, pg 60
      55                 :            :     m_covariance = m_M2 / last_count;
      56                 :            :   }
      57                 :            : 
      58                 :            :   // set invertibility to unknown since the covariance has changed
      59                 :    5103060 :   m_invertible = Invertibility::UNKNOWN;
      60                 :    5103060 : }
      61                 :            : 
      62                 :            : 
      63                 :      19951 : bool8_t DynamicNDTVoxel::try_stabilize()
      64                 :            : {
      65                 :      19951 :   bool8_t invertible = try_stabilize_covariance(m_covariance);
      66         [ +  + ]:      19951 :   if (invertible) {
      67                 :      16684 :     m_invertible = Invertibility::INVERTIBLE;
      68                 :            :   } else {
      69                 :       3267 :     m_invertible = Invertibility::NOT_INVERTIBLE;
      70                 :            :   }
      71                 :            : 
      72                 :      19951 :   return invertible;
      73                 :            : }
      74                 :            : 
      75                 :    5164670 : bool8_t DynamicNDTVoxel::usable() const noexcept
      76                 :            : {
      77                 :    5164670 :   return m_num_points >= NUM_POINT_THRESHOLD;
      78                 :            : }
      79                 :            : 
      80                 :        132 : const Eigen::Matrix3d & DynamicNDTVoxel::covariance() const
      81                 :            : {
      82         [ +  + ]:        132 :   if (!usable()) {
      83                 :            :     throw std::out_of_range(
      84                 :            :             "DynamicNDTVoxel: Cannot get covariance from a "
      85         [ +  - ]:          4 :             "voxel without sufficient number of points");
      86                 :            :   }
      87                 :        128 :   return m_covariance;
      88                 :            : }
      89                 :            : 
      90                 :      19110 : std::experimental::optional<Eigen::Matrix3d> DynamicNDTVoxel::inverse_covariance() const
      91                 :            : {
      92         [ -  + ]:      19110 :   if (!usable()) {
      93                 :            :     throw std::out_of_range(
      94                 :            :             "DynamicNDTVoxel: Cannot get covariance from a "
      95         [ #  # ]:          0 :             "voxel without sufficient number of points");
      96                 :            :   }
      97                 :            : 
      98         [ +  + ]:      19110 :   if (m_invertible == Invertibility::NOT_INVERTIBLE) {
      99                 :            :     // if stabilization has been performed and covariance is not invertible
     100                 :         50 :     return {};
     101                 :            :   }
     102                 :            : 
     103                 :            :   Eigen::Matrix3d inv_covariance;
     104                 :            :   bool8_t invertible;
     105                 :      19060 :   m_covariance.computeInverseWithCheck(inv_covariance, invertible);
     106         [ +  - ]:      19060 :   if (invertible) {
     107                 :            :     return inv_covariance;
     108                 :            :   } else {
     109                 :          0 :     return {};
     110                 :            :   }
     111                 :            : }
     112                 :            : 
     113                 :      19338 : const Eigen::Vector3d & DynamicNDTVoxel::centroid() const
     114                 :            : {
     115                 :            :   // Using the overloaded function as the parent function will use the hidden occupancy check
     116         [ +  + ]:      19338 :   if (!usable()) {
     117         [ +  - ]:          2 :     throw std::out_of_range("DynamicNDTVoxel: Cannot get centroid from an unoccupied voxel");
     118                 :            :   }
     119                 :      19336 :   return m_centroid;
     120                 :            : }
     121                 :            : 
     122                 :        177 : uint64_t DynamicNDTVoxel::count() const noexcept
     123                 :            : {
     124                 :        177 :   return m_num_points;
     125                 :            : }
     126                 :            : 
     127                 :            : /////////////////////////////////////////////////
     128                 :            : 
     129                 :          1 : StaticNDTVoxel::StaticNDTVoxel()
     130                 :            : {
     131                 :            :   m_centroid.setZero();
     132                 :            :   m_inv_covariance.setZero();
     133                 :          1 : }
     134                 :            : 
     135                 :      34468 : StaticNDTVoxel::StaticNDTVoxel(const Point & centroid, const Cov & inv_covariance)
     136                 :      34468 : : m_centroid{centroid}, m_inv_covariance{inv_covariance}, m_occupied{true}
     137                 :      34468 : {}
     138                 :            : 
     139                 :          2 : Eigen::Matrix3d StaticNDTVoxel::covariance() const
     140                 :            : {
     141                 :            :   Eigen::Matrix3d covariance;
     142         [ +  + ]:          2 :   if (m_occupied) {
     143                 :          1 :     bool8_t invertible{false};
     144                 :          1 :     m_inv_covariance.computeInverseWithCheck(covariance, invertible);
     145         [ -  + ]:          1 :     if (!invertible) {
     146         [ #  # ]:          0 :       throw std::out_of_range("StaticNDTVoxel: Inverse covariance is not invertible");
     147                 :            :     }
     148                 :            :   } else {
     149         [ +  - ]:          1 :     throw std::out_of_range("StaticNDTVoxel: Cannot get covariance from an unoccupied voxel");
     150                 :            :   }
     151                 :          1 :   return covariance;
     152                 :            : }
     153                 :            : 
     154                 :      69257 : const Eigen::Vector3d & StaticNDTVoxel::centroid() const
     155                 :            : {
     156         [ +  + ]:      69257 :   if (!m_occupied) {
     157         [ +  - ]:          1 :     throw std::out_of_range("StaticNDTVoxel: Cannot get centroid from an unoccupied voxel");
     158                 :            :   }
     159                 :      69256 :   return m_centroid;
     160                 :            : }
     161                 :            : 
     162                 :      69206 : const Eigen::Matrix3d & StaticNDTVoxel::inverse_covariance() const
     163                 :            : {
     164         [ -  + ]:      69206 :   if (!m_occupied) {
     165                 :            :     throw std::out_of_range(
     166                 :            :             "StaticNDTVoxel: Cannot get inverse covariance "
     167         [ #  # ]:          0 :             "from an unoccupied voxel");
     168                 :            :   }
     169                 :      69206 :   return m_inv_covariance;
     170                 :            : }
     171                 :            : 
     172                 :     138512 : bool8_t StaticNDTVoxel::usable() const noexcept
     173                 :            : {
     174                 :     138512 :   return m_occupied;
     175                 :            : }
     176                 :            : }  // namespace ndt
     177                 :            : }  // namespace localization
     178                 :            : }  // namespace autoware

Generated by: LCOV version 1.14