参考博文：
NDT 公式推导及源码解析（1）
NDT点云配准算法原理及PCL实现
原始论文：
【1】Scan Registration for Autonomous Mining Vehicles Using 3D-NDT
【2】The Three-Dimensional Normal-Distributions Transform — an Efficient Representation for Registration, Surface Analysis, and Loop Detection
该算法的核心流程如下截图：
LZ用C++和PCL库简单实现了一下（未使用PCL中ndt算法的api）。发现该算法在点云初始位姿较好时可以精调优化位姿（怎么感觉和LZ之前了解的ndt算法一般用于粗配准不太一样？），初始位姿较差时难以收敛。下面的代码写的比较粗糙，上图原理基本上都实现了。不过只实现了第一次迭代，后续迭代过程LZ看了PCL中的ndt.hpp源码，需要使用高斯牛顿法求最小值，其中牵涉到了梯度下降方向，步长等问题（可见ComputeStepLengthMt函数），在源代码里也提到了论文名称，原理比较复杂就没有继续深究了。
#include <iostream>
#include <Eigen/Eigenvalues>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/common/common.h>
#include <pcl/common/transforms.h>
#include <pcl/search/kdtree.h>
int main(int argc, char** argv)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr target_cloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::io::loadPCDFile("room_scan1.pcd", *target_cloud);
pcl::PointCloud<pcl::PointXYZ>::Ptr source_cloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::io::loadPCDFile("room_scan2.pcd", *source_cloud);
pcl::PointCloud<pcl::PointXYZ>::Ptr source_cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>);
pcl::VoxelGrid<pcl::PointXYZ> voxel_filter;
voxel_filter.setLeafSize(0.2f, 0.2f, 0.2f);
voxel_filter.setInputCloud(source_cloud);
voxel_filter.filter(*source_cloud_filtered);
pcl::PointCloud<pcl::PointXYZ>::Ptr source_cloud_copy(new pcl::PointCloud<pcl::PointXYZ>);
pcl::copyPointCloud(*source_cloud_filtered, *source_cloud_copy);
pcl::search::KdTree<pcl::PointXYZ>::Ptr kdtree(new pcl::search::KdTree<pcl::PointXYZ>);
kdtree->setInputCloud(target_cloud);
double outlier_ratio = 0.55f;
double epsilon = 0.1f;
double resolution = 1.0f;
int max_iterations = 1;
//公式（6.8）
double c1 = 10.0f * (1 - outlier_ratio);
double c2 = outlier_ratio / pow(resolution, 3);
double d3 = -std::log(c2);
double d1 = -std::log(c1 + c2) - d3;
double d2 = -2 * std::log((-std::log(c1 * std::exp(-0.5) + c2) - d3) / d1);
//构建栅格结构
pcl::PointXYZ minPt, maxPt;
pcl::getMinMax3D(*target_cloud, minPt, maxPt);
double lx = maxPt.x - minPt.x;
double ly = maxPt.y - minPt.y;
double lz = maxPt.z - minPt.z;
int nx = lx / resolution + 1;
int ny = ly / resolution + 1;
int nz = lz / resolution + 1;
struct cell
{
pcl::PointCloud<pcl::PointXYZ> points;
Eigen::Vector4d center;
Eigen::Matrix3d sigma;
};
//将点云各点分配到栅格中
std::vector<cell> B(nx*ny*nz);
for (size_t i = 0; i < target_cloud->points.size(); i++)
{
int ix = (target_cloud->points[i].x - minPt.x) / resolution;
int iy = (target_cloud->points[i].y - minPt.y) / resolution;
int iz = (target_cloud->points[i].z - minPt.z) / resolution;
int id = ix + iy * nx + iz * nx * ny;
B[id].points.push_back(target_cloud->points[i]);
}
//计算栅格内各点的均值和协方差
for (size_t i = 0; i < B.size(); i++)
{
Eigen::Vector4d voxel_centroid;
Eigen::MatrixXd voxel_demean;
if (B[i].points.size() >= 5)
{
pcl::compute3DCentroid(B[i].points, voxel_centroid);
pcl::demeanPointCloud(B[i].points, voxel_centroid, voxel_demean);
Eigen::Matrix3d sigma = (voxel_demean * voxel_demean.transpose()).topLeftCorner(3, 3) / (B[i].points.size() - 1);
Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> eigen_solver(sigma);
Eigen::Vector3d eigenvalues = eigen_solver.eigenvalues();
Eigen::Matrix3d eigenvectors = eigen_solver.eigenvectors();
double lamda1 = eigenvalues(0), lamda2 = eigenvalues(1), lamda3 = eigenvalues(2);
if (lamda3 > 100 * lamda1 || lamda3 > 100 * lamda2)
{
lamda1 = lamda3 / 100;
lamda2 = lamda3 / 100;
Eigen::DiagonalMatrix<double, 3> A(lamda1, lamda2, lamda3);
eigenvectors.col(0).swap(eigenvectors.col(2));
sigma = eigenvectors.transpose() * A * eigenvectors;
}
B[i].center = voxel_centroid;
B[i].sigma = sigma;
}
}
//设定初始值
Eigen::AngleAxisd init_rotation(0.6931, Eigen::Vector3d::UnitZ());
Eigen::Translation3d init_translation(1.79387, 0, 0);
Eigen::Matrix4d transformation_matrix = (init_translation * init_rotation).matrix();
Eigen::Matrix3d R = transformation_matrix.topLeftCorner(3, 3);
Eigen::Matrix<double, 3, 1> T = transformation_matrix.topRightCorner(3, 1);
Eigen::Transform<double, 3, Eigen::Affine, Eigen::ColMajor> transformation;
transformation.matrix() = transformation_matrix;
Eigen::Vector3d translation = transformation.translation();
Eigen::Vector3d rotation = transformation.rotation().eulerAngles(0, 1, 2);
double x = rotation(0), y = rotation(1), z = rotation(2);
double tx = translation(0), ty = translation(1), tz = translation(2);
//点云去中心化
Eigen::Vector4d source_cloud_filtered_centroid;
Eigen::MatrixXd source_cloud_filtered_demean;
pcl::compute3DCentroid(*source_cloud_filtered, source_cloud_filtered_centroid);
pcl::demeanPointCloud(*source_cloud_filtered, source_cloud_filtered_centroid, source_cloud_filtered_demean);
source_cloud_filtered_demean = source_cloud_filtered_demean.topLeftCorner(3, source_cloud_filtered_demean.cols());
//迭代求解位姿
int iters = 0;
while (iters < max_iterations)
{
double score = 0.0;
Eigen::Matrix<double, 1, 6> gradient; //梯度向量
gradient.setZero();
Eigen::Matrix<double, 6, 6> hessian; //海塞矩阵
hessian.setZero();
for (size_t k = 0; k < source_cloud_filtered->size(); k++) //遍历点云各点
{
std::cout << k << "/" << source_cloud_filtered->size() << std::endl;
double x1 = source_cloud_filtered->points[k].x;
double x2 = source_cloud_filtered->points[k].y;
double x3 = source_cloud_filtered->points[k].z;
Eigen::Matrix<double, 3, 1> X;
X << x1, x2, x3;
Eigen::Matrix<double, 3, 1> X_trans;
X_trans = R * X + T;
std::vector<int> indices;
std::vector<float> distances;
pcl::PointXYZ p(X_trans(0, 0), X_trans(1, 0), X_trans(2, 0));
int index = -1;
//kdtree->nearestKSearch(p, 1, index, distance);
if (kdtree->radiusSearch(p, resolution, indices, distances, 1) > 0)
index = indices[0];
else
continue;
int ix = (target_cloud->points[index].x - minPt.x) / resolution;
int iy = (target_cloud->points[index].y - minPt.y) / resolution;
int iz = (target_cloud->points[index].z - minPt.z) / resolution;
int id = ix + iy * nx + iz * nx * ny;
if (B[id].points.size() < 5)
continue;
Eigen::Matrix3d sigma = B[id].sigma;
Eigen::Vector4d center = B[id].center;
Eigen::Matrix<double, 3, 1> uk;
uk << center(0), center(1), center(2);
Eigen::Matrix<double, 3, 1> X_;
X_ = X_trans - uk;
//double pxk = -d1 * exp((-d2 / 2 * source_cloud_filtered_demean.transpose() * sigma.inverse() * source_cloud_filtered_demean)(0, 0)); //公式（6.9）
//score += pxk;
//std::cout << score << std::endl;
//公式（6.19）
double a = x1 * (-sin(x) * sin(z) + cos(x) * sin(y) * cos(z)) + x2 * (-sin(x) * cos(z) - cos(x) * sin(y) * sin(z)) + x3 * (-cos(x) * cos(y));
double b = x1 * (cos(x) * sin(z) + sin(x) * sin(y) * cos(z)) + x2 * (-sin(x) * sin(y)* sin(z) + cos(x) * cos(z)) + x3 * (-sin(x) * cos(y));
double c = x1 * (-sin(y) * cos(z)) + x2 * (sin(y) * sin(z)) + x3 * (cos(y));
double d = x1 * (sin(x) * cos(y) * cos(z)) + x2 * (-sin(x) * cos(y) * sin(z)) + x3 * (sin(x) * sin(y));
double e = x1 * (-cos(x) * cos(y) * cos(z)) + x2 * (cos(x) * cos(y) * sin(z)) + x3 * (-cos(x) * sin(y));
double f = x1 * (-cos(y) * sin(z)) + x2 * (-cos(y) * cos(z));
double g = x1 * (cos(x) * cos(z) - sin(x) * sin(y) * sin(z)) + x2 * (-cos(x) * sin(z) - sin(x) * sin(y) * cos(z));
double h = x1 * (sin(x) * cos(z) + cos(x) * sin(y) * sin(z)) + x2 * (-sin(x) * sin(z) + cos(x) * sin(y) * cos(z));
Eigen::Matrix<double, 3, 6> J; //公式（6.18）
J << 1, 0, 0, 0, c, f,
0, 1, 0, a, d, g,
0, 0, 1, b, e, h;
gradient += d1 * d2 * X_.transpose() * sigma.inverse() * J * exp(-d2 / 2 * (X_.transpose() * sigma.inverse() * X_)(0, 0)); //公式（6.12）
//1*6 1*3 3*3 3*6
Eigen::Vector3d a_, b_, c_, d_, e_, f_; //公式（6.21）
a_ << 0,
x1 * (-cos(x) * sin(z) - sin(x) * sin(y) * cos(z)) + x2 * (-cos(x) * cos(z) + sin(x) * sin(y) * sin(z)) + x3 * (sin(x) * cos(y)),
x1 * (-sin(x) * sin(z) + cos(x) * sin(y) * cos(z)) + x2 * (-sin(x) * cos(z) - cos(x) * sin(y) * sin(z)) + x3 * (-cos(x) * cos(y));
b_ << 0,
x1 * (cos(x) * cos(y) * cos(z)) + x2 * (-cos(x) * cos(y) * sin(z)) + x3 * (cos(x) * sin(y)),
x1 * (sin(x) * cos(y) * cos(z)) + x2 * (-sin(x) * cos(y) * sin(z)) + x3 * (sin(x) * sin(y));
c_ << 0,
x1 * (-sin(x) * cos(z) - cos(x) * sin(y) * sin(z)) + x2 * (-sin(x) * sin(z) - cos(x) * sin(y) * cos(z)),
x1 * (cos(x) * cos(z) - sin(x) * sin(y) * sin(z)) + x2 * (-cos(x) * sin(z) - sin(x) * sin(y) * cos(z));
d_ <<x1 * (-cos(y) * cos(z)) + x2 * (cos(y) * sin(z)) + x3 * (-sin(y)),
x1 * (-sin(x) * sin(y) * cos(z)) + x2 * (sin(x) * sin(y) * sin(z)) + x3 * (sin(x) * cos(y)),
x1 * (cos(x) * sin(y) * cos(z)) + x2 * (-cos(x) * sin(y) * sin(z)) + x3 * (-cos(x) * cos(y));
e_ <<x1 * (sin(y)* sin(z)) + x2 * (sin(y)* cos(z)),
x1 * (-sin(x) * cos(y) * sin(z)) + x2 * (-sin(x) * cos(y) * cos(z)),
x1* (cos(x) * cos(y) * sin(z)) + x2 * (cos(x) * cos(y) * cos(z));
f_ <<x1 * (-cos(y)* cos(z)) + x2 * (cos(y)* sin(z)),
x1 * (-cos(x) * sin(z) - sin(x) * sin(y) * cos(z)) + x2 * (-cos(x) * cos(z) + sin(x) * sin(y) * sin(z)),
x1 * (-sin(x) * sin(z) + cos(x) * sin(y) * cos(z)) + x2 * (-sin(x) * cos(z) - cos(x) * sin(y) * sin(z));
Eigen::Matrix<double, 18, 6> H; //公式（6.20）
H.setZero();
H.block<3, 1>(9, 3) = a_;
H.block<3, 1>(12, 3) = b_;
H.block<3, 1>(15, 3) = c_;
H.block<3, 1>(9, 4) = b_;
H.block<3, 1>(12, 4) = d_;
H.block<3, 1>(15, 4) = e_;
H.block<3, 1>(9, 5) = c_;
H.block<3, 1>(12, 5) = e_;
H.block<3, 1>(15, 5) = f_;
for (size_t i = 0; i < hessian.rows(); i++)
{
for (size_t j = 0; j < hessian.cols(); j++)
{
//-d2 / 2 * X_.transpose() * sigma.inverse() * X_ //1*3 3*3 3*1
//-d2 * (X_.transpose()* sigma.inverse()* J.col(j)); //1*3 3*3 3*1
//X_.transpose()* sigma.inverse()* H.block<3, 1>(3 * i, j); //1*3 3*3 3*1
//(J.col(j)).transpose()* sigma.inverse()* J.col(i); //1*3 3*3 3*1
hessian(i, j) += d1 * d2 * exp((-d2 / 2 * X_.transpose() * sigma.inverse() * X_)(0, 0))
* (-d2 * (X_.transpose() * sigma.inverse() * J.col(i))(0, 0)* (X_.transpose()* sigma.inverse()* J.col(j))(0, 0)
+ (X_.transpose() * sigma.inverse()* H.block<3, 1>(3 * i, j))(0, 0) + (J.col(j).transpose() * sigma.inverse()*J.col(i))(0, 0)); //公式（6.13）
}
}
}
Eigen::Matrix<double, 6, 1> delta = - hessian.inverse() * gradient.transpose();
tx += delta(0, 0);
ty += delta(1, 0);
tz += delta(2, 0);
x += delta(3, 0);
y += delta(4, 0);
z += delta(5, 0);
R << cos(y)* cos(z), -cos(y)* sin(z), sin(y),
cos(x)* sin(z) + sin(x)* sin(y)* cos(z), cos(x)* cos(z) - sin(x)* sin(y)* sin(z), -sin(x)* cos(y),
sin(x)* sin(z) - cos(x)* sin(y)* cos(z), sin(x)* cos(z) + cos(x)* sin(y)* sin(z), cos(x)* cos(y);
T << tx, ty, tz;
transformation_matrix << R, T, 0, 0, 0, 1; //公式（6.17）
pcl::transformPointCloud(*source_cloud_filtered, *source_cloud_copy, transformation_matrix);
iters++;
std::cout << transformation_matrix << std::endl;
}
pcl::io::savePCDFile("icp_cloud.pcd", *source_cloud_copy);
return 0;
}
这个算法实现下来感觉还是有些复杂的，主要是公式推导比较多，论文中的求解过程还是比较清晰的。LZ水平有限，如果实现过程存在问题欢迎大家批评指正。