ICP算法、ICP算法概述Algorithm1Suinmarj,ofICPalgorithm.Require:KVi>readingRequire:>referen.eeRequire:忌沈>initialtransformation■da.tafiltcr(AP)>datafilters^'Q!■datafiltcrf^Q)r>datafiltersi_(丁JTinitrepeatomovereading>associat巳pointsWi<—outlier(Alj>filteroutlierst+^TJargmin(error(7(巧©))untilconvergeri(:eEnsure:肘=(»—巧》、转换矩阵T"-[0T1其中，R是旋转矩阵，t是转换向量。三、滤波器(DataFilters)滤波器的目的是为了增加差异性，减少处理时间，去噪。四、匹配度(matchfunction)匹配度，是测量两个点集的关联度的，又称之为associationsolver。比如，用欧式距离来测量两个点集的关联度，定义如下：”VI丄matclifP,Q)_{(皿qm):eP.qm—argmin(d(pri1如))}withd(p,q)=||p-Q||3.五、离群滤波器(outlierfilters)离群滤波器主要是降低匹配带来的误差。」v.yEc一表ICP算法模块的实现方法DatafilteringFixStepSampling,MaxDensity,MaxPointCount,MaxQuantileOnAxis,MinDist,ObservationDirection,OrientNormals,RandomSampling,RemoveNaN,SamplingSurfaceNormal,Shadow,SimpleSensorNoise,SurfaceNormalDataassociationKDTree,KDTreeVarDistOutlierfilteringMaxDist,MedianDist,MinDist,SurfaceNormal,TrimmedDist,VarTrimmedDistErrorminimizationPointToPlane,PointToPointTransformationcheckingBound,Counter,Differential图ICP算法流程图TrimmedDistanceRandomSamplinFigure:ModulescomprisingthedefaultICPchainReferenceReadingMatcheOutlierFiltersMtnimizerKDTre^MatchPointtoPlaneTransformationCheckersDataFiltersRandomSamplinCounterDifferentialTrartsformationNDT算法一、NDT算法概述ICP算法存在两个重要问题：一是点与点的匹配没有考虑到点与周围平面的联系;二是算法中用到的最邻近搜索计算量比较大。二、NDT算法具体步骤将空间(referencescan)划分成各个格子cell将点云投票到各个格子计算格子的正态分布PDF参数将第二幅scan的每个点按转移矩阵T的变换第二幅scan的点落于reference的哪个格子，计算响应的概率分布函数求所有点的最优值，目标函数为$=f［拭丁（瓦轨））4=1PDF可以当做表面的近似表达，协方差矩阵的特征向量和特征值可以表达表面信息（朝向、平整度）格子内少于3个点，经常会协方差矩阵不存在逆矩阵，所以只计算点数大于5的cell,涉及到下采样方法。、2D-NDT算法Fig.I.AnexampleoftheNDT:Theoriginallaserscanandtheresultingprobabilitydensity.1)Collectall2D-Pointscontainedinthisbox.2)Calculatethemeanq—工声g3)CalculatethecovariancematrixS=-q)(xi-q)tTheprobabilityofmeasuringasampleat2D-pointxcontainedinthiscellisnowmodeledbythenormaldistributionN(g：£):卩(X)〜g_Qq)罕《-叭⑴2D-NDT算法是在2维平面上描述连续，可微的概率密度。具体步骤如下：1)BuildtheNDTof什】亡firstscan.2)Initializetheestimatefortheparameters(byzeroorbyusingodometrydata)*3)Foreachsampleofthesecondscan:Mapthereconstructed2Dpointintothecoordinateframeofthefirstscanaccordingtolheparameters.4)Determinethecorrespondingnormaldistributionsforeachmappedpoint・5)Thescorefortheparametersisdetenninedbyevaluatingthedistributionforeachmappedpointandsummingtheresult-6)Calculateanewparameterestimatebytryingtooptimizethescore.ThisisdonebyperformingonestepofNewtotvsAlgorithin.7)Goto3untilaconvergencecriterionisme匚参考文献：[1]PBeslandHMcKay,1992.Amethodforregistrationof3-Dshapes.PatternAnalysisandMachineIntelligence,IEEETransactionson,14,pp.239-256.[2]AReviewofPointCloudRegistrationAlgorithmsforMobileRobotics.[3]ComparingICPvariantsonreal-worlddatasets.[4]PeterBiberandWolfgangStraRer.Thenormaldistributionstransform:Anewapproachtolaserscanmatching.InProceedingsoftheIEEEInternationalConferenceonIntelligentRobotsandSystems(IROS),pages27432748,LasVegas,USA,October2003.
