Adjustment computation最早是由geodesy的人搞出来的。19世纪中期的时候，geodetics的学者就开始研究large scale triangulations（大型三角剖分）了。20世纪中期，随着camera和computer的出现，photogrammetry(照相测量法)也开始研究adjustment computation，所以他们给起了个名字叫bundle adjustment。21世纪前后，robotics领域开始兴起SLAM，最早用的recursive bayesian filter（递归贝叶斯滤波），后来把问题搞成个graph然后用least squares方法解。
Bundle Adjustment中文译作光束平差法、捆集调整等，是指从视觉重建中提炼出最优的3D模型和相机参数（内参和外参）。从每个特征点反射出来的几束光线（bundles of light rays），在我们把相机姿态和特征点的位置做出最优的调整（adjustment）之后，最后收束到光心的这个过程，简称BA.
算法原理
现假设空间位置的3D点为,
\[
X = \{x_1, x_2, \cdots , x_{nx} \}
\]
相机中心位姿为,
\[
P = \{ p_1, p_2, \cdots , p_{np} \}
\]
\(u_i\)为\(X_i\)对应的像素位置，\(K\)为相机内参矩阵，\(s_i\)为\(u_i\)对应的深度值。
对于bundle adjustment可以构建重投影误差最小二乘如下：
\[
\{X, T\} = min \frac{1}{2} \sum_{i = 0}^{n} || u_i - \frac{1}{s_i} KTX_i ||^2
\]
对于变换矩阵T，满足如下约束：
\[
T = \begin{bmatrix} R & t \\ 0^T & 1 \end{bmatrix}, R^TR = I, det(R) = 1, t \in R^3
\]
对于有约束的变换矩阵在最小二乘中不好求解，转换为无约束的李群求解：
\[
\{X, \xi\} = min \frac{1}{2} \sum_{i = 0}^{n} || u_i - \frac{1}{s_i} Kexp(\xi^{\land})X_i ||^2
\]
定义误差函数为：
\[
f(X,\xi) = u_i - \frac{1}{s} Kexp(\xi^{\land})X
\]
对于高斯牛顿：
\[
J(x)^TJ(x) \Delta x = -J(x)^Tf(x)
\]
需要求解误差函数\(f(X,\xi)\)对\(X\)以及\(\xi\)的偏导数。
重投影误差函数对相机位姿求导
\[
\frac{\partial u}{\partial \xi} = \frac{\partial u}{\partial X} \frac{\partial X}{\partial \xi}= \begin{bmatrix} \frac{xy}{z^2}f_x & -(1 + \frac{x^2}{z^2})f_x & \frac{y}{z}f_x & -\frac{1}{z}f_x & 0 & \frac{x}{z^2}f_x \\ (1 + \frac{y^2}{z^2})f_y & -\frac{xy}{z^2} f_y & -\frac{x}{z}f_y & 0 & -\frac{1}{z}f_y & \frac{y}{z^2}f_y\end{bmatrix}_{2 \times 6}
\]
重投影误差函数对3D点坐标求导
\[
\frac{\partial u}{\partial X_w} = \frac{\partial u}{\partial X_c} \frac{\partial X_c}{\partial X_w} = \begin{bmatrix} -\frac{f_x}{z} & 0 & \frac{xf_x}{z^2} \\ 0 & -\frac{f_y}{z} & \frac{yf_y}{z^2}\end{bmatrix}_{2 \times 3} \cdot R
\]
Code
先通过g2o实现bundle adjustment，主要定义两个vertex和一个edge。误差函数的定义和误差函数对位姿以及3D点求导jacobian使用如上推导。
/**
******************************************************************************
* @file
* @author maky <chengwei920412@outlook.com>
* @version
* @date 2018-12-12 20:22:58
* @brief
******************************************************************************
* @attention
*
*
******************************************************************************
*/
#include <iostream>
#include <random>
#include <vector>
#include <memory>
#include <algorithm>
#include <Eigen/Eigen>
#include <sophus/se3.hpp>
#include <g2o/core/sparse_optimizer.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/solver.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/core/robust_kernel.h>
#include <g2o/core/robust_kernel_impl.h>
#include <g2o/solvers/cholmod/linear_solver_cholmod.h>
#include <g2o/core/base_vertex.h>
#include <g2o/core/base_binary_edge.h>
#include <g2o/core/base_unary_edge.h>
#include <opencv2/opencv.hpp>
class BundleAdjustment {
public:
class VertexPose : public g2o::BaseVertex<6, Sophus::SE3d> {
public:EIGEN_MAKE_ALIGNED_OPERATOR_NEW
VertexPose()
: BaseVertex<6, Sophus::SE3d>()
{}
virtual bool read(std::istream& is) { return true; }
virtual bool write(std::ostream& os) const { return true; }
virtual void setToOriginImpl() {
setEstimate(Sophus::SE3d());
}
virtual void oplusImpl(const number_t* update_) {
Eigen::Map<const g2o::Vector6> update(update_);
setEstimate(Sophus::SE3d::exp(update)*estimate());
}
};
class VertexPoint : public g2o::BaseVertex<3, g2o::Vector3>
{
public:EIGEN_MAKE_ALIGNED_OPERATOR_NEW
VertexPoint()
: g2o::BaseVertex<3, g2o::Vector3>()
{}
virtual bool read(std::istream& is) { return true; }
virtual bool write(std::ostream& os) const { return true; }
virtual void setToOriginImpl() {
setEstimate(Eigen::Vector3d(0., 0., 0.));
}
virtual void oplusImpl(const number_t* update)
{
Eigen::Map<const g2o::Vector3> v(update);
_estimate += v;
}
};
class CameraModel {
public:
CameraModel(double fx, double fy, double cx, double cy)
: fx_(fx), fy_(fy), cx_(cx), cy_(cy)
{}
virtual ~CameraModel() {}
public:
g2o::Vector2 reproject(const g2o::Vector3& v) {
g2o::Vector2 result;
result(0) = v(0) / v(2);
result(1) = v(1) / v(2);
result[0] = result[0] * fx_ + cx_;
result[1] = result[1] * fy_ + cy_;
return result;
}
double fx() { return fx_; }
double fy() { return fy_; }
double cx() { return cx_; }
double cy() { return cy_; }
protected:
double fx_; double fy_; double cx_; double cy_;
};
class EdgeProject : public g2o::BaseBinaryEdge<2, g2o::Vector2, VertexPoint, VertexPose> {
public:EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
EdgeProject(std::shared_ptr<CameraModel> camera)
: g2o::BaseBinaryEdge<2, g2o::Vector2, VertexPoint, VertexPose>()
, camera_(camera)
{}
virtual bool read(std::istream& is) { return true; }
virtual bool write(std::ostream& os) const { return true; }
void computeError() {
const VertexPose* v1 = static_cast<const VertexPose*>(_vertices[1]);
const VertexPoint* v2 = static_cast<const VertexPoint*>(_vertices[0]);
g2o::Vector2 obs(_measurement);
// f(x) = u_i - \frac{1}{s} Kexp(\xi^{\land})X
_error = obs - camera_->reproject(v1->estimate().rotationMatrix() * v2->estimate() + v1->estimate().translation());
}
virtual void linearizeOplus()
{
VertexPose * pose_vertex = static_cast<VertexPose *>(_vertices[1]);
Sophus::SE3d transform(pose_vertex->estimate());
VertexPoint* point_vertex = static_cast<VertexPoint*>(_vertices[0]);
g2o::Vector3 point = transform.rotationMatrix() * point_vertex->estimate() + transform.translation();
number_t &x = point[0];
number_t &y = point[1];
number_t &z = point[2];
number_t z_2 = z*z;
// \frac{\partial u}{\partial X_w} = \frac{\partial u}{\partial X_c} \frac{\partial X_c}{\partial X_w} = \begin{bmatrix} -\frac{f_x}{z} & 0 & \frac{xf_x}{z^2} \\ 0 & -\frac{f_y}{z} & \frac{yf_y}{z^2}\end{bmatrix}_{2 \times 3} \cdot R
Eigen::Matrix<number_t, 2, 3> jp;
jp(0, 0) = -camera_->fx() / z;
jp(0, 1) = 0;
jp(0, 2) = x * camera_->fx() / z_2;
jp(1, 0) = 0;
jp(1, 1) = -camera_->fy() / z;
jp(1, 2) = y * camera_->fy() / z_2;
_jacobianOplusXi = jp * transform.rotationMatrix();
// \frac{\partial u}{\partial \xi} = \frac{\partial u}{\partial X} \frac{\partial X}{\partial \xi}= \begin{bmatrix} \frac{xy}{z^2}f_x & -(1 + \frac{x^2}{z^2})f_x & \frac{y}{z}f_x & -\frac{1}{z}f_x & 0 & \frac{x}{z^2}f_x \\ (1 + \frac{y^2}{z^2})f_y & -\frac{xy}{z^2} f_y & -\frac{x}{z}f_y & 0 & -\frac{1}{z}f_y & \frac{y}{z^2}f_y\end{bmatrix}_{2 \times 6}
_jacobianOplusXj(0, 0) = x*y / z_2 *camera_->fx();
_jacobianOplusXj(0, 1) = -(1 + (x*x / z_2)) *camera_->fx();
_jacobianOplusXj(0, 2) = y / z *camera_->fx();
_jacobianOplusXj(0, 3) = -1. / z *camera_->fx();
_jacobianOplusXj(0, 4) = 0;
_jacobianOplusXj(0, 5) = x / z_2 *camera_->fx();
_jacobianOplusXj(1, 0) = (1 + y*y / z_2) *camera_->fy();
_jacobianOplusXj(1, 1) = -x*y / z_2 *camera_->fy();
_jacobianOplusXj(1, 2) = -x / z *camera_->fy();
_jacobianOplusXj(1, 3) = 0;
_jacobianOplusXj(1, 4) = -1. / z *camera_->fy();
_jacobianOplusXj(1, 5) = y / z_2 *camera_->fy();
}
protected:
std::shared_ptr<CameraModel> camera_;
};
public:
BundleAdjustment(std::shared_ptr<CameraModel> camera)
: counter_(1)
, camera_(camera)
{
std::unique_ptr<g2o::BlockSolver_6_3::LinearSolverType> solver = g2o::make_unique<g2o::LinearSolverCholmod<g2o::BlockSolver_6_3::PoseMatrixType>>();
g2o::OptimizationAlgorithmLevenberg* algorithm = new g2o::OptimizationAlgorithmLevenberg(g2o::make_unique<g2o::BlockSolver_6_3>(std::move(solver)));
optimizer_.setAlgorithm(algorithm);
optimizer_.setVerbose(true);
}
virtual ~BundleAdjustment() {}
public:
int addVertex(const Eigen::Matrix4d &transform, bool fixed = false)
{
VertexPose *vertex = new VertexPose();
vertex->setId(counter_++);
vertex->setFixed(fixed);
vertex->setEstimate(Sophus::SE3d(transform.block<3, 3>(0, 0), transform.block<3, 1>(0, 3)));
optimizer_.addVertex(vertex);
return vertex->id();
}
bool getVertexEstimate(int id, Eigen::Matrix4d &transform)
{
if (!optimizer_.vertex(id)) {
return false;
}
transform = dynamic_cast<VertexPose*>(optimizer_.vertex(id))->estimate().matrix();
return true;
}
bool getVertexEstimate(int id, Eigen::Vector3d &point)
{
if (!optimizer_.vertex(id)) {
return false;
}
point = dynamic_cast<VertexPoint*>(optimizer_.vertex(id))->estimate();
return true;
}
int addVertex(const Eigen::Vector3d &point)
{
VertexPoint* vertex = new VertexPoint();
vertex->setId(counter_++);
vertex->setMarginalized(true);
vertex->setEstimate(point);
optimizer_.addVertex(vertex);
return vertex->id();
}
bool addEdge(const Eigen::Vector2d &observation, const Eigen::Vector2i &endpoint)
{
if (!optimizer_.vertex(endpoint.x()) || !optimizer_.vertex(endpoint.y())) {
std::cerr << "addEdge failed, nullptr of vertex." << std::endl;
return false;
}
EdgeProject *edge = new EdgeProject(camera_);
edge->setVertex(0, dynamic_cast<VertexPoint*>(optimizer_.vertex(endpoint.x())));
edge->setVertex(1, dynamic_cast<VertexPose*>(optimizer_.vertex(endpoint.y())));
edge->setMeasurement(observation);
edge->setInformation(Eigen::Matrix2d::Identity());
edge->setParameterId(0, 0);
edge->setRobustKernel(new g2o::RobustKernelHuber());
optimizer_.addEdge(edge);
return true;
}
bool optimize()
{
optimizer_.initializeOptimization();
int result = optimizer_.optimize(10);
return true;
}
protected:
std::shared_ptr<CameraModel> camera_;
g2o::SparseOptimizer optimizer_;
int counter_;
};
bool reproject(const Eigen::Matrix4d &transform, const Eigen::Matrix3d &intrinsic, const Eigen::Vector4d &object, Eigen::Vector3d &point)
{
point = (intrinsic * ((transform * object).block<3, 1>(0, 0))) / object.z();
return true;
}
bool reproject(const Eigen::Matrix4d &transform, const Eigen::Matrix3d &intrinsic, const std::vector<Eigen::Vector4d> &objects, std::vector<Eigen::Vector3d> &points)
{
points.clear();
for (auto &object : objects) {
Eigen::Vector3d point;
if (!reproject(transform, intrinsic, object, point)) {
continue;
}
points.push_back(point);
}
return true;
}
bool triangulate(const Eigen::Matrix4d &left_transform, const Eigen::Matrix4d &right_transform, const Eigen::Vector3d &left_point, const Eigen::Vector3d &right_point, Eigen::Vector4d &object)
{
Eigen::Matrix4d matrix = Eigen::Matrix4d::Zero();
matrix.row(0) = left_point[0] * left_transform.row(2) - left_transform.row(0);
matrix.row(1) = left_point[1] * left_transform.row(2) - left_transform.row(1);
matrix.row(2) = right_point[0] * right_transform.row(2) - right_transform.row(0);
matrix.row(3) = right_point[1] * right_transform.row(2) - right_transform.row(1);
Eigen::Vector4d point = matrix.jacobiSvd(Eigen::ComputeFullV).matrixV().rightCols<1>();
object(0) = point(0) / point(3);
object(1) = point(1) / point(3);
object(2) = point(2) / point(3);
object(3) = 1.;
return true;
}
bool triangulate(const Eigen::Matrix4d &left_transform, const Eigen::Matrix4d &right_transform, const std::vector<Eigen::Vector3d> &left_points, const std::vector<Eigen::Vector3d> &right_points, std::vector<Eigen::Vector4d> &objects)
{
objects.clear();
if (left_points.size() != right_points.size()) {
return false;
}
for (auto pos = 0; pos < left_points.size(); pos++) {
Eigen::Vector4d object;
if (!triangulate(left_transform, right_transform, left_points.at(pos), right_points.at(pos), object)) {
continue;
}
objects.push_back(object);
}
return true;
}
int main(int argc, char * argv[])
{
std::uint16_t object_size = 5;
std::vector<Eigen::Vector4d> object_points;
Eigen::Vector3d translation = Eigen::Vector3d(0., 0., 4.);
Eigen::Isometry3d transform = Eigen::Isometry3d::Identity();
transform.translation() = translation;
std::srand((unsigned int)std::time(nullptr));
std::cout << "object_points: " << std::endl;
for (auto pos = 0; pos < object_size; pos++) {
Eigen::Vector4d point(0., 0., 0., 1.);
point.x() = (float)(std::rand() % 20000 - 10000) / 10000.;
point.y() = (float)(std::rand() % 20000 - 10000) / 10000.;
point.z() = (float)(std::rand() % 20000 - 10000) / 10000.;
point = transform * point;
std::cout << point.transpose() << std::endl;
object_points.push_back(point);
}
Eigen::Matrix3d intrinsic;
intrinsic << 500., 0., 752 / 2., 0., 500., 480 / 2., 0., 0., 1.;
std::cout << "intrinsic: " << std::endl << intrinsic << std::endl;
// left camera
std::vector<Eigen::Vector3d> left_points;
Eigen::Matrix4d left_pose = Eigen::Matrix4d::Identity();
left_pose.block<3, 1>(0, 3) = Eigen::Vector3d(1., 0., 0.);
std::cout << "left_pose :" << std::endl << left_pose << std::endl;
reproject(left_pose, intrinsic, object_points, left_points);
std::cout << "left: " << std::endl;
for (auto &point : left_points) {
std::cout << point.transpose() << std::endl;
}
// right camera
std::vector<Eigen::Vector3d> right_points;
Eigen::Matrix4d right_pose = Eigen::Matrix4d::Identity();
right_pose.block<3, 1>(0, 3) = Eigen::Vector3d(-1., 0., 0.);
std::cout << "right_pose :" << std::endl << right_pose << std::endl;
reproject(right_pose, intrinsic, object_points, right_points);
std::cout << "right: " << std::endl;
for (auto &point : right_points) {
std::cout << point.transpose() << std::endl;
}
// triangulate
std::vector<Eigen::Vector4d> objects;
{
std::vector<Eigen::Vector3d> left, right;
for (auto &point : left_points) {
left.push_back(intrinsic.inverse() * point);
}
for (auto &point : right_points) {
right.push_back(intrinsic.inverse() * point);
}
triangulate(left_pose, right_pose, left, right, objects);
}
std::cout << "objects: " << std::endl;
for (auto &object : objects) {
std::cout << object.transpose() << std::endl;
}
// bundle adjustment
{
BundleAdjustment bundle_adjuster(std::make_shared<BundleAdjustment::CameraModel>(500., 500., 752 / 2., 480 / 2.));
int left_id = bundle_adjuster.addVertex(left_pose, true);
int right_id = bundle_adjuster.addVertex(right_pose, false);
std::vector<int> ids;
for (auto pos = 0; pos < objects.size(); pos++) {
auto &point = objects.at(pos);
int id = bundle_adjuster.addVertex(Eigen::Vector3d(point.block<3, 1>(0, 0)));
ids.push_back(id);
bundle_adjuster.addEdge(Eigen::Vector2d(left_points.at(pos).block<2, 1>(0, 0)), Eigen::Vector2i(id, left_id));
bundle_adjuster.addEdge(Eigen::Vector2d(right_points.at(pos).block<2, 1>(0, 0)), Eigen::Vector2i(id, right_id));
}
bundle_adjuster.optimize();
std::cout << "bundle adjustment: " << std::endl;
Eigen::Matrix4d left_transfrom, right_transform;
bundle_adjuster.getVertexEstimate(left_id, left_transfrom);
bundle_adjuster.getVertexEstimate(right_id, right_transform);
std::cout << left_transfrom << std::endl;
std::cout << right_transform << std::endl;
for (auto &id : ids) {
Eigen::Vector3d point;
bundle_adjuster.getVertexEstimate(id, point);
std::cout << point.transpose() << std::endl;
}
}
return 0;
}
ceres 实现如下：
class ProjecteCostFunction : public ceres::SizedCostFunction<2/* residuals */, 6/* pose */, 3/* point landmark */>
{
public:EIGEN_MAKE_ALIGNED_OPERATOR_NEW
ProjecteCostFunction(boost::shared_ptr<PinholeProjection> camera, const Eigen::Vector2d &observation)
: camera_(camera), observation_(observation)
{}
virtual ~ProjecteCostFunction() {}
protected:
virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const
{
if (!parameters || !residuals) {
return false;
}
// residuals
Sophus::SE3d transform = Sophus::SE3d::exp(Eigen::Map<const Sophus::Vector6d>(parameters[0]));
Eigen::Map<const Eigen::Vector3d> point(parameters[1]);
Eigen::Map<Eigen::Vector2d> residual(residuals);
Eigen::Vector3d local = transform * point;
Eigen::Vector2d estimate(local.x() / local.z(), local.y() / local.z());
camera_->euclideanToPixel(local, estimate);
residual = observation_ - estimate;
// jacobians
if (jacobians) {
const double &x = point.x();
const double &y = point.y();
const double &z = point.z();
const double z_2 = z*z;
double fx = camera_->intrinsics()(0, 0);
double fy = camera_->intrinsics()(1, 0);
double cx = camera_->intrinsics()(2, 0);
double cy = camera_->intrinsics()(3, 0);
if (jacobians && jacobians[0]/* pose */) {
Eigen::Matrix<double, 2, 6> jacobian;
jacobian(0, 0) = x*y / z_2 * fx;
jacobian(0, 1) = -(1 + (x*x / z_2)) * fx;
jacobian(0, 2) = y / z * fx;
jacobian(0, 3) = -1. / z * fx;
jacobian(0, 4) = 0;
jacobian(0, 5) = x / z_2 * fx;
jacobian(1, 0) = (1 + y*y / z_2) * fy;
jacobian(1, 1) = -x*y / z_2 * fy;
jacobian(1, 2) = -x / z * fy;
jacobian(1, 3) = 0;
jacobian(1, 4) = -1. / z * fy;
jacobian(1, 5) = y / z_2 * fy;
int pos = 0;
for (int row = 0; row < jacobian.rows(); ++row) {
for (int col = 0; col < jacobian.cols(); ++col) {
jacobians[0][pos++] = jacobian(row, col);
}
}
}
if (jacobians && jacobians[1]/* point landmark */) {
Eigen::Matrix<double, 2, 3> jacobian;
jacobian(0, 0) = -fx / z;
jacobian(0, 1) = 0;
jacobian(0, 2) = x * fx / z_2;
jacobian(1, 0) = 0;
jacobian(1, 1) = -fy / z;
jacobian(1, 2) = y * fy / z_2;
jacobian = jacobian * transform.rotationMatrix();
int pos = 0;
for (int row = 0; row < jacobian.rows(); ++row) {
for (int col = 0; col < jacobian.cols(); ++col) {
jacobians[1][pos++] = jacobian(row, col);
}
}
}
}
return true;
}
private:
boost::shared_ptr<PinholeProjection> camera_;
Eigen::Vector2d observation_;
};
Reference
《视觉slam十四讲从理论到实践》
https://en.wikipedia.org/wiki/Bundle_adjustment
SBA: A Software Package for Generic Sparse Bundle Adjustment
Triggs B, McLauchlan P F, Hartley R I, et al. Bundle adjustment—a modern synthesis[C]//International workshop on vision algorithms. Springer Berlin Heidelberg, 1999: 298-372.
Multiple View Geometry in Computer Vision, Richard Hartley, Andrew Zisserman
H. Johannsson, M. Kaess, M. Fallon, and J. J. Leonard, “Temporally scalable visual slam using a reduced pose graph,” in Proceedings of the IEEE International Conference on Robotics and Automation (ICRA), 2013, pp. 54–61.
G. Grisetti, R. Kummerle, C. Stachniss, and W. Burgard, “A tutorial on graph-based SLAM,” IEEE Transactions on Intelligent Transportation Systems Magazine, vol. 2, pp. 32–43, 2010.
https://www.ifi.uzh.ch/dam/jcr:5759a719-55db-4930-8051-4cc534f812b1/VO_Part_I_Scaramuzza.pdf
https://www.zora.uzh.ch/id/eprint/71030/1/Fraundorfer_Scaramuzza_Visual_odometry.pdf
https://bitbucket.org/gtborg/gtsam
https://github.com/strasdat/Sophus
https://blog.csdn.net/heyijia0327/article/details/60143160
https://blog.csdn.net/heyijia0327/article/details/51773578
https://blog.csdn.net/zhubaohua_bupt/article/details/74011005
https://blog.csdn.net/xiaocainiaodeboke/article/details/75041547
https://www.cnblogs.com/Jessica-jie/p/7739775.html