《点云处理》 点云刚体变换

前言

点云刚体变换是点云常见操作之一，像点云配准就需要使用刚体变换。点云刚体变换的表现形式就是在某一个指定空间坐标系下，点云形状不发生改变的情况下，位置发生了变换，一般是指平移，旋转。而点云刚体变换的实现本质就是点云每个点的坐标都发生了改变，即乘以变换矩阵，得到了新的坐标值。这是从点云变换来理解的，另一个角度就是原始点云的坐标系做了变换，表现来看点云就有所变化。

1.点云旋转变换

点云旋转一般都需要指定其绕某个轴（或者说是向量）旋转了某个角度。这种情况是需要已知轴和角度值的。

代码：

/// <summary>
/// 点云绕某个轴（向量）旋转某个角度
/// </summary>
/// <param name="cloud">输入点云</param>
/// <param name="cloudout">输出旋转后的点云</param>
/// <param name="axis">轴（向量）</param>
/// <param name="angle">旋转角度</param>
void AxisAngleTransformEigen(const pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud, pcl::PointCloud<pcl::PointXYZ>::Ptr& cloudout, 
	const Eigen::Vector3d& axis, const double& angle)
{
	Eigen::Affine3d transform = Eigen::Affine3d::Identity();
	transform.rotate(Eigen::AngleAxisd(angle, axis));
	pcl::transformPointCloud(*cloud, *cloudout, transform);
}

对于数量级较大的点云，上述方法进行点云变换是非常慢。

2.点云旋转变换（OPENMP加速）

使用OPENMP对点云变换加速，会省时一些。

/// <summary>
/// 使用OPENMP加速，点云绕某个轴选择指定角度
/// </summary>
/// <param name="cloud">输入点云</param>
/// <param name="cloudout">输出旋转后的点云</param>
/// <param name="axis">轴（向量）</param>
/// <param name="angle">旋转角度</param>
void AxisAngleTransformEigenOMP(const pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud, pcl::PointCloud<pcl::PointXYZ>::Ptr& cloudout,
	const Eigen::Vector3d& axis, const double& angle)
{
	Eigen::Affine3d transform = Eigen::Affine3d::Identity();
	transform.rotate(Eigen::AngleAxisd(angle, axis));
	double* matrixPtr = transform.matrix().data();

	cloudout->points.resize(cloud->size());
	omp_set_num_threads(12);
#pragma omp parallel for
	for (int i = 0; i < cloud->size(); ++i)
	{
		(*cloudout)[i].x = matrixPtr[0] * (*cloud)[i].x + matrixPtr[1] * (*cloud)[i].y +
			matrixPtr[2] * (*cloud)[i].z + matrixPtr[3];
		(*cloudout)[i].y = matrixPtr[4] * (*cloud)[i].x + matrixPtr[5] * (*cloud)[i].y +
			matrixPtr[6] * (*cloud)[i].z + matrixPtr[7];
		(*cloudout)[i].z = matrixPtr[8] * (*cloud)[i].x + matrixPtr[9] * (*cloud)[i].y +
			matrixPtr[10] * (*cloud)[i].z + matrixPtr[11];
	}
}

3.求解旋转轴和旋转角度

上述两种方案都是已知旋转轴和旋转角度来求解旋转之后的点云。现在假如已知旋转之后和旋转之前的点云法向都是已知的，反向求解点云需要绕哪个轴旋转多少角度才能得到。应用场景：点云是个平面，但不平行于XOY平面或者其他。现在想要将其旋转至与XOY平面平行，即与Z轴垂直，那么可以使用该方法把旋转轴和旋转角度求出来，然后使用上述的1或者2将点云进行变换。这种方法的原理和实现来自这里。

/// <summary>
/// 求解出轴axis和角度theta，使得点云绕轴axis旋转theta之后，其法向由CloudNormal转变成Vec
/// </summary>
/// <param name="Vec">点云旋转之后的法向</param>
/// <param name="CloudNormal">点云旋转之前的法向</param>
/// <param name="theta">旋转角度</param>
/// <param name="axis">旋转轴</param>
void CaculateAxisAngle(const Eigen::Vector3d& Vec, const Eigen::Vector3d& CloudNormal, double& theta, Eigen::Vector3d& axis)
{
	Eigen::Vector3d CloudNormalTemp = CloudNormal;
	CloudNormalTemp.normalize();
	double dotmul = CloudNormalTemp.dot(Vec);
	double NormalNorm = CloudNormalTemp.norm();
	double VecNorm = Vec.norm();
	theta = std::acos(dotmul / (std::sqrt(NormalNorm) * std::sqrt(VecNorm)));
	axis = Vec.cross(CloudNormalTemp);
	axis.normalize();
}

