Main MRPT website > C++ reference
MRPT logo
Classes | Public Member Functions | Public Attributes | Static Public Attributes | Protected Member Functions | Protected Attributes | Friends

mrpt::slam::CPointsMap Class Reference


Detailed Description

A cloud of points in 2D or 3D, which can be built from a sequence of laser scans or other sensors.

This is a virtual class, thus only a derived class can be instantiated by the user. The user most usually wants to use CSimplePointsMap.

See also:
CMetricMap, CPoint, mrpt::utils::CSerializable

Definition at line 63 of file CPointsMap.h.

#include <mrpt/slam/CPointsMap.h>

Inheritance diagram for mrpt::slam::CPointsMap:
Inheritance graph
[legend]

List of all members.

Classes

struct  TInsertionOptions
 With this struct options are provided to the observation insertion process. More...
struct  TLikelihoodOptions
 Options used when evaluating "computeObservationLikelihood" in the derived classes. More...

Public Member Functions

 CPointsMap ()
 Constructor.
virtual ~CPointsMap ()
 Virtual destructor.
virtual float squareDistanceToClosestCorrespondence (float x0, float y0) const
 Returns the square distance from the 2D point (x0,y0) to the closest correspondence in the map.
float squareDistanceToClosestCorrespondenceT (const TPoint2D &p0) const
virtual void copyFrom (const CPointsMap &obj)=0
 Virtual assignment operator, to be implemented in derived classes.
virtual void fuseWith (CPointsMap *otherMap, float minDistForFuse=0.02f, std::vector< bool > *notFusedPoints=NULL)=0
 Insert the contents of another map into this one, fusing the previous content with the new one.
virtual void loadFromRangeScan (const CObservation2DRangeScan &rangeScan, const CPose3D *robotPose=NULL)=0
 Transform the range scan into a set of cartessian coordinated points.
virtual bool load2D_from_text_file (std::string file)=0
 Load from a text file.
virtual bool load3D_from_text_file (std::string file)=0
 Load from a text file.
bool save2D_to_text_file (const std::string &file) const
 Save to a text file.
bool save3D_to_text_file (const std::string &file) const
 Save to a text file.
void saveMetricMapRepresentationToFile (const std::string &filNamePrefix) const
 This virtual method saves the map to a file "filNamePrefix"+< some_file_extension >, as an image or in any other applicable way (Notice that other methods to save the map may be implemented in classes implementing this virtual interface).
size_t size () const
 Returns the number of stored points in the map.
size_t getPointsCount () const
 Returns the number of stored points in the map (DEPRECATED, use "size()" instead better)
unsigned long getPoint (size_t index, CPoint2D &p) const
 Access to a given point from map, as a 2D point.
unsigned long getPoint (size_t index, CPoint3D &p) const
 Access to a given point from map, as a 3D point.
unsigned long getPoint (size_t index, mrpt::math::TPoint3D &p) const
 Access to a given point from map, as a 3D point.
unsigned long getPoint (size_t index, mrpt::math::TPoint2D &p) const
 Access to a given point from map, as a 2D point.
unsigned long getPoint (size_t index, float &x, float &y) const
 Access to a given point from map.
unsigned long getPoint (size_t index, float &x, float &y, float &z) const
 Access to a given point from map.
virtual void getPoint (size_t index, float &x, float &y, float &z, float &R, float &G, float &B) const
 Access to a given point from map, and its colors, if the map defines them (othersise, R=G=B=1.0).
virtual bool hasColorPoints () const
 Returns true if the point map has a color field for each point.
virtual void setPoint (size_t index, CPoint2D &p)=0
 Changes a given point from map, as a 2D point.
virtual void setPoint (size_t index, CPoint3D &p)=0
 Changes a given point from map, as a 3D point.
virtual void setPoint (size_t index, float x, float y)=0
 Changes a given point from map.
virtual void setPoint (size_t index, float x, float y, float z)=0
 Changes a given point from map.
void getPointsBuffer (size_t &outPointsCount, const float *&xs, const float *&ys, const float *&zs) const
 Provides a direct access to points buffer, or NULL if there is no points in the map.
const std::vector< float > & getPointsBufferRef_x () const
 Provides a direct access to a read-only reference of the internal point buffer.
const std::vector< float > & getPointsBufferRef_y () const
 Provides a direct access to a read-only reference of the internal point buffer.
