视觉SLAM14讲——ch7课后习题六（代码学习）

最新推荐文章于 2023-09-13 09:00:17 发布

林pangpang

最新推荐文章于 2023-09-13 09:00:17 发布

阅读量520

点赞数 1

分类专栏：视觉SLAM学习文章标签：学习

本文链接：https://blog.csdn.net/linxinlrh/article/details/128928300

版权

视觉SLAM学习专栏收录该内容

5 篇文章 2 订阅

订阅专栏

1. 代码

代码参考：视觉slam十四讲第七章课后习题6 - 灰色的石头 - 博客园

使用来自视觉SLAM14讲ch7源码中的图像和深度图，两帧图像间相机的位姿变换。

在上文的基础上做了一点小改动：

1）本人的opencv版本是4.2.0，为了使用CV_LOAD_IMAGE_COLOR，另外添加了头文件

#include <opencv2/imgcodecs/legacy/constants_c.h>

2）在工程文件创建一个cmake文件夹，里面放入FindCSparse.cmake和FindG2O.cmake。

CMakeList.txt文件如下：

cmake_minimum_required(VERSION 2.8)
project(cap7_6_pnp)

set( CMAKE_CXX_STANDARD 14)
#SET(G2O_LIBS g2o_cli g2o_ext_freeglut_minimal g2o_simulator g2o_solver_slam2d_linear g2o_types_icp g2o_types_slam2d g2o_core g2o_interface g2o_solver_csparse g2o_solver_structure_only g2o_types_sba g2o_types_slam3d g2o_csparse_extension g2o_opengl_helper g2o_solver_dense g2o_stuff g2o_types_sclam2d g2o_parser g2o_solver_pcg g2o_types_data g2o_types_sim3 cxsparse )
set( CMAKE_BUILD_TYPE Debug )
# 添加cmake模块以使用g2o和CSprase
list(APPEND CMAKE_MODULE_PATH  ${PROJECT_SOURCE_DIR}/cmake)

find_package(OpenCV REQUIRED)
include_directories(${OpenCV_INCLUDE_DIRECTORIES})

include_directories("/usr/include/eigen3")

find_package(G2O REQUIRED)
include_directories(${G2O_INCLUDE_DIRECTORIES})

find_package(CSparse REQUIRED)
include_directories(${CSPARSE_INCLUDE_DIR})

find_package(Sophus  REQUIRED)
include_directories(${Sophus_INCLUDE_DIRS})

add_executable(cap7_6_pnp cap7_6_pnp.cpp)

target_link_libraries( cap7_6_pnp
        ${OpenCV_LIBS}
        g2o_core g2o_stuff g2o_types_sba g2o_csparse_extension
        g2o_types_slam3d
        #${G2O_LIBS}
        ${CSPARSE_LIBRARY}
        ${Sophus_LIBRARIES} 
        fmt
        )

2. 代码分析以及学习（助于学习g2o使用）

2.1 主函数

可分为三个部分：

1）读取图片的信息，并进行特征的提取与匹配。

    //读取图片
    img_1 = cv::imread(argv[1], CV_LOAD_IMAGE_COLOR);//读取彩色图
    img_2 = cv::imread(argv[2],1);
    //接下来的是建立3d点 利用深度图可以获取深度信息
    //depth1是图1对应的深度图 depCV_LOAD_IMAGE_UNCHANGEDth2是图2对应的深度图
    d1 = cv::imread(argv[3], CV_LOAD_IMAGE_UNCHANGED);// 深度图为16位无符号数，单通道图像
    d2 = cv::imread(argv[4], -1);
    assert(img_1.data != nullptr && img_2.data != nullptr);//若读取的图片没有内容，就终止程序


    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;

    Mat K = ( Mat_<double> ( 3,3 ) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1 );
    //初始化
    vector<Point3f> pts_3d;     //相机坐标系下的3D坐标
    vector<Point2f> pts_2d;     //第二帧下的像素坐标
#if MyselfBAFunc
    vector<Point2f> pts1_2d;    //第一帧下的像素坐标
#endif
    for ( DMatch m:matches )
    {
        //找匹配点对应的深度
        ushort d = d1.ptr<unsigned short> (int ( keypoints_1[m.queryIdx].pt.y )) [ int ( keypoints_1[m.queryIdx].pt.x ) ];
        if ( d == 0 )   // bad depth
            continue;
        float dd = d/5000.0;
        Point2d p1 = pixel2cam ( keypoints_1[m.queryIdx].pt, K );  //p1里面放的是图1特征点在相机坐标下的归一化坐标
        pts_3d.push_back ( Point3f ( p1.x*dd, p1.y*dd, dd ) );//计算相机坐标系下的3D坐标 
        pts_2d.push_back ( keypoints_2[m.trainIdx].pt );      //第二帧匹配好的点的2D像素坐标
#if MyselfBAFunc
        pts1_2d.push_back( keypoints_1[m.queryIdx].pt );      //第一帧匹配好的点2D像素坐标
#endif
    }

