Public Member Functions | Private Types | Private Member Functions | Private Attributes
karto_scan_matcher::KartoScanMatcher Class Reference

Wraps the Karto Scan matcher. More...

#include <karto_scan_matcher.h>

List of all members.

Public Member Functions

 KartoScanMatcher (const sensor_msgs::LaserScan &init_scan, const tf::TransformListener &tf, double search_space_size, double search_grid_resolution)
 Constructor.
 KartoScanMatcher (const sensor_msgs::LaserScan &init_scan, const geometry_msgs::Pose2D &laser_pose, double search_space_size, double search_grid_resolution)
 Constructor when laser relative pose is known.
 KartoScanMatcher (const sensor_msgs::LaserScan &init_scan, const geometry_msgs::Pose2D &laser_pose, const DoubleVector &search_sizes, const DoubleVector &search_resolutions)
 Constructor when we have multiple matchers we want to use in sequence (increasingly fine)
ScanMatchResult scanMatch (const sensor_msgs::LaserScan &scan, const geometry_msgs::Pose2D &pose, const vector< ScanWithPose > &reference_scans) const
 Match scans.
void setPenalizeDistance (bool penalize)
 Set flag indicating whether to penalize distance from initial estimate in scan matching.
void setVisualizationPublisher (const std::string &topic, const std::string &frame)
 Sets the visualization topic. Scan match results will be visualized on this topic as PointCloud2 messages.

Private Types

typedef boost::shared_ptr
< karto::Mapper > 
MapperPtr
typedef boost::shared_ptr
< karto::ScanMatcher > 
MatcherPtr

Private Member Functions

void initialize (const sensor_msgs::LaserScan &init_scan, const geometry_msgs::Pose2D &laser_pose, const DoubleVector &search_space_sizes, const DoubleVector &search_space_resolutions)
void visualizeResult (const sensor_msgs::LaserScan &scan, const geometry_msgs::Pose2D &pose, const vector< ScanWithPose > &reference_scans, const geometry_msgs::Pose2D &res) const

Private Attributes

boost::shared_ptr< karto::Dataset > dataset_
karto::LaserRangeFinder * laser_
btTransform laser_to_base_
std::vector< MapperPtrmappers_
std::vector< MatcherPtrmatchers_
bool penalize_distance_
std::string vis_frame_
boost::optional< ros::Publishervis_pub_

Detailed Description

Wraps the Karto Scan matcher.

To use it, just construct an instance and call scanMatch as many times as you want. The main constraint is that laser offset and frame should be the same for all scans passed into the constructor and to scanMatch.

All scans are assumed to be in the base_laser_link frame, and poses are of the base_link frame.

Definition at line 111 of file karto_scan_matcher.h.


Member Typedef Documentation

typedef boost::shared_ptr<karto::Mapper> karto_scan_matcher::KartoScanMatcher::MapperPtr [private]

Definition at line 182 of file karto_scan_matcher.h.

typedef boost::shared_ptr<karto::ScanMatcher> karto_scan_matcher::KartoScanMatcher::MatcherPtr [private]

Definition at line 181 of file karto_scan_matcher.h.


Constructor & Destructor Documentation

karto_scan_matcher::KartoScanMatcher::KartoScanMatcher ( const sensor_msgs::LaserScan &  init_scan,
const tf::TransformListener tf,
double  search_space_size,
double  search_grid_resolution 
)

Constructor.