const std::vector< float > & getPointsBufferRef_z () const
 Provides a direct access to a read-only reference of the internal point buffer.
template<class VECTOR >
void getAllPoints (VECTOR &xs, VECTOR &ys, VECTOR &zs, size_t decimation=1) const
 Returns a copy of the 2D/3D points as a std::vector of float coordinates.
void getAllPoints (std::vector< TPoint3D > &ps, size_t decimation=1) const
void getAllPoints (std::vector< float > &xs, std::vector< float > &ys, size_t decimation=1) const
 Returns a copy of the 2D/3D points as a std::vector of float coordinates.
void getAllPoints (std::vector< TPoint2D > &ps, size_t decimation=1) const
virtual void insertPoint (float x, float y, float z=0)=0
 Provides a way to insert individual points into the map.
void insertPoint (const CPoint3D &p)
 Provides a way to insert individual points into the map.
void insertPoint (const mrpt::math::TPoint3D &p)
 Provides a way to insert individual points into the map.
virtual void reserve (size_t newLength)=0
 Reserves memory for a given number of points: the size of the map does not change, it only reserves the memory.
virtual void setAllPoints (const std::vector< float > &X, const std::vector< float > &Y, const std::vector< float > &Z)=0
 Set all the points at once from vectors with X,Y and Z coordinates.
virtual void setAllPoints (const std::vector< float > &X, const std::vector< float > &Y)=0
 Set all the points at once from vectors with X and Y coordinates (Z=0).
void clipOutOfRangeInZ (float zMin, float zMax)
 Delete points out of the given "z" axis range have been removed.
void clipOutOfRange (const CPoint2D &point, float maxRange)
 Delete points which are more far than "maxRange" away from the given "point".
virtual void applyDeletionMask (std::vector< bool > &mask)=0
 Remove from the map the points marked in a bool's array as "true".
void computeMatchingWith2D (const CMetricMap *otherMap, const CPose2D &otherMapPose, float maxDistForCorrespondence, float maxAngularDistForCorrespondence, const CPose2D &angularDistPivotPoint, TMatchingPairList &correspondences, float &correspondencesRatio, float *sumSqrDist=NULL, bool onlyKeepTheClosest=false, bool onlyUniqueRobust=false) const
 Computes the matchings between this and another 2D/3D points map.
void computeMatchingWith3D (const CMetricMap *otherMap, const CPose3D &otherMapPose, float maxDistForCorrespondence, float maxAngularDistForCorrespondence, const CPoint3D &angularDistPivotPoint, TMatchingPairList &correspondences, float &correspondencesRatio, float *sumSqrDist=NULL, bool onlyKeepTheClosest=true, bool onlyUniqueRobust=false) const
 Computes the matchings between this and another 3D points map - method used in 3D-ICP.
void changeCoordinatesReference (const CPose2D &b)
 Replace each point $ p_i $ by $ p'_i = b \oplus p_i $ (pose compounding operator).
void changeCoordinatesReference (const CPose3D &b)
 Replace each point $ p_i $ by $ p'_i = b \oplus p_i $ (pose compounding operator).
void changeCoordinatesReference (const CPointsMap &other, const CPose3D &b)
 Copy all the points from "other" map to "this", replacing each point $ p_i $ by $ p'_i = b \oplus p_i $ (pose compounding operator).
virtual bool isEmpty () const
 Returns true if the map is empty/no observation has been inserted.
bool empty () const
 STL-like method to check whether the map is empty:
virtual void getAs3DObject (mrpt::opengl::CSetOfObjectsPtr &outObj) const
 Returns a 3D object representing the map.
float compute3DMatchingRatio (const CMetricMap *otherMap, const CPose3D &otherMapPose, float minDistForCorr=0.10f, float minMahaDistForCorr=2.0f) const
 Computes the ratio in [0,1] of correspondences between "this" and the "otherMap" map, whose 6D pose relative to "this" is "otherMapPose" In the case of a multi-metric map, this returns the average between the maps.
float getLargestDistanceFromOrigin () const
 This method returns the largest distance from the origin to any of the points, such as a sphere centered at the origin with this radius cover ALL the points in the map (the results are buffered, such as, if the map is not modified, the second call will be much faster than the first one).
void boundingBox (float &min_x, float &max_x, float &min_y, float &max_y, float &min_z, float &max_z) const
 Computes the bounding box of all the points, or (0,0 ,0,0, 0,0) if there are no points.
