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
00058 class MRPTDLLIMPEXP CPointsMap : public CMetricMap
00059 {
00060 friend class CMultiMetricMap;
00061 friend class CMultiMetricMapPDF;
00062 friend class CSimplePointsMap;
00063 friend class CColouredPointsMap;
00064 friend class COccupancyGridMap2D;
00065
00066
00067 DEFINE_VIRTUAL_SERIALIZABLE( CPointsMap )
00068
00069 protected:
00073 utils::safe_ptr<CMultiMetricMap> m_parent;
00074
00077 std::vector<float> x,y,z;
00078
00081 std::vector<uint32_t> pointWeight;
00082
00086 mutable float m_largestDistanceFromOrigin;
00087
00091 mutable bool m_largestDistanceFromOriginIsUpdated;
00092
00095 mutable bool m_KDTreeDataIsUpdated;
00096
00097
00100 struct MRPTDLLIMPEXP TKDTreeData
00101 {
00104 TKDTreeData();
00105
00108 TKDTreeData(const TKDTreeData &o);
00109
00112 TKDTreeData& operator =(const TKDTreeData &o);
00113
00116 ~TKDTreeData();
00117
00120 void clear();
00121
00122 ANNkd_tree *m_pDataTree;
00123 ANNpointArray m_DataPoints;
00124 ANNdist m_NearNeighbourDistances[10];
00125 ANNidx m_NearNeighbourIndices[10];
00126 ANNpoint m_QueryPoint;
00127 size_t m_nTreeSize;
00128 size_t m_nDim;
00129 size_t m_nk;
00130 };
00131
00132 mutable TKDTreeData KDTreeData;
00133
00136 void build_kdTree2D() const;
00137
00140 void build_kdTree3D() const;
00141
00142 public:
00145 CPointsMap();
00146
00149 virtual ~CPointsMap();
00150
00151
00167 size_t kdTreeClosestPoint2D(
00168 const float &x0,
00169 const float &y0,
00170 float &out_x,
00171 float &out_y,
00172 float &out_dist_sqr
00173 ) const;
00174
00177 float kdTreeClosestPoint2DsqrError(
00178 const float &x0,
00179 const float &y0 ) const;
00180
00183 virtual float squareDistanceToClosestCorrespondence(
00184 const float &x0,
00185 const float &y0 ) const;
00186
00187
00205 void kdTreeTwoClosestPoint2D(
00206 const float &x0,
00207 const float &y0,
00208 float &out_x1,
00209 float &out_y1,
00210 float &out_x2,
00211 float &out_y2,
00212 float &out_dist_sqr1,
00213 float &out_dist_sqr2 ) const;
00214
00231 void kdTreeNClosestPoint2D(
00232 const float &x0,
00233 const float &y0,
00234 const unsigned int &N,
00235 std::vector<float> &out_x,
00236 std::vector<float> &out_y,
00237 std::vector<float> &out_dist_sqr ) const;
00238
00253 void kdTreeNClosestPoint2DIdx(
00254 const float &x0,
00255 const float &y0,
00256 const unsigned int &N,
00257 std::vector<int> &out_idx,
00258 std::vector<float> &out_dist_sqr ) const;
00259
00277 size_t kdTreeClosestPoint3D(
00278 const float &x0,
00279 const float &y0,
00280 const float &z0,
00281 float &out_x,
00282 float &out_y,
00283 float &out_z,
00284 float &out_dist_sqr
00285 ) const;
00286
00304 void kdTreeNClosestPoint3D(
00305 const float &x0,
00306 const float &y0,
00307 const float &z0,
00308 const unsigned int &N,
00309 std::vector<float> &out_x,
00310 std::vector<float> &out_y,
00311 std::vector<float> &out_z,
00312 std::vector<float> &out_dist_sqr ) const;
00313
00329 void kdTreeNClosestPoint3DIdx(
00330 const float &x0,
00331 const float &y0,
00332 const float &z0,
00333 const unsigned int &N,
00334 std::vector<int> &out_idx,
00335 std::vector<float> &out_dist_sqr ) const;
00336
00340 struct MRPTDLLIMPEXP TInsertionOptions : public utils::CLoadableOptions
00341 {
00344 TInsertionOptions( );
00345 virtual ~TInsertionOptions() {}
00346
00349 void loadFromConfigFile(
00350 const mrpt::utils::CConfigFileBase &source,
00351 const std::string §ion);
00352
00355 void dumpToTextStream(utils::CStream &out);
00356
00357
00360 float minDistBetweenLaserPoints;
00361
00364 bool addToExistingPointsMap;
00365
00368 bool also_interpolate;
00369
00372 bool disableDeletion;
00373
00376 bool fuseWithExisting;
00377
00381 bool isPlanarMap;
00382
00385 float horizontalTolerance;
00386
00389 bool matchStaticPointsOnly;
00390
00393 float maxDistForInterpolatePoints;
00394
00395 } insertionOptions;
00396
00399 virtual void copyFrom(const CPointsMap &obj) = 0;
00400
00410 virtual void fuseWith(
00411 CPointsMap *otherMap,
00412 float minDistForFuse = 0.02f,
00413 std::vector<bool> *notFusedPoints = NULL) = 0;
00414
00424 virtual void loadFromRangeScan(
00425 const CObservation2DRangeScan &rangeScan,
00426 const CPose3D *robotPose = NULL ) = 0;
00427
00431 virtual bool load2D_from_text_file(std::string file) = 0;
00432
00436 virtual bool load3D_from_text_file(std::string file) = 0;
00437
00441 bool save2D_to_text_file(const std::string &file) const;
00442
00446 bool save3D_to_text_file(const std::string &file)const;
00447
00450 void saveMetricMapRepresentationToFile(
00451 const std::string &filNamePrefix
00452 )const
00453 {
00454 std::string fil( filNamePrefix + std::string(".txt") );
00455 save3D_to_text_file( fil );
00456 }
00457
00458
00461 virtual void clear() = 0;
00462
00465 size_t size() const;
00466
00469 size_t getPointsCount() const;
00470
00475 unsigned long getPoint(const size_t &index,CPoint2D &p) const;
00476
00481 unsigned long getPoint(const size_t &index,CPoint3D &p) const;
00482
00487 unsigned long getPoint(const size_t &index,float &x,float &y) const;
00488
00493 unsigned long getPoint(const size_t &index,float &x,float &y,float &z) const;
00494
00498 virtual void setPoint(const size_t &index,CPoint2D &p)=0;
00499
00503 virtual void setPoint(const size_t &index,CPoint3D &p)=0;
00504
00508 virtual void setPoint(const size_t &index,float x, float y)=0;
00509
00513 virtual void setPoint(const size_t &index,float x, float y, float z)=0;
00514
00517 void getPointsBuffer( size_t &outPointsCount, const float *&xs, const float *&ys, const float *&zs ) const;
00518
00522 void getAllPoints( std::vector<float> &xs, std::vector<float> &ys,std::vector<float> &zs, size_t decimation = 1 ) const;
00523
00527 void getAllPoints( std::vector<float> &xs, std::vector<float> &ys, size_t decimation = 1 ) const;
00528
00531 virtual void insertPoint( float x, float y, float z = 0 ) = 0;
00532
00535 virtual void insertPoint( CPoint3D p ) = 0;
00536
00541 virtual void reserve(size_t newLength) = 0;
00542
00545 void clipOutOfRangeInZ(float zMin, float zMax);
00546
00549 void clipOutOfRange(CPoint2D &point, float maxRange);
00550
00555 virtual void applyDeletionMask( std::vector<bool> &mask ) = 0;
00556
00576 void computeMatchingWith2D(
00577 const CMetricMap *otherMap,
00578 const CPose2D &otherMapPose,
00579 float maxDistForCorrespondence,
00580 float maxAngularDistForCorrespondence,
00581 const CPose2D &angularDistPivotPoint,
00582 TMatchingPairList &correspondences,
00583 float &correspondencesRatio,
00584 float *sumSqrDist = NULL,
00585 bool onlyKeepTheClosest = false,
00586 bool onlyUniqueRobust = false ) const;
00587
00605 void computeMatchingWith3D(
00606 const CMetricMap *otherMap,
00607 const CPose3D &otherMapPose,
00608 float maxDistForCorrespondence,
00609 float maxAngularDistForCorrespondence,
00610 const CPoint3D &angularDistPivotPoint,
00611 TMatchingPairList &correspondences,
00612 float &correspondencesRatio,
00613 float *sumSqrDist = NULL,
00614 bool onlyKeepTheClosest = true,
00615 bool onlyUniqueRobust = false ) const;
00616
00617
00620 void changeCoordinatesReference(const CPose2D &b);
00621
00624 void changeCoordinatesReference(const CPose3D &b);
00625
00628 void changeCoordinatesReference(const CPointsMap &other, const CPose3D &b);
00629
00632 virtual bool isEmpty() const;
00633
00637 virtual void getAs3DObject ( mrpt::opengl::CSetOfObjectsPtr &outObj ) const;
00638
00639
00650 float compute3DMatchingRatio(
00651 const CMetricMap *otherMap,
00652 const CPose3D &otherMapPose,
00653 float minDistForCorr = 0.10f,
00654 float minMahaDistForCorr = 2.0f
00655 ) const;
00656
00659 float getLargestDistanceFromOrigin() const;
00660
00662 void boundingBox( float &min_x,float &max_x,float &min_y,float &max_y,float &min_z,float &max_z ) const;
00663
00664
00666 static float COLOR_3DSCENE_R;
00667 static float COLOR_3DSCENE_G;
00668 static float COLOR_3DSCENE_B;
00669
00670 };
00671
00672 DEFINE_SERIALIZABLE_PRE_CUSTOM_BASE( CPointsMap , CMetricMap )
00673
00674 }
00675 }
00676
00677 #endif