困境:经典ICP算法的一些问题

因为学校项目的原因,可能要实现一个三维场景重建的功能,然后从经典的ICP算法开始,啃了很多文档,对其原理也是一知半解。

迭代最近点算法综述

大致参考了这份文档之后,照着流程用MATLAB实现了一个简单的ICP算法,首先是发现这份文档中一个明显的错误,

公式6

求两个点集的协方差,其中(Pi-p)和(Qi-p‘)分别求两个点集的各点与重心的差,都是(3*1)向量,这是不能相乘的,根据后文推断,此物的结果应为(3*3)矩阵,所以我大(zuo)胆(si)的改为(Pi-p)‘ * (Qi-p‘),做一次尝试。

Matlab代码如下:

%%% ICP迭代最近点算法

function [sourcePoint,aimPoint,distance] = ICPiterator( sourcePoint , targetPoint )
%%% 获得匹配点集,重心
aimPoint = getAimPoint(sourcePoint,targetPoint);

sourcePointCentre = getCentre(sourcePoint);
aimPointCentre = getCentre(aimPoint);

%%% 平移矩阵
T = getTranslation(aimPointCentre,sourcePointCentre);

%%% 中心化
midSourcePoint = centreTransform(sourcePoint, sourcePointCentre);
midAimPoint = centreTransform(aimPoint, aimPointCentre);

%%%旋转四元数
quaternion = getRevolveQuaternion(midSourcePoint,midAimPoint);

%%%旋转矩阵
revolveMatrix = getRevolveMatrix(quaternion);

%%%变换

sourcePoint = midSourcePoint * revolveMatrix;
sourcePoint = counterCentreTransform(sourcePoint,sourcePointCentre);

range = length(sourcePoint);
for i = 1:1:range
    sourcePoint(i,:) = sourcePoint(i,:) + T;
end

%%%阈值判定,欧拉距离和
distance = getDistance(sourcePoint,aimPoint);

end

%%% 点对搜索匹配,得到匹配点集
function [aimPoint] = getAimPoint( sourcePoint , targetPoint )
rangeS = length(sourcePoint );
rangeT = length(targetPoint);
aimPoint = zeros(rangeS,3);

for i = 1:1:rangeS
    minDistance = getDistance(sourcePoint(i,:),targetPoint(1,:));
    aimPoint(i,:) = targetPoint(1,:);
    for j = 1:1:rangeT
        distance = getDistance(sourcePoint(i,:),targetPoint(j,:));
        if distance < minDistance
            minDistance = distance;
            aimPoint(i,:) = targetPoint(j,:);
        end
    end
end
end

%%%旋转四元数
function [quaternion] = getRevolveQuaternion( sourcePoint , targetPoint )
    %%% 协方差
    pp = sourcePoint‘ * targetPoint;
    range = size(sourcePoint,1);
    pp = pp / range;

    %%% 反对称矩阵
    dissymmetryMatrix = pp - pp‘ ;

    %%% 列向量delta
    delta = [dissymmetryMatrix(2,3) ; dissymmetryMatrix(3,1) ; dissymmetryMatrix(1,2)];

    %%%对称矩阵Q
    Q = [ trace(pp) delta‘ ; delta   pp + pp‘ - trace(pp)*eye(3) ];

    %%%最大特征值,对应特征向量即为旋转四元数
    maxEigenvalues = max(eig(Q));
    quaternion = null(Q - maxEigenvalues*eye(length(Q)));

end

%%% 旋转矩阵
function [revolveMatrix] = getRevolveMatrix(quaternion)
    revolveMatrix = [ quaternion(1,1)^2 + quaternion(2,1)^2 - quaternion(3,1)^2 - quaternion(4,1)^2    2 * (quaternion(2,1)*quaternion(3,1) - quaternion(1,1)*quaternion(4,1))  2 * (quaternion(2,1)*quaternion(4,1) + quaternion(1,1)*quaternion(3,1));
                        2 * (quaternion(2,1)*quaternion(3,1) + quaternion(1,1)*quaternion(4,1))    quaternion(1,1)^2 - quaternion(2,1)^2 + quaternion(3,1)^2 - quaternion(4,1)^2     2 * (quaternion(3,1)*quaternion(4,1) - quaternion(1,1)*quaternion(2,1));
                        2 * (quaternion(2,1)*quaternion(4,1) - quaternion(1,1)*quaternion(3,1))  2 * (quaternion(3,1)*quaternion(4,1) + quaternion(1,1)*quaternion(2,1))   quaternion(1,1)^2 - quaternion(2,1)^2 - quaternion(3,1)^2 + quaternion(4,1)^2  ];
end

%%% 点集重心
function [centre] = getCentre( point )
    range = length(point);
    centre = sum(point)/range;
end

%%% 获取平移矩阵
function [T] = getTranslation( aimPointCentre , sourcePointCentre )
    T = aimPointCentre - sourcePointCentre;
end

%%% 点集中心化
function [point] = centreTransform(point,centre)
range = size(point,1);
for i = 1:1:range
    point(i,:) = point(i,:) - centre;
end
end

function [point] = counterCentreTransform(point,centre)
range = size(point,1);
for i = 1:1:range
    point(i,:) = point(i,:) + centre;
end
end

%%% 计算两点距离的平方,即欧拉距离和
function [distance] = getDistance(point1,point2)
    distance = (point1(1,1) - point2(1,1))^2 + (point1(1,2) - point2(1,2))^2 + (point1(1,3) - point2(1,3))^2;
end

为了看到迭代过程,这段代码每次只是进行一次迭代,但是实际情况下需要不断迭代,直到两点集的方差收敛,达到拟合要求。

