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 : : #ifndef NDT__NDT_LOCALIZER_HPP_
18 : : #define NDT__NDT_LOCALIZER_HPP_
19 : :
20 : : #include <helper_functions/template_utils.hpp>
21 : : #include <localization_common/optimized_registration_summary.hpp>
22 : : #include <sensor_msgs/msg/point_cloud2.hpp>
23 : : #include <geometry_msgs/msg/transform.hpp>
24 : : #include <geometry_msgs/msg/transform_stamped.hpp>
25 : : #include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
26 : : #include <ndt/ndt_common.hpp>
27 : : #include <ndt/ndt_optimization_problem.hpp>
28 : : #include <ndt/constraints.hpp>
29 : : #include <optimization/optimizer_options.hpp>
30 : : #include <experimental/optional>
31 : : #include <utility>
32 : : #include <string>
33 : :
34 : : namespace autoware
35 : : {
36 : : namespace localization
37 : : {
38 : : namespace ndt
39 : : {
40 : : using CloudT = sensor_msgs::msg::PointCloud2;
41 : :
42 : : /// Base class for NDT based localizers. Implementations must implement the validation logic.
43 : : /// \tparam ScanT Type of ndt scan.
44 : : /// \tparam NDTOptimizationProblemT Type of ndt optimization problem.
45 : : /// \tparam OptimizerT Type of optimizer.
46 : : /// \tparam ConfigT Type of localization configuration.
47 : : template<
48 : : typename ScanT,
49 : : typename NDTOptimizationProblemT,
50 : : typename OptimizationProblemConfigT,
51 : : typename OptimizerT>
52 : : class NDT_PUBLIC NDTLocalizerBase
53 : : {
54 : : public:
55 : : using Transform = geometry_msgs::msg::TransformStamped;
56 : : using PoseWithCovarianceStamped = geometry_msgs::msg::PoseWithCovarianceStamped;
57 : : using Summary = localization_common::OptimizedRegistrationSummary;
58 : :
59 : : /// Constructor
60 : : ///
61 : : /// @param config NDT localizer config
62 : : /// @param optimization_problem_config The optimization problem config
63 : : /// @param optimizer Optimizer to use during optimization.
64 : : /// @param scan Initial value of the ndt scan. This element is
65 : : /// expected to be constructed in the implementation
66 : : /// class and moved to the base class.
67 : : ///
68 : 6 : explicit NDTLocalizerBase(
69 : : const NDTLocalizerConfigBase & config,
70 : : const OptimizationProblemConfigT & optimization_problem_config,
71 : : const OptimizerT & optimizer,
72 : : ScanT && scan)
73 : : : m_config{config},
74 : : m_optimization_problem_config{optimization_problem_config},
75 : : m_optimizer{optimizer},
76 : 6 : m_scan{std::forward<ScanT>(scan)} {}
77 : :
78 : : /// Register a measurement to the current map and return the transformation from map to the
79 : : /// measurement.
80 : : /// \tparam MapT Map type to be used. This map should conform the interface specied in
81 : : /// `LocalizationMapConstraint`
82 : : /// \param[in] msg Measurement message to register.
83 : : /// \param[in] transform_initial Initial guess of the pose to initialize the localizer with
84 : : /// in iterative processes like solving optimization problems.
85 : : /// \param[in] map Map to register.
86 : : /// \param[out] summary (Optional) Reference to the registration summary.
87 : : /// \return Pose estimate after registration.
88 : : /// \throws std::logic_error on measurements older than the map.
89 : : /// \throws std::domain_error on pose estimates that are not within the configured duration
90 : : /// range from the measurement.
91 : : /// \throws std::runtime_error on numerical errors in the optimizer.
92 : : template<typename MapT,
93 : : Requires = traits::LocalizationMapConstraint<MapT>::value>
94 : 12 : PoseWithCovarianceStamped register_measurement(
95 : : const CloudT & msg,
96 : : const Transform & transform_initial,
97 : : const MapT & map,
98 : : Summary * const summary = nullptr)
99 : : {
100 [ + - ]: 12 : PoseWithCovarianceStamped pose_out{};
101 [ + + ]: 12 : validate_msg(msg, map);
102 [ + + ]: 11 : validate_guess(msg, transform_initial);
103 : : // Initial checks passed, proceed with initialization
104 : : // Eigen representations to be used for internal computations.
105 [ + - + - ]: 9 : EigenPose<Real> eig_pose_initial, eig_pose_result;
106 [ + - ]: 9 : eig_pose_initial.setZero();
107 [ + - ]: 9 : eig_pose_result.setZero();
108 : : // Convert the ros transform/pose to eigen pose vector
109 [ + - ]: 9 : transform_adapters::transform_to_pose(transform_initial.transform, eig_pose_initial);
110 : :
111 : : // Set the scan
112 [ + - ]: 9 : m_scan.clear();
113 [ + - ]: 9 : m_scan.insert(msg);
114 : :
115 : : // Define and solve the problem.
116 [ + - ]: 9 : NDTOptimizationProblemT problem(m_scan, map, m_optimization_problem_config);
117 [ + - ]: 9 : const auto opt_summary = m_optimizer.solve(problem, eig_pose_initial, eig_pose_result);
118 : :
119 [ - + ]: 9 : if (opt_summary.termination_type() == common::optimization::TerminationType::FAILURE) {
120 : : throw std::runtime_error(
121 : : "NDT localizer has likely encountered a numerical "
122 [ # # ]: 0 : "error during optimization.");
123 : : }
124 : :
125 : : // Convert eigen pose back to ros pose/transform
126 : 9 : transform_adapters::pose_to_transform(
127 : : eig_pose_result,
128 [ + - ]: 9 : pose_out.pose.pose);
129 : :
130 : 9 : pose_out.header.stamp = msg.header.stamp;
131 [ + - ]: 9 : pose_out.header.frame_id = map.frame_id();
132 : :
133 : : // Populate covariance information. It is implementation defined.
134 [ + - ]: 9 : set_covariance(problem, eig_pose_initial, eig_pose_result, pose_out);
135 [ - + ]: 9 : if (summary != nullptr) {
136 [ # # ]: 0 : *summary = localization_common::OptimizedRegistrationSummary{opt_summary};
137 : : }
138 : 18 : return pose_out;
139 : : }
140 : :
141 : :
142 : : /// Get the last used scan.
143 : : const ScanT & scan() const noexcept
144 : : {
145 : : return m_scan;
146 : : }
147 : : /// Get the optimizer.
148 : : const OptimizerT & optimizer() const noexcept
149 : : {
150 : : return m_optimizer;
151 : : }
152 : : /// Get the localizer configuration.
153 : : const NDTLocalizerConfigBase & config() const noexcept
154 : : {
155 : : return m_config;
156 : : }
157 : : /// Get the optimization problem configuration.
158 : : const OptimizationProblemConfigT & optimization_problem_config() const noexcept
159 : : {
160 : : return m_optimization_problem_config;
161 : : }
162 : :
163 : : protected:
164 : : /// Populate the covariance information of an ndt estimate using the information using existing
165 : : /// information regarding scan, map and the optimization problem.
166 : : /// \param[in] problem Optimization problem.
167 : : /// \param[in] initial_guess Initial transformation guess as a pose.
168 : : /// \param[in] pose_result Estimated transformation as a pose.
169 : : /// \param[out] solution Estimated transform message.
170 : 0 : virtual void set_covariance(
171 : : const NDTOptimizationProblemT & problem,
172 : : const EigenPose<Real> & initial_guess,
173 : : const EigenPose<Real> & pose_result,
174 : : PoseWithCovarianceStamped & solution) const
175 : : {
176 : : (void) problem;
177 : : (void) initial_guess;
178 : : (void) pose_result;
179 : : (void) solution;
180 : : // For now, do nothing.
181 : 0 : }
182 : :
183 : : /// Check if the received message is valid to be registered. Following checks are made:
184 : : /// * Message timestamp is not older than the map timestamp.
185 : : /// \param msg Message to register.
186 : : /// \param map The map to be registered on.
187 : : /// \throws std::logic_error on old data.
188 : : template<typename MapT>
189 : 12 : void validate_msg(const CloudT & msg, const MapT & map) const
190 : : {
191 : 12 : const auto message_time = ::time_utils::from_message(msg.header.stamp);
192 : : // Map shouldn't be newer than a measurement.
193 [ + - + + ]: 12 : if (message_time < map.stamp()) {
194 : : throw std::logic_error(
195 : : "Lidar scan should not have a timestamp older than the timestamp of"
196 [ + - ]: 1 : "the current map.");
197 : : }
198 : 11 : }
199 : :
200 : : /// Check if the initial guess is valid. Following checks are made:
201 : : /// * pose guess timestamp is within a tolerated range from the scan timestamp.
202 : : /// \param msg Message to register
203 : : /// \param transform_initial Initial pose estimate
204 : : /// \throws std::domain_error on untimely initial pose.
205 : 11 : virtual void validate_guess(const CloudT & msg, const Transform & transform_initial) const
206 : : {
207 : 11 : const auto message_time = ::time_utils::from_message(msg.header.stamp);
208 : :
209 : 11 : const auto guess_scan_diff = ::time_utils::from_message(transform_initial.header.stamp) -
210 : : message_time;
211 : 11 : const auto stamp_tol = m_config.guess_time_tolerance();
212 : : // An initial estimate should be comparable in time to the measurement's time stamp
213 : 11 : if (std::abs(
214 [ + - ]: 11 : std::chrono::duration_cast<std::decay_t<decltype(stamp_tol)>>(guess_scan_diff).
215 [ + + ]: 11 : count()) >
216 : 11 : std::abs(stamp_tol.count()))
217 : : {
218 : : throw std::domain_error(
219 : : "Initial guess is not within: " + std::to_string(stamp_tol.count()) +
220 : : "ns range of the scan's time stamp. Either increase the tolerance range or"
221 [ + - + - : 2 : "make sure the localizer takes in timely initial pose guesses.");
+ - + - ]
222 : : }
223 : 9 : }
224 : :
225 : : private:
226 : : NDTLocalizerConfigBase m_config;
227 : : OptimizationProblemConfigT m_optimization_problem_config;
228 : : OptimizerT m_optimizer;
229 : : ScanT m_scan;
230 : : };
231 : :
232 : : /// P2D localizer implementation.
233 : : /// This implementation specifies a covariance computation method for the P2D optimization problem.
234 : : /// \tparam OptimizerT Optimizer type.
235 : : /// \tparam OptimizerOptionsT Optimizer options type.
236 : : /// \tparam MapT Type of map to be used. By default it is StaticNDTMap.
237 : : template<typename OptimizerT, typename MapT = StaticNDTMap>
238 : : class NDT_PUBLIC P2DNDTLocalizer : public NDTLocalizerBase<
239 : : P2DNDTScan, P2DNDTOptimizationProblem<MapT>, P2DNDTOptimizationConfig, OptimizerT>
240 : : {
241 : : public:
242 : : using CloudT = sensor_msgs::msg::PointCloud2;
243 : : using ParentT = NDTLocalizerBase<
244 : : P2DNDTScan, P2DNDTOptimizationProblem<MapT>, P2DNDTOptimizationConfig, OptimizerT>;
245 : : using Transform = typename ParentT::Transform;
246 : : using PoseWithCovarianceStamped = typename ParentT::PoseWithCovarianceStamped;
247 : : using ScanT = P2DNDTScan;
248 : :
249 : 6 : explicit P2DNDTLocalizer(
250 : : const P2DNDTLocalizerConfig & config,
251 : : const OptimizerT & optimizer,
252 : : const Real outlier_ratio)
253 : : : ParentT{
254 : : config,
255 : : P2DNDTOptimizationConfig{outlier_ratio},
256 : : optimizer,
257 [ + - ]: 6 : ScanT{config.scan_capacity()}} {}
258 : :
259 : : protected:
260 : 9 : void set_covariance(
261 : : const P2DNDTOptimizationProblem<MapT> &,
262 : : const EigenPose<Real> &,
263 : : const EigenPose<Real> &,
264 : : PoseWithCovarianceStamped &) const override
265 : : {
266 : : // For now, do nothing.
267 : 9 : }
268 : : };
269 : :
270 : : } // namespace ndt
271 : : } // namespace localization
272 : : } // namespace autoware
273 : :
274 : : #endif // NDT__NDT_LOCALIZER_HPP_
|
