#include <ThreadLocalize.h>

Inheritance diagram for ohm_tsd_slam::ThreadLocalize:

Public Member Functions
void	laserCallBack (const sensor_msgs::LaserScan &scan)
	ThreadLocalize (obvious::TsdGrid grid, ThreadMapping mapper, ros::NodeHandle *nh, std::string nameSpace, const double xOffset, const double yOffset)
virtual	~ThreadLocalize ()
Protected Member Functions
virtual void	eventLoop (void)
Private Types
enum	EnumRegModes { ICP = 0, EXP }
Private Member Functions
double	calcAngle (obvious::Matrix *T)
obvious::Matrix	doRegistration (obvious::SensorPolar2D sensor, obvious::Matrix M, obvious::Matrix Mvalid, obvious::Matrix N, obvious::Matrix Nvalid, obvious::Matrix S, obvious::Matrix *Svalid, const bool experimental)
void	init (const sensor_msgs::LaserScan &scan)
bool	isPoseChangeSignificant (obvious::Matrix lastPose, obvious::Matrix curPose)
bool	isRegistrationError (obvious::Matrix *T, const double trnsMax, const double rotMax)
obvious::Matrix	maskMatrix (obvious::Matrix Mat, bool mask, unsigned int maskSize, unsigned int validPoints)
void	reduceResolution (bool const maskIn, const obvious::Matrix matIn, bool const maskOut, obvious::Matrix matOut, unsigned int pointsIn, unsigned int pointsOut, unsigned int reductionFactor)
void	sendNanTransform ()
void	sendTransform (obvious::Matrix *T)
Private Attributes
obvious::PairAssignment *	_assigner
boost::mutex	_dataMutex
obvious::IRigidEstimator *	_estimator
obvious::OutOfBoundsFilter2D *	_filterBounds
obvious::DistanceFilter *	_filterDist
obvious::ReciprocalFilter *	_filterReciprocal
const double	_gridHeight
const double	_gridOffSetX
const double	_gridOffSetY
const double	_gridWidth
obvious::Icp *	_icp
bool	_initialized
std::deque < sensor_msgs::LaserScan * >	_laserData
obvious::Matrix *	_lastPose
ThreadMapping &	_mapper
bool *	_maskM
bool *	_maskS
double *	_modelCoords
double *	_modelNormals
std::string	_nameSpace
ros::NodeHandle *	_nh
ros::Publisher	_posePub
geometry_msgs::PoseStamped	_poseStamped
double	_ranEpsThresh
double	_ranPhiMax
unsigned int	_ranSizeCtrlSet
unsigned int	_ranTrials
obvious::RayCastPolar2D *	_rayCaster
EnumRegModes	_regMode
double	_rotMax
double *	_scene
obvious::SensorPolar2D *	_sensor
tf::StampedTransform	_tf
tf::TransformBroadcaster	_tfBroadcaster
double	_trnsMax
const double	_xOffset
const double	_yOffset

Detailed Description

Definition at line 48 of file ThreadLocalize.h.

Member Enumeration Documentation

enum ohm_tsd_slam::ThreadLocalize::EnumRegModes [private]

Enumerator:

ICP	Registration with Icp only.
EXP	Experimental Registration scheme, use with caution.

Definition at line 50 of file ThreadLocalize.h.

Constructor & Destructor Documentation

ohm_tsd_slam::ThreadLocalize::ThreadLocalize	(	obvious::TsdGrid *	grid,
		ThreadMapping *	mapper,
		ros::NodeHandle *	nh,
		std::string	nameSpace,
		const double	xOffset,
		const double	yOffset
	)

Constructor

Parameters:

grid	Pointer to representation
mapper	Pointer to mapping thread instance
nh	Pointer to main node handle
nameSpace	Namespace of this localization thread
xOffFactor	Origin x position
yOffFactor	Origin y position

Initialize member modules

Definition at line 29 of file ThreadLocalize.cpp.

ohm_tsd_slam::ThreadLocalize::~ThreadLocalize ( ) [virtual]

Destructor

Definition at line 120 of file ThreadLocalize.cpp.

Member Function Documentation

double ohm_tsd_slam::ThreadLocalize::calcAngle ( obvious::Matrix * T ) [private]

