高翔Slambook第七讲代码解读（3d-3d位姿估计）

小白学视觉

发布于 2019-10-24 11:33:49

2.1K1

发布于 2019-10-24 11:33:49

上回咱们读完了pose_estimation_3d2d.cpp这个程序，也找到了3d-2d位姿估计与2d-2d位姿估计之间的联系与差别：

在2d-2d使用对极几何求解相机位姿时直接调用了OpenCV提供的findFundamentalMat、findEssentialMat函数，直接求取了基础矩阵F和本质矩阵E，进而调用recoverPose求解R、t。
在3d-3d使用PnP求解相机位姿时，则直接调用OpenCV提供的solvePnP函数，直接求解R、t。

区别则在于：在3d-2d位姿估计过程中，我们做了一次显式的非线性优化，即构建图模型使用g2o库进行优化操作。即便2d-2d中使用的findEssentialMat函数内部会有最小二乘求解过程，以及3d-2d中使用的solvePnP或solvePnPRansac内部会有最小二乘与随机选点剔除错误姿态等优化过程，我们还是希望使用一次图优化处理来最大程度地优化位姿估计。

在2d-2d的基础上，我们引入前一帧图像对应的深度图，于是得到了3d-2d位姿估计算法，并使用PnP进行求解。那么在当前帧如果我们也引入深度信息，将得到3d-3d位姿估计算法。同样，小绿简单准备了几张图表示了一下这个演变过程：

↑两张平面图

↑一张平面图+一张深度图与一张平面图

↑两组平面图+深度图

下面我们来看一下本次要解读的pose_estimation_3d3d.cpp。

在这个程序中，我们同样使用了find_feature_matches函数进行特征点的搜寻与匹配，也使用了pixel2cam函数在需要时进行像素坐标到归一化平面坐标的转化。与3d2d.cpp不同的是，在这里我们声明了一个子函数pose_estimation_3d3d：

void pose_estimation_3d3d (
    const vector<Point3f>& pts1,
    const vector<Point3f>& pts2,
    Mat& R, Mat& t
);

以及针对3d-3d的BundleAdjustment函数bundleAdjustment：

void bundleAdjustment(
    const vector<Point3f>& points_3d,
    const vector<Point3f>& points_2d,
    Mat& R, Mat& t
);

并定义了一个类EdgeProjectXYZRGBDPoseOnly：

class EdgeProjectXYZRGBDPoseOnly : public g2o::BaseUnaryEdge<3, Eigen::Vector3d, g2o::VertexSE3Expmap>
{
public:
    ...
protected:
    ...
};

这里是一个类的继承，继承于g2o定义的一元边BaseUnaryEdge，相当于对g2o提供的一元边模型进行改写，即将观测量由2维变为3维。

类的定义内的代码这里就不贴了，其目的在于使用g2o提供的李代数位姿节点，定义一种g2o没有给出模板的3d-3d的边，用来优化关键点的空间3d坐标。这里3d节点定义为相机（这里使用RGB-D相机）对路标点的观测，每次观测便会得到其三维位置并作为观测数据。在定义完边之后，在类的内部以虚函数直接给出了其jacobian矩阵，向g2o提供解析求导方式。

我们还是先从主函数开始看：

int main ( int argc, char** argv )
{
    if ( argc != 5 )
    {
        cout<<"usage: pose_estimation_3d3d img1 img2 depth1 depth2"<<endl;
        return 1;
    }
    //-- 读取图像
    Mat img_1 = imread ( argv[1], CV_LOAD_IMAGE_COLOR );
    Mat img_2 = imread ( argv[2], CV_LOAD_IMAGE_COLOR );

    vector<KeyPoint> keypoints_1, keypoints_2;
    vector<DMatch> matches;
    find_feature_matches ( img_1, img_2, keypoints_1, keypoints_2, matches );
    cout<<"一共找到了"<<matches.size() <<"组匹配点"<<endl;

    // 建立3D点
    Mat depth1 = imread ( argv[3], CV_LOAD_IMAGE_UNCHANGED );       // 深度图为16位无符号数，单通道图像
    Mat depth2 = imread ( argv[4], CV_LOAD_IMAGE_UNCHANGED );       // 深度图为16位无符号数，单通道图像
    Mat K = ( Mat_<double> ( 3,3 ) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1 );
    vector<Point3f> pts1, pts2;

    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;
    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;

    bundleAdjustment( pts1, pts2, R, t );

    // verify 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;
    }
}

这里我们需要前一帧图像、当前帧图像、前一帧深度、当前帧深度四个额外引入的参数，故argc判别值为5。

同样，在读取图像、寻找特征点并匹配等操作后，我们需要将特征点的坐标进行存储用以进行后续的位姿求解。这里同样需要联合深度信息来构造特征点在当前相机坐标系下的3d坐标：

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 ) );
    }