void boundingBox (TPoint3D &pMin, TPoint3D &pMax) const
void extractCylinder (const CPoint2D &center, const double radius, const double zmin, const double zmax, CPointsMap *outMap)
virtual double computeObservationLikelihood (const CObservation *obs, const CPose3D &takenFrom)
 Computes the likelihood of taking a given observation from a given pose in the world being modeled with this map.

Public Attributes

TInsertionOptions insertionOptions
 The options used when inserting observations in the map.
TLikelihoodOptions likelihoodOptions

Static Public Attributes

static float COLOR_3DSCENE_R
 The color [0,1] of points when extracted from getAs3DObject (default=blue)
static float COLOR_3DSCENE_G
static float COLOR_3DSCENE_B

Protected Member Functions

void mark_as_modified () const
 Called only by this class or children classes, set m_largestDistanceFromOriginIsUpdated=false and such.
Virtual methods that MUST be implemented by children classes of KDTreeCapable
virtual size_t kdtree_get_point_count () const
 Must return the number of data points.
virtual void kdtree_fill_point_data (ANNpointArray &data, const int nDims) const
 Must fill out the data points in "data", such as the i'th point will be stored in (data[i][0],...,data[i][nDims-1]).

Protected Attributes

std::vector< float > x
 The points coordinates.
std::vector< float > y
std::vector< float > z
std::vector< uint32_t > pointWeight
 The points weights.
float m_largestDistanceFromOrigin
 Auxiliary variables used in "getLargestDistanceFromOrigin".
bool m_largestDistanceFromOriginIsUpdated
 Auxiliary variables used in "getLargestDistanceFromOrigin".

Friends

class CMultiMetricMap
class CMultiMetricMapPDF
class CSimplePointsMap
class CColouredPointsMap
class COccupancyGridMap2D

RTTI stuff

class mrpt::utils::CStream
static const
mrpt::utils::TRuntimeClassId 
classCPointsMap
static const
mrpt::utils::TRuntimeClassId
_GetBaseClass ()
virtual const
mrpt::utils::TRuntimeClassId
GetRuntimeClass () const
 Returns information about the class of an object in runtime.

Constructor & Destructor Documentation

mrpt::slam::CPointsMap::CPointsMap ( )

Constructor.

virtual mrpt::slam::CPointsMap::~CPointsMap ( ) [virtual]

Virtual destructor.


Member Function Documentation

static const mrpt::utils::TRuntimeClassId* mrpt::slam::CPointsMap::_GetBaseClass ( ) [static, protected]
virtual void mrpt::slam::CPointsMap::applyDeletionMask ( std::vector< bool > &  mask) [pure virtual]

Remove from the map the points marked in a bool's array as "true".

Exceptions:
std::exceptionIf mask size is not equal to points count.

Implemented in mrpt::slam::CColouredPointsMap, and mrpt::slam::CSimplePointsMap.

void mrpt::slam::CPointsMap::boundingBox ( float &  min_x,
float &  max_x,
float &  min_y,
float &  max_y,
float &  min_z,
float &  max_z 
) const

Computes the bounding box of all the points, or (0,0 ,0,0, 0,0) if there are no points.

void mrpt::slam::CPointsMap::boundingBox ( TPoint3D pMin,
TPoint3D pMax 
) const [inline]
void mrpt::slam::CPointsMap::changeCoordinatesReference ( const CPose3D b)

Replace each point $ p_i $ by $ p'_i = b \oplus p_i $ (pose compounding operator).

void mrpt::slam::CPointsMap::changeCoordinatesReference ( const CPointsMap other,
const CPose3D b 
)

Copy all the points from "other" map to "this", replacing each point $ p_i $ by $ p'_i = b \oplus p_i $ (pose compounding operator).

void mrpt::slam::CPointsMap::changeCoordinatesReference ( const CPose2D b)

Replace each point $ p_i $ by $ p'_i = b \oplus p_i $ (pose compounding operator).

void mrpt::slam::CPointsMap::clipOutOfRange ( const CPoint2D point,
float  maxRange 
)

Delete points which are more far than "maxRange" away from the given "point".

void mrpt::slam::CPointsMap::clipOutOfRangeInZ ( float  zMin,
float  zMax 
)

Delete points out of the given "z" axis range have been removed.

