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 CGridMapAligner_H
00029 #define CGridMapAligner_H
00030
00031 #include <mrpt/slam/CMetricMapsAlignmentAlgorithm.h>
00032 #include <mrpt/slam/CLandmarksMap.h>
00033 #include <mrpt/utils/CLoadableOptions.h>
00034 #include <mrpt/poses/CPosePDFSOG.h>
00035
00036 namespace mrpt
00037 {
00038 namespace slam
00039 {
00040 using namespace poses;
00041 using namespace utils;
00042
00043
00044
00054 class MRPTDLLIMPEXP CGridMapAligner : public CMetricMapsAlignmentAlgorithm
00055 {
00056 private:
00059 CPosePDFPtr AlignPDF_correlation(
00060 const CMetricMap *m1,
00061 const CMetricMap *m2,
00062 const CPosePDFGaussian &initialEstimationPDF,
00063 float *runningTime = NULL,
00064 void *info = NULL );
00065
00068 CPosePDFPtr AlignPDF_robustMatch(
00069 const CMetricMap *m1,
00070 const CMetricMap *m2,
00071 const CPosePDFGaussian &initialEstimationPDF,
00072 float *runningTime = NULL,
00073 void *info = NULL );
00074
00075 public:
00076
00077 CGridMapAligner() :
00078 options()
00079 {}
00080
00083 enum TAlignerMethod
00084 {
00085 amRobustMatch = 0,
00086 amCorrelation
00087 };
00088
00091 class MRPTDLLIMPEXP TConfigParams : public utils::CLoadableOptions
00092 {
00093 public:
00096 TConfigParams();
00097
00100 void loadFromConfigFile(
00101 const mrpt::utils::CConfigFileBase &source,
00102 const std::string §ion);
00103
00106 void dumpToTextStream(
00107 CStream &out);
00108
00109
00112 TAlignerMethod methodSelection;
00113
00117 unsigned int ransac_minSetSize,ransac_maxSetSize,ransac_nSimulations;
00118
00121 float ransac_SOG_sigma_m;
00122
00126 float ransac_mahalanobisDistanceThreshold;
00127
00130 float featsPerSquareMeter;
00131
00134 unsigned int feats_DirectionSectors;
00135
00138 unsigned int feats_RangeSectors;
00139
00142 float feats_startDist;
00143
00146 float feats_endDist;
00147
00150 float thresholdStdTimes;
00151
00153 double maxKLd_for_merge;
00154
00155 bool save_feat_coors;
00156 bool debug_show_corrs;
00157
00158 } options;
00159
00162 struct MRPTDLLIMPEXP TReturnInfo
00163 {
00166 TReturnInfo() :
00167 cbSize(sizeof(TReturnInfo)),
00168 goodness(0),
00169 noRobustEstimation()
00170 {
00171 }
00172
00173 size_t cbSize;
00174
00177 float goodness;
00178
00181 CPose2D noRobustEstimation;
00182
00191 CPosePDFSOGPtr sog1,sog2,sog3;
00192
00194 CLandmarksMapPtr landmarks_map1, landmarks_map2;
00195
00197 CMetricMap::TMatchingPairList correspondences;
00198
00199 struct TPairPlusDistance
00200 {
00201 TPairPlusDistance(size_t i1, size_t i2, float d) :
00202 idx_this(i1), idx_other(i2), dist(d)
00203 { }
00204 size_t idx_this,idx_other;
00205 float dist;
00206 };
00207
00208 std::vector<TPairPlusDistance> correspondences_dists_maha;
00209 };
00210
00233 CPosePDFPtr AlignPDF(
00234 const CMetricMap *m1,
00235 const CMetricMap *m2,
00236 const CPosePDFGaussian &initialEstimationPDF,
00237 float *runningTime = NULL,
00238
00239 void *info = NULL );
00242 static void landmarksMatchingCorrelation(
00243 mrpt::slam::CLandmark *lm1,
00244 mrpt::slam::CLandmark *lm2,
00245 float &minDistance,
00246 float &minDistAngle);
00247
00248 };
00249
00250 }
00251 }
00252
00253 #endif