这里的find_feature_matches函数是自己封装定义的，原理较简单，在视觉SLAM14讲ch7源码中经常用到。

pixel2cam函数也是自己封装定义的，将像素坐标转换为相机归一化平面坐标，原理见书P99(5.5)，内容如下：

Point2d pixel2cam ( const Point2d& p, const Mat& K )
{
    return Point2d
            (
                    ( p.x - K.at<double> ( 0,2 ) ) / K.at<double> ( 0,0 ),
                    ( p.y - K.at<double> ( 1,2 ) ) / K.at<double> ( 1,1 )
            );
}

2）调用Opencv的solvePnP（）进行求解

   //在视觉SLAM书P182中讲P3P的时候提及使用的3D点坐标是世界坐标系中的坐标，
   //但在代码中用的是相机坐标系下的3D坐标，这里应当是因为仅有两帧图像帧，求的是相机的相对运动，所以将第一帧图像的坐标系看做世界坐标系
   //思路类似书P180 7.7.1的第一段
    cout<<"3d-2d pairs: "<<pts_3d.size() <<endl;
    Mat r, t; //定义旋转和平移变量
    //参数信息: 第一帧3D 第二帧像素2D 内参矩阵k 无失真补偿  旋转向量r 平移向量t false表示输入的r t不作为初始化值 如果是true则此时会把t r作为初始值进行迭代
    solvePnP ( pts_3d, pts_2d, K, Mat(), r, t, false,SOLVEPNP_EPNP ); // 调用OpenCV 的 PnP 求解，可选择EPNP，DLS等方法SOLVEPNP_EPNP
    Mat R;
    cv::Rodrigues ( r, R );                                 // r为旋转向量形式，用Rodrigues公式转换为矩阵

    cout<<"R="<<endl<<R<<endl;
    cout<<"t="<<endl<<t<<endl;

3）在solvePnP()结果的基础上使用g2o进行优化

MyselfBAFun( pts_3d, pts1_2d, pts_2d, K, R, t);

该优化函数为自己封装的，具体内容见下文。

2.2 MyselfBAFun函数

2.2.1 设置优化器

这个Solver实际是由一个BlockSolver组成的。这个BlockSolver有两个部分，一个是SparseBlockMatrix ，用于计算稀疏的雅可比和Hessian矩阵；一个是线性方程的求解器（LinearSolver），它用于计算迭代过程中最关键的一步HΔx=−b，LinearSolver有几种方法可以选择：PCG, CSparse, Choldmod。

//*****优化器的设置*****//
    typedef g2o::BlockSolver< g2o::BlockSolverTraits<6,3> > Block;  //优化位姿6维（误差项优化变量）  优化路标点3维（误差值维度）
    //创建一个线性求解器LinearSolver，设为CSparse
    std::unique_ptr<Block::LinearSolverType> linearSolver=g2o::make_unique < g2o::LinearSolverCSparse<Block::PoseMatrixType> >();
    //创建BlockSolver。并用上面定义的线性求解器初始化
    std::unique_ptr<Block> solver_ptr (new Block(std::move(linearSolver) ) );
    //创建总求解器solver。并从GN, LM, DogLeg 中选一个，再用上述块求解器BlockSolver初始化
    g2o::OptimizationAlgorithmLevenberg *solver = new g2o::OptimizationAlgorithmLevenberg( std::move(solver_ptr) );

/*  Block::LinearSolverType *linearSolver = new g2o::LinearSolverCSparse<Block::PoseMatrixType>();  //设置线性求解器类型
    Block *solver_ptr = new Block( std::unique_ptr<Block::LinearSolverType>(linearSolver) );  //矩阵块求解器
    g2o::OptimizationAlgorithmLevenberg *solver = new g2o::OptimizationAlgorithmLevenberg( std::unique_ptr<g2o::Solver>(solver_ptr) ); //LM优化算法
*/
    g2o::SparseOptimizer optimizer;     //设置稀疏优化器
    optimizer.setAlgorithm(solver);     //设置优化算法

代码中的std::move只是进行了左右值的转换，也就是类型的转换，并没有改变对象本身。

2.2.2 向优化器添加节点

