刚性配准python

泛型配准

• 前言
• 一、super 4pcs

• 1.1 简介
• 1.2原理
• 1.3 开源库介绍

前言

最近看了下几种配准的算法啊，发现会有一些共性，即不管是使用哪种方法，最终大多是关注于如何计算对应点的问题上来，icp及其变种是这样，super 4pcs也是如此。为了减少博客的数，就在这里统一称作为泛型配准，后续会追加一些同模式的配准原理。

一、super 4pcs

1.1 简介

Super 4pcs是4pcs的升级版，优化点是在于对应点对的选择策略上，将原本的随机选取三个点优化为4个共面点，在一定程度上可以增强算法的稳健性。

四点一致集算法的理论基础在于共面四点对的仿射不变性。在仿射变换中，由三个共线点所确定的比例$r = \frac{||a - b||}{||a - c||}$是恒定不变的，那么，对于平面内非共线的四个点a，b，c，d中，有ab与cd相交与e，则确定的比例也是不变的：

$r_1 = \frac{||a - e||}{||a - b||} \\ r_2 = \frac{||c - e||}{||c - d||}$

1.2原理

4PCS算法的基本原理如下：
步骤1：从源点云$Q$中选择共面四点：选取时遵循距离最大化原则，即保证点与点之间的距离较大但又小于某个阈值，而这个阈值是由重复率 $f$ 确定。选择点时，先选择3个，因为3个点可以确定一个平面，再在确定的平面内选择第四个点作为共面点。将确定的点构成集合$Q = {q_a, q_b, q_c, q_d}$，除可获得比例$r_1$与$r_2$外，还可以确定两个点的欧式距离，$d_1 = || q_a - q_b||, d_2 = || q_c - q_d ||$。

步骤2：在目标点$P$中选取对应点：先以$d_1, d_2$过滤部分样点，构成集合$S_1, S_2$：
$S_1 = {(p_i, p_j) | ||p_i - p_j|| \in [d_1 - e, d_1 + e]} \\ S_2 = {(p_i, p_j) | ||p_i - p_j|| \in [d_2 - e, d_2 + e]}$

步骤3：剔除错误的对应点：对于步骤2中构建的集合$S_1, S_2$，可通过比例，角度与距离进行约束，$<d_1, d_2, r_1, r_2, \theta>$

步骤4：根据对应点集计算矩阵T，并对Q进行变换，统计变换后的点云与目标点云中最近点距离小于某个阈值的点的数量，迭代执行上述步骤，直至满足条件为止。

假设有两片点云数据，P表示源点云，Q表示目标点云，在源点云P中确定4个共面点，并根据上述公式计算出$r_1, r_2$，对于Q中任意点对$q_1, q_2$，计算可能的交点$e_1, e_2$:
$e_1 = q_1 + r_1(q_2 - q_1) \\ e_2 = q_1 + r_2(q_2 - q_1) \\$
若在Q中存在两对这样的点使得一对的$e_1$与另一对的$e_2$在误差允许的范围内是相等的，那么这两对点可认为是P中所给的基的对应共面点，如上图中的(b)所示，其中灰色点代表$e_1$，黄色点代表$e_2$。

1.3 开源库介绍

CGAL中已利用OpenGR集成了Super 4PCS的函数，可直接调用，详细代码为：

#define CGAL_LINKED_WITH_OPENGR

#include <CGAL/Simple_cartesian.h>
#include <CGAL/IO/read_ply_points.h>
#include <CGAL/IO/write_ply_points.h>
#include <CGAL/property_map.h>

#include <CGAL/OpenGR/compute_registration_transformation.h>
#include <CGAL/OpenGR/register_point_sets.h>

#include <fstream>
#include <iostream>
#include <utility>
#include <string>

typedef CGAL::Simple_cartesian<double> K;
typedef K::Point_3 Point_3;
typedef K::Vector_3 Vector_3;
typedef std::pair<Point_3, Vector_3> Pwn;
typedef CGAL::First_of_pair_property_map<Pwn> Point_map;
typedef CGAL::Second_of_pair_property_map<Pwn> Normal_map;
namespace params = CGAL::parameters;
int main(int argc, const char** argv)
{
    std::string fname1 = "hippo1.ply";
    std::string fname2 = "hippo2.ply";

    std::vector<Pwn> pwns1, pwns2;
    std::ifstream input(fname1);
    if (!input ||
        !CGAL::read_ply_points(input, std::back_inserter(pwns1),
            CGAL::parameters::point_map(CGAL::First_of_pair_property_map<Pwn>()).
            normal_map(Normal_map())))
    {
        std::cerr << "Error: cannot read file " << fname1 << std::endl;
        system("pause");
        return EXIT_FAILURE;
    }
    input.close();
    input.open(fname2);
    if (!input ||
        !CGAL::read_ply_points(input, std::back_inserter(pwns2),
            CGAL::parameters::point_map(Point_map()).
            normal_map(Normal_map())))
    {
        std::cerr << "Error: cannot read file " << fname2 << std::endl;
        system("pause");
        return EXIT_FAILURE;
    }
    input.close();

    double score =
        CGAL::OpenGR::
        (pwns1, pwns2,
            params::point_map(Point_map())
            .normal_map(Normal_map())
            .number_of_samples(200)
            .maximum_running_time(60)
            .accuracy(0.01),
            params::point_map(Point_map())
            .normal_map(Normal_map()));
    
    std::ofstream out("out.ply");
    if (!out ||
        !CGAL::write_ply_points(
            out, pwns2,
            params::point_map(Point_map()).
            normal_map(Normal_map())
        ))
        std::cout << "AAAAAAA" << std::endl;


    system("pause");
    return 0;
}