BA的概念
批量状态估计问题可以转化为最大似然估计问题，并使用最小二乘法进行求解。
视觉SLAM的主流为非线性优化方法。
所谓Bundle Adjustment（BA），是指从视觉图像中提炼出最优的3D模型和相机参数（内参和外参数）。
考虑从任意特征点发射出来的几束光线（bundles of light rays），它们会在几个相机的成像平面上变成像素或是检测到的特征点。如果我们调整（adjustment）各相机姿态和各特征点的空间位置，使得这些光线最终收束到相机的光心，就称为BA。
求解BA
Ceres求解BA
定义重投影误差模型
ch9\SnavelyReprojectionError.h
class SnavelyReprojectionError {
public:
SnavelyReprojectionError(double observation_x, double observation_y) : observed_x(observation_x),
observed_y(observation_y) {}
template<typename T>
bool operator()(const T *const camera,
const T *const point,
T *residuals) const {
// camera[0,1,2] are the angle-axis rotation
T predictions[2];
CamProjectionWithDistortion(camera, point, predictions);
residuals[0] = predictions[0] - T(observed_x);
residuals[1] = predictions[1] - T(observed_y);
return true;
}
// camera : 9 dims array
// [0-2] : angle-axis rotation
// [3-5] : translateion
// [6-8] : camera parameter, [6] focal length, [7-8] second and forth order radial distortion
// point : 3D location.
// predictions : 2D predictions with center of the image plane.
template<typename T>
static inline bool CamProjectionWithDistortion(const T *camera, const T *point, T *predictions) {
// Rodrigues' formula
T p[3];
AngleAxisRotatePoint(camera, point, p);
// camera[3,4,5] are the translation
p[0] += camera[3];
p[1] += camera[4];
p[2] += camera[5];
// Compute the center fo distortion
T xp = -p[0] / p[2];
T yp = -p[1] / p[2];
// Apply second and fourth order radial distortion
const T &l1 = camera[7];
const T &l2 = camera[8];
T r2 = xp * xp + yp * yp;
T distortion = T(1.0) + r2 * (l1 + l2 * r2);
const T &focal = camera[6];
predictions[0] = focal * distortion * xp;
predictions[1] = focal * distortion * yp;
return true;
}
static ceres::CostFunction *Create(const double observed_x, const double observed_y) {
return (new ceres::AutoDiffCostFunction<SnavelyReprojectionError, 2, 9, 3>(
new SnavelyReprojectionError(observed_x, observed_y)));
}
private:
double observed_x;
double observed_y;
};
BA搭建和求解
ch9\bundle_adjustment_ceres.cpp
SolveBA函数
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();
// Observations is 2 * num_observations long array observations
// [u_1, u_2, ... u_n], where each u_i is two dimensional, the x
// and y position of the observation.
const double *observations = bal_problem.observations();
ceres::Problem problem;
for (int i = 0; i < bal_problem.num_observations(); ++i) {
ceres::CostFunction *cost_function;
// Each Residual block takes a point and a camera as input
// and outputs a 2 dimensional Residual
cost_function = SnavelyReprojectionError::Create(observations[2 * i + 0], observations[2 * i + 1]);
// If enabled use Huber's loss function.
ceres::LossFunction *loss_function = new ceres::HuberLoss(1.0);
// Each observation corresponds to a pair of a camera and a point
// which are identified by camera_index()[i] and point_index()[i]
// respectively.
double *camera = cameras + camera_block_size * bal_problem.camera_index()[i];
double *point = points + point_block_size * bal_problem.point_index()[i];
problem.AddResidualBlock(cost_function, loss_function, camera, point);
}
// show some information here ...
std::cout << "bal problem file loaded..." << std::endl;
std::cout << "bal problem have " << bal_problem.num_cameras() << " cameras and "
<< bal_problem.num_points() << " points. " << std::endl;
std::cout << "Forming " << bal_problem.num_observations() << " observations. " << std::endl;
std::cout << "Solving ceres BA ... " << endl;
ceres::Solver::Options options;
options.linear_solver_type = ceres::LinearSolverType::SPARSE_SCHUR;
options.minimizer_progress_to_stdout = true;
ceres::Solver::Summary summary;
ceres::Solve(options, &problem, &summary);
std::cout << summary.FullReport() << "\n";
}
调用方法
int main(int argc, char **argv) {
if (argc != 2) {
cout << "usage: bundle_adjustment_ceres 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;
}
g2o求解BA
g2o使用图模型来描述问题的结构，所以我们用节点来表示相机和路标，用边来表示它们之间的观测。
相机和路标结构体定义
ch9\bundle_adjustment_ceres.cpp
自定义的节点和边，override表示对基类虚函数的覆盖。
/// 姿态和内参的结构
struct PoseAndIntrinsics {
PoseAndIntrinsics() {}
/// set from given data address
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 {}
};
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 {}
};
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 {}
};
BA搭建和求解
SloveBA函数
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::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;
for (int i = 0; i < bal_problem.num_cameras(); ++i) {
VertexPoseAndIntrinsics *v = new VertexPoseAndIntrinsics();
double *camera = cameras + camera_block_size * i;
v->setId(i);
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());
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]));
edge->setInformation(Matrix2d::Identity());
edge->setRobustKernel(new g2o::RobustKernelHuber());
optimizer.addEdge(edge);
}
optimizer.initializeOptimization();
optimizer.optimize(40);
// set to bal problem
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];
}
}
滑动窗口优化
待补充。
位姿图优化
g2o原生位姿图
顶点和边使用g2o默认的。求解器使用的LM方法。
ch10\pose_graph_g2o_SE3.cpp
/************************************************
* 本程序演示如何用g2o solver进行位姿图优化
* sphere.g2o是人工生成的一个Pose graph，我们来优化它。
* 尽管可以直接通过load函数读取整个图，但我们还是自己来实现读取代码，以期获得更深刻的理解
* 这里使用g2o/types/slam3d/中的SE3表示位姿，它实质上是四元数而非李代数.
* **********************************************/
int main(int argc, char **argv) {
if (argc != 2) {
cout << "Usage: pose_graph_g2o_SE3 sphere.g2o" << endl;
return 1;
}
ifstream fin(argv[1]);
if (!fin) {
cout << "file " << argv[1] << " does not exist." << endl;
return 1;
}
// 设定g2o
typedef g2o::BlockSolver<g2o::BlockSolverTraits<6, 6>> BlockSolverType;
typedef g2o::LinearSolverEigen<BlockSolverType::PoseMatrixType> LinearSolverType;
auto solver = new g2o::OptimizationAlgorithmLevenberg(
g2o::make_unique<BlockSolverType>(g2o::make_unique<LinearSolverType>()));
g2o::SparseOptimizer optimizer; // 图模型
optimizer.setAlgorithm(solver); // 设置求解器
optimizer.setVerbose(true); // 打开调试输出
int vertexCnt = 0, edgeCnt = 0; // 顶点和边的数量
while (!fin.eof()) {
string name;
fin >> name;
if (name == "VERTEX_SE3:QUAT") {
// SE3 顶点
g2o::VertexSE3 *v = new g2o::VertexSE3();
int index = 0;
fin >> index;
v->setId(index);
v->read(fin);
optimizer.addVertex(v);
vertexCnt++;
if (index == 0)
v->setFixed(true);
} else if (name == "EDGE_SE3:QUAT") {
// SE3-SE3 边
g2o::EdgeSE3 *e = new g2o::EdgeSE3();
int idx1, idx2; // 关联的两个顶点
fin >> idx1 >> idx2;
e->setId(edgeCnt++);
e->setVertex(0, optimizer.vertices()[idx1]);
e->setVertex(1, optimizer.vertices()[idx2]);
e->read(fin);
optimizer.addEdge(e);
}
if (!fin.good()) break;
}
cout << "read total " << vertexCnt << " vertices, " << edgeCnt << " edges." << endl;
cout << "optimizing ..." << endl;
optimizer.initializeOptimization();
optimizer.optimize(30);
cout << "saving optimization results ..." << endl;
optimizer.save("result.g2o");
return 0;
}
李代数上的位姿图优化
雅可比的计算过程，方法可以二选一：
方法1：不提供雅可比计算函数，让g2o自定计算数值雅可比。
方法2：提供完整或近似的雅可比计算过程。
ch10\pose_graph_g2o_lie_algebra.cpp
这里JRInv()函数提供近似的雅可比。
/************************************************
* 本程序演示如何用g2o solver进行位姿图优化
* sphere.g2o是人工生成的一个Pose graph，我们来优化它。
* 尽管可以直接通过load函数读取整个图，但我们还是自己来实现读取代码，以期获得更深刻的理解
* 本节使用李代数表达位姿图，节点和边的方式为自定义
* **********************************************/
typedef Matrix<double, 6, 6> Matrix6d;
// 给定误差求J_R^{-1}的近似
Matrix6d JRInv(const SE3d &e) {
Matrix6d J;
J.block(0, 0, 3, 3) = SO3d::hat(e.so3().log());
J.block(0, 3, 3, 3) = SO3d::hat(e.translation());
J.block(3, 0, 3, 3) = Matrix3d::Zero(3, 3);
J.block(3, 3, 3, 3) = SO3d::hat(e.so3().log());
// J = J * 0.5 + Matrix6d::Identity();
J = Matrix6d::Identity(); // try Identity if you want
return J;
}
// 李代数顶点
typedef Matrix<double, 6, 1> Vector6d;
class VertexSE3LieAlgebra : public g2o::BaseVertex<6, SE3d> {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
virtual bool read(istream &is) override {
double data[7];
for (int i = 0; i < 7; i++)
is >> data[i];
setEstimate(SE3d(
Quaterniond(data[6], data[3], data[4], data[5]),
Vector3d(data[0], data[1], data[2])
));
}
virtual bool write(ostream &os) const override {
os << id() << " ";
Quaterniond q = _estimate.unit_quaternion();
os << _estimate.translation().transpose() << " ";
os << q.coeffs()[0] << " " << q.coeffs()[1] << " " << q.coeffs()[2] << " " << q.coeffs()[3] << endl;
return true;
}
virtual void setToOriginImpl() override {
_estimate = SE3d();
}
// 左乘更新
virtual void oplusImpl(const double *update) override {
Vector6d upd;
upd << update[0], update[1], update[2], update[3], update[4], update[5];
_estimate = SE3d::exp(upd) * _estimate;
}
};
// 两个李代数节点之边
class EdgeSE3LieAlgebra : public g2o::BaseBinaryEdge<6, SE3d, VertexSE3LieAlgebra, VertexSE3LieAlgebra> {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
virtual bool read(istream &is) override {
double data[7];
for (int i = 0; i < 7; i++)
is >> data[i];
Quaterniond q(data[6], data[3], data[4], data[5]);
q.normalize();
setMeasurement(SE3d(q, Vector3d(data[0], data[1], data[2])));
for (int i = 0; i < information().rows() && is.good(); i++)
for (int j = i; j < information().cols() && is.good(); j++) {
is >> information()(i, j);
if (i != j)
information()(j, i) = information()(i, j);
}
return true;
}
virtual bool write(ostream &os) const override {
VertexSE3LieAlgebra *v1 = static_cast<VertexSE3LieAlgebra *> (_vertices[0]);
VertexSE3LieAlgebra *v2 = static_cast<VertexSE3LieAlgebra *> (_vertices[1]);
os << v1->id() << " " << v2->id() << " ";
SE3d m = _measurement;
Eigen::Quaterniond q = m.unit_quaternion();
os << m.translation().transpose() << " ";
os << q.coeffs()[0] << " " << q.coeffs()[1] << " " << q.coeffs()[2] << " " << q.coeffs()[3] << " ";
// information matrix
for (int i = 0; i < information().rows(); i++)
for (int j = i; j < information().cols(); j++) {
os << information()(i, j) << " ";
}
os << endl;
return true;
}
// 误差计算与书中推导一致
virtual void computeError() override {
SE3d v1 = (static_cast<VertexSE3LieAlgebra *> (_vertices[0]))->estimate();
SE3d v2 = (static_cast<VertexSE3LieAlgebra *> (_vertices[1]))->estimate();
_error = (_measurement.inverse() * v1.inverse() * v2).log();
}
// 雅可比计算
virtual void linearizeOplus() override {
SE3d v1 = (static_cast<VertexSE3LieAlgebra *> (_vertices[0]))->estimate();
SE3d v2 = (static_cast<VertexSE3LieAlgebra *> (_vertices[1]))->estimate();
Matrix6d J = JRInv(SE3d::exp(_error));
// 尝试把J近似为I？
_jacobianOplusXi = -J * v2.inverse().Adj();
_jacobianOplusXj = J * v2.inverse().Adj();
}
};
int main(int argc, char **argv) {
if (argc != 2) {
cout << "Usage: pose_graph_g2o_SE3_lie sphere.g2o" << endl;
return 1;
}
ifstream fin(argv[1]);
if (!fin) {
cout << "file " << argv[1] << " does not exist." << endl;
return 1;
}
// 设定g2o
typedef g2o::BlockSolver<g2o::BlockSolverTraits<6, 6>> BlockSolverType;
typedef g2o::LinearSolverEigen<BlockSolverType::PoseMatrixType> LinearSolverType;
auto solver = new g2o::OptimizationAlgorithmLevenberg(
g2o::make_unique<BlockSolverType>(g2o::make_unique<LinearSolverType>()));
g2o::SparseOptimizer optimizer; // 图模型
optimizer.setAlgorithm(solver); // 设置求解器
optimizer.setVerbose(true); // 打开调试输出
int vertexCnt = 0, edgeCnt = 0; // 顶点和边的数量
vector<VertexSE3LieAlgebra *> vectices;
vector<EdgeSE3LieAlgebra *> edges;
while (!fin.eof()) {
string name;
fin >> name;
if (name == "VERTEX_SE3:QUAT") {
// 顶点
VertexSE3LieAlgebra *v = new VertexSE3LieAlgebra();
int index = 0;
fin >> index;
v->setId(index);
v->read(fin);
optimizer.addVertex(v);
vertexCnt++;
vectices.push_back(v);
if (index == 0)
v->setFixed(true);
} else if (name == "EDGE_SE3:QUAT") {
// SE3-SE3 边
EdgeSE3LieAlgebra *e = new EdgeSE3LieAlgebra();
int idx1, idx2; // 关联的两个顶点
fin >> idx1 >> idx2;
e->setId(edgeCnt++);
e->setVertex(0, optimizer.vertices()[idx1]);
e->setVertex(1, optimizer.vertices()[idx2]);
e->read(fin);
optimizer.addEdge(e);
edges.push_back(e);
}
if (!fin.good()) break;
}
cout << "read total " << vertexCnt << " vertices, " << edgeCnt << " edges." << endl;
cout << "optimizing ..." << endl;
optimizer.initializeOptimization();
optimizer.optimize(30);
cout << "saving optimization results ..." << endl;
// 因为用了自定义顶点且没有向g2o注册，这里保存自己来实现
// 伪装成 SE3 顶点和边，让 g2o_viewer 可以认出
ofstream fout("result_lie.g2o");
for (VertexSE3LieAlgebra *v:vectices) {
fout << "VERTEX_SE3:QUAT ";
v->write(fout);
}
for (EdgeSE3LieAlgebra *e:edges) {
fout << "EDGE_SE3:QUAT ";
e->write(fout);
}
fout.close();
return 0;
}
参考资料：
1、书籍：《视觉SLAM十四讲：从理论到实践（第2版）》
2、代码：https://github.com/gaoxiang12/slambook2
扫一扫
1462
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
