一、算法原理

见：

点云配准精度评价指标——均方根误差
PCL 点云配准精度评价——点到面的均方根误差
Open3D(C++) 点云配准精度评价——点到点的均方根误差
Open3D(C++) 点云配准精度评价——点到面的均方根误差
Open3D 计算点云配准的精度和重叠度
matlab 点云配准——计算配准精度
经历诸多事，我眼中点云已有新意，今时今日，所书所写定然较他日不同，谨以此文献给学习点云配准的萌新。

二、代码实现

#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/PCLPointCloud2.h>
#include <pcl/common/distances.h>
#include <pcl/kdtree/kdtree_flann.h>

using namespace std;

#pragma region//计算配准的均方根误差
float caculateRMSE(pcl::PCLPointCloud2::Ptr& cloud_source, pcl::PCLPointCloud2::Ptr& cloud_target)
{
	pcl::PointCloud<pcl::PointXYZ>::Ptr xyz_source(new pcl::PointCloud<pcl::PointXYZ>());
	fromPCLPointCloud2(*cloud_source, *xyz_source);
	pcl::PointCloud<pcl::PointXYZ>::Ptr xyz_target(new pcl::PointCloud<pcl::PointXYZ>());
	fromPCLPointCloud2(*cloud_target, *xyz_target);

	float rmse = 0.0f;

	pcl::KdTreeFLANN<pcl::PointXYZ>::Ptr tree(new pcl::KdTreeFLANN<pcl::PointXYZ>());
	tree->setInputCloud(xyz_target);

	for (auto point_i : *xyz_source)
	{
		// 去除无效的点
		if (!pcl_isfinite(point_i.x) || !pcl_isfinite(point_i.y) || !pcl_isfinite(point_i.z))
			continue; 
		pcl::Indices nn_indices(1);
		std::vector<float> nn_distances(1);
		if (!tree->nearestKSearch(point_i, 1, nn_indices, nn_distances)) // K近邻搜索获取匹配点对
			continue;
		/*dist的计算方法之一
		size_t point_nn_i = nn_indices.front();
		float dist = squaredEuclideanDistance(point_i, xyz_target->points[point_nn_i]);
		*/

		float dist = nn_distances[0]; // 获取最近邻对应点之间欧氏距离的平方
		rmse += dist;                 // 计算平方距离之和
	}
	rmse = std::sqrt(rmse / static_cast<float> (xyz_source->points.size())); // 计算均方根误差

	return rmse;
}
#pragma endregion

int
main(int argc, char** argv)
{
    // ---------------------------读取源点云---------------------------------
    pcl::PCLPointCloud2::Ptr cloud_source(new pcl::PCLPointCloud2());
    if (pcl::io::loadPCDFile("data//1.pcd", *cloud_source) < 0)
    {
        PCL_ERROR("Could not read file\n");
        return (-1);
    }
    // --------------------------读取目标点云--------------------------------
    pcl::PCLPointCloud2::Ptr cloud_target(new pcl::PCLPointCloud2());

    if (pcl::io::loadPCDFile("data//2.pcd", *cloud_target) < 0)
    {
        PCL_ERROR("Could not read file\n");
        return (-1);
    }
   // ---------------------------计算均方根误差------------------------------
   auto Rmse= caculateRMSE(cloud_source, cloud_target);
   cout << "配准误差为：" << Rmse << endl;
    return 0;
}

三、代码解析

pcl::PointCloud<pcl::PointXYZ>::Ptr xyz_source(new pcl::PointCloud<pcl::PointXYZ>());
	fromPCLPointCloud2(*cloud_source, *xyz_source);

从读取的“模版”点云中获取xyz坐标，此时输入的原始点云已不再局限于pcl::PointXYZ格式或是其他。代码使用方法见：PCL 读取、保存点云

// 去除无效的点
		if (!pcl_isfinite(point_i.x) || !pcl_isfinite(point_i.y) || !pcl_isfinite(point_i.z))
			continue;

去除点云中无效的点。代码使用方法见：删除无效点

pcl::Indices nn_indices(1);
		std::vector<float> nn_distances(1);
		if (!tree->nearestKSearch(point_i, 1, nn_indices, nn_distances)) // K近邻搜索获取匹配点对
			continue;

K近邻搜索获取匹配点对，更多详细解析见：PCL KD树的使用、PCL KD-ICP实现点云精配准

		size_t point_nn_i = nn_indices.front();
		float dist = squaredEuclideanDistance(point_i, xyz_target->points[point_nn_i]);

计算最近邻匹配点对欧氏距离的平方。代码使用方法见：PCL 距离计算

float dist = nn_distances[0]; // 获取最近邻对应点之间欧氏距离的平方

另一种计算最近邻匹配点对欧氏距离平方的方法。为什么是这样，同样见：PCL KD树的使用、PCL KD-ICP实现点云精配准

rmse = std::sqrt(rmse / static_cast<float> (xyz_source->points.size())); // 计算均方根误差

计算均方根误差。

四、备注

最准确且最完美的方法仍然是点云配准精度评价指标——均方根误差一文中的，版本一。

点云侠

PCL （再探）点云配准精度评价指标——均方根误差

目录

一、算法原理

二、代码实现

三、代码解析

四、备注