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
00233 protected:
00238 float kernel(const float &x2, const float &rho2);
00239
00242 CPosePDFPtr ICP_Method_Classic(
00243 const CMetricMap *m1,
00244 const CMetricMap *m2,
00245 const CPosePDFGaussian &initialEstimationPDF,
00246 TReturnInfo &outInfo );
00247
00250 CPosePDFPtr ICP_Method_LM(
00251 const CMetricMap *m1,
00252 const CMetricMap *m2,
00253 const CPosePDFGaussian &initialEstimationPDF,
00254 TReturnInfo &outInfo );
00255
00258 CPosePDFPtr ICP_Method_IKF(
00259 const CMetricMap *m1,
00260 const CMetricMap *m2,
00261 const CPosePDFGaussian &initialEstimationPDF,
00262 TReturnInfo &outInfo );
00263
00264 };
00265
00266 }
00267 }
00268
00269 #endif