VSLAM算法（三）： 3D-3D 的ICP求解方法：SVD 及 BA优化算法

 3D-3D模型：

1、根据两张图像的特征点（orb特征点）求出匹配点；

2、根据obr特征的深度计算 空间左边点位置 P1，P2 ；

3、根据 SVD 方法，计算相机 位姿 （ R , t ）；

4、用 BA 方法 优化 位姿 （ R , t ）；

5、验证 p1 = R*p2 + t

//--1、根据两张图像的特征点（orb特征点）求出匹配点；
 find_feature_matches ( img_1, img_2, keypoints_1, keypoints_2, matches );

//--2、根据obr特征的深度计算 空间左边点位置 P1，P2 ；
 for ( DMatch m:matches )
    {
        ushort d1 = depth1.ptr<unsigned short> ( int ( keypoints_1[m.queryIdx].pt.y ) ) [ int ( keypoints_1[m.queryIdx].pt.x ) ];
        ushort d2 = depth2.ptr<unsigned short> ( int ( keypoints_2[m.trainIdx].pt.y ) ) [ int ( keypoints_2[m.trainIdx].pt.x ) ];
        if ( d1==0 || d2==0 )   // bad depth
            continue;
        Point2d p1 = pixel2cam ( keypoints_1[m.queryIdx].pt, K );
        Point2d p2 = pixel2cam ( keypoints_2[m.trainIdx].pt, K );
        float dd1 = float ( d1 ) /5000.0;
        float dd2 = float ( d2 ) /5000.0;
        pts1.push_back ( Point3f ( p1.x*dd1, p1.y*dd1, dd1 ) );
        pts2.push_back ( Point3f ( p2.x*dd2, p2.y*dd2, dd2 ) );
    }

    cout<<"3d-3d pairs: "<<pts1.size() <<endl;

//--3、根据 SVD 方法，计算相机 位姿 （ R , t ）；
    Mat R, t;
    pose_estimation_3d3d ( pts1, pts2, R, t );
    cout<<"ICP via SVD results: "<<endl;
    cout<<"R = "<<R<<endl;
    cout<<"t = "<<t<<endl;
    cout<<"R_inv = "<<R.t() <<endl;
    cout<<"t_inv = "<<-R.t() *t<<endl;

    cout<<"calling bundle adjustment"<<endl;

//--4、用 BA 方法 优化 位姿 （ R , t ）；
    bundleAdjustment( pts1, pts2, R, t );

//--5、验证 p1 = R*p2 + t
    for ( int i=0; i<5; i++ )
    {
        cout<<"p1 = "<<pts1[i]<<endl;
        cout<<"p2 = "<<pts2[i]<<endl;
        cout<<"(R*p2+t) = "<<
            R * (Mat_<double>(3,1)<<pts2[i].x, pts2[i].y, pts2[i].z) + t
            <<endl;
        cout<<endl;
    }

https://blog.youkuaiyun.com/ziliwangmoe/article/details/81460392

SparseOptimizer
    1、这个应该是总的流程和数据管理器
    2、setAlgorithm设置具体计算更新值得算法
    3、addVertex加入顶点
    4、addEdge()
    5、addParameter()可以传入一些超参
    6、initializeOptimization()给顶点填入初始值
    7、optimize()启动优化流程（可在 sparse_optimizer.cpp）：
        A、调用所有边的linearizeOplus()函数，得到每个边的雅克比矩阵，然后把矩阵连接成一个大矩阵
        B、调用所有边的computeError()，得到误差矩阵
        C、基于雅克比和误差，使用优化器计算更新值（矩阵求逆，主要时间花费在这里）
        D、把拆分成各个顶点的值
        E、调用顶点的oplusImpl函数，更新顶点的_estimate变量
        F、回到第一步