经典的ICP方法对上面的优化问题的处理思路如下:
(1)初始化
R 和 t 确定初始的
R 和 t 的方法很多,如果什么方法都不知道,那随便赋一个
R 和 t ,然后就迭代的算呀。随便给一个值从原理上来说也可以得到最终的一个结果呀,但是准不准就不知道了。相信有基本的优化概念的人都知道,初始值的选取很重要,如果初始值选的不好很容易收敛到一个局部最优解,然后局部最优解好不好那就另说了。ICP发展了这么多年了,当然有很多的方法来估计初始的R和t了,像PCL给的SampleConsensusInitalAlignment函数以及TransformationEstimationSVD函数都可以得到较好的初始估计。
(2)迭代
得到初始的估计后,接下来的步骤就顺理成章了:对于
X中的每一个点用当前的R 和
t在Y中找最近的点(比如用欧式距离),然后这两个点就成了一对了~就这样,对所有的点都这么做一次,然后我们就得到了所有的匹配对了~然后呢,用每一对的坐标列一个方程,就得到一系列的方程。然后就求解最优的R和t最小化上面的误差。如此循环往复。