[人工智能]《SLAM十四讲》Ch9 run_vo.cpp一些问题的解决

编译run_vo.cpp报错

发现是未添加对fmt库的依赖，但添加后依然报错，如下：
对src/CMakeLists.txt更改，更改前：

cmake_minimum_required(VERSION 2.8)
project(vo_practice)

add_library(vo_practice_lib SHARED
    frame.cpp
    mappoint.cpp
    map.cpp
    camera.cpp
    config.cpp
    visual_odometry.cpp
)

target_link_libraries(vo_practice_lib ${THIRD_PARTY_LIBS})

更改后：

cmake_minimum_required(VERSION 2.8)
project(vo_practice)

add_library(vo_practice_lib STATIC
    frame.cpp
    mappoint.cpp
    map.cpp
    camera.cpp
    config.cpp
    visual_odometry.cpp
)

target_link_libraries(vo_practice_lib ${THIRD_PARTY_LIBS})

可以看到将动态库更改为静态库，编译成功

运行run_vo.cpp错误

问题1：内点数始终为0(PnP inliers:0)

检查源程序，在run_vo.cpp文件中，在新建Camera::Ptr时，直接使用的是默认拷贝构造函数，没有提供相机的内参，利用Config类的get函数初始化相机内参

vo_practice::Camera::Ptr camera ( new vo_practice::Camera );
    camera->cx_=vo_practice::Config::get<float>("camera.cx");
    camera->cy_=vo_practice::Config::get<float>("camera.cy");
    camera->fx_=vo_practice::Config::get<float>("camera.fx");
    camera->fy_=vo_practice::Config::get<float>("camera.fy");
    camera->depth_scale_=vo_practice::Config::get<float>("camera.depth_scale");

问题2：坐标轴没有变化

调试后发现Tcw矩阵和Aff_mat矩阵并不一致，如下所示：

原代码：

// show the map and the camera pose 
        cv::Affine3d::Mat3 Aff_mat;
        for(int i=0;i<3;++i)
        {
            for(int j=0;j<3;++j)
            {
                Aff_mat<<Tcw.rotationMatrix()(i,j);
            }
        }
        cout<<"Tcw:\n"<<Tcw.rotationMatrix()<<endl<<endl;
        cout<<"Aff_mat:\n"<<Aff_mat<<endl<<endl;
        cv::Affine3d M(
            // cv::Affine3d::Mat3(
            //     Tcw.rotation_matrix()(0,0), Tcw.rotation_matrix()(0,1), Tcw.rotation_matrix()(0,2),
            //     Tcw.rotation_matrix()(1,0), Tcw.rotation_matrix()(1,1), Tcw.rotation_matrix()(1,2),
            //     Tcw.rotation_matrix()(2,0), Tcw.rotation_matrix()(2,1), Tcw.rotation_matrix()(2,2)

            // ), 
            Aff_mat,
            // Tcw.rotationMatrix(),
            cv::Affine3d::Vec3(
                Tcw.translation()(0,0), Tcw.translation()(1,0), Tcw.translation()(2,0)
            )
        );

修改代码如下：

// show the map and the camera pose 
        cv::Affine3d::Mat3 Aff_mat;
        for(int i=0;i<3;++i)
        {
            for(int j=0;j<3;++j)
            {
                Aff_mat(i,j)=Tcw.rotationMatrix()(i,j);
            }
        }
        // cout<<"Tcw:\n"<<Tcw.rotationMatrix()<<endl<<endl;
        // cout<<"Aff_mat:\n"<<Aff_mat<<endl<<endl;
        cv::Affine3d M(
            // cv::Affine3d::Mat3(
            //     Tcw.rotation_matrix()(0,0), Tcw.rotation_matrix()(0,1), Tcw.rotation_matrix()(0,2),
            //     Tcw.rotation_matrix()(1,0), Tcw.rotation_matrix()(1,1), Tcw.rotation_matrix()(1,2),
            //     Tcw.rotation_matrix()(2,0), Tcw.rotation_matrix()(2,1), Tcw.rotation_matrix()(2,2)

            // ), 
            Aff_mat,
            // Tcw.rotationMatrix(),
            cv::Affine3d::Vec3(
                Tcw.translation()(0,0), Tcw.translation()(1,0), Tcw.translation()(2,0)
            )
        );

成功解决问题

不同版本的g2o

问题1：g2o::VertexSBAPointXYZ不是g2o的成员

新版本没有，旧版本有

问题2：g2o的构建有差别

// using bundle adjustment to optimize the pose
    //构建图优化，设定g2o
    //旧版的g2o用下面的方法
    typedef g2o::BlockSolver<g2o::BlockSolverTraits<6,2>> Block;
    Block::LinearSolverType* linearSolver = new g2o::LinearSolverDense<Block::PoseMatrixType>();
    Block* solver_ptr = new Block( linearSolver );
    g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg ( solver_ptr );

    //新版的g2o用下面的方法
    // typedef g2o::BlockSolver<g2o::BlockSolverTraits<6, 2>> BlockSolverType;           // 求解的向量是6＊1的
    // typedef g2o::LinearSolverDense<BlockSolverType::PoseMatrixType> LinearSolverType; // 线性求解器类型
    // // 梯度下降方法，可以从GN, LM, DogLeg 中选
    // auto solver = new g2o::OptimizationAlgorithmLevenberg(
    //     g2o::make_unique<BlockSolverType>(g2o::make_unique<LinearSolverType>()));
    g2o::SparseOptimizer optimizer;
    optimizer.setAlgorithm ( solver );

新版的g2o也可以使用unique_ptr< >指针进行构造，如下：

// 构建图优化，先设定g2o
    typedef g2o::BlockSolver<g2o::BlockSolverTraits<6,1>> DirectBlock;  // 求解的向量是6＊1的
    DirectBlock::LinearSolverType* linearSolver = new g2o::LinearSolverDense<DirectBlock::PoseMatrixType> ();
    // DirectBlock* solver_ptr = new DirectBlock(linearSolver);
    DirectBlock* solver_ptr = new DirectBlock(unique_ptr<DirectBlock::LinearSolverType>(linearSolver)); 
    
    // g2o::OptimizationAlgorithmGaussNewton* solver = new g2o::OptimizationAlgorithmGaussNewton(solver_ptr); // G-N
    g2o::OptimizationAlgorithmGaussNewton* solver = new g2o::OptimizationAlgorithmGaussNewton(unique_ptr<DirectBlock>(solver_ptr)); // G-N

    // g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(solver); // L-M