#include <ndt_localizer.hpp>

Inheritance diagram for autoware::localization::ndt::NDTLocalizerBase< ScanT, MapT, NDTOptimizationProblemT, OptimizationProblemConfigT, OptimizerT >:

[legend]

Collaboration diagram for autoware::localization::ndt::NDTLocalizerBase< ScanT, MapT, NDTOptimizationProblemT, OptimizationProblemConfigT, OptimizerT >:

[legend]

Public Types
using	Transform = geometry_msgs::msg::TransformStamped

using	PoseWithCovarianceStamped = geometry_msgs::msg::PoseWithCovarianceStamped

Public Types inherited from autoware::localization::localization_common::RelativeLocalizerBase< CloudT, CloudT, localization_common::OptimizedRegistrationSummary >
using	PoseWithCovarianceStamped = geometry_msgs::msg::PoseWithCovarianceStamped

using	Transform = geometry_msgs::msg::TransformStamped

using	RegistrationSummary = localization_common::OptimizedRegistrationSummary

using	InputMsg = CloudT

using	MapMsg = CloudT

Public Member Functions
	NDTLocalizerBase (const NDTLocalizerConfigBase &config, const OptimizationProblemConfigT &optimization_problem_config, const OptimizerT &optimizer, ScanT &&scan, MapT &&map)

RegistrationSummary	register_measurement_impl (const CloudT &msg, const Transform &transform_initial, PoseWithCovarianceStamped &pose_out) override

void	set_map_impl (const CloudT &msg) override

void	insert_to_map_impl (const CloudT &msg) override

const ScanT &	scan () const noexcept
	Get the last used scan. More...

const MapT &	map () const noexcept
	Get the current map. 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...

const std::string &	map_frame_id () const noexcept override
	Get the frame id of the current map.(Required for base interface) More...

std::chrono::system_clock::time_point	map_stamp () const noexcept override
	Get the timestamp of the current map. (Required for base interface) More...

Public Member Functions inherited from autoware::localization::localization_common::RelativeLocalizerBase< CloudT, CloudT, localization_common::OptimizedRegistrationSummary >
localization_common::OptimizedRegistrationSummary	register_measurement (const CloudT &msg, const Transform &transform_initial, PoseWithCovarianceStamped &pose_out)

void	set_map (const CloudT &msg)

void	insert_to_map (const CloudT &msg)

virtual const std::string &	map_frame_id () const noexcept=0
	Get the frame id of the current map. More...

virtual std::chrono::system_clock::time_point	map_stamp () const noexcept=0
	Get the timestamp of the current map. More...

bool	map_valid () const noexcept

RelativeLocalizerBase &	operator= (RelativeLocalizerBase &&other)=delete

virtual	~RelativeLocalizerBase ()=default

Protected Member Functions
virtual void	set_covariance (const NDTOptimizationProblemT &problem, const EigenPose< Real > &initial_guess, const EigenPose< Real > &pose_result, PoseWithCovarianceStamped &solution) const

virtual void	validate_msg (const CloudT &msg) const

virtual void	validate_guess (const CloudT &msg, const Transform &transform_initial) const

Protected Member Functions inherited from autoware::localization::localization_common::RelativeLocalizerBase< CloudT, CloudT, localization_common::OptimizedRegistrationSummary >
virtual void	set_map_impl (const CloudT &msg)=0
	`set_map` implementation. More...

virtual void	insert_to_map_impl (const CloudT &)
	`insert_to_map` implementation. More...

virtual void	on_bad_map (std::exception_ptr eptr)
	Action to take on failure to set a new map. More...

virtual void	on_bad_map_insertion (std::exception_ptr eptr)
	Action to take on failure to insert to the map. More...

virtual localization_common::OptimizedRegistrationSummary	register_measurement_impl (const CloudT &msg, const Transform &transform_initial, PoseWithCovarianceStamped &pose_out)=0
	`register_measurement(...)` implementation. More...

