Ubuntu下对单帧点云进行icp匹配

#include <iostream>
#include <string>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/registration/icp.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/console/time.h> 
#include <vector>
#include <eigen3/Eigen/Dense>
#include <omp.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/search/impl/search.hpp>
#include <pcl/range_image/range_image.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/common/common.h>
#include <pcl/common/transforms.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/registration/icp.h>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/filter.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/octree/octree_pointcloud_voxelcentroid.h>
#include <pcl/filters/crop_box.h> 
//#include <pcl_conversions/pcl_conversions.h>
using namespace std;
typedef pcl::PointXYZI PointType;
typedef pcl::PointXYZ PointT;
typedef pcl::PointCloud<PointT> PointCloudT;
bool next_iteration = false;
void keyboardEventOccurred(const pcl::visualization::KeyboardEvent& event, void* nothing)
{
	if (event.getKeySym() == "space" && event.keyDown())
		next_iteration = true;
}

void myICP(pcl::PointCloud<pcl::PointXYZ>::Ptr& source, pcl::PointCloud<pcl::PointXYZ>::Ptr& target)
{
	

	pcl::visualization::PCLVisualizer viewer("viewer");

	pcl::visualization::PointCloudColorHandlerCustom<PointT> cloud_source(source, 255, 0, 0), cloud_target(target, 255, 255, 255);

	viewer.addPointCloud(source, cloud_source, "source");
	viewer.addPointCloud(target, cloud_target, "target");

	viewer.registerKeyboardCallback(&keyboardEventOccurred, (void*)NULL);
	while(!viewer.wasStopped())
	{
		viewer.spinOnce();
		if (next_iteration)
		{
			
			viewer.updatePointCloud(source, cloud_source,"source");
		}
		next_iteration = false;
	}	
}

int main()
{
	
    pcl::PointCloud<pcl::PointXYZ>::Ptr source(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PointCloud<pcl::PointXYZ>::Ptr target(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PCDReader reader;
	
	

	reader.read("000426.pcd", *source);
	reader.read("000873.pcd", *target);
    pcl::PointCloud<pcl::PointXYZI>::Ptr sourceXYZI (new pcl::PointCloud<pcl::PointXYZI>);
    pcl::PointCloud<pcl::PointXYZI>::Ptr targetXYZI (new pcl::PointCloud<pcl::PointXYZI>);
    pcl::copyPointCloud(*source, *sourceXYZI);
    pcl::copyPointCloud(*target, *targetXYZI);
    // ICP Settings
    pcl::IterativeClosestPoint<PointType, PointType> icp;
    icp.setMaxCorrespondenceDistance(150); // giseop , use a value can cover 2*historyKeyframeSearchNum range in meter 
    icp.setMaximumIterations(100);
    icp.setTransformationEpsilon(1e-6);
    icp.setEuclideanFitnessEpsilon(1e-6);
    icp.setRANSACIterations(10);

    // Align pointclouds
    icp.setInputSource(sourceXYZI);
    icp.setInputTarget(targetXYZI);
    pcl::PointCloud<PointType>::Ptr unused_result(new pcl::PointCloud<PointType>());
    icp.align(*unused_result);
 
    float loopFitnessScoreThreshold = 0.3; // user parameter but fixed low value is safe. 
    if (icp.hasConverged() == false || icp.getFitnessScore() > loopFitnessScoreThreshold) {
        std::cout << "[SC loop] ICP fitness test failed (" << icp.getFitnessScore() << " > " << loopFitnessScoreThreshold << "). Reject this SC loop." << std::endl;
        //return std::nullopt;
    } else {
        std::cout << "[SC loop] ICP fitness test passed (" << icp.getFitnessScore() << " < " << loopFitnessScoreThreshold << "). Add this SC loop." << std::endl;
    }

    // Get pose transformation
    float x, y, z, roll, pitch, yaw;
    Eigen::Affine3f correctionLidarFrame;
    correctionLidarFrame = icp.getFinalTransformation();
    std::cout << "correctionLidarFrame:\n" << correctionLidarFrame.matrix() << std::endl;
    Eigen::Matrix4f transformMatrix;

// 将 Affine3f 转换为变换矩阵
    transformMatrix = correctionLidarFrame.matrix();
    std::ofstream outputFile("transform_matrix.txt");
     if(outputFile.is_open()) 
     {
        // 将变换矩阵写入文件
        for(int i = 0; i < 4; ++i) {
            for(int j = 0; j < 4; ++j) {
                outputFile << transformMatrix(i, j) << " ";
            }
            outputFile << std::endl; // 换行
        }

        // 关闭文件
        outputFile.close();
        std::cout << "变换矩阵已成功写入到 transform_matrix.txt 文件中。" << std::endl;
       } 
       else {
        std::cerr << "无法打开文件进行写入。" << std::endl;
        }
    pcl::getTranslationAndEulerAngles (correctionLidarFrame, x, y, z, roll, pitch, yaw);
    std::cout << "Translation: x=" << x << ", y=" << y << ", z=" << z << std::endl;
    std::cout << "EulerAngles: roll=" << roll << ", pitch=" << pitch << ", yaw=" << yaw << std::endl;
	pcl::transformPointCloud(*source, *source, transformMatrix);
	//pcl::transformPointCloud(*source, *source, transformation_matrix);
    myICP(source, target);
	
	return 0;
}