IRI ROS Specific Driver Class. More...

#include <poseslam_alg.h>

Public Types
typedef iri_poseslam::PoseslamConfig	Config
	define config type
Public Member Functions
bool	any_candidate () const
	any candidate
void	augmentation (const uint &step, const geometry_msgs::PoseWithCovarianceStamped &odom)
	augmentation
void	config_update (Config &new_cfg, uint32_t level=0)
	config update
void	create_candidates_list (const bool &realOdometryCov)
	create candidates list
geometry_msgs::Pose	get_candidate_d () const
	get candidate d
double	get_candidate_ig () const
	get candidate Information Gain
uint	get_candidate_step () const
	get candidate step
std::vector< double >	get_last_covariance () const
	get last covariance
std::vector< double >	get_last_covariance_no_loops () const
	get last covariance no loops
VectorXd	get_last_pose () const
	get last pose
VectorXd	get_last_pose_no_loops () const
	get last pose no loops
uint	get_last_step () const
	get last step
uint	get_nStates () const
	get nStates
std::vector< VectorXd >	get_trajectory () const
	get trajectory
std::vector< std::vector < double > >	get_trajectory_covariance () const
	get trajectory covariance
std::vector< std::vector < double > >	get_trajectory_covariance_no_loops () const
	get trajectory covariance no loops
std::vector< VectorXd >	get_trajectory_no_loops () const
	get trajectory no loops
std::vector< uint >	get_trajectory_steps () const
	get trajectory steps
bool	is_redundant () const
	is redundant
void	lock (void)
	Lock Algorithm.
bool	loop_closure_requeriments () const
	loop closure requeriments
	PoseslamAlgorithm (void)
	constructor
void	print_matrix_in_file (const MatrixXd &M, std::fstream &file) const
	print matrix in file
void	print_time_file (std::fstream &file) const
	print time file
void	print_vector_in_file (const std::vector< double > &v, std::fstream &file) const
	print vector in file
void	print_vector_in_file (const std::vector< uint > &v, std::fstream &file) const
	print vector in file
void	redundant_evaluation ()
	redundant evaluation
void	select_best_candidate ()
	select best candidate
bool	try_enter (void)
	Tries Access to Algorithm.
bool	try_loop_closure (const geometry_msgs::PoseWithCovarianceStamped &odom)
	try loop closure
void	unlock (void)
	Unlock Algorithm.
void	update_candidates_list (const bool LoopClosure, const bool &realOdometryCov)
	update candidates list
	~PoseslamAlgorithm (void)
	Destructor.
Public Attributes
Config	config_
	config variable
std::vector< Eigen::VectorXd >	d_aug_
std::vector< Eigen::VectorXd >	d_loop_
bool	inicialitzat_
CPoseSLAM *	pose_SLAM_
CPoseSLAM *	pose_SLAM_noLoops_
std::vector< Eigen::MatrixXd >	Q_aug_
std::vector< Eigen::MatrixXd >	Q_loop_
Eigen::MatrixXd	Q_odom_
std::vector< bool >	success_loop_
std::vector< double >	timePub
std::vector< double >	timeSensClose
std::vector< double >	timeSensOpen
double	tPub
double	tSens
Protected Attributes
CMutex	alg_mutex_

Detailed Description

IRI ROS Specific Driver Class.

Definition at line 42 of file poseslam_alg.h.

Member Typedef Documentation

typedef iri_poseslam::PoseslamConfig PoseslamAlgorithm::Config

define config type

Define a Config type with the PoseslamConfig. All driver implementations will then use the same variable type Config.

Definition at line 58 of file poseslam_alg.h.

Constructor & Destructor Documentation

PoseslamAlgorithm::PoseslamAlgorithm ( void )

constructor

In this constructor parameters related to the specific driver can be initalized. Those parameters can be also set in the openDriver() function. Attributes from the main node driver class IriBaseDriver such as loop_rate, may be also overload here.

Definition at line 3 of file poseslam_alg.cpp.

PoseslamAlgorithm::~PoseslamAlgorithm ( void )

Destructor.

This destructor is called when the object is about to be destroyed.

Definition at line 9 of file poseslam_alg.cpp.

Member Function Documentation

bool PoseslamAlgorithm::any_candidate ( ) const

any candidate

Check if the candidate's list is empty or not.

Returns:: true if there is any candidate, false otherwise

Definition at line 180 of file poseslam_alg.cpp.

void PoseslamAlgorithm::augmentation	(	const uint &	step,
		const geometry_msgs::PoseWithCovarianceStamped &	odom
	)

augmentation

Does the state augmentation of the FlashFilter overwritting the previous pose if was redundant.

Parameters:

step	the step index of the new pose
d	the odometry with the previous pose
Q	the covariance of the odometry

Definition at line 85 of file poseslam_alg.cpp.

void PoseslamAlgorithm::config_update	(	Config &	new_cfg,
		uint32_t	level = `0`
	)

config update

