Resolviendo algunos problemas en "SLAM Fourteen Lectures" Ch9 run_vo.cpp

Al compilar run_vo.cpp se notifica un error

Se encontró que no se agregó la dependencia de la biblioteca fmt, pero aún se informó un error después de agregarla, de la siguiente manera:
Cambios en src/CMakeLists.txt, antes de los cambios:

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})

Después de cambiar:

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})

Puede ver que la biblioteca dinámica se cambia a una biblioteca estática y la compilación es exitosa

Ejecutando el error run_vo.cpp

Problema 1: el número de valores internos siempre es 0 (valores internos PnP: 0)

Verifique el programa fuente. En el archivo run_vo.cpp, al crear una nueva Camera::Ptr, el constructor de copia predeterminado se usa directamente y no se proporcionan los parámetros internos de la cámara. Use la función get de la clase Config para inicializar los parámetros internos de la cámara

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");

Problema 2: Los ejes no cambian

Después de la depuración, se encuentra que la matriz Tcw y la matriz Aff_mat no son consistentes, como se muestra a continuación:

Código original:

// 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)
            )
        );

Modifique el código de la siguiente manera:

// 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)
            )
        );

resolución exitosa de problemas

Diferentes versiones de g2o

Pregunta 1: g2o::VertexSBAPointXYZ no es miembro de g2o

La nueva versión no, la versión anterior tiene

Problema 2: Hay diferencias en la construcción de 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 );

La nueva versión de g2o también se puede construir utilizando punteros unique_ptr<>, de la siguiente manera:

// 构建图优化，先设定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