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