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 CICP_H
00029 #define CICP_H
00030
00031 #include <mrpt/slam/CMetricMapsAlignmentAlgorithm.h>
00032 #include <mrpt/utils/CLoadableOptions.h>
00033
00034 namespace mrpt
00035 {
00036 namespace poses
00037 {
00038 class CPosePDFParticles;
00039 }
00040
00041 namespace slam
00042 {
00043 using namespace poses;
00044
00048 enum TICPAlgorithm
00049 {
00050 icpClassic = 0,
00051 icpLevenbergMarquardt,
00052 icpIKF
00053 };
00054
00066 class MRPTDLLIMPEXP CICP : public CMetricMapsAlignmentAlgorithm
00067 {
00068 public:
00069 CICP() : options()
00070 {}
00071
00072
00074 virtual ~CICP() { }
00075
00078 class MRPTDLLIMPEXP TConfigParams : public utils::CLoadableOptions
00079 {
00080 public:
00083 TConfigParams();
00084
00087 void loadFromConfigFile(
00088 const mrpt::utils::CConfigFileBase &source,
00089 const std::string §ion);
00090
00093 void dumpToTextStream(utils::CStream &out);
00094
00095
00099 TICPAlgorithm ICP_algorithm;
00100
00103 bool onlyClosestCorrespondences;
00104
00107 bool onlyUniqueRobust;
00108
00111 unsigned int maxIterations;
00112
00115 float thresholdDist,thresholdAng;
00116
00119 float ALFA;
00120
00123 float smallestThresholdDist;
00124
00129 float covariance_varPoints;
00130
00133 bool doRANSAC;
00134
00138 unsigned int ransac_minSetSize,ransac_maxSetSize,ransac_nSimulations;
00139
00143 float ransac_mahalanobisDistanceThreshold;
00144
00148 float normalizationStd;
00149
00153 bool ransac_fuseByCorrsMatch;
00154
00158 float ransac_fuseMaxDiffXY, ransac_fuseMaxDiffPhi;
00159
00162 float kernel_rho;
00163
00166 bool use_kernel;
00167
00170 float Axy_aprox_derivatives;
00171
00174 float LM_initial_lambda;
00175
00176 } options;
00177
00180 struct MRPTDLLIMPEXP TReturnInfo
00181 {
00182 TReturnInfo() :
00183 cbSize(sizeof(TReturnInfo)),
00184 nIterations(0),
00185 goodness(0),
00186 quality(0)
00187 {
00188 }
00189
00191 unsigned int cbSize;
00192
00195 unsigned short nIterations;
00196
00199 float goodness;
00200
00204 float quality;
00205 };
00206
00226 CPosePDFPtr AlignPDF(
00227 const CMetricMap *m1,
00228 const CMetricMap *m2,
00229 const CPosePDFGaussian &initialEstimationPDF,
00230 float *runningTime = NULL,
00231 void *info = NULL );
00232
00251 CPose3DPDFPtr Align3DPDF(
00252 const CMetricMap *m1,
00253 const CMetricMap *m2,
00254 const CPose3DPDFGaussian &initialEstimationPDF,
00255 float *runningTime = NULL,
00256 void *info = NULL );
00257
00258
00259 protected:
00264 float kernel(const float &x2, const float &rho2);
00265
00268 CPosePDFPtr ICP_Method_Classic(
00269 const CMetricMap *m1,
00270 const CMetricMap *m2,
00271 const CPosePDFGaussian &initialEstimationPDF,
00272 TReturnInfo &outInfo );
00273
00276 CPosePDFPtr ICP_Method_LM(
00277 const CMetricMap *m1,
00278 const CMetricMap *m2,
00279 const CPosePDFGaussian &initialEstimationPDF,
00280 TReturnInfo &outInfo );
00281
00284 CPosePDFPtr ICP_Method_IKF(
00285 const CMetricMap *m1,
00286 const CMetricMap *m2,
00287 const CPosePDFGaussian &initialEstimationPDF,
00288 TReturnInfo &outInfo );
00289
00292 CPose3DPDFPtr ICP3D_Method_Classic(
00293 const CMetricMap *m1,
00294 const CMetricMap *m2,
00295 const CPose3DPDFGaussian &initialEstimationPDF,
00296 TReturnInfo &outInfo );
00297
00298
00299 };
00300
00301 }
00302 }
00303
00304 #endif