Public Member Functions | Protected Member Functions | Private Types | Private Member Functions | Private Attributes
ohm_tsd_slam::ThreadLocalize Class Reference

#include <ThreadLocalize.h>

Inheritance diagram for ohm_tsd_slam::ThreadLocalize:
Inheritance graph

List of all members.

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
< sensor_msgs::LaserScan * > 
obvious::Matrix * _lastPose
bool * _maskM
bool * _maskS
double * _modelCoords
double * _modelNormals
std::string _nameSpace
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


Registration with Icp only.


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 


gridPointer to representation
mapperPointer to mapping thread instance
nhPointer to main node handle
nameSpaceNamespace of this localization thread
xOffFactorOrigin x position
yOffFactorOrigin y position

Initialize member modules

Definition at line 29 of file ThreadLocalize.cpp.


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

TPointer to transformation matrix
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.

sensorLaser data container
MPointer to model points
NPointer to the normals of the model
SPointer to scene points
experimentalFlag to enable experimental matching
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

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

lastPosePointer to last known pose
curPosePointer to current pose
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.

TCurrent 2D transformation
trnsMaxTranslation thresh
rotMaxRotation thresh
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

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

MatInput data
maskValue mask
maskSizeAmount of values in the mask
validPointsValue determining the number of values in the output matrix
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.

maskInInput mask
matInInput data
maskOutOutput mask
matOutFiltered data
pointsInAmount of points in un filtered data
pointsOutAmount of points in filtered data
reductionFactorReduction factor

Definition at line 446 of file ThreadLocalize.cpp.

sendNanTransform Method sending an irregular pose and tf (consisting only of NAN values), broadcasting a detected registration error.

Definition at line 383 of file ThreadLocalize.cpp.

void ohm_tsd_slam::ThreadLocalize::sendTransform ( obvious::Matrix *  T) [private]

sendTransform Method to broadcast the gained transformation via ros::geometry_msgs::PoseStamped and ros::tf.

TBroadcasted transformation

Definition at line 359 of file ThreadLocalize.cpp.

Member Data Documentation

obvious::PairAssignment* ohm_tsd_slam::ThreadLocalize::_assigner [private]

ICP pair assigner

Definition at line 327 of file ThreadLocalize.h.

Mutex to synchronize main thread (subscriber) and thread event loop toDO: either this or the flas are obsolete

Definition at line 219 of file ThreadLocalize.h.

obvious::IRigidEstimator* ohm_tsd_slam::ThreadLocalize::_estimator [private]

ICP transformation estimator

Definition at line 347 of file ThreadLocalize.h.

obvious::OutOfBoundsFilter2D* ohm_tsd_slam::ThreadLocalize::_filterBounds [private]

ICP out of bounds filter

Definition at line 332 of file ThreadLocalize.h.

obvious::DistanceFilter* ohm_tsd_slam::ThreadLocalize::_filterDist [private]

ICP distance filter

Definition at line 337 of file ThreadLocalize.h.

obvious::ReciprocalFilter* ohm_tsd_slam::ThreadLocalize::_filterReciprocal [private]

ICP reciprogal filter

Definition at line 342 of file ThreadLocalize.h.

Height of tsd grid in m

Definition at line 214 of file ThreadLocalize.h.

Grid origin x offset

Definition at line 224 of file ThreadLocalize.h.

Grid origin y offset

Definition at line 229 of file ThreadLocalize.h.

Width of tsd grid in m

Definition at line 209 of file ThreadLocalize.h.

obvious::Icp* ohm_tsd_slam::ThreadLocalize::_icp [private]

ICP main icp instance

Definition at line 352 of file ThreadLocalize.h.

Flag signifying successful initialization of this thread

Definition at line 204 of file ThreadLocalize.h.

std::deque<sensor_msgs::LaserScan*> ohm_tsd_slam::ThreadLocalize::_laserData [private]

Container for laser sensor data (filled by callback)

Definition at line 292 of file ThreadLocalize.h.

obvious::Matrix* ohm_tsd_slam::ThreadLocalize::_lastPose [private]

Last pose

Definition at line 357 of file ThreadLocalize.h.

Pointer to mapping thread

Definition at line 194 of file ThreadLocalize.h.

Mask of model

Definition at line 307 of file ThreadLocalize.h.

Mask of scene

Definition at line 317 of file ThreadLocalize.h.

Buffer for model coordinates

Definition at line 297 of file ThreadLocalize.h.

Buffer for model normals

Definition at line 302 of file ThreadLocalize.h.

namespace for all topics and services

Definition at line 287 of file ThreadLocalize.h.

Pointer to main NodeHandle

Definition at line 189 of file ThreadLocalize.h.

Ros pose publisher

Definition at line 362 of file ThreadLocalize.h.

geometry_msgs::PoseStamped ohm_tsd_slam::ThreadLocalize::_poseStamped [private]

Ros current pose

Definition at line 367 of file ThreadLocalize.h.

Threshold for experimental registration algorithm

Definition at line 272 of file ThreadLocalize.h.

Angle threshold for experimental registration algorithm

Definition at line 282 of file ThreadLocalize.h.

Control size set for experimental registration algorithm

Definition at line 277 of file ThreadLocalize.h.

Iterations for experimental registration alorithm

Definition at line 267 of file ThreadLocalize.h.

obvious::RayCastPolar2D* ohm_tsd_slam::ThreadLocalize::_rayCaster [private]


Definition at line 322 of file ThreadLocalize.h.

Registration mode

Definition at line 254 of file ThreadLocalize.h.

ICP rotation threshold

Definition at line 239 of file ThreadLocalize.h.

Buffer for scene coordinates

Definition at line 312 of file ThreadLocalize.h.

obvious::SensorPolar2D* ohm_tsd_slam::ThreadLocalize::_sensor [private]

Sensor container for handeling the current laser input and pose

Definition at line 199 of file ThreadLocalize.h.

Ros current transform

Definition at line 377 of file ThreadLocalize.h.

Ros tf interface

Definition at line 372 of file ThreadLocalize.h.

ICP translation threshold

Definition at line 234 of file ThreadLocalize.h.

Starting x offset

Definition at line 244 of file ThreadLocalize.h.

Starting y offset

Definition at line 249 of file ThreadLocalize.h.

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

Author(s): Philipp Koch, Stefan May, Markus Kühn
autogenerated on Thu Jun 6 2019 17:41:12