测试效果如下，白色为原始点云，红色为旋转后的点云：

4.使用CUDA进行变换

之前提到，点云刚体变换其实就是向量与矩阵的乘法。向量自然就是x，y，z。而矩阵就是刚体变换矩阵。所以很容易就可以写一个计算核函数

核函数写在kernel.cu文件下，核函数代码如下：

/// <summary>
/// 点云刚体变换核函数
/// </summary>
/// <param name="A">存放点云所有点x坐标值的指针</param>
/// <param name="B">存放点云所有点y坐标值的指针</param>
/// <param name="C">存放点云所有点z坐标值的指针</param>
/// <param name="D">存放刚体变换矩阵的指针</param>
/// <param name="N">点云的梳理</param>
/// <returns></returns>
__global__ void cloudTransformKernel(float* A, float* B, float* C, double* D, int N) 
{
    int tid = blockIdx.x * blockDim.x + threadIdx.x;

    if (tid < N) {
        float x = A[tid];
        float y = B[tid];
        float z = C[tid];

        // 矩阵乘法计算
        A[tid] = D[0] * x + D[1] * y + D[2] * z + D[3];
        B[tid] = D[4] * x + D[5] * y + D[6] * z + D[7];
        C[tid] = D[8] * x + D[9] * y + D[10] * z + D[11];
    }
}

自然还需要一个调用核函数的函数，也是在kernel.cu中实现，该函数是供在其他需要使用cuda的cpp代码中使用调用的。代码如下：

/// <summary>
/// 完成线程资源数量分配，调用核函数完成点云刚体变换
/// </summary>
/// <param name="A">存放点云所有点x坐标值的指针</param>
/// <param name="B">存放点云所有点y坐标值的指针</param>
/// <param name="C">存放点云所有点z坐标值的指针</param>
/// <param name="D">存放刚体变换矩阵的指针</param>
/// <param name="N">点云的梳理</param>
/// <returns></returns>
void doCloudTransform(float* A, float* B, float* C, double* D, int N)
{
    int threadsPerBlock = 512;  // 每个块中的线程数量
    int blocksPerGrid = (N + threadsPerBlock - 1) / threadsPerBlock;  // 网格中的块数量

    cloudTransformKernel << <blocksPerGrid, threadsPerBlock >> > (A, B, C, D, N);
}

这个函数的声明需要在kernel.h文件中。

#pragma once
#ifndef KERNEL_CUH_
#define KERNEL_CUH_

void matrixMultiplication(float* A, float* B, float* C, int N);

void doCloudTransform(float* A, float* B, float* C, double* D, int N);

#endif

在CPP文件中的函数中，当然要进一步封装，方便调用了，代码如下：

