The Fitter class contains the algorithm to do the 2D fit of an entity. More...

#include <fitter.h>

Public Member Functions
void	configureBeamModel (const image_geometry::PinholeCameraModel &cammodel)
	configure the beam model (nr of data points and focal length) according to the camera you are using. More...

bool	estimateEntityPose (const FitterData &data, const ed::WorldModel &world, const ed::UUID &id, const geo::Pose3D &expected_pose, geo::Pose3D &fitted_pose, double max_yaw_change=M_PI) const
	estimateEntityPose performs the entity pose estimation. Basically, tries to call estimateEntityPoseImp and return the results, catches any FitterErrors and returns false More...

std::unique_ptr< OptimalFit >	findOptimum (const EstimationInputData &input_data, const YawRange &yaw_range) const
	findOptimum finds the 'optimal' fit by evaluating candidates over all beams and over the entire yaw range. More...

	Fitter ()
	Fitter default constructor, must be configured later. More...

	Fitter (const image_geometry::PinholeCameraModel &cammodel)
	Fitter constructor. More...

	Fitter (unsigned int nr_data_points, float fx)
	Fitter constructor. More...

EntityRepresentation2D	GetOrCreateEntity2D (const ed::EntityConstPtr &e) const
	GetOrCreateEntity2D returns the downprojected shape of the entity. If it's already in the cache, it's returned directly. If not, it's obtained from the entity. More...

bool	isConfigured ()

void	processSensorData (const rgbd::Image &image, const geo::Pose3D &sensor_pose, FitterData &data) const
	processSensorData pre-processes sensor data, i.e., performs a downprojection of the input depth image based on the provided sensor pose and stores the result in the FitterData struct More...

	~Fitter ()

Private Member Functions
void	checkExpectedBeamThroughEntity (const std::vector< double > &model_ranges, ed::EntityConstPtr entity, const geo::Pose3D &sensor_pose_xya, const int expected_center_beam) const
	checkExpectedBeamThroughEntity checks if the expected center beam passes through the entity. If not: throw a fitter error More...

bool	estimateEntityPoseImp (const FitterData &data, const ed::WorldModel &world, const ed::UUID &id, const geo::Pose3D &expected_pose, geo::Pose3D &fitted_pose, double max_yaw_change) const
	estimateEntityPoseImp actual implementation of the entity pose estimation. Preprocess input data iterates over all beams and the yaw range to compuate the best fit. Finally, a correction of the sensor pose is computed and the result is transformed back to 3D More...

bool	evaluateCandidate (const EstimationInputData &static_data, Candidate &candidate) const
	evaluateCandidate renders the model, transform the model along the beam direction to get this distance right and render once again More...

Shape2D	get2DShape (ed::EntityConstPtr entity_ptr) const
	get2DShape gets the downprojected, 2D shape from an entity More...

EstimationInputData	preProcessInputData (const ed::WorldModel &world, const ed::UUID &id, const geo::Pose3D &expected_pose, const FitterData &data) const
	preProcessInputData pre-processes the inputs for the fitting algorithm, i.e., gets the shape of the entity, compute its center, transform the shape to (0, 0), and compute the expected center beam. Everything is stored in a struct that can be used (const) throughout the rest of the algorithm More...

void	renderEntity (const ed::EntityConstPtr &e, const geo::Pose3D &sensor_pose_xya, int identifier, std::vector< double > &model_ranges, std::vector< int > &identifiers) const
	renderEntity renders the entity in 2D using the beam model with the provided (2D) sensor pose. The ranges of the simulated measurement are stored in 'model ranges' and the 'identifier' is stored in the 'identifiers' vector in order to be able to distinguish multiple entity shapes. More...

void	renderWorldModel2D (const ed::WorldModel &world, const geo::Pose3D &sensor_pose_xya, const ed::UUID &skip_id, std::vector< double > &model_ranges, std::vector< int > &identifiers) const
	renderWorldModel2D renders all world model entities More...

Private Attributes
BeamModel	beam_model_

bool	configured_ = false

std::map< ed::UUID, EntityRepresentation2D >	entity_shapes_

ed::models::ModelLoader	model_loader_

uint	nr_data_points_

Detailed Description

The Fitter class contains the algorithm to do the 2D fit of an entity.

Definition at line 100 of file fitter.h.

Constructor & Destructor Documentation

◆ Fitter() [1/3]

Fitter::Fitter ( )

Fitter default constructor, must be configured later.

Definition at line 235 of file fitter.cpp.

