上回咱们读完了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。
联系客服
