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 CMetricMap_H
00029 #define CMetricMap_H
00030
00031 #include <mrpt/utils/CSerializable.h>
00032 #include <mrpt/opengl/CSetOfObjects.h>
00033
00034 #include <mrpt/slam/CObservation.h>
00035
00036 #include <mrpt/poses/CPoint2D.h>
00037 #include <mrpt/poses/CPoint3D.h>
00038 #include <mrpt/poses/CPose2D.h>
00039 #include <mrpt/poses/CPose3D.h>
00040
00041
00042 namespace mrpt
00043 {
00044 namespace slam
00045 {
00046 using namespace mrpt::utils;
00047
00048 class CObservation;
00049 class CPointsMap;
00050 class CSensFrameProbSequence;
00051 class CSensoryFrame;
00052
00064 class MRPTDLLIMPEXP CMetricMap : public mrpt::utils::CSerializable
00065 {
00066
00067 DEFINE_VIRTUAL_SERIALIZABLE( CMetricMap )
00068
00069 public:
00072 virtual void clear() = 0;
00073
00076 virtual bool isEmpty() const = 0;
00077
00085 void loadFromProbabilisticPosesAndObservations( CSensFrameProbSequence &sfSeq );
00086
00094 virtual bool insertObservation(
00095 const CObservation *obs,
00096 const CPose3D *robotPose = NULL ) = 0;
00097
00099 bool insertObservationPtr(
00100 const CObservationPtr &obs,
00101 const CPose3D *robotPose = NULL )
00102 {
00103 MRPT_TRY_START;
00104 if (!obs.present()) { THROW_EXCEPTION("Trying to pass a null pointer."); }
00105 return insertObservation(obs.pointer(),robotPose);
00106 MRPT_TRY_END;
00107 }
00108
00117 virtual double computeObservationLikelihood( const CObservation *obs, const CPose3D &takenFrom ) = 0;
00118
00127 double computeObservationLikelihood( const CObservation *obs, const CPose2D &takenFrom )
00128 {
00129 return computeObservationLikelihood(obs,CPose3D(takenFrom));
00130 }
00131
00136 virtual bool canComputeObservationLikelihood( const CObservation *obs )
00137 {
00138 return true;
00139 }
00140
00148 double computeObservationsLikelihood( const CSensoryFrame &sf, const CPose2D &takenFrom );
00149
00154 bool canComputeObservationsLikelihood( const CSensoryFrame &sf );
00155
00158 CMetricMap();
00159
00162 virtual ~CMetricMap();
00163
00169 void alignBylikelihoodHillClimbing( CObservation *obs, CPose2D in_initialEstimatedPose, CPose2D &out_ResultingPose );
00170
00174 struct MRPTDLLIMPEXP TMatchingPair
00175 {
00176 TMatchingPair() :
00177 this_idx(0), other_idx(0),
00178 this_x(0),this_y(0),this_z(0),
00179 other_x(0),other_y(0),other_z(0),
00180 errorSquareAfterTransformation(0)
00181 {
00182 }
00183
00184 TMatchingPair( unsigned int _this_idx,unsigned int _other_idx, float _this_x, float _this_y,float _this_z, float _other_x,float _other_y,float _other_z ) :
00185 this_idx(_this_idx), other_idx(_other_idx),
00186 this_x(_this_x),this_y(_this_y),this_z(_this_z),
00187 other_x(_other_x),other_y(_other_y),other_z(_other_z),
00188 errorSquareAfterTransformation(0)
00189 {
00190 }
00191
00192 unsigned int this_idx;
00193 unsigned int other_idx;
00194 float this_x,this_y,this_z;
00195 float other_x,other_y,other_z;
00196 float errorSquareAfterTransformation;
00197
00198 };
00199
00200 typedef TMatchingPair* TMatchingPairPtr;
00201
00204 class TMatchingPairList
00205 {
00206 protected:
00207 std::deque<TMatchingPair> m_matchs;
00208
00209 public:
00210 TMatchingPairList() : m_matchs() { }
00211
00212 typedef std::deque<TMatchingPair>::iterator iterator;
00213 typedef std::deque<TMatchingPair>::const_iterator const_iterator;
00214
00215 iterator begin() { return m_matchs.begin(); }
00216 const_iterator begin() const { return m_matchs.begin(); }
00217
00218 iterator end() { return m_matchs.end(); }
00219 const_iterator end() const { return m_matchs.end(); }
00220
00221 void clear() { m_matchs.clear(); }
00222
00223 size_t size() const { return m_matchs.size(); }
00224
00225 void resize(const size_t &i) { m_matchs.resize(i); }
00226
00227 void push_back( const TMatchingPair&d ) { m_matchs.push_back(d); }
00228
00229 iterator erase(const iterator &i) { return m_matchs.erase(i); }
00230
00231
00232 TMatchingPair & operator[](const size_t &i) { return m_matchs[i]; }
00233 const TMatchingPair & operator[](const size_t &i) const { return m_matchs[i]; }
00234
00235
00236
00237
00240 bool indexOtherMapHasCorrespondence(unsigned int idx);
00241
00244 void dumpToFile(const char *fileName);
00245
00248 void saveAsMATLABScript( const std::string &filName );
00249
00255 float overallSquareError( const CPose2D &q ) const;
00256
00262 float overallSquareErrorAndPoints(
00263 const CPose2D &q,
00264 vector_float &xs,
00265 vector_float &ys ) const;
00266
00267
00273 void squareErrorVector(const CPose2D &q, vector_float &out_sqErrs ) const;
00274
00280 void squareErrorVector(
00281 const CPose2D &q,
00282 vector_float &out_sqErrs,
00283 vector_float &xs,
00284 vector_float &ys ) const;
00285
00286 };
00287
00306 virtual void computeMatchingWith2D(
00307 const CMetricMap *otherMap,
00308 const CPose2D &otherMapPose,
00309 float maxDistForCorrespondence,
00310 float maxAngularDistForCorrespondence,
00311 const CPose2D &angularDistPivotPoint,
00312 TMatchingPairList &correspondences,
00313 float &correspondencesRatio,
00314 float *sumSqrDist = NULL,
00315 bool onlyKeepTheClosest = true,
00316 bool onlyUniqueRobust = false ) const
00317 {
00318 MRPT_TRY_START
00319 THROW_EXCEPTION("Virtual method not implemented in derived class.")
00320 MRPT_TRY_END
00321 }
00322
00340 virtual void computeMatchingWith3D(
00341 const CMetricMap *otherMap,
00342 const CPose3D &otherMapPose,
00343 float maxDistForCorrespondence,
00344 float maxAngularDistForCorrespondence,
00345 const CPoint3D &angularDistPivotPoint,
00346 TMatchingPairList &correspondences,
00347 float &correspondencesRatio,
00348 float *sumSqrDist = NULL,
00349 bool onlyKeepTheClosest = true,
00350 bool onlyUniqueRobust = false ) const
00351 {
00352 MRPT_TRY_START
00353 THROW_EXCEPTION("Virtual method not implemented in derived class.")
00354 MRPT_TRY_END
00355 }
00356
00357
00368 virtual float compute3DMatchingRatio(
00369 const CMetricMap *otherMap,
00370 const CPose3D &otherMapPose,
00371 float minDistForCorr = 0.10f,
00372 float minMahaDistForCorr = 2.0f
00373 ) const = 0;
00374
00377 virtual void saveMetricMapRepresentationToFile(
00378 const std::string &filNamePrefix
00379 ) const = 0;
00380
00383 virtual void getAs3DObject( mrpt::opengl::CSetOfObjectsPtr &outObj ) const = 0;
00384
00387 bool m_disableSaveAs3DObject;
00388
00392 virtual void auxParticleFilterCleanUp() = 0;
00393
00394
00397 virtual float squareDistanceToClosestCorrespondence(
00398 const float &x0,
00399 const float &y0 ) const
00400 {
00401 MRPT_TRY_START
00402 THROW_EXCEPTION("Virtual method not implemented in derived class.")
00403 MRPT_TRY_END
00404 }
00405
00406
00407 };
00408
00409 DEFINE_SERIALIZABLE_PRE( CMetricMap )
00410
00411
00413 typedef std::deque<CMetricMap*> TMetricMapList;
00414
00417 bool operator < (const mrpt::slam::CMetricMap::TMatchingPair& a, const mrpt::slam::CMetricMap::TMatchingPair& b);
00418
00421 bool operator == (const mrpt::slam::CMetricMap::TMatchingPair& a,const mrpt::slam::CMetricMap::TMatchingPair& b);
00422
00425 bool operator == (const mrpt::slam::CMetricMap::TMatchingPairList& a,const mrpt::slam::CMetricMap::TMatchingPairList& b);
00426
00427 }
00428 }
00429
00430 #endif