ゼロからの手書きvio-宿題4-スライディングウィンドウアルゴリズム

1

2

コードは次のように表示されます。

//
// Created by hyj on 18-11-11.
//
#include <iostream>
#include <vector>
#include <random>  
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <Eigen/Eigenvalues>

struct Pose //位姿结构体
{
    
    
    Pose(Eigen::Matrix3d R, Eigen::Vector3d t):Rwc(R),qwc(R),twc(t) {
    
    };//有参构造初始化pose
    Eigen::Matrix3d Rwc;//旋转矩阵
    Eigen::Quaterniond qwc;//四元数
    Eigen::Vector3d twc;//位置/平移
};
int main()
{
    
    
    int featureNums = 20;//特征路标点个数
    int poseNums = 10;//相机个数
    int diem = poseNums * 6 + featureNums * 3;//信息矩阵/H矩阵维数
    double fx = 1.;//焦距
    double fy = 1.;//
    Eigen::MatrixXd H(diem,diem);
    H.setZero();//H矩阵初始化

    //生成各个相机位姿
    std::vector<Pose> camera_pose;//容器 camera_pose存放各个相机位姿
    double radius = 8;//圆弧半径
    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));//自定义R t
    }

    // 随机数生成三维特征点
    std::default_random_engine generator;//随机数产生器
    std::vector<Eigen::Vector3d> points;//容器points存放三维特征点
    for(int j = 0; j < featureNums; ++j)
    {
    
    
        std::uniform_real_distribution<double> xy_rand(-4, 4.0);
        std::uniform_real_distribution<double> 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);//世界坐标
        points.push_back(Pw);

        //遍历相机
        for (int i = 0; i < poseNums; ++i) {
    
    
            //得到从世界到相机的位姿
            Eigen::Matrix3d Rcw = camera_pose[i].Rwc.transpose();
            Eigen::Vector3d Pc = Rcw * (Pw - camera_pose[i].twc);//相机下的三维点
            // 这里用了Pw - camera_pose[i].twc 我怎么感觉应该是Pw + camera_pose[i].twc (如果camera_pose[i].twc是相机到世界的平移的话)

            double x = Pc.x();
            double y = Pc.y();
            double z = Pc.z();
            double z_2 = z * z;
            Eigen::Matrix<double,2,3> jacobian_uv_Pc;//这里的jacobian_uv_Pc雅克比矩阵是
            jacobian_uv_Pc<< fx/z, 0 , -x * fx/z_2,
                    0, fy/z, -y * fy/z_2;
            Eigen::Matrix<double,2,3> jacobian_Pj = jacobian_uv_Pc * Rcw;
            Eigen::Matrix<double,2,6> jacobian_Ti;//这里的jacobian_Ti雅克比矩阵和slambook2的p187页的公式7.46的顺序不太一样
            jacobian_Ti << -x* y * fx/z_2, (1+ x*x/z_2)*fx, -y/z*fx, fx/z, 0 , -x * fx/z_2,
                            -(1+y*y/z_2)*fy, x*y/z_2 * fy, x/z * fy, 0,fy/z, -y * fy/z_2;
            //这里指的是左上角的相机与相机构成的h/信息矩阵
            H.block(i*6,i*6,6,6) += jacobian_Ti.transpose() * jacobian_Ti;
            /// 请补充完整作业信息矩阵块的计算

            //这里指的是右下角的路标点与路标点构成的h/信息矩阵
             H.block(j*3 + 6*poseNums,j*3 + 6*poseNums,3,3) +=jacobian_Pj.transpose()*jacobian_Pj;

             //这里指的是左下角相机与路标点构成的h/信息矩阵
             H.block(i*6,j*3 + 6*poseNums, 6,3) += jacobian_Ti.transpose()*jacobian_Pj;

            //这里指的是右上角的路标点和相机构成的h/信息矩阵
            H.block(j*3 + 6*poseNums,i*6 , 3,6) += jacobian_Pj.transpose() * jacobian_Ti;
        }
    }

//    std::cout << H << std::endl;
//    Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> saes(H);
//    std::cout << saes.eigenvalues() <<std::endl;

    Eigen::JacobiSVD<Eigen::MatrixXd> svd(H, Eigen::ComputeThinU | Eigen::ComputeThinV);
    std::cout << svd.singularValues() <<std::endl;
  
    return 0;
}

最も重要なコードは次のとおりです。

//这里指的是左上角的相机与相机构成的h/信息矩阵
            H.block(i*6,i*6,6,6) += jacobian_Ti.transpose() * jacobian_Ti;
            /// 请补充完整作业信息矩阵块的计算

            //这里指的是右下角的路标点与路标点构成的h/信息矩阵
             H.block(j*3 + 6*poseNums,j*3 + 6*poseNums,3,3) +=jacobian_Pj.transpose()*jacobian_Pj;

             //这里指的是左下角相机与路标点构成的h/信息矩阵
             H.block(i*6,j*3 + 6*poseNums, 6,3) += jacobian_Ti.transpose()*jacobian_Pj;

            //这里指的是右上角的路标点和相机构成的h/信息矩阵
            H.block(j*3 + 6*poseNums,i*6 , 3,6) += jacobian_Pj.transpose() * jacobian_Ti;

操作の結果は次のとおりです。

     147.51
    129.146
     108.42
    86.8786
    66.7165
    53.3602
    50.0706
    49.9072
    46.1212
     41.646
    37.4764
    36.9655
    32.5123
    29.4255
    28.6926
    25.7302
    25.2799
    23.9525
    23.4874
    22.1444
    3.75546
    3.63333
    3.59899
    3.48363
    3.44025
    3.26522
     3.2201
    2.97983
    2.74213
    2.53162
   0.420392
   0.394421
   0.349009
   0.304702
   0.284198
   0.269621
   0.261614
   0.253395
   0.250928
   0.247525
   0.233361
    0.20687
   0.195922
   0.185235
   0.182312
   0.180179
    0.17752
   0.174138
    0.16958
   0.168057
   0.163943
   0.161402
   0.158017
   0.154701
   0.152969
   0.146307
   0.145153
   0.144042
   0.141768
   0.135736
   0.131691
   0.126308
   0.122724
   0.122139
   0.120157
   0.119543
   0.116115
  0.0479554
  0.0391613
  0.0382714
  0.0370505
  0.0348714
  0.0338867
  0.0307974
  0.0285107
  0.0273249
  0.0256439
   0.024074
  0.0217149
  0.0189657
  0.0180311
  0.0160844
  0.0156509
  0.0143411
  0.0136067
  0.0133183
  0.0130406
  0.0128135
  0.0125244
  0.0122748
  0.0104077
 0.00982936
 0.00897393
  0.0082873
 0.00763381
 0.00734249
 0.00701361
 0.00634341
 0.00608493
 0.00547299
  0.0053236
 0.00520788
 0.00502341
  0.0048434
 0.00451083
  0.0042627
 0.00386223
 0.00351651
 0.00302963
 0.00253459
 0.00230246
 0.00172459
0.000422374
3.21708e-17
2.06732e-17
1.43188e-17
7.66992e-18
6.08423e-18
6.05715e-18
3.94363e-18