In this function the driver parameters must be updated with the input config variable. Then the new configuration state will be stored in the Config attribute.

Parameters:

new_cfg	the new driver configuration state
level	level in which the update is taken place

Definition at line 15 of file poseslam_alg.cpp.

void PoseslamAlgorithm::create_candidates_list ( const bool & realOdometryCov )

create candidates list

Creates a list of the link candidates for loop closure with actual pose.

Parameters:

realOdometryCov if the information gain of link with previous must take the real covariance of the sensor

Definition at line 121 of file poseslam_alg.cpp.

geometry_msgs::Pose PoseslamAlgorithm::get_candidate_d ( ) const

get candidate d

Get the displacement of current link candidate.

Returns:: d of current link candidate

Definition at line 195 of file poseslam_alg.cpp.

double PoseslamAlgorithm::get_candidate_ig ( ) const

get candidate Information Gain

Get the information gain of current link candidate.

Returns:: the information gain of the candidate

Definition at line 209 of file poseslam_alg.cpp.

uint PoseslamAlgorithm::get_candidate_step ( ) const

get candidate step

Get the step of current candidate.

Returns:: index of current candidate

Definition at line 190 of file poseslam_alg.cpp.

std::vector< double > PoseslamAlgorithm::get_last_covariance ( ) const

get last covariance

Get last not-redundant position covariance in the trajectory.

Returns:: std::vector double of last non-redundant position' covariance in trajectory

Definition at line 273 of file poseslam_alg.cpp.

std::vector< double > PoseslamAlgorithm::get_last_covariance_no_loops ( ) const

get last covariance no loops

Get last not-redundant position covariance in the trajectory with no loop closure.

Returns:: std::vector double of last non-redundant position' covariance in trajectory

Definition at line 292 of file poseslam_alg.cpp.

VectorXd PoseslamAlgorithm::get_last_pose ( ) const

get last pose

Get last not-redundant position in the trajectory.

Returns:: vector of last non-redundant position in trajectory

Definition at line 268 of file poseslam_alg.cpp.

VectorXd PoseslamAlgorithm::get_last_pose_no_loops ( ) const

get last pose no loops

Get last not-redundant position in the trajectory with no loop closure.

Returns:: vector of last non-redundant position in trajectory

Definition at line 287 of file poseslam_alg.cpp.

uint PoseslamAlgorithm::get_last_step ( ) const

get last step

Get last not-redundant position index in the trajectory.

Returns:: integer, index of last non-redundant position in trajectory

Definition at line 282 of file poseslam_alg.cpp.

uint PoseslamAlgorithm::get_nStates ( ) const

get nStates

Get number of non redundant steps.

Returns:: integer

Definition at line 301 of file poseslam_alg.cpp.

std::vector< VectorXd > PoseslamAlgorithm::get_trajectory ( ) const

get trajectory

Get all not-redundant positions in the trajectory.

Returns:: std::vector of eigen::VectorXd of all non-redundant positions in trajectory

Definition at line 219 of file poseslam_alg.cpp.

std::vector< std::vector< double > > PoseslamAlgorithm::get_trajectory_covariance ( ) const

get trajectory covariance

Get all not-redundant positions' covariances in the trajectory.

Returns:: std::vector double of all non-redundant positions' covariances in trajectory

Definition at line 224 of file poseslam_alg.cpp.

std::vector< std::vector< double > > PoseslamAlgorithm::get_trajectory_covariance_no_loops ( ) const

get trajectory covariance no loops

Get all positions' covariances in the trajectory with no loop closure.

Returns:: std::vector double of all non-redundant positions' covariances in trajectory

Definition at line 251 of file poseslam_alg.cpp.

std::vector< VectorXd > PoseslamAlgorithm::get_trajectory_no_loops ( ) const

get trajectory no loops

Get all positions in the trajectorywith no loop closure.

Returns:: std::vector of row-major matrix floats of all positions in no the loop closure trajectory

Definition at line 246 of file poseslam_alg.cpp.

std::vector< uint > PoseslamAlgorithm::get_trajectory_steps ( ) const

get trajectory steps

Get all not-redundant positions index in the trajectory.

Returns:: std::vector of integers, indexs of all non-redundant positions in trajectory

Definition at line 241 of file poseslam_alg.cpp.

bool PoseslamAlgorithm::is_redundant ( ) const

is redundant

Check if last pose was redundant.

Returns:: true if last pose was redundant, false otherwise

Definition at line 214 of file poseslam_alg.cpp.

void PoseslamAlgorithm::lock ( void ) [inline]

Lock Algorithm.

Locks access to the Algorithm class

Definition at line 83 of file poseslam_alg.h.

bool PoseslamAlgorithm::loop_closure_requeriments ( ) const

loop closure requeriments

Executes the conditions of a link candidate for try to close the loop.

Returns:: true if the link can be a loop closed, false otherwise

Definition at line 134 of file poseslam_alg.cpp.

void PoseslamAlgorithm::print_matrix_in_file	(	const MatrixXd &	M,
		std::fstream &	file
	)		const

