#include <mrpt/slam/CPointsMap.h>
Classes | |
struct | TInsertionOptions |
With this struct options are provided to the observation insertion process. More... | |
struct | TKDTreeData |
Internal structure with a KD-tree representation. More... | |
Public Member Functions | |
CPointsMap () | |
Constructor. | |
virtual | ~CPointsMap () |
Virtual destructor. | |
size_t | kdTreeClosestPoint2D (const float &x0, const float &y0, float &out_x, float &out_y, float &out_dist_sqr) const |
KD Tree-based search for the closest point (only ONE) to some given 2D coordinates. | |
float | kdTreeClosestPoint2DsqrError (const float &x0, const float &y0) const |
Like kdTreeClosestPoint2D, but just return the square error from some point to its closest neighbor. | |
virtual float | squareDistanceToClosestCorrespondence (const float &x0, const float &y0) const |
Returns the square distance from the 2D point (x0,y0) to the closest correspondence in the map. | |
void | kdTreeTwoClosestPoint2D (const float &x0, const float &y0, float &out_x1, float &out_y1, float &out_x2, float &out_y2, float &out_dist_sqr1, float &out_dist_sqr2) const |
KD Tree-based search for the TWO closest point to some given 2D coordinates. | |
void | kdTreeNClosestPoint2D (const float &x0, const float &y0, const unsigned int &N, std::vector< float > &out_x, std::vector< float > &out_y, std::vector< float > &out_dist_sqr) const |
KD Tree-based search for the N closest point to some given 2D coordinates. | |
void | kdTreeNClosestPoint2DIdx (const float &x0, const float &y0, const unsigned int &N, std::vector< int > &out_idx, std::vector< float > &out_dist_sqr) const |
KD Tree-based search for the N closest point to some given 2D coordinates and returns their indexes. | |
size_t | kdTreeClosestPoint3D (const float &x0, const float &y0, const float &z0, float &out_x, float &out_y, float &out_z, float &out_dist_sqr) const |
KD Tree-based search for the closest point (only ONE) to some given 3D coordinates. | |
void | kdTreeNClosestPoint3D (const float &x0, const float &y0, const float &z0, const unsigned int &N, std::vector< float > &out_x, std::vector< float > &out_y, std::vector< float > &out_z, std::vector< float > &out_dist_sqr) const |
KD Tree-based search for the N closest points to some given 3D coordinates. | |
void | kdTreeNClosestPoint3DIdx (const float &x0, const float &y0, const float &z0, const unsigned int &N, std::vector< int > &out_idx, std::vector< float > &out_dist_sqr) const |
KD Tree-based search for the N closest point to some given 3D coordinates and returns their indexes. | |
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). | |
virtual void | clear ()=0 |
Clear the map, erasing all the points. | |
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 (const size_t &index, CPoint2D &p) const |
Access to a given point from map, as a 2D point. | |
unsigned long | getPoint (const size_t &index, CPoint3D &p) const |
Access to a given point from map, as a 3D point. | |
unsigned long | getPoint (const size_t &index, float &x, float &y) const |
Access to a given point from map. | |
unsigned long | getPoint (const size_t &index, float &x, float &y, float &z) const |
Access to a given point from map. | |
virtual void | setPoint (const size_t &index, CPoint2D &p)=0 |
Changes a given point from map, as a 2D point. | |
virtual void | setPoint (const size_t &index, CPoint3D &p)=0 |
Changes a given point from map, as a 3D point. | |
virtual void | setPoint (const size_t &index, float x, float y)=0 |
Changes a given point from map. | |
virtual void | setPoint (const 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. | |
void | getAllPoints (std::vector< float > &xs, std::vector< float > &ys, std::vector< float > &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< 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. | |
virtual void | insertPoint (float x, float y, float z=0)=0 |
Provides a way to insert individual points into the map:. | |
virtual void | insertPoint (CPoint3D p)=0 |
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. | |
void | clipOutOfRangeInZ (float zMin, float zMax) |
Delete points out of the given "z" axis range have been removed. | |
void | clipOutOfRange (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 ![]() ![]() | |
void | changeCoordinatesReference (const CPose3D &b) |
Replace each point ![]() ![]() | |
void | changeCoordinatesReference (const CPointsMap &other, const CPose3D &b) |
Copy all the points from "other" map to "this", replacing each point ![]() ![]() | |
virtual bool | isEmpty () const |
Returns true if the map is empty/no observation has been inserted. | |
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. | |
Public Attributes | |
mrpt::slam::CPointsMap::TInsertionOptions | insertionOptions |
With this struct options are provided to the observation insertion process. | |
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 | build_kdTree2D () const |
Private method to construct the KD-tree (if required). | |
void | build_kdTree3D () const |
Private method to construct the KD-tree (if required). | |
Protected Attributes | |
utils::safe_ptr< CMultiMetricMap > | m_parent |
Only for objects stored into a CMultiMetricMap (among a grid map). | |
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". | |
bool | m_KDTreeDataIsUpdated |
Auxiliary variables used in "buildKDTree2D" / "buildKDTree3D". | |
TKDTreeData | KDTreeData |
Friends | |
class | CMultiMetricMap |
class | CMultiMetricMapPDF |
class | CSimplePointsMap |
class | CColouredPointsMap |
class | COccupancyGridMap2D |
This is a virtual class, thus only a derived class can be instantiated by the user. The user most usually wants to use CSimplePointsMap.
Definition at line 58 of file CPointsMap.h.
mrpt::slam::CPointsMap::CPointsMap | ( | ) |
Constructor.
virtual mrpt::slam::CPointsMap::~CPointsMap | ( | ) | [virtual] |
Virtual destructor.
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".
std::exception | If 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::build_kdTree2D | ( | ) | const [protected] |
Private method to construct the KD-tree (if required).
void mrpt::slam::CPointsMap::build_kdTree3D | ( | ) | const [protected] |
Private method to construct the KD-tree (if required).
void mrpt::slam::CPointsMap::changeCoordinatesReference | ( | const CPointsMap & | other, | |
const CPose3D & | b | |||
) |
Copy all the points from "other" map to "this", replacing each point by
(pose compounding operator).
void mrpt::slam::CPointsMap::changeCoordinatesReference | ( | const CPose3D & | b | ) |
Replace each point by
(pose compounding operator).
void mrpt::slam::CPointsMap::changeCoordinatesReference | ( | const CPose2D & | b | ) |
Replace each point by
(pose compounding operator).
virtual void mrpt::slam::CPointsMap::clear | ( | ) | [pure virtual] |
Clear the map, erasing all the points.
Implements mrpt::slam::CMetricMap.
Implemented in mrpt::slam::CColouredPointsMap, and mrpt::slam::CSimplePointsMap.
void mrpt::slam::CPointsMap::clipOutOfRange | ( | 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.
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. |
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:
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) |
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.
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. |
Reimplemented from 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.
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.
otherMap | The other map whose points are to be inserted into this one. | |
minDistForFuse | Minimum distance (in meters) between two points, each one in a map, to be considered the same one and be fused rather than added. | |
notFusedPoints | If 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.
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.
void mrpt::slam::CPointsMap::getAllPoints | ( | std::vector< float > & | xs, | |
std::vector< float > & | ys, | |||
std::vector< float > & | zs, | |||
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.
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
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 | ( | const size_t & | index, | |
float & | x, | |||
float & | y, | |||
float & | z | |||
) | const |
Access to a given point from map.
First index is 0.
Throws | std::exception on index out of bound. |
unsigned long mrpt::slam::CPointsMap::getPoint | ( | const size_t & | index, | |
float & | x, | |||
float & | y | |||
) | const |
Access to a given point from map.
First index is 0.
Throws | std::exception on index out of bound. |
unsigned long mrpt::slam::CPointsMap::getPoint | ( | const size_t & | index, | |
CPoint3D & | p | |||
) | const |
Access to a given point from map, as a 3D point.
First index is 0.
Throws | std::exception on index out of bound. |
unsigned long mrpt::slam::CPointsMap::getPoint | ( | const size_t & | index, | |
CPoint2D & | p | |||
) | const |
Access to a given point from map, as a 2D point.
First index is 0.
Throws | std::exception on index out of bound. |
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.
size_t mrpt::slam::CPointsMap::getPointsCount | ( | ) | const |
Returns the number of stored points in the map (DEPRECATED, use "size()" instead better).
virtual void mrpt::slam::CPointsMap::insertPoint | ( | CPoint3D | p | ) | [pure virtual] |
Provides a way to insert individual points into the map:.
Implemented in mrpt::slam::CColouredPointsMap, and mrpt::slam::CSimplePointsMap.
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.
virtual bool mrpt::slam::CPointsMap::isEmpty | ( | ) | const [virtual] |
Returns true if the map is empty/no observation has been inserted.
Implements mrpt::slam::CMetricMap.
size_t mrpt::slam::CPointsMap::kdTreeClosestPoint2D | ( | const float & | x0, | |
const float & | y0, | |||
float & | out_x, | |||
float & | out_y, | |||
float & | out_dist_sqr | |||
) | const |
KD Tree-based search for the closest point (only ONE) to some given 2D coordinates.
This method automatically build the "KDTreeData" structure when:
x0 | The X coordinate of the query. | |
y0 | The Y coordinate of the query. | |
out_x | The X coordinate of the found closest correspondence. | |
out_y | The Y coordinate of the found closest correspondence. | |
out_dist_sqr | The square distance between the query and the returned point. |
float mrpt::slam::CPointsMap::kdTreeClosestPoint2DsqrError | ( | const float & | x0, | |
const float & | y0 | |||
) | const |
Like kdTreeClosestPoint2D, but just return the square error from some point to its closest neighbor.
size_t mrpt::slam::CPointsMap::kdTreeClosestPoint3D | ( | const float & | x0, | |
const float & | y0, | |||
const float & | z0, | |||
float & | out_x, | |||
float & | out_y, | |||
float & | out_z, | |||
float & | out_dist_sqr | |||
) | const |
KD Tree-based search for the closest point (only ONE) to some given 3D coordinates.
This method automatically build the "KDTreeData" structure when:
x0 | The X coordinate of the query. | |
y0 | The Y coordinate of the query. | |
z0 | The Z coordinate of the query. | |
out_x | The X coordinate of the found closest correspondence. | |
out_y | The Y coordinate of the found closest correspondence. | |
out_z | The Z coordinate of the found closest correspondence. | |
out_dist_sqr | The square distance between the query and the returned point. |
void mrpt::slam::CPointsMap::kdTreeNClosestPoint2D | ( | const float & | x0, | |
const float & | y0, | |||
const unsigned int & | N, | |||
std::vector< float > & | out_x, | |||
std::vector< float > & | out_y, | |||
std::vector< float > & | out_dist_sqr | |||
) | const |
KD Tree-based search for the N closest point to some given 2D coordinates.
This method automatically build the "KDTreeData" structure when:
x0 | The X coordinate of the query. | |
y0 | The Y coordinate of the query. | |
N | The number of closest points to search. | |
out_x | The vector containing the X coordinates of the correspondences. | |
out_y | The vector containing the Y coordinates of the correspondences. | |
out_dist_sqr | The vector containing the square distance between the query and the returned points. |
void mrpt::slam::CPointsMap::kdTreeNClosestPoint2DIdx | ( | const float & | x0, | |
const float & | y0, | |||
const unsigned int & | N, | |||
std::vector< int > & | out_idx, | |||
std::vector< float > & | out_dist_sqr | |||
) | const |
KD Tree-based search for the N closest point to some given 2D coordinates and returns their indexes.
This method automatically build the "KDTreeData" structure when:
x0 | The X coordinate of the query. | |
y0 | The Y coordinate of the query. | |
N | The number of closest points to search. | |
out_idx | The indexes of the found closest correspondence. | |
out_dist_sqr | The square distance between the query and the returned point. |
void mrpt::slam::CPointsMap::kdTreeNClosestPoint3D | ( | const float & | x0, | |
const float & | y0, | |||
const float & | z0, | |||
const unsigned int & | N, | |||
std::vector< float > & | out_x, | |||
std::vector< float > & | out_y, | |||
std::vector< float > & | out_z, | |||
std::vector< float > & | out_dist_sqr | |||
) | const |
KD Tree-based search for the N closest points to some given 3D coordinates.
This method automatically build the "KDTreeData" structure when:
x0 | The X coordinate of the query. | |
y0 | The Y coordinate of the query. | |
z0 | The Z coordinate of the query. | |
N | The number of closest points to search. | |
out_x | The vector containing the X coordinates of the correspondences. | |
out_y | The vector containing the Y coordinates of the correspondences. | |
out_z | The vector containing the Z coordinates of the correspondences. | |
out_dist_sqr | The vector containing the square distance between the query and the returned points. |
void mrpt::slam::CPointsMap::kdTreeNClosestPoint3DIdx | ( | const float & | x0, | |
const float & | y0, | |||
const float & | z0, | |||
const unsigned int & | N, | |||
std::vector< int > & | out_idx, | |||
std::vector< float > & | out_dist_sqr | |||
) | const |
KD Tree-based search for the N closest point to some given 3D coordinates and returns their indexes.
This method automatically build the "KDTreeData" structure when:
x0 | The X coordinate of the query. | |
y0 | The Y coordinate of the query. | |
z0 | The Z coordinate of the query. | |
N | The number of closest points to search. | |
out_idx | The indexes of the found closest correspondence. | |
out_dist_sqr | The square distance between the query and the returned point. |
void mrpt::slam::CPointsMap::kdTreeTwoClosestPoint2D | ( | const float & | x0, | |
const float & | y0, | |||
float & | out_x1, | |||
float & | out_y1, | |||
float & | out_x2, | |||
float & | out_y2, | |||
float & | out_dist_sqr1, | |||
float & | out_dist_sqr2 | |||
) | const |
KD Tree-based search for the TWO closest point to some given 2D coordinates.
This method automatically build the "KDTreeData" structure when:
x0 | The X coordinate of the query. | |
y0 | The Y coordinate of the query. | |
out_x1 | The X coordinate of the first correspondence. | |
out_y1 | The Y coordinate of the first correspondence. | |
out_x2 | The X coordinate of the second correspondence. | |
out_y2 | The Y coordinate of the second correspondence. | |
out_dist_sqr1 | The square distance between the query and the first returned point. | |
out_dist_sqr2 | The square distance between the query and the second returned point. |
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.
rangeScan | The scan to be inserted into this map | |
robotPose | The 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. |
Implemented in mrpt::slam::CColouredPointsMap, and mrpt::slam::CSimplePointsMap.
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 450 of file CPointsMap.h.
virtual void mrpt::slam::CPointsMap::setPoint | ( | const size_t & | index, | |
float | x, | |||
float | y, | |||
float | z | |||
) | [pure virtual] |
Changes a given point from map.
First index is 0.
Throws | std::exception on index out of bound. |
Implemented in mrpt::slam::CColouredPointsMap, and mrpt::slam::CSimplePointsMap.
virtual void mrpt::slam::CPointsMap::setPoint | ( | const size_t & | index, | |
float | x, | |||
float | y | |||
) | [pure virtual] |
Changes a given point from map.
First index is 0.
Throws | std::exception on index out of bound. |
Implemented in mrpt::slam::CColouredPointsMap, and mrpt::slam::CSimplePointsMap.
virtual void mrpt::slam::CPointsMap::setPoint | ( | const size_t & | index, | |
CPoint3D & | p | |||
) | [pure virtual] |
Changes a given point from map, as a 3D point.
First index is 0.
Throws | std::exception on index out of bound. |
Implemented in mrpt::slam::CColouredPointsMap, and mrpt::slam::CSimplePointsMap.
virtual void mrpt::slam::CPointsMap::setPoint | ( | const size_t & | index, | |
CPoint2D & | p | |||
) | [pure virtual] |
Changes a given point from map, as a 2D point.
First index is 0.
Throws | std::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 | ( | const float & | x0, | |
const float & | y0 | |||
) | const [virtual] |
Returns the square distance from the 2D point (x0,y0) to the closest correspondence in the map.
Reimplemented from mrpt::slam::CMetricMap.
friend class CColouredPointsMap [friend] |
Definition at line 63 of file CPointsMap.h.
friend class CMultiMetricMap [friend] |
Definition at line 60 of file CPointsMap.h.
friend class CMultiMetricMapPDF [friend] |
Definition at line 61 of file CPointsMap.h.
friend class COccupancyGridMap2D [friend] |
Definition at line 64 of file CPointsMap.h.
friend class CSimplePointsMap [friend] |
Definition at line 62 of file CPointsMap.h.
float mrpt::slam::CPointsMap::COLOR_3DSCENE_B [static] |
Definition at line 668 of file CPointsMap.h.
float mrpt::slam::CPointsMap::COLOR_3DSCENE_G [static] |
Definition at line 667 of file CPointsMap.h.
float mrpt::slam::CPointsMap::COLOR_3DSCENE_R [static] |
The color [0,1] of points when extracted from getAs3DObject (default=blue).
Definition at line 666 of file CPointsMap.h.
With this struct options are provided to the observation insertion process.
TKDTreeData mrpt::slam::CPointsMap::KDTreeData [mutable, protected] |
Definition at line 132 of file CPointsMap.h.
bool mrpt::slam::CPointsMap::m_KDTreeDataIsUpdated [mutable, protected] |
Auxiliary variables used in "buildKDTree2D" / "buildKDTree3D".
Definition at line 95 of file CPointsMap.h.
float mrpt::slam::CPointsMap::m_largestDistanceFromOrigin [mutable, protected] |
Auxiliary variables used in "getLargestDistanceFromOrigin".
Definition at line 86 of file CPointsMap.h.
bool mrpt::slam::CPointsMap::m_largestDistanceFromOriginIsUpdated [mutable, protected] |
Auxiliary variables used in "getLargestDistanceFromOrigin".
Definition at line 91 of file CPointsMap.h.
Only for objects stored into a CMultiMetricMap (among a grid map).
Otherwise, "parent" will be NULL. This wrapper class is for managing elegantly copy constructors & = operators in parent class.
Definition at line 73 of file CPointsMap.h.
std::vector<uint32_t> mrpt::slam::CPointsMap::pointWeight [protected] |
std::vector<float> mrpt::slam::CPointsMap::x [protected] |
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.5.8 for MRPT 0.6.5 SVN: at Sun Aug 9 21:47:23 CEST 2009 |