Public Member Functions | Protected Member Functions | Private Types | Private Attributes
Mapper Class Reference

List of all members.

Public Member Functions

 Mapper (ros::NodeHandle &n, ros::NodeHandle &pn)
 Mapper (ros::NodeHandle &n, ros::NodeHandle &pn)
 ~Mapper ()
 ~Mapper ()

Protected Member Functions

bool correctPose (ethzasl_icp_mapper::CorrectPose::Request &req, ethzasl_icp_mapper::CorrectPose::Response &res)
bool correctPose (ethzasl_icp_mapper::CorrectPose::Request &req, ethzasl_icp_mapper::CorrectPose::Response &res)
bool getBoundedMap (ethzasl_icp_mapper::GetBoundedMap::Request &req, ethzasl_icp_mapper::GetBoundedMap::Response &res)
bool getBoundedMap (ethzasl_icp_mapper::GetBoundedMap::Request &req, ethzasl_icp_mapper::GetBoundedMap::Response &res)
bool getMode (ethzasl_icp_mapper::GetMode::Request &req, ethzasl_icp_mapper::GetMode::Response &res)
bool getMode (ethzasl_icp_mapper::GetMode::Request &req, ethzasl_icp_mapper::GetMode::Response &res)
bool getPointMap (map_msgs::GetPointMap::Request &req, map_msgs::GetPointMap::Response &res)
bool getPointMap (map_msgs::GetPointMap::Request &req, map_msgs::GetPointMap::Response &res)
string getTransParamCsvHeader (const PM::TransformationParameters T, const string prefix)
void gotCloud (const sensor_msgs::PointCloud2 &cloudMsgIn)
void gotCloud (const sensor_msgs::PointCloud2 &cloudMsgIn)
void gotScan (const sensor_msgs::LaserScan &scanMsgIn)
void gotScan (const sensor_msgs::LaserScan &scanMsgIn)
bool loadMap (ethzasl_icp_mapper::LoadMap::Request &req, ethzasl_icp_mapper::LoadMap::Response &res)
bool loadMap (ethzasl_icp_mapper::LoadMap::Request &req, ethzasl_icp_mapper::LoadMap::Response &res)
void processCloud (unique_ptr< DP > cloud, const std::string &scannerFrame, const ros::Time &stamp, uint32_t seq)
void processCloud (unique_ptr< DP > cloud, const std::string &scannerFrame, const ros::Time &stamp, uint32_t seq)
void processNewMapIfAvailable ()
void processNewMapIfAvailable ()
void publishLoop (double publishPeriod)
void publishLoop (double publishPeriod)
void publishTransform ()
void publishTransform ()
bool reset (std_srvs::Empty::Request &req, std_srvs::Empty::Response &res)
bool reset (std_srvs::Empty::Request &req, std_srvs::Empty::Response &res)
bool saveMap (map_msgs::SaveMap::Request &req, map_msgs::SaveMap::Response &res)
bool saveMap (map_msgs::SaveMap::Request &req, map_msgs::SaveMap::Response &res)
void savePathToCsv (string fileName)
string serializeTransParamCSV (const PM::TransformationParameters T, const bool getHeader=false, const string prefix="")
void setMap (DP *newPointCloud)
void setMap (DP *newPointCloud)
bool setMode (ethzasl_icp_mapper::SetMode::Request &req, ethzasl_icp_mapper::SetMode::Response &res)
bool setMode (ethzasl_icp_mapper::SetMode::Request &req, ethzasl_icp_mapper::SetMode::Response &res)
DPupdateMap (DP *newPointCloud, const PM::TransformationParameters Ticp, bool updateExisting)
DPupdateMap (DP *newPointCloud, const PM::TransformationParameters Ticp, bool updateExisting)
void waitForMapBuildingCompleted ()
void waitForMapBuildingCompleted ()

Private Types

typedef PM::DataPoints DP
typedef PM::DataPoints DP
typedef PM::Matches Matches
typedef
Nabo::NearestNeighbourSearch
< float > 
NNS
typedef NNS::SearchType NNSearchType
typedef PointMatcher< float > PM
typedef PointMatcher< float > PM

Private Attributes

const float alpha
 ratio. Propability of staying static given that the point was dynamic
const float beta
 ratio. Propability of staying dynamic given that the point was static
ros::Subscriber cloudSub
ros::ServiceServer correctPoseSrv
ros::Publisher debugPub
const float eps
const float eps_a
 ratio. Error proportional to the laser distance
const float eps_d
 in meter. Fix error on the laser distance
std::vector
< PM::TransformationParameters
errors
 vector of poses representing the correction applied by ICP.
