SLAM十四讲中ch9的bundle_adjustment_g2o运行完毕后无法正常进行迭代（lambda、levenbergIter的数据也出不来）
xiyang@xiyang-virtual-machine:~/slambook2/ch9-2/build$ ./bundle_adjustment_g2o /home/xiyang/slambook2/ch9-2/problem-16-22106-pre.txt
Header: 16 22106 83718iteration= 0 chi2= 45883174188165936218898432.000000 time= 0.220091 cumTime= 0.220091 edges= 83718 schur= 1
iteration= 1 chi2= 364708460123077073572045745291264.000000 time= 0.192502 cumTime= 0.412593 edges= 83718 schur= 1
因为g2o的版本跟高翔的不一致，因此针对bundle_adjustment_g2o代码我进行了如下修改（这种修改在ch6、7、8中都没有任何问题）：
typedef g2o::BlockSolver< g2o::BlockSolverTraits<9,3> > Block;
Block::LinearSolverType* linearSolver = new g2o::LinearSolverDense<Block::PoseMatrixType>(); // 线性方程求解器
Block* solver_ptr = new Block( std::unique_ptr<Block::LinearSolverType>( linearSolver ) ); // 矩阵块求解器
g2o::OptimizationAlgorithmGaussNewton* solver = new g2o::OptimizationAlgorithmGaussNewton( unique_ptr<Block>(solver_ptr) );
g2o::SparseOptimizer optimizer; // 使用稀疏求解器
optimizer.setAlgorithm(solver);
optimizer.setVerbose(true);
这是bundle_adjustment_g2o完整代码
#include <g2o/core/base_vertex.h>
#include <g2o/core/base_unary_edge.h>
#include <g2o/core/sparse_optimizer.h>
#include <g2o/core/solver.h>
#include <g2o/core/optimization_algorithm_gauss_newton.h>
#include <g2o/solvers/dense/linear_solver_dense.h>
#include <chrono>
#include <g2o/core/base_binary_edge.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/solvers/csparse/linear_solver_csparse.h>
#include <g2o/core/robust_kernel_impl.h>
#include <iostream>
#include "common.h"
#include "sophus/se3.hpp"
using namespace Sophus;
using namespace Eigen;
using namespace std;
/// 姿态和内参的结构 Intrinsics内参
struct PoseAndIntrinsics {
PoseAndIntrinsics() {}
/// set from given data address
// 在构造函数之前显式地声明explicit，防止隐式转换。比如类的构造函数形参为int，外部一个函数的形参为这个类，如果
// 调用这个外部函数的时候传入int类型，那么编译器不会报错，而是会进行隐式的类型转化，把int作为类的构造函数的形参，
// 然后把生成一个类的对象传给外部调用的这个函数
explicit PoseAndIntrinsics(double *data_addr) { // 传入的数组数据
rotation = SO3d::exp(Vector3d(data_addr[0], data_addr[1], data_addr[2]));
translation = Vector3d(data_addr[3], data_addr[4], data_addr[5]);
focal = data_addr[6];
k1 = data_addr[7];
k2 = data_addr[8];
}
/// 将估计值放入内存
void set_to(double *data_addr) {
auto r = rotation.log(); // 把李群转成李代数存储
for (int i = 0; i < 3; ++i) data_addr[i] = r[i];
for (int i = 0; i < 3; ++i) data_addr[i + 3] = translation[i];
data_addr[6] = focal;
data_addr[7] = k1;
data_addr[8] = k2;
}
SO3d rotation; // 旋转矩阵
Vector3d translation = Vector3d::Zero();
double focal = 0;
double k1 = 0, k2 = 0;
};
/// 位姿加相机内参的顶点，9维，前三维为so3，接下去为t, f, k1, k2
// 这里设置顶点优化变量维度为9，要优化的变量类型是PoseAndIntrinsics这个类
class VertexPoseAndIntrinsics : public g2o::BaseVertex<9, PoseAndIntrinsics> {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
VertexPoseAndIntrinsics() {}
virtual void setToOriginImpl() override {
_estimate = PoseAndIntrinsics();
}
virtual void oplusImpl(const double *update) override {
_estimate.rotation = SO3d::exp(Vector3d(update[0], update[1], update[2])) * _estimate.rotation;
_estimate.translation += Vector3d(update[3], update[4], update[5]);
_estimate.focal += update[6];
_estimate.k1 += update[7];
_estimate.k2 += update[8];
}
/// 根据估计值投影一个点
Vector2d project(const Vector3d &point) { // 把3D点进行投影，得到估计的投影点位置
Vector3d pc = _estimate.rotation * point + _estimate.translation;
pc = -pc / pc[2];
double r2 = pc.squaredNorm();
double distortion = 1.0 + r2 * (_estimate.k1 + _estimate.k2 * r2);
return Vector2d(_estimate.focal * distortion * pc[0],
_estimate.focal * distortion * pc[1]);
}
virtual bool read(istream &in) {return true;}
virtual bool write(ostream &out) const {return true;}
};
// 顶点：路标点
class VertexPoint : public g2o::BaseVertex<3, Vector3d> {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
VertexPoint() {}
virtual void setToOriginImpl() override {
_estimate = Vector3d(0, 0, 0);
}
virtual void oplusImpl(const double *update) override {
_estimate += Vector3d(update[0], update[1], update[2]);
}
virtual bool read(istream &in) {return true;}
virtual bool write(ostream &out) const {return true;}
};
// 投影的边
// 边的维度为2，数据类型是Vecor2d，连接的两个顶点类型分别是VertexPoseAndIntrinsics, VertexPoint
class EdgeProjection :
public g2o::BaseBinaryEdge<2, Vector2d, VertexPoseAndIntrinsics, VertexPoint> {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
virtual void computeError() override {
auto v0 = (VertexPoseAndIntrinsics *) _vertices[0]; // 强制类型转换，_vertices[0]中存放的就是第一个输入的数据
auto v1 = (VertexPoint *) _vertices[1];
auto proj = v0->project(v1->estimate()); // 计算投影值
_error = proj - _measurement; // 计算误差值
}
// use numeric derivatives
// 这里没有声明雅克比的函数，所以是数值求导
virtual bool read(istream &in) {return true;}
virtual bool write(ostream &out) const {return true;}
};
void SolveBA(BALProblem &bal_problem);
int main(int argc, char **argv) {
if (argc != 2) {
cout << "usage: bundle_adjustment_g2o bal_data.txt" << endl;
return 1;
}
BALProblem bal_problem(argv[1]);
bal_problem.Normalize();
bal_problem.Perturb(0.1, 0.5, 0.5);
bal_problem.WriteToPLYFile("initial.ply");
SolveBA(bal_problem);
bal_problem.WriteToPLYFile("final.ply");
return 0;
}
void SolveBA(BALProblem &bal_problem) {
const int point_block_size = bal_problem.point_block_size();
const int camera_block_size = bal_problem.camera_block_size();
double *points = bal_problem.mutable_points();
double *cameras = bal_problem.mutable_cameras();
// pose dimension 9, landmark is 3
//typedef g2o::BlockSolver<g2o::BlockSolverTraits<9, 3>> BlockSolverType;
typedef g2o::BlockSolver< g2o::BlockSolverTraits<9,3> > Block;
Block::LinearSolverType* linearSolver = new g2o::LinearSolverDense<Block::PoseMatrixType>(); // 线性方程求解器(第二次实验)
Block* solver_ptr = new Block( std::unique_ptr<Block::LinearSolverType>( linearSolver ) ); // 矩阵块求解器(第二次实验)
g2o::OptimizationAlgorithmGaussNewton* solver = new g2o::OptimizationAlgorithmGaussNewton( unique_ptr<Block>(solver_ptr) );
//typedef g2o::LinearSolverCSparse<BlockSolverType::PoseMatrixType> LinearSolverType;
// use LM
//auto solver = new g2o::OptimizationAlgorithmLevenberg(
//g2o::make_unique<BlockSolverType>(g2o::make_unique<LinearSolverType>()));
g2o::SparseOptimizer optimizer; // 使用稀疏求解器
optimizer.setAlgorithm(solver);
optimizer.setVerbose(true);
/// build g2o problem
const double *observations = bal_problem.observations();
// vertex 存放顶点指针的容器
vector<VertexPoseAndIntrinsics *> vertex_pose_intrinsics;
vector<VertexPoint *> vertex_points;
// 对于16个相机位姿，添加16个顶点
for (int i = 0; i < bal_problem.num_cameras(); ++i) {
VertexPoseAndIntrinsics *v = new VertexPoseAndIntrinsics(); // 这个顶点类型继承自基础顶点类，所以后面有setId等成员函数
double *camera = cameras + camera_block_size * i; // 当前这个相机位姿的指针，索引9个数
v->setId(i); // 设置顶点Id
v->setEstimate(PoseAndIntrinsics(camera)); // 设置顶点的估计初始值
optimizer.addVertex(v); // 在优化器中添加这个顶点
vertex_pose_intrinsics.push_back(v);
}
// 对于22106个路标点，添加22106个顶点
for (int i = 0; i < bal_problem.num_points(); ++i) {
VertexPoint *v = new VertexPoint();
double *point = points + point_block_size * i;
v->setId(i + bal_problem.num_cameras());
v->setEstimate(Vector3d(point[0], point[1], point[2]));
// g2o在BA中需要手动设置待Marg的顶点
v->setMarginalized(true); // 手动设置稀疏求解
optimizer.addVertex(v);
vertex_points.push_back(v);
}
// edge 每一个观测值都是一条边
for (int i = 0; i < bal_problem.num_observations(); ++i) {
EdgeProjection *edge = new EdgeProjection;
// bal_problem.camera_index()[i]得到的是当前观测数据对应的相机位姿的序号，
// 作为数组索引恰好得到vector容器中存放的相机位姿顶点的指针
edge->setVertex(0, vertex_pose_intrinsics[bal_problem.camera_index()[i]]); // 设置这条边的第一个顶点
// bal_problem.point_index()[i]得到的是当前观测数据对应的路标点的序号，
// 作为数组索引恰好得到vector容器中存放的路标点顶点的指针
edge->setVertex(1, vertex_points[bal_problem.point_index()[i]]); // 设置这条边的第二个顶点
edge->setMeasurement(Vector2d(observations[2 * i + 0], observations[2 * i + 1])); // 设置观测值
edge->setInformation(Matrix2d::Identity()); // 设置信息矩阵，这里设置成单位阵，表示各个观测数据的误差权重都相等
edge->setRobustKernel(new g2o::RobustKernelHuber()); // 鲁邦核函数，防止噪声数据的过大影响
optimizer.addEdge(edge); //添加边
}
optimizer.initializeOptimization();
optimizer.optimize(40);
// set to bal problem
// 程序执行到这里的时候，已经优化完毕了，为了后面
// BLAproblem这个类把优化的结果存盘使用，所以还需要把这个结果
for (int i = 0; i < bal_problem.num_cameras(); ++i) {
double *camera = cameras + camera_block_size * i; // 获得要存储的这组数据的指针
auto vertex = vertex_pose_intrinsics[i]; // 获取顶点
auto estimate = vertex->estimate(); // 顶点的估计值是顶点数据类型（自定义的对象）
estimate.set_to(camera); // 调用自定义对象中存到内存中的函数，把优化后的数据，存到读取数据的内存中，相当于更新了内存值
}
for (int i = 0; i < bal_problem.num_points(); ++i) {
double *point = points + point_block_size * i;
auto vertex = vertex_points[i]; // 这里位姿顶点的数据类型是Vector3d，所以这里返回的估计值也是Vector3d
for (int k = 0; k < 3; ++k) point[k] = vertex->estimate()[k];
}
}
使用的是ubuntu20.04，g2o的版本是最新版（虽然跟高翔的版本不一样，但是ch6、7、8都正确的过来了）
恳求各位指点迷津。
【以下回答由 GPT 生成】
问题:
回答:
根据提供的代码，我觉得可能有几个地方出了问题：
检查你使用的g2o版本是否与SLAM十四讲中的版本不兼容，或者是否是一个已知的有问题的版本。你可以尝试使用更早或更晚的g2o版本来解决这个问题。
检查你的代码是否有其他地方出现了问题，比如初始化相关的代码或者其他使用到g2o的地方。你可以逐步注释掉代码，然后重新运行来排除问题。
检查你的数据是否正确加载到g2o中，以及数据的格式是否符合g2o的要求。你可以在加载数据之前添加一些调试输出语句，以确保数据被正确加载。
如果以上解决方法都没有成功，那可能需要对g2o库进行调试或查阅更多的资料来解决问题。由于没有更多的上下文和具体的错误信息，我无法提供更加具体的解决方案。
【相关推荐】