2.2.2.1 添加位姿节点

 //（1）添加位姿节点 第一帧作为世界坐标系（不优化） 同时也是相机坐标系
    int poseVertexIndex = 0;                                       //位姿节点索引为0  总共两个位姿态节点 第一帧和第二帧
    Eigen::Matrix3d R2Init;
    R2Init <<
           R.at<double> ( 0,0 ), R.at<double> ( 0,1 ), R.at<double> ( 0,2 ) ,
            R.at<double> ( 1,0 ), R.at<double> ( 1,1 ), R.at<double> ( 1,2 ) ,
            R.at<double> ( 2,0 ), R.at<double> ( 2,1 ), R.at<double> ( 2,2 ) ;
    for( ; poseVertexIndex < PoseVertexNum ; poseVertexIndex++ )
    {
        auto pose = new g2o::VertexSE3Expmap();  //相机位姿
        pose->setId( poseVertexIndex );          //设置节点标号
        pose->setFixed( poseVertexIndex == 0 );  //如果是第一帧，则固定住不优化
        if( poseVertexIndex == 1 )               //第二帧时让RT估计值为pnp得到的值
            pose->setEstimate(
                    g2o::SE3Quat( R2Init,
                                  Eigen::Vector3d( t.at<double> ( 0,0 ), t.at<double> ( 1,0 ), t.at<double> ( 2,0 ) )
                    )
            );        
        else
            pose->setEstimate( g2o::SE3Quat() );
        optimizer.addVertex( pose );             //位姿节点加入优化器

一些细节：

这里使用的节点类为g2o::VertexSE3Expmap()，定义如下：

VertexSE3Expmap();

  bool read(std::istream& is);

  bool write(std::ostream& os) const;

  virtual void setToOriginImpl() {
    _estimate = SE3Quat();
  }

  virtual void oplusImpl(const number_t* update_)  {
    Eigen::Map<const Vector6> update(update_);
    setEstimate(SE3Quat::exp(update)*estimate());
  }
};

估计值参数的形式为SE3Quat()，定义如下，在不给参数时_r为单位四元数，_t为0，即不旋转不平移，_r与_t为重要参数，后面还有用到。

class G2O_TYPES_SLAM3D_API SE3Quat {
    public:
      EIGEN_MAKE_ALIGNED_OPERATOR_NEW;

    protected:

      Quaternion _r;
      Vector3 _t;

    public:
      SE3Quat(){
        _r.setIdentity();
        _t.setZero();
      }

      SE3Quat(const Matrix3& R, const Vector3& t):_r(Quaternion(R)),_t(t){ 
        normalizeRotation();
      }

      SE3Quat(const Quaternion& q, const Vector3& t):_r(q),_t(t){
        normalizeRotation();
      }

2.2.2.2 添加路标节点

int landmarkVertexIndex = PoseVertexNum ;
    for( int i = 0;  i < points1_3d.size() ; i ++ ){
        auto point = new g2o::VertexSBAPointXYZ();                 //路标点
        point->setId( landmarkVertexIndex + i );                   //设置路标点节点标号
        point->setMarginalized( true );                            //设置边缘化
        //设置估计值 实际上就是第一帧坐标下的3d点坐标（也是世界坐标系下的坐标）
        point->setEstimate( Eigen::Vector3d( points1_3d[i].x, points1_3d[i].y, points1_3d[i].z ) ); 
        optimizer.addVertex( point );                              //路标节点加入优化器
    }

一些细节：

使用的节点的类为g2o::VertexSBAPointXYZ()，定义如下：

class G2O_TYPES_SBA_API VertexSBAPointXYZ : public BaseVertex<3, Vector3>
{
  public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW    
    VertexSBAPointXYZ();
    virtual bool read(std::istream& is);
    virtual bool write(std::ostream& os) const;

    virtual void setToOriginImpl() {
      _estimate.fill(0);
    }

