slam14讲ch7实践记录

^。_ 。^

已于 2024-08-02 14:20:02 修改

阅读量514

点赞数 11

分类专栏： slam十四讲文章标签： linux ubuntu

于 2024-08-02 14:19:04 首次发布

本文链接：https://blog.csdn.net/phhone/article/details/140838542

版权

slam十四讲专栏收录该内容

4 篇文章 0 订阅

订阅专栏

目前用的opencv版本是3.4.12，g2o使用的版本是2023.02.23版的，对于新版本原代码里面的一些用法需要修改。

首先在CMakeList.txt里面把C++11改为C++17

1.orb_cv

编译完成后终端输入：

./feature_extraction ../1.png ../2.png

输出结果正确

2.2d_2d

终端输入：

./pose_estimation_2d2d ../1.png ../2.png

输出结果

3.3d_2d（这里遇到了很多问题）

这里编译出现了错误

报错1：

 g2o::VertexSBAPointXYZ* point = new g2o::VertexSBAPointXYZ();

根据报错内容在较新版本的g2o库中,VertexSBAPointXYZ已经被改名为VertexPointXYZ，所以这里把代码里面的VertexSBAPointXYZ改为VertexPointXYZ就可以了。

`报错2：`

/home/phh/SLAM/slambook-master/ch7/pose_estimation_3d2d.cpp: In function ‘void bundleAdjustment(std::vector<cv::Point3_<float> >, std::vector<cv::Point_<float> >, const cv::Mat&, cv::Mat&, cv::Mat&)’:
/home/phh/SLAM/slambook-master/ch7/pose_estimation_3d2d.cpp:151:50: error: no matching function for call to ‘g2o::BlockSolver<g2o::BlockSolverTraits<6, 3> >::BlockSolver(g2o::BlockSolver<g2o::BlockSolverTraits<6, 3> >::LinearSolverType*&)

解决办法：

根据GPT的解释：

这段代码片段是在使用 g2o 库进行优化求解的过程中,构建 BlockSolver 和 OptimizationAlgorithmLevenberg 对象。

我注意到您使用的是 g2o::LinearSolverCSparse 作为线性方程求解器,这是一个非常好的选择。

在新版本的 g2o 库中,BlockSolver 的构造函数确实发生了一些变化,主要是需要使用 std::unique_ptr 来管理线性求解器的内存。

您提供的代码中,使用的是旧版本的构造方式,这可能会导致编译错误。

修改之后的代码：

// 初始化g2o
    // typedef g2o::BlockSolver< g2o::BlockSolverTraits<6,3> > Block;  // pose 维度为 6, landmark 维度为 3
    // Block::LinearSolverType* linearSolver = new g2o::LinearSolverCSparse<Block::PoseMatrixType>(); // 线性方程求解器
    // Block* solver_ptr = new Block ( linearSolver );     // 矩阵块求解器
    // g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg ( solver_ptr );
    // g2o::SparseOptimizer optimizer;
    // optimizer.setAlgorithm ( solver );

    typedef g2o::BlockSolver<g2o::BlockSolverTraits<6, 3>> Block;

    // 创建线性方程求解器
    // auto linearSolver = g2o::make_unique<g2o::LinearSolverCSparse<Block::PoseMatrixType>>();
    std::unique_ptr<g2o::LinearSolverCSparse<Block::PoseMatrixType>> linearSolver =
        std::make_unique<g2o::LinearSolverCSparse<Block::PoseMatrixType>>();
    // 创建 BlockSolver
    std::unique_ptr<Block> solver_ptr(new Block(std::move(linearSolver)));

    // 创建优化算法
    g2o::OptimizationAlgorithmLevenberg *solver = new g2o::OptimizationAlgorithmLevenberg(std::move(solver_ptr));

    // 创建优化器
    g2o::SparseOptimizer optimizer;
    optimizer.setAlgorithm(solver);

这里我把原代码给注释掉了。

报错3：

/usr/bin/ld: pose_estimation_3d2d.cpp:(.text+0x117a): undefined reference to vtable for g2o::VertexPointXYZ' /usr/bin/ld: pose_estimation_3d2d.cpp:(.text+0x1339): undefined reference to typeinfo for g2o::VertexPointXYZ'
collect2: error: ld returned 1 exit status
make[2]: *** [CMakeFiles/pose_estimation_3d2d.dir/build.make:115：pose_estimation_3d2d] 错误 1
make[1]: *** [CMakeFiles/Makefile2:169：CMakeFiles/pose_estimation_3d2d.dir/all] 错误 2
make: *** [Makefile:91：all] 错误 2

在修改完代码之后再次编译又出现了该报错

解决办法：

提示在编译 pose_estimation_3d2d.cpp 文件时找不到 g2o::VertexPointXYZ 的虚函数表和类型信息。这通常意味着您的代码没有正确链接到 g2o 库

在CMakeLisit.txt中pose_estimation_3d2d链接库那里进行修改，多链接了g2o_solver_csparse g2o_types_slam3d，下面是修改后的代码：

target_link_libraries( pose_estimation_3d2d 
   ${OpenCV_LIBS}
   ${CSPARSE_LIBRARY}
   g2o_core g2o_stuff g2o_types_sba g2o_csparse_extension g2o_solver_csparse g2o_types_slam3d
)

接着再进行编译，编译成功，输出结果：

./pose_estimation_3d2d ../1.png ../2.png ../1_depth.png ../2_depth.png

4.3d_3d

