写一串C++代码，要求根据4pcs点云配准变换矩阵输出平移矩阵、旋转矩阵和RMSE值

在C++中，处理四点(4Pcs)对应问题通常涉及到特征匹配和变换估计。这里我们将假设你已经有了4对对应的点，并且已经计算出了齐次坐标下的变换矩阵（例如Homography Matrix）。要分解出平移矩阵（Translation）、旋转矩阵（Rotation），并计算均方根误差(RMSE)，可以参考以下步骤。注意，这只是一个简化版本的代码示例，实际应用中可能需要更复杂的库如Eigen或OpenCV的支持。

#include <iostream>
#include <Eigen/Dense>

// 假设我们有一个4x3的齐次坐标变换矩阵H
Eigen::Matrix<double, 3, 3> H;

// 提取旋转矩阵R (通过SVD)
Eigen::Matrix3d R;
Eigen::JacobiSVD<Eigen::Matrix3d> svd(H.block<3, 3>(0, 0)); // 截取3x3部分进行SVD
R = svd.matrixU() * svd.matrixV().transpose();

// 提取平移向量t
Eigen::Vector3d t = H.row(2); // 第三行代表平移

// 计算逆变换矩阵，用于找到原始点
Eigen::Matrix3d inv_R = R.inverse();
Eigen::Vector3d inv_t = -inv_R * t; // 因为H = R * [I | t]，所以逆H = [R^-1 | -R^-1*t]

// 检查并计算RMSE，这里假设你有已知的对应点对pairs
std::vector<Eigen::Vector3f> original_points;
std::vector<Eigen::Vector3f> transformed_points = ... // 从H变换得到的点

double rmse = 0.0;
for (int i = 0; i < pairs.size(); ++i) {
    Eigen::Vector3f diff = transformed_points[i] - (original_points[i] + inv_t);
    rmse += diff.squaredNorm();
}
rmse /= pairs.size(); // 平均RMSE

std::cout << "Translation Matrix:\n" << t.transpose() << std::endl;
std::cout << "Rotation Matrix:\n" << R << std::endl;
std::cout << "RMSE: " << sqrt(rmse) << std::endl;

//