ros::ServiceServer getBoundedMapSrv
ros::ServiceServer getModeSrv
ros::ServiceServer getPointMapSrv
PM::ICPSequence icp
PM::DataPointsFilters inputFilters
int inputQueueSize
ros::Time lastPoinCloudTime
ros::ServiceServer loadMapSrv
bool localizing
ros::Time mapCreationTime
const double mapElevation
string mapFrame
bool mapping
PM::DataPointsmapPointCloud
PM::DataPointsFilters mapPostFilters
PM::DataPointsFilters mapPreFilters
ros::Publisher mapPub
const float maxAngle
 in rad. Openning angle of a laser beam
const float maxDistNewPoint
 in meter. Distance at which a new point will be added in the global map.
const float maxDyn
 ratio. Threshold for which a point will stay dynamic
double maxOverlapToMerge
int minMapPointCount
double minOverlap
int minReadingPointCount
ros::NodeHandlen
ros::Publisher odomErrorPub
string odomFrame
ros::Publisher odomPub
ros::Publisher outlierPub
std::vector
< PM::TransformationParameters
path
 vector of poses representing the path after ICP correction.
ros::NodeHandlepn
const float priorDyn
 ratio. Prior to be dynamic when a new point is added
const float priorStatic
 ratio. Prior to be static when a new point is added
bool processingNewCloud
boost::mutex publishLock
bool publishMapTf
ros::Time publishStamp
boost::thread publishThread
ros::ServiceServer resetSrv
ros::ServiceServer saveMapSrv
ros::Subscriber scanSub
const float sensorMaxRange
 in meter. Radius in which the global map needs to be updated when a new scan arrived.
ros::ServiceServer setModeSrv
tf::TransformBroadcaster tfBroadcaster
tf::TransformListener tfListener
double tfRefreshPeriod
 if set to zero, tf will be publish at the rate of the incoming point cloud messages
PM::TransformationParameters TOdomToMap
unique_ptr< PM::Transformationtransformation
bool useConstMotionModel
string vtkFinalMapName
 name of the final vtk map

Detailed Description

Definition at line 43 of file dynamic_mapper.cpp.


Member Typedef Documentation

typedef PM::DataPoints Mapper::DP [private]

Definition at line 42 of file mapper.cpp.

typedef PM::DataPoints Mapper::DP [private]

Definition at line 46 of file dynamic_mapper.cpp.

typedef PM::Matches Mapper::Matches [private]

Definition at line 47 of file dynamic_mapper.cpp.

typedef Nabo::NearestNeighbourSearch<float> Mapper::NNS [private]

Definition at line 49 of file dynamic_mapper.cpp.

Definition at line 50 of file dynamic_mapper.cpp.

typedef PointMatcher<float> Mapper::PM [private]

Definition at line 41 of file mapper.cpp.

typedef PointMatcher<float> Mapper::PM [private]

Definition at line 45 of file dynamic_mapper.cpp.


Constructor & Destructor Documentation

Definition at line 230 of file dynamic_mapper.cpp.

Definition at line 376 of file dynamic_mapper.cpp.


Member Function Documentation

bool Mapper::correctPose ( ethzasl_icp_mapper::CorrectPose::Request &  req,
ethzasl_icp_mapper::CorrectPose::Response &  res 
) [protected]
bool Mapper::correctPose ( ethzasl_icp_mapper::CorrectPose::Request &  req,
ethzasl_icp_mapper::CorrectPose::Response &  res 
) [protected]

Definition at line 1153 of file dynamic_mapper.cpp.

bool Mapper::getBoundedMap ( ethzasl_icp_mapper::GetBoundedMap::Request &  req,
ethzasl_icp_mapper::GetBoundedMap::Response &  res 
) [protected]
bool Mapper::getBoundedMap ( ethzasl_icp_mapper::GetBoundedMap::Request &  req,
ethzasl_icp_mapper::GetBoundedMap::Response &  res 
) [protected]

Definition at line 1196 of file dynamic_mapper.cpp.

bool Mapper::getMode ( ethzasl_icp_mapper::GetMode::Request &  req,
ethzasl_icp_mapper::GetMode::Response &  res 
) [protected]
bool Mapper::getMode ( ethzasl_icp_mapper::GetMode::Request &  req,
ethzasl_icp_mapper::GetMode::Response &  res 
) [protected]

Definition at line 1187 of file dynamic_mapper.cpp.

bool Mapper::getPointMap ( map_msgs::GetPointMap::Request &  req,
map_msgs::GetPointMap::Response &  res 
) [protected]
bool Mapper::getPointMap ( map_msgs::GetPointMap::Request &  req,
map_msgs::GetPointMap::Response &  res 
) [protected]

Definition at line 1067 of file dynamic_mapper.cpp.

