编写pinv函数

下面是使用C++编写的一个伪逆矩阵函数 pinv：

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

using namespace Eigen;

MatrixXd pinv(const MatrixXd& A)
{
    JacobiSVD<MatrixXd> svd(A, ComputeThinU | ComputeThinV); // 对A进行SVD分解

    MatrixXd U = svd.matrixU(); // U矩阵
    MatrixXd V = svd.matrixV(); // V矩阵
    VectorXd S = svd.singularValues(); // 奇异值

    MatrixXd S_inv = MatrixXd::Zero(A.cols(), A.rows()); // 初始化奇异值矩阵的伪逆矩阵
    for(int i = 0; i < S.size(); i++)
    {
        if(S(i) > 1e-6) // 非零奇异值
            S_inv(i, i) = 1 / S(i);
    }

    MatrixXd A_pinv = V * S_inv * U.transpose(); // 计算伪逆矩阵

    return A_pinv;
}

int main()
{
    MatrixXd A(3, 2); // 创建一个3行2列的矩阵
    A << 1, 2, 3, 4, 5, 6;

    MatrixXd A_pinv = pinv(A); // 计算伪逆矩阵

    std::cout << "A: " << std::endl << A << std::endl << std::endl;
    std::cout << "A_pinv: " << std::endl << A_pinv << std::endl;

    return 0;
}

上述代码中定义了一个名为pinv的函数，可以直接传入一个MatrixXd类型的矩阵，返回其伪逆矩阵。在函数内部，使用了SVD分解来计算伪逆矩阵，并将其返回。