"#include "iostream"\n#include "time.h"\n#include "pcl/io/pcd_io.h"\n#include "pcl/point_types.h"\n#include "pcl/registration/icp.h"\n#include "pcl/registration/ia_ransac.h"\n#include "pcl/point_types.h"\n#include "pcl/features/normal_3d.h"\n#include "pcl/point_types.h"\n#include "pcl/features/pfh.h"\n#include "pcl/filters/passthrough.h"\n#include "pcl/visualization/cloud_viewer.h"\n#include "limits"\n#include "fstream"\n#include "vector"\n#include "Eigen/Core"\n#include "pcl/point_cloud.h" \n#include "pcl/kdtree/kdtree_flann.h" \n#include "pcl/filters/passthrough.h" \n#include "pcl/filters/voxel_grid.h" \n#include "pcl/features/fpfh.h" \n\nusing namespace pcl;\n\nSampleConsensusInitialAlignment<pcl::PointXYZ, pcl::PointXYZ, pcl::FPFHSignature33>\n\nalign(PointCloud::Ptr cloud1, PointCloud::Ptr cloud2,\nPointCloud::Ptr features1, PointCloud::Ptr features2,\nint max_sacia_iterations, double min_correspondence_dist, double max_correspondence_dist) {\n\n\tSampleConsensusInitialAlignment<pcl::PointXYZ, pcl::PointXYZ, pcl::FPFHSignature33> sac_ia;\n\tEigen::Matrix4f final_transformation;\n\tsac_ia.setInputCloud(cloud2);\n\tsac_ia.setSourceFeatures(features2);\n\tsac_ia.setInputTarget(cloud1);\n\tsac_ia.setTargetFeatures(features1);\n\tsac_ia.setMaximumIterations(max_sacia_iterations);\n\tsac_ia.setMinSampleDistance(min_correspondence_dist);\n\tsac_ia.setMaxCorrespondenceDistance(max_correspondence_dist);\n\tPointCloud finalcloud;\n\tsac_ia.align(finalcloud);\n\tsac_ia.getCorrespondenceRandomness();\n\treturn sac_ia;\n}\n\nint main() {\n\n\t// Load point clouds\n\tPointCloud::Ptr cloud1(new PointCloud);\n\tPointCloud::Ptr cloud2(new PointCloud);\n\tpcl::io::loadPCDFile("cloud1.pcd", *cloud1);\n\tpcl::io::loadPCDFile("cloud2.pcd", *cloud2);\n\n\t// Downsample point clouds\n\tfloat leaf_size = 0.01;\n\tVoxelGrid vg;\n\tvg.setInputCloud(cloud1);\n\tvg.setLeafSize(leaf_size, leaf_size, leaf_size);\n\tPointCloud::Ptr cloud1_downsampled(new PointCloud);\n\tvg.filter(*cloud1_downsampled);\n\tvg.setInputCloud(cloud2);\n\tvg.filter(*cloud2);\n\n\t// Calculate normals\n\tNormalEstimation<PointXYZ, Normal> ne;\n\tne.setInputCloud(cloud1_downsampled);\n\tsearch::KdTree::Ptr tree(new search::KdTree);\n\tne.setSearchMethod(tree);\n\tPointCloud::Ptr cloud1_normals(new PointCloud);\n\tne.setRadiusSearch(0.03);\n\tne.compute(*cloud1_normals);\n\n\tne.setInputCloud(cloud2);\n\tne.compute(*cloud2_normals);\n\n\t// Calculate PFH features\n\tPFHEstimation<PointXYZ, Normal, PFHSignature33> pfh;\n\tpfh.setInputCloud(cloud1_downsampled);\n\tpfh.setInputNormals(cloud1_normals);\n\tpfh.setRadiusSearch(0.05);\n\tPointCloud::Ptr cloud1_features(new PointCloud);\n\tpfh.compute(*cloud1_features);\n\n\tpfh.setInputCloud(cloud2);\n\tpfh.setInputNormals(cloud2_normals);\n\tpfh.compute(*cloud2_features);\n\n\t// Align point clouds using Sample Consensus Initial Alignment\n\tint max_sacia_iterations = 500;\n\tdouble min_correspondence_dist = 0.01;\n\tdouble max_correspondence_dist = 0.1;\n\tSampleConsensusInitialAlignment<pcl::PointXYZ, pcl::PointXYZ, pcl::FPFHSignature33> sac_ia = align(cloud1_downsampled, cloud2, cloud1_features, cloud2_features, max_sacia_iterations, min_correspondence_dist, max_correspondence_dist);\n\n\t// Get final transformation\n\tEigen::Matrix4f final_transformation = sac_ia.getFinalTransformation();\n\n\t// Transform cloud2 to align with cloud1\n\tPointCloud::Ptr aligned_cloud2(new PointCloud);\n\ttransformPointCloud(*cloud2, *aligned_cloud2, final_transformation);\n\n\t// Visualize aligned point clouds\n\tvisualization::CloudViewer viewer("Aligned Point Clouds");\n\tviewer.showCloud(cloud1);\n\tviewer.showCloud(aligned_cloud2);\n\twhile (!viewer.wasStopped()) {}\n\n\treturn 0;\n
