00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028 #ifndef CPOINTSMAP_H
00029 #define CPOINTSMAP_H
00030
00031 #include <mrpt/slam/CMetricMap.h>
00032 #include <mrpt/utils/CSerializable.h>
00033 #include <mrpt/math/CMatrix.h>
00034 #include <mrpt/utils/CLoadableOptions.h>
00035 #include <mrpt/utils/safe_pointers.h>
00036
00037 #include <mrpt/poses/CPoint2D.h>
00038
00039 #include <mrpt/otherlibs/ann/ANN.h>
00040
00041
00042 namespace mrpt
00043 {
00044 namespace slam
00045 {
00046 using namespace mrpt::poses;
00047
00048 class CObservation2DRangeScan;
00049 class CSimplePointsMap;
00050 class CMultiMetricMap;
00051 class CColouredPointsMap;
00052 class COccupancyGridMap2D;
00053
00057 class MRPTDLLIMPEXP CPointsMap : public CMetricMap
00058 {
00059 friend class CMultiMetricMap;
00060 friend class CMultiMetricMapPDF;
00061 friend class CSimplePointsMap;
00062 friend class CColouredPointsMap;
00063 friend class COccupancyGridMap2D;
00064
00065
00066 DEFINE_VIRTUAL_SERIALIZABLE( CPointsMap )
00067
00068 protected:
00072 utils::safe_ptr<CMultiMetricMap> m_parent;
00073
00076 std::vector<float> x,y,z;
00077
00080 std::vector<uint32_t> pointWeight;
00081
00085 mutable float m_largestDistanceFromOrigin;
00086
00090 mutable bool m_largestDistanceFromOriginIsUpdated;
00091
00094 mutable bool m_KDTreeDataIsUpdated;
00095
00096
00099 struct MRPTDLLIMPEXP TKDTreeData
00100 {
00103 TKDTreeData();
00104
00107 TKDTreeData(const TKDTreeData &o);
00108
00111 TKDTreeData& operator =(const TKDTreeData &o);
00112
00115 ~TKDTreeData();
00116
00119 void clear();
00120
00121 ANNkd_tree *m_pDataTree;
00122 ANNpointArray m_DataPoints;
00123 ANNdist m_NearNeighbourDistances[10];
00124 ANNidx m_NearNeighbourIndices[10];
00125 ANNpoint m_QueryPoint;
00126 size_t m_nTreeSize;
00127 size_t m_nDim;
00128 size_t m_nk;
00129 };
00130
00131 mutable TKDTreeData KDTreeData;
00132
00135 void build_kdTree2D() const;
00136
00139 void build_kdTree3D() const;
00140
00141 public:
00144 CPointsMap();
00145
00148 virtual ~CPointsMap();
00149
00150
00166 size_t kdTreeClosestPoint2D(
00167 const float &x0,
00168 const float &y0,
00169 float &out_x,
00170 float &out_y,
00171 float &out_dist_sqr
00172 ) const;
00173
00176 float kdTreeClosestPoint2DsqrError(
00177 const float &x0,
00178 const float &y0 ) const;
00179
00182 virtual float squareDistanceToClosestCorrespondence(
00183 const float &x0,
00184 const float &y0 ) const;
00185
00186
00204 void kdTreeTwoClosestPoint2D(
00205 const float &x0,
00206 const float &y0,
00207 float &out_x1,
00208 float &out_y1,
00209 float &out_x2,
00210 float &out_y2,
00211 float &out_dist_sqr1,
00212 float &out_dist_sqr2 ) const;
00213
00214
00232 size_t kdTreeClosestPoint3D(
00233 const float &x0,
00234 const float &y0,
00235 const float &z0,
00236 float &out_x,
00237 float &out_y,
00238 float &out_z,
00239 float &out_dist_sqr
00240 ) const;
00241
00242
00246 struct MRPTDLLIMPEXP TInsertionOptions : public utils::CLoadableOptions
00247 {
00250 TInsertionOptions( );
00251 virtual ~TInsertionOptions() {}
00252
00255 void loadFromConfigFile(
00256 const mrpt::utils::CConfigFileBase &source,
00257 const std::string §ion);
00258
00261 void dumpToTextStream(utils::CStream &out);
00262
00263
00266 float minDistBetweenLaserPoints;
00267
00270 bool addToExistingPointsMap;
00271
00274 bool also_interpolate;
00275
00278 bool disableDeletion;
00279
00282 bool fuseWithExisting;
00283
00287 bool isPlanarMap;
00288
00291 float horizontalTolerance;
00292
00295 bool matchStaticPointsOnly;
00296
00299 float maxDistForInterpolatePoints;
00300
00301 } insertionOptions;
00302
00305 virtual void copyFrom(const CPointsMap &obj) = 0;
00306
00316 virtual void fuseWith(
00317 CPointsMap *otherMap,
00318 float minDistForFuse = 0.02f,
00319 std::vector<bool> *notFusedPoints = NULL) = 0;
00320
00330 virtual void loadFromRangeScan(
00331 const CObservation2DRangeScan &rangeScan,
00332 const CPose3D *robotPose = NULL ) = 0;
00333
00337 virtual bool load2D_from_text_file(std::string file) = 0;
00338
00342 virtual bool load3D_from_text_file(std::string file) = 0;
00343
00347 bool save2D_to_text_file(const std::string &file) const;
00348
00352 bool save3D_to_text_file(const std::string &file)const;
00353
00356 void saveMetricMapRepresentationToFile(
00357 const std::string &filNamePrefix
00358 )const
00359 {
00360 std::string fil( filNamePrefix + std::string(".txt") );
00361 save3D_to_text_file( fil );
00362 }
00363
00364
00367 virtual void clear() = 0;
00368
00371 size_t size() const;
00372
00375 size_t getPointsCount() const;
00376
00381 unsigned long getPoint(const size_t &index,CPoint2D &p) const;
00382
00387 unsigned long getPoint(const size_t &index,CPoint3D &p) const;
00388
00393 unsigned long getPoint(const size_t &index,float &x,float &y) const;
00394
00399 unsigned long getPoint(const size_t &index,float &x,float &y,float &z) const;
00400
00404 virtual void setPoint(const size_t &index,CPoint2D &p)=0;
00405
00409 virtual void setPoint(const size_t &index,CPoint3D &p)=0;
00410
00414 virtual void setPoint(const size_t &index,float x, float y)=0;
00415
00419 virtual void setPoint(const size_t &index,float x, float y, float z)=0;
00420
00423 void getPointsBuffer( size_t &outPointsCount, const float *&xs, const float *&ys, const float *&zs ) const;
00424
00428 void getAllPoints( std::vector<float> &xs, std::vector<float> &ys,std::vector<float> &zs, size_t decimation = 1 ) const;
00429
00433 void getAllPoints( std::vector<float> &xs, std::vector<float> &ys, size_t decimation = 1 ) const;
00434
00437 virtual void insertPoint( float x, float y, float z = 0 ) = 0;
00438
00441 virtual void insertPoint( CPoint3D p ) = 0;
00442
00447 virtual void reserve(size_t newLength) = 0;
00448
00451 void clipOutOfRangeInZ(float zMin, float zMax);
00452
00455 void clipOutOfRange(CPoint2D &point, float maxRange);
00456
00461 virtual void applyDeletionMask( std::vector<bool> &mask ) = 0;
00462
00482 void computeMatchingWith2D(
00483 const CMetricMap *otherMap,
00484 const CPose2D &otherMapPose,
00485 float maxDistForCorrespondence,
00486 float maxAngularDistForCorrespondence,
00487 const CPose2D &angularDistPivotPoint,
00488 TMatchingPairList &correspondences,
00489 float &correspondencesRatio,
00490 float *sumSqrDist = NULL,
00491 bool onlyKeepTheClosest = false,
00492 bool onlyUniqueRobust = false ) const;
00493
00496 void changeCoordinatesReference(const CPose2D &b);
00497
00500 void changeCoordinatesReference(const CPose3D &b);
00501
00504 void changeCoordinatesReference(const CPointsMap &other, const CPose3D &b);
00505
00508 virtual bool isEmpty() const;
00509
00513 virtual void getAs3DObject ( mrpt::opengl::CSetOfObjectsPtr &outObj ) const;
00514
00515
00526 float compute3DMatchingRatio(
00527 const CMetricMap *otherMap,
00528 const CPose3D &otherMapPose,
00529 float minDistForCorr = 0.10f,
00530 float minMahaDistForCorr = 2.0f
00531 ) const;
00532
00535 float getLargestDistanceFromOrigin() const;
00536
00538 void boundingBox( float &min_x,float &max_x,float &min_y,float &max_y,float &min_z,float &max_z ) const;
00539
00540
00542 static float COLOR_3DSCENE_R;
00543 static float COLOR_3DSCENE_G;
00544 static float COLOR_3DSCENE_B;
00545
00546 };
00547
00548 DEFINE_SERIALIZABLE_PRE_CUSTOM_BASE( CPointsMap , CMetricMap )
00549
00550 }
00551 }
00552
00553 #endif