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 : : #ifndef NDT__NDT_CONFIG_HPP_ 18 : : #define NDT__NDT_CONFIG_HPP_ 19 : : 20 : : #include <ndt/ndt_common.hpp> 21 : : #include <voxel_grid/config.hpp> 22 : : #include <utility> 23 : : 24 : : namespace autoware 25 : : { 26 : : namespace localization 27 : : { 28 : : namespace ndt 29 : : { 30 : : /// Base config class for all ndt localizers 31 : : /// \tparam OptimizationConfigT Configuration class type for the used optimization problem. 32 : : class NDTLocalizerConfigBase 33 : : { 34 : : public: 35 : : /// Constructor 36 : : /// \param guess_time_tolerance 37 : : NDTLocalizerConfigBase( 38 : : std::chrono::nanoseconds guess_time_tolerance) 39 : 6 : : m_guess_time_tol{guess_time_tolerance} {} 40 : : 41 : : /// Get optimizer config. 42 : : /// \return optimizer config 43 : : const std::chrono::nanoseconds & guess_time_tolerance() const noexcept 44 : : { 45 : : return m_guess_time_tol; 46 : : } 47 : : 48 : : private: 49 : : std::chrono::nanoseconds m_guess_time_tol; 50 : : }; 51 : : 52 : : 53 : : /// Config class for p2d optimziation problem 54 : : class NDT_PUBLIC P2DNDTOptimizationConfig 55 : : { 56 : : public: 57 : : /// Constructor 58 : : /// \param outlier_ratio Outlier ratio to be used in the gaussian distribution variation used 59 : : /// in (eq. 6.7) [Magnusson 2009] 60 : : explicit P2DNDTOptimizationConfig(Real outlier_ratio) 61 : : : m_outlier_ratio{outlier_ratio} {} 62 : : 63 : : /// Get outlier ratio. 64 : : /// \return outlier ratio. 65 : : Real outlier_ratio() const noexcept {return m_outlier_ratio;} 66 : : 67 : : private: 68 : : Real m_outlier_ratio; 69 : : }; 70 : : 71 : : 72 : : /// config class for p2d ndt localizer 73 : : class NDT_PUBLIC P2DNDTLocalizerConfig : public NDTLocalizerConfigBase 74 : : { 75 : : public: 76 : : /// Constructor 77 : : /// \param scan_capacity Capacity of the ndt scan. This corresponds to the maximum number of 78 : : /// points expected in a single lidar scan. 79 : : /// \param guess_time_tolerance Time difference tolerance between the initial guess timestamp 80 : : /// and the timestamp of the scan. 81 : : P2DNDTLocalizerConfig( 82 : : const uint32_t scan_capacity, 83 : : std::chrono::nanoseconds guess_time_tolerance) 84 : 6 : : NDTLocalizerConfigBase{guess_time_tolerance}, 85 : 6 : m_scan_capacity(scan_capacity) {} 86 : : 87 : : /// Get scan capacity. 88 : : /// \return scan capacity. 89 : : uint32_t scan_capacity() const noexcept 90 : : { 91 : : return m_scan_capacity; 92 : : } 93 : : 94 : : private: 95 : : uint32_t m_scan_capacity; 96 : : }; 97 : : 98 : : } // namespace ndt 99 : : } // namespace localization 100 : : } // namespace autoware 101 : : 102 : : #endif // NDT__NDT_CONFIG_HPP_