ICP算法手写实现（C++和python）

用C++实现的ICP（Iterative Closest Point，迭代最近点）算法，借助了PCL库实现点云基本变换、KD-tree以及可视化部分，核心迭代部分没有调用PCL的api。代码在KD-tree搜索部分采用了openmp加速优化，在适当的数据集下运行速度能超过PCL自带的ICP算法配准api。

ICP算法的原理和推导，网上有很多，在此不赘述。下面链接是我觉得讲的比较详细的，可以参考：
三维点云配准 – ICP 算法原理及推导
ICP算法公式推导和PCL源码解析

另外，本文的代码实现过程还参考了：
迭代最近点 ICP 详细推导（C++实现）（借助Eigen库）
C++实现ICP点云匹配（借助OpenCV库和Eigen库）

废话不多说，下面贴代码。

myicp.h

/******************************************************************************** 
** @Copyright(c) $year$ $registered organization$ All Rights Reserved.
** @auth： taify
** @date： 2021/01/12
** @desc： myicp头文件
** @Ver : V1.0.0
*********************************************************************************/

#ifndef MYICP_H_
#define MYICP_H_

#include <iostream>
#include <fstream>
#include <ctime>
#include <cmath>
#include <string>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/common/transforms.h>
#include <pcl/search/kdtree.h> 
#include <pcl/correspondence.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <omp.h>

/**
 * @brief The MyICP class
 */
class MyICP
{
public:
    MyICP();

	~MyICP();

    /**
     * @brief setSourceCloud 设置输入点云
     * @param cloud 输入点云
     */
	void setSourceCloud(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud);

    /**
     * @brief setTargetCloud 设置目标点云
     * @param cloud 目标点云
     */
	void setTargetCloud(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud);

    /**
     * @brief setLeafSize 设置体素滤波网格尺寸
     * @param size 体素滤波网格尺寸
     */
	void setLeafSize(float size);

    /**
     * @brief setMinError 设置最小误差
     * @param error 最小误差
     */
	void setMinError(float error);

    /**
     * @brief setMaxIters 设置最大迭代次数
     * @param iters 最大迭代次数
     */
	void setMaxIters(int iters);

    /**
     * @brief setEpsilon 设置配准误差
     * @param eps 配准误差
     */
	void setEpsilon(float eps);

    /**
     * @brief downsample 下采样
     */
	void downsample();

    /**
     * @brief registration 配准
     */
	void registration();

    /**
     * @brief saveICPCloud 保存配准得到的点云
     * @param filename 点云文件名
     */
	void saveICPCloud(const std::string filename);

    /**
     * @brief getTransformationMatrix 得到变换矩阵
     */
	void getTransformationMatrix();

    /**
     * @brief getScore 得到配准得分
     */
	void getScore();

    /**
     * @brief visualize 配准结果可视化（输入点云为绿色，目标点云为红色，配准点云为蓝色）
     */
	void visualize();

private:
    /**
     * @brief source_cloud 输入点云
     */
	pcl::PointCloud<pcl::PointXYZ>::Ptr source_cloud;

    /**
     * @brief target_cloud 目标点云
     */
	pcl::PointCloud<pcl::PointXYZ>::Ptr target_cloud;

    /**
     * @brief source_cloud_downsampled 输入点云下采样得到的点云
     */
	pcl::PointCloud<pcl::PointXYZ>::Ptr source_cloud_downsampled;

    /**
     * @brief target_cloud_downsampled 目标点云下采样得到的点云
     */
	pcl::PointCloud<pcl::PointXYZ>::Ptr target_cloud_downsampled;

    /**
     * @brief icp_cloud 配准得到的点云
     */
	pcl::PointCloud<pcl::PointXYZ>::Ptr icp_cloud;

    /**
     * @brief leaf_size 体素网格尺寸
     */
	float leaf_size;

     /**
      * @brief min_error 最小误差
      */
	float min_error;

    /**
     * @brief max_iters 最大迭代次数
     */
	int max_iters;

    /**
     * @brief epsilon 配准误差
     */
	float epsilon;

