#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

Detailed Description

template<typename ScanT, typename NDTOptimizationProblemT, typename OptimizationProblemConfigT, typename OptimizerT>
class autoware::localization::ndt::NDTLocalizerBase< ScanT, NDTOptimizationProblemT, OptimizationProblemConfigT, OptimizerT >

Base class for NDT based localizers. Implementations must implement the validation logic.

Template Parameters

ScanT	Type of ndt scan.
NDTOptimizationProblemT	Type of ndt optimization problem.
OptimizerT	Type of optimizer.
ConfigT	Type of localization configuration.

Member Typedef Documentation

◆ PoseWithCovarianceStamped

template<typename ScanT, typename NDTOptimizationProblemT, typename OptimizationProblemConfigT, typename OptimizerT>

using autoware::localization::ndt::NDTLocalizerBase< ScanT, NDTOptimizationProblemT, OptimizationProblemConfigT, OptimizerT >::PoseWithCovarianceStamped = geometry_msgs::msg::PoseWithCovarianceStamped

◆ Summary

template<typename ScanT, typename NDTOptimizationProblemT, typename OptimizationProblemConfigT, typename OptimizerT>

using autoware::localization::ndt::NDTLocalizerBase< ScanT, NDTOptimizationProblemT, OptimizationProblemConfigT, OptimizerT >::Summary = localization_common::OptimizedRegistrationSummary

◆ Transform

template<typename ScanT, typename NDTOptimizationProblemT, typename OptimizationProblemConfigT, typename OptimizerT>

using autoware::localization::ndt::NDTLocalizerBase< ScanT, NDTOptimizationProblemT, OptimizationProblemConfigT, OptimizerT >::Transform = geometry_msgs::msg::TransformStamped

Constructor & Destructor Documentation

◆ NDTLocalizerBase()

template<typename ScanT, typename NDTOptimizationProblemT, typename OptimizationProblemConfigT, typename OptimizerT>

autoware::localization::ndt::NDTLocalizerBase< ScanT, NDTOptimizationProblemT, OptimizationProblemConfigT, OptimizerT >::NDTLocalizerBase	(	const NDTLocalizerConfigBase &	config,
		const OptimizationProblemConfigT &	optimization_problem_config,
		const OptimizerT &	optimizer,
		ScanT &&	scan
	)

inlineexplicit

Constructor

Parameters

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.

Member Function Documentation

◆ config()

template<typename ScanT, typename NDTOptimizationProblemT, typename OptimizationProblemConfigT, typename OptimizerT>

const NDTLocalizerConfigBase& autoware::localization::ndt::NDTLocalizerBase< ScanT, NDTOptimizationProblemT, OptimizationProblemConfigT, OptimizerT >::config ( ) const

inlinenoexcept

Get the localizer configuration.

◆ optimization_problem_config()

template<typename ScanT, typename NDTOptimizationProblemT, typename OptimizationProblemConfigT, typename OptimizerT>

const OptimizationProblemConfigT& autoware::localization::ndt::NDTLocalizerBase< ScanT, NDTOptimizationProblemT, OptimizationProblemConfigT, OptimizerT >::optimization_problem_config ( ) const

inlinenoexcept

Get the optimization problem configuration.

◆ optimizer()

template<typename ScanT, typename NDTOptimizationProblemT, typename OptimizationProblemConfigT, typename OptimizerT>

const OptimizerT& autoware::localization::ndt::NDTLocalizerBase< ScanT, NDTOptimizationProblemT, OptimizationProblemConfigT, OptimizerT >::optimizer ( ) const

inlinenoexcept

Get the optimizer.

◆ register_measurement()

template<typename ScanT, typename NDTOptimizationProblemT, typename OptimizationProblemConfigT, typename OptimizerT>

template<typename MapT , Requires = traits::LocalizationMapConstraint<MapT>::value>

