LCOV - code coverage report
Current view: top level - src/localization/ndt/src - ndt_voxel_view.cpp (source / functions) Hit Total Coverage
Test: lcov.total.filtered Lines: 20 21 95.2 %
Date: 2023-03-03 05:44:19 Functions: 8 8 100.0 %
Legend: Lines: hit not hit | Branches: + taken - not taken # not executed Branches: 4 8 50.0 %

           Branch data     Line data    Source code
       1                 :            : // Copyright 2020 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_view.hpp>
      18                 :            : #include <utility>
      19                 :            : 
      20                 :            : namespace autoware
      21                 :            : {
      22                 :            : namespace localization
      23                 :            : {
      24                 :            : namespace ndt
      25                 :            : {
      26                 :            : 
      27                 :            : using StaticView = VoxelView<StaticNDTVoxel>;
      28                 :            : using DynamicView = VoxelView<DynamicNDTVoxel>;
      29                 :            : 
      30                 :      69254 : StaticView::VoxelView(const StaticNDTVoxel & voxel) : Base(voxel), m_usable{voxel.usable()} {}
      31                 :            : 
      32                 :      69204 : const StaticView::Cov & StaticView::inverse_covariance_() const
      33                 :            : {
      34                 :      69204 :   return this->get().inverse_covariance();
      35                 :            : }
      36                 :            : 
      37                 :      69254 : const StaticView::Point & StaticView::centroid_() const
      38                 :            : {
      39                 :      69254 :   return this->get().centroid();
      40                 :            : }
      41                 :            : 
      42                 :      69079 : bool8_t StaticView::usable_() const noexcept
      43                 :            : {
      44                 :      69079 :   return m_usable;
      45                 :            : }
      46                 :            : 
      47                 :       1925 : DynamicView::VoxelView(const DynamicNDTVoxel & voxel) : Base{voxel}, m_usable{voxel.usable()} {
      48         [ +  - ]:       1925 :   if (m_usable) {
      49                 :       1925 :     auto res = voxel.inverse_covariance();
      50         [ +  + ]:       1925 :     if (res) {
      51                 :            :       m_inverse_covariance = std::move(res.value());
      52                 :            :     } else {
      53                 :         50 :       m_usable = false;
      54                 :            :     }
      55                 :            :   }
      56                 :       1925 : }
      57                 :            : 
      58                 :       1750 : const DynamicView::Cov & DynamicView::inverse_covariance_() const
      59                 :            : {
      60         [ -  + ]:       1750 :   if (!m_usable) {
      61         [ #  # ]:          0 :     throw std::runtime_error("Cannot get inverse covariance from an invalid voxel.");
      62                 :            :   }
      63                 :       1750 :   return m_inverse_covariance;
      64                 :            : }
      65                 :            : 
      66                 :       1925 : const DynamicView::Point & DynamicView::centroid_() const
      67                 :            : {
      68                 :       1925 :   return this->get().centroid();
      69                 :            : }
      70                 :            : 
      71                 :       1625 : bool8_t DynamicView::usable_() const noexcept
      72                 :            : {
      73                 :       1625 :   return m_usable;
      74                 :            : }
      75                 :            : }  // namespace ndt
      76                 :            : }  // namespace localization
      77                 :            : }  // namespace autoware

Generated by: LCOV version 1.14