距离上次读程序好像已经过去了好久,因为有一门考试需要复习。上次读的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的阅读就到此结束,小绿最近也开始有很多门课要学要复习要备考,但依然会抽出时间来读程序,希望大家能够体谅!
|