    virtual void oplusImpl(const number_t* update)
    {
      Eigen::Map<const Vector3> v(update);
      _estimate += v;
    }
};

在添加 VertexSBAPointXYZ 类型节点时，会使用到其中的 setMarginalized(true) 函数，需要将所有的三维点边缘化掉，以便稀疏化求解。简单的说G2O 中对路标点设置边缘化是为了在计算求解过程中，先消去路标点变量，实现先求解相机位姿，然后再利用求解出来的相机位姿，反过来计算路标点的过程，目的是为了加速求解，并非真的将路标点给边缘化掉。

参考：(01)ORB-SLAM2源码无死角解析-(63) BA优化(g2o)→局部建图线程:Optimizer::LocalBundleAdjustment→位姿与地图点优化_need.xyz_江南才尽，年少无知！的博客-CSDN博客

2.2.3 相机参数的设置

//加入相机参数(最后一项为0 默认fx = fy)
    g2o::CameraParameters *camera = new g2o::CameraParameters(
            K.at< double >(0,0), Eigen::Vector2d( K.at< double >(0,2), K.at< double >(1,2) ),0
    );
    camera->setId( 0 );
    optimizer.addParameter( camera );

2.2.4 向优化器添加边

分别将相机坐标系下的路标节点与第一帧位姿节点和第二帧位姿节点相连接。

/*******向优化器中添加边********//
    //加入边edge
    //世界坐标下路标点连接到第一帧位姿节点（因为以第一帧坐标系当做世界坐标系 所以我们前面没有优化第一帧的RT  仅仅优化第一帧到第二帧的RT）
    for(int i= 0 ;i < points1_2d.size() ; i++ ){
        auto edge = new g2o::EdgeProjectXYZ2UV();               //设置连接到第一帧的边
        //二元边 连接节点
        edge->setVertex( 0, dynamic_cast< g2o::VertexSBAPointXYZ * >( optimizer.vertex( landmarkVertexIndex + i ) ) ); //世界坐标系下的路标节点
        edge->setVertex( 1, dynamic_cast< g2o::VertexSE3Expmap * >( optimizer.vertex(0) ) );
        edge->setMeasurement( Eigen::Vector2d ( points1_2d[i].x, points1_2d[i].y ) );   //设置测量值为第一帧下的相机归一化平面坐标
        edge->setParameterId(0,0); //最后一位设置使用的相机参数(因为上面仅仅输入了一个相机参数id=0, 对应上面camer->setId(0),第一个参数0不知道是什么，但是必须为0)
        edge->setInformation ( Eigen::Matrix2d::Identity() );   //信息矩阵2x2的单位阵
        optimizer.addEdge( edge );
    }
    //第一帧路标点链接到第二帧位姿节点
    for( int i=0 ;i < points1_2d.size() ; i++){
        auto edge = new g2o::EdgeProjectXYZ2UV();   //设置链接到第二帧的边
        edge->setVertex( 0, dynamic_cast< g2o::VertexSBAPointXYZ * >( optimizer.vertex( landmarkVertexIndex + i) ) ); //第一帧坐标系下的路标点
        edge->setVertex( 1, dynamic_cast< g2o::VertexSE3Expmap *> ( optimizer.vertex(1) ) ); //连接到第二个位姿节点
        edge->setMeasurement( Eigen::Vector2d ( points2_2d[i].x, points2_2d[i].y ) );        //设置测量值为第二帧下的相机归一化平面坐标
        edge->setInformation( Eigen::Matrix2d::Identity() ); //信息矩阵为2x2 实际上就是误差的加权为1：1的
        edge->setParameterId(0,0);
        optimizer.addEdge( edge );

一些细节：

使用的边的类为g2o::EdgeProjectXYZ2UV()，具体内容如下：

class G2O_TYPES_SBA_API EdgeProjectXYZ2UV : public  BaseBinaryEdge<2, Vector2, VertexSBAPointXYZ, VertexSE3Expmap>{
  public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;

    EdgeProjectXYZ2UV();

    bool read(std::istream& is);

    bool write(std::ostream& os) const;

    void computeError()  {
      const VertexSE3Expmap* v1 = static_cast<const VertexSE3Expmap*>(_vertices[1]);
      const VertexSBAPointXYZ* v2 = static_cast<const VertexSBAPointXYZ*>(_vertices[0]);
      const CameraParameters * cam
        = static_cast<const CameraParameters *>(parameter(0));
      Vector2 obs(_measurement);
      //cam_map 函数功能是把相机坐标系下三维点（输入）用内参转换为图像坐标（输出），具体代码如下所示
      _error = obs-cam->cam_map(v1->estimate().map(v2->estimate()));  
      
    }

    virtual void linearizeOplus();

    CameraParameters * _cam;
};

map函数：

Vector3 map(const Vector3 & xyz) const
      {
        return _r*xyz + _t;
      }

这里的误差定义为：观测值（各帧中相机归一化平面坐标）- cam_map（相机坐标系下的三维点坐标）。

v1->estimate().map(v2->estimate())这部分的含义为将此时的路标节点的估计值（世界坐标系下）通过此时位姿节点的估计值变换到次位姿下的三维点坐标。

2.2.5 优化开始

    cout<<"开始优化!"<<endl;
    chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
    optimizer.setVerbose ( true );          //设置详细信息
    optimizer.initializeOptimization( );    //优化器初始化
    optimizer.optimize( 100 );              //进行最多100次的优化
    chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
    cout<<"优化结束!"<<endl;
    chrono::duration< double > time_used = chrono::duration_cast< chrono::duration<double> >( t2 -t1 );
    cout<<"optimization costs time: "<<time_used.count() << "seconds."<<endl;
    cout<<endl<<"after optimization:"<<endl;
    //Eigen::Isometry3d()用estimate()返回的SE3Quat类型初始化
    cout<<"T="<<endl<<Eigen::Isometry3d ( dynamic_cast<g2o::VertexSE3Expmap *>(optimizer.vertex(1))->estimate()).matrix() <<endl;

PS：本文仅为个人理解，有不正确欢迎指正。