    /**
     * @brief transformation_matrix 变换矩阵
     */
	Eigen::Matrix4f transformation_matrix = Eigen::Matrix4f::Identity();
};

#endif // MYICP_H

myicp_helpers.h

/********************************************************************************
** @Copyright(c) $year$ $registered organization$ All Rights Reserved.
** @auth： taify
** @date： 2021/01/12
** @desc： myicp_helpers头文件
** @Ver : V1.0.0
*********************************************************************************/

#ifndef MYICP_HELPERS_H_
#define MYICP_HELPERS_H_

#include <pcl/point_types.h>
#include <pcl/common/transforms.h>
#include <pcl/search/kdtree.h> 

/**
 * @brief calNearestPointPairs 计算最邻近点对
 * @param H 变换矩阵
 * @param source_cloud 源点云
 * @param target_cloud 目标点云
 * @param target_cloud_mid 中间点云
 * @param kdtree kd树
 * @param error 误差
 */
void calNearestPointPairs(Eigen::Matrix4f H, pcl::PointCloud<pcl::PointXYZ>::Ptr& source_cloud, const pcl::PointCloud<pcl::PointXYZ>::Ptr& target_cloud,
	pcl::PointCloud<pcl::PointXYZ>::Ptr& target_cloud_mid, pcl::search::KdTree<pcl::PointXYZ>::Ptr kdtree, double &error);

#endif // MYICP_HELPERS_H_

myicp.cpp

/******************************************************************************** 
** @Copyright(c) $year$ $registered organization$ All Rights Reserved.
** @auth： taify
** @date： 2021/01/12
** @desc： myicp源文件
** @Ver : V1.0.0
*********************************************************************************/

#include "myicp.h"
#include "myicp_helpers.h"

MyICP::MyICP()
{

}

MyICP::~MyICP()
{

}

void MyICP::setSourceCloud(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud) 
{
	source_cloud = cloud;
}

void MyICP::setTargetCloud(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud)
{
	target_cloud = cloud;
}

void MyICP::setLeafSize(float size)
{
	leaf_size = size;
}

void MyICP::setMinError(float error)
{
	min_error = error;
}

void MyICP::setMaxIters(int iters)
{
	max_iters = iters;
}

void MyICP::setEpsilon(float eps)
{
	epsilon = eps;
}

void MyICP::downsample()
{
	pcl::VoxelGrid<pcl::PointXYZ> voxel_grid;
	voxel_grid.setLeafSize(leaf_size, leaf_size, leaf_size);
	voxel_grid.setInputCloud(source_cloud);
	source_cloud_downsampled.reset(new pcl::PointCloud<pcl::PointXYZ>);
	voxel_grid.filter(*source_cloud_downsampled);
	voxel_grid.setInputCloud(target_cloud);
	target_cloud_downsampled.reset(new pcl::PointCloud<pcl::PointXYZ>);
	voxel_grid.filter(*target_cloud_downsampled);
	std::cout << "down size *cloud_src_o from " << source_cloud->size() << " to " << source_cloud_downsampled->size() << endl;
	std::cout << "down size *cloud_tgt_o from " << target_cloud->size() << " to " << target_cloud_downsampled->size() << endl;
}

