基于转盘的点云配准技术

一、概述

随着3D技术的成熟和普及，越来越多的3D相机应用到工程领域的测量、重建等应用当中。受到相机视野的局限，对于大尺寸的工件使用一个相机往往难以一次成像完全（即拍不全），所以需要将相机移动到不同位置对目标物体进行拍照，再将这些点云拼接在一起形成一张完整的点云，这就是点云配准技术。当然，另一种方式是将相机固定安装，将目标物体进行旋转或者平移，每隔固定时间拍照一次，再对所拍图片进行拼接，这种方式和第一种方式是可以互相转换的。点云配准的核心原理是将不同位姿下的点云转换到同一个坐标系下表示。以下图为例，将一款基于面阵结构光的3D相机固定放置在一个位置上，在距离其大概1m的位置有一个转盘，将目标物体（杯子）放在转盘上作逆时针旋转，转速为13.473度/秒，相机每隔2672ms拍照一次，这样可以计算出相邻两幅点云之间的旋转角度为36度。值得一提的是，相机固定安装，将物体放置在转台上绕转台的中心轴逆时针旋转。该过程等价于将物体静止不动，然后相机绕着中心轴作顺时针旋转同样的角度，这两种方式下取到的点云数据是一模一样的。所以虽然我们的实际场景是相机固定安装，物体在转台上转动，但是我依然把它转换成一个相机绕着转台中心轴旋转的问题来进行分析，最终将不同视角下的点云转化到同一个坐标系（视角）下。

二、原理

将概述中描述的场景抽象成数学模型。点Q为转盘的中心点，OXZ是初始位姿下的相机坐标系，Z轴表示目标点距离相机的X轴的垂直距离。注意，这里没有标出相机坐标系的Y轴是默认相机坐标系的Y轴和转台的中心轴是平行的，即保证转台平面和相机坐标系的OXZ平面是平行的，下面我们讨论的问题都是基于这个前提，如果要考虑与Y轴不平行的情形，可以在该问题上进一步延伸。O’X’Z’表示将相机坐标系沿着中心轴顺时针旋转θ角。OXZ和O‘X’Z‘这两个坐标系之间的转换关系既有旋转关系也有平移关系。我们需要做到事情就是求出这两个坐标系之间的转换关系，然后将其中一个坐标系下的目标点云转换到另外一个坐标系下表示。旋转关系可以通过旋转矩阵来描述，由于旋转角度可以通过转盘的转速来计算，所以可以很快求出旋转矩阵R。

平移关系T即O‘在OXZ坐标系下的坐标(dx,dz)，计算过程如下：
值得一提的是，要想求O’坐标，需要事先标定出点Q在OXZ中的坐标，我这里采用的办法是将一个圆锥体放在转台上，使用相机拍照，然后手动点取圆锥顶点获得点Q坐标。如示意图，得到点Q坐标以后，可以求出m、n、L。
tanγ = m / n ①
β = （180 - θ) / 2 ②
α = 180 - γ - β ③
oo’ = sqrt(L^2 + L^2 - 2L^2cosθ) ④
联立上面4个关系式和简单的三角函数可以求出Q’(dx,dz)。dy默认为0。
这样，我们便计算出了相邻两个坐标系之间的旋转矩阵R和平移矩阵T。

三、代码实现

#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/filters/passthrough.h>
#include <opencv2/opencv.hpp>
#include "Camera.hpp"
#include <ctime>
#define PI 3.1415926
Camera camera;
struct CallbackArgs {
   
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr clicked_points_3d;
    pcl::visualization::PCLVisualizer::Ptr viewerPtr;
};
struct CallbackArgs  cb_args;
void pickPointCallback(const pcl::visualization::PointPickingEvent& event, void *args);
void Delay(int time);
 
int main(int argc, char *argv[])
{
   
    if (camera.Open() == 0) {
   
      std::cout << "open camera success" << std::endl;
    } else {
   
      std::cout << "open camera fail" << std::endl;
    }
    boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("3D Viewer"));
    pcl::PointCloud<pcl::PointXYZ>::Ptr  recons_cloud(new pcl::PointCloud<pcl::PointXYZ>);
    std::chrono::system_clock::time_point now = std::chrono::system_clock::now();
    std::chrono::system_clock::time_point last = std::chrono::system_clock::now();
    int delay_time = 0;
    for (int i = 1; i <= 9