#include "dev_array.h"
#include "kernel.h"
/// <summary>
/// 使用CUDA进行点云刚体变换
/// </summary>
/// <param name="cloud">输入点云</param>
/// <param name="cloudout">输出点云</param>
/// <param name="axis">旋转轴</param>
/// <param name="angle">旋转角度</param>
void AxisAngleTransformCUDA(const pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud, pcl::PointCloud<pcl::PointXYZ>::Ptr& cloudout,
	const Eigen::Vector3d& axis, const double& angle)
{
	auto startTime = std::chrono::high_resolution_clock::now();  // 开始计时
	Eigen::Affine3d transform = Eigen::Affine3d::Identity();
	transform.rotate(Eigen::AngleAxisd(angle, axis));
	double* matrixPtr = transform.matrix().data();
	int size = cloud->points.size();
	dev_array<float> d_A(size);
	dev_array<float> d_B(size);
	dev_array<float> d_C(size);
	std::vector<float> h_A(size);
	std::vector<float> h_B(size);
	std::vector<float> h_C(size);
	omp_set_num_threads(24);
#pragma omp parallel for
	for (int i = 0; i < cloud->points.size(); ++i)
	{
		h_A[i] = cloud->points[i].x;
		h_B[i] = cloud->points[i].y;
		h_C[i] = cloud->points[i].z;
	}
	d_A.set(&h_A[0], size);
	d_B.set(&h_B[0], size);
	d_C.set(&h_C[0], size);
	dev_array<double> d_D(transform.matrix().size());
	d_D.set(matrixPtr, transform.matrix().size());

	auto endTime = std::chrono::high_resolution_clock::now();     // 结束计时
	std::chrono::duration<double> duration = endTime - startTime; // 计算运行时间
	double seconds = duration.count();
	std::cout << "CUDA CPU2GPU 运行时间: " << seconds << " 秒" << std::endl;
	
	startTime = std::chrono::high_resolution_clock::now();  // 开始计时
	doCloudTransform(d_A.getData(), d_B.getData(), d_C.getData(), d_D.getData(), size);
	cudaDeviceSynchronize();
	endTime = std::chrono::high_resolution_clock::now();     // 结束计时
	duration = endTime - startTime; // 计算运行时间
	seconds = duration.count();
	std::cout << "CUDA Caculate 运行时间: " << seconds << " 秒" << std::endl;

	startTime = std::chrono::high_resolution_clock::now();  // 开始计时
	d_A.get(&h_A[0], size);
	d_B.get(&h_B[0], size);
	d_C.get(&h_C[0], size);
	cudaDeviceSynchronize();
	cloudout->points.resize(size);
#pragma omp parallel for
	for (int i = 0; i < cloud->points.size(); ++i)
	{
		cloudout->points[i].x = h_A[i];
		cloudout->points[i].y = h_B[i];
		cloudout->points[i].z = h_C[i];
	}
	endTime = std::chrono::high_resolution_clock::now();     // 结束计时
	duration = endTime - startTime; // 计算运行时间
	seconds = duration.count();
	std::cout << "CUDA GPU2CPU 运行时间: " << seconds << " 秒" << std::endl;
}

上述代码用到的dev_array.h的实现如下：

#pragma once

#ifndef _DEV_ARRAY_H_
#define _DEV_ARRAY_H_

#include <stdexcept>
#include <algorithm>
#include <cuda_runtime.h>

template <class T>
class dev_array
{
    // public functions
public:
    explicit dev_array()
        : start_(0),
        end_(0)
    {}

    // constructor
    explicit dev_array(size_t size)
    {
        allocate(size);
    }
    // destructor
    ~dev_array()
    {
        free();
    }

    // resize the vector
    void resize(size_t size)
    {
        free();
        allocate(size);
    }

    // get the size of the array
    size_t getSize() const
    {
        return end_ - start_;
    }

    // get data
    const T* getData() const
    {
        return start_;
    }

    T* getData()
    {
        return start_;
    }

    // set
    void set(const T* src, size_t size)
    {
        size_t min = std::min(size, getSize());
        cudaError_t result = cudaMemcpy(start_, src, min * sizeof(T), cudaMemcpyHostToDevice);
        if (result != cudaSuccess)
        {
            throw std::runtime_error("failed to copy to device memory");
        }
    }
    // get
    void get(T* dest, size_t size)
    {
        size_t min = std::min(size, getSize());
        cudaError_t result = cudaMemcpy(dest, start_, min * sizeof(T), cudaMemcpyDeviceToHost);
        if (result != cudaSuccess)
        {
            throw std::runtime_error("failed to copy to host memory");
        }
    }


    // private functions
private:
    // allocate memory on the device
    void allocate(size_t size)
    {
        cudaError_t result = cudaMalloc((void**)&start_, size * sizeof(T));
        if (result != cudaSuccess)
        {
            start_ = end_ = 0;
            throw std::runtime_error("failed to allocate device memory");
        }
        end_ = start_ + size;
    }

    // free memory on the device
    void free()
    {
        if (start_ != 0)
        {
            cudaFree(start_);
            start_ = end_ = 0;
        }
    }

    T* start_;
    T* end_;
};

#endif

所以想要使用cuda进行点云处理，需要好几个步骤。首先需要在.cu实现一个核函数，这个核函数中完成点云计算。随后，需要在h文件中声明一个调用函数（主要是方便cpp文件中include），并且可以在实现核函数的.cu文件中实现这个调用函数。最后，在cpp文件中实现一个函数（可以在,h中声明），该函数完成数据从CPU到GPU的拷贝，调用函数的调用，数据从GPU到CPU的拷贝。

上述三种方法的运行时间测试结果如下，openmp版本的最快，其次是pcl中实现的transform，cuda版本的最慢。cuda之所以这么慢，是因为数据拷贝占用了太多时间，cuda的实际计算时间只有0.004s，还是非常快的。测试结果表明这类计算直接还是CPU处理吧。