接下来便调用子函数pose_estimation_3d3d进行R、t的求取。可以看出求解3d-3d位姿求解问题时，使用了ICP（Iterative Closest Point，迭代最近点）方法，由于OpenCV没有提供类似于solvePnP这种直接求解PnP问题的函数来求解ICP问题，在这里需要自己编写程序进行求解。

bundleAdjustment( pts1, pts2, R, t );

同样，最后仍然调用BA进行最小二乘优化。此时由3d-3d信息点构造的最小二乘问题也就是使用使用非线性优化法求解ICP的过程。

下面我们来看一下ICP求解函数pose_estimation_3d3d。

void pose_estimation_3d3d (
    const vector<Point3f>& pts1,
    const vector<Point3f>& pts2,
    Mat& R, Mat& t
)
{
    Point3f p1, p2;     // center of mass
    int N = pts1.size();
    for ( int i=0; i<N; i++ )
    {
        p1 += pts1[i];
        p2 += pts2[i];
    }
    p1 = Point3f( Vec3f(p1) /  N);
    p2 = Point3f( Vec3f(p2) / N);
    vector<Point3f>     q1 ( N ), q2 ( N ); // remove the center
    for ( int i=0; i<N; i++ )
    {
        q1[i] = pts1[i] - p1;
        q2[i] = pts2[i] - p2;
    }

    // compute q1*q2^T
    Eigen::Matrix3d W = Eigen::Matrix3d::Zero();
    for ( int i=0; i<N; i++ )
    {
        W += Eigen::Vector3d ( q1[i].x, q1[i].y, q1[i].z ) * Eigen::Vector3d ( q2[i].x, q2[i].y, q2[i].z ).transpose();
    }
    cout<<"W="<<W<<endl;

    // SVD on W
    Eigen::JacobiSVD<Eigen::Matrix3d> svd ( W, Eigen::ComputeFullU|Eigen::ComputeFullV );
    Eigen::Matrix3d U = svd.matrixU();
    Eigen::Matrix3d V = svd.matrixV();
    
    if (U.determinant() * V.determinant() < 0)
  {
        for (int x = 0; x < 3; ++x)
        {
            U(x, 2) *= -1;
        }
  }
    
    cout<<"U="<<U<<endl;
    cout<<"V="<<V<<endl;

    Eigen::Matrix3d R_ = U* ( V.transpose() );
    Eigen::Vector3d t_ = Eigen::Vector3d ( p1.x, p1.y, p1.z ) - R_ * Eigen::Vector3d ( p2.x, p2.y, p2.z );

    // convert to cv::Mat
    R = ( Mat_<double> ( 3,3 ) <<
          R_ ( 0,0 ), R_ ( 0,1 ), R_ ( 0,2 ),
          R_ ( 1,0 ), R_ ( 1,1 ), R_ ( 1,2 ),
          R_ ( 2,0 ), R_ ( 2,1 ), R_ ( 2,2 )
        );
    t = ( Mat_<double> ( 3,1 ) << t_ ( 0,0 ), t_ ( 1,0 ), t_ ( 2,0 ) );
}

在这个子函数中，我们需要完成的任务是通过给定的两组3d点的坐标pts1和pts2，使用线性求解ICP问题来解算位姿R、t。

Point3f p1, p2;     // center of mass
    int N = pts1.size();
    for ( int i=0; i<N; i++ )
    {
        p1 += pts1[i];
        p2 += pts2[i];
    }
    p1 = Point3f( Vec3f(p1) /  N);
    p2 = Point3f( Vec3f(p2) / N);

这里定义了两个Point3f类的变量p1和p2，计算方式为对每组特征点的3d坐标进行加和并求平均，即计算每组特征点的“质心”，进而将每组3d点坐标变换为去质心3d坐标（从后面的程序中可以看到分别存为q1与q2两个Point3f型vector，q可以理解为qù质心的首字母）。

Eigen::Matrix3d W = Eigen::Matrix3d::Zero();
    for ( int i=0; i<N; i++ )
    {
        W += Eigen::Vector3d ( q1[i].x, q1[i].y, q1[i].z ) * Eigen::Vector3d ( q2[i].x, q2[i].y, q2[i].z ).transpose();
    }
    cout<<"W="<<W<<endl;

进而，为了使用SVD（奇异值分解）求解R的最优化问题：

其中f(R)为关于R的优化目标函数，即求得使得f取极小值的R作为最优解。为了线性求解这个最优问题，我们通过以上代码来定义一个3×3的矩阵W，使得：

进而通过以下代码将W进行奇异值分解：

// SVD on W
    Eigen::JacobiSVD<Eigen::Matrix3d> svd ( W, Eigen::ComputeFullU|Eigen::ComputeFullV );
    Eigen::Matrix3d U = svd.matrixU();
    Eigen::Matrix3d V = svd.matrixV();

通过以上代码将矩阵W分解为：