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 ( int ( keypoints_1[m.queryIdx].pt.y ) ) [ int ( keypoints_1[m.queryIdx].pt.x ) ];
        ushort d2 = depth2.ptr ( 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: "<initializeOptimization()给顶点填入初始值
     7、optimize()启动优化流程（可在 sparse_optimizer.cpp）：
         A、调用所有边的linearizeOplus()函数，得到每个边的雅克比矩阵，然后把矩阵连接成一个大矩阵
         B、调用所有边的computeError()，得到误差矩阵
         C、基于雅克比和误差，使用优化器计算更新值（矩阵求逆，主要时间花费在这里）
         D、把拆分成各个顶点的值
         E、调用顶点的oplusImpl函数，更新顶点的_estimate变量
         F、回到第一步