Point Cloud Library學習之ICP迭代最近點匹配法NDT2D正態分佈轉換法
參考來源:
http://epsilonjohn.club/2020/02/29/Autoware.ai/2D-NDT-%E5%8C%B9%E9%85%8D%E7%AE%97%E6%B3%95/
NDT進行匹配時耗時較多,且角度誤差較大;
NDT引數不明確:對於我的情況GridStep一般設定0.6以上,預設1,GridExtent設定2或以上,預設值為20;
pcl::PointCloud<PointXYZ>::makeShared()函式可以得到已知點雲的指標
tictoc()函式可以得到執行時間;
icp.hasConverged()可以得到是否收斂;
注意匹配時的source和Target區分;
其中具體引數需要調整
#include <pcl/registration/icp.h>
#include <pcl/registration/ndt_2d.h>
#include <pcl/console/time.h>
#include <pcl/registration/sample_consensus_prerejective.h>
#include <pcl/features/brisk_2d.h>
typedef pcl::PointXYZ PointT;
typedef pcl::PointCloud<PointT> PointCloudT;
typedef pcl::BRISKSignature512 FeatureT;
typedef pcl::BRISK2DEstimation<pcl::PointXYZ> FeatureEstimationT;
typedef pcl::PointCloud<FeatureT> FeatureCloudT;
typedef pcl::PointWithScale KeyPointT;
typedef pcl::PointCloud<KeyPointT> KeyPointCloudT;
pcl::console::TicToc tt;
tt.tic ();
// // Initializing Normal Distributions Transform (NDT).
// pcl::NormalDistributionsTransform2D<pcl::PointXYZ, pcl::PointXYZ> ndt;
// // Setting scale dependent NDT parameters
// // Setting minimum transformation difference for termination condition.
// ndt.setTransformationEpsilon (NDT_EPSILON);
// // Setting maximum step size for More-Thuente line search.
// //Setting Resolution of NDT grid structure (VoxelGridCovariance).
// //ndt.setResolution (1.0);
// ndt.setGridStep (Eigen::Vector2f(NDT_GRID_STEP, NDT_GRID_STEP));
// ndt.setGridExtent (Eigen::Vector2f(NDT_GRID_EXTEND, NDT_GRID_EXTEND));
// // Setting max number of registration iterations.
// //ndt.setMaximumIterations (100);
// // Setting point cloud to be aligned.
// ndt.setInputSource (cloud_out.makeShared());
// // Setting point cloud to be aligned to.
// ndt.setInputTarget (cloud_in.makeShared());
// // Set initial alignment estimate found using robot odometry.
// Eigen::AngleAxisf init_rotation(initial_rad, Eigen::Vector3f::UnitZ ());
// Eigen::Translation3f init_translation (0, 0, 0);
// Eigen::Matrix4f init_guess = (init_translation * init_rotation).matrix ();
// // Calculating required rigid transform to align the input cloud to the target cloud.
// pcl::PointCloud<pcl::PointXYZ>::Ptr output_cloud (new pcl::PointCloud<pcl::PointXYZ>);
// ndt.align (*output_cloud, init_guess);
//icp
pcl::IterativeClosestPoint<PointT, PointT> icp;
PointCloudT::Ptr finalCloud(new PointCloudT);
icp.setInputSource(curr_keypoints.makeShared());
icp.setInputTarget(templates[id].pointcloud_data.makeShared());
icp.setMaximumIterations (ICP_MAX_ITERATIONS);
icp.setTransformationEpsilon (ICP_TRANSFORM_EPSILON);
icp.setMaxCorrespondenceDistance (ICP_MAX_CORRESPONDANCE_DISTANCE);
icp.setEuclideanFitnessEpsilon (ICP_EUCLIDEAN_FITNESS_EPSILON);
icp.setRANSACOutlierRejectionThreshold (ICP_OUTLIER_REJECTION_THRESHOLD);
//icp.setMaxCorrespondenceDistance(100);
//icp.setMaximumIterations(100);
//icp.setTransformationEpsilon(1e-6);
//icp.setEuclideanFitnessEpsilon(1e-6);
//icp.setRANSACIterations(0);
//Set initial alignment estimate found using robot odometry.
Eigen::AngleAxisf init_rotation(vt_relative_rad*M_PI/180.0, Eigen::Vector3f::UnitZ ());
Eigen::Translation3f init_translation (0, 0, 0);
Eigen::Matrix4f init_guess = (init_translation * init_rotation).matrix ();
icp.align(*finalCloud, init_guess);
if (icp.hasConverged() == false)
{
cout << "icp alignment failed score = " << icp.getFitnessScore() << " time: " << tt.toc () << "ms" << endl;
cout.flush();
return;
}
float x, y, z, roll, pitch, yaw;
Eigen::Affine3f correctionCameraFrame;
correctionCameraFrame = icp.getFinalTransformation(); // get transformation in camera frame (because points are in camera frame)
pcl::getTranslationAndEulerAngles(correctionCameraFrame, x, y, z, roll, pitch, yaw);
cout << "icp alignment x:" << x << " y:" << y << " yaw:" << yaw*180/M_PI << " score:" << icp.getFitnessScore() << " time: " << tt.toc () << "ms" << endl;
cout.flush();
由於BRISK2D特徵提取點雲要求Organized PointCloud 因此不適用 !
//BRISK2DEstimation
// Keypoint Description
// pcl::BRISK2DEstimation<pcl::PointXYZ> brisk_descriptor_estimation;
// // Source Cloud
// FeatureCloudT::Ptr Source_descriptors (new FeatureCloudT);
// brisk_descriptor_estimation.setInputCloud (cloud_out.makeShared());
// brisk_descriptor_estimation.setKeypoints (keypoints_out.makeShared());
// brisk_descriptor_estimation.compute (*Source_descriptors);
// // Target Cloud
// FeatureCloudT::Ptr Target_descriptors (new FeatureCloudT);
// brisk_descriptor_estimation.setInputCloud (cloud_in.makeShared());
// brisk_descriptor_estimation.setKeypoints (keypoints_in.makeShared());
// brisk_descriptor_estimation.compute (*Target_descriptors);
// std::cout << "Target descriptor number : " << Target_descriptors->size() << std::endl;
// std::cout << "Source descriptor number : " << Source_descriptors->size() << std::endl;
// // // Correspondences matching
// // pcl::registration::CorrespondenceEstimation<FeatureT, FeatureT> correspondence_estimation;
// // pcl::CorrespondencesPtr correspondences(new pcl::Correspondences);
// // correspondence_estimation.setInputSource (Source_descriptors);
// // correspondence_estimation.setInputTarget (Target_descriptors);
// // correspondence_estimation.determineCorrespondences (*correspondences);
// pcl::SampleConsensusPrerejective<PointT,PointT,FeatureT> align;
// align.setInputSource (cloud_out.makeShared());
// align.setSourceFeatures (Source_descriptors);
// align.setInputTarget (cloud_in.makeShared());
// align.setTargetFeatures (Target_descriptors);
// align.setMaximumIterations (5000); // Number of RANSAC iterations
// align.setNumberOfSamples (3); // Number of points to sample for generating/prerejecting a pose
// align.setCorrespondenceRandomness (5); // Number of nearest features to use
// align.setSimilarityThreshold (0.9f); // Polygonal edge length similarity threshold
// align.setMaxCorrespondenceDistance (0.5f); // Inlier threshold
// align.setInlierFraction (0.25f); // Required inlier fraction for accepting a pose hypothesis
相關文章
- 正態分佈簡述
- 學習MYSQL之ICP、MRR、BKAMySql
- 不動點迭代(Fixed Point Iteration)
- 正態分佈函式值函式
- 多元統計分析04:多元正態分佈的抽樣分佈
- 優思學院|初學六西格瑪?你先要了解正態分佈!
- 迭代最近點演算法 Iterative Closest Points演算法
- 統計公差分析--正態分佈基本概念
- NumPy 正態分佈與 Seaborn 視覺化指南視覺化
- 功能點分析(Function Point Analysis)學習筆記(一)Function筆記
- 功能點分析(Function Point Analysis)學習筆記(二)Function筆記
- 功能點分析(Function Point Analysis)學習筆記(三)Function筆記
- 功能點分析(Function Point Analysis)學習筆記(四)Function筆記
- 談談最近的一點感悟和之後的學習安排
- Java基礎之IO轉換流學習Java
- CUDA 學習筆記之型別轉換筆記型別
- 動態分佈與靜態分佈的區別
- Python學習之迭代器協議Python協議
- 正態分佈的應用——基於描述性統計與分佈的推論
- 十分鐘學習泊松分佈
- 知識表示學習 (一) —— Point-Wise Space之2
- pyhanlp 繁簡轉換之拼音轉換與字元正則化HanLP字元
- 資料並非都是正態分佈:三種常見的統計分佈及其應用
- Python學習之正則Python
- RequireJs學習筆記之data-main Entry PointUIJS筆記AI
- JAVA學習路線——匹馬行天下Java
- 最近學習了HBase
- 強化學習10——迭代學習強化學習
- java 正規表示式語法學習Java
- 【SQL 學習】行列轉換SQL
- Irwin-Hall 分佈學習筆記筆記
- 浮點型(Floating-Point Types)(轉)
- 多元統計分析03:多元正態分佈的引數估計
- MATLAB 檢驗資料正態分佈及程式碼實現Matlab
- Python學習筆記|Python之索引迭代Python筆記索引
- 設計模式學習筆記之迭代器模式設計模式筆記
- 點分樹學習筆記筆記
- (轉)Oracle動態效能檢視學習之v$processOracle