Autoware.Auto
|
|
#include <ndt_localizer.hpp>
Public Types | |
using | Transform = geometry_msgs::msg::TransformStamped |
using | PoseWithCovarianceStamped = geometry_msgs::msg::PoseWithCovarianceStamped |
using | Summary = localization_common::OptimizedRegistrationSummary |
Public Member Functions | |
NDTLocalizerBase (const NDTLocalizerConfigBase &config, const OptimizationProblemConfigT &optimization_problem_config, const OptimizerT &optimizer, ScanT &&scan) | |
template<typename MapT , Requires = traits::LocalizationMapConstraint<MapT>::value> | |
PoseWithCovarianceStamped | register_measurement (const CloudT &msg, const Transform &transform_initial, const MapT &map, Summary *const summary=nullptr) |
const ScanT & | scan () const noexcept |
Get the last used scan. More... | |
const OptimizerT & | optimizer () const noexcept |
Get the optimizer. More... | |
const NDTLocalizerConfigBase & | config () const noexcept |
Get the localizer configuration. More... | |
const OptimizationProblemConfigT & | optimization_problem_config () const noexcept |
Get the optimization problem configuration. More... | |
Protected Member Functions | |
virtual void | set_covariance (const NDTOptimizationProblemT &problem, const EigenPose< Real > &initial_guess, const EigenPose< Real > &pose_result, PoseWithCovarianceStamped &solution) const |
template<typename MapT > | |
void | validate_msg (const CloudT &msg, const MapT &map) const |
virtual void | validate_guess (const CloudT &msg, const Transform &transform_initial) const |
Base class for NDT based localizers. Implementations must implement the validation logic.
ScanT | Type of ndt scan. |
NDTOptimizationProblemT | Type of ndt optimization problem. |
OptimizerT | Type of optimizer. |
ConfigT | Type of localization configuration. |
using autoware::localization::ndt::NDTLocalizerBase< ScanT, NDTOptimizationProblemT, OptimizationProblemConfigT, OptimizerT >::PoseWithCovarianceStamped = geometry_msgs::msg::PoseWithCovarianceStamped |
using autoware::localization::ndt::NDTLocalizerBase< ScanT, NDTOptimizationProblemT, OptimizationProblemConfigT, OptimizerT >::Summary = localization_common::OptimizedRegistrationSummary |
using autoware::localization::ndt::NDTLocalizerBase< ScanT, NDTOptimizationProblemT, OptimizationProblemConfigT, OptimizerT >::Transform = geometry_msgs::msg::TransformStamped |
|
inlineexplicit |
Constructor
config | NDT localizer config |
optimization_problem_config | The optimization problem config |
optimizer | Optimizer to use during optimization. |
scan | Initial value of the ndt scan. This element is expected to be constructed in the implementation class and moved to the base class. |
|
inlinenoexcept |
Get the localizer configuration.
|
inlinenoexcept |
Get the optimization problem configuration.
|
inlinenoexcept |
Get the optimizer.
|
inline |
Register a measurement to the current map and return the transformation from map to the measurement.
MapT | Map type to be used. This map should conform the interface specied in LocalizationMapConstraint |
[in] | msg | Measurement message to register. |
[in] | transform_initial | Initial guess of the pose to initialize the localizer with in iterative processes like solving optimization problems. |
[in] | map | Map to register. |
[out] | summary | (Optional) Reference to the registration summary. |
std::logic_error | on measurements older than the map. |
std::domain_error | on pose estimates that are not within the configured duration range from the measurement. |
std::runtime_error | on numerical errors in the optimizer. |
|
inlinenoexcept |
Get the last used scan.
|
inlineprotectedvirtual |
Populate the covariance information of an ndt estimate using the information using existing information regarding scan, map and the optimization problem.
[in] | problem | Optimization problem. |
[in] | initial_guess | Initial transformation guess as a pose. |
[in] | pose_result | Estimated transformation as a pose. |
[out] | solution | Estimated transform message. |
|
inlineprotectedvirtual |
Check if the initial guess is valid. Following checks are made:
msg | Message to register |
transform_initial | Initial pose estimate |
std::domain_error | on untimely initial pose. |
|
inlineprotected |
Check if the received message is valid to be registered. Following checks are made:
msg | Message to register. |
map | The map to be registered on. |
std::logic_error | on old data. |