这里也报了3d_2d的报错2类似的错误，这里也像上面类似进行代码修改：

 // 初始化g2o
    //typedef g2o::BlockSolver< g2o::BlockSolverTraits<6,3> > Block;  // pose维度为 6, landmark 维度为 3
    //Block::LinearSolverType* linearSolver = new g2o::LinearSolverEigen<Block::PoseMatrixType>(); // 线性方程求解器
    //Block* solver_ptr = new Block( linearSolver );      // 矩阵块求解器
    //g2o::OptimizationAlgorithmGaussNewton* solver = new g2o::OptimizationAlgorithmGaussNewton( solver_ptr );
    //g2o::SparseOptimizer optimizer;
    //optimizer.setAlgorithm( solver );

    typedef g2o::BlockSolver<g2o::BlockSolverTraits<6, 3>> Block;

    // 创建线性方程求解器
    // auto linearSolver = g2o::make_unique<g2o::LinearSolverCSparse<Block::PoseMatrixType>>();
    std::unique_ptr<g2o::LinearSolverEigen<Block::PoseMatrixType>> linearSolver =
        std::make_unique<g2o::LinearSolverEigen<Block::PoseMatrixType>>();
    // 创建 BlockSolver
    std::unique_ptr<Block> solver_ptr(new Block(std::move(linearSolver)));

    // 创建优化算法
    g2o::OptimizationAlgorithmGaussNewton *solver = new g2o::OptimizationAlgorithmGaussNewton(std::move(solver_ptr));

    // 创建优化器
    g2o::SparseOptimizer optimizer;
    optimizer.setAlgorithm(solver);

成功编译并输出：

./pose_estimation_3d3d ../1.png ../2.png ../1_depth.png ../2_depth.png

-- Max dist : 94.000000 
-- Min dist : 4.000000 
一共找到了79组匹配点
3d-3d pairs: 72
W=  10.871 -1.01948  2.54771
-2.16033  3.85307 -5.77742
 3.94738 -5.79979  9.62203
U=  0.558087  -0.829399 -0.0252034
 -0.428009  -0.313755   0.847565
  0.710878   0.462228   0.530093
V=  0.617887  -0.784771 -0.0484806
 -0.399894  -0.366747   0.839989
  0.676979   0.499631   0.540434
ICP via SVD results: 
R = [0.9969452349468715, 0.05983347698056557, -0.05020113095482046;
 -0.05932607657705309, 0.9981719679735133, 0.01153858699565957;
 0.05079975545906246, -0.008525103184062521, 0.9986724725659557]
t = [0.144159841091821;
 -0.06667849443812729;
 -0.03009747273569774]
R_inv = [0.9969452349468715, -0.05932607657705309, 0.05079975545906246;
 0.05983347698056557, 0.9981719679735133, -0.008525103184062521;
 -0.05020113095482046, 0.01153858699565957, 0.9986724725659557]
t_inv = [-0.1461462958593589;
 0.05767443542067568;
 0.03806388018483625]
calling bundle adjustment
iteration= 0     chi2= 18161.146626      time= 2.0339e-05        cumTime= 2.0339e-05     edges= 72       schur= 0
iteration= 1     chi2= 18155.141919      time= 6.338e-06         cumTime= 2.6677e-05     edges= 72       schur= 0
iteration= 2     chi2= 18155.140765      time= 6.939e-06         cumTime= 3.3616e-05     edges= 72       schur= 0
iteration= 3     chi2= 18155.140764      time= 6.558e-06         cumTime= 4.0174e-05     edges= 72       schur= 0
iteration= 4     chi2= 18155.140764      time= 4.679e-06         cumTime= 4.4853e-05     edges= 72       schur= 0
iteration= 5     chi2= 18155.140764      time= 4.754e-06         cumTime= 4.9607e-05     edges= 72       schur= 0
iteration= 6     chi2= 18155.140764      time= 4.746e-06         cumTime= 5.4353e-05     edges= 72       schur= 0
iteration= 7     chi2= 18155.140764      time= 4.735e-06         cumTime= 5.9088e-05     edges= 72       schur= 0
iteration= 8     chi2= 18155.140764      time= 6.221e-06         cumTime= 6.5309e-05     edges= 72       schur= 0
iteration= 9     chi2= 18155.140764      time= 5.995e-06         cumTime= 7.1304e-05     edges= 72       schur= 0
optimization costs time: 0.000186302 seconds.

after optimization:
T=
  0.996945  0.0598335 -0.0502011    0.14416
-0.0593261   0.998172  0.0115386 -0.0666785
 0.0507998 -0.0085251   0.998672 -0.0300979
         0          0          0          1
p1 = [-0.0374123, -0.830816, 2.7448]
p2 = [-0.0111479, -0.746763, 2.7652]
(R*p2+t) = [-0.0504515635307114;
 -0.7795086988435547;
 2.737231472749827]

p1 = [-0.243698, -0.117719, 1.5848]
p2 = [-0.297211, -0.0956614, 1.6558]
(R*p2+t) = [-0.2409901730263577;
 -0.1254270238477566;
 1.60922166204228]

p1 = [-0.627753, 0.160186, 1.3396]
p2 = [-0.709645, 0.159033, 1.4212]
(R*p2+t) = [-0.625147830912345;
 0.1505624417599349;
 1.351810316139747]

p1 = [-0.323443, 0.104873, 1.4266]
p2 = [-0.399079, 0.12047, 1.4838]
(R*p2+t) = [-0.3209806451352846;
 0.09436780234544834;
 1.43043264949907]

p1 = [-0.627221, 0.101454, 1.3116]
p2 = [-0.709709, 0.100216, 1.3998]
(R*p2+t) = [-0.6276559464201075;
 0.09161024371284004;
 1.330936826808424]