想直接改动,在《还是g2o初始化一些》那篇,这篇比较啰嗦,主要是记录自己思考的步骤。
首先说明主题:没文化真可怕
好了,说干货。之前高博的代码。只要涉及g2o的部分,一律跑不通,没重装之前,只是报一个段错误,就没了。也排查不出来问题。
今日2017年9月24日重装系统,重新搭建环境,除了opencv外,其他一律重新下载安装的。g2o从github下载安装的。
先贴一下g2o初始化的代码:
// 初始化g2o
typedef g2o::BlockSolver< g2o::BlockSolverTraits<6,3> > Block; // pose 维度为 6, landmark 维度为 3
Block::LinearSolverType* linearSolver = new g2o::LinearSolverCSparse<Block::PoseMatrixType>(); // 线性方程求解器
//std::unique_ptr<Block::LinearSolverType> linearSolver ( new g2o::LinearSolverCSparse<Block::PoseMatrixType>());
Block* solver_ptr = new Block ( linearSolver );
//std::unique_ptr<Block> solver_ptr ( new Block ( std::move(linearSolver))); // 矩阵块求解器
g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg ( solver_ptr);
//g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg ( std::move(solver_ptr));
g2o::SparseOptimizer optimizer;
optimizer.setAlgorithm ( solver );
安装好之后,运行第七章的3d2d程序,还是跑不通:
报错:
/home/robin/CLionProjects/slambook-robin/ch7/pose_estimation_3d2d/main.cpp:217:50: error: no matching function for call to ‘g2o::BlockSolver<g2o::BlockSolverTraits<6, 3> >::BlockSolver(g2o::BlockSolver<g2o::BlockSolverTraits<6, 3> >::LinearSolverType*&)’
Block* solver_ptr = new Block ( linearSolver );
点开位置发现是g2o初始化中的Block构造的时候报错:
Block* solver_ptr = new Block ( linearSolver );
继续点下面的错误信息:
In file included from /usr/local/include/g2o/core/block_solver.h:199:0,
from /home/robin/CLionProjects/slambook-robin/ch7/pose_estimation_3d2d/main.cpp:11:
/usr/local/include/g2o/core/block_solver.hpp:40:1: note: candidate: g2o::BlockSolver<Traits>::BlockSolver(std::unique_ptr<typename Traits::LinearSolverType>) [with Traits = g2o::BlockSolverTraits<6, 3>; typename Traits::LinearSolverType = g2o::LinearSolver<Eigen::Matrix<double, 6, 6, 0> >]
BlockSolver<Traits>::BlockSolver(std::unique_ptr<LinearSolverType> linearSolver)
发现跳转到了block_solver.hpp中BlockSolver的构造函数去了,发现是这样写的:
BlockSolver<Traits>::BlockSolver(std::unique_ptr<LinearSolverType> linearSolver)
: BlockSolverBase(),
_linearSolver(std::move(linearSolver))
发现BlockSolver构造时需要的是std::unique_ptr类型的linearSolver参数,这里还知道是啥玩意儿,不过看见ptr猜测是个指针。对比程序中上面写的,发现是这样的:
Block::LinearSolverType* linearSolver
猜测是不是因为std::unique_ptr与Block::LinearSolverType*类型不一样导致的?
但是高博写的应该没有错误,是不是g2o更新导致用法不一样了?看一下高博书中自带的g2o的代码,发现果然不一样:
高博书中的g2o是这么写的:
BlockSolver<Traits>::BlockSolver(LinearSolverType* linearSolver) :
BlockSolverBase(),
_linearSolver(linearSolver)
好,找到了问题原因,下面修改:
百度一发unique_ptr用法,这里是个网址:
http://blog.csdn.net/pi9nc/article/details/12227887
参考着进行改动:
// 初始化g2o
typedef g2o::BlockSolver< g2o::BlockSolverTraits<6,3> > Block; // pose 维度为 6, landmark 维度为 3
//Block::LinearSolverType* linearSolver = new g2o::LinearSolverCSparse<Block::PoseMatrixType>(); // 线性方程求解器
std::unique_ptr<Block::LinearSolverType> linearSolver ( new g2o::LinearSolverCSparse<Block::PoseMatrixType>());
//Block* solver_ptr = new Block ( linearSolver );
std::unique_ptr<Block> solver_ptr ( new Block ( linearSolver));
//std::unique_ptr<Block> solver_ptr ( new Block ( std::move(linearSolver))); // 矩阵块求解器
g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg ( solver_ptr);
//g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg ( std::move(solver_ptr));
g2o::SparseOptimizer optimizer;
optimizer.setAlgorithm ( solver );
把linearSolver和solver_ptr都改成了std::unique_ptr<>类型,并且初始化时不再用等号。
运行,发现还是报错:
/home/robin/CLionProjects/slambook-robin/ch7/pose_estimation_3d2d/main.cpp:218:65: error: use of deleted function ‘std::unique_ptr<_Tp, _Dp>::unique_ptr(const std::unique_ptr<_Tp, _Dp>&) [with _Tp = g2o::LinearSolver<Eigen::Matrix<double, 6, 6, 0> >; _Dp = std::default_delete<g2o::LinearSolver<Eigen::Matrix<double, 6, 6, 0> > >]’
std::unique_ptr<Block> solver_ptr ( new Block ( linearSolver));
在这一句:
std::unique_ptr<Block> solver_ptr ( new Block ( linearSolver));
并且后面的一句:
g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg ( solver_ptr);
也是报错。
第一句的:
std::unique_ptr<Block::LinearSolverType> linearSolver ( new g2o::LinearSolverCSparse<Block::PoseMatrixType>());
并没有报错,说明这一句是对的!
观察报错两句,发现都存在将unique_ptr作为参数传递给类的构造函数中的用法,然后那篇博客中有个move指令,猜测是不是直接传递不行,要用move才对,继续修改:
// 初始化g2o
typedef g2o::BlockSolver< g2o::BlockSolverTraits<6,3> > Block; // pose 维度为 6, landmark 维度为 3
//Block::LinearSolverType* linearSolver = new g2o::LinearSolverCSparse<Block::PoseMatrixType>(); // 线性方程求解器
std::unique_ptr<Block::LinearSolverType> linearSolver ( new g2o::LinearSolverCSparse<Block::PoseMatrixType>());
//Block* solver_ptr = new Block ( linearSolver );
//std::unique_ptr<Block> solver_ptr ( new Block ( linearSolver));
std::unique_ptr<Block> solver_ptr ( new Block ( std::move(linearSolver))); // 矩阵块求解器
//g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg ( solver_ptr);
g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg ( std::move(solver_ptr));
g2o::SparseOptimizer optimizer;
optimizer.setAlgorithm ( solver );
在传递linearSolver和solver_ptr时,加上了std::move()。再试一发!
OK,没毛病了,在得出RT之后调用BA,优化结果输出,并且退出时没有报错!
运行结果:
R=
[0.9978662026232452, -0.05167241669574823, 0.03991244185821538;
0.0505958920800001, 0.9983397626650758, 0.02752769489929838;
-0.04126860025362437, -0.02544955077522656, 0.9988239199170421]
t=
[-0.6361298163857759;
-0.03753650998182134;
0.3069292389205321]
calling bundle adjustment
iteration= 0 chi2= 1.040183 time= 0.00270625 cumTime= 0.00270625 edges= 76 schur= 1 lambda= 78.843278 levenbergIter= 1
iteration= 1 chi2= 0.000789 time= 0.00202973 cumTime= 0.00473598 edges= 76 schur= 1 lambda= 26.281093 levenbergIter= 1
iteration= 2 chi2= 0.000000 time= 0.00200341 cumTime= 0.00673939 edges= 76 schur= 1 lambda= 17.520729 levenbergIter= 1
iteration= 3 chi2= 0.000000 time= 0.00202996 cumTime= 0.00876935 edges= 76 schur= 1 lambda= 11.680486 levenbergIter= 1
iteration= 4 chi2= 0.000000 time= 0.00200547 cumTime= 0.0107748 edges= 76 schur= 1 lambda= 7.786990 levenbergIter= 1
iteration= 5 chi2= 0.000000 time= 0.00200838 cumTime= 0.0127832 edges= 76 schur= 1 lambda= 5.191327 levenbergIter= 1
iteration= 6 chi2= 0.000000 time= 0.00200928 cumTime= 0.0147925 edges= 76 schur= 1 lambda= 3.460885 levenbergIter= 1
iteration= 7 chi2= 0.000000 time= 0.00202289 cumTime= 0.0168154 edges= 76 schur= 1 lambda= 2.307256 levenbergIter= 1
iteration= 8 chi2= 0.000000 time= 0.00197821 cumTime= 0.0187936 edges= 76 schur= 1 lambda= 1.538171 levenbergIter= 1
iteration= 9 chi2= 0.000000 time= 0.00202251 cumTime= 0.0208161 edges= 76 schur= 1 lambda= 1.025447 levenbergIter= 1
iteration= 10 chi2= 0.000000 time= 0.00206846 cumTime= 0.0228846 edges= 76 schur= 1 lambda= 0.683632 levenbergIter= 1
iteration= 11 chi2= 0.000000 time= 0.00207496 cumTime= 0.0249595 edges= 76 schur= 1 lambda= 0.455754 levenbergIter= 1
iteration= 12 chi2= 0.000000 time= 0.0133918 cumTime= 0.0383513 edges= 76 schur= 1 lambda= 122340628.619249 levenbergIter= 7
after optimization:
T=
0.997777 -0.0521927 0.0414303 -0.647023
0.0510161 0.998278 0.0289658 -0.0492099
-0.0428708 -0.0267878 0.998721 0.3004
0 0 0 1
Process finished with exit code 0