print matrix in file

Prints the given matrix in a matlab file.

Parameters:

M	Matrix to be printed
file	M-File to print

Definition at line 307 of file poseslam_alg.cpp.

void PoseslamAlgorithm::print_time_file ( std::fstream & file ) const

print time file

Prints all computation time data in a matlab file.

Parameters:

file	M-File to print

Definition at line 322 of file poseslam_alg.cpp.

void PoseslamAlgorithm::print_vector_in_file	(	const std::vector< double > &	v,
		std::fstream &	file
	)		const

print vector in file

Prints the given matrix in a matlab file.

Parameters:

v	std::vector<float> to be printed
file	M-File to print

Definition at line 312 of file poseslam_alg.cpp.

void PoseslamAlgorithm::print_vector_in_file	(	const std::vector< uint > &	v,
		std::fstream &	file
	)		const

print vector in file

Prints the given matrix in a matlab file.

Parameters:

v	std::vector<int> to be printed
file	M-File to print

Definition at line 317 of file poseslam_alg.cpp.

void PoseslamAlgorithm::redundant_evaluation ( )

redundant evaluation

Executes the conditions of a pose for be redundant for overwriting the next pose if it's the case.

Definition at line 129 of file poseslam_alg.cpp.

void PoseslamAlgorithm::select_best_candidate ( )

select best candidate

Select the best candidate of the list for trying loop closure.

Definition at line 185 of file poseslam_alg.cpp.

bool PoseslamAlgorithm::try_enter ( void ) [inline]

Tries Access to Algorithm.

Tries access to Algorithm

Returns:: true if the lock was adquired, false otherwise

Definition at line 99 of file poseslam_alg.h.

bool PoseslamAlgorithm::try_loop_closure ( const geometry_msgs::PoseWithCovarianceStamped & odom )

try loop closure

Try to close a loop if it's information gain enough with the real values of odometry and its noise and update the filter.

Parameters:

LoopClosureNoise	the real covariance of the odometry of the loop closure
LoopClosureD	the real odometry of the loop closure

Returns:: true if the loop closure, false otherwise

Definition at line 139 of file poseslam_alg.cpp.

void PoseslamAlgorithm::unlock ( void ) [inline]

Unlock Algorithm.

Unlocks access to the Algorithm class

Definition at line 90 of file poseslam_alg.h.

void PoseslamAlgorithm::update_candidates_list	(	const bool	LoopClosure,
		const bool &	realOdometryCov
	)

update candidates list

Update candidates list.

Parameters:

LoopClosure	if the loop was closed
realOdometryCov	if the information gain of link with previous must take the real covariance of the sensor

Definition at line 172 of file poseslam_alg.cpp.

Member Data Documentation

CMutex PoseslamAlgorithm::alg_mutex_ [protected]

Definition at line 46 of file poseslam_alg.h.

Config PoseslamAlgorithm::config_

config variable

This variable has all the driver parameters defined in the cfg config file. Is updated everytime function config_update() is called.

Definition at line 66 of file poseslam_alg.h.

std::vector<Eigen::VectorXd> PoseslamAlgorithm::d_aug_

Definition at line 388 of file poseslam_alg.h.

std::vector<Eigen::VectorXd> PoseslamAlgorithm::d_loop_

Definition at line 389 of file poseslam_alg.h.

bool PoseslamAlgorithm::inicialitzat_

Definition at line 385 of file poseslam_alg.h.

CPoseSLAM* PoseslamAlgorithm::pose_SLAM_

Definition at line 383 of file poseslam_alg.h.

CPoseSLAM* PoseslamAlgorithm::pose_SLAM_noLoops_

Definition at line 384 of file poseslam_alg.h.

std::vector<Eigen::MatrixXd> PoseslamAlgorithm::Q_aug_

Definition at line 386 of file poseslam_alg.h.

std::vector<Eigen::MatrixXd> PoseslamAlgorithm::Q_loop_

Definition at line 387 of file poseslam_alg.h.

Eigen::MatrixXd PoseslamAlgorithm::Q_odom_

Definition at line 391 of file poseslam_alg.h.

std::vector<bool> PoseslamAlgorithm::success_loop_

Definition at line 390 of file poseslam_alg.h.

std::vector<double> PoseslamAlgorithm::timePub

Definition at line 392 of file poseslam_alg.h.

std::vector<double> PoseslamAlgorithm::timeSensClose

Definition at line 394 of file poseslam_alg.h.

std::vector<double> PoseslamAlgorithm::timeSensOpen

Definition at line 393 of file poseslam_alg.h.

double PoseslamAlgorithm::tPub

Definition at line 395 of file poseslam_alg.h.

double PoseslamAlgorithm::tSens

Definition at line 395 of file poseslam_alg.h.

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

Public Types

Public Member Functions

Public Attributes

Protected Attributes

Detailed Description

Member Typedef Documentation

Constructor & Destructor Documentation

Member Function Documentation

Member Data Documentation