calAngle Method to analyze a 2D transformation matrix

Parameters:

T	Pointer to transformation matrix

Returns:: Calculated angle

Definition at line 404 of file ThreadLocalize.cpp.

obvious::Matrix ohm_tsd_slam::ThreadLocalize::doRegistration	(	obvious::SensorPolar2D *	sensor,
		obvious::Matrix *	M,
		obvious::Matrix *	Mvalid,
		obvious::Matrix *	N,
		obvious::Matrix *	Nvalid,
		obvious::Matrix *	S,
		obvious::Matrix *	Svalid,
		const bool	experimental
	)		`[private]`

doRegistration Main registration method. Aligns two laser scans and calculates the referring 2D transformation matrix.

Parameters:

sensor	Laser data container
M	Pointer to model points
Mvalid
N	Pointer to the normals of the model
Nvalid
S	Pointer to scene points
Svalid
experimental	Flag to enable experimental matching

Returns:: 2D transformation matrix

Definition at line 292 of file ThreadLocalize.cpp.

void ohm_tsd_slam::ThreadLocalize::eventLoop ( void ) [protected, virtual]

eventLoop Thread event loop

Create Point Matrixes with structure [x1 y1; x2 y2; ..] M, N, and S are matrices that preserve the ray model of a laser scanner Xvalid matrices are matrices that do not preserve the ray model but contain only valid points

Align Laser scans

analyze registration result

Update MAP if necessary

Implements ohm_tsd_slam::ThreadSLAM.

Definition at line 147 of file ThreadLocalize.cpp.

void ohm_tsd_slam::ThreadLocalize::init ( const sensor_msgs::LaserScan & scan ) [private]

init Init function automatically called by firs received laser data

Parameters:

scan	Initial scan used to initialize parameters of the thread

Definition at line 245 of file ThreadLocalize.cpp.

bool ohm_tsd_slam::ThreadLocalize::isPoseChangeSignificant	(	obvious::Matrix *	lastPose,
		obvious::Matrix *	curPose
	)		`[private]`

isPoseChangeSignificant Method to determine whether the localized sensor has been moved significantly (value above thresh). A map change is only initiated in case this method returns true.

Parameters:

lastPose	Pointer to last known pose
curPose	Pointer to current pose

Returns:: True in case of a significant pose change

Definition at line 417 of file ThreadLocalize.cpp.

bool ohm_tsd_slam::ThreadLocalize::isRegistrationError	(	obvious::Matrix *	T,
		const double	trnsMax,
		const double	rotMax
	)		`[private]`

isRegistrationError Method to prevent registration errors by comparing the computed transformation to thresholds.

Parameters:

T	Current 2D transformation
trnsMax	Translation thresh
rotMax	Rotation thresh

Returns:: True in case of an error

Definition at line 350 of file ThreadLocalize.cpp.

void ohm_tsd_slam::ThreadLocalize::laserCallBack ( const sensor_msgs::LaserScan & scan )

laserCallBack Callback method for laser scan message

Parameters:

scan	Laser data

Definition at line 130 of file ThreadLocalize.cpp.

obvious::Matrix ohm_tsd_slam::ThreadLocalize::maskMatrix	(	obvious::Matrix *	Mat,
		bool *	mask,
		unsigned int	maskSize,
		unsigned int	validPoints
	)		`[private]`

maskMatrix Method to remove certain values in a matrix using a given mask.

Parameters:

Mat	Input data
mask	Value mask
maskSize	Amount of values in the mask
validPoints	Value determining the number of values in the output matrix

Returns:: Filtered matrix

Definition at line 427 of file ThreadLocalize.cpp.

void ohm_tsd_slam::ThreadLocalize::reduceResolution	(	bool *const	maskIn,
		const obvious::Matrix *	matIn,
		bool *const	maskOut,
		obvious::Matrix *	matOut,
		unsigned int	pointsIn,
		unsigned int	pointsOut,
		unsigned int	reductionFactor
	)		`[private]`

reduceResolution Method to reduce the values in a matrix with a given factor.

Parameters:

maskIn	Input mask
matIn	Input data
maskOut	Output mask
matOut	Filtered data
pointsIn	Amount of points in un filtered data
pointsOut	Amount of points in filtered data
reductionFactor	Reduction factor