string Mapper::getTransParamCsvHeader ( const PM::TransformationParameters  T,
const string  prefix 
) [protected]

Definition at line 172 of file dynamic_mapper.cpp.

void Mapper::gotCloud ( const sensor_msgs::PointCloud2 &  cloudMsgIn) [protected]
void Mapper::gotCloud ( const sensor_msgs::PointCloud2 &  cloudMsgIn) [protected]

Definition at line 411 of file dynamic_mapper.cpp.

void Mapper::gotScan ( const sensor_msgs::LaserScan &  scanMsgIn) [protected]
void Mapper::gotScan ( const sensor_msgs::LaserScan &  scanMsgIn) [protected]

Definition at line 401 of file dynamic_mapper.cpp.

bool Mapper::loadMap ( ethzasl_icp_mapper::LoadMap::Request &  req,
ethzasl_icp_mapper::LoadMap::Response &  res 
) [protected]
bool Mapper::loadMap ( ethzasl_icp_mapper::LoadMap::Request &  req,
ethzasl_icp_mapper::LoadMap::Response &  res 
) [protected]

Definition at line 1100 of file dynamic_mapper.cpp.

void Mapper::processCloud ( unique_ptr< DP cloud,
const std::string scannerFrame,
const ros::Time stamp,
uint32_t  seq 
) [protected]
void Mapper::processCloud ( unique_ptr< DP cloud,
const std::string scannerFrame,
const ros::Time stamp,
uint32_t  seq 
) [protected]

Definition at line 437 of file dynamic_mapper.cpp.

void Mapper::processNewMapIfAvailable ( ) [protected]
void Mapper::processNewMapIfAvailable ( ) [protected]

Definition at line 662 of file dynamic_mapper.cpp.

void Mapper::publishLoop ( double  publishPeriod) [protected]
void Mapper::publishLoop ( double  publishPeriod) [protected]

Definition at line 1040 of file dynamic_mapper.cpp.

void Mapper::publishTransform ( ) [protected]
void Mapper::publishTransform ( ) [protected]

Definition at line 1052 of file dynamic_mapper.cpp.

bool Mapper::reset ( std_srvs::Empty::Request &  req,
std_srvs::Empty::Response &  res 
) [protected]
bool Mapper::reset ( std_srvs::Empty::Request &  req,
std_srvs::Empty::Response &  res 
) [protected]

Definition at line 1139 of file dynamic_mapper.cpp.

bool Mapper::saveMap ( map_msgs::SaveMap::Request &  req,
map_msgs::SaveMap::Response &  res 
) [protected]
bool Mapper::saveMap ( map_msgs::SaveMap::Request &  req,
map_msgs::SaveMap::Response &  res 
) [protected]

Definition at line 1077 of file dynamic_mapper.cpp.

void Mapper::savePathToCsv ( string  fileName) [protected]

Definition at line 208 of file dynamic_mapper.cpp.

string Mapper::serializeTransParamCSV ( const PM::TransformationParameters  T,
const bool  getHeader = false,
const string  prefix = "" 
) [protected]

Definition at line 178 of file dynamic_mapper.cpp.

void Mapper::setMap ( DP newPointCloud) [protected]
void Mapper::setMap ( DP newPointCloud) [protected]

Definition at line 672 of file dynamic_mapper.cpp.

bool Mapper::setMode ( ethzasl_icp_mapper::SetMode::Request &  req,
ethzasl_icp_mapper::SetMode::Response &  res 
) [protected]
bool Mapper::setMode ( ethzasl_icp_mapper::SetMode::Request &  req,
ethzasl_icp_mapper::SetMode::Response &  res 
) [protected]

Definition at line 1175 of file dynamic_mapper.cpp.

DP* Mapper::updateMap ( DP newPointCloud,
const PM::TransformationParameters  Ticp,
bool  updateExisting 
) [protected]
Mapper::DP * Mapper::updateMap ( DP newPointCloud,
const PM::TransformationParameters  Ticp,
bool  updateExisting 
) [protected]

Definition at line 692 of file dynamic_mapper.cpp.

Definition at line 1028 of file dynamic_mapper.cpp.


Member Data Documentation

const float Mapper::alpha [private]

ratio. Propability of staying static given that the point was dynamic

Definition at line 121 of file dynamic_mapper.cpp.

const float Mapper::beta [private]

ratio. Propability of staying dynamic given that the point was static

Definition at line 122 of file dynamic_mapper.cpp.

Definition at line 57 of file dynamic_mapper.cpp.

Definition at line 70 of file dynamic_mapper.cpp.

Definition at line 61 of file dynamic_mapper.cpp.

const float Mapper::eps [private]

Definition at line 139 of file dynamic_mapper.cpp.

const float Mapper::eps_a [private]

ratio. Error proportional to the laser distance

