svd奇异值分解

基础题：

 

证明式（15）中，取y=u4是该问题的最优解，

证明：

提示设置y’=u4+v,其中v正交于u4，证明

该式方法基于奇异值构造矩阵零空间。

解：

//
// Created by hyj on 18-11-11.
//
#include 
#include 
#include 
#include 
#include 
#include 

struct Pose
{
    Pose(Eigen::Matrix3d R, Eigen::Vector3d t) : Rwc(R), qwc(R), twc(t){};
    Eigen::Matrix3d Rwc;
    Eigen::Quaterniond qwc;
    Eigen::Vector3d twc;

    Eigen::Vector2d uv; // 这帧图像观测到的特征坐标
};
int main()
{
    std::cout << "1" << std::endl;
    int poseNums = 10;
    double radius = 8;
    double fx = 1.;
    double fy = 1.;
    std::vector camera_pose;
    for (int n = 0; n < poseNums; ++n)
    {
        double theta = n * 2 * M_PI / (poseNums * 4); // 1/4 圆弧
        // 绕 z轴 旋转
        Eigen::Matrix3d R;
        R = Eigen::AngleAxisd(theta, Eigen::Vector3d::UnitZ());
        Eigen::Vector3d t = Eigen::Vector3d(radius * cos(theta) - radius, radius * sin(theta), 1 * sin(2 * theta));
        camera_pose.push_back(Pose(R, t));
    }

    // 随机数生成 1 个 三维特征点
    std::default_random_engine generator;
    std::uniform_real_distribution xy_rand(-4, 4.0);
    std::uniform_real_distribution z_rand(8., 10.);
    double tx = xy_rand(generator);
    double ty = xy_rand(generator);
    double tz = z_rand(generator);

    Eigen::Vector3d Pw(tx, ty, tz);

    // 这个特征从第三帧相机开始被观测，i=3
    int start_frame_id = 3;
    int end_frame_id = poseNums;
    for (int i = start_frame_id; i < end_frame_id; ++i)
    {
        Eigen::Matrix3d Rcw = camera_pose[i].Rwc.transpose();
        Eigen::Vector3d Pc = Rcw * (Pw - camera_pose[i].twc);

        double x = Pc.x();
        double y = Pc.y();
        double z = Pc.z();

        camera_pose[i].uv = Eigen::Vector2d(x / z, y / z);
    }

    /// TODO::homework; 请完成三角化估计深度的代码
    // 遍历所有的观测数据，并三角化
    Eigen::Vector3d P_est; // 结果保存到这个变量
    P_est.setZero();
    /* your code begin */

    //主要思想：相机多帧观测对应空间在一点
    Eigen::Matrix4d D;
    Eigen::MatrixXd P(2 * (end_frame_id - start_frame_id), 4);

    for (int i = 3, j = 0; i < poseNums; i++)
    {
        Eigen::Isometry3d Tcw = Eigen::Isometry3d::Identity();
        Tcw.rotate(camera_pose[i].Rwc.transpose());
        Tcw.pretranslate(-camera_pose[i].Rwc.transpose() * camera_pose[i].twc);

        P.block(j, 0, 1, 4) = camera_pose[i].uv.x() * Tcw.matrix().row(2) - Tcw.matrix().row(0);
        P.block(j + 1, 0, 1, 4) = camera_pose[i].uv.y() * Tcw.matrix().row(2) - Tcw.matrix().row(1);
        j += 2;
    }

    D = P.transpose() * P;
    Eigen::JacobiSVD svd(D, Eigen::ComputeThinU | Eigen::ComputeThinV);

    Eigen::MatrixXd U = svd.matrixU();
    Eigen::MatrixXd V = svd.matrixV();

    P_est << (U.rightCols(1) * U.rightCols(1).bottomRows(1).inverse()).topRows(3);

    /* your code end */

    std::cout << "ground truth: \n"
              << Pw.transpose() << std::endl;
    std::cout << "your result: \n"
              << P_est.transpose() << std::endl;
    return 0;
}

 

 

 

 

参考：

https://blog.csdn.net/m0_37870649/article/details/80547167

https://blog.csdn.net/xiaocong1990/article/details/54909126