【三维点云配准】4PCS算法实现点云粗配准

算法原理

4PCS[1]配准算法使用的是RANSAC算法框架,通过构建与匹配全等四点对的方式来减少空间匹配运算,进而加速配准过程。 具体的在任意姿态的输入点云P和Q中构建共面四点集合,使用仿射不变性约束,在共面四点集合中匹配符合条件的对应点对,使用LCP(Largest Common Pointset)策略寻找配准后最大重叠度四点对,得到最优匹配结果,完成点云粗匹配。

1. 问题描述

给定两个处于任意初始位置的点集P 和Q ,找到两个点集之间的最佳刚性变换,使得P ,Q 中两点间距离小于δ 的点数最多。

1.1 RANSAC

  • 分别在点集P和点集Q 中任意选取三个点来组成一个基础关联对
  • 计算这个关联对的旋转矩阵Ti
  • 计算点集P 中处在点集Q 中的点δ 距离内的点的个数ki
  • 如果ki足够大,则认为kiki 是个好结果,否则重读以上步骤
  • 这个过程将被重复L 次,选取最高的ki 作为最后的结果

1.2 Randomized Alignment

  1. 在点集P 中随机选取一个base
  2. 计算在点集Q 中所有有可能的bases,得到旋转矩阵
  3. 验证配准
  4. 先验证部分点集重合
  5. 验证剩余点

如何有效的计算可能的bases,提出4点法,基于共麺点(planar congruent sets)

2 4PCS概述

使用从点集P 中选取的共面四点作为base B ,然后采用一定的算法在点集Q中找到所有的与B近似一致的共面4点对,近似一致指的是两个4点对可以经过刚性变换(rigid transformation)在允许范围(allowed tolerance)δ 内对齐

2.1 Overview

仿射变换遵循以下法则,4PCS算法构建与匹配点间空间拓扑关系过程如图1所示。 首先在基准点云P中寻找满足要求的长基线(基线的确定与输入参数中overlap有很大关系,overlap越大,基线选择越长,长基线能够保证匹配的鲁棒性,且匹配数量较少)共面四点基,如图1用B={a,b,c,d}表示,然后提取共面四点基的拓扑信息,按式(1)计算四点基间的两个比例因子r1、r2, 两比例因子在点云旋转和平移变化中具有仿射不变性。 按式(2)计算q1,q2∈Q四种可能存在的交点位置,进而计算所有Q中长基线点对交点坐标,比较交点坐标确定匹配集合,ei≈ej表示寻找到对应的一致全等四点,i,j分别表示第i个和第j个Q中长基线点对。 图1中B={a,b,c,d}的全等四点对为C={q1,q3,q4,q5}。 寻找点云中所有P的共面四点集合记为E={B1,B2,…,Bm},m为P中四点集总数,重复上述步骤可得到全等四点集合D={C1,C2,…,Cn},n为全等四点集合总数。

2.2 4点对的仿射不变性

r1=a−ea−br1=a−ea−br2=c−ec−dr2=c−ec−d

在仿射变化中是不变的并且是唯一的。

现在给定一个具有仿射不变性n 个点的点集Q ,以及两个由点P 得到的仿射不变的比率r1r1 ,r2r2 ,对每一对点q1,q2⊂Qq1,q2⊂Q ,计算他们的中间点:
e1=q1+r1(q2−q1)
e1=q1+r1(q2 −q1)
e2=q1+r2(q2−q1)
e2=q1+r2(q2 −q1)

若任意两对这样的点,一对由r1 计算得到的中间点和另一对由r2 计算得到的中间点在允许范围内一致,那么可以认为这两对点可能是PP 中基础点的仿射对应点。
仿射对应点
请添加图片描述
在集合D={C1,C2,…,Cn}寻找最优全等四点对,4PCS使用LCP策略寻找最优全等四点匹配,即计算全等四点旋转和平移变化参数,将四点转化应用到全局点云转化,记录全局配准中包含最大的一致区域匹配记为最优匹配,至此完成4PCS算法局部粗配准工作。

4PCS算法官方介绍

C++实现代码

//PCL 4PCS算法实现点云粗配准
#include <pcl/registration/ia_fpcs.h> // 4PCS算法
void _4PCS(pcl::PointCloud<pcl::PointXYZ>::Ptr& target_cloud,
    pcl::PointCloud<pcl::PointXYZ>::Ptr& source_cloud)
{
    pcl::console::TicToc time;
    time.tic();
    //--------------初始化4PCS配准对象-------------------
    pcl::registration::FPCSInitialAlignment<pcl::PointXYZ, pcl::PointXYZ> fpcs;
    fpcs.setInputSource(source_cloud);  // 源点云
    fpcs.setInputTarget(target_cloud);  // 目标点云
    fpcs.setApproxOverlap(0.7);         // 设置源和目标之间的近似重叠度。
    fpcs.setDelta(0.01);                // 设置配准后对应点之间的距离(以米为单位)。
    fpcs.setNumberOfSamples(100);       // 设置验证配准效果时要使用的采样点数量
    pcl::PointCloud<pcl::PointXYZ>::Ptr pcs(new pcl::PointCloud<pcl::PointXYZ>);
    fpcs.align(*pcs);                   // 计算变换矩阵
    cout << "FPCS配准用时: " << time.toc() << " ms" << endl;
    cout << "变换矩阵:" << fpcs.getFinalTransformation() << endl;
    // 使用创建的变换对为输入点云进行变换
    pcl::transformPointCloud(*source_cloud, *pcs, fpcs.getFinalTransformation());
    visualizeCloud(target_cloud, source_cloud, pcs);
}

实验结果

无结果
初始位置差距稍微有点大,就无法配准。

  • 2
    点赞
  • 29
    收藏
    觉得还不错? 一键收藏
  • 打赏
    打赏
  • 32
    评论
评论 32
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包

打赏作者

点云兔子

你的鼓励将是我创作的最大动力

¥1 ¥2 ¥4 ¥6 ¥10 ¥20
扫码支付:¥1
获取中
扫码支付

您的余额不足,请更换扫码支付或充值

打赏作者

实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值