float mrpt::slam::CPointsMap::compute3DMatchingRatio ( const CMetricMap otherMap,
const CPose3D otherMapPose,
float  minDistForCorr = 0.10f,
float  minMahaDistForCorr = 2.0f 
) const [virtual]

Computes the ratio in [0,1] of correspondences between "this" and the "otherMap" map, whose 6D pose relative to "this" is "otherMapPose" In the case of a multi-metric map, this returns the average between the maps.

This method always return 0 for grid maps.

Parameters:
otherMap[IN] The other map to compute the matching with.
otherMapPose[IN] The 6D pose of the other map as seen from "this".
minDistForCorr[IN] The minimum distance between 2 non-probabilistic map elements for counting them as a correspondence.
minMahaDistForCorr[IN] The minimum Mahalanobis distance between 2 probabilistic map elements for counting them as a correspondence.
Returns:
The matching ratio [0,1]
See also:
computeMatchingWith2D

Implements mrpt::slam::CMetricMap.

void mrpt::slam::CPointsMap::computeMatchingWith2D ( const CMetricMap otherMap,
const CPose2D otherMapPose,
float  maxDistForCorrespondence,
float  maxAngularDistForCorrespondence,
const CPose2D angularDistPivotPoint,
TMatchingPairList correspondences,
float &  correspondencesRatio,
float *  sumSqrDist = NULL,
bool  onlyKeepTheClosest = false,
bool  onlyUniqueRobust = false 
) const [virtual]

Computes the matchings between this and another 2D/3D points map.

This includes finding:

  • The set of points pairs in each map
  • The mean squared distance between corresponding pairs. This method is the most time critical one into the ICP algorithm.
Parameters:
otherMap[IN] The other map to compute the matching with.
otherMapPose[IN] The pose of the other map as seen from "this".
maxDistForCorrespondence[IN] Maximum 2D distance between two points to be matched.
maxAngularDistForCorrespondence[IN] Maximum angular distance in radians to allow far points to be matched.
angularDistPivotPoint[IN] The point from which to measure the "angular distances"
correspondences[OUT] The detected matchings pairs.
correspondencesRatio[OUT] The number of correct correspondences.
sumSqrDist[OUT] The sum of all matched points squared distances.If undesired, set to NULL, as default.
covariance[OUT] The resulting matching covariance 3x3 matrix, or NULL if undesired.
onlyKeepTheClosest[OUT] Returns only the closest correspondence (default=false)
See also:
computeMatching3DWith

Reimplemented from mrpt::slam::CMetricMap.

void mrpt::slam::CPointsMap::computeMatchingWith3D ( const CMetricMap otherMap,
const CPose3D otherMapPose,
float  maxDistForCorrespondence,
float  maxAngularDistForCorrespondence,
const CPoint3D angularDistPivotPoint,
TMatchingPairList correspondences,
float &  correspondencesRatio,
float *  sumSqrDist = NULL,
bool  onlyKeepTheClosest = true,
bool  onlyUniqueRobust = false 
) const [virtual]

Computes the matchings between this and another 3D points map - method used in 3D-ICP.

This method finds the set of point pairs in each map.

The method is the most time critical one into the ICP algorithm.

Parameters:
otherMap[IN] The other map to compute the matching with.
otherMapPose[IN] The pose of the other map as seen from "this".
maxDistForCorrespondence[IN] Maximum 2D linear distance between two points to be matched.
maxAngularDistForCorrespondence[IN] In radians: The aim is to allow larger distances to more distant correspondences.
angularDistPivotPoint[IN] The point used to calculate distances from in both maps.
correspondences[OUT] The detected matchings pairs.
correspondencesRatio[OUT] The ratio [0,1] of points in otherMap with at least one correspondence.
sumSqrDist[OUT] The sum of all matched points squared distances.If undesired, set to NULL, as default.
onlyKeepTheClosest[IN] If set to true, only the closest correspondence will be returned. If false (default) all are returned.
See also:
compute3DMatchingRatio

Reimplemented from mrpt::slam::CMetricMap.

virtual double mrpt::slam::CPointsMap::computeObservationLikelihood ( const CObservation obs,
const CPose3D takenFrom 
) [virtual]

Computes the likelihood of taking a given observation from a given pose in the world being modeled with this map.

Parameters:
takenFromThe robot's pose the observation is supposed to be taken from.
obsThe observation.
Returns:
This method returns a likelihood in the range [0,1].
See also:
Used in particle filter algorithms, see: CMultiMetricMapPDF
Note:
In CPointsMap this method is virtual so it can be redefined in derived classes, if desired.

