SLAM程序阅读（第8讲 稀疏直接法）
本次阅读的程序为第八章的第2个程序direct_sparse.cpp，该程序实现了稀疏直接法进行位姿变换的计算。
首先来看一下程序的运行结果：
可以看出，运行结果是以第一张图为基准，分别圈画出了特征点对，并计算了相机位姿相对初始状态的位姿变换。下面来看一下子函数声明与一些类和结构体的定义。
struct Measurement
{
Measurement ( Eigen::Vector3d p, float g ) : pos_world ( p ), grayscale ( g ) {}
Eigen::Vector3d pos_world;
float grayscale;
};这里定义了一个结构体Measurement，用来方便存储每次测量得到的值。同时定义了结构体的构造函数：其内部有两个成员变量，分别为三维向量pos_world与浮点数grayscale，用来分别存储某个特征点的相机坐标与灰度值。这里其实将Measurement定义为一个class的结果也是一样的。
inline Eigen::Vector3d project2Dto3D ( int x, int y, int d, float fx, float fy, float cx, float cy, float scale )
{
float zz = float ( d ) /scale;
float xx = zz* ( x-cx ) /fx;
float yy = zz* ( y-cy ) /fy;
return Eigen::Vector3d ( xx, yy, zz );
}
inline Eigen::Vector2d project3Dto2D ( float x, float y, float z, float fx, float fy, float cx, float cy )
{
float u = fx*x/z+cx;
float v = fy*y/z+cy;
return Eigen::Vector2d ( u,v );
}这里定义了两个内联函数project2Dto3D与project3Dto2D，分别返回一个三维坐标与一个二维坐标，用于进行相机坐标与像素坐标的转化。这里使用内联函数是为了提高程序的运行效率。
// 直接法估计位姿
// 输入：测量值（空间点的灰度），新的灰度图，相机内参； 输出：相机位姿
// 返回：true为成功，false失败
bool poseEstimationDirect ( const vector<Measurement>& measurements, cv::Mat* gray, Eigen::Matrix3f& intrinsics, Eigen::Isometry3d& Tcw );这个函数声明则是本程序的重点：poseEstimationDirect，即用来使用直接法进行位姿估计。传入的参数有一个存储Measurement类变量的容器，一个指向Mat类的指针，一个3×3的矩阵，和一个4×4的矩阵，分别存储了特征点的空间位置及灰度信息、当前帧的图像、相机内参、解算出的位姿。
class EdgeSE3ProjectDirect: public BaseUnaryEdge< 1, double, VertexSE3Expmap>
{
...
virtual void computeError()
{
...
}
virtual void linearizeOplus( )
{
...
}
};这里定义了一个边，继承自BaseUnaryEdge，在模板参数中填入测量值的维度、类型，以及连接此边的顶点。同时其内部定义了两个虚函数computeError与linearizeOplus，分别用来计算误差与jacobian矩阵。在定义完边之后，便可直接在g2o中使用此类型的边进行优化。
下面来看一下主函数：
int main ( int argc, char** argv )
{
if ( argc != 2 )
{
cout<<"usage: useLK path_to_dataset"<<endl;
return 1;
}
srand ( ( unsigned int ) time ( 0 ) );
string path_to_dataset = argv[1];
string associate_file = path_to_dataset + "/associate.txt";
ifstream fin ( associate_file );
string rgb_file, depth_file, time_rgb, time_depth;
cv::Mat color, depth, gray;
vector<Measurement> measurements;
// 相机内参
float cx = 325.5;
float cy = 253.5;
float fx = 518.0;
float fy = 519.0;
float depth_scale = 1000.0;
Eigen::Matrix3f K;
K<<fx,0.f,cx,0.f,fy,cy,0.f,0.f,1.0f;
Eigen::Isometry3d Tcw = Eigen::Isometry3d::Identity();
cv::Mat prev_color;
// 我们以第一个图像为参考，对后续图像和参考图像做直接法
for ( int index=0; index<10; index++ )
{
cout<<"*********** loop "<<index<<" ************"<<endl;
fin>>time_rgb>>rgb_file>>time_depth>>depth_file;
color = cv::imread ( path_to_dataset+"/"+rgb_file );
depth = cv::imread ( path_to_dataset+"/"+depth_file, -1 );
if ( color.data==nullptr || depth.data==nullptr )
continue;
cv::cvtColor ( color, gray, cv::COLOR_BGR2GRAY );
if ( index ==0 )
{
// 对第一帧提取FAST特征点
vector<cv::KeyPoint> keypoints;
cv::Ptr<cv::FastFeatureDetector> detector = cv::FastFeatureDetector::create();
detector->detect ( color, keypoints );
for ( auto kp:keypoints )
{
// 去掉邻近边缘处的点
if ( kp.pt.x < 20 || kp.pt.y < 20 || ( kp.pt.x+20 ) >color.cols || ( kp.pt.y+20 ) >color.rows )
continue;
ushort d = depth.ptr<ushort> ( cvRound ( kp.pt.y ) ) [ cvRound ( kp.pt.x ) ];
if ( d==0 )
continue;
Eigen::Vector3d p3d = project2Dto3D ( kp.pt.x, kp.pt.y, d, fx, fy, cx, cy, depth_scale );
float grayscale = float ( gray.ptr<uchar> ( cvRound ( kp.pt.y ) ) [ cvRound ( kp.pt.x ) ] );
measurements.push_back ( Measurement ( p3d, grayscale ) );
}
prev_color = color.clone();
continue;
}
// 使用直接法计算相机运动
chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
poseEstimationDirect ( measurements, &gray, K, Tcw );
chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>> ( t2-t1 );
cout<<"direct method costs time: "<<time_used.count() <<" seconds."<<endl;
cout<<"Tcw="<<Tcw.matrix() <<endl;
// plot the feature points
cv::Mat img_show ( color.rows*2, color.cols, CV_8UC3 );
prev_color.copyTo ( img_show ( cv::Rect ( 0,0,color.cols, color.rows ) ) );
color.copyTo ( img_show ( cv::Rect ( 0,color.rows,color.cols, color.rows ) ) );
for ( Measurement m:measurements )
{
if ( rand() > RAND_MAX/5 )
continue;
Eigen::Vector3d p = m.pos_world;
Eigen::Vector2d pixel_prev = project3Dto2D ( p ( 0,0 ), p ( 1,0 ), p ( 2,0 ), fx, fy, cx, cy );
Eigen::Vector3d p2 = Tcw*m.pos_world;
Eigen::Vector2d pixel_now = project3Dto2D ( p2 ( 0,0 ), p2 ( 1,0 ), p2 ( 2,0 ), fx, fy, cx, cy );
if ( pixel_now(0,0)<0 || pixel_now(0,0)>=color.cols || pixel_now(1,0)<0 || pixel_now(1,0)>=color.rows )
continue;
float b = 255*float ( rand() ) /RAND_MAX;
float g = 255*float ( rand() ) /RAND_MAX;
float r = 255*float ( rand() ) /RAND_MAX;
cv::circle ( img_show, cv::Point2d ( pixel_prev ( 0,0 ), pixel_prev ( 1,0 ) ), 8, cv::Scalar ( b,g,r ), 2 );
cv::circle ( img_show, cv::Point2d ( pixel_now ( 0,0 ), pixel_now ( 1,0 ) +color.rows ), 8, cv::Scalar ( b,g,r ), 2 );
cv::line ( img_show, cv::Point2d ( pixel_prev ( 0,0 ), pixel_prev ( 1,0 ) ), cv::Point2d ( pixel_now ( 0,0 ), pixel_now ( 1,0 ) +color.rows ), cv::Scalar ( b,g,r ), 1 );
}
cv::imshow ( "result", img_show );
cv::waitKey ( 0 );
}
return 0;
}同样，输入的参数依然为data文件夹的存储路径，因此argc的判别值为2。
srand ( ( unsigned int ) time ( 0 ) );利用系统时间，近似随机地生成随机数种子。至于在这个程序中哪里能用上随机数，我们稍后会看到。
cv::cvtColor ( color, gray, cv::COLOR_BGR2GRAY );调用OpenCV内置的图像颜色空间转换函数cvtColor将RGB图像color转为灰度图gray，在之后的直接法求取位姿中使用灰度图作为每一帧图像输入。
for ( auto kp:keypoints )
{
// 去掉邻近边缘处的点
if ( kp.pt.x < 20 || kp.pt.y < 20 || ( kp.pt.x+20 ) >color.cols || ( kp.pt.y+20 ) >color.rows )
continue;
ushort d = depth.ptr<ushort> ( cvRound ( kp.pt.y ) ) [ cvRound ( kp.pt.x ) ];
if ( d==0 )
continue;
Eigen::Vector3d p3d = project2Dto3D ( kp.pt.x, kp.pt.y, d, fx, fy, cx, cy, depth_scale );
float grayscale = float ( gray.ptr<uchar> ( cvRound ( kp.pt.y ) ) [ cvRound ( kp.pt.x ) ] );
measurements.push_back ( Measurement ( p3d, grayscale ) );
}这里进行了几次判断，分别判断首帧特征点的像素坐标是否在图像边缘20像素的范围内，以及特征点所查询到的的深度值是否为零。去掉不符合标准的特征点后，调用project2Dto3D计算特征点的3d坐标，并查询该特征点像素坐标所对应的深度信息，最后一并存入容器measurements中。
进而，使用计算好的特征点深度信息与灰度值信息，调用poseEstimationDirect函数进行直接法的位姿求取。求取得到相机位姿变换矩阵Tcw后将其进行输出展示，并利用其进行计算特征点在当前帧中的位置，最后进行圈画并以图片的形式进行展示。
for ( Measurement m:measurements )
{
if ( rand() > RAND_MAX/5 )
continue;
Eigen::Vector3d p = m.pos_world;
Eigen::Vector2d pixel_prev = project3Dto2D ( p ( 0,0 ), p ( 1,0 ), p ( 2,0 ), fx, fy, cx, cy );
Eigen::Vector3d p2 = Tcw*m.pos_world;
Eigen::Vector2d pixel_now = project3Dto2D ( p2 ( 0,0 ), p2 ( 1,0 ), p2 ( 2,0 ), fx, fy, cx, cy );
if ( pixel_now(0,0)<0 || pixel_now(0,0)>=color.cols || pixel_now(1,0)<0 || pixel_now(1,0)>=color.rows )
continue;
float b = 255*float ( rand() ) /RAND_MAX;
float g = 255*float ( rand() ) /RAND_MAX;
float r = 255*float ( rand() ) /RAND_MAX;
cv::circle ( img_show, cv::Point2d ( pixel_prev ( 0,0 ), pixel_prev ( 1,0 ) ), 8, cv::Scalar ( b,g,r ), 2 );
cv::circle ( img_show, cv::Point2d ( pixel_now ( 0,0 ), pixel_now ( 1,0 ) +color.rows ), 8, cv::Scalar ( b,g,r ), 2 );
cv::line ( img_show, cv::Point2d ( pixel_prev ( 0,0 ), pixel_prev ( 1,0 ) ), cv::Point2d ( pixel_now ( 0,0 ), pixel_now ( 1,0 ) +color.rows ), cv::Scalar ( b,g,r ), 1 );
}
cv::imshow ( "result", img_show );
cv::waitKey ( 0 );以上的这些代码便是将每一帧图像与第一帧进行拼接，并将两帧图像中的特征点进行圈画及连线。其中，
if ( rand() > RAND_MAX/5 )
continue;这是为了适当减少所圈画的特征点的个数，即只取五分之一的特征点进行展示。
最后，调用函数poseEstimationDirect使用直接法求取相机位姿，这里函数代码不做展开，依然使用g2o实现图优化进行非线性最优值的求解。下面来看一下程序在RGBD数据集中的运行结果：
本文参与 腾讯云自媒体同步曝光计划，分享自微信公众号。
原始发表：2018-11-22，如有侵权请联系 cloudcommunity@tencent.com 删除
评论
登录后参与评论
推荐阅读
相关产品与服务
对象存储
对象存储（Cloud Object Storage，COS）是由腾讯云推出的无目录层次结构、无数据格式限制，可容纳海量数据且支持 HTTP/HTTPS 协议访问的分布式存储服务。腾讯云 COS 的存储桶空间无容量上限，无需分区管理，适用于 CDN 数据分发、数据万象处理或大数据计算与分析的数据湖等多种场景。
腾讯云开发者
