目前用的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]