用随机数生成了一个含一百个点的点集A,并对A进行一次随机的空间变化,得到B,这样A,B是完全可以拟合的两个点集;

点集A:

点集B:

用A,B来验证算法能不能实现点集的拟合。

试验了几次之后,发现无法收敛:

问题应该出在旋转四元数和旋转矩阵求解上,这块是一直没能理解透彻的部分。

困境:经典ICP算法的一些问题

时间: 2024-10-11 23:08:09

困境:经典ICP算法的一些问题的相关文章

经典排序算法 - 冒泡排序Bubble sort

 原文出自于 http://www.cnblogs.com/kkun/archive/2011/11/23/bubble_sort.html 经典排序算法 - 冒泡排序Bubble sort 原理是临近的数字两两进行比较,按照从小到大或者从大到小的顺序进行交换, 这样一趟过去后,最大或最小的数字被交换到了最后一位, 然后再从头开始进行两两比较交换,直到倒数第二位时结束,其余类似看例子 例子为从小到大排序, 原始待排序数组| 6 | 2 | 4 | 1 | 5 | 9 | 第一趟排序(外循环) 第

经典排序算法

经典排序算法(via  kkun) 经典排序算法,以下文章参考了大量网上的资料,大部分都给出了出处 这一系列重点在理解,所以例子什么的都是最简单的情况,难免失误之处,多指教 大多数排序算法都给出了每一步的状态,以方便初学者更容易理解,通俗易懂,部分难以理解的排序算法则给出了大量的图示,也算是一个特色吧 经典排序算法 - 快速排序Quick sort 经典排序算法 - 桶排序Bucket sort 经典排序算法 -  插入排序Insertion sort 经典排序算法 - 基数排序Radix so

C#实现所有经典排序算法

C# 实现所有经典排序算法 1 .选择排序 选择排序 原理: 选择排序是从冒泡排序演化而来的,每一轮比较得出最小的那个值,然后依次和每轮比较的第一个值进行交换. 目的:按从小到大排序. 方法:假设存在数组:72, 54, 59, 30, 31, 78, 2, 77, 82, 72 第一轮依次比较相邻两个元素,将最小的一个元素的索引和值记录下来,然后和第一个元素进行交换. 如上面的数组中,首先比较的是72,54,记录比较小的索引是54的索引1.接着比较54和59,比较小的索引还是1.直到最后得到最

经典滤波算法总结

第1种方法限幅滤波法(又称程序判断滤波法) A方法 根据经验判断,确定两次采样允许的最大偏差值(设为A) 每次检测到新值时判断: 如果本次值与上次值之差<=A,则本次值有效 如果本次值与上次值之差>A,则本次值无效,放弃本次值,用上次值代替本次值 B优点 能有效克服因偶然因素引起的脉冲干扰 C缺点 无法抑制那种周期性的干扰 平滑度差 /* A 值可根据实际情况调整 value 为有效值,new_value 为当前采样值 滤波程序返回有效的实际值 */ #define A 10 char val

经典白话算法之归并排序

void Merge(int A[],int p,int q,int r){ int i,j,k; //计算子数组A[p..q]的元素个数 int n1 = q - p + 1; //计算子数组A[q+1..r]元素个数 int n2 = r - q; //创建子数组L,R int* L = (int*)malloc(sizeof(int)*(n1+1)); int* R = (int*)malloc(sizeof(int)*(n2+1)); //将子数组A[p..q]赋值到L数组 for(i

经典白话算法之快速排序

[分析] [伪代码] [运行过程] [代码] /********************************* * 日期:2014-04-01 * 作者:SJF0115 * 题目:快速排序 **********************************/ #include <iostream> #include <stdio.h> using namespace std; //对子数组array[p...r]就地重排 int Partition(int array[],i

ICP算法(Iterative Closest Point迭代最近点算法)

标签: 图像匹配ICP算法机器视觉 2015-12-01 21:09 2217人阅读 评论(0) 收藏 举报 分类: Computer Vision(27) 版权声明:本文为博主原创文章,未经博主允许不得转载. 最近在做点云匹配,需要用c++实现ICP算法,下面是简单理解,期待高手指正. ICP算法能够使不同的坐标下的点云数据合并到同一个坐标系统中,首先是找到一个可用的变换,配准操作实际是要找到从坐标系1到坐标系2的一个刚性变换. ICP算法本质上是基于最小二乘法的最优配准方法.该算法重复进行选

经典排序算法的PHP实现类

近期广受笔试摧残,对于各种排序也是晕头转向. 更坑爹的是貌似大多都是用C++.Java实现相关算法,让我搞PHP的情何以堪,更何况,PHP本身就有排序函数sort(),其实来说,是很简单的,这也可能是为什么不用PHP进行排序吧. 但考虑到PHP毕竟也是一门面向对象的语言吧,我们利用原生的语法,也是可以实现经典排序算法的,先不说性能如何,切不要妄自菲薄吧. 下面为具体的经典排序算法的PHP实现类. <?php /** * Author: helen * CreateTime: 2016/4/15

ICP算法使用遇到的问题

这几天在学习数据关联的方法,本来想使用ICP算法进行距离测距数据的配准,但是用的过程中出现问题,配的不准,而且偏差更大了. 红色的和黄色的2维激光点进行ICP配准,但将变换矩阵和黄色进行乘之后偏差更大了.怀疑是因为两个点集只有部分数据重合,而ICP算法最好是点能一一对应. 之后使用PCL进行点集匹配测试,出现同样的问题. 于是我自己构造了一个数据,将A点集进行一个刚体变换,之后用ICP配准,再进行对比,发现算法可以配准的很好,应该是部分点集重合造成的.