SLAM程序阅读（第8讲 稀疏直接法）

距离上次读程序好像已经过去了好久，因为有一门考试需要复习。上次读的LK光流法程序主要调用了CV内置的LK光流法求解函数calcOpticalFlowPyrLK，进行了光流法跟踪特征点的一些演示。本次阅读的程序为第八章的第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数据集中的运行结果：

本期direct_sparse.cpp的阅读就到此结束，小绿最近也开始有很多门课要学要复习要备考，但依然会抽出时间来读程序，希望大家能够体谅！

赞 (0)

相关推荐