點雲配準6：tricp算法在pcl上的實作

目錄

• 配準結果
• 點雲配準系列
• 準備
• 完整項目檔案
• 參數設定及說明
• • 資料
  • 參數
• 代碼
• 結果
• • Bunny
  • hippo
  • 算法小結
• 參考及感謝
• 完

配準結果

紅色是目标點雲（target），綠色是源點雲（source），藍色是配準後的源點雲。

對Bunny成功實作配準

點雲配準系列

點雲配準1：配準基礎及icp算法

點雲配準2：icp算法在PCL1.10.0上的實作+源碼解析

點雲配準3：3d-ndt算法在pcl上的實作以及參數設定

點雲配準4：cloudcompare的使用以及點雲配準功能

點雲配準5：4pcs算法在pcl上的實作

點雲配準6：tricp算法在pcl上的實作

點雲配準論文閱讀筆記–Efficient Variants of the ICP Algorithm

點雲配準論文閱讀筆記–Comparing ICP variants on real-world data sets

點雲配準論文閱讀筆記–(4PCS)4-Points Congruent Sets for Robust Pairwise Surface Registration

點雲配準論文閱讀筆記–3d-dnt博士論文

準備

本執行個體是在visual studio2019+pcl1.10.0上進行，環境配置方法在之前的部落格已進行詳細說明：

vs2019配置pcl1.10.0+點雲可視化示例

完整項目檔案

免費下載下傳位址：share_noel/PCL/tricp/tricp_registration_carlos20201226.zip

https://pan.baidu.com/s/1IsN2Ze2FNts-3v4ZH1m-9A 提取碼: mack

願意用c币支援的朋友也可在此下載下傳：

tricp_registration_carlos20201226.zip

（上述下載下傳連結中csdn與網盤的檔案完全相同，隻不過網盤免費下載下傳）

參數設定及說明

資料

配準點雲來自斯坦福的兔子點雲，本文使用其中0度及45度掃描點雲，但經過放大處理，xyz都放大了100倍。

本文将45度點雲配準至0度點雲。

Bunny所有掃描角度點雲下載下傳位址：

Stanford官方：http://graphics.stanford.edu/data/3Dscanrep/

或：share_noel/PCL/dataset/stanford_bunny https://pan.baidu.com/s/1IsN2Ze2FNts-3v4ZH1m-9A 提取碼: mack，pcd格式與原ply格式都有。

參數

tricp的參數設定較3d-ndt與4pcs來說要容易得多，設定重疊度就能實作了

這裡的0.7就是重疊度（靠視覺大緻推斷）,配準的結果也就是變換矩陣放在transformation_matrix裡面。

代碼

#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <iostream>
#include <pcl/recognition/trimmed_icp.h>//tricp頭檔案
#include <time.h>
#include <boost/thread/thread.hpp>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/filter.h>


using namespace std;
typedef pcl::PointXYZ PointT;
typedef pcl::PointCloud<PointT> PointCloud;


void visualize_pcd(PointCloud::Ptr pcd_src, PointCloud::Ptr pcd_tgt, PointCloud::Ptr pcd_final)
{
	pcl::visualization::PCLVisualizer viewer("registration Viewer");
	//原始點雲綠色
	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> src_h(pcd_src, 0, 255, 0);
	//目标點雲紅色
	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> tgt_h(pcd_tgt, 255, 0, 0);
	//比對好的點雲藍色
	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> final_h(pcd_final, 0, 0, 255);

	viewer.setBackgroundColor(255, 255, 255);
	viewer.addPointCloud(pcd_src, src_h, "source cloud");
	viewer.addPointCloud(pcd_tgt, tgt_h, "target cloud");
	viewer.addPointCloud(pcd_final, final_h, "result cloud");
	while (!viewer.wasStopped())
	{
		viewer.spinOnce(100);
		boost::this_thread::sleep(boost::posix_time::microseconds(100000));
	}

}