5. 使用Thrust进行点云变换

CUDA中还有类似于STL vector的实现——Thrust。正常情况下可以将其当作vector那样使用，用法区别不大。Thrust中有thrust::device_vector和thrust::host_vector两种容器，分别存储GPU和CPU上的数据。可以使用transform来实现点云变换，这就需要实现一个仿函数。这种仿函数除了点云刚体变换之外，当然也可以进行其他的数学运算。想要进行任何形式的运算只需要在仿函数进行实现，然后使用transform调用仿函数即可。

仿函数如下：

struct PointCloudTransformFunctor
{
    double* transform;
   
    __host__ __device__ PointCloudTransformFunctor(double* transform)
        : transform(transform) {}

    __host__ __device__ float3 operator()(const float3& pt) const

    {
        float3 transformed_pt;
        
        transformed_pt.x = transform[0] * pt.x + transform[1] * pt.y +
            transform[2] * pt.z + transform[3];
        transformed_pt.y = transform[4] * pt.x + transform[5] * pt.y +
            transform[6] * pt.z + transform[7];
        transformed_pt.z = transform[8] * pt.x + transform[9] * pt.y +
            transform[10] * pt.z + transform[11];
        

        return transformed_pt;
    }
};

使用transform和仿函数完成点云刚体变换，这里使用transform，作用在thrust::device_vector，不是thrust::host_vector。所以一般在host上的数据都需要拷贝到device上，然后再进行想要实现的运算：

void CloudTransformThrustCore(thrust::device_vector<float3>& cloudin, thrust::device_vector<float3>& cloudout, double* transform)
{
    thrust::transform(cloudin.begin(), cloudin.end(), cloudout.begin(),
        PointCloudTransformFunctor(transform)
    );
}


void CloudTransformThrust(std::vector<float3>& cloudin, std::vector<float3>& cloudout, double* transform)
{
    thrust::device_vector<float3> orig_points(cloudin.begin(), cloudin.end());
    float3 temp;
    temp.x = temp.y = temp.z = 0.0;
    thrust::device_vector<float3> transformed_points(cloudin.size(), temp);
    CloudTransformThrustCore(orig_points, transformed_points, transform);
    thrust::host_vector<float3> temp_vector = transformed_points;
    cloudout.assign(temp_vector.begin(), temp_vector.end());
}

上述仿函数和transform函数都在.cu文件中实现。那么在CPP文件中调用还可以完成一层封装，输入输出都是PCL数据结构的点云。

封装的代码如下：

/// <summary>
/// 使用Thrust对点云进行刚体变换
/// </summary>
/// <param name="cloud">输入点云</param>
/// <param name="cloudout">输出点云</param>
/// <param name="axis">旋转轴</param>
/// <param name="angle">旋转角度</param>
/// <returns></returns>
bool TransformPointCloud(const pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud, pcl::PointCloud<pcl::PointXYZ>::Ptr& cloudout, const Eigen::Vector3d& axis, const double& angle)
{
	Eigen::Affine3d transform = Eigen::Affine3d::Identity();
	transform.rotate(Eigen::AngleAxisd(angle, axis));
	double* matrixPtr = transform.matrix().data();

	std::vector<float3> hostWrapper(cloud->points.size());
	for (int i = 0; i < cloud->points.size(); ++i)
	{
		hostWrapper[i].x = cloud->points[i].x;
		hostWrapper[i].y = cloud->points[i].y;
		hostWrapper[i].z = cloud->points[i].z;
	}
	
	std::vector<float3> hostWrapperOut;
	dev_array<double> d_D(transform.matrix().size());
	d_D.set(matrixPtr, transform.matrix().size());
	CloudTransformThrust(hostWrapper, hostWrapperOut, d_D.getData());
	
	cloudout->points.resize(hostWrapperOut.size());

	for (int i = 0; i < cloudout->points.size(); ++i)
	{
		cloudout->points[i].x = hostWrapperOut[i].x;
		cloudout->points[i].y = hostWrapperOut[i].y;
		cloudout->points[i].z = hostWrapperOut[i].z;
	}

	return true;
};

运行时间如下，该方法也没有比自己手动写的核函数实现方法快很多，条条大路通罗马，这只是一条路：

有关于Thrust的官方教程网址，非常详细：
教程1
教程2