Go to the documentation of this file.
17 #ifndef NDT__NDT_LOCALIZER_HPP_
18 #define NDT__NDT_LOCALIZER_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>
30 #include <experimental/optional>
36 namespace localization
49 typename NDTOptimizationProblemT,
50 typename OptimizationProblemConfigT,
70 const OptimizationProblemConfigT & optimization_problem_config,
71 const OptimizerT & optimizer,
74 m_optimization_problem_config{optimization_problem_config},
75 m_optimizer{optimizer},
76 m_scan{std::forward<ScanT>(scan)} {}
92 template<
typename MapT,
98 Summary *
const summary =
nullptr)
101 validate_msg(msg, map);
102 validate_guess(msg, transform_initial);
106 eig_pose_initial.setZero();
107 eig_pose_result.setZero();
116 NDTOptimizationProblemT problem(m_scan, map, m_optimization_problem_config);
117 const auto opt_summary = m_optimizer.solve(problem, eig_pose_initial, eig_pose_result);
120 throw std::runtime_error(
121 "NDT localizer has likely encountered a numerical "
122 "error during optimization.");
130 pose_out.header.stamp = msg.header.stamp;
131 pose_out.header.frame_id = map.frame_id();
134 set_covariance(problem, eig_pose_initial, eig_pose_result, pose_out);
135 if (summary !=
nullptr) {
143 const ScanT &
scan() const noexcept
160 return m_optimization_problem_config;
171 const NDTOptimizationProblemT & problem,
177 (void) initial_guess;
188 template<
typename MapT>
193 if (message_time < map.stamp()) {
194 throw std::logic_error(
195 "Lidar scan should not have a timestamp older than the timestamp of"
211 const auto stamp_tol = m_config.guess_time_tolerance();
214 std::chrono::duration_cast<std::decay_t<decltype(stamp_tol)>>(guess_scan_diff).
216 std::abs(stamp_tol.count()))
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 "make sure the localizer takes in timely initial pose guesses.");
227 OptimizationProblemConfigT m_optimization_problem_config;
228 OptimizerT m_optimizer;
237 template<
typename OptimizerT,
typename MapT = StaticNDTMap>
239 P2DNDTScan, P2DNDTOptimizationProblem<MapT>, P2DNDTOptimizationConfig, OptimizerT>
251 const OptimizerT & optimizer,
252 const Real outlier_ratio)
274 #endif // NDT__NDT_LOCALIZER_HPP_
sensor_msgs::msg::PointCloud2 CloudT
Definition: ndt_localizer.hpp:242
uint32_t scan_capacity() const noexcept
Definition: ndt_config.hpp:89
Definition: optimization_problem.hpp:333
geometry_msgs::msg::TransformStamped Transform
Definition: ndt_localizer.hpp:55
void validate_msg(const CloudT &msg, const MapT &map) const
Definition: ndt_localizer.hpp:189
const OptimizerT & optimizer() const noexcept
Get the optimizer.
Definition: ndt_localizer.hpp:148
PoseWithCovarianceStamped register_measurement(const CloudT &msg, const Transform &transform_initial, const MapT &map, Summary *const summary=nullptr)
Definition: ndt_localizer.hpp:94
Definition: optimized_registration_summary.hpp:32
virtual void validate_guess(const CloudT &msg, const Transform &transform_initial) const
Definition: ndt_localizer.hpp:205
float64_t Real
Definition: ndt_common.hpp:38
const ScanT & scan() const noexcept
Get the last used scan.
Definition: ndt_localizer.hpp:143
NDTLocalizerBase(const NDTLocalizerConfigBase &config, const OptimizationProblemConfigT &optimization_problem_config, const OptimizerT &optimizer, ScanT &&scan)
Definition: ndt_localizer.hpp:68
P2DNDTLocalizer(const P2DNDTLocalizerConfig &config, const OptimizerT &optimizer, const Real outlier_ratio)
Definition: ndt_localizer.hpp:249
Config class for p2d optimziation problem.
Definition: ndt_config.hpp:54
Eigen::Matrix< T, 6U, 1U > EigenPose
Definition: localization/ndt/include/ndt/utils.hpp:128
Requires
Definition: ndt/include/ndt/constraints.hpp:31
This file defines the lanelet2_map_provider_node class.
Definition: quick_sort.hpp:24
Definition: ndt_scan.hpp:101
Definition: ndt_localizer.hpp:238
virtual void set_covariance(const NDTOptimizationProblemT &problem, const EigenPose< Real > &initial_guess, const EigenPose< Real > &pose_result, PoseWithCovarianceStamped &solution) const
Definition: ndt_localizer.hpp:170
Definition: ndt_config.hpp:32
geometry_msgs::msg::TransformStamped Transform
Definition: recordreplay_planner_node.hpp:56
typename ParentT::Transform Transform
Definition: ndt_localizer.hpp:245
geometry_msgs::msg::TransformStamped TransformStamped
Definition: motion_testing_publisher.hpp:37
TIME_UTILS_PUBLIC std::chrono::system_clock::time_point from_message(builtin_interfaces::msg::Time t) noexcept
Convert from std::chrono::time_point from time message.
Definition: time_utils.cpp:80
typename ParentT::PoseWithCovarianceStamped PoseWithCovarianceStamped
Definition: ndt_localizer.hpp:246
sensor_msgs::msg::PointCloud2 CloudT
Definition: ndt_localizer.hpp:40
Definition: ndt_localizer.hpp:52
static constexpr Requires value
Definition: ndt/include/ndt/constraints.hpp:61
geometry_msgs::msg::PoseWithCovarianceStamped PoseWithCovarianceStamped
Definition: ndt_localizer.hpp:56
const OptimizationProblemConfigT & optimization_problem_config() const noexcept
Get the optimization problem configuration.
Definition: ndt_localizer.hpp:158
void set_covariance(const P2DNDTOptimizationProblem< MapT > &, const EigenPose< Real > &, const EigenPose< Real > &, PoseWithCovarianceStamped &) const override
Definition: ndt_localizer.hpp:260
sensor_msgs::msg::PointCloud2 PointCloud2
Definition: filter_node_base.cpp:32
config class for p2d ndt localizer
Definition: ndt_config.hpp:73
const NDTLocalizerConfigBase & config() const noexcept
Get the localizer configuration.
Definition: ndt_localizer.hpp:153