int main(int argc, char** argv)
{
	//加載點雲檔案
	PointCloud::Ptr cloud_source(new PointCloud);
	PointCloud::Ptr cloud_target(new PointCloud);

	//std::string filename = "point_source/hippo1-1224_s100.pcd";//
	std::string filename = "point_source/bun000_s100.pcd";//

	if (pcl::io::loadPCDFile<pcl::PointXYZ>(filename, *cloud_target) == -1)//*打開點雲檔案

	{
		PCL_ERROR("Couldn't read file test_pcd.pcd\n");
		return(-1);
	}

	//filename = "point_source/hippo2-1224_s100.pcd";//
	filename = "point_source/bun045_s100.pcd";//

	if (pcl::io::loadPCDFile<pcl::PointXYZ>(filename, *cloud_source) == -1)//*打開點雲檔案

	{
		PCL_ERROR("Couldn't read file test_pcd.pcd\n");
		return(-1);
	}
	pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_cloud(new pcl::PointCloud<pcl::PointXYZ>);//定義降采樣後點雲
	*filtered_cloud = *cloud_source;

	//體素降采樣
	pcl::VoxelGrid<pcl::PointXYZ> voxel_grid;
	voxel_grid.setLeafSize(0.5, 0.5, 0.5);//bunny
	//voxel_grid.setLeafSize(2.0, 2.0, 2.0);//hippo
	voxel_grid.setInputCloud(filtered_cloud);
	voxel_grid.filter(*filtered_cloud);
	std::cout << "down size input_cloud to" << filtered_cloud->size() << endl;//顯示降采樣後的點雲數量

	pcl::recognition::TrimmedICP<PointT, double> tricp;
	tricp.init(cloud_target);//target

	Eigen::Matrix4d transformation_matrix = Eigen::Matrix4d::Identity();
	clock_t start = clock(); 
	/*align(source, num_source_points_to_use, guess_and_result)
	* source是輸入待配準點雲（源點雲）
	* num_source_points_to_use就是點雲乘以重疊度，不同點雲模型重疊度
	* guess_and_result是輸出算法得到的變換矩陣
	*/
	tricp.align(*filtered_cloud, (int)filtered_cloud->size() * 0.7f, transformation_matrix);
	clock_t end = clock();

	cout << "time:" << (double)(end - start) / (double)CLOCKS_PER_SEC << " s" << endl;
	cout << "matrix:" << endl << transformation_matrix << endl << endl << endl;

	PointCloud::Ptr cloud_end(new PointCloud);
	pcl::transformPointCloud(*cloud_source, *cloud_end, transformation_matrix);

	visualize_pcd(cloud_source, cloud_target, cloud_end);
	return (0);
}

           

結果

Bunny

配準成功，且效果較好

hippo

配準失敗，陷入局部最優

算法小結

1、初始位置不好的時候，tricp容易陷入局部最優，比如hippo；

2、但是在初始位置良好的情況下，比如0°和45°的Bunny，tricp使用起來比3d-ndt與4pcs都要友善，設定個重疊度就好，而3d-ndt與4pcs要手動調節參數才能配準。

3、有的點雲對初始位置差距太大（比如90°的Bunny配準到0°或者45°的Bunny，3個算法都沒法實作），導緻這3d-ndt與4pcs也無法得到正确的結果，這時往往不知道是因為點雲的問題還是參數設定的問題，就會浪費更多調節參數的時間。

參考及感謝

paper:

Paul J. Besl, Neil D. McKay. A method for registration of 3-D shapes[J]. IEEE Transactions on Pattern Analysis and Machine Intelligence, 1992. 14(2): 239-256.

Chetverikov D, Stepanov D, Krsek P. Robust Euclidean alignment of 3D point sets: the trimmed iterative closest point algorithm[J]. Image and Vision Computing, 2005, 23(3):299-309.

blog:

PCD格式、Trimmed ICP實作、旋轉矩陣四元數歐拉角

完

邊學邊用，如有錯漏，敬請指正

--------------------------------------------------------------------------------------------諾有缸的高飛鳥20201226