void	set_map_valid () noexcept
	Set current map as valid. More...

void	set_map_invalid () noexcept
	Set current map as invalid. More...

virtual void	on_register_without_map ()
	Action to take when a measurement is attempted to be set without a valid map. More...

Detailed Description

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

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

Template Parameters

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

Member Typedef Documentation

◆ PoseWithCovarianceStamped

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

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

◆ Transform

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

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

Constructor & Destructor Documentation

◆ NDTLocalizerBase()

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

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

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.
map	Initial value of the ndt map. 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 MapT , typename NDTOptimizationProblemT , typename OptimizationProblemConfigT , typename OptimizerT >

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

inlinenoexcept

Get the localizer configuration.

◆ insert_to_map_impl()

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

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

inlineoverride

Insert the given message to the existing map.

Parameters

msg	Message containing the map addition.

◆ map()

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

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

inlinenoexcept

Get the current map.

◆ map_frame_id()

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

const std::string& autoware::localization::ndt::NDTLocalizerBase< ScanT, MapT, NDTOptimizationProblemT, OptimizationProblemConfigT, OptimizerT >::map_frame_id ( ) const

inlineoverridenoexcept

Get the frame id of the current map.(Required for base interface)

◆ map_stamp()

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

std::chrono::system_clock::time_point autoware::localization::ndt::NDTLocalizerBase< ScanT, MapT, NDTOptimizationProblemT, OptimizationProblemConfigT, OptimizerT >::map_stamp ( ) const

inlineoverridenoexcept

Get the timestamp of the current map. (Required for base interface)

◆ optimization_problem_config()

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

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

inlinenoexcept

Get the optimization problem configuration.

◆ optimizer()

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

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

inlinenoexcept

Get the optimizer.

◆ register_measurement_impl()

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

RegistrationSummary autoware::localization::ndt::NDTLocalizerBase< ScanT, MapT, NDTOptimizationProblemT, OptimizationProblemConfigT, OptimizerT >::register_measurement_impl	(	const CloudT &	msg,
		const Transform &	transform_initial,
		PoseWithCovarianceStamped &	pose_out
	)

inlineoverride

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

Parameters

[in]	msg	Point cloud to be registered.
[in]	transform_initial	Initial transformation guess.
[out]	pose_out	Transformation from the map frame to the measurement's frame.

Returns: Registration summary. T

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 MapT , typename NDTOptimizationProblemT , typename OptimizationProblemConfigT , typename OptimizerT >

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

inlinenoexcept

Get the last used scan.

◆ set_covariance()

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

virtual void autoware::localization::ndt::NDTLocalizerBase< ScanT, MapT, 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.

◆ set_map_impl()

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

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

inlineoverride

Replace the map with a given message

Parameters

msg	Message containing the map

◆ validate_guess()

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

virtual void autoware::localization::ndt::NDTLocalizerBase< ScanT, MapT, 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 MapT , typename NDTOptimizationProblemT , typename OptimizationProblemConfigT , typename OptimizerT >

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

inlineprotectedvirtual

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.

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 MapT, typename NDTOptimizationProblemT, typename OptimizationProblemConfigT, typename OptimizerT> class autoware::localization::ndt::NDTLocalizerBase< ScanT, MapT, NDTOptimizationProblemT, OptimizationProblemConfigT, OptimizerT >

Member Typedef Documentation

◆ PoseWithCovarianceStamped

◆ Transform

Constructor & Destructor Documentation

◆ NDTLocalizerBase()

Member Function Documentation

◆ config()

◆ insert_to_map_impl()

◆ map()

◆ map_frame_id()

◆ map_stamp()

◆ optimization_problem_config()

◆ optimizer()

◆ register_measurement_impl()

◆ scan()

◆ set_covariance()

◆ set_map_impl()

◆ validate_guess()

◆ validate_msg()

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