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