点云配准(四) Sparse Point Registration 算法浅析

2023-10-29

        Sparse Point Registration (SPR)是一篇2017年的点云配准算法,该算法的主要目的是对稀疏点云进行配准,并且取得了不错的成果和突破。本文一方面是对SPR配准算法模型进行了简单的原理解析以及附加代码实现,另一方面是对之前工作的总结,也算水篇博文,接下来的工作主要就是分割和光流预测方面的学习了。

一.算法模型概述

1.算法背景

        所谓稀疏点云就是点数稀少的点云模型,有时我们需要用到一些物体上的关键点来和目标模型进行配准,计算一些关键指标。而传统的点云配准算法要求待配准的两片点云数量级相当,并且还包括粗配准和精配准两个阶段。经实验可得,传统点云配准算法在稀疏点云配准上表现较差,因此稀疏点云配准十分关键。

 2.算法模型 

        SPR算法不需要进行粗配准就可以实现效果较好的稀疏点云配准效果,该算法的核心思想主要包括扰动、迭代、细化三个部分。SPR算法模型包括以下步骤:

  • 初始化目标模型点云数据A,稀疏点云数据B,最大迭代次数MaxIterations,配准误差阈值Threshold、扰动次数P
  • 根据点云Size和高斯分布计算当前迭代的扰动量(扰动量随着迭代次数不断减小,最后减小到零)获取P个扰动扰动变换矩阵,然后对当前B点云进行扰动变换
  • 分别计算P个扰动变换后的稀疏点云B和目标模型点云A的 Cost (这里可以使用K-D Tree 计算最近点偏差和作为Cost),选择Cost最小的作为局部最优扰动
  • 基于当前的最优扰动变换后的稀疏点云B,进行 ICP or DQF 算法精配准细化(本文选择ICP),获得精配准矩阵Tk,并计算此时的误差
  • 若计算的误差小于最小误差e,则更新全局最优边换矩阵T和最小误差e为当前迭代结果。
  • 重复迭代以上过程,直到达到最大迭代次数MaxIterations或计算误差e<配准误差阈值Threshold

 3.算法流程图

         除此之外,论文中还阐释了一些比较细节的注意事项和参数设置,比如最大迭代次数MaxIterations、配准误差阈值Threshold、扰动次数P设置为多少,如何进行扰动计算,以及如何在试验中取点等等,当然稀疏点云中越具有特征性的关键点配准效果越好。

二.算法实现(ICP版本)

        本文的SPR算法实现使用Matlab编程进行,采用ICP的精配准版本,在实现过程中对原算法进行了改进:在迭代过程中,对每一个出现的扰动变换都进行ICP精配准计算,然后基于这个误差进行比较,而不只是原算法所言的选出最优扰动后再计算ICP,这样可以最大程度的提高算法精确度,但是牺牲了时间复杂度。下面给出部分代码。

(1)SPR.m

% SPR Main logic function:transform A sparse point cloud into B model point cloud coordinate system for registration
% - Params:
%   - A:sparse point cloud [n,3]
%   - B:model point cloud [m,3]
% - Return:
%   - R:optimal rotation matrix [3,3]
%   - T:optimal translation matrix [1,3]
%   - trans_res:transform point cloud [n,3]
function [ R,T,trans_res ] = SPR(A,B)

%1.kdtree model for B(euclidean distance) and size of the model
Mdl = KDTreeSearcher(B);
Size = max(range(B));
%2.Initialize R0,T0,e0
R = eye(3);
%Align the centroids of two point clouds A->B
cen_A = sum(A)/length(A);
cen_B = sum(B)/length(B);
T = cen_B - cen_A;
trans_res = transform(A,R,T);
e = calculateCost(trans_res,B,Mdl);
%3.Initialize the number of disturbed pose P, the maximum number of iterations I, and the minimum error threshold RMS
P = 10;
I = 30;
RMS = 1e-6;
%4.Initialize loop variable
k = 1;
%record data
x_loop = 1:I;
y_error = zeros(1,I);

