NDTアルゴリズムの実装

開発 2023-05-18 02:37:45 訪問数: null

参考ブログ記事:
NDT 式の導出とソースコード分析 (1)
NDT 点群登録アルゴリズムの原理と PCL 実装
元論文:
[1] 3D-NDT を使用した自律型鉱山車両のスキャン登録
[2]三次元正規分布変換— レジストレーション、表面解析、ループ検出の効率的な表現

アルゴリズムのコア プロセスは、次のスクリーンショットに示されています。
ここに画像の説明を挿入
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 のレベルは限られており、実装プロセスに問題がある場合は、誰でも批判して修正することができます。

おすすめ

転載: blog.csdn.net/taifyang/article/details/129781822
おすすめ
GitHub のホットリストに急上昇 — オープンソースのプログラミング言語とフレームワークは、どうしてこんなにも魅力的なのでしょうか?
北京ヒューマノイドロボットイノベーションセンターが、擬人化走行用の純粋な電気駆動を備えた世界初のフルサイズヒューマノイドロボット「Tiangong」を発売
ランキング
アリクラウドサーバ_ubuntu ECSシステムディスク拡張システム_
ボタンをクリックしてクリップボードにコピーします
脅威ハンティングを使用してサイバー セキュリティを強化する
ライン画像やコンテナnginxのの建設
Python での try の使用について書く
リアルタイムのデータウェアハウスを構築する方法高速データ時代、MySQLのAnalyticDBはあなたを教えて
Docker でのネットワークおよびデータ管理 (Docker Swarm クラスターおよび Docker データ管理を含む)
Java ループの比較: 従来の for ループ、拡張された for ループ、および forEach ループ
浙江大学版「C言語プログラミング（第3版）」演習2-18
Gestion du temps RTOS - Utilisation de la fonction de crochet SysTick pour réaliser l'échantillonnage de synchronisation et les sous-sections de ce chapitre
アーカイブ
もっと
2024-04-27(29)
2024-04-26(22)
2024-04-25(31)
2024-04-24(30)
2024-04-23(31)
2024-04-22(5)
2024-04-21(0)
2024-04-20(6)
2024-04-19(5)
2024-04-18(0)

コードワールド

© 2018 codetd.com