◆ Fitter() [2/3]

Fitter::Fitter	(	unsigned int	nr_data_points,
		float	fx
	)

Fitter constructor.

Parameters

nr_data_points	Number of data points for the beam model
fx	focal length for the beam model

Definition at line 242 of file fitter.cpp.

◆ Fitter() [3/3]

Fitter::Fitter ( const image_geometry::PinholeCameraModel & cammodel )

inline

Fitter constructor.

Parameters

cammodel Cameramodel used to configure the fitter

Definition at line 121 of file fitter.h.

◆ ~Fitter()

Fitter::~Fitter ( )

Definition at line 252 of file fitter.cpp.

Member Function Documentation

◆ checkExpectedBeamThroughEntity()

void Fitter::checkExpectedBeamThroughEntity	(	const std::vector< double > &	model_ranges,
		ed::EntityConstPtr	entity,
		const geo::Pose3D &	sensor_pose_xya,
		const int	expected_center_beam
	)		const

private

checkExpectedBeamThroughEntity checks if the expected center beam passes through the entity. If not: throw a fitter error

Parameters

model_ranges
entity
sensor_pose_xya
expected_center_beam	expected index of the beam through the center of the object. range: any int. indices outside bounds will also throw an error.

Definition at line 458 of file fitter.cpp.

◆ configureBeamModel()

void Fitter::configureBeamModel ( const image_geometry::PinholeCameraModel & cammodel )

configure the beam model (nr of data points and focal length) according to the camera you are using.

Parameters

cammodel camera model

Definition at line 475 of file fitter.cpp.

◆ estimateEntityPose()

bool Fitter::estimateEntityPose	(	const FitterData &	data,
		const ed::WorldModel &	world,
		const ed::UUID &	id,
		const geo::Pose3D &	expected_pose,
		geo::Pose3D &	fitted_pose,
		double	max_yaw_change = `M_PI`
	)		const

estimateEntityPose performs the entity pose estimation. Basically, tries to call estimateEntityPoseImp and return the results, catches any FitterErrors and returns false

Parameters

data	pre-processed sensor data
world	world model
id	id of the entity we're trying to fit
expected_pose	pose where the entity is expected
fitted_pose	the fitted pose is stored here
max_yaw_change	maximum allowed yaw rotation

Returns: success or failure

Definition at line 258 of file fitter.cpp.

◆ estimateEntityPoseImp()

bool Fitter::estimateEntityPoseImp	(	const FitterData &	data,
		const ed::WorldModel &	world,
		const ed::UUID &	id,
		const geo::Pose3D &	expected_pose,
		geo::Pose3D &	fitted_pose,
		double	max_yaw_change
	)		const

private

estimateEntityPoseImp actual implementation of the entity pose estimation. Preprocess input data iterates over all beams and the yaw range to compuate the best fit. Finally, a correction of the sensor pose is computed and the result is transformed back to 3D

Parameters

data	pre-processed sensor data
world	world model
id	id of the entity we're trying to fit
expected_pose	pose where the entity is expected
fitted_pose	the fitted pose is stored here
max_yaw_change	maximum allowed yaw rotation

Returns: success or failure

Definition at line 274 of file fitter.cpp.

◆ evaluateCandidate()

bool Fitter::evaluateCandidate	(	const EstimationInputData &	static_data,
		Candidate &	candidate
	)		const

private

evaluateCandidate renders the model, transform the model along the beam direction to get this distance right and render once again

Parameters

static_data	'static' input data that was the result from the pre-processing
candidate	contains the 'candidate' beam index and yaw that is evaluated

Returns

Definition at line 388 of file fitter.cpp.

◆ findOptimum()

std::unique_ptr< OptimalFit > Fitter::findOptimum	(	const EstimationInputData &	input_data,
		const YawRange &	yaw_range
	)		const

findOptimum finds the 'optimal' fit by evaluating candidates over all beams and over the entire yaw range.

Parameters

input_data	EstimationInputData: everything that's required
yaw_range	min and max yaw range to sample over

Returns: pointer to the optimum

Definition at line 347 of file fitter.cpp.

◆ get2DShape()

Shape2D Fitter::get2DShape ( ed::EntityConstPtr entity_ptr ) const

private

get2DShape gets the downprojected, 2D shape from an entity

Parameters

entity_ptr points to the entity

Returns: the computed shape

Definition at line 417 of file fitter.cpp.

◆ GetOrCreateEntity2D()

EntityRepresentation2D Fitter::GetOrCreateEntity2D ( const ed::EntityConstPtr & e ) const