void MyICP::registration()
{
	std::cout << "icp registration start..." << std::endl;

	Eigen::Matrix3f R_12 = Eigen::Matrix3f::Identity();
	Eigen::Vector3f T_12 = Eigen::Vector3f::Zero();
	Eigen::Matrix4f H_12 = Eigen::Matrix4f::Identity();

	pcl::PointCloud<pcl::PointXYZ>::Ptr target_cloud_mid(new pcl::PointCloud<pcl::PointXYZ>());

	//建立kd树
	pcl::search::KdTree<pcl::PointXYZ>::Ptr kdtree(new pcl::search::KdTree<pcl::PointXYZ>);
	kdtree->setInputCloud(target_cloud_downsampled);

	double error = INT_MAX, score = INT_MAX;
	Eigen::Matrix4f H_final = H_12;
	int iters = 0;

	//开始迭代，直到满足条件
	while (error > min_error && iters < max_iters)
	{
		iters++;
		double last_error = error;

		//计算最邻近点对
		calNearestPointPairs(H_12, source_cloud_downsampled, target_cloud_downsampled, target_cloud_mid, kdtree, error);

		if (last_error - error < epsilon)
			break;

		//计算点云中心坐标
		Eigen::Vector4f source_centroid, target_centroid_mid;
		pcl::compute3DCentroid(*source_cloud_downsampled, source_centroid);
		pcl::compute3DCentroid(*target_cloud_mid, target_centroid_mid);

		//去中心化
		Eigen::MatrixXf souce_cloud_demean, target_cloud_demean;
		pcl::demeanPointCloud(*source_cloud_downsampled, source_centroid, souce_cloud_demean);
		pcl::demeanPointCloud(*target_cloud_mid, target_centroid_mid, target_cloud_demean);

		//计算W=q1*q2^T
		Eigen::Matrix3f W = (souce_cloud_demean*target_cloud_demean.transpose()).topLeftCorner(3, 3);

		//SVD分解得到新的旋转矩阵和平移矩阵
		Eigen::JacobiSVD<Eigen::Matrix3f> svd(W, Eigen::ComputeFullU | Eigen::ComputeFullV);
		Eigen::Matrix3f U = svd.matrixU();
		Eigen::Matrix3f V = svd.matrixV();

		if (U.determinant()*V.determinant() < 0)
		{
			for (int x = 0; x < 3; ++x)
				V(x, 2) *= -1;
		}

		R_12 = V* U.transpose();
		T_12 = target_centroid_mid.head(3) - R_12*source_centroid.head(3);
		H_12 << R_12, T_12, 0, 0, 0, 1;
		H_final = H_12*H_final; //更新变换矩阵

		std::cout << "iters:"  << iters << "  "<< "error:" << error << std::endl;
	}
	transformation_matrix << H_final;
}