Implements mrpt::slam::CMetricMap.

virtual void mrpt::slam::CPointsMap::copyFrom ( const CPointsMap obj) [pure virtual]

Virtual assignment operator, to be implemented in derived classes.

Implemented in mrpt::slam::CColouredPointsMap, and mrpt::slam::CSimplePointsMap.

bool mrpt::slam::CPointsMap::empty ( ) const [inline]

STL-like method to check whether the map is empty:

Definition at line 500 of file CPointsMap.h.

void mrpt::slam::CPointsMap::extractCylinder ( const CPoint2D center,
const double  radius,
const double  zmin,
const double  zmax,
CPointsMap outMap 
)
virtual void mrpt::slam::CPointsMap::fuseWith ( CPointsMap otherMap,
float  minDistForFuse = 0.02f,
std::vector< bool > *  notFusedPoints = NULL 
) [pure virtual]

Insert the contents of another map into this one, fusing the previous content with the new one.

This means that points very close to existing ones will be "fused", rather than "added". This prevents the unbounded increase in size of these class of maps. NOTICE that "otherMap" is neither translated nor rotated here, so if this is desired it must done before calling this method.

Parameters:
otherMapThe other map whose points are to be inserted into this one.
minDistForFuseMinimum distance (in meters) between two points, each one in a map, to be considered the same one and be fused rather than added.
notFusedPointsIf a pointer is supplied, this list will contain at output a list with a "bool" value per point in "this" map. This will be false/true according to that point having been fused or not.

Implemented in mrpt::slam::CColouredPointsMap, and mrpt::slam::CSimplePointsMap.

template<class VECTOR >
void mrpt::slam::CPointsMap::getAllPoints ( VECTOR &  xs,
VECTOR &  ys,
VECTOR &  zs,
size_t  decimation = 1 
) const [inline]

Returns a copy of the 2D/3D points as a std::vector of float coordinates.

If decimation is greater than 1, only 1 point out of that number will be saved in the output, effectively performing a subsampling of the points.

See also:
getPointsBufferRef_x, getPointsBufferRef_y, getPointsBufferRef_z
Template Parameters:
VECTORcan be std::vector<float or double> or any row/column Eigen::Array or Eigen::Matrix (this includes mrpt::vector_float and mrpt::vector_double).

Definition at line 339 of file CPointsMap.h.

References ASSERT_, MRPT_END, MRPT_START, and internal::y.

void mrpt::slam::CPointsMap::getAllPoints ( std::vector< TPoint3D > &  ps,
size_t  decimation = 1 
) const [inline]

Definition at line 357 of file CPointsMap.h.

void mrpt::slam::CPointsMap::getAllPoints ( std::vector< float > &  xs,
std::vector< float > &  ys,
size_t  decimation = 1 
) const

Returns a copy of the 2D/3D points as a std::vector of float coordinates.

If decimation is greater than 1, only 1 point out of that number will be saved in the output, effectively performing a subsampling of the points.

See also:
setAllPoints
void mrpt::slam::CPointsMap::getAllPoints ( std::vector< TPoint2D > &  ps,
size_t  decimation = 1 
) const [inline]

Definition at line 374 of file CPointsMap.h.

virtual void mrpt::slam::CPointsMap::getAs3DObject ( mrpt::opengl::CSetOfObjectsPtr outObj) const [virtual]

Returns a 3D object representing the map.

The color of the points is given by the static variables: COLOR_3DSCENE_R,COLOR_3DSCENE_G,COLOR_3DSCENE_B

See also:
mrpt::global_settings::POINTSMAPS_3DOBJECT_POINTSIZE

Implements mrpt::slam::CMetricMap.

Reimplemented in mrpt::slam::CColouredPointsMap.

float mrpt::slam::CPointsMap::getLargestDistanceFromOrigin ( ) const

This method returns the largest distance from the origin to any of the points, such as a sphere centered at the origin with this radius cover ALL the points in the map (the results are buffered, such as, if the map is not modified, the second call will be much faster than the first one).

unsigned long mrpt::slam::CPointsMap::getPoint ( size_t  index,
CPoint2D p 
) const

Access to a given point from map, as a 2D point.

First index is 0.

Returns:
The return value is the weight of the point (the times it has been fused)
Exceptions:
Throwsstd::exception on index out of bound.
unsigned long mrpt::slam::CPointsMap::getPoint ( size_t  index,
CPoint3D p 
) const