GetOrCreateEntity2D returns the downprojected shape of the entity. If it's already in the cache, it's returned directly. If not, it's obtained from the entity.

Parameters

e	input entity

Returns: 2D entity representation

Definition at line 528 of file fitter.cpp.

◆ isConfigured()

bool Fitter::isConfigured ( )

inline

Definition at line 125 of file fitter.h.

◆ preProcessInputData()

EstimationInputData Fitter::preProcessInputData	(	const ed::WorldModel &	world,
		const ed::UUID &	id,
		const geo::Pose3D &	expected_pose,
		const FitterData &	data
	)		const

private

preProcessInputData pre-processes the inputs for the fitting algorithm, i.e., gets the shape of the entity, compute its center, transform the shape to (0, 0), and compute the expected center beam. Everything is stored in a struct that can be used (const) throughout the rest of the algorithm

Parameters

world	world model
id	id of the entity to fit
expected_pose	expected pose of the entity
data	input sensor data

Returns: EstimationInputData used throughout the rest of the algorithm

Definition at line 307 of file fitter.cpp.

◆ processSensorData()

void Fitter::processSensorData	(	const rgbd::Image &	image,
		const geo::Pose3D &	sensor_pose,
		FitterData &	data
	)		const

processSensorData pre-processes sensor data, i.e., performs a downprojection of the input depth image based on the provided sensor pose and stores the result in the FitterData struct

Parameters

[in]	image	input (depth) image
[in]	sensor_pose	pose of the sensor in the world while taking the image
[out]	data	processed data is stored here

Definition at line 486 of file fitter.cpp.

◆ renderEntity()

void Fitter::renderEntity	(	const ed::EntityConstPtr &	e,
		const geo::Pose3D &	sensor_pose_xya,
		int	identifier,
		std::vector< double > &	model_ranges,
		std::vector< int > &	identifiers
	)		const

private

renderEntity renders the entity in 2D using the beam model with the provided (2D) sensor pose. The ranges of the simulated measurement are stored in 'model ranges' and the 'identifier' is stored in the 'identifiers' vector in order to be able to distinguish multiple entity shapes.

Parameters

e	entity to render
sensor_pose_xya	2D sensor pose (i.e., only x, y and yaw are used)
identifier
model_ranges
identifiers

Definition at line 548 of file fitter.cpp.

◆ renderWorldModel2D()

void Fitter::renderWorldModel2D	(	const ed::WorldModel &	world,
		const geo::Pose3D &	sensor_pose_xya,
		const ed::UUID &	skip_id,
		std::vector< double > &	model_ranges,
		std::vector< int > &	identifiers
	)		const

private

renderWorldModel2D renders all world model entities

Parameters

world	world model
sensor_pose_xya	sensor pose
skip_id	id to skip (so we don't render the entity we're fitting
model_ranges	data is stored here
identifiers	to distinguish which 'range' belongs to which entity. In this case, -1 will be used as an identifier

Definition at line 431 of file fitter.cpp.

Member Data Documentation

◆ beam_model_

BeamModel Fitter::beam_model_

private

Definition at line 255 of file fitter.h.

◆ configured_

bool Fitter::configured_ = false

private

Definition at line 264 of file fitter.h.

◆ entity_shapes_

std::map<ed::UUID, EntityRepresentation2D> Fitter::entity_shapes_

mutableprivate

Definition at line 258 of file fitter.h.

◆ model_loader_

ed::models::ModelLoader Fitter::model_loader_

private

Definition at line 261 of file fitter.h.

◆ nr_data_points_

uint Fitter::nr_data_points_

private

Definition at line 263 of file fitter.h.

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

Public Member Functions

Private Member Functions

Private Attributes

Detailed Description

Constructor & Destructor Documentation

◆ Fitter() [1/3]

◆ Fitter() [2/3]

◆ Fitter() [3/3]

◆ ~Fitter()

Member Function Documentation

◆ checkExpectedBeamThroughEntity()

◆ configureBeamModel()

◆ estimateEntityPose()

◆ estimateEntityPoseImp()

◆ evaluateCandidate()

◆ findOptimum()

◆ get2DShape()

◆ GetOrCreateEntity2D()

◆ isConfigured()

◆ preProcessInputData()

◆ processSensorData()

◆ renderEntity()

◆ renderWorldModel2D()

Member Data Documentation

◆ beam_model_

◆ configured_

◆ entity_shapes_

◆ model_loader_

◆ nr_data_points_