%5.SPR Loop
while e > RMS && k <= I
    cost_best = inf;
    R_k = R;
    T_k = T;
    %(1). Perturbations for Sparse point cloud,and then the perturbation transformation matrix is obtained
    % - R_p: [3,3,P+1]
    % - T_p: [p+1,3]
    [R_p, T_p] = perturb(Size,P,k,I);
    %(2). Calculate the optimal pose
    for j = 1:P+1
        %Obtain the transformation matrix parameters rp_j(3,3), tp_j(1,3) of the current p-th disturbance pose
        rp_j = R_p(:,:,j)*R;
        tp_j = (R_p(:,:,j)*T')'+T_p(j,:);
        %Calculate current optimal cost(from ICP)
        [R_j,T_j,cost_j] = solveicp(A,B,Mdl,rp_j,tp_j);
        %judge optimal cost
        if cost_j < cost_best
            cost_best = cost_j;
            R_k = R_j;
            T_k = T_j;
        end
    end
    %(3). Error calculation and judge for update
    trans_k = transform(A,R_k,T_k);
    e_k = calculateCost(trans_k,B,Mdl);
    if e_k < e
        e = e_k;
        R = R_k;
        T = T_k;
    end
    y_error(k) = e;
    k = k+1;
end
trans_res = transform(A,R,T);

figure
plot(x_loop,y_error,marker='o', MarkerFaceColor='r');
xlabel('loop');
ylabel('ems error');

end

(2)Perturb.m

%Disturbance function: Generates random perturbations based on iteration within loop and specified
% - Params:
%   - size:Size of the model
%   - P:Number of disturbances
%   - k:Number of iterations
%   - I:Iteration upper bound
% - Return:
%   - R_p:Perturbation rotation matrix [3,3,P+1]
%   - T_p:Perturbation translation matrix [P+1,3]
function [ R_p, T_p ] = perturb( size,P,k,I )

%1.Initial translation disturbance:10% of the model size
T_p = normrnd(0,(0.1*size*((I+1-k)/I)),[P,3]); %[P,3]

%2.Initial perturbation number of rotation:10 ° normal distribution(Right hand coordinate system)
%Around x
a = normrnd(0,(10*((I+1-k)/I)),[1,P]); %[1,P]
%Around y
b = normrnd(0,(10*((I+1-k)/I)),[1,P]);
%Around z
c = normrnd(0,(10*((I+1-k)/I)),[1,P]);

%3.convert to radians
a = a.*(pi()/180);
b = b.*(pi()/180);
c = c.*(pi()/180);

%4.initialize R_p
R_p = zeros(3,3,P);

%5.Generate P disturbances respectively (matlab subscript starts from 1)
for j = 1:P
    %rotation matrix about x
    r1 = [1,0,0; 0, cos(a(j)), -sin(a(j)); 0, sin(a(j)), cos(a(j))]; 
    %rotation matrix about y
    r2 = [cos(b(j)), 0, sin(b(j)); 0,1,0; -sin(b(j)), 0, cos(b(j))];
    %rotation matrix about z
    r3 = [cos(c(j)), -sin(c(j)), 0; sin(c(j)), cos(c(j)), 0; 0,0,1];
    %Perturbation Rotation
    R_p(:,:,j) = r1*r2*r3;
end
%P+1:Transformation without disturbance
R_p(:,:,P+1) = eye(3);
T_p(P+1,:) = [0,0,0];
end

(3)solveicp.m

%Function takes in A and B clusters, registers A to B using ICP, returns resultant transformation matrix and error calculation
% - Params:
%   - A:initial point cloud [n,3]
%   - B:target point cloud [m,3]
% - Return:
%   - TR:solve rotation matrix [1,3]
%   - RO:solve translation matrix [3,3]
%   - e:registers error
function [ R,T,e ] = solveicp( A,B,Mdl,R_0,T_0 )
 
%1.initialize variable parameters
k = 1;
error = 0.0001;
Iterator = 50;
R = R_0;
T = T_0;
%2.initial:kdtree algorithm to find the nearest point pair of two point clouds
trans = transform(A,R,T);
e = calculateCost(trans,B,Mdl);
%3.ICP loop
while e > error && k <= Iterator
    %find the nearest point
    idx = knnsearch(Mdl,trans);
    NN = B(idx,:);
    %svd solve: optimal matching matrix of two point clouds in current state
    [T_k, R_k] = solvesvd(trans,NN);
    %Cumulative transformation T=[1,3] R=[3,3]
    T = (R_k*T')'+T_k;
    R = R_k*R;
    trans = transform(A,R,T);
    %Update error E
    e = sum(sqrt(sum((NN-trans).^2,2)))/length(trans);
    k = k+1;
    
end

end

(4)solvesvd.m

%function: svd solve yo find T and R between point set B and it nearest neighbour set
% - Params:
%   - B:initial point clouds [n,3]
%   - NN: target nearest neighbour point clouds [n,3]
% - Return:
%   - T:solve optimal translation matrix [1,3]
%   - R:solve optimal rotation matrix [3,3]
function [ T, R ] = solvesvd( B, NN )

num = length(B);
%1.Seeking centroid
cen_B = sum(B)/num;
cen_NN = sum(NN)/num;
%2.Decentralization:Set centroids as reference origin for both clusters
B2 = B-repmat(cen_B,num,1);
NN2 = NN-repmat(cen_NN,num,1);
%3.svd decompose:[U,S,V] = svd(A) ==> A = U*S*V'
H = B2'*NN2;
[U,~,V] = svd(H);
R = V*diag([1 1 sign(det(V*U'))])*U';
T = cen_NN' - R*cen_B';
T = T';

end

三.实现效果

 注意:上图中蓝色为初始稀疏点云,黑色为配准后的稀疏点云结果

本文内容由网友自发贡献,版权归原作者所有,本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容,请联系:hwhale#tublm.com(使用前将#替换为@)

点云配准(四) Sparse Point Registration 算法浅析 的相关文章

随机推荐

  • 软件测试策略

    历史的车轮滚滚向前 科技的创新生生不息 大数据 虚拟现实 智能汽车 区块链等为代表的新技术和新应用层出不穷 它们都与软件息息相关 而软件的质量保证离不开软件测试 软件测试要在成本 范围 进度 资源等因素的制约下取得最佳产出 就离不开测试策略
  • 【多字典公共键】快速找到多个字典的公共键及非公共键

    前言 在进行一个项目过程中遇到一个多个URL参数对比与分析的问题 所以就有了这个博客 单纯的了解找到多个字典的公共键的问题 可以移步到 python进阶 python进阶技巧 找到2 5 快速找到多个字典的公共键 想看实操 不妨浏览一下下面
  • keil关于use MicroLIB 和半主机模式的总结

    半主机是这么一种机制 它使得在ARM目标上跑的代码 如果主机电脑运行了调试器 那么该代码可以使用该主机电脑的输入输出设备 这点非常重要 因为开发初期 可能开发者根本不知道该 ARM 器件上有什么输入输出设备 而半主基机制使得你不用知道ARM
  • 虚拟机连接本地数据库

    我们在运行虚拟机上面的项目时 可能要用到本机的数据库 在使用过程中会遇到数据库拒绝访问的情况 这是因为在安装本地数据库时没有启动远程连接 使用两种方法来解决这种问题 一 使用命令行模式 第一步 先切换到MySQL的安装路径下面的bin目录
  • 百分率的sql

    百分比的sql语句 方式一的百分比的sql 说明 这条sql是在一张表里面进行查询 将不同的级别的工程师的百分率查询出来 第一个sql select tmp job grade tmp tcount tmp1 t1count from se
  • 基于单片机的数字频率计设计

    数字频率计概述 数字频率计是计算机 通讯设备 音频视频等科研生产领域不可缺少的测量仪器 它是一种用十进制数字显示被测信号频率的数字测量仪器 它的基本功能是测量正弦信号 方波信号及其他各种单位时间内变化的物理量 在进行模拟 数字电路的设计 安
  • 【力扣】205.同构字符串

    同构字符串 同构字符串 1 题目描述 2 示例 3 代码 方式1 方式2 5 编译结果 同构字符串 1 题目描述 给定两个字符串 s 和 t 判断它们是否是同构的 如果 s 中的字符可以按某种映射关系替换得到 t 那么这两个字符串是同构的
  • 【蓝桥杯Python】2023.2.3-寻找2020

    题目描述 本题为填空题 只需要算出结果后 在代码中使用输出语句将所填结果输出即可 小蓝有一个数字矩阵 里面只包含数字 00 和 22 小蓝很喜欢 20202020 他想找 到这个数字矩阵中有多少个 20202020 小蓝只关注三种构成 20
  • 2023华为OD机试真题【施肥问题】

    题目描述 思路题解 首先需要计算每个果园的施肥时间 即果园面积除以施肥机能效 然后找到最小的施肥机能效 保证施肥任务能在规定时间内完成 如果施肥天数小于果园数量 则无法完成施肥任务 返回 1 如果施肥天数等于果园数量 则直接返回最大果园面积
  • 编译原理第七章笔记 -- 中间代码生成

    本文中内容整理西安交通大学软件学院吴晓军老师的ppt中 仅供学习使用 请勿转载或他用 参考教材 程序设计语言 编译原理 第3版 陈火旺等 国防工业出版社 这一章分数在35左右 两个大题 数组的引用四元式生成 控制语句当中布尔表达式的翻译 考
  • 运维必学

    欢迎关注 全栈工程师修炼指南 设为 星标 每天带你 基础入门 到 进阶实践 再到 放弃学习 专注 企业运维实践 网络安全 系统运维 应用开发 物联网实战 全栈文章 等知识分享 花开堪折直须折 莫待无花空折枝 作者 lt 安全开发运维 gt
  • VS2012编译安装VTK-5.10.1(支持Python)

    1 源码下载 到参考资料 1 下载vtk 5 10 1 zip和vtkdata 5 10 1 zip 2 源码解压 这里以D 盘为例进行说明 在D 盘中创建一个目录VTK 然后在其中创建4个目录 source build data和inst
  • mysql explain详解

    转自 http www blogjava net persister archive 2008 10 27 236813 html 在 explain的帮助下 您就知道什么时候该给表添加索引 以使用索引来查找记录从而让select 运行更快
  • Nginx反向代理与负载均衡

    文章目录 一 网关 代理与反向代理的关系 二 反向代理在系统架构中的应用场景 三 Nginx反向代理配置 1 不重定向配置 2 重定向配置 四 基于反向代理的负载均衡器 不支持https 五 负载均衡介绍 1 负载均衡策略 2 负载均衡调度
  • 三个java超级变态逻辑循环编程题

    1 有一根27厘米的细木杆 在第3厘米 7厘米 11厘米 17厘米 23厘米这五个位置上各有一只蚂蚁 木杆很细 不能同时通过一只蚂蚁 开始时 蚂蚁的头朝左还是朝右是任意的 它们只会朝前走或调头 但不会后退 当任意两只蚂蚁碰头时 两只蚂蚁会同
  • 【4】测试用例设计-判定表法

    判定表适用于有几个原因 导致几个结果的情况 实际测试中 如果输入条件较多 再加上各种输入与输出之间相互的作用关系 画出的因果图会比较复杂 容易使人混乱 为了避免这种情况 人们往往使用决策表法代替因果图法 决策表也称为 判定表 其实质就是一种
  • 各大公司薪资

    联合利华 MKT 9500 3000元安家费 普通职位 8KX12 联合利华销售代表 底薪加提成 总体一般 一般能拿到5K以上 宝洁 本8600 硕9700 博10500发14个月 11年数据 欧莱雅 MKT 6 6K X 13 11年数据
  • ajax工作原理 网页从输入url到呈现过程(TCP ,渲染引擎) 头像上传 下拉加载 节流 防抖 常见状态码

    Ajax工作原理 1 http网络传输协议 规定 前后端交互的 数据传输格式 协议 规定 前后端交互的数据传输格式 2 http协议组成两个部分 2 1前端 必须发送 请求报文格式 2 2后端 必须响应 响应报文格式 3 请求报文格式组成
  • VueX是什么?好处?何时使用?

    VueX相关 1 VueX是什么 2 使用VueX统一管理状态的好处 3 什么样的数据适合存储到Vuex中 1 VueX是什么 VueX是实现组件全局状态 数据 管理的一种机制 可以方便的实现组件之间数据的共享 如果没有VueX实现数据间的
  • 点云配准(四) Sparse Point Registration 算法浅析

    Sparse Point Registration SPR 是一篇2017年的点云配准算法 该算法的主要目的是对稀疏点云进行配准 并且取得了不错的成果和突破 本文一方面是对SPR配准算法模型进行了简单的原理解析以及附加代码实现 另一方面是对