使用C++编写上述代码

以下是使用C++编写的上述代码：

#include <iostream>
#include <cmath>
#include <Eigen/Dense>
#include <pcl/point_types.h>
#include <pcl/io/ply_io.h>
#include <pcl/point_cloud.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/common/transforms.h>

using namespace std;
using namespace Eigen;

// 定义点云类型
typedef pcl::PointXYZ PointT;
typedef pcl::PointCloud<PointT> PointCloud;

// 定义Icp类
class Icp {
public:
    Icp() : max_iter_(100), epsilon_(1e-5) {}

    // 设置源点云和目标点云
    void setInputSource(const PointCloud::Ptr &cloud) {
        cloud_src_ = cloud;
    }

    void setInputTarget(const PointCloud::Ptr &cloud) {
        cloud_tgt_ = cloud;
    }

    // 设置最大迭代次数和收敛阈值
    void setMaxIterations(int max_iter) {
        max_iter_ = max_iter;
    }

    void setConvergenceThreshold(double epsilon) {
        epsilon_ = epsilon;
    }

    // 执行ICP算法
    void align(PointCloud::Ptr &output) {
        // 初始化变换矩阵
        Matrix4f T = Matrix4f::Identity();

        // 迭代ICP算法
        for (int i = 0; i < max_iter_; ++i) {
            // 1. 计算源点云中每个