75 std::vector<unsigned int>
uVars;
87 std::vector<unsigned int>
uVars;
104 virtual void reserve(
size_t newLength) = 0;
110 virtual void resize(
size_t newLength) = 0;
116 virtual void setSize(
size_t newLength) = 0;
119 virtual void setPointFast(
size_t index,
float x,
float y,
float z)=0;
122 virtual void insertPointFast(
float x,
float y,
float z = 0 ) = 0;
125 virtual void copyFrom(
const CPointsMap &obj) = 0;
131 virtual void getPointAllFieldsFast(
const size_t index, std::vector<float> & point_data )
const = 0;
137 virtual void setPointAllFieldsFast(
const size_t index,
const std::vector<float> & point_data ) = 0;
142 virtual void addFrom_classSpecific(
const CPointsMap &anotherMap,
const size_t nPreviousPoints) = 0;
152 virtual float squareDistanceToClosestCorrespondence(
157 return squareDistanceToClosestCorrespondence(static_cast<float>(p0.
x),static_cast<float>(p0.
y));
220 virtual void addFrom(
const CPointsMap &anotherMap);
225 this->addFrom(anotherMap);
233 void insertAnotherMap(
252 bool load2Dor3D_from_text_file(
const std::string &file,
const bool is_3D);
257 bool save2D_to_text_file(
const std::string &file)
const;
262 bool save3D_to_text_file(
const std::string &file)
const;
267 std::string fil( filNamePrefix + std::string(
".txt") );
268 save3D_to_text_file( fil );
272 virtual bool savePCDFile(
const std::string &filename,
bool save_as_binary)
const;
275 virtual bool loadPCDFile(
const std::string &filename);
307 virtual bool saveLASFile(
const std::string &filename,
const LAS_WriteParams & params = LAS_WriteParams() )
const;
312 virtual bool loadLASFile(
const std::string &filename, LAS_HeaderInfo &out_headerInfo,
const LAS_LoadParams ¶ms = LAS_LoadParams() );
319 inline size_t size()
const {
return x.size(); }
326 unsigned long getPoint(
size_t index,
float &x,
float &y,
float &z)
const;
328 unsigned long getPoint(
size_t index,
float &x,
float &y)
const;
330 unsigned long getPoint(
size_t index,
double &x,
double &y,
double &z)
const;
332 unsigned long getPoint(
size_t index,
double &x,
double &y)
const;
342 virtual void getPoint(
size_t index,
float &x,
float &y,
float &z,
float &R,
float &G,
float &B )
const 344 getPoint(index,x,y,z);
350 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]; }
358 inline void setPoint(
size_t index,
float x,
float y,
float z) {
360 setPointFast(index,x,y,z);
370 virtual void setPoint(
size_t index,
float x,
float y,
float z,
float R,
float G,
float B)
373 setPoint(index,x,y,z);
387 void getPointsBuffer(
size_t &outPointsCount,
const float *&xs,
const float *&ys,
const float *&zs )
const;
401 template <
class VECTOR>
402 void getAllPoints( VECTOR &xs, VECTOR &ys, VECTOR &zs,
size_t decimation = 1 )
const 406 const size_t Nout = x.size() / decimation;
410 size_t idx_in, idx_out;
411 for (idx_in=0,idx_out=0;idx_out<Nout;idx_in+=decimation,++idx_out)
413 xs[idx_out]=x[idx_in];
414 ys[idx_out]=y[idx_in];
415 zs[idx_out]=z[idx_in];
420 inline void getAllPoints(std::vector<mrpt::math::TPoint3D> &ps,
size_t decimation=1)
const {
421 std::vector<float> dmy1,dmy2,dmy3;
422 getAllPoints(dmy1,dmy2,dmy3,decimation);
423 ps.resize(dmy1.size());
424 for (
size_t i=0;i<dmy1.size();i++) {
425 ps[i].x=
static_cast<double>(dmy1[i]);
426 ps[i].y=
static_cast<double>(dmy2[i]);
427 ps[i].z=
static_cast<double>(dmy3[i]);
435 void getAllPoints( std::vector<float> &xs, std::vector<float> &ys,
size_t decimation = 1 )
const;
437 inline void getAllPoints(std::vector<mrpt::math::TPoint2D> &ps,
size_t decimation=1)
const {
438 std::vector<float> dmy1,dmy2;
439 getAllPoints(dmy1,dmy2,decimation);
440 ps.resize(dmy1.size());
441 for (
size_t i=0;i<dmy1.size();i++) {
442 ps[i].x=
static_cast<double>(dmy1[i]);
443 ps[i].y=
static_cast<double>(dmy2[i]);
450 inline void insertPoint(
float x,
float y,
float z=0 ) { insertPointFast(x,y,z); mark_as_modified(); }
454 virtual void insertPoint(
float x,
float y,
float z,
float R,
float G,
float B )
463 template <
typename VECTOR>
466 const size_t N = X.size();
468 ASSERT_(Z.size()==0 || Z.size()==X.size())
470 const bool z_valid = !Z.empty();
471 if (z_valid)
for (
size_t i=0;i<N;i++) { this->setPointFast(i,X[i],Y[i],Z[i]); }
472 else for (
size_t i=0;i<N;i++) { this->setPointFast(i,X[i],Y[i],0); }
477 inline void setAllPoints(
const std::vector<float> &X,
const std::vector<float> &Y,
const std::vector<float> &Z) {
478 setAllPointsTemplate(X,Y,Z);
482 inline void setAllPoints(
const std::vector<float> &X,
const std::vector<float> &Y) {
483 setAllPointsTemplate(X,Y);
491 getPointAllFieldsFast(index,point_data);
500 setPointAllFieldsFast(index,point_data);
506 void clipOutOfRangeInZ(
float zMin,
float zMax);
515 void applyDeletionMask(
const std::vector<bool> &mask );
518 virtual void determineMatching2D(
526 virtual void determineMatching3D(
547 void compute3DDistanceToMesh(
550 float maxDistForCorrespondence,
552 float &correspondencesRatio );
567 virtual void loadFromRangeScan(
581 virtual void loadFromRangeScan(
591 void loadFromVelodyneScan(
607 float minDistForFuse = 0.02f,
608 std::vector<bool> *notFusedPoints = NULL);
627 inline
bool empty()
const {
return isEmpty(); }
633 virtual void getAs3DObject( mrpt::opengl::CSetOfObjectsPtr &outObj )
const MRPT_OVERRIDE;
643 float getLargestDistanceFromOrigin()
const;
647 output_is_valid = m_largestDistanceFromOriginIsUpdated;
648 return m_largestDistanceFromOrigin;
654 void boundingBox(
float &min_x,
float &max_x,
float &min_y,
float &max_y,
float &min_z,
float &max_z )
const;
657 float dmy1,dmy2,dmy3,dmy4,dmy5,dmy6;
658 boundingBox(dmy1,dmy2,dmy3,dmy4,dmy5,dmy6);
685 inline void setHeightFilterLevels(
const double _z_min,
const double _z_max) { m_heightfilter_z_min=_z_min; m_heightfilter_z_max=_z_max; }
687 inline void getHeightFilterLevels(
double &_z_min,
double &_z_max)
const { _z_min=m_heightfilter_z_min; _z_max=m_heightfilter_z_max; }
714 template <
class POINTCLOUD>
717 const size_t nThis = this->
size();
721 cloud.is_dense =
false;
722 cloud.points.resize(cloud.width * cloud.height);
723 for (
size_t i = 0; i < nThis; ++i) {
724 cloud.points[i].x =this->x[i];
725 cloud.points[i].y =this->y[i];
726 cloud.points[i].z =this->z[i];
740 template <
class POINTCLOUD>
743 const size_t N = cloud.points.size();
746 for (
size_t i=0;i<N;++i)
747 this->insertPointFast(cloud.points[i].x,cloud.points[i].y,cloud.points[i].z);
760 if (dim==0)
return this->x[idx];
761 else if (dim==1)
return this->y[idx];
762 else if (dim==2)
return this->z[idx];
else return 0;
770 const float d0 = p1[0]-x[idx_p2];
771 const float d1 = p1[1]-y[idx_p2];
776 const float d0 = p1[0]-x[idx_p2];
777 const float d1 = p1[1]-y[idx_p2];
778 const float d2 = p1[2]-z[idx_p2];
779 return d0*d0+d1*d1+d2*d2;
786 template <
typename BBOX>
791 bb[0].low, bb[0].high,
792 bb[1].low, bb[1].high,
795 bb[2].low = min_z; bb[2].high = max_z;
804 std::vector<float> x,y,
z;
819 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;
825 m_largestDistanceFromOriginIsUpdated=
false;
826 m_boundingBoxIsUpdated =
false;
827 kdtree_mark_as_outdated();
875 namespace global_settings
894 static const int HAS_RGB = 0;
895 static const int HAS_RGBf = 0;
896 static const int HAS_RGBu8 = 0;
901 inline size_t size()
const {
return m_obj.
size(); }
906 template <
typename T>
907 inline void getPointXYZ(
const size_t idx, T &x,T &y, T &z)
const {
#define ASSERT_EQUAL_(__A, __B)
const std::vector< float > & getPointsBufferRef_x() const
Provides a direct access to a read-only reference of the internal point buffer.
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:
Parameters for CMetricMap::compute3DMatchingRatio()
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).
EIGEN_STRONG_INLINE bool empty() const
bool isPlanarMap
If set to true, only HORIZONTAL (in the XY plane) measurements will be inserted in the map (Default v...
void getPointXYZ(const size_t idx, T &x, T &y, T &z) const
Get XYZ coordinates of i'th point.
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...
A CObservation-derived class for RAW DATA (and optionally, point cloud) of scans from 3D Velodyne LID...
bool m_heightfilter_enabled
Whether or not (default=not) filter the input points by height.
const mrpt::obs::CObservation2DRangeScan & rangeScan
const mrpt::obs::CObservation3DRangeScan & rangeScan
const std::vector< float > & getPointsBufferRef_y() const
Provides a direct access to a read-only reference of the internal point buffer.
#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).
#define DECLARE_MEXPLUS_FROM(complete_type)
This must be inserted if a custom conversion method for MEX API is implemented in the class...
#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...
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.
void getAllPoints(std::vector< mrpt::math::TPoint2D > &ps, size_t decimation=1) const
static float COLOR_3DSCENE_R
The color [0,1] of points when extracted from getAs3DObject (default=blue)
void saveMetricMapRepresentationToFile(const std::string &filNamePrefix) const MRPT_OVERRIDE
This virtual method saves the map to a file "filNamePrefix"+< some_file_extension >...
size_t kdtree_get_point_count() const
Must return the number of data points.
A cloud of points in 2D or 3D, which can be built from a sequence of laser scans. ...
static float COLOR_3DSCENE_G
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.
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...
double z
X,Y,Z coordinates.
bool m_boundingBoxIsUpdated
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 ...
float squareDistanceToClosestCorrespondenceT(const mrpt::math::TPoint2D &p0) const
size_t size() const
Get number of points.
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).
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...
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:
bool kdtree_get_bbox(BBOX &bb) const
unsigned long getPoint(size_t index, mrpt::math::TPoint2D &p) const
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 ...
bool isFilterByHeightEnabled() const
Return whether filter-by-height is enabled.
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...
float kdtree_get_pt(const size_t idx, int dim) const
Returns the dim'th component of the idx'th point in the class:
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
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.
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.
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.
virtual void PLY_import_set_face_count(const size_t N) MRPT_OVERRIDE
In a base class, reserve memory to prepare subsequent calls to PLY_import_set_face.
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, float x, float y)
bool m_largestDistanceFromOriginIsUpdated
Auxiliary variables used in "getLargestDistanceFromOrigin".
void getAllPoints(std::vector< mrpt::math::TPoint3D > &ps, size_t decimation=1) const
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...
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 getPCLPointCloud(POINTCLOUD &cloud) const
Use to convert this MRPT point cloud object into a PCL point cloud object (PointCloud<PointXYZ>).
float m_largestDistanceFromOrigin
Auxiliary variables used in "getLargestDistanceFromOrigin".
Optional settings for saveLASFile()
std::vector< float > fVars
Extra variables to be used as desired by the derived class.
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...
mrpt::obs::CSinCosLookUpTableFor2DScans m_scans_sincos_cache
Cache of sin/cos values for the latest 2D scan geometries.
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...
void mark_as_modified() const
Called only by this class or children classes, set m_largestDistanceFromOriginIsUpdated=false and suc...
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
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 ...
TLikelihoodOptions likelihoodOptions
bool load3D_from_text_file(const std::string &file)
Load from a text file.
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.
unsigned long getPoint(size_t index, mrpt::math::TPoint3D &p) const
virtual void insertPoint(float x, float y, float z, float R, float G, float B)
std::vector< uint8_t > bVars
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].
const std::vector< float > & getPointsBufferRef_z() const
Provides a direct access to a read-only reference of the internal point buffer.
PointCloudAdapter(const mrpt::maps::CPointsMap &obj)
Constructor (accept a const ref for convenience)
Helper struct used for internal_loadFromRangeScan3D_prepareOneRange()
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 ...
virtual const mrpt::maps::CSimplePointsMap * getAsSimplePointsMap() const MRPT_OVERRIDE
If the map is a simple points map or it's a multi-metric map that contains EXACTLY one simple points ...
void enableFilterByHeight(bool enable=true)
Enable/disable the filter-by-height functionality.
An adapter to different kinds of point cloud object.
virtual mrpt::maps::CSimplePointsMap * getAsSimplePointsMap() MRPT_OVERRIDE
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_)
size_t size() const
Returns the number of stored points in the map.
float getLargestDistanceFromOriginNoRecompute(bool &output_is_valid) const
Like getLargestDistanceFromOrigin() but returns in output_is_valid = false if the distance was not al...
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 boundingBox(mrpt::math::TPoint3D &pMin, mrpt::math::TPoint3D &pMax) const
void setPoint(size_t index, mrpt::math::TPoint2D &p)
void insertPoint(const mrpt::math::TPoint3D &p)
virtual bool hasColorPoints() const
Returns true if the point map has a color field for each point.
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