计算两个点集之间的变换矩阵代码 c++

以下是使用C++语言计算两个点集之间的变换矩阵的代码，同样使用了最小二乘法：

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

using namespace Eigen;

// 源点集
Matrix<double, Dynamic, 2> src_points(3,2);
src_points << 0, 0,
              0, 1,
              1, 0;

// 目标点集
Matrix<double, Dynamic, 2> dst_points(3,2);
dst_points << 1, 1,
              1, 2,
              2, 1;

// 构造方程
Matrix<double, Dynamic, 6> A(2*src_points.rows(), 6);
Matrix<double, Dynamic, 1> b(2*src_points.rows(), 1);
for (int i=0; i<src_points.rows(); ++i) {
    A.row(2*i) << src_points(i,0), src_points(i,1), 0, 0, 1, 0;
    A.row(2*i+1) << 0, 0, src_points(i,0), src_points(i,1), 0, 1;
    b.row(2*i) = dst_points(i,0);
    b.row(2*i+1) = dst_points(i,1);
}

// 求解未知元素
VectorXd x = A.jacobiSvd(ComputeThinU | ComputeThinV).solve(b);

// 填充变换矩阵
Matrix<double, 3, 3> T;
T << x(0), x(1), x(4),
     x(2), x(3), x(5),
     0, 0, 1;

std::cout << T << std::endl;

其中，变换矩阵T的形式与上面Python代码中的一致。上述C++代码使用了Eigen库，通过矩阵运算实现了方程的构造和求解。