SLAM - 视觉里程计 - 直接稀疏、半稠密法

 #include <iostream>

    
 #include <fstream>
    
 #include <list>
    
 #include <vector>
    
 #include <chrono>
    
 #include <ctime>
    
 #include <climits>//一些类型的最值库
    
  
    
 #include <opencv4/opencv2/core/core.hpp>
    
 #include <opencv4/opencv2/imgproc/imgproc.hpp>
    
 #include <opencv4/opencv2/highgui/highgui.hpp>
    
 #include <opencv4/opencv2/features2d/features2d.hpp>
    
  
    
 #include <g2o/core/base_unary_edge.h>
    
 #include <g2o/core/block_solver.h>
    
 #include <g2o/core/optimization_algorithm_levenberg.h>
    
 #include <g2o/solvers/dense/linear_solver_dense.h>
    
 #include <g2o/core/robust_kernel.h>
    
 #include <g2o/types/sba/types_six_dof_expmap.h>
    
  
    
 using namespace std;
    
 using namespace g2o;
    
  
    
 /******************************************** * 本节演示了RGBD上的稀疏直接法 
    
  ********************************************/
    
  
    
 // 一次测量的值，包括一个世界坐标系下三维点与一个灰度值
    
 //定义一个类结构体，用来存储每次测量得到的 世界坐标 和 灰度
    
 struct Measurement
    
 {
    
     //构造函数：参数为 三维向量pos_world和灰度值grayscale
    
     Measurement ( Eigen::Vector3d p, float g ) : pos_world ( p ), grayscale ( g ) {}
    
     Eigen::Vector3d pos_world;
    
     float grayscale;
    
 };
    
 //此处结构体可以写为class类
    
  
    
 //2D转换为3D坐标，返回的是一个三维坐标，像素坐标-》相机坐标
    
 //inline（编译过程）类似于#define（宏定义），称为内联函数，主要用来修饰函数，表示该函数在被调用时，直接将函数代码插入到调用处，因此节省了内存跳转的花销，提高了程序的效率
    
 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 );
    
 }
    
 //3D转2D，返回二维坐标，相机坐标-》像素坐标
    
 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 );
    
 }
    
  
    
 // 直接法估计位姿
    
 // 输入：测量值（空间点的灰度），新的灰度图，相机内参； 输出：相机位姿
    
 // 返回：true为成功，false失败
    
 bool poseEstimationDirect ( const vector<Measurement>& measurements, cv::Mat* gray, Eigen::Matrix3f& intrinsics, Eigen::Isometry3d& Tcw );
    
 //     直接法位姿估计函数             存储Measurement类对象的容器          Mat类的一个指针         3×3的矩阵                      4×4的矩阵
    
 //                                  存储特征点的空间位置和灰度             当前帧的图像            相机内参                    结算出来的位姿
    
  
    
 // project a 3d point into an image plane, the error is photometric error
    
 // an unary edge with one vertex SE3Expmap (the pose of camera)
    
 //定义g2o图优化的边，继承BaseUnaryEdge类，参数分别为：测量值的维度、类型，连接此边的顶点
    
 class EdgeSE3ProjectDirect: public BaseUnaryEdge< 1, double, VertexSE3Expmap>
    
 {
    
 public:
    
     EIGEN_MAKE_ALIGNED_OPERATOR_NEW//这个宏就是运算符new的对齐版本重载
    
  
    
     EdgeSE3ProjectDirect() {}
    
  
    
     EdgeSE3ProjectDirect ( Eigen::Vector3d point, float fx, float fy, float cx, float cy, cv::Mat* image )
    
     : x_world_ ( point ), fx_ ( fx ), fy_ ( fy ), cx_ ( cx ), cy_ ( cy ), image_ ( image )
    
     {}
    
  
    
     virtual void computeError()//虚函数，用来计算误差
    
     {
    
     const VertexSE3Expmap* v  =static_cast<const VertexSE3Expmap*> ( _vertices[0] );
    
     Eigen::Vector3d x_local = v->estimate().map ( x_world_ );
    
     float x = x_local[0]*fx_/x_local[2] + cx_;
    
     float y = x_local[1]*fy_/x_local[2] + cy_;
    
     // check x,y is in the image
    
     if ( x-4<0 || ( x+4 ) >image_->cols || ( y-4 ) <0 || ( y+4 ) >image_->rows )
    
     {
    
         _error ( 0,0 ) = 0.0;
    
         this->setLevel ( 1 );
    
     }
    
     else
    
     {
    
         _error ( 0,0 ) = getPixelValue ( x,y ) - _measurement;
    
     }
    
     }
    
  
    
     // plus in manifold
    
     virtual void linearizeOplus( )//虚函数，用来计算雅可比矩阵
    
     {
    
     if ( level() == 1 )
    
     {
    
         _jacobianOplusXi = Eigen::Matrix<double, 1, 6>::Zero();
    
         return;
    
     }
    
     VertexSE3Expmap* vtx = static_cast<VertexSE3Expmap*> ( _vertices[0] );
    
     Eigen::Vector3d xyz_trans = vtx->estimate().map ( x_world_ );   // q in book
    
  
    
     double x = xyz_trans[0];
    
     double y = xyz_trans[1];
    
     double invz = 1.0/xyz_trans[2];
    
     double invz_2 = invz*invz;
    
  
    
     float u = x*fx_*invz + cx_;
    
     float v = y*fy_*invz + cy_;
    
  
    
     // jacobian from se3 to u,v
    
     // NOTE that in g2o the Lie algebra is (\omega, \epsilon), where \omega is so(3) and \epsilon the translation
    
     Eigen::Matrix<double, 2, 6> jacobian_uv_ksai;
    
  
    
     jacobian_uv_ksai ( 0,0 ) = - x*y*invz_2 *fx_;
    
     jacobian_uv_ksai ( 0,1 ) = ( 1+ ( x*x*invz_2 ) ) *fx_;
    
     jacobian_uv_ksai ( 0,2 ) = - y*invz *fx_;
    
     jacobian_uv_ksai ( 0,3 ) = invz *fx_;
    
     jacobian_uv_ksai ( 0,4 ) = 0;
    
     jacobian_uv_ksai ( 0,5 ) = -x*invz_2 *fx_;
    
  
    
     jacobian_uv_ksai ( 1,0 ) = - ( 1+y*y*invz_2 ) *fy_;
    
     jacobian_uv_ksai ( 1,1 ) = x*y*invz_2 *fy_;
    
     jacobian_uv_ksai ( 1,2 ) = x*invz *fy_;
    
     jacobian_uv_ksai ( 1,3 ) = 0;
    
     jacobian_uv_ksai ( 1,4 ) = invz *fy_;
    
     jacobian_uv_ksai ( 1,5 ) = -y*invz_2 *fy_;
    
  
    
     Eigen::Matrix<double, 1, 2> jacobian_pixel_uv;
    
  
    
     jacobian_pixel_uv ( 0,0 ) = ( getPixelValue ( u+1,v )-getPixelValue ( u-1,v ) ) /2;
    
     jacobian_pixel_uv ( 0,1 ) = ( getPixelValue ( u,v+1 )-getPixelValue ( u,v-1 ) ) /2;
    
  
    
     _jacobianOplusXi = jacobian_pixel_uv*jacobian_uv_ksai;
    
     }
    
  
    
     // dummy read and write functions because we don't care...
    
     virtual bool read ( std::istream& in ) {}
    
     virtual bool write ( std::ostream& out ) const {}
    
  
    
 protected:
    
     // get a gray scale value from reference image (bilinear interpolated)
    
     inline float getPixelValue ( float x, float y )
    
     {
    
     uchar* data = & image_->data[ int ( y ) * image_->step + int ( x ) ];
    
     float xx = x - floor ( x );
    
     float yy = y - floor ( y );
    
     return float (
    
                ( 1-xx ) * ( 1-yy ) * data[0] +
    
                xx* ( 1-yy ) * data[1] +
    
                ( 1-xx ) *yy*data[ image_->step ] +
    
                xx*yy*data[image_->step+1]
    
            );
    
     }
    
 public:
    
     Eigen::Vector3d x_world_;   // 3D point in world frame
    
     float cx_=0, cy_=0, fx_=0, fy_=0; // Camera intrinsics
    
     cv::Mat* image_=nullptr;    // reference image
    
 };
    
  
    
 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; 
    
     //调用cvtColor图像颜色空间转换函数将RGB图像color转换为灰度图的gray，之后直接法求取位姿中使用灰度图输入每一帧
    
     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 )
    
             //首帧特征点的像素坐标是否在图像边缘20像素的范围内？
    
                 continue;
    
             ushort d = depth.ptr<ushort> ( cvRound ( kp.pt.y ) ) [ cvRound ( kp.pt.x ) ];
    
             if ( d==0 )//特征点所查询的深度值是否为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 ) );//存入measurements容器对象
    
         }
    
         prev_color = color.clone();
    
         continue;
    
     }
    
     // 使用直接法计算相机运动
    
     chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
    
     //使用计算好的特征点深度信息与灰度值信息，调用poseEstimationDirect函数进行直接法的位姿求取
    
     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;
    
     //取得到相机位姿变换矩阵Tcw后将其进行输出展示，
    
     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;
    
 }
    
  
    
 bool poseEstimationDirect(const vector< Measurement >& measurements, cv::Mat* gray, Eigen::Matrix3f& K, Eigen::Isometry3d& Tcw)
    
 {
    
     //     // 初始化g2o
    
     // 
    
     //     //Block::LinearSolverType* linearSolver = new g2o::LinearSolverCSparse<Block::PoseMatrixType>(); // 线性方程求解器
    
     //     std::unique_ptr<Block::LinearSolverType> linearSolver ( new g2o::LinearSolverCSparse<Block::PoseMatrixType>());
    
     // 
    
     //     //Block* solver_ptr = new Block ( linearSolver );
    
     //     //std::unique_ptr<Block> solver_ptr ( new Block ( linearSolver));
    
     //     std::unique_ptr<Block> solver_ptr ( new Block ( std::move(linearSolver)));     // 矩阵块求解器
    
     // 
    
     //     //g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg ( solver_ptr);
    
     //     g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg ( std::move(solver_ptr));
    
     // 
    
     //     g2o::SparseOptimizer optimizer;
    
     // 
    
     //     optimizer.setAlgorithm ( solver );
    
     
    
     
    
     // 初始化g2o
    
     typedef g2o::BlockSolver<g2o::BlockSolverTraits<6, 1>> DirectBlock;  // 求解的向量是6＊1的
    
  
    
     std::unique_ptr<DirectBlock::LinearSolverType> linearSolver(new LinearSolver <DirectBlock::PoseMatrixType>());
    
  
    
     std::unique_ptr<DirectBlock> solver_ptr(new DirectBlock(unique_ptr<DirectBlock::LinearSolverType>(linearSolver)));
    
  
    
     //g2o::OptimizationAlgorithmGaussNewton* solver = new g2o::OptimizationAlgorithmGaussNewton( unique_ptr<DirectBlock>(solver_ptr) ); // G-N
    
     g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(std::move(solver_ptr)); // L-M
    
     g2o::SparseOptimizer optimizer;
    
     optimizer.setAlgorithm(solver);
    
     optimizer.setVerbose(true);
    
     ///home/kyle/projects/directMethod/direct_sparse.cpp:273:116: error: expected primary-expression before ‘>’ token
    
     ///home/kyle/projects/directMethod/direct_sparse.cpp:273:113: error: invalid new-expression of abstract class type ‘g2o::LinearSolver<Eigen::Matrix<double, 6, 6, 0> >’
    
  
    
     g2o::VertexSE3Expmap* pose = new g2o::VertexSE3Expmap();
    
     pose->setEstimate(g2o::SE3Quat(Tcw.rotation(), Tcw.translation()));
    
     pose->setId(0);
    
     optimizer.addVertex(pose);
    
     // 添加边
    
     int id = 1;
    
     for (Measurement m : measurements)
    
     {
    
     EdgeSE3ProjectDirect* edge = new EdgeSE3ProjectDirect(
    
         m.pos_world,
    
         K(0, 0), K(1, 1), K(0, 2), K(1, 2), gray
    
     );
    
     edge->setVertex(0, pose);
    
     edge->setMeasurement(m.grayscale);
    
     edge->setInformation(Eigen::Matrix<double, 1, 1>::Identity());
    
     edge->setId(id++);
    
     optimizer.addEdge(edge);
    
     }
    
     cout << "edges in graph: " << optimizer.edges().size() << endl;
    
     optimizer.initializeOptimization();
    
     optimizer.optimize(30);
    
     Tcw = pose->estimate();
    
 }

 cmake_minimum_required( VERSION 2.8 )

    
 project( directMethod )
    
  
    
 set( CMAKE_BUILD_TYPE Release )
    
 set( CMAKE_CXX_FLAGS "-std=c++11 -O3" )
    
  
    
 # 添加cmake模块路径
    
 list( APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake_modules )
    
  
    
 find_package( OpenCV )
    
 include_directories( ${OpenCV_INCLUDE_DIRS} )
    
  
    
 find_package( G2O )
    
 include_directories( ${G2O_INCLUDE_DIRS} ) 
    
  
    
 include_directories( "/usr/include/eigen3" )
    
  
    
 set( G2O_LIBS 
    
     g2o_core g2o_types_sba g2o_solver_csparse g2o_stuff g2o_csparse_extension 
    
 )
    
  
    
 add_executable( direct_sparse direct_sparse.cpp )
    
 target_link_libraries( direct_sparse ${OpenCV_LIBS} ${G2O_LIBS} )
    
  
    
 add_executable( direct_semidense direct_semidense.cpp )
    
 target_link_libraries( direct_semidense ${OpenCV_LIBS} ${G2O_LIBS} )

 // select the pixels with high gradiants

    
         //双层遍历循环像素点，上下左右10像素以内的边缘不考虑
    
         for ( int x=10; x<gray.cols-10; x++ )
    
             for ( int y=10; y<gray.rows-10; y++ )
    
             {
    
 //delta为梯度向量，x方向梯度值为 x+1 灰度值 减 x-1 灰度值，y方向梯度值为 y+1 灰度值 减 y-1 灰度值，所以（x,y）处的像素梯度与上下左右的像素灰度有关
    
                 Eigen::Vector2d delta (
    
                     gray.ptr<uchar>(y)[x+1] - gray.ptr<uchar>(y)[x-1], 
    
                     gray.ptr<uchar>(y+1)[x] - gray.ptr<uchar>(y-1)[x]
    
                 );
    
 //如果模长小于50,即任务就是梯度不明显，continue，其他的就开始对应深度和空间点，push_back到measurements
    
 //跟稀疏比在第一帧中多取了一些像素点。稠密将所有点全push进measurements
    
                 if ( delta.norm() < 50 )
    
                     continue;
    
                 ushort d = depth.ptr<ushort> (y)[x];
    
                 if ( d==0 )
    
                     continue;
    
                 Eigen::Vector3d p3d = project2Dto3D ( x, y, d, fx, fy, cx, cy, depth_scale );
    
                 float grayscale = float ( gray.ptr<uchar> (y) [x] );
    
                 measurements.push_back ( Measurement ( p3d, grayscale ) );
    
             }