Access to a given point from map, as a 3D point.

First index is 0.

Returns:
The return value is the weight of the point (the times it has been fused)
Exceptions:
Throwsstd::exception on index out of bound.
unsigned long mrpt::slam::CPointsMap::getPoint ( size_t  index,
mrpt::math::TPoint2D p 
) const

Access to a given point from map, as a 2D point.

First index is 0.

Returns:
The return value is the weight of the point (the times it has been fused)
Exceptions:
Throwsstd::exception on index out of bound.
unsigned long mrpt::slam::CPointsMap::getPoint ( size_t  index,
mrpt::math::TPoint3D p 
) const

Access to a given point from map, as a 3D point.

First index is 0.

Returns:
The return value is the weight of the point (the times it has been fused)
Exceptions:
Throwsstd::exception on index out of bound.
unsigned long mrpt::slam::CPointsMap::getPoint ( size_t  index,
float &  x,
float &  y 
) const

Access to a given point from map.

First index is 0.

Returns:
The return value is the weight of the point (the times it has been fused)
Exceptions:
Throwsstd::exception on index out of bound.
unsigned long mrpt::slam::CPointsMap::getPoint ( size_t  index,
float &  x,
float &  y,
float &  z 
) const

Access to a given point from map.

First index is 0.

Returns:
The return value is the weight of the point (the times it has been fused)
Exceptions:
Throwsstd::exception on index out of bound.
virtual void mrpt::slam::CPointsMap::getPoint ( size_t  index,
float &  x,
float &  y,
float &  z,
float &  R,
float &  G,
float &  B 
) const [inline, virtual]

Access to a given point from map, and its colors, if the map defines them (othersise, R=G=B=1.0).

First index is 0.

Returns:
The return value is the weight of the point (the times it has been fused)
Exceptions:
Throwsstd::exception on index out of bound.

Reimplemented in mrpt::slam::CColouredPointsMap.

Definition at line 293 of file CPointsMap.h.

void mrpt::slam::CPointsMap::getPointsBuffer ( size_t &  outPointsCount,
const float *&  xs,
const float *&  ys,
const float *&  zs 
) const

Provides a direct access to points buffer, or NULL if there is no points in the map.

const std::vector<float>& mrpt::slam::CPointsMap::getPointsBufferRef_x ( ) const [inline]

Provides a direct access to a read-only reference of the internal point buffer.

See also:
getAllPoints

Definition at line 327 of file CPointsMap.h.

const std::vector<float>& mrpt::slam::CPointsMap::getPointsBufferRef_y ( ) const [inline]

Provides a direct access to a read-only reference of the internal point buffer.

See also:
getAllPoints

Definition at line 329 of file CPointsMap.h.

References internal::y.

const std::vector<float>& mrpt::slam::CPointsMap::getPointsBufferRef_z ( ) const [inline]

Provides a direct access to a read-only reference of the internal point buffer.

See also:
getAllPoints

Definition at line 331 of file CPointsMap.h.

size_t mrpt::slam::CPointsMap::getPointsCount ( ) const

Returns the number of stored points in the map (DEPRECATED, use "size()" instead better)

virtual const mrpt::utils::TRuntimeClassId* mrpt::slam::CPointsMap::GetRuntimeClass ( ) const [virtual]

Returns information about the class of an object in runtime.

Reimplemented from mrpt::slam::CMetricMap.

Reimplemented in mrpt::slam::CColouredPointsMap, and mrpt::slam::CSimplePointsMap.

virtual bool mrpt::slam::CPointsMap::hasColorPoints ( ) const [inline, virtual]

Returns true if the point map has a color field for each point.

Reimplemented in mrpt::slam::CColouredPointsMap.

Definition at line 300 of file CPointsMap.h.

virtual void mrpt::slam::CPointsMap::insertPoint ( float  x,
float  y,
float  z = 0 
) [pure virtual]

Provides a way to insert individual points into the map.

Implemented in mrpt::slam::CColouredPointsMap, and mrpt::slam::CSimplePointsMap.

void mrpt::slam::CPointsMap::insertPoint ( const mrpt::math::TPoint3D p) [inline]

Provides a way to insert individual points into the map.

Reimplemented in mrpt::slam::CSimplePointsMap.

Definition at line 393 of file CPointsMap.h.