void MyICP::saveICPCloud(const std::string filename)
{
	icp_cloud.reset(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::transformPointCloud(*source_cloud, *icp_cloud, transformation_matrix); //点云变换
	pcl::io::savePCDFileBinary(filename, *icp_cloud);
}

void MyICP::getTransformationMatrix()
{
	std::cout << "transformation_matrix:" << std::endl << transformation_matrix << std::endl;
}

void MyICP::getScore()
{
	double fitness_score = 0.0;
	pcl::KdTreeFLANN <pcl::PointXYZ> kdtree;
	kdtree.setInputCloud(target_cloud);

#pragma omp parallel for reduction(+:fitness_score) //采用openmmp加速
	for (int i = 0; i < icp_cloud->points.size(); ++i)
	{
		std::vector<int> nn_indices(1);
		std::vector<float> nn_dists(1);
		kdtree.nearestKSearch(icp_cloud->points[i], 1, nn_indices, nn_dists);
		fitness_score += nn_dists[0];
	}

	std::cout << "score:" << std::endl << fitness_score / icp_cloud->points.size() << std::endl;
}

void MyICP::visualize()
{
	pcl::visualization::PCLVisualizer viewer("registration Viewer");
	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> src_h(source_cloud, 0, 255, 0); 	//原始点云绿色
	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> tgt_h(target_cloud, 255, 0, 0); 	//目标点云红色
	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> final_h(icp_cloud, 0, 0, 255); 	//匹配好的点云蓝色

	viewer.setBackgroundColor(255, 255, 255);
	viewer.addPointCloud(source_cloud, src_h, "source cloud");
	viewer.addPointCloud(target_cloud, tgt_h, "target cloud");
	viewer.addPointCloud(icp_cloud, final_h, "result cloud");

	while (!viewer.wasStopped())
	{
		viewer.spinOnce(100);
		boost::this_thread::sleep(boost::posix_time::microseconds(100000));
	}
}

myicp_helpers.cpp

/******************************************************************************** 
** @Copyright(c) $year$ $registered organization$ All Rights Reserved.
** @auth： taify
** @date： 2021/01/12
** @desc： myicp_helpers源文件
** @Ver : V1.0.0
*********************************************************************************/

#include "myicp_helpers.h"

void calNearestPointPairs(Eigen::Matrix4f H, pcl::PointCloud<pcl::PointXYZ>::Ptr& source_cloud, const pcl::PointCloud<pcl::PointXYZ>::Ptr& target_cloud,
	pcl::PointCloud<pcl::PointXYZ>::Ptr& target_cloud_mid, pcl::search::KdTree<pcl::PointXYZ>::Ptr kdtree, double &error)
{
	double err = 0.0;
	pcl::transformPointCloud(*source_cloud, *source_cloud, H);
	std::vector<int>indexs(source_cloud->size());

#pragma omp parallel for reduction(+:err) //采用openmmp加速
	for (int i = 0; i < source_cloud->size(); ++i)
	{
		std::vector<int>index(1);
		std::vector<float>distance(1);
		kdtree->nearestKSearch(source_cloud->points[i], 1, index, distance);
		err = err + sqrt(distance[0]);
		indexs[i] = index[0];
	}

	pcl::copyPointCloud(*target_cloud, indexs, *target_cloud_mid);
	error = err / source_cloud->size();
}

贴一张效果图：
完整工程文件见我的GitHub.
由于代码写的比较仓促，不恰当的地方还请指教。

python版本的ICP算法实现如下：

import numpy as np
import open3d as o3d
from scipy.spatial import KDTree


def translate(points, t):
    return points + t

def rotate(points, R):
    return np.dot(points, R.T)

def nearest_neighbor(src, dst):
    tree = KDTree(dst)
    distances, indices = tree.query(src, k=1)
    return indices

def compute_transform(src, dst):
    src_mean = np.mean(src, axis=0)
    dst_mean = np.mean(dst, axis=0) 
    src_centered = src - src_mean
    dst_centered = dst - dst_mean  
    H = np.dot(src_centered.T, dst_centered)
    U, _, Vt = np.linalg.svd(H)
    R = np.dot(Vt.T, U.T)   
    t = dst_mean - np.dot(R, src_mean)   
    return R, t

def icp(src, dst, max_iterations=100, tolerance=1e-6):
    src_points = np.copy(src)
    dst_points = np.copy(dst)  
    R_total = np.eye(src.shape[1])  
    t_total = np.zeros(src.shape[1])  
    prev_error = 0
    for i in range(max_iterations):
        indices = nearest_neighbor(src_points, dst_points)
        corresponding_points = dst_points[indices]
        R, t = compute_transform(src_points, corresponding_points)     
        src_points = rotate(src_points, R)
        src_points = translate(src_points, t)     
        R_total = np.dot(R, R_total)
        t_total = np.dot(R, t_total) + t     
        mean_error = np.mean(np.linalg.norm(corresponding_points - src_points, axis=1))
        print(f'Iteration {i+1}: Mean Error = {mean_error}')      
        if abs(prev_error - mean_error) < tolerance:
            break
        prev_error = mean_error 
    return R_total, t_total


if __name__ == "__main__":
    source = o3d.io.read_point_cloud("bunny1.pcd") 
    target = o3d.io.read_point_cloud("bunny2.pcd")   
    source_points = np.array(source.points)
    target_points = np.array(target.points)
    R_est, t_est = icp(source_points, target_points)
    icp_points = rotate(source_points, R_est)
    icp_points = translate(icp_points, t_est)
    print("\nEstimated Rotation Matrix:", R_est)
    print("\nEstimated Translation Vector:", t_est)
    result = o3d.geometry.PointCloud()
    result.points = o3d.utility.Vector3dVector(icp_points)
    o3d.visualization.draw_geometries([source, result, target])
    o3d.io.write_point_cloud("result.pcd", result)

后续文章更新：
ICP算法的三种实现方法（SVD分解、四元数、BA）
ICP算法加速优化-多线程和GPU
点到面的ICP算法
KinectFusion中的ICP算法
只用Eigen库实现的ICP算法