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

上回咱们读完了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分解为：

其中U、V为对角阵，Σ为存储奇异值的对角阵。进而通过：

求解得到R。并带回计算t。至此，SVD求解ICP问题计算R、t的过程结束。

最终，调用写好的BA函数，按照图优化模型定义好节点与边，进行最小二乘求解。在这里展示一下程序运行结果：

在进行BA优化后，求解得到的R、t使用几组特征点的相机坐标进行验算，即验证

得到的结果是基本满足该等式的。

到此，高翔Slambook第七讲中的位姿估计求解方式已经解读完成。由于3d-2d、3d-3d中使用g2o进行非线性优化的函数，以及3d-3d中为了定义一种新的一元边所进行的类的声明与继承，这些操作对小绿目前的C++水平来说实在有点花哨（无奈脸），因而多少在解读时进行了一些回避与黑箱化处理。

按照书上的顺序，第七讲在使用2d-2d对极几何求解相机位姿后需要使用三角化计算特征点深度，这里小绿为了思维的连贯性将三角化放到最后，也即下一期进行解读。希望大家能不厌其烦，与小绿共同深入到代码中去入门SLAM。

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

相关推荐