Definition at line 119 of file dynamic_mapper.cpp.

const float Mapper::eps_d [private]

in meter. Fix error on the laser distance

Definition at line 120 of file dynamic_mapper.cpp.

vector of poses representing the correction applied by ICP.

Definition at line 129 of file dynamic_mapper.cpp.

Definition at line 73 of file dynamic_mapper.cpp.

Definition at line 72 of file dynamic_mapper.cpp.

Definition at line 66 of file dynamic_mapper.cpp.

Definition at line 84 of file dynamic_mapper.cpp.

Definition at line 80 of file dynamic_mapper.cpp.

int Mapper::inputQueueSize [private]

Definition at line 105 of file dynamic_mapper.cpp.

Definition at line 77 of file dynamic_mapper.cpp.

Definition at line 68 of file dynamic_mapper.cpp.

bool Mapper::localizing [private]

Definition at line 101 of file dynamic_mapper.cpp.

Definition at line 76 of file dynamic_mapper.cpp.

const double Mapper::mapElevation [private]

Definition at line 113 of file dynamic_mapper.cpp.

Definition at line 110 of file dynamic_mapper.cpp.

bool Mapper::mapping [private]

Definition at line 102 of file dynamic_mapper.cpp.

Definition at line 83 of file dynamic_mapper.cpp.

Definition at line 82 of file dynamic_mapper.cpp.

Definition at line 81 of file dynamic_mapper.cpp.

Definition at line 60 of file dynamic_mapper.cpp.

const float Mapper::maxAngle [private]

in rad. Openning angle of a laser beam

Definition at line 118 of file dynamic_mapper.cpp.

const float Mapper::maxDistNewPoint [private]

in meter. Distance at which a new point will be added in the global map.

Definition at line 124 of file dynamic_mapper.cpp.

const float Mapper::maxDyn [private]

ratio. Threshold for which a point will stay dynamic

Definition at line 123 of file dynamic_mapper.cpp.

double Mapper::maxOverlapToMerge [private]

Definition at line 107 of file dynamic_mapper.cpp.

int Mapper::minMapPointCount [private]

Definition at line 104 of file dynamic_mapper.cpp.

double Mapper::minOverlap [private]

Definition at line 106 of file dynamic_mapper.cpp.

Definition at line 103 of file dynamic_mapper.cpp.

Definition at line 52 of file dynamic_mapper.cpp.

Definition at line 63 of file dynamic_mapper.cpp.

Definition at line 109 of file dynamic_mapper.cpp.

Definition at line 62 of file dynamic_mapper.cpp.

Definition at line 53 of file mapper.cpp.

vector of poses representing the path after ICP correction.

Definition at line 128 of file dynamic_mapper.cpp.

Definition at line 53 of file dynamic_mapper.cpp.

const float Mapper::priorDyn [private]

ratio. Prior to be dynamic when a new point is added

Definition at line 117 of file dynamic_mapper.cpp.

const float Mapper::priorStatic [private]

ratio. Prior to be static when a new point is added

Definition at line 116 of file dynamic_mapper.cpp.

Definition at line 96 of file dynamic_mapper.cpp.

boost::mutex Mapper::publishLock [private]

Definition at line 133 of file dynamic_mapper.cpp.

bool Mapper::publishMapTf [private]

Definition at line 99 of file dynamic_mapper.cpp.

Definition at line 134 of file dynamic_mapper.cpp.

boost::thread Mapper::publishThread [private]

Definition at line 132 of file dynamic_mapper.cpp.

Definition at line 69 of file dynamic_mapper.cpp.

Definition at line 67 of file dynamic_mapper.cpp.

Definition at line 56 of file dynamic_mapper.cpp.

const float Mapper::sensorMaxRange [private]

in meter. Radius in which the global map needs to be updated when a new scan arrived.

Definition at line 125 of file dynamic_mapper.cpp.

Definition at line 71 of file dynamic_mapper.cpp.

Definition at line 137 of file dynamic_mapper.cpp.

Definition at line 136 of file dynamic_mapper.cpp.

double Mapper::tfRefreshPeriod [private]

if set to zero, tf will be publish at the rate of the incoming point cloud messages

Definition at line 108 of file dynamic_mapper.cpp.

Definition at line 131 of file dynamic_mapper.cpp.

unique_ptr< PM::Transformation > Mapper::transformation [private]

Definition at line 85 of file dynamic_mapper.cpp.

Definition at line 100 of file dynamic_mapper.cpp.

name of the final vtk map

Definition at line 111 of file dynamic_mapper.cpp.


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


ethzasl_icp_mapper
Author(s): François Pomerleau and Stéphane Magnenat
autogenerated on Mon Oct 6 2014 10:28:23