Main MRPT website > C++ reference for MRPT 1.4.0
maps/CPointsMap.h
Go to the documentation of this file.
1/* +---------------------------------------------------------------------------+
2 | Mobile Robot Programming Toolkit (MRPT) |
3 | http://www.mrpt.org/ |
4 | |
5 | Copyright (c) 2005-2016, Individual contributors, see AUTHORS file |
6 | See: http://www.mrpt.org/Authors - All rights reserved. |
7 | Released under BSD License. See details in http://www.mrpt.org/License |
8 +---------------------------------------------------------------------------+ */
9#ifndef CPOINTSMAP_H
10#define CPOINTSMAP_H
11
21#include <mrpt/obs/obs_frwds.h>
23#include <mrpt/utils/adapters.h>
24
25// Add for declaration of mexplus::from template specialization
27
28namespace mrpt
29{
30/** \ingroup mrpt_maps_grp */
31namespace maps
32{
34
35 // Forward decls. needed to make its static methods friends of CPointsMap
36 namespace detail {
37 template <class Derived> struct loadFromRangeImpl;
38 template <class Derived> struct pointmap_traits;
39 }
40
41
42 /** A cloud of points in 2D or 3D, which can be built from a sequence of laser scans or other sensors.
43 * This is a virtual class, thus only a derived class can be instantiated by the user. The user most usually wants to use CSimplePointsMap.
44 *
45 * This class implements generic version of mrpt::maps::CMetric::insertObservation() accepting these types of sensory data:
46 * - mrpt::obs::CObservation2DRangeScan: 2D range scans
47 * - mrpt::obs::CObservation3DRangeScan: 3D range scans (Kinect, etc...)
48 * - mrpt::obs::CObservationRange: IRs, Sonars, etc.
49 * - mrpt::obs::CObservationVelodyneScan
50 *
51 * If built against liblas, this class also provides method for loading and saving in the standard LAS LiDAR point cloud format: saveLASFile(), loadLASFile()
52 *
53 * \sa CMetricMap, CPoint, mrpt::utils::CSerializable
54 * \ingroup mrpt_maps_grp
55 */
57 public CMetricMap,
58 public mrpt::math::KDTreeCapable<CPointsMap>,
61 {
62 // This must be added to any CSerializable derived class:
64 // This must be added for declaration of MEX-related functions
66
67 protected:
68 /** Helper struct used for \a internal_loadFromRangeScan2D_prepareOneRange() */
70 TLaserRange2DInsertContext(const mrpt::obs::CObservation2DRangeScan &_rangeScan) : HM(mrpt::math::UNINITIALIZED_MATRIX), rangeScan(_rangeScan)
71 { }
72 mrpt::math::CMatrixDouble44 HM; //!< Homog matrix of the local sensor pose within the robot
74 std::vector<float> fVars; //!< Extra variables to be used as desired by the derived class.
75 std::vector<unsigned int> uVars;
76 std::vector<uint8_t> bVars;
77 };
78
79 /** Helper struct used for \a internal_loadFromRangeScan3D_prepareOneRange() */
81 TLaserRange3DInsertContext(const mrpt::obs::CObservation3DRangeScan &_rangeScan) : HM(mrpt::math::UNINITIALIZED_MATRIX), rangeScan(_rangeScan)
82 { }
83 mrpt::math::CMatrixDouble44 HM; //!< Homog matrix of the local sensor pose within the robot
85 float scan_x, scan_y,scan_z; //!< In \a internal_loadFromRangeScan3D_prepareOneRange, these are the local coordinates of the scan points being inserted right now.
86 std::vector<float> fVars; //!< Extra variables to be used as desired by the derived class.
87 std::vector<unsigned int> uVars;
88 std::vector<uint8_t> bVars;
89 };
90
91 public:
92 CPointsMap(); //!< Ctor
93 virtual ~CPointsMap(); //!< Virtual destructor.
94
95
96 // --------------------------------------------
97 /** @name Pure virtual interfaces to be implemented by any class derived from CPointsMap
98 @{ */
99
100 /** Reserves memory for a given number of points: the size of the map does not change, it only reserves the memory.
101 * This is useful for situations where it is approximately known the final size of the map. This method is more
102 * efficient than constantly increasing the size of the buffers. Refer to the STL C++ library's "reserve" methods.
103 */
104 virtual void reserve(size_t newLength) = 0;
105
106 /** Resizes all point buffers so they can hold the given number of points: newly created points are set to default values,
107 * and old contents are not changed.
108 * \sa reserve, setPoint, setPointFast, setSize
109 */
110 virtual void resize(size_t newLength) = 0;
111
112 /** Resizes all point buffers so they can hold the given number of points, *erasing* all previous contents
113 * and leaving all points to default values.
114 * \sa reserve, setPoint, setPointFast, setSize
115 */
116 virtual void setSize(size_t newLength) = 0;
117
118 /** Changes the coordinates of the given point (0-based index), *without* checking for out-of-bounds and *without* calling mark_as_modified() \sa setPoint */
119 virtual void setPointFast(size_t index,float x, float y, float z)=0;
120
121 /** The virtual method for \a insertPoint() *without* calling mark_as_modified() */
122 virtual void insertPointFast( float x, float y, float z = 0 ) = 0;
123
124 /** Virtual assignment operator, copies as much common data (XYZ, color,...) as possible from the source map into this one. */
125 virtual void copyFrom(const CPointsMap &obj) = 0;
126
127 /** Get all the data fields for one point as a vector: depending on the implementation class this can be [X Y Z] or [X Y Z R G B], etc...
128 * Unlike getPointAllFields(), this method does not check for index out of bounds
129 * \sa getPointAllFields, setPointAllFields, setPointAllFieldsFast
130 */
131 virtual void getPointAllFieldsFast( const size_t index, std::vector<float> & point_data ) const = 0;
132
133 /** Set all the data fields for one point as a vector: depending on the implementation class this can be [X Y Z] or [X Y Z R G B], etc...
134 * Unlike setPointAllFields(), this method does not check for index out of bounds
135 * \sa setPointAllFields, getPointAllFields, getPointAllFieldsFast
136 */
137 virtual void setPointAllFieldsFast( const size_t index, const std::vector<float> & point_data ) = 0;
138
139 protected:
140
141 /** Auxiliary method called from within \a addFrom() automatically, to finish the copying of class-specific data */
142 virtual void addFrom_classSpecific(const CPointsMap &anotherMap, const size_t nPreviousPoints) = 0;
143
144 public:
145
146 /** @} */
147 // --------------------------------------------
148
149
150 /** Returns the square distance from the 2D point (x0,y0) to the closest correspondence in the map.
151 */
153 float x0,
154 float y0 ) const MRPT_OVERRIDE;
155
157 return squareDistanceToClosestCorrespondence(static_cast<float>(p0.x),static_cast<float>(p0.y));
158 }
159
160
161 /** With this struct options are provided to the observation insertion process.
162 * \sa CObservation::insertIntoPointsMap
163 */
165 {
166 /** Initilization of default parameters */
168 void loadFromConfigFile(const mrpt::utils::CConfigFileBase &source,const std::string &section) MRPT_OVERRIDE; // See base docs
169 void dumpToTextStream(mrpt::utils::CStream &out) const MRPT_OVERRIDE; // See base docs
170
171 float minDistBetweenLaserPoints; //!< The minimum distance between points (in 3D): If two points are too close, one of them is not inserted into the map. Default is 0.02 meters.
172 bool addToExistingPointsMap; //!< Applicable to "loadFromRangeScan" only! If set to false, the points from the scan are loaded, clearing all previous content. Default is false.
173 bool also_interpolate; //!< If set to true, far points (<1m) are interpolated with samples at "minDistSqrBetweenLaserPoints" intervals (Default is false).
174 bool disableDeletion; //!< If set to false (default=true) points in the same plane as the inserted scan and inside the free space, are erased: i.e. they don't exist yet.
175 bool fuseWithExisting; //!< If set to true (default=false), inserted points are "fused" with previously existent ones. This shrink the size of the points map, but its slower.
176 bool isPlanarMap; //!< If set to true, only HORIZONTAL (in the XY plane) measurements will be inserted in the map (Default value is false, thus 3D maps are generated). \sa horizontalTolerance
177 float horizontalTolerance; //!< The tolerance in rads in pitch & roll for a laser scan to be considered horizontal, considered only when isPlanarMap=true (default=0).
178 float maxDistForInterpolatePoints; //!< The maximum distance between two points to interpolate between them (ONLY when also_interpolate=true)
179 bool insertInvalidPoints; //!< Points with x,y,z coordinates set to zero will also be inserted
180
181 void writeToStream(mrpt::utils::CStream &out) const; //!< Binary dump to stream - for usage in derived classes' serialization
182 void readFromStream(mrpt::utils::CStream &in); //!< Binary dump to stream - for usage in derived classes' serialization
183 };
184
185 TInsertionOptions insertionOptions; //!< The options used when inserting observations in the map
186
187 /** Options used when evaluating "computeObservationLikelihood" in the derived classes.
188 * \sa CObservation::computeObservationLikelihood
189 */
191 {
192 /** Initilization of default parameters
193 */
196 void loadFromConfigFile(const mrpt::utils::CConfigFileBase &source,const std::string &section) MRPT_OVERRIDE; // See base docs
197 void dumpToTextStream(mrpt::utils::CStream &out) const MRPT_OVERRIDE; // See base docs
198
199 void writeToStream(mrpt::utils::CStream &out) const; //!< Binary dump to stream - for usage in derived classes' serialization
200 void readFromStream(mrpt::utils::CStream &in); //!< Binary dump to stream - for usage in derived classes' serialization
201
202 double sigma_dist; //!< Sigma (standard deviation, in meters) of the exponential used to model the likelihood (default= 0.5meters)
203 double max_corr_distance; //!< Maximum distance in meters to consider for the numerator divided by "sigma_dist", so that each point has a minimum (but very small) likelihood to avoid underflows (default=1.0 meters)
204 uint32_t decimation; //!< Speed up the likelihood computation by considering only one out of N rays (default=10)
205 };
206
208
209
210 /** Adds all the points from \a anotherMap to this map, without fusing.
211 * This operation can be also invoked via the "+=" operator, for example:
212 * \code
213 * mrpt::maps::CSimplePointsMap m1, m2;
214 * ...
215 * m1.addFrom( m2 ); // Add all points of m2 to m1
216 * m1 += m2; // Exactly the same than above
217 * \endcode
218 * \note The method in CPointsMap is generic but derived classes may redefine this virtual method to another one more optimized.
219 */
220 virtual void addFrom(const CPointsMap &anotherMap);
221
222 /** This operator is synonymous with \a addFrom. \sa addFrom */
223 inline void operator +=(const CPointsMap &anotherMap)
224 {
225 this->addFrom(anotherMap);
226 }
227
228 /** Insert the contents of another map into this one with some geometric transformation, without fusing close points.
229 * \param otherMap The other map whose points are to be inserted into this one.
230 * \param otherPose The pose of the other map in the coordinates of THIS map
231 * \sa fuseWith, addFrom
232 */
234 const CPointsMap *otherMap,
235 const mrpt::poses::CPose3D &otherPose);
236
237 // --------------------------------------------------
238 /** @name File input/output methods
239 @{ */
240
241 /** Load from a text file. Each line should contain an "X Y" coordinate pair, separated by whitespaces.
242 * Returns false if any error occured, true elsewere.
243 */
244 inline bool load2D_from_text_file(const std::string &file) { return load2Dor3D_from_text_file(file,false); }
245
246 /** Load from a text file. Each line should contain an "X Y Z" coordinate tuple, separated by whitespaces.
247 * Returns false if any error occured, true elsewere.
248 */
249 inline bool load3D_from_text_file(const std::string &file) { return load2Dor3D_from_text_file(file,true); }
250
251 /** 2D or 3D generic implementation of \a load2D_from_text_file and load3D_from_text_file */
252 bool load2Dor3D_from_text_file(const std::string &file, const bool is_3D);
253
254 /** Save to a text file. Each line will contain "X Y" point coordinates.
255 * Returns false if any error occured, true elsewere.
256 */
257 bool save2D_to_text_file(const std::string &file) const;
258
259 /** Save to a text file. Each line will contain "X Y Z" point coordinates.
260 * Returns false if any error occured, true elsewere.
261 */
262 bool save3D_to_text_file(const std::string &file)const;
263
264 /** 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) */
265 void saveMetricMapRepresentationToFile(const std::string &filNamePrefix)const MRPT_OVERRIDE
266 {
267 std::string fil( filNamePrefix + std::string(".txt") );
268 save3D_to_text_file( fil );
269 }
270
271 /** Save the point cloud as a PCL PCD file, in either ASCII or binary format (requires MRPT built against PCL) \return false on any error */
272 virtual bool savePCDFile(const std::string &filename, bool save_as_binary) const;
273
274 /** Load the point cloud from a PCL PCD file (requires MRPT built against PCL) \return false on any error */
275 virtual bool loadPCDFile(const std::string &filename);
276
277
278 /** Optional settings for saveLASFile() */
280 {
281 // None.
282 };
283
284 /** Optional settings for loadLASFile() */
286 {
287 // None.
288 };
289
290 /** Extra information gathered from the LAS file header */
292 {
293 std::string FileSignature;
294 std::string SystemIdentifier;
296 std::string project_guid;
297 std::string spatial_reference_proj4; //!< Proj.4 string describing the Spatial Reference System.
298 uint16_t creation_year;//!< Creation date (Year number)
299 uint16_t creation_DOY; //!< Creation day of year
300
301 LAS_HeaderInfo() : creation_year(0),creation_DOY(0)
302 {}
303 };
304
305 /** Save the point cloud as an ASPRS LAS binary file (requires MRPT built against liblas). Refer to http://www.liblas.org/
306 * \return false on any error */
307 virtual bool saveLASFile(const std::string &filename, const LAS_WriteParams & params = LAS_WriteParams() ) const;
308
309 /** Load the point cloud from an ASPRS LAS binary file (requires MRPT built against liblas). Refer to http://www.liblas.org/
310 * \note Color (RGB) information will be taken into account if using the derived class mrpt::maps::CColouredPointsMap
311 * \return false on any error */
312 virtual bool loadLASFile(const std::string &filename, LAS_HeaderInfo &out_headerInfo, const LAS_LoadParams &params = LAS_LoadParams() );
313
314 /** @} */ // End of: File input/output methods
315 // --------------------------------------------------
316
317 /** Returns the number of stored points in the map.
318 */
319 inline size_t size() const { return x.size(); }
320
321 /** Access to a given point from map, as a 2D point. First index is 0.
322 * \return The return value is the weight of the point (the times it has been fused), or 1 if weights are not used.
323 * \exception Throws std::exception on index out of bound.
324 * \sa setPoint, getPointFast
325 */
326 unsigned long getPoint(size_t index,float &x,float &y,float &z) const;
327 /// \overload
328 unsigned long getPoint(size_t index,float &x,float &y) const;
329 /// \overload
330 unsigned long getPoint(size_t index,double &x,double &y,double &z) const;
331 /// \overload
332 unsigned long getPoint(size_t index,double &x,double &y) const;
333 /// \overload
334 inline unsigned long getPoint(size_t index,mrpt::math::TPoint2D &p) const { return getPoint(index,p.x,p.y); }
335 /// \overload
336 inline unsigned long getPoint(size_t index,mrpt::math::TPoint3D &p) const { return getPoint(index,p.x,p.y,p.z); }
337
338 /** 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.
339 * \return The return value is the weight of the point (the times it has been fused)
340 * \exception Throws std::exception on index out of bound.
341 */
342 virtual void getPoint( size_t index, float &x, float &y, float &z, float &R, float &G, float &B ) const
343 {
344 getPoint(index,x,y,z);
345 R=G=B=1;
346 }
347
348 /** Just like \a getPoint() but without checking out-of-bound index and without returning the point weight, just XYZ.
349 */
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]; }
351
352 /** Returns true if the point map has a color field for each point */
353 virtual bool hasColorPoints() const { return false; }
354
355 /** Changes a given point from map, with Z defaulting to 0 if not provided.
356 * \exception Throws std::exception on index out of bound.
357 */
358 inline void setPoint(size_t index,float x, float y, float z) {
359 ASSERT_BELOW_(index,this->size())
360 setPointFast(index,x,y,z);
361 mark_as_modified();
362 }
363 /// \overload
364 inline void setPoint(size_t index,mrpt::math::TPoint2D &p) { setPoint(index,p.x,p.y,0); }
365 /// \overload
366 inline void setPoint(size_t index,mrpt::math::TPoint3D &p) { setPoint(index,p.x,p.y,p.z); }
367 /// \overload
368 inline void setPoint(size_t index,float x, float y) { setPoint(index,x,y,0); }
369 /// \overload (RGB data is ignored in classes without color information)
370 virtual void setPoint(size_t index,float x, float y, float z, float R, float G, float B)
371 {
373 setPoint(index,x,y,z);
374 }
375
376 /// Sets the point weight, which is ignored in all classes but those which actually store that field (Note: No checks are done for out-of-bounds index). \sa getPointWeight
377 virtual void setPointWeight(size_t index,unsigned long w)
378 {
380 }
381 /// Gets the point weight, which is ignored in all classes (defaults to 1) but in those which actually store that field (Note: No checks are done for out-of-bounds index). \sa setPointWeight
382 virtual unsigned int getPointWeight(size_t index) const { MRPT_UNUSED_PARAM(index); return 1; }
383
384
385 /** Provides a direct access to points buffer, or NULL if there is no points in the map.
386 */
387 void getPointsBuffer( size_t &outPointsCount, const float *&xs, const float *&ys, const float *&zs ) const;
388
389 /** Provides a direct access to a read-only reference of the internal point buffer. \sa getAllPoints */
390 inline const std::vector<float> & getPointsBufferRef_x() const { return x; }
391 /** Provides a direct access to a read-only reference of the internal point buffer. \sa getAllPoints */
392 inline const std::vector<float> & getPointsBufferRef_y() const { return y; }
393 /** Provides a direct access to a read-only reference of the internal point buffer. \sa getAllPoints */
394 inline const std::vector<float> & getPointsBufferRef_z() const { return z; }
395
396 /** Returns a copy of the 2D/3D points as a std::vector of float coordinates.
397 * 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.
398 * \sa getPointsBufferRef_x, getPointsBufferRef_y, getPointsBufferRef_z
399 * \tparam VECTOR can be std::vector<float or double> or any row/column Eigen::Array or Eigen::Matrix (this includes mrpt::math::CVectorFloat and mrpt::math::CVectorDouble).
400 */
401 template <class VECTOR>
402 void getAllPoints( VECTOR &xs, VECTOR &ys, VECTOR &zs, size_t decimation = 1 ) const
403 {
405 ASSERT_(decimation>0)
406 const size_t Nout = x.size() / decimation;
407 xs.resize(Nout);
408 ys.resize(Nout);
409 zs.resize(Nout);
410 size_t idx_in, idx_out;
411 for (idx_in=0,idx_out=0;idx_out<Nout;idx_in+=decimation,++idx_out)
412 {
413 xs[idx_out]=x[idx_in];
414 ys[idx_out]=y[idx_in];
415 zs[idx_out]=z[idx_in];
416 }
418 }
419
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]);
428 }
429 }
430
431 /** Returns a copy of the 2D/3D points as a std::vector of float coordinates.
432 * 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.
433 * \sa setAllPoints
434 */
435 void getAllPoints( std::vector<float> &xs, std::vector<float> &ys, size_t decimation = 1 ) const;
436
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]);
444 }
445 }
446
447 /** Provides a way to insert (append) individual points into the map: the missing fields of child
448 * classes (color, weight, etc) are left to their default values
449 */
450 inline void insertPoint( float x, float y, float z=0 ) { insertPointFast(x,y,z); mark_as_modified(); }
451 /// \overload of \a insertPoint()
452 inline void insertPoint( const mrpt::math::TPoint3D &p ) { insertPoint(p.x,p.y,p.z); }
453 /// \overload (RGB data is ignored in classes without color information)
454 virtual void insertPoint( float x, float y, float z, float R, float G, float B )
455 {
457 insertPoint(x,y,z);
458 }
459
460 /** 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).
461 * \tparam VECTOR can be mrpt::math::CVectorFloat or std::vector<float> or any other column or row Eigen::Matrix.
462 */
463 template <typename VECTOR>
464 inline void setAllPointsTemplate(const VECTOR &X,const VECTOR &Y,const VECTOR &Z = VECTOR())
465 {
466 const size_t N = X.size();
467 ASSERT_EQUAL_(X.size(),Y.size())
468 ASSERT_(Z.size()==0 || Z.size()==X.size())
469 this->setSize(N);
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); }
473 mark_as_modified();
474 }
475
476 /** Set all the points at once from vectors with X,Y and Z coordinates. \sa getAllPoints */
477 inline void setAllPoints(const std::vector<float> &X,const std::vector<float> &Y,const std::vector<float> &Z) {
478 setAllPointsTemplate(X,Y,Z);
479 }
480
481 /** Set all the points at once from vectors with X and Y coordinates (Z=0). \sa getAllPoints */
482 inline void setAllPoints(const std::vector<float> &X,const std::vector<float> &Y) {
483 setAllPointsTemplate(X,Y);
484 }
485
486 /** Get all the data fields for one point as a vector: depending on the implementation class this can be [X Y Z] or [X Y Z R G B], etc...
487 * \sa getPointAllFieldsFast, setPointAllFields, setPointAllFieldsFast
488 */
489 void getPointAllFields( const size_t index, std::vector<float> & point_data ) const {
490 ASSERT_BELOW_(index,this->size())
491 getPointAllFieldsFast(index,point_data);
492 }
493
494 /** Set all the data fields for one point as a vector: depending on the implementation class this can be [X Y Z] or [X Y Z R G B], etc...
495 * Unlike setPointAllFields(), this method does not check for index out of bounds
496 * \sa setPointAllFields, getPointAllFields, getPointAllFieldsFast
497 */
498 void setPointAllFields( const size_t index, const std::vector<float> & point_data ){
499 ASSERT_BELOW_(index,this->size())
500 setPointAllFieldsFast(index,point_data);
501 }
502
503
504 /** Delete points out of the given "z" axis range have been removed.
505 */
506 void clipOutOfRangeInZ(float zMin, float zMax);
507
508 /** Delete points which are more far than "maxRange" away from the given "point".
509 */
510 void clipOutOfRange(const mrpt::math::TPoint2D &point, float maxRange);
511
512 /** Remove from the map the points marked in a bool's array as "true".
513 * \exception std::exception If mask size is not equal to points count.
514 */
515 void applyDeletionMask( const std::vector<bool> &mask );
516
517 // See docs in base class.
519 const mrpt::maps::CMetricMap * otherMap,
520 const mrpt::poses::CPose2D & otherMapPose,
521 mrpt::utils::TMatchingPairList & correspondences,
522 const TMatchingParams & params,
523 TMatchingExtraResults & extraResults ) const MRPT_OVERRIDE;
524
525 // See docs in base class
527 const mrpt::maps::CMetricMap * otherMap,
528 const mrpt::poses::CPose3D & otherMapPose,
529 mrpt::utils::TMatchingPairList & correspondences,
530 const TMatchingParams & params,
531 TMatchingExtraResults & extraResults ) const MRPT_OVERRIDE;
532
533 // See docs in base class
534 float compute3DMatchingRatio(const mrpt::maps::CMetricMap *otherMap, const mrpt::poses::CPose3D &otherMapPose, const TMatchingRatioParams &params) const MRPT_OVERRIDE;
535
536 /** Computes the matchings between this and another 3D points map.
537 This method matches each point in the other map with the centroid of the 3 closest points in 3D
538 from this map (if the distance is below a defined threshold).
539 * \param otherMap [IN] The other map to compute the matching with.
540 * \param otherMapPose [IN] The pose of the other map as seen from "this".
541 * \param maxDistForCorrespondence [IN] Maximum 2D linear distance between two points to be matched.
542 * \param correspondences [OUT] The detected matchings pairs.
543 * \param correspondencesRatio [OUT] The ratio [0,1] of points in otherMap with at least one correspondence.
544 *
545 * \sa determineMatching3D
546 */
548 const mrpt::maps::CMetricMap *otherMap2,
549 const mrpt::poses::CPose3D &otherMapPose,
550 float maxDistForCorrespondence,
551 mrpt::utils::TMatchingPairList &correspondences,
552 float &correspondencesRatio );
553
554 /** Transform the range scan into a set of cartessian coordinated
555 * points. The options in "insertionOptions" are considered in this method.
556 * \param rangeScan The scan to be inserted into this map
557 * \param robotPose Default to (0,0,0|0deg,0deg,0deg). Changes the frame of reference for the point cloud (i.e. the vehicle/robot pose in world coordinates).
558 *
559 * Only ranges marked as "valid=true" in the observation will be inserted
560 *
561 * \note Each derived class may enrich points in different ways (color, weight, etc..), so please refer to the description of the specific
562 * implementation of mrpt::maps::CPointsMap you are using.
563 * \note The actual generic implementation of this file lives in <src>/CPointsMap_crtp_common.h, but specific instantiations are generated at each derived class.
564 *
565 * \sa CObservation2DRangeScan, CObservation3DRangeScan
566 */
567 virtual void loadFromRangeScan(
568 const mrpt::obs::CObservation2DRangeScan &rangeScan,
569 const mrpt::poses::CPose3D *robotPose = NULL ) = 0;
570
571 /** Overload of \a loadFromRangeScan() for 3D range scans (for example, Kinect observations).
572 *
573 * \param rangeScan The scan to be inserted into this map
574 * \param robotPose Default to (0,0,0|0deg,0deg,0deg). Changes the frame of reference for the point cloud (i.e. the vehicle/robot pose in world coordinates).
575 *
576 * \note Each derived class may enrich points in different ways (color, weight, etc..), so please refer to the description of the specific
577 * implementation of mrpt::maps::CPointsMap you are using.
578 * \note The actual generic implementation of this file lives in <src>/CPointsMap_crtp_common.h, but specific instantiations are generated at each derived class.
579 * \sa loadFromVelodyneScan
580 */
581 virtual void loadFromRangeScan(
582 const mrpt::obs::CObservation3DRangeScan &rangeScan,
583 const mrpt::poses::CPose3D *robotPose = NULL ) = 0;
584
585 /** Like \a loadFromRangeScan() for Velodyne 3D scans. Points are translated and rotated according to the \a sensorPose field in the observation and, if provided, to the \a robotPose parameter.
586 *
587 * \param scan The Raw LIDAR data to be inserted into this map. It MUST contain point cloud data, generated by calling to \a mrpt::obs::CObservationVelodyneScan::generatePointCloud() prior to insertion in this map.
588 * \param robotPose Default to (0,0,0|0deg,0deg,0deg). Changes the frame of reference for the point cloud (i.e. the vehicle/robot pose in world coordinates).
589 * \sa loadFromRangeScan
590 */
593 const mrpt::poses::CPose3D *robotPose = NULL );
594
595 /** Insert the contents of another map into this one, fusing the previous content with the new one.
596 * This means that points very close to existing ones will be "fused", rather than "added". This prevents
597 * the unbounded increase in size of these class of maps.
598 * NOTICE that "otherMap" is neither translated nor rotated here, so if this is desired it must done
599 * before calling this method.
600 * \param otherMap The other map whose points are to be inserted into this one.
601 * \param 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.
602 * \param 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.
603 * \sa loadFromRangeScan, addFrom
604 */
606 CPointsMap *anotherMap,
607 float minDistForFuse = 0.02f,
608 std::vector<bool> *notFusedPoints = NULL);
609
610 /** Replace each point \f$ p_i \f$ by \f$ p'_i = b \oplus p_i \f$ (pose compounding operator).
611 */
613
614 /** Replace each point \f$ p_i \f$ by \f$ p'_i = b \oplus p_i \f$ (pose compounding operator).
615 */
617
618 /** Copy all the points from "other" map to "this", replacing each point \f$ p_i \f$ by \f$ p'_i = b \oplus p_i \f$ (pose compounding operator).
619 */
621
622 /** Returns true if the map is empty/no observation has been inserted.
623 */
624 virtual bool isEmpty() const MRPT_OVERRIDE;
625
626 /** STL-like method to check whether the map is empty: */
627 inline bool empty() const { return isEmpty(); }
628
629 /** Returns a 3D object representing the map.
630 * The color of the points is given by the static variables: COLOR_3DSCENE_R,COLOR_3DSCENE_G,COLOR_3DSCENE_B
631 * \sa mrpt::global_settings::POINTSMAPS_3DOBJECT_POINTSIZE
632 */
634
635 /** If the map is a simple points map or it's a multi-metric map that contains EXACTLY one simple points map, return it.
636 * Otherwise, return NULL
637 */
638 virtual const mrpt::maps::CSimplePointsMap * getAsSimplePointsMap() const MRPT_OVERRIDE { return NULL; }
640
641
642 /** 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). */
644
645 /** Like \a getLargestDistanceFromOrigin() but returns in \a output_is_valid = false if the distance was not already computed, skipping its computation then, unlike getLargestDistanceFromOrigin() */
646 float getLargestDistanceFromOriginNoRecompute(bool &output_is_valid) const {
647 output_is_valid = m_largestDistanceFromOriginIsUpdated;
648 return m_largestDistanceFromOrigin;
649 }
650
651 /** Computes the bounding box of all the points, or (0,0 ,0,0, 0,0) if there are no points.
652 * Results are cached unless the map is somehow modified to avoid repeated calculations.
653 */
654 void boundingBox( float &min_x,float &max_x,float &min_y,float &max_y,float &min_z,float &max_z ) const;
655
657 float dmy1,dmy2,dmy3,dmy4,dmy5,dmy6;
658 boundingBox(dmy1,dmy2,dmy3,dmy4,dmy5,dmy6);
659 pMin.x=dmy1;
660 pMin.y=dmy3;
661 pMin.z=dmy5;
662 pMax.x=dmy2;
663 pMax.y=dmy4;
664 pMax.z=dmy6;
665 }
666
667 /** Extracts the points in the map within a cylinder in 3D defined the provided radius and zmin/zmax values.
668 */
669 void extractCylinder( const mrpt::math::TPoint2D &center, const double radius, const double zmin, const double zmax, CPointsMap *outMap );
670
671 /** Extracts the points in the map within the area defined by two corners.
672 * The points are coloured according the R,G,B input data.
673 */
674 void extractPoints( const mrpt::math::TPoint3D &corner1, const mrpt::math::TPoint3D &corner2, CPointsMap *outMap, const double &R = 1, const double &G = 1, const double &B = 1 );
675
676 /** @name Filter-by-height stuff
677 @{ */
678
679 /** Enable/disable the filter-by-height functionality \sa setHeightFilterLevels \note Default upon construction is disabled. */
680 inline void enableFilterByHeight(bool enable=true) { m_heightfilter_enabled=enable; }
681 /** Return whether filter-by-height is enabled \sa enableFilterByHeight */
682 inline bool isFilterByHeightEnabled() const { return m_heightfilter_enabled; }
683
684 /** Set the min/max Z levels for points to be actually inserted in the map (only if \a enableFilterByHeight() was called before). */
685 inline void setHeightFilterLevels(const double _z_min, const double _z_max) { m_heightfilter_z_min=_z_min; m_heightfilter_z_max=_z_max; }
686 /** Get the min/max Z levels for points to be actually inserted in the map \sa enableFilterByHeight, setHeightFilterLevels */
687 inline void getHeightFilterLevels(double &_z_min, double &_z_max) const { _z_min=m_heightfilter_z_min; _z_max=m_heightfilter_z_max; }
688
689 /** @} */
690
691 /** The color [0,1] of points when extracted from getAs3DObject (default=blue) */
692 static float COLOR_3DSCENE_R;
693 static float COLOR_3DSCENE_G;
694 static float COLOR_3DSCENE_B;
695
696
697 // See docs in base class
699
700 /** @name PCL library support
701 @{ */
702
703
704 /** Use to convert this MRPT point cloud object into a PCL point cloud object (PointCloud<PointXYZ>).
705 * Usage example:
706 * \code
707 * mrpt::maps::CPointsCloud pc;
708 * pcl::PointCloud<pcl::PointXYZ> cloud;
709 *
710 * pc.getPCLPointCloud(cloud);
711 * \endcode
712 * \sa setFromPCLPointCloud, CColouredPointsMap::getPCLPointCloudXYZRGB (for color data)
713 */
714 template <class POINTCLOUD>
715 void getPCLPointCloud(POINTCLOUD &cloud) const
716 {
717 const size_t nThis = this->size();
718 // Fill in the cloud data
719 cloud.width = nThis;
720 cloud.height = 1;
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];
727 }
728 }
729
730 /** Loads a PCL point cloud into this MRPT class (note: this method ignores potential RGB information, see CColouredPointsMap::setFromPCLPointCloudRGB() ).
731 * Usage example:
732 * \code
733 * pcl::PointCloud<pcl::PointXYZ> cloud;
734 * mrpt::maps::CPointsCloud pc;
735 *
736 * pc.setFromPCLPointCloud(cloud);
737 * \endcode
738 * \sa getPCLPointCloud, CColouredPointsMap::setFromPCLPointCloudRGB()
739 */
740 template <class POINTCLOUD>
741 void setFromPCLPointCloud(const POINTCLOUD &cloud)
742 {
743 const size_t N = cloud.points.size();
744 clear();
745 reserve(N);
746 for (size_t i=0;i<N;++i)
747 this->insertPointFast(cloud.points[i].x,cloud.points[i].y,cloud.points[i].z);
748 }
749
750 /** @} */
751
752 /** @name Methods that MUST be implemented by children classes of KDTreeCapable
753 @{ */
754
755 /// Must return the number of data points
756 inline size_t kdtree_get_point_count() const { return this->size(); }
757
758 /// Returns the dim'th component of the idx'th point in the class:
759 inline float kdtree_get_pt(const size_t idx, int dim) const {
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;
763 }
764
765 /// Returns the distance between the vector "p1[0:size-1]" and the data point with index "idx_p2" stored in the class:
766 inline float kdtree_distance(const float *p1, const size_t idx_p2,size_t size) const
767 {
768 if (size==2)
769 {
770 const float d0 = p1[0]-x[idx_p2];
771 const float d1 = p1[1]-y[idx_p2];
772 return d0*d0+d1*d1;
773 }
774 else
775 {
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;
780 }
781 }
782
783 // Optional bounding-box computation: return false to default to a standard bbox computation loop.
784 // Return true if the BBOX was already computed by the class and returned in "bb" so it can be avoided to redo it again.
785 // Look at bb.size() to find out the expected dimensionality (e.g. 2 or 3 for point clouds)
786 template <typename BBOX>
787 bool kdtree_get_bbox(BBOX &bb) const
788 {
789 float min_z,max_z;
790 this->boundingBox(
791 bb[0].low, bb[0].high,
792 bb[1].low, bb[1].high,
793 min_z,max_z);
794 if (bb.size()==3) {
795 bb[2].low = min_z; bb[2].high = max_z;
796 }
797 return true;
798 }
799
800
801 /** @} */
802
803 protected:
804 std::vector<float> x,y,z; //!< The point coordinates
805
806 mrpt::obs::CSinCosLookUpTableFor2DScans m_scans_sincos_cache; //!< Cache of sin/cos values for the latest 2D scan geometries.
807
808 /** Auxiliary variables used in "getLargestDistanceFromOrigin"
809 * \sa getLargestDistanceFromOrigin
810 */
812
813 /** Auxiliary variables used in "getLargestDistanceFromOrigin"
814 * \sa getLargestDistanceFromOrigin
815 */
817
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;
820
821
822 /** Called only by this class or children classes, set m_largestDistanceFromOriginIsUpdated=false and such. */
823 inline void mark_as_modified() const
824 {
825 m_largestDistanceFromOriginIsUpdated=false;
826 m_boundingBoxIsUpdated = false;
827 kdtree_mark_as_outdated();
828 }
829
830 /** This is a common version of CMetricMap::insertObservation() for point maps (actually, CMetricMap::internal_insertObservation),
831 * so derived classes don't need to worry implementing that method unless something special is really necesary.
832 * See mrpt::maps::CPointsMap for the enumeration of types of observations which are accepted. */
834
835 /** Helper method for ::copyFrom() */
836 void base_copyFrom(const CPointsMap &obj);
837
838
839 /** @name PLY Import virtual methods to implement in base classes
840 @{ */
841 /** In a base class, reserve memory to prepare subsequent calls to PLY_import_set_face */
842 virtual void PLY_import_set_face_count(const size_t N) MRPT_OVERRIDE { MRPT_UNUSED_PARAM(N); }
843
844 /** In a base class, will be called after PLY_import_set_vertex_count() once for each loaded point.
845 * \param pt_color Will be NULL if the loaded file does not provide color info.
846 */
847 virtual void PLY_import_set_vertex(const size_t idx, const mrpt::math::TPoint3Df &pt, const mrpt::utils::TColorf *pt_color = NULL) MRPT_OVERRIDE;
848 /** @} */
849
850 /** @name PLY Export virtual methods to implement in base classes
851 @{ */
853 size_t PLY_export_get_face_count() const MRPT_OVERRIDE { return 0; }
854 virtual void PLY_export_get_vertex(const size_t idx,mrpt::math::TPoint3Df &pt,bool &pt_has_color,mrpt::utils::TColorf &pt_color) const MRPT_OVERRIDE;
855 /** @} */
856
857 /** The minimum and maximum height for a certain laser scan to be inserted into this map
858 * \sa m_heightfilter_enabled */
859 double m_heightfilter_z_min, m_heightfilter_z_max;
860
861 /** Whether or not (default=not) filter the input points by height
862 * \sa m_heightfilter_z_min, m_heightfilter_z_max */
864
865
866 // Friend methods:
867 template <class Derived> friend struct detail::loadFromRangeImpl;
868 template <class Derived> friend struct detail::pointmap_traits;
869
870 }; // End of class def.
872
873 } // End of namespace
874
875 namespace global_settings
876 {
877 /** The size of points when exporting with getAs3DObject() (default=3.0)
878 * Affects to:
879 * - mrpt::maps::CPointsMap and all its children classes.
880 */
882 }
883
884 namespace utils
885 {
886 /** Specialization mrpt::utils::PointCloudAdapter<mrpt::maps::CPointsMap> \ingroup mrpt_adapters_grp*/
887 template <>
889 {
890 private:
892 public:
893 typedef float coords_t; //!< The type of each point XYZ coordinates
894 static const int HAS_RGB = 0; //!< Has any color RGB info?
895 static const int HAS_RGBf = 0; //!< Has native RGB info (as floats)?
896 static const int HAS_RGBu8 = 0; //!< Has native RGB info (as uint8_t)?
897
898 /** Constructor (accept a const ref for convenience) */
899 inline PointCloudAdapter(const mrpt::maps::CPointsMap &obj) : m_obj(*const_cast<mrpt::maps::CPointsMap*>(&obj)) { }
900 /** Get number of points */
901 inline size_t size() const { return m_obj.size(); }
902 /** Set number of points (to uninitialized values) */
903 inline void resize(const size_t N) { m_obj.resize(N); }
904
905 /** Get XYZ coordinates of i'th point */
906 template <typename T>
907 inline void getPointXYZ(const size_t idx, T &x,T &y, T &z) const {
908 m_obj.getPointFast(idx,x,y,z);
909 }
910 /** Set XYZ coordinates of i'th point */
911 inline void setPointXYZ(const size_t idx, const coords_t x,const coords_t y, const coords_t z) {
912 m_obj.setPointFast(idx,x,y,z);
913 }
914 }; // end of PointCloudAdapter<mrpt::maps::CPointsMap>
915
916 }
917
918} // End of namespace
919
920#endif
#define DEFINE_SERIALIZABLE_POST_CUSTOM_BASE_LINKAGE(class_name, base_name, _LINKAGE_)
#define DEFINE_VIRTUAL_SERIALIZABLE(class_name)
This declaration must be inserted in virtual CSerializable classes definition:
#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...
#define DECLARE_MEXPLUS_FROM(complete_type)
This must be inserted if a custom conversion method for MEX API is implemented in the class.
#define DECLARE_MEX_CONVERSION
This must be inserted if a custom conversion method for MEX API is implemented in the class.
Declares a virtual base class for all metric maps storage classes.
A cloud of points in 2D or 3D, which can be built from a sequence of laser scans or other sensors.
virtual bool isEmpty() const MRPT_OVERRIDE
Returns true if the map is empty/no observation has been inserted.
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,...
bool load2D_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.
void loadFromVelodyneScan(const mrpt::obs::CObservationVelodyneScan &scan, const mrpt::poses::CPose3D *robotPose=NULL)
Like loadFromRangeScan() for Velodyne 3D scans.
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 ...
bool isFilterByHeightEnabled() const
Return whether filter-by-height is enabled.
void applyDeletionMask(const std::vector< bool > &mask)
Remove from the map the points marked in a bool's array as "true".
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 save2D_to_text_file(const std::string &file) const
Save to a text file.
void insertAnotherMap(const CPointsMap *otherMap, const mrpt::poses::CPose3D &otherPose)
Insert the contents of another map into this one with some geometric transformation,...
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 float squareDistanceToClosestCorrespondence(float x0, float y0) const MRPT_OVERRIDE
Returns the square distance from the 2D point (x0,y0) to the closest correspondence in the map.
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 changeCoordinatesReference(const mrpt::poses::CPose2D &b)
Replace each point by (pose compounding operator).
virtual void copyFrom(const CPointsMap &obj)=0
Virtual assignment operator, copies as much common data (XYZ, color,...) as possible from the source ...
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.
virtual void determineMatching2D(const mrpt::maps::CMetricMap *otherMap, const mrpt::poses::CPose2D &otherMapPose, mrpt::utils::TMatchingPairList &correspondences, const TMatchingParams &params, TMatchingExtraResults &extraResults) const MRPT_OVERRIDE
Computes the matching between this and another 2D point map, which includes finding:
virtual void addFrom_classSpecific(const CPointsMap &anotherMap, const size_t nPreviousPoints)=0
Auxiliary method called from within addFrom() automatically, to finish the copying of class-specific ...
void clipOutOfRangeInZ(float zMin, float zMax)
Delete points out of the given "z" axis range have been removed.
void setPoint(size_t index, float x, float y)
This is an overloaded member function, provided for convenience. It differs from the above function o...
virtual double internal_computeObservationLikelihood(const mrpt::obs::CObservation *obs, const mrpt::poses::CPose3D &takenFrom) MRPT_OVERRIDE
Internal method called by computeObservationLikelihood()
void getAllPoints(std::vector< mrpt::math::TPoint3D > &ps, size_t decimation=1) const
virtual void PLY_import_set_vertex(const size_t idx, const mrpt::math::TPoint3Df &pt, const mrpt::utils::TColorf *pt_color=NULL) MRPT_OVERRIDE
In a base class, will be called after PLY_import_set_vertex_count() once for each loaded point.
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,...
void mark_as_modified() const
Called only by this class or children classes, set m_largestDistanceFromOriginIsUpdated=false and suc...
virtual void getPointAllFieldsFast(const size_t index, std::vector< float > &point_data) const =0
Get all the data fields for one point as a vector: depending on the implementation class this can be ...
float compute3DMatchingRatio(const mrpt::maps::CMetricMap *otherMap, const mrpt::poses::CPose3D &otherMapPose, const TMatchingRatioParams &params) const MRPT_OVERRIDE
Computes the ratio in [0,1] of correspondences between "this" and the "otherMap" map,...
void boundingBox(mrpt::math::TPoint3D &pMin, mrpt::math::TPoint3D &pMax) const
void setFromPCLPointCloud(const POINTCLOUD &cloud)
Loads a PCL point cloud into this MRPT class (note: this method ignores potential RGB information,...
void setPoint(size_t index, mrpt::math::TPoint2D &p)
This is an overloaded member function, provided for convenience. It differs from the above function o...
virtual void PLY_export_get_vertex(const size_t idx, mrpt::math::TPoint3Df &pt, bool &pt_has_color, mrpt::utils::TColorf &pt_color) const MRPT_OVERRIDE
In a base class, will be called after PLY_export_get_vertex_count() once for each exported point.
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.
unsigned long getPoint(size_t index, mrpt::math::TPoint2D &p) const
This is an overloaded member function, provided for convenience. It differs from the above function o...
virtual ~CPointsMap()
Virtual destructor.
virtual bool loadPCDFile(const std::string &filename)
Load the point cloud from a PCL PCD file (requires MRPT built against PCL)
virtual void setPointAllFieldsFast(const size_t index, const std::vector< float > &point_data)=0
Set all the data fields for one point as a vector: depending on the implementation class this can be ...
virtual bool saveLASFile(const std::string &filename, const LAS_WriteParams &params=LAS_WriteParams()) const
Save the point cloud as an ASPRS LAS binary file (requires MRPT built against liblas).
bool m_largestDistanceFromOriginIsUpdated
Auxiliary variables used in "getLargestDistanceFromOrigin".
bool m_heightfilter_enabled
Whether or not (default=not) filter the input points by height.
virtual void setPoint(size_t index, float x, float y, float z, float R, float G, float B)
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.
const std::vector< float > & getPointsBufferRef_x() const
Provides a direct access to a read-only reference of the internal point buffer.
float getLargestDistanceFromOriginNoRecompute(bool &output_is_valid) const
Like getLargestDistanceFromOrigin() but returns in output_is_valid = false if the distance was not al...
const std::vector< float > & getPointsBufferRef_z() const
Provides a direct access to a read-only reference of the internal point buffer.
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 ...
size_t kdtree_get_point_count() const
Must return the number of data points.
void getPCLPointCloud(POINTCLOUD &cloud) const
Use to convert this MRPT point cloud object into a PCL point cloud object (PointCloud<PointXYZ>).
float getLargestDistanceFromOrigin() const
This method returns the largest distance from the origin to any of the points, such as a sphere cente...
virtual bool hasColorPoints() const
Returns true if the point map has a color field for each point.
void fuseWith(CPointsMap *anotherMap, float minDistForFuse=0.02f, std::vector< bool > *notFusedPoints=NULL)
Insert the contents of another map into this one, fusing the previous content with the new one.
void getHeightFilterLevels(double &_z_min, double &_z_max) const
Get the min/max Z levels for points to be actually inserted in the map.
size_t size() const
Returns the number of stored points in the map.
void enableFilterByHeight(bool enable=true)
Enable/disable the filter-by-height functionality.
float squareDistanceToClosestCorrespondenceT(const mrpt::math::TPoint2D &p0) const
void clipOutOfRange(const mrpt::math::TPoint2D &point, float maxRange)
Delete points which are more far than "maxRange" away from the given "point".
void insertPoint(const mrpt::math::TPoint3D &p)
void compute3DDistanceToMesh(const mrpt::maps::CMetricMap *otherMap2, const mrpt::poses::CPose3D &otherMapPose, float maxDistForCorrespondence, mrpt::utils::TMatchingPairList &correspondences, float &correspondencesRatio)
Computes the matchings between this and another 3D points map.
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 mrpt::maps::CSimplePointsMap * getAsSimplePointsMap() MRPT_OVERRIDE
void extractCylinder(const mrpt::math::TPoint2D &center, const double radius, const double zmin, const double zmax, CPointsMap *outMap)
Extracts the points in the map within a cylinder in 3D defined the provided radius and zmin/zmax valu...
virtual void insertPointFast(float x, float y, float z=0)=0
The virtual method for insertPoint() without calling mark_as_modified()
bool save3D_to_text_file(const std::string &file) const
Save to a text file.
unsigned long getPoint(size_t index, float &x, float &y, float &z) const
Access to a given point from map, as a 2D point.
unsigned long getPoint(size_t index, float &x, float &y) const
This is an overloaded member function, provided for convenience. It differs from the above function o...
void getAllPoints(std::vector< mrpt::math::TPoint2D > &ps, size_t decimation=1) const
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 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 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...
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
virtual void setSize(size_t newLength)=0
Resizes all point buffers so they can hold the given number of points, erasing all previous contents ...
virtual void reserve(size_t newLength)=0
Reserves memory for a given number of points: the size of the map does not change,...
static float COLOR_3DSCENE_R
The color [0,1] of points when extracted from getAs3DObject (default=blue)
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...
void extractPoints(const mrpt::math::TPoint3D &corner1, const mrpt::math::TPoint3D &corner2, CPointsMap *outMap, const double &R=1, const double &G=1, const double &B=1)
Extracts the points in the map within the area defined by two corners.
virtual bool loadLASFile(const std::string &filename, LAS_HeaderInfo &out_headerInfo, const LAS_LoadParams &params=LAS_LoadParams())
Load the point cloud from an ASPRS LAS binary file (requires MRPT built against liblas).
void base_copyFrom(const CPointsMap &obj)
Helper method for copyFrom()
mrpt::obs::CSinCosLookUpTableFor2DScans m_scans_sincos_cache
Cache of sin/cos values for the latest 2D scan geometries.
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,...
void changeCoordinatesReference(const mrpt::poses::CPose3D &b)
Replace each point by (pose compounding operator).
bool internal_insertObservation(const mrpt::obs::CObservation *obs, const mrpt::poses::CPose3D *robotPose) MRPT_OVERRIDE
This is a common version of CMetricMap::insertObservation() for point maps (actually,...
virtual void addFrom(const CPointsMap &anotherMap)
Adds all the points from anotherMap to this map, without fusing.
bool load3D_from_text_file(const std::string &file)
Load from a text file.
size_t PLY_export_get_vertex_count() const MRPT_OVERRIDE
In a base class, return the number of vertices.
virtual void loadFromRangeScan(const mrpt::obs::CObservation3DRangeScan &rangeScan, const mrpt::poses::CPose3D *robotPose=NULL)=0
Overload of loadFromRangeScan() for 3D range scans (for example, Kinect observations).
unsigned long getPoint(size_t index, double &x, double &y, double &z) const
This is an overloaded member function, provided for convenience. It differs from the above function o...
void changeCoordinatesReference(const CPointsMap &other, const mrpt::poses::CPose3D &b)
Copy all the points from "other" map to "this", replacing each point by (pose compounding operator)...
TInsertionOptions insertionOptions
The options used when inserting observations in the map.
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 ...
std::vector< float > x
void saveMetricMapRepresentationToFile(const std::string &filNamePrefix) const MRPT_OVERRIDE
This virtual method saves the map to a file "filNamePrefix"+< some_file_extension >,...
void setPoint(size_t index, mrpt::math::TPoint3D &p)
This is an overloaded member function, provided for convenience. It differs from the above function o...
virtual void determineMatching3D(const mrpt::maps::CMetricMap *otherMap, const mrpt::poses::CPose3D &otherMapPose, mrpt::utils::TMatchingPairList &correspondences, const TMatchingParams &params, TMatchingExtraResults &extraResults) const MRPT_OVERRIDE
Computes the matchings between this and another 3D points map - method used in 3D-ICP.
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).
virtual bool savePCDFile(const std::string &filename, bool save_as_binary) const
Save the point cloud as a PCL PCD file, in either ASCII or binary format (requires MRPT built against...
bool load2Dor3D_from_text_file(const std::string &file, const bool is_3D)
2D or 3D generic implementation of load2D_from_text_file and load3D_from_text_file
bool kdtree_get_bbox(BBOX &bb) const
unsigned long getPoint(size_t index, double &x, double &y) const
This is an overloaded member function, provided for convenience. It differs from the above function o...
virtual void insertPoint(float x, float y, float z, float R, float G, float B)
virtual void loadFromRangeScan(const mrpt::obs::CObservation2DRangeScan &rangeScan, const mrpt::poses::CPose3D *robotPose=NULL)=0
Transform the range scan into a set of cartessian coordinated points.
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.
const std::vector< float > & getPointsBufferRef_y() const
Provides a direct access to a read-only reference of the internal point buffer.
virtual void getAs3DObject(mrpt::opengl::CSetOfObjectsPtr &outObj) const MRPT_OVERRIDE
Returns a 3D object representing the map.
float m_largestDistanceFromOrigin
Auxiliary variables used in "getLargestDistanceFromOrigin".
unsigned long getPoint(size_t index, mrpt::math::TPoint3D &p) const
This is an overloaded member function, provided for convenience. It differs from the above function o...
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.
A cloud of points in 2D or 3D, which can be built from a sequence of laser scans.
A numeric matrix of compile-time fixed size.
A generic adaptor class for providing Nearest Neighbor (NN) lookup via the nanoflann library.
A "CObservation"-derived class that represents a 2D range scan measurement (typically from a laser sc...
Declares a class derived from "CObservation" that encapsules a 3D range scan measurement,...
Declares a class that represents any robot's observation.
A CObservation-derived class for RAW DATA (and optionally, point cloud) of scans from 3D Velodyne LID...
A smart look-up-table (LUT) of sin/cos values for 2D laser scans.
A class used to store a 2D pose.
Definition CPose2D.h:37
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition CPose3D.h:73
This class allows loading and storing values and vectors of different types from a configuration text...
This is a virtual base class for sets of options than can be loaded from and/or saved to configuratio...
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
Definition CStream.h:39
A virtual base class that implements the capability of exporting 3D point clouds and faces to a file ...
A virtual base class that implements the capability of importing 3D point clouds and faces from a fil...
void getPointXYZ(const size_t idx, T &x, T &y, T &z) const
Get XYZ coordinates of i'th point.
PointCloudAdapter(const mrpt::maps::CPointsMap &obj)
Constructor (accept a const ref for convenience)
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.
float coords_t
The type of each point XYZ coordinates.
void resize(const size_t N)
Set number of points (to uninitialized values)
An adapter to different kinds of point cloud object.
Definition adapters.h:38
A list of TMatchingPair.
A helper base class for those PointCloudAdapter<> which do not handle RGB data; it declares needed in...
Definition adapters.h:49
EIGEN_STRONG_INLINE bool empty() 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...
#define ASSERT_EQUAL_(__A, __B)
#define MRPT_START
#define ASSERT_(f)
#define MRPT_OVERRIDE
C++11 "override" for virtuals:
Definition mrpt_macros.h:28
#define MRPT_END
#define ASSERT_BELOW_(__A, __B)
#define MRPT_UNUSED_PARAM(a)
Can be used to avoid "not used parameters" warnings from the compiler.
MAPS_IMPEXP float POINTSMAPS_3DOBJECT_POINTSIZE
The size of points when exporting with getAs3DObject() (default=3.0) Affects to:
struct OPENGL_IMPEXP CSetOfObjectsPtr
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
unsigned long uint32_t
Definition pstdint.h:216
unsigned int uint16_t
Definition pstdint.h:170
Extra information gathered from the LAS file header.
std::string spatial_reference_proj4
Proj.4 string describing the Spatial Reference System.
uint16_t creation_DOY
Creation day of year.
uint16_t creation_year
Creation date (Year number)
Optional settings for loadLASFile()
Optional settings for saveLASFile()
With this struct options are provided to the observation insertion process.
void writeToStream(mrpt::utils::CStream &out) const
Binary dump to stream - for usage in derived classes' serialization.
bool disableDeletion
If set to false (default=true) points in the same plane as the inserted scan and inside the free spac...
void loadFromConfigFile(const mrpt::utils::CConfigFileBase &source, const std::string &section) MRPT_OVERRIDE
This method load the options from a ".ini"-like file or memory-stored string list.
void dumpToTextStream(mrpt::utils::CStream &out) const MRPT_OVERRIDE
This method should clearly display all the contents of the structure in textual form,...
float minDistBetweenLaserPoints
The minimum distance between points (in 3D): If two points are too close, one of them is not inserted...
bool insertInvalidPoints
Points with x,y,z coordinates set to zero will also be inserted.
TInsertionOptions()
Initilization of default parameters.
bool fuseWithExisting
If set to true (default=false), inserted points are "fused" with previously existent ones....
bool isPlanarMap
If set to true, only HORIZONTAL (in the XY plane) measurements will be inserted in the map (Default v...
float maxDistForInterpolatePoints
The maximum distance between two points to interpolate between them (ONLY when also_interpolate=true)
bool also_interpolate
If set to true, far points (<1m) are interpolated with samples at "minDistSqrBetweenLaserPoints" inte...
float horizontalTolerance
The tolerance in rads in pitch & roll for a laser scan to be considered horizontal,...
void readFromStream(mrpt::utils::CStream &in)
Binary dump to stream - for usage in derived classes' serialization.
bool addToExistingPointsMap
Applicable to "loadFromRangeScan" only! If set to false, the points from the scan are loaded,...
Helper struct used for internal_loadFromRangeScan2D_prepareOneRange()
mrpt::math::CMatrixDouble44 HM
Homog matrix of the local sensor pose within the robot.
TLaserRange2DInsertContext(const mrpt::obs::CObservation2DRangeScan &_rangeScan)
const mrpt::obs::CObservation2DRangeScan & rangeScan
std::vector< float > fVars
Extra variables to be used as desired by the derived class.
Helper struct used for internal_loadFromRangeScan3D_prepareOneRange()
mrpt::math::CMatrixDouble44 HM
Homog matrix of the local sensor pose within the robot.
TLaserRange3DInsertContext(const mrpt::obs::CObservation3DRangeScan &_rangeScan)
std::vector< float > fVars
Extra variables to be used as desired by the derived class.
const mrpt::obs::CObservation3DRangeScan & rangeScan
Options used when evaluating "computeObservationLikelihood" in the derived classes.
double sigma_dist
Sigma (standard deviation, in meters) of the exponential used to model the likelihood (default= 0....
uint32_t decimation
Speed up the likelihood computation by considering only one out of N rays (default=10)
double max_corr_distance
Maximum distance in meters to consider for the numerator divided by "sigma_dist", so that each point ...
void loadFromConfigFile(const mrpt::utils::CConfigFileBase &source, const std::string &section) MRPT_OVERRIDE
This method load the options from a ".ini"-like file or memory-stored string list.
void writeToStream(mrpt::utils::CStream &out) const
Binary dump to stream - for usage in derived classes' serialization.
void readFromStream(mrpt::utils::CStream &in)
Binary dump to stream - for usage in derived classes' serialization.
TLikelihoodOptions()
Initilization of default parameters.
void dumpToTextStream(mrpt::utils::CStream &out) const MRPT_OVERRIDE
This method should clearly display all the contents of the structure in textual form,...
Additional results from the determination of matchings between point clouds, etc.,...
Parameters for the determination of matchings between point clouds, etc.
Parameters for CMetricMap::compute3DMatchingRatio()
Lightweight 2D point.
double y
X,Y coordinates.
Lightweight 3D point.
double z
X,Y,Z coordinates.
Lightweight 3D point (float version).
A RGB color - floats in the range [0,1].
Definition TColor.h:53



Page generated by Doxygen 1.9.8 for MRPT 1.4.0 SVN: at Thu Dec 14 16:54:58 UTC 2023