Parameters:
init_scanA scan that's used to set the parameters of the laser
tfUsed to figure out the offset of the laser (we're assuming this will be fixed henceforth)
namePrefixed to the sensor name (since all karto instances in this process have a global namespace)
search_space_sizeSize in meters of space to search over centered at initial estimate
search_grid_resolutionResolution (m/cell) of discretization of search space

Other parameters: penalize_distance is true on construction

karto_scan_matcher::KartoScanMatcher::KartoScanMatcher ( const sensor_msgs::LaserScan &  init_scan,
const geometry_msgs::Pose2D &  laser_pose,
double  search_space_size,
double  search_grid_resolution 
)

Constructor when laser relative pose is known.

Parameters:
init_scanA scan that's used to set the parameters of the laser
namePrefixed to the sensor name (since all karto instances in this process have a global namespace)
laser_poseRelative pose of the laser w.r.t. the base
search_space_sizeSize in meters of space to search over centered at initial estimate
search_grid_resolutionResolution (m/cell) of discretization of search space

Other parameters: penalize_distance is true on construction

karto_scan_matcher::KartoScanMatcher::KartoScanMatcher ( const sensor_msgs::LaserScan &  init_scan,
const geometry_msgs::Pose2D &  laser_pose,
const DoubleVector search_sizes,
const DoubleVector search_resolutions 
)

Constructor when we have multiple matchers we want to use in sequence (increasingly fine)

Parameters:
init_scanA scan that's used to set the parameters of the laser
laser_poseRelative pose of the laser w.r.t. the base
namePrefixed to the sensor name (since all karto instances in this process have a global namespace)
search_space_sizesThe sizes in meters of space to search over at each level (coarse to fine)
search_grid_resolutionsResolutions (m/cells) of discretizations of search space (match up with the ones in search_space_sizes)

Other parameters: penalize_distance is true on construction

Definition at line 106 of file karto_scan_matcher.cpp.


Member Function Documentation

void karto_scan_matcher::KartoScanMatcher::initialize ( const sensor_msgs::LaserScan &  init_scan,
const geometry_msgs::Pose2D &  laser_pose,
const DoubleVector search_space_sizes,
const DoubleVector search_space_resolutions 
) [private]

Definition at line 112 of file karto_scan_matcher.cpp.

ScanMatchResult karto_scan_matcher::KartoScanMatcher::scanMatch ( const sensor_msgs::LaserScan &  scan,
const geometry_msgs::Pose2D &  pose,
const vector< ScanWithPose > &  reference_scans 
) const

Match scans.

Parameters:
scanThe current laser scan.
poseThe initial estimate of the pose at which this scan was taken.
reference_scansEach of these is a scan, together with the pose at which it was taken
Return values:
Theoptimal pose, covariance, and response strength

If setVisualizationPublisher has been called, a visualization of the input scans and result will be published on the given ROS topic.

Definition at line 197 of file karto_scan_matcher.cpp.

Set flag indicating whether to penalize distance from initial estimate in scan matching.

Definition at line 167 of file karto_scan_matcher.cpp.

void karto_scan_matcher::KartoScanMatcher::setVisualizationPublisher ( const std::string &  topic,
const std::string &  frame 
)

Sets the visualization topic. Scan match results will be visualized on this topic as PointCloud2 messages.

Definition at line 258 of file karto_scan_matcher.cpp.

void karto_scan_matcher::KartoScanMatcher::visualizeResult ( const sensor_msgs::LaserScan &  scan,
const geometry_msgs::Pose2D &  pose,
const vector< ScanWithPose > &  reference_scans,
const geometry_msgs::Pose2D &  res 
) const [private]

Definition at line 300 of file karto_scan_matcher.cpp.


Member Data Documentation

boost::shared_ptr<karto::Dataset> karto_scan_matcher::KartoScanMatcher::dataset_ [private]

Definition at line 184 of file karto_scan_matcher.h.

karto::LaserRangeFinder* karto_scan_matcher::KartoScanMatcher::laser_ [private]

Definition at line 187 of file karto_scan_matcher.h.

Definition at line 192 of file karto_scan_matcher.h.

Definition at line 186 of file karto_scan_matcher.h.

Definition at line 185 of file karto_scan_matcher.h.

Definition at line 191 of file karto_scan_matcher.h.

Definition at line 189 of file karto_scan_matcher.h.

Definition at line 188 of file karto_scan_matcher.h.


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


karto_scan_matcher
Author(s): Bhaskara Marthi
autogenerated on Thu Jan 2 2014 12:02:42