References mrpt::math::TPoint3D::x, mrpt::math::TPoint3D::y, and mrpt::math::TPoint3D::z.

void mrpt::slam::CPointsMap::insertPoint ( const CPoint3D p) [inline]

Provides a way to insert individual points into the map.

Definition at line 388 of file CPointsMap.h.

References mrpt::poses::CPoseOrPoint< DERIVEDCLASS >::x(), and mrpt::poses::CPoseOrPoint< DERIVEDCLASS >::y().

virtual bool mrpt::slam::CPointsMap::isEmpty ( ) const [virtual]

Returns true if the map is empty/no observation has been inserted.

Implements mrpt::slam::CMetricMap.

virtual void mrpt::slam::CPointsMap::kdtree_fill_point_data ( ANNpointArray data,
const int  nDims 
) const [protected, virtual]

Must fill out the data points in "data", such as the i'th point will be stored in (data[i][0],...,data[i][nDims-1]).

Implements mrpt::math::KDTreeCapable.

virtual size_t mrpt::slam::CPointsMap::kdtree_get_point_count ( ) const [protected, virtual]

Must return the number of data points.

Implements mrpt::math::KDTreeCapable.

virtual bool mrpt::slam::CPointsMap::load2D_from_text_file ( std::string  file) [pure virtual]

Load from a text file.

In each line there are a point coordinates. Returns false if any error occured, true elsewere.

Implemented in mrpt::slam::CColouredPointsMap, and mrpt::slam::CSimplePointsMap.

virtual bool mrpt::slam::CPointsMap::load3D_from_text_file ( std::string  file) [pure virtual]

Load from a text file.

In each line there are a point coordinates. Returns false if any error occured, true elsewere.

Implemented in mrpt::slam::CColouredPointsMap, and mrpt::slam::CSimplePointsMap.

virtual void mrpt::slam::CPointsMap::loadFromRangeScan ( const CObservation2DRangeScan rangeScan,
const CPose3D robotPose = NULL 
) [pure virtual]

Transform the range scan into a set of cartessian coordinated points.

The options in "insertionOptions" are considered in this method.

Parameters:
rangeScanThe scan to be inserted into this map
robotPoseThe robot 3D pose, default to (0,0,0|0deg,0deg,0deg). It is used to compute the sensor pose relative to the robot actual pose. Recall sensor pose is embeded in the observation class.

NOTE: Only ranges marked as "valid=true" in the observation will be inserted

See also:
CObservation2DRangeScan

Implemented in mrpt::slam::CColouredPointsMap, and mrpt::slam::CSimplePointsMap.

void mrpt::slam::CPointsMap::mark_as_modified ( ) const [protected]

Called only by this class or children classes, set m_largestDistanceFromOriginIsUpdated=false and such.

virtual void mrpt::slam::CPointsMap::reserve ( size_t  newLength) [pure virtual]

Reserves memory for a given number of points: the size of the map does not change, it only reserves the memory.

This is useful for situations where it is approximately known the final size of the map. This method is more efficient than constantly increasing the size of the buffers. Refer to the STL C++ library's "reserve" methods.

Implemented in mrpt::slam::CColouredPointsMap, and mrpt::slam::CSimplePointsMap.

bool mrpt::slam::CPointsMap::save2D_to_text_file ( const std::string &  file) const

Save to a text file.

In each line there are a point coordinates. Returns false if any error occured, true elsewere.

bool mrpt::slam::CPointsMap::save3D_to_text_file ( const std::string &  file) const

Save to a text file.

In each line there are a point coordinates. Returns false if any error occured, true elsewere.

void mrpt::slam::CPointsMap::saveMetricMapRepresentationToFile ( const std::string &  filNamePrefix) const [inline, virtual]

This virtual method saves the map to a file "filNamePrefix"+< some_file_extension >, as an image or in any other applicable way (Notice that other methods to save the map may be implemented in classes implementing this virtual interface).

Implements mrpt::slam::CMetricMap.

Definition at line 235 of file CPointsMap.h.

virtual void mrpt::slam::CPointsMap::setAllPoints ( const std::vector< float > &  X,
const std::vector< float > &  Y,
const std::vector< float > &  Z 
) [pure virtual]

Set all the points at once from vectors with X,Y and Z coordinates.

See also:
getAllPoints

Implemented in mrpt::slam::CColouredPointsMap, and mrpt::slam::CSimplePointsMap.