PoseWithCovarianceStamped autoware::localization::ndt::NDTLocalizerBase< ScanT, NDTOptimizationProblemT, OptimizationProblemConfigT, OptimizerT >::register_measurement	(	const CloudT &	msg,
		const Transform &	transform_initial,
		const MapT &	map,
		Summary *const	summary = `nullptr`
	)

inline

Register a measurement to the current map and return the transformation from map to the measurement.

Template Parameters

MapT	Map type to be used. This map should conform the interface specied in `LocalizationMapConstraint`

Parameters

[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.

Returns: Pose estimate after registration.

Exceptions

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.

◆ scan()

template<typename ScanT, typename NDTOptimizationProblemT, typename OptimizationProblemConfigT, typename OptimizerT>

const ScanT& autoware::localization::ndt::NDTLocalizerBase< ScanT, NDTOptimizationProblemT, OptimizationProblemConfigT, OptimizerT >::scan ( ) const

inlinenoexcept

Get the last used scan.

◆ set_covariance()

template<typename ScanT, typename NDTOptimizationProblemT, typename OptimizationProblemConfigT, typename OptimizerT>

virtual void autoware::localization::ndt::NDTLocalizerBase< ScanT, NDTOptimizationProblemT, OptimizationProblemConfigT, OptimizerT >::set_covariance	(	const NDTOptimizationProblemT &	problem,
		const EigenPose< Real > &	initial_guess,
		const EigenPose< Real > &	pose_result,
		PoseWithCovarianceStamped &	solution
	)		const

inlineprotectedvirtual

Populate the covariance information of an ndt estimate using the information using existing information regarding scan, map and the optimization problem.

Parameters

[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.

◆ validate_guess()

template<typename ScanT, typename NDTOptimizationProblemT, typename OptimizationProblemConfigT, typename OptimizerT>

virtual void autoware::localization::ndt::NDTLocalizerBase< ScanT, NDTOptimizationProblemT, OptimizationProblemConfigT, OptimizerT >::validate_guess	(	const CloudT &	msg,
		const Transform &	transform_initial
	)		const

inlineprotectedvirtual

Check if the initial guess is valid. Following checks are made:

pose guess timestamp is within a tolerated range from the scan timestamp.
Parameters

msg Message to register

transform_initial Initial pose estimate

Exceptions

std::domain_error on untimely initial pose.

◆ validate_msg()

template<typename ScanT, typename NDTOptimizationProblemT, typename OptimizationProblemConfigT, typename OptimizerT>

template<typename MapT >

void autoware::localization::ndt::NDTLocalizerBase< ScanT, NDTOptimizationProblemT, OptimizationProblemConfigT, OptimizerT >::validate_msg	(	const CloudT &	msg,
		const MapT &	map
	)		const

inlineprotected

Check if the received message is valid to be registered. Following checks are made:

Message timestamp is not older than the map timestamp.
Parameters

msg Message to register.

map The map to be registered on.

Exceptions

std::logic_error on old data.

The documentation for this class was generated from the following file:

ndt_localizer.hpp

msg	Message to register
transform_initial	Initial pose estimate

Public Types

Public Member Functions

Protected Member Functions

Detailed Description

template<typename ScanT, typename NDTOptimizationProblemT, typename OptimizationProblemConfigT, typename OptimizerT> class autoware::localization::ndt::NDTLocalizerBase< ScanT, NDTOptimizationProblemT, OptimizationProblemConfigT, OptimizerT >

Member Typedef Documentation

◆ PoseWithCovarianceStamped

◆ Summary

◆ Transform

Constructor & Destructor Documentation

◆ NDTLocalizerBase()

Member Function Documentation

◆ config()

◆ optimization_problem_config()

◆ optimizer()

◆ register_measurement()

◆ scan()

◆ set_covariance()

◆ validate_guess()

◆ validate_msg()

template<typename ScanT, typename NDTOptimizationProblemT, typename OptimizationProblemConfigT, typename OptimizerT>
class autoware::localization::ndt::NDTLocalizerBase< ScanT, NDTOptimizationProblemT, OptimizationProblemConfigT, OptimizerT >