使用g2o求解BA

用节点表示相机和路标，用边表示他们之间的观测，使用自定义的点和边
#include <g2o/core/base_vertex.h>
#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;

/// 姿态和内参的结构
struct PoseAndIntrinsics {
PoseAndIntrinsics() {}
//
/// 从给定的data地址来获取相机R,t,和内参
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以及四个基本函数
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) {
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) {}
//
virtual bool write(ostream &out) const {}
};
//定义路标点P顶点
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) {}
//
virtual bool write(ostream &out) const {}
};
定义边（观测值z的维度u,v是2），边连接的两个顶点的类型
class EdgeProjection :
public g2o::BaseBinaryEdge<2, Vector2d, VertexPoseAndIntrinsics, VertexPoint> {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
//
virtual void computeError() override {
auto v0 = (VertexPoseAndIntrinsics *) _vertices[0];
auto v1 = (VertexPoint *) _vertices[1];
auto proj = v0->project(v1->estimate());
_error = proj - _measurement;
}
//
// use numeric derivatives
virtual bool read(istream &in) {}
//
virtual bool write(ostream &out) const {}

};

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(); //定义points和cameras
double *cameras = bal_problem.mutable_cameras();
//
// pose dimension 9, landmark is 3
typedef g2o::BlockSolver<g2o::BlockSolverTraits<9, 3>> BlockSolverType;
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(); //定义Observations
// vertex
vector<VertexPoseAndIntrinsics *> vertex_pose_intrinsics; 存储相机位姿节点
vector<VertexPoint *> vertex_points; 存储路标节点
for (int i = 0; i < bal_problem.num_cameras(); ++i) {
VertexPoseAndIntrinsics *v = new VertexPoseAndIntrinsics();
double *camera = cameras + camera_block_size * i;
v->setId(i); //对每一个位姿camera设定一个ID
v->setEstimate(PoseAndIntrinsics(camera));
optimizer.addVertex(v); 在结构中加入相机位姿
vertex_pose_intrinsics.push_back(v); 相机位姿进行存储
}
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()); 对每一个位姿camera设定一个ID，为了避免ID重复，把节点ID往后推迟camera数量个
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;
edge->setVertex(0, vertex_pose_intrinsics[bal_problem.camera_index()[i]]); 设置每条边连接的节点
edge->setVertex(1, vertex_points[bal_problem.point_index()[i]]);
edge->setMeasurement(Vector2d(observations[2 * i + 0], observations[2 * i + 1]));将已有的observations投入measurement与计算得到的Projection相减做误差函数
edge->setInformation(Matrix2d::Identity());
edge->setRobustKernel(new g2o::RobustKernelHuber());
optimizer.addEdge(edge);
}
//
optimizer.initializeOptimization();
optimizer.optimize(40);
//
// set to bal problem，由于最终problem生成.ply文件所以要使用set_to函数
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];
for (int k = 0; k < 3; ++k) point[k] = vertex->estimate()[k];
}
}