virtual void mrpt::slam::CPointsMap::setAllPoints ( const std::vector< float > &  X,
const std::vector< float > &  Y 
) [pure virtual]

Set all the points at once from vectors with X and Y coordinates (Z=0).

See also:
getAllPoints

Implemented in mrpt::slam::CColouredPointsMap, and mrpt::slam::CSimplePointsMap.

virtual void mrpt::slam::CPointsMap::setPoint ( size_t  index,
CPoint2D p 
) [pure virtual]

Changes a given point from map, as a 2D point.

First index is 0.

Exceptions:
Throwsstd::exception on index out of bound.

Implemented in mrpt::slam::CColouredPointsMap, and mrpt::slam::CSimplePointsMap.

virtual void mrpt::slam::CPointsMap::setPoint ( size_t  index,
CPoint3D p 
) [pure virtual]

Changes a given point from map, as a 3D point.

First index is 0.

Exceptions:
Throwsstd::exception on index out of bound.

Implemented in mrpt::slam::CColouredPointsMap, and mrpt::slam::CSimplePointsMap.

virtual void mrpt::slam::CPointsMap::setPoint ( size_t  index,
float  x,
float  y,
float  z 
) [pure virtual]

Changes a given point from map.

First index is 0.

Exceptions:
Throwsstd::exception on index out of bound.

Implemented in mrpt::slam::CColouredPointsMap, and mrpt::slam::CSimplePointsMap.

virtual void mrpt::slam::CPointsMap::setPoint ( size_t  index,
float  x,
float  y 
) [pure virtual]

Changes a given point from map.

First index is 0.

Exceptions:
Throwsstd::exception on index out of bound.

Implemented in mrpt::slam::CColouredPointsMap, and mrpt::slam::CSimplePointsMap.

size_t mrpt::slam::CPointsMap::size ( ) const

Returns the number of stored points in the map.

virtual float mrpt::slam::CPointsMap::squareDistanceToClosestCorrespondence ( float  x0,
float  y0 
) const [virtual]

Returns the square distance from the 2D point (x0,y0) to the closest correspondence in the map.

float mrpt::slam::CPointsMap::squareDistanceToClosestCorrespondenceT ( const TPoint2D p0) const [inline]

Definition at line 124 of file CPointsMap.h.


Friends And Related Function Documentation

friend class CColouredPointsMap [friend]

Definition at line 68 of file CPointsMap.h.

friend class CMultiMetricMap [friend]

Definition at line 65 of file CPointsMap.h.

friend class CMultiMetricMapPDF [friend]

Definition at line 66 of file CPointsMap.h.

friend class COccupancyGridMap2D [friend]

Definition at line 69 of file CPointsMap.h.

friend class CSimplePointsMap [friend]

Definition at line 67 of file CPointsMap.h.

friend class mrpt::utils::CStream [friend]

Reimplemented from mrpt::slam::CMetricMap.

Definition at line 72 of file CPointsMap.h.


Member Data Documentation

Definition at line 72 of file CPointsMap.h.

Definition at line 550 of file CPointsMap.h.

Definition at line 549 of file CPointsMap.h.

The color [0,1] of points when extracted from getAs3DObject (default=blue)

Definition at line 548 of file CPointsMap.h.

The options used when inserting observations in the map.

Definition at line 152 of file CPointsMap.h.

Definition at line 180 of file CPointsMap.h.

Auxiliary variables used in "getLargestDistanceFromOrigin".

See also:
getLargestDistanceFromOrigin

Definition at line 86 of file CPointsMap.h.

Auxiliary variables used in "getLargestDistanceFromOrigin".

See also:
getLargestDistanceFromOrigin

Definition at line 91 of file CPointsMap.h.

std::vector<uint32_t> mrpt::slam::CPointsMap::pointWeight [protected]

The points weights.

Definition at line 81 of file CPointsMap.h.

std::vector<float> mrpt::slam::CPointsMap::x [protected]

The points coordinates.

Definition at line 77 of file CPointsMap.h.

std::vector<float> mrpt::slam::CPointsMap::y [protected]

Definition at line 77 of file CPointsMap.h.

std::vector<float> mrpt::slam::CPointsMap::z [protected]

Definition at line 77 of file CPointsMap.h.




Page generated by Doxygen 1.7.3 for MRPT 0.9.4 SVN: at Sat Mar 26 06:16:28 UTC 2011