surface matching结果验证_C

最近在仿真PPF算法，通过配置opencv-contrib，然后调用sample里面的surface-matcing实例代码，仿真实现surface matching的结果。

再配置PCL库对结果进行验证观察。

采用的场景为：opencv43\opencv_contrib-4.3.0\modules\surface_matching\samples\data\rs1_normals.ply

采用的模型为：opencv43\opencv_contrib-4.3.0\modules\surface_matching\samples\data\parasaurolophus_6700.ply

跑出来的结果如下：

为了验证这个结果的正确性，使用PCL1.10.1观察。

这是场景与模型的原始位置：

经过4乘4的旋转平移矩阵变换，可以看到匹配成功。

如图：

验证代码：

#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 

using namespace std;

typedef pcl::PointXYZRGB PointT;
typedef pcl::PointCloud PointCloudT;


// This function displays the help
void
showHelp(char* program_name)
{
	std::cout << std::endl;
	std::cout << "Usage: " << program_name << " cloud_filename.[pcd|ply]" << std::endl;
	std::cout << "-h:  Show this help." << std::endl;
}

// This is the main function
int
main(int argc, char** argv)
{

	// Show help
	if (pcl::console::find_switch(argc, argv, "-h") || pcl::console::find_switch(argc, argv, "--help")) {
		showHelp(argv[0]);
		return 0;
	}

	// Fetch point cloud filename in arguments | Works with PCD and PLY files
	std::vector filenames;
	bool file_is_pcd = false;

	filenames = pcl::console::parse_file_extension_argument(argc, argv, ".ply");

	if (filenames.size() != 1) {
		filenames = pcl::console::parse_file_extension_argument(argc, argv, ".pcd");

		if (filenames.size() != 1) {
			showHelp(argv[0]);
			return -1;
		}
		else {
			file_is_pcd = true;
		}
	}

	// Load file | Works with PCD and PLY files
	pcl::PointCloud::Ptr source_cloud(new pcl::PointCloud());

	if (file_is_pcd) {
		if (pcl::io::loadPCDFile(argv[filenames[0]], *source_cloud) < 0) {
			std::cout << "Error loading point cloud " << argv[filenames[0]] << std::endl << std::endl;
			showHelp(argv[0]);
			return -1;
		}
	}
	else {
		if (pcl::io::loadPLYFile(argv[filenames[0]], *source_cloud) < 0) {
			std::cout << "Error loading point cloud " << argv[filenames[0]] << std::endl << std::endl;
			showHelp(argv[0]);
			return -1;
		}
	}

	/*  METHOD #: Using a Affine3f
	*/
	//Eigen::Affine3f transform_2 = Eigen::Affine3f::Identity();

	 Define a translation of 2.5 meters on the x axis.
	//transform_2.translation() << 2.5, 0.0, 0.0;

	 The same rotation matrix as before; theta radians arround Z axis
	//transform_2.rotate(Eigen::AngleAxisf(theta, Eigen::Vector3f::UnitZ()));
	Eigen::Matrix4f transform_2(4, 4);
	transform_2 << 0.9945623974519273, -0.08461722803758925, 0.06070883209145846, -73.75124558167937,
		0.09770049075457124, 0.5562689396701043, -0.8252390446801545, -603.9620880011939,
		0.03605900277309694, 0.8266830054365047, 0.5615113149719042, -295.8832976775827,
		0, 0, 0, 1;
	// Print the transformation
	printf("\nMethod #: using an Affine3f\n");
	std::cout << transform_2.matrix() << std::endl;

	// Executing the transformation
	pcl::PointCloud::Ptr transformed_cloud(new pcl::PointCloud());
	// You can either apply transform_1 or transform_2; they are the same
	pcl::transformPointCloud(*source_cloud, *transformed_cloud, transform_2);

	// Visualization
	printf("\nPoint cloud colors :  white  = scene point cloud\n"
		"                        red  = transformed point cloud\n");
	pcl::visualization::PCLVisualizer viewer("Matrix transformation example");
	pcl::PointCloud::Ptr cloud_scene(new pcl::PointCloud());
	//PointCloudT::Ptr cloud_scene(new PointCloudT);
	pcl::io::loadPLYFile("rs1_normals.ply", *cloud_scene);

	// Define R,G,B colors for the point cloud
	pcl::visualization::PointCloudColorHandlerCustom scene_cloud_color_handler(cloud_scene, 255, 255, 255);
	// We add the point cloud to the viewer and pass the color handler
	viewer.addPointCloud(cloud_scene, scene_cloud_color_handler, "cloud_scene");

	pcl::visualization::PointCloudColorHandlerCustom transformed_cloud_color_handler(transformed_cloud, 230, 20, 20); // Red
	viewer.addPointCloud(transformed_cloud, transformed_cloud_color_handler, "transformed_cloud");

	viewer.addCoordinateSystem(1.0, "cloud", 0);
	viewer.setBackgroundColor(0.05, 0.05, 0.05, 0); // Setting background to a dark grey
	viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "cloud_scene");
	viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "transformed_cloud");
	//viewer.setPosition(800, 400); // Setting visualiser window position

	while (!viewer.wasStopped()) { // Display the visualiser until 'q' key is pressed
		viewer.spinOnce();
	}

	return 0;
}