74 std::vector<unsigned int>
uVars;
86 std::vector<unsigned int>
uVars;
103 virtual void reserve(
size_t newLength) = 0;
109 virtual void resize(
size_t newLength) = 0;
115 virtual void setSize(
size_t newLength) = 0;
118 virtual void setPointFast(
size_t index,
float x,
float y,
float z)=0;
121 virtual void insertPointFast(
float x,
float y,
float z = 0 ) = 0;
124 virtual void copyFrom(
const CPointsMap &obj) = 0;
130 virtual void getPointAllFieldsFast(
const size_t index, std::vector<float> & point_data )
const = 0;
136 virtual void setPointAllFieldsFast(
const size_t index,
const std::vector<float> & point_data ) = 0;
141 virtual void addFrom_classSpecific(
const CPointsMap &anotherMap,
const size_t nPreviousPoints) = 0;
151 virtual float squareDistanceToClosestCorrespondence(
156 return squareDistanceToClosestCorrespondence(static_cast<float>(p0.
x),static_cast<float>(p0.
y));
199 void loadFromConfigFile(
201 const std::string §ion);
227 virtual void addFrom(
const CPointsMap &anotherMap);
232 this->addFrom(anotherMap);
240 void insertAnotherMap(
259 bool load2Dor3D_from_text_file(
const std::string &file,
const bool is_3D);
264 bool save2D_to_text_file(
const std::string &file)
const;
269 bool save3D_to_text_file(
const std::string &file)
const;
274 const std::string &filNamePrefix
277 std::string fil( filNamePrefix + std::string(
".txt") );
278 save3D_to_text_file( fil );
282 virtual bool savePCDFile(
const std::string &filename,
bool save_as_binary)
const;
285 virtual bool loadPCDFile(
const std::string &filename);
317 virtual bool saveLASFile(
const std::string &filename,
const LAS_WriteParams & params = LAS_WriteParams() )
const;
322 virtual bool loadLASFile(
const std::string &filename, LAS_HeaderInfo &out_headerInfo,
const LAS_LoadParams ¶ms = LAS_LoadParams() );
329 inline size_t size()
const {
return x.size(); }
336 unsigned long getPoint(
size_t index,
float &x,
float &y,
float &z)
const;
338 unsigned long getPoint(
size_t index,
float &x,
float &y)
const;
340 unsigned long getPoint(
size_t index,
double &x,
double &y,
double &z)
const;
342 unsigned long getPoint(
size_t index,
double &x,
double &y)
const;
352 virtual void getPoint(
size_t index,
float &x,
float &y,
float &z,
float &R,
float &G,
float &B )
const
354 getPoint(index,x,y,z);
360 inline void getPointFast(
size_t index,
float &x,
float &y,
float &z)
const { x=this->x[index]; y=this->y[index]; z=this->z[index]; }
368 inline void setPoint(
size_t index,
float x,
float y,
float z) {
370 setPointFast(index,x,y,z);
380 virtual void setPoint(
size_t index,
float x,
float y,
float z,
float R,
float G,
float B)
383 setPoint(index,x,y,z);
397 void getPointsBuffer(
size_t &outPointsCount,
const float *&xs,
const float *&ys,
const float *&zs )
const;
411 template <
class VECTOR>
412 void getAllPoints( VECTOR &xs, VECTOR &ys, VECTOR &zs,
size_t decimation = 1 )
const
416 const size_t Nout = x.size() / decimation;
420 size_t idx_in, idx_out;
421 for (idx_in=0,idx_out=0;idx_out<Nout;idx_in+=decimation,++idx_out)
423 xs[idx_out]=x[idx_in];
424 ys[idx_out]=y[idx_in];
425 zs[idx_out]=z[idx_in];
430 inline void getAllPoints(std::vector<mrpt::math::TPoint3D> &ps,
size_t decimation=1)
const {
431 std::vector<float> dmy1,dmy2,dmy3;
432 getAllPoints(dmy1,dmy2,dmy3,decimation);
433 ps.resize(dmy1.size());
434 for (
size_t i=0;i<dmy1.size();i++) {
435 ps[i].x=
static_cast<double>(dmy1[i]);
436 ps[i].y=
static_cast<double>(dmy2[i]);
437 ps[i].z=
static_cast<double>(dmy3[i]);
445 void getAllPoints( std::vector<float> &xs, std::vector<float> &ys,
size_t decimation = 1 )
const;
447 inline void getAllPoints(std::vector<mrpt::math::TPoint2D> &ps,
size_t decimation=1)
const {
448 std::vector<float> dmy1,dmy2;
449 getAllPoints(dmy1,dmy2,decimation);
450 ps.resize(dmy1.size());
451 for (
size_t i=0;i<dmy1.size();i++) {
452 ps[i].x=
static_cast<double>(dmy1[i]);
453 ps[i].y=
static_cast<double>(dmy2[i]);
460 inline void insertPoint(
float x,
float y,
float z=0 ) { insertPointFast(x,y,z); mark_as_modified(); }
464 virtual void insertPoint(
float x,
float y,
float z,
float R,
float G,
float B )
473 template <
typename VECTOR>
476 const size_t N = X.size();
478 ASSERT_(Z.size()==0 || Z.size()==X.size())
480 const bool z_valid = !Z.empty();
481 if (z_valid)
for (
size_t i=0;i<N;i++) { this->setPointFast(i,X[i],Y[i],Z[i]); }
482 else for (
size_t i=0;i<N;i++) { this->setPointFast(i,X[i],Y[i],0); }
487 inline void setAllPoints(
const std::vector<float> &X,
const std::vector<float> &Y,
const std::vector<float> &Z) {
488 setAllPointsTemplate(X,Y,Z);
492 inline void setAllPoints(
const std::vector<float> &X,
const std::vector<float> &Y) {
493 setAllPointsTemplate(X,Y);
501 getPointAllFieldsFast(index,point_data);
510 setPointAllFieldsFast(index,point_data);
516 void clipOutOfRangeInZ(
float zMin,
float zMax);
525 void applyDeletionMask(
const std::vector<bool> &mask );
528 virtual void determineMatching2D(
536 virtual void determineMatching3D(
544 float compute3DMatchingRatio(
547 float maxDistForCorr = 0.10f,
548 float maxMahaDistForCorr = 2.0f
564 void compute3DDistanceToMesh(
567 float maxDistForCorrespondence,
569 float &correspondencesRatio );
584 virtual void loadFromRangeScan(
597 virtual void loadFromRangeScan(
613 float minDistForFuse = 0.02f,
614 std::vector<bool> *notFusedPoints = NULL);
630 virtual bool isEmpty()
const;
633 inline bool empty()
const {
return isEmpty(); }
639 virtual void getAs3DObject ( mrpt::opengl::CSetOfObjectsPtr &outObj )
const;
649 float getLargestDistanceFromOrigin()
const;
653 output_is_valid = m_largestDistanceFromOriginIsUpdated;
654 return m_largestDistanceFromOrigin;
660 void boundingBox(
float &min_x,
float &max_x,
float &min_y,
float &max_y,
float &min_z,
float &max_z )
const;
663 float dmy1,dmy2,dmy3,dmy4,dmy5,dmy6;
664 boundingBox(dmy1,dmy2,dmy3,dmy4,dmy5,dmy6);
691 inline void setHeightFilterLevels(
const double _z_min,
const double _z_max) { m_heightfilter_z_min=_z_min; m_heightfilter_z_max=_z_max; }
693 inline void getHeightFilterLevels(
double &_z_min,
double &_z_max)
const { _z_min=m_heightfilter_z_min; _z_max=m_heightfilter_z_max; }
720 template <
class POINTCLOUD>
723 const size_t nThis = this->
size();
727 cloud.is_dense =
false;
728 cloud.points.resize(cloud.width * cloud.height);
729 for (
size_t i = 0; i < nThis; ++i) {
730 cloud.points[i].x =this->x[i];
731 cloud.points[i].y =this->y[i];
732 cloud.points[i].z =this->z[i];
746 template <
class POINTCLOUD>
749 const size_t N = cloud.points.size();
752 for (
size_t i=0;i<N;++i)
753 this->insertPointFast(cloud.points[i].x,cloud.points[i].y,cloud.points[i].z);
766 if (dim==0)
return this->x[idx];
767 else if (dim==1)
return this->y[idx];
768 else if (dim==2)
return this->z[idx];
else return 0;
776 const float d0 = p1[0]-x[idx_p2];
777 const float d1 = p1[1]-y[idx_p2];
782 const float d0 = p1[0]-x[idx_p2];
783 const float d1 = p1[1]-y[idx_p2];
784 const float d2 = p1[2]-z[idx_p2];
785 return d0*d0+d1*d1+d2*d2;
792 template <
typename BBOX>
797 bb[0].low, bb[0].high,
798 bb[1].low, bb[1].high,
801 bb[2].low = min_z; bb[2].high = max_z;
810 std::vector<float> x,y,
z;
825 mutable float m_bb_min_x,m_bb_max_x, m_bb_min_y,m_bb_max_y,
m_bb_min_z,m_bb_max_z;
831 m_largestDistanceFromOriginIsUpdated=
false;
832 m_boundingBoxIsUpdated =
false;
833 kdtree_mark_as_outdated();
840 bool internal_insertObservation(
863 virtual size_t PLY_export_get_vertex_count()
const;
871 virtual void PLY_export_get_vertex(
897 namespace global_settings
916 static const int HAS_RGB = 0;
917 static const int HAS_RGBf = 0;
918 static const int HAS_RGBu8 = 0;
923 inline size_t size()
const {
return m_obj.
size(); }
928 template <
typename T>
929 inline void getPointXYZ(
const size_t idx, T &x,T &y, T &z)
const {
933 inline void setPointXYZ(
const size_t idx,
const coords_t x,
const coords_t y,
const coords_t z) {
#define ASSERT_EQUAL_(__A, __B)
mrpt::maps::CPointsMap & m_obj
void setPoint(size_t index, mrpt::math::TPoint3D &p)
void setFromPCLPointCloud(const POINTCLOUD &cloud)
Loads a PCL point cloud into this MRPT class (note: this method ignores potential RGB information...
MAPS_IMPEXP float POINTSMAPS_3DOBJECT_POINTSIZE
The size of points when exporting with getAs3DObject() (default=3.0) Affects to:
void setAllPointsTemplate(const VECTOR &X, const VECTOR &Y, const VECTOR &Z=VECTOR())
Set all the points at once from vectors with X,Y and Z coordinates (if Z is not provided, it will be set to all zeros).
bool isPlanarMap
If set to true, only HORIZONTAL (in the XY plane) measurements will be inserted in the map (Default v...
float kdtree_get_pt(const size_t idx, int dim) const
Returns the dim'th component of the idx'th point in the class:
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).
void boundingBox(mrpt::math::TPoint3D &pMin, mrpt::math::TPoint3D &pMax) const
bool m_heightfilter_enabled
Whether or not (default=not) filter the input points by height.
void getAllPoints(std::vector< mrpt::math::TPoint2D > &ps, size_t decimation=1) const
const mrpt::obs::CObservation2DRangeScan & rangeScan
const mrpt::obs::CObservation3DRangeScan & rangeScan
void getPointAllFields(const size_t index, std::vector< float > &point_data) const
Get all the data fields for one point as a vector: depending on the implementation class this can be ...
#define MRPT_OVERRIDE
C++11 "override" for virtuals:
A helper base class for those PointCloudAdapter<> which do not handle RGB data; it declares needed in...
void setAllPoints(const std::vector< float > &X, const std::vector< float > &Y)
Set all the points at once from vectors with X and Y coordinates (Z=0).
size_t size() const
Get number of points.
size_t kdtree_get_point_count() const
Must return the number of data points.
#define DECLARE_MEXPLUS_FROM(complete_type)
This must be inserted if a custom conversion method for MEX API is implemented in the class...
float getLargestDistanceFromOriginNoRecompute(bool &output_is_valid) const
Like getLargestDistanceFromOrigin() but returns in output_is_valid = false if the distance was not al...
bool isFilterByHeightEnabled() const
Return whether filter-by-height is enabled.
#define ASSERT_BELOW_(__A, __B)
virtual void setPointFast(size_t index, float x, float y, float z)=0
Changes the coordinates of the given point (0-based index), without checking for out-of-bounds and wi...
virtual bool hasColorPoints() const
Returns true if the point map has a color field for each point.
virtual size_t PLY_export_get_face_count() const
In a base class, return the number of faces.
Declares a class derived from "CObservation" that encapsules a 3D range scan measurement, as from a time-of-flight range camera or any other RGBD sensor.
static float COLOR_3DSCENE_R
The color [0,1] of points when extracted from getAs3DObject (default=blue)
A cloud of points in 2D or 3D, which can be built from a sequence of laser scans. ...
static float COLOR_3DSCENE_G
const std::vector< float > & getPointsBufferRef_x() const
Provides a direct access to a read-only reference of the internal point buffer.
void insertPoint(float x, float y, float z=0)
Provides a way to insert (append) individual points into the map: the missing fields of child classes...
void setPointXYZ(const size_t idx, const coords_t x, const coords_t y, const coords_t z)
Set XYZ coordinates of i'th point.
double z
X,Y,Z coordinates.
bool m_boundingBoxIsUpdated
const std::vector< float > & getPointsBufferRef_y() const
Provides a direct access to a read-only reference of the internal point buffer.
virtual unsigned int getPointWeight(size_t index) const
Gets the point weight, which is ignored in all classes (defaults to 1) but in those which actually st...
float maxDistForInterpolatePoints
The maximum distance between two points to interpolate between them (ONLY when also_interpolate=true)...
float scan_z
In internal_loadFromRangeScan3D_prepareOneRange, these are the local coordinates of the scan points b...
void mark_as_modified() const
Called only by this class or children classes, set m_largestDistanceFromOriginIsUpdated=false and suc...
This class allows loading and storing values and vectors of different types from a configuration text...
#define DEFINE_VIRTUAL_SERIALIZABLE(class_name)
This declaration must be inserted in virtual CSerializable classes definition:
A generic adaptor class for providing Nearest Neighbor (NN) lookup via the nanoflann library...
void setPointAllFields(const size_t index, const std::vector< float > &point_data)
Set all the data fields for one point as a vector: depending on the implementation class this can be ...
With this struct options are provided to the observation insertion process.
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
A cloud of points in 2D or 3D, which can be built from a sequence of laser scans or other sensors...
A numeric matrix of compile-time fixed size.
bool insertInvalidPoints
Points with x,y,z coordinates set to zero will also be inserted.
TLaserRange3DInsertContext(const mrpt::obs::CObservation3DRangeScan &_rangeScan)
#define DEFINE_SERIALIZABLE_PRE_CUSTOM_BASE_LINKAGE(class_name, base_name, _LINKAGE_)
This declaration must be inserted in all CSerializable classes definition, before the class declarati...
Lightweight 3D point (float version).
#define MRPT_UNUSED_PARAM(a)
Can be used to avoid "not used parameters" warnings from the compiler.
std::vector< unsigned int > uVars
bool kdtree_get_bbox(BBOX &bb) const
double sigma_dist
Sigma (standard deviation, in meters) of the exponential used to model the likelihood (default= 0...
#define DECLARE_MEX_CONVERSION
This must be inserted if a custom conversion method for MEX API is implemented in the class...
void setAllPoints(const std::vector< float > &X, const std::vector< float > &Y, const std::vector< float > &Z)
Set all the points at once from vectors with X,Y and Z coordinates.
A smart look-up-table (LUT) of sin/cos values for 2D laser scans.
std::vector< float > z
The point coordinates.
mrpt::math::CMatrixDouble44 HM
Homog matrix of the local sensor pose within the robot.
std::vector< T1 > & operator+=(std::vector< T1 > &a, const std::vector< T2 > &b)
a+=b (element-wise sum)
float coords_t
The type of each point XYZ coordinates.
void setPoint(size_t index, float x, float y)
bool m_largestDistanceFromOriginIsUpdated
Auxiliary variables used in "getLargestDistanceFromOrigin".
EIGEN_STRONG_INLINE void setSize(size_t row, size_t col)
Changes the size of matrix, maintaining its previous content as possible and padding with zeros where...
TLaserRange2DInsertContext(const mrpt::obs::CObservation2DRangeScan &_rangeScan)
std::vector< float > fVars
Extra variables to be used as desired by the derived class.
virtual void setPointWeight(size_t index, unsigned long w)
Sets the point weight, which is ignored in all classes but those which actually store that field (Not...
void getPointXYZ(const size_t idx, T &x, T &y, T &z) const
Get XYZ coordinates of i'th point.
bool load2D_from_text_file(const std::string &file)
Load from a text file.
A virtual base class that implements the capability of importing 3D point clouds and faces from a fil...
void saveMetricMapRepresentationToFile(const std::string &filNamePrefix) const
This virtual method saves the map to a file "filNamePrefix"+< some_file_extension >...
float m_largestDistanceFromOrigin
Auxiliary variables used in "getLargestDistanceFromOrigin".
virtual void PLY_import_set_face_count(const size_t N)
In a base class, reserve memory to prepare subsequent calls to PLY_import_set_face.
Optional settings for saveLASFile()
std::vector< float > fVars
Extra variables to be used as desired by the derived class.
const std::vector< float > & getPointsBufferRef_z() const
Provides a direct access to a read-only reference of the internal point buffer.
unsigned long getPoint(size_t index, mrpt::math::TPoint2D &p) const
virtual void setPoint(size_t index, float x, float y, float z, float R, float G, float B)
void setHeightFilterLevels(const double _z_min, const double _z_max)
Set the min/max Z levels for points to be actually inserted in the map (only if enableFilterByHeight(...
virtual ~TLikelihoodOptions()
bool also_interpolate
If set to true, far points (<1m) are interpolated with samples at "minDistSqrBetweenLaserPoints" inte...
void getAllPoints(std::vector< mrpt::math::TPoint3D > &ps, size_t decimation=1) const
mrpt::obs::CSinCosLookUpTableFor2DScans m_scans_sincos_cache
Cache of sin/cos values for the latest 2D scan geometries.
void getPCLPointCloud(POINTCLOUD &cloud) const
Use to convert this MRPT point cloud object into a PCL point cloud object (PointCloud
).
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
static float COLOR_3DSCENE_B
A "CObservation"-derived class that represents a 2D range scan measurement (typically from a laser sc...
virtual void resize(size_t newLength)=0
Resizes all point buffers so they can hold the given number of points: newly created points are set t...
bool addToExistingPointsMap
Applicable to "loadFromRangeScan" only! If set to false, the points from the scan are loaded...
bool fuseWithExisting
If set to true (default=false), inserted points are "fused" with previously existent ones...
std::vector< uint8_t > bVars
TLikelihoodOptions likelihoodOptions
bool load3D_from_text_file(const std::string &file)
Load from a text file.
void getPointFast(size_t index, float &x, float &y, float &z) const
Just like getPoint() but without checking out-of-bound index and without returning the point weight...
float kdtree_distance(const float *p1, const size_t idx_p2, size_t size) const
Returns the distance between the vector "p1[0:size-1]" and the data point with index "idx_p2" stored ...
void setPoint(size_t index, float x, float y, float z)
Changes a given point from map, with Z defaulting to 0 if not provided.
size_t size(const MATRIXLIKE &m, int dim)
Declares a virtual base class for all metric maps storage classes.
A class used to store a 2D pose.
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Declares a class that represents any robot's observation.
Options used when evaluating "computeObservationLikelihood" in the derived classes.
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.
virtual void insertPoint(float x, float y, float z, float R, float G, float B)
unsigned long getPoint(size_t index, mrpt::math::TPoint3D &p) const
std::vector< uint8_t > bVars
size_t size() const
Returns the number of stored points in the map.
float horizontalTolerance
The tolerance in rads in pitch & roll for a laser scan to be considered horizontal, considered only when isPlanarMap=true (default=0).
A RGB color - floats in the range [0,1].
virtual mrpt::maps::CSimplePointsMap * getAsSimplePointsMap()
PointCloudAdapter(const mrpt::maps::CPointsMap &obj)
Constructor (accept a const ref for convenience)
Helper struct used for internal_loadFromRangeScan3D_prepareOneRange()
virtual const mrpt::maps::CSimplePointsMap * getAsSimplePointsMap() const
If the map is a simple points map or it's a multi-metric map that contains EXACTLY one simple points ...
TInsertionOptions insertionOptions
The options used when inserting observations in the map.
double max_corr_distance
Maximum distance in meters to consider for the numerator divided by "sigma_dist", so that each point ...
bool empty() const
STL-like method to check whether the map is empty:
void enableFilterByHeight(bool enable=true)
Enable/disable the filter-by-height functionality.
An adapter to different kinds of point cloud object.
uint32_t decimation
Speed up the likelihood computation by considering only one out of N rays (default=10) ...
Parameters for the determination of matchings between point clouds, etc.
mrpt::math::CMatrixDouble44 HM
Homog matrix of the local sensor pose within the robot.
#define DEFINE_SERIALIZABLE_POST_CUSTOM_BASE_LINKAGE(class_name, base_name, _LINKAGE_)
A virtual base class that implements the capability of exporting 3D point clouds and faces to a file ...
bool disableDeletion
If set to false (default=true) points in the same plane as the inserted scan and inside the free spac...
void getHeightFilterLevels(double &_z_min, double &_z_max) const
Get the min/max Z levels for points to be actually inserted in the map.
void setPoint(size_t index, mrpt::math::TPoint2D &p)
void insertPoint(const mrpt::math::TPoint3D &p)
float squareDistanceToClosestCorrespondenceT(const mrpt::math::TPoint2D &p0) const
This is a virtual base class for sets of options than can be loaded from and/or saved to configuratio...
Helper struct used for internal_loadFromRangeScan2D_prepareOneRange()
void resize(const size_t N)
Set number of points (to uninitialized values)
double m_heightfilter_z_min
The minimum and maximum height for a certain laser scan to be inserted into this map.
float minDistBetweenLaserPoints
The minimum distance between points (in 3D): If two points are too close, one of them is not inserted...
Optional settings for loadLASFile()
std::vector< unsigned int > uVars