ICP配准MATLAB实现
ICP配准MATLAB实现
本文介绍了ICP算法及其MATLAB实现
目录
- ICP配准MATLAB实现
- 一、ICP算法
- 1.算法简介
- 2. 算法局限性
- 二、代码实现
- 1.平移旋转计算
- 2.ICP算法代码
- 参考文献
一、ICP算法
经典的ICP(iterative closet point)算法是由McKay和Besl1提出的,它的核心思想是迭代调姿使距离偏差最小化。原始ICP算法的基本描述是:对于点云P的每个点,在另一个点云Q中求取距离它最近的点(理想状态下本应重合的点),计算对应点的欧式距离平方的平均值,然后通过迭代算法,最小化平均值,这样不断更新点云片间的相对位置,达到点云片之间配准对齐的效果,如图6所示。
1.算法简介
扫描点云与CAD模型最佳拟合对齐是利用奇异值分解法、四元组法找出扫描点云与模型表面对应点的变换矩阵,多次迭代直至目标函数满足一定的精度为止。其目标函数为:
式中:pi(i=0,1,…,n) 为待配准的扫描点; qi(i=0,1,…,n)为pi在CAD模型表面上的匹配点; R,T分别为待求的旋转矩阵与平移向量。
具体计算步骤如下:
Step1. 计算两个待配准点集的质心:
Step2. 计算两个待配准点集相对于各自质心的位移:
Step3. 计算协方差矩阵:
Step4. 构造对称矩阵:
Step5. 计算旋转矩阵
Step6. 计算平移变换矢量
2. 算法局限性
ICP有以下局限性:
(1)初始位置的依赖性
初始位置对它的影响很大,ICP算法的收敛速度及收敛的最终位置直接受制于求取的匹配点(最近点)是否精确,而只有在扫面点云与CAD模型有较好的初始位置关系时,匹配点才能精准地获取到。
(2)局部最优性
ICP算法得到的是局部最优解,从本质上来讲,该算法每次迭代总是选择距离差的最小值,并不能得到全局最优解。
(3)不区分正负性
ICP算法是以最小二乘的方式最小化距离差,其收敛结果趋向于使得各配对点的差值均匀化,并且该距离差不具有正负性和方向性。
(4)计算匹配点复杂度较高
ICP算法的时间复杂度非常高:计算扫描点的匹配点(求取最近点)时,对于点云片P中的每个点,需要遍历CAD模型的曲面集合 中每个面,计算匹配点集 时间复杂度为 ,并且点到曲面最近点的计算也是较为耗时的。为了提高最佳拟合对齐的效率,点云数据需要精简,计算最近点的方式需要改进。
二、代码实现
1.平移旋转计算
代码如下:
function [data_q,T] = rotate(data,x,y,z,t)%欧拉角转旋转矩阵x = x/180*pi;
y = y/180*pi;
z = z/180*pi;Rx = [1 0 0;0 cos(x) -sin(x);0 sin(x) cos(x)];
Ry = [cos(y) 0 sin(y);0 1 0;-sin(y) 0 cos(y)];
Rz = [cos(z) -sin(z) 0;sin(z) cos(z) 0;0 0 1];
T = Rz*Ry*Rx; %旋转矩阵T = [T(1,1),T(1,2),T(1,3),t(1);T(2,1),T(2,2),T(2,3),t(2);T(3,1),T(3,2),T(3,3),t(3);0 0 0 1]; % 复合rows=size(data,2);
rows_one=ones(1,rows);
data=[data;rows_one]; %化为齐次坐标data_q=T*data;
data_q=data_q(1:3,:); %返回三维坐标
2.ICP算法代码
代码如下:
% ICP 算法
clear;
close all;
clc;data_source=load('16-3.txt');
data_source=data_source';
%旋转角度
alpha = 20;
theta = 5;
grama = 5;t=[1,1,30]; %平移向量[data_target,T0]=rotate(data_source,alpha ,theta,grama,t);%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%绘制两幅原始图像
x1=data_source(1,:);
y1=data_source(2,:);
z1=data_source(3,:);
x2=data_target(1,:);
y2=data_target(2,:);
z2=data_target(3,:);
figure(1);
title('初始位置');
scatter3(x1,y1,z1,'b*');
hold on;
scatter3(x2,y2,z2,'r*');
hold off;T_final=eye(4,4); %旋转矩阵初始值
iteration=0;
Rf=T_final(1:3,1:3);
Tf=T_final(1:3,4);
data_target=Rf*data_target+Tf*ones(1,size(data_target,2)); %初次更新点集(代表粗配准结果)
err=1;
while(err>0.0001)iteration=iteration+1; %迭代次数disp(['迭代次数ieration=',num2str(iteration)]);%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%利用欧式距离找出对应点集 搜索每个点k=size(data_target,2);for i = 1:kdata_q1(1,:) = data_source(1,:) - data_target(1,i); % 两个点集中的点x坐标之差data_q1(2,:) = data_source(2,:) - data_target(2,i); % 两个点集中的点y坐标之差data_q1(3,:) = data_source(3,:) - data_target(3,i); % 两个点集中的点z坐标之差distance = data_q1(1,:).^2 + data_q1(2,:).^2 + data_q1(3,:).^2; % 欧氏距离[min_dis, min_index] = min(distance); % 找到距离最小的那个点data_mid(:,i) = data_source(:,min_index); % 将那个点保存为对应点error(i) = min_dis; % 保存距离差值end%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%去中心化% 质心data_target_mean=mean(data_target,2);data_mid_mean=mean(data_mid,2);data_target_c=data_target-data_target_mean*ones(1,size(data_target,2));data_mid_c=data_mid-data_mid_mean*ones(1,size(data_mid,2));%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%SVD分解W=zeros(3,3);% 计算协方差矩阵for j=1:size(data_target_c,2)W=W+data_mid_c(:,j)*data_target_c(:,j)';end[U,S,V]=svd(W);Rf=U*V';% 计算质心平移矢量Tf=data_mid_mean-Rf*data_target_mean;err=mean(error);T_t=[Rf,Tf];T_t=[T_t;0,0,0,1];T_final=T_t*T_final; %更新变换矩阵disp(['误差err=',num2str(err)]);disp('变换矩阵T=');disp(inv(T_final));data_target=Rf*data_target+Tf*ones(1,size(data_target,2)); %更新点集if iteration >= 200breakend
enddisp('真值');
disp(T0); %旋转矩阵真值,对矩阵求逆x1=data_source(1,:);
y1=data_source(2,:);
z1=data_source(3,:);
x2=data_target(1,:);
y2=data_target(2,:);
z2=data_target(3,:);
figure(2);
title('配准后');
scatter3(x1,y1,z1,'r*');
hold on;
scatter3(x2,y2,z2,'b*');
hold off;
注:代码中的导入的文件是零件的点云数据。
参考文献
戴静兰, 陈志杨, 叶修梓. ICP算法在点云配准中的应用[J]. 中国图象图形学报, 2007(03):517-521. ↩︎
ICP配准MATLAB实现相关推荐
- ICP算法MATLAB仿真
2022.4.15再次附上数据satellite.txt新链接: 链接:https://pan.baidu.com/s/1mEN-FQbTlHOWfxCHyu0Dxg 提取码:02i4 (永久有效) ...
- PCL点云处理之点面ICP配准(附代码,参数设置,实验结果)(六十七)
PCL点云处理之点面ICP配准(附代码,参数设置,实验结果)(六十七) 前言 一.点面ICP是什么? 二.使用步骤 1.代码 效果 总结 前言 学习点云配准,我辈义不容辞 一.点面ICP是什么? 将之 ...
- 图像的配准——MATLAB实现
通过使用MATLAB中的cpselect函数选择基准点,将两幅图像进行配准/对准. 首先读取两幅图像 clear all; Inimg = imread('g.jpg'); Baseimg= imre ...
- open3d完成点云ICP配准
环境:win10,python3.6+open3d==0.8.0 完成点云配准之后的效果图演示 这里直接放上完整的代码 import open3d as o3d import numpy as np# ...
- 【图像配准】基于Horn-Schunck和Lucas-Kanade等光流场实现图像配准matlab源码含GUI界面
光流法理论背景 1.什么是光流 光流(optical flow)是空间运动物体在观察成像平面上的像素运动的瞬时速度. 光流法是利用图像序列中像素在时间域上的变化以及相邻帧之间的相关性来找到上一帧跟当前 ...
- 图像平移配准matlab,(MATLAB应用图像处理)第6章MATLAB图像配准.ppt
第6章 MATLAB图像配准 6.1 图像配准概述 6.1.1 图像配准定义 图像配准是对从不同传感器.不同时间.不同视点所获得的两幅或多幅图像进行最佳匹配,以达到空间匹配和叠加目的的处理过程.本质上 ...
- 互信息配准matlab,基于图像特征和互信息的图像配准方法
基于图像特征和互信息的图像配准方法 [专利摘要]本发明公开一种基于图像特征和互信息的图像配准方法,主要用于提高现有基于互信息配准方法的精确度.其实现步骤为:(1)输入两幅图像,一幅为参考图像r,另一幅 ...
- matlab点集配准,matlab练习程序(对应点集配准的四元数法)
clear all; close all; clc; %生成原始点集 X=[];Y=[];Z=[]; for i=-180:2:180 for j=-90:2:90 x = i * pi / 180. ...
- 【图像配准】基于粒子群改进的sift图像配准matlab源码
SIFT \ SIFT尺度不变特征转换,具有选择,尺度不变性.由David Lowe在1999年所发表,2004年完善总结. owe将SIFT算法分解为如下四步: \ 1. 尺度空间极值检测:搜索所有 ...
最新文章
- CodeForces #369 div2 D Directed Roads DFS
- Tungsten Fabric SDN — 与 OpenStack 的集成架构
- 机器学习算法基础——数据特征预处理
- 再来一顿贺岁宴 | 从K-Means到Capsule
- Python3——字典
- pycharm快捷键_春节快结束了回单位途中总结下pycharm快捷键
- docker daemon 配置文件
- 计算机word艺术字形状设置,4.11 Word 2016 自定义艺术字的形状效果,制作漂亮的艺术字...
- 学界还是业界?当 IT 女孩们面临未来选择
- 并发 锁和隔离等级的关系
- Dedecms文件夹目录解释完整版
- 同样是OpenJDK8,有的平台需要libpng12,有的不需要
- Octave GNU默认配置文件位置
- 结合thinkphp5与hplus(h+)写的一个带权限的后台管理系统
- 甲醛测量仪 DART WZ-S + stm32f103c + stmDuino IDE + I2C OLED
- 计算机网络中速率(date rate)和带宽的区别
- 馄饨 (hún tun)
- SpringBoot整合阿里云短信服务详细过程(保证初学者也能实现)
- Practical Full Resolution Learned Lossless Image Compression
- Python自然语言处理 10 分析语句的含义