Reference blog post:
NDT formula derivation and source code analysis (1)
NDT point cloud registration algorithm principle and PCL implementation
Original paper:
[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
The core process of the algorithm is shown in the following screenshot:
LZ simply implemented it with C++ and PCL library (the api of the ndt algorithm in PCL was not used). It is found that the algorithm can fine-tune and optimize the pose when the initial pose of the point cloud is good (how does it feel that it is not the same as the ndt algorithm that LZ knew before for rough registration?), but it is difficult to converge when the initial pose is poor. The following code is relatively rough, and the principle of the above figure is basically realized. However, only the first iteration was implemented. In the subsequent iteration process, LZ looked at the ndt.hpp source code in PCL, and needed to use the Gauss-Newton method to find the minimum value, which involved issues such as gradient descent direction and step size (see the ComputeStepLengthMt function). The name of the paper is also mentioned in the source code, and the principle is more complicated, so I didn't go into it further.
#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;
}
The implementation of this algorithm feels a bit complicated, mainly because there are many formula derivations, and the solution process in the paper is relatively clear. The level of LZ is limited. If there are problems in the implementation process, everyone is welcome to criticize and correct.