視覺十四講:第九講_BA優化_g2o

1.投影模型和BA代價函數

這個流程就是觀測方程
之前抽象的記為： \(z = h（x, y）\)
現在給出具體的參數話過程，x指此時相機的位姿R,t，它對應的李代數為\(\xi\)。路標y即為這里的三維點p，而觀測數據則是像素坐標(u,v)。
此次觀測的誤差為： \(e = z - h(\xi, p)\)
如果把其他時刻的觀測量也考慮進來，則整體的代價函數為：

相當於對位姿和3D路標點同時進行優化，也就是所謂的BA。

2.BA的求解

在BA的目標函數上，把自變量定義成所有待優化的變量：
\(x = [\xi_{1}, ..., \xi_{m}, p_{1}, ..., p_{n}]^{T}\)
相應的，增量方程中的\(\Delta x\)則是對整體自變量的增量，在這個意義下，當給一個增量時，目標函數為：

其中F表示整個代價函數在當前狀態下對相機姿態的偏導數，而E代表該函數對路標點位置的偏導。
無論是使用G-N還是L-M方法，最后都將面對增量線性方程： \(H\Delta x = g\)
主要區別是H取 \(J^{T}J\)還是取 \(J^{T}J + \lambda I\)的形式。
以G-N為例，H矩陣為：

3.H矩陣的稀疏性

H的稀疏性是有雅可比矩陣J引起的。考慮其中一個e，這個誤差項只描述了在\(\xi_{i}\)看到\(p_{j}\)這件事，只涉及到第i個相機位姿和第j個路標點，其余都是0
故：

假設一個場景有2個相機位姿（C1，C2）和6個路標點（P1,P2,P3,P4,P5,P6）觀測過程為：

J為：

由上面的結構，我們把H分為4個矩陣塊B,E,C：

於是，對應的線性方程組也可以變為如下形式：

4.G2O實踐

1.開始前，先講一下BAL數據集的數據格式

第一行表示有16個相機，22106個3D路標點 83718個觀測點

第一列是代表第幾個相機，第二列代表第幾個路標點，后面是觀測到的像素坐標。
該組數據一共是83718行。

后面的數據是16個相機的參數，9維，分別是-R（3維），t（3維），f（焦距），k1,k2畸變參數
一共16組數據。
再后面的數據，就是22106個路標點的3D坐標（3維）

2.bundle_adjustment_g2o.cpp

#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() {}

    /// 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) {
		//把一個世界的3D點變換到當前相機點
        Vector3d pc = _estimate.rotation * point + _estimate.translation;
        pc = -pc / pc[2];  //投影到前方的距離1的相機平面
        double r2 = pc.squaredNorm();  //r2
		//去畸變 1 + k1*r2 + k2*r2*r2  
        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 {}
};

//路標3D點的頂點
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 {}
};


//誤差模型  觀測維度2，類型為2d，   連接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數據集
    bal_problem.Normalize();  //對相機參數和路標點3D數據進行處理
    bal_problem.Perturb(0.1, 0.5, 0.5); //給路標3D點添加噪聲
    bal_problem.WriteToPLYFile("initial.ply"); //生成噪聲ply文件
    SolveBA(bal_problem);  //BA優化
    bal_problem.WriteToPLYFile("final.ply"); //生成優化后的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(); //3D點
    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) {  //16個相機位姿
        VertexPoseAndIntrinsics *v = new VertexPoseAndIntrinsics();   
        double *camera = cameras + camera_block_size * i;
        v->setId(i);   //頂點設置ID，
        v->setEstimate(PoseAndIntrinsics(camera));  //往圖里增加頂點位姿，相機的位姿數據9維
        optimizer.addVertex(v);
        vertex_pose_intrinsics.push_back(v);
    }
    for (int i = 0; i < bal_problem.num_points(); ++i) {  //22106個路標點
        VertexPoint *v = new VertexPoint();    
        double *point = points + point_block_size * i;
        v->setId(i + bal_problem.num_cameras());  //設置ID,不能和上面重復，直接往后排
        v->setEstimate(Vector3d(point[0], point[1], point[2]));    //路標點  3維
        // g2o在BA中需要手動設置待Marg的頂點
        v->setMarginalized(true);  //路標要被邊緣化計算，所以設置邊緣化屬性為true
        optimizer.addVertex(v);
        vertex_points.push_back(v);
    }

    // edge
    for (int i = 0; i < bal_problem.num_observations(); ++i) {   //增加邊，總共83718個觀測數據
        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);   //迭代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();  //獲取位姿的最優值9維  
        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];     //獲取3D路標的最優值3維
        for (int k = 0; k < 3; ++k) point[k] = vertex->estimate()[k];  //路標覆蓋保存
    }
}

3.common.cpp

#include <cstdio>
#include <fstream>
#include <iostream>
#include <string>
#include <vector>
#include <Eigen/Core>
#include <Eigen/Dense>

#include "common.h"
#include "rotation.h"
#include "random.h"

typedef Eigen::Map<Eigen::VectorXd> VectorRef;
typedef Eigen::Map<const Eigen::VectorXd> ConstVectorRef;

//這個函數從fptr文件中讀出一個format類型的值，賦值給參數value，從開頭開始，找到一個合適的就停止。
//這個函數主要是給BALProblem()構造函數讀取txt數據文件用的，比較簡陋
template<typename T>
void FscanfOrDie(FILE *fptr, const char *format, T *value) {
    int num_scanned = fscanf(fptr, format, value);
    if (num_scanned != 1)
        std::cerr << "Invalid UW data file. ";
}

//給一個三維向量加入噪聲，很簡單xyz依次加入隨機值就好了。定義這個的目的是為了后面的Perturb()函數在增加噪聲時，
// 是分開對路標點，相機的旋轉，相機的平移分別加入噪聲的，並且這三個量都是三維的，所以定義一個三維向量添加噪聲的函數
void PerturbPoint3(const double sigma, double *point) {
    for (int i = 0; i < 3; ++i)
        point[i] += RandNormal() * sigma;
}

double Median(std::vector<double> *data) {
    int n = data->size();
    std::vector<double>::iterator mid_point = data->begin() + n / 2;
    std::nth_element(data->begin(), mid_point, data->end());
    return *mid_point;
}

BALProblem::BALProblem(const std::string &filename, bool use_quaternions) {
    FILE *fptr = fopen(filename.c_str(), "r");

    if (fptr == NULL) {
        std::cerr << "Error: unable to open file " << filename;
        return;
    };

    // This wil die horribly on invalid files. Them's the breaks.
    FscanfOrDie(fptr, "%d", &num_cameras_);  //讀取總的相機數
    FscanfOrDie(fptr, "%d", &num_points_);   //讀取總的路標數
    FscanfOrDie(fptr, "%d", &num_observations_);//讀取總的觀測數據個數

    std::cout << "Header: " << num_cameras_
              << " " << num_points_
              << " " << num_observations_;

    point_index_ = new int[num_observations_];  //取出3D路標點的標號
    camera_index_ = new int[num_observations_]; //相機的標號
    observations_ = new double[2 * num_observations_]; //觀測的像素點

    num_parameters_ = 9 * num_cameras_ + 3 * num_points_;//每個相機9個參數，每個路標3個參數 
    parameters_ = new double[num_parameters_];  //參數的總大小

    for (int i = 0; i < num_observations_; ++i) {  //拷貝數據
        FscanfOrDie(fptr, "%d", camera_index_ + i);  //第幾個相機
        FscanfOrDie(fptr, "%d", point_index_ + i);   //第幾個路標
        for (int j = 0; j < 2; ++j) {
            FscanfOrDie(fptr, "%lf", observations_ + 2 * i + j);//觀測到的像素坐標
        }
    }

	//每個相機是一組9個參數，-R:3維(羅德里格斯向量)  t:3維  f,k1,k2。后面是3D路標的數據3維
    for (int i = 0; i < num_parameters_; ++i) {   
        FscanfOrDie(fptr, "%lf", parameters_ + i);
    }

    fclose(fptr);

    use_quaternions_ = use_quaternions;
    if (use_quaternions) {
        // Switch the angle-axis rotations to quaternions.
        num_parameters_ = 10 * num_cameras_ + 3 * num_points_;
        double *quaternion_parameters = new double[num_parameters_];//指針指向一個新的四元數數組
        double *original_cursor = parameters_;   //指針指向原始數據參數數組
        double *quaternion_cursor = quaternion_parameters;//指針指向指向四元數數組的指針
        for (int i = 0; i < num_cameras_; ++i) {
            AngleAxisToQuaternion(original_cursor, quaternion_cursor); //R轉換為四元數 
            quaternion_cursor += 4;  
            original_cursor += 3;  
            for (int j = 4; j < 10; ++j) {
                *quaternion_cursor++ = *original_cursor++; 
            }
        }
        // Copy the rest of the points.
        for (int i = 0; i < 3 * num_points_; ++i) {
            *quaternion_cursor++ = *original_cursor++;
        }
        // Swap in the quaternion parameters.
        delete[]parameters_;
        parameters_ = quaternion_parameters;
    }
}

//構造函數讀入數據txt，將數據存入類成員中。猜測這里是反向過程，由類成員中存儲的數據，生成一個待優化數據.txt。
void BALProblem::WriteToFile(const std::string &filename) const {
    FILE *fptr = fopen(filename.c_str(), "w");

    if (fptr == NULL) {
        std::cerr << "Error: unable to open file " << filename;
        return;
    }

    fprintf(fptr, "%d %d %d %d\n", num_cameras_, num_cameras_, num_points_, num_observations_);

    for (int i = 0; i < num_observations_; ++i) {
        fprintf(fptr, "%d %d", camera_index_[i], point_index_[i]);
        for (int j = 0; j < 2; ++j) {
            fprintf(fptr, " %g", observations_[2 * i + j]);
        }
        fprintf(fptr, "\n");
    }

    for (int i = 0; i < num_cameras(); ++i) {
        double angleaxis[9];
        if (use_quaternions_) {
            //OutPut in angle-axis format.
            QuaternionToAngleAxis(parameters_ + 10 * i, angleaxis);
            memcpy(angleaxis + 3, parameters_ + 10 * i + 4, 6 * sizeof(double));
        } else {
            memcpy(angleaxis, parameters_ + 9 * i, 9 * sizeof(double));
        }
        for (int j = 0; j < 9; ++j) {
            fprintf(fptr, "%.16g\n", angleaxis[j]);
        }
    }

    const double *points = parameters_ + camera_block_size() * num_cameras_;
    for (int i = 0; i < num_points(); ++i) {
        const double *point = points + i * point_block_size();
        for (int j = 0; j < point_block_size(); ++j) {
            fprintf(fptr, "%.16g\n", point[j]);
        }
    }

    fclose(fptr);
}

//將相機的世界坐標位移和3D路標點寫入文件
// Write the problem to a PLY file for inspection in Meshlab or CloudCompare
void BALProblem::WriteToPLYFile(const std::string &filename) const {
    std::ofstream of(filename.c_str());

    of << "ply"
       << '\n' << "format ascii 1.0"
       << '\n' << "element vertex " << num_cameras_ + num_points_
       << '\n' << "property float x"
       << '\n' << "property float y"
       << '\n' << "property float z"
       << '\n' << "property uchar red"
       << '\n' << "property uchar green"
       << '\n' << "property uchar blue"
       << '\n' << "end_header" << std::endl;

    // Export extrinsic data (i.e. camera centers) as green points.
    double angle_axis[3];
    double center[3];
    for (int i = 0; i < num_cameras(); ++i) {
        const double *camera = cameras() + camera_block_size() * i;
        CameraToAngelAxisAndCenter(camera, angle_axis, center);
        of << center[0] << ' ' << center[1] << ' ' << center[2]
           << "0 255 0" << '\n';
    }

    // Export the structure (i.e. 3D Points) as white points.
    const double *points = parameters_ + camera_block_size() * num_cameras_;
    for (int i = 0; i < num_points(); ++i) {
        const double *point = points + i * point_block_size();
        for (int j = 0; j < point_block_size(); ++j) {
            of << point[j] << ' ';
        }
        of << "255 255 255\n";
    }
    of.close();
}

/**
 * 
 * 由camera數據中的旋轉向量和平移向量解析出相機世界坐標系下的姿態(依舊是旋轉向量)和位置(世界坐標系下的xyz)，也是用於生成點雲用的
 * @param camera 要解析的相機參數，前三維旋轉，接着三維平移，這里指用到這6維
 * @param angle_axis 解析出的相機姿態承接數組，也是旋轉向量形式
 * @param center 解析出來的相機原點在世界坐標系下的坐標承接數組，XYZ
 */
void BALProblem::CameraToAngelAxisAndCenter(const double *camera,
                                            double *angle_axis,
                                            double *center) const {
    VectorRef angle_axis_ref(angle_axis, 3);
    if (use_quaternions_) {
        QuaternionToAngleAxis(camera, angle_axis);
    } else {
        angle_axis_ref = ConstVectorRef(camera, 3); //讀取R
    }

    Eigen::VectorXd inverse_rotation = -angle_axis_ref;  //-R，BAL文件定義，取負號
	
	
    /**
     * 這里解釋一下center的計算邏輯：
     * center是指相機原點在世界坐標系下的坐標，那么定義一下：
     * PW_center, 世界坐標系下相機原點的坐標
     * PC_center, 相機坐標系下的相機原點坐標
     * 它倆的關系是什么呢？
     * PW_center*R+t = PC_center
	 * 反向過程就是
	 * PC_center * T^(-1) = PW_center
	 * 那么相機坐標系的原點，在世界坐標系下的坐標就可以求出來了
	 * [R^(T)  -R^(T)*t ] * [相機原點也就是000]
	 * [0      1        ]   [ 1 ]
     * 結果就是   -R^(T) * t   
     *由旋轉向量形式表示的旋轉，反向過程(也就是求逆)就是旋轉向量取負即可。
	 * 所以結果就是cos(theta) * t + ( 1 - cos(theta) ) (n 點乘 t) n  + sin(theta) ( n 叉乘 t ) 
	 */
	
    AngleAxisRotatePoint(inverse_rotation.data(),  //R
                         camera + camera_block_size() - 6, //平移t的數據
                         center);   //結果
    
	//最后加上負號。記住，map類型構造的是引用，能直接對原構造數組進行操作的。
    //說一下這句，這句還是，用center數組的前3維，去構造一個無名的map類型矩陣，並且后面直接跟上*-1操作。
    //VectorRef是Map的一個define。
    //記住Map構造出來是引用，能對原始值直接操作。
	VectorRef(center, 3) *= -1.0;
}

/**
 * 由世界坐標系下的相機姿態和原點位置，生成一個camera數據
 * @param angle_axis 世界坐標到相機坐標變化的旋轉向量數據
 * @param center 相機中心在世界坐標系下的位置坐標
 * @param camera 承接數據的camera數組，由於這里只是生成旋轉和平移，所以是camera的前6維
 */
void BALProblem::AngleAxisAndCenterToCamera(const double *angle_axis,
                                            const double *center,
                                            double *camera) const {
    ConstVectorRef angle_axis_ref(angle_axis, 3);
    if (use_quaternions_) {
        AngleAxisToQuaternion(angle_axis, camera);
    } else {
        VectorRef(camera, 3) = angle_axis_ref;
    }

	//這里相機姿態R沒有取反，原始數據是-R，代表是相機坐標系對世界坐標系的變換
	
	/* 和上面類似
     * 結果就是   -R^(T) * t   
     *
	 * 所以結果就是cos(theta) * t + ( 1 - cos(theta) ) (n 點乘 t) n  + sin(theta) ( n 叉乘 t ) 
	 */
	
	//該函數直接修改了儲存相機平移數據的數據
    AngleAxisRotatePoint(angle_axis, center, camera + camera_block_size() - 6);
	
	//最后再取個反
    VectorRef(camera + camera_block_size() - 6, 3) *= -1.0;
}


void BALProblem::Normalize() {
    // Compute the marginal median of the geometry
    std::vector<double> tmp(num_points_);
    Eigen::Vector3d median;
    double *points = mutable_points();//獲取路標3D點的位置  
    for (int i = 0; i < 3; ++i) {
        for (int j = 0; j < num_points_; ++j) {
            tmp[j] = points[3 * j + i];
        }
        median(i) = Median(&tmp);  //返回中位數，如果是偶數，取平均值
    }
						
    for (int i = 0; i < num_points_; ++i) {
        VectorRef point(points + 3 * i, 3);
        tmp[i] = (point - median).lpNorm<1>(); //每個點 - 中位數 的LP范數
    }

    const double median_absolute_deviation = Median(&tmp); //再取中位數

    // Scale so that the median absolute deviation of the resulting
    // reconstruction is 100

    const double scale = 100.0 / median_absolute_deviation;

    // X = scale * (X - median)
    for (int i = 0; i < num_points_; ++i) {
        VectorRef point(points + 3 * i, 3);  //
        point = scale * (point - median);   //對每個3D點進行處理，MAP是引用，會改變原數據
    }

    double *cameras = mutable_cameras(); //相機參數
    double angle_axis[3];
    double center[3];
    for (int i = 0; i < num_cameras_; ++i) {
        double *camera = cameras + camera_block_size() * i;
		//angle_axis賦值了R，center為結果
        CameraToAngelAxisAndCenter(camera, angle_axis, center);  //求解世界坐標系下的相機中心坐標
        // center = scale * (center - median)
        VectorRef(center, 3) = scale * (VectorRef(center, 3) - median);  //因為世界路標3D點做了處理，所以這個也要處理
		
		//最終，修改了*camera指向儲存的數據的平移數據
        AngleAxisAndCenterToCamera(angle_axis, center, camera);  //因為世界坐標進行處理了，所以將處理后的數據轉到相機坐標去
    }
}

//添加噪聲
void BALProblem::Perturb(const double rotation_sigma,
                         const double translation_sigma,
                         const double point_sigma) {
    assert(point_sigma >= 0.0);
    assert(rotation_sigma >= 0.0);
    assert(translation_sigma >= 0.0);

    double *points = mutable_points();
    if (point_sigma > 0) {
        for (int i = 0; i < num_points_; ++i) {
            PerturbPoint3(point_sigma, points + 3 * i);
        }
    }

	//這里相機是被分成兩塊，旋轉和平移，
    //旋轉是考慮到四元數形式，增加了一步用CameraToAngelAxisAndCenter()從camera中取出三維的angle_axis,
    //然后添加噪聲，添加完后再用AngleAxisAndCenterToCamera()重構camera參數
    //平移部分就直接用PerturbPoint3()添加了
	
    for (int i = 0; i < num_cameras_; ++i) {
        double *camera = mutable_cameras() + camera_block_size() * i;

        double angle_axis[3];
        double center[3];
        // Perturb in the rotation of the camera in the angle-axis
        // representation
        CameraToAngelAxisAndCenter(camera, angle_axis, center);
        if (rotation_sigma > 0.0) {
            PerturbPoint3(rotation_sigma, angle_axis);
        }
        AngleAxisAndCenterToCamera(angle_axis, center, camera);

        if (translation_sigma > 0.0)
            PerturbPoint3(translation_sigma, camera + camera_block_size() - 6);
    }
}

common.h

#pragma once

/// 從文件讀入BAL dataset原始數據，然后進行分割儲存
class BALProblem {
public:
    /// load bal data from text file
    explicit BALProblem(const std::string &filename, bool use_quaternions = false);

    ~BALProblem() {
        delete[] point_index_;
        delete[] camera_index_;
        delete[] observations_;
        delete[] parameters_;
    }

    /// save results to text file
    void WriteToFile(const std::string &filename) const;

    /// save results to ply pointcloud
    void WriteToPLYFile(const std::string &filename) const;

    void Normalize();

    void Perturb(const double rotation_sigma,
                 const double translation_sigma,
                 const double point_sigma);

    int camera_block_size() const { return use_quaternions_ ? 10 : 9; }

    int point_block_size() const { return 3; }

    int num_cameras() const { return num_cameras_; }

    int num_points() const { return num_points_; }

    int num_observations() const { return num_observations_; }

    int num_parameters() const { return num_parameters_; }

    const int *point_index() const { return point_index_; }

    const int *camera_index() const { return camera_index_; }

    const double *observations() const { return observations_; }

    const double *parameters() const { return parameters_; }

    const double *cameras() const { return parameters_; }

    const double *points() const { return parameters_ + camera_block_size() * num_cameras_; }

    /// camera參數的起始地址
    double *mutable_cameras() { return parameters_; }

    double *mutable_points() { return parameters_ + camera_block_size() * num_cameras_; }

    double *mutable_camera_for_observation(int i) {
        return mutable_cameras() + camera_index_[i] * camera_block_size();
    }

    double *mutable_point_for_observation(int i) {
        return mutable_points() + point_index_[i] * point_block_size();
    }

    const double *camera_for_observation(int i) const {
        return cameras() + camera_index_[i] * camera_block_size();
    }

    const double *point_for_observation(int i) const {
        return points() + point_index_[i] * point_block_size();
    }

private:
    void CameraToAngelAxisAndCenter(const double *camera,
                                    double *angle_axis,
                                    double *center) const;

    void AngleAxisAndCenterToCamera(const double *angle_axis,
                                    const double *center,
                                    double *camera) const;

    int num_cameras_;
    int num_points_;
    int num_observations_;
    int num_parameters_;
    bool use_quaternions_;

    int *point_index_;      // 每個observation對應的point index
    int *camera_index_;     // 每個observation對應的camera index
    double *observations_;  
    double *parameters_;
};

4.random.h

#ifndef RAND_H
#define RAND_H

#include <math.h>
#include <stdlib.h>

inline double RandDouble()
{
    double r = static_cast<double>(rand());
    return r / RAND_MAX;
}

inline double RandNormal()
{
    double x1, x2, w;
    do{
        x1 = 2.0 * RandDouble() - 1.0;
        x2 = 2.0 * RandDouble() - 1.0;
        w = x1 * x1 + x2 * x2;
    }while( w >= 1.0 || w == 0.0);

    w = sqrt((-2.0 * log(w))/w);
    return x1 * w;
}

#endif // random.h

rotation.h

#ifndef ROTATION_H
#define ROTATION_H

#include <algorithm>
#include <cmath>
#include <limits>

//////////////////////////////////////////////////////////////////
// math functions needed for rotation conversion. 

// dot and cross production 

template<typename T>
inline T DotProduct(const T x[3], const T y[3]) {
    return (x[0] * y[0] + x[1] * y[1] + x[2] * y[2]);
}

template<typename T>
inline void CrossProduct(const T x[3], const T y[3], T result[3]) {
    result[0] = x[1] * y[2] - x[2] * y[1];
    result[1] = x[2] * y[0] - x[0] * y[2];
    result[2] = x[0] * y[1] - x[1] * y[0];
}


//////////////////////////////////////////////////////////////////


// Converts from a angle anxis to quaternion : 
template<typename T>
inline void AngleAxisToQuaternion(const T *angle_axis, T *quaternion) {
    const T &a0 = angle_axis[0];
    const T &a1 = angle_axis[1];
    const T &a2 = angle_axis[2];
    const T theta_squared = a0 * a0 + a1 * a1 + a2 * a2;

    if (theta_squared > T(std::numeric_limits<double>::epsilon())) {
        const T theta = sqrt(theta_squared);
        const T half_theta = theta * T(0.5);
        const T k = sin(half_theta) / theta;
        quaternion[0] = cos(half_theta);
        quaternion[1] = a0 * k;
        quaternion[2] = a1 * k;
        quaternion[3] = a2 * k;
    } else { // in case if theta_squared is zero
        const T k(0.5);
        quaternion[0] = T(1.0);
        quaternion[1] = a0 * k;
        quaternion[2] = a1 * k;
        quaternion[3] = a2 * k;
    }
}

template<typename T>
inline void QuaternionToAngleAxis(const T *quaternion, T *angle_axis) {
    const T &q1 = quaternion[1];
    const T &q2 = quaternion[2];
    const T &q3 = quaternion[3];
    const T sin_squared_theta = q1 * q1 + q2 * q2 + q3 * q3;

    // For quaternions representing non-zero rotation, the conversion
    // is numercially stable
    if (sin_squared_theta > T(std::numeric_limits<double>::epsilon())) {
        const T sin_theta = sqrt(sin_squared_theta);
        const T &cos_theta = quaternion[0];

        // If cos_theta is negative, theta is greater than pi/2, which
        // means that angle for the angle_axis vector which is 2 * theta
        // would be greater than pi...

        const T two_theta = T(2.0) * ((cos_theta < 0.0)
                                      ? atan2(-sin_theta, -cos_theta)
                                      : atan2(sin_theta, cos_theta));
        const T k = two_theta / sin_theta;

        angle_axis[0] = q1 * k;
        angle_axis[1] = q2 * k;
        angle_axis[2] = q3 * k;
    } else {
        // For zero rotation, sqrt() will produce NaN in derivative since
        // the argument is zero. By approximating with a Taylor series,
        // and truncating at one term, the value and first derivatives will be
        // computed correctly when Jets are used..
        const T k(2.0);
        angle_axis[0] = q1 * k;
        angle_axis[1] = q2 * k;
        angle_axis[2] = q3 * k;
    }

}

template<typename T>
inline void AngleAxisRotatePoint(const T angle_axis[3], const T pt[3], T result[3]) {
    const T theta2 = DotProduct(angle_axis, angle_axis); 
    if (theta2 > T(std::numeric_limits<double>::epsilon())) {
        // Away from zero, use the rodriguez formula
        //
        //   result = pt costheta +
        //            (w x pt) * sintheta +
        //            w (w . pt) (1 - costheta)
        //
        // We want to be careful to only evaluate the square root if the
        // norm of the angle_axis vector is greater than zero. Otherwise
        // we get a division by zero.
        //
        const T theta = sqrt(theta2);  //旋轉角度，單位弧度
        const T costheta = cos(theta);
        const T sintheta = sin(theta);
        const T theta_inverse = 1.0 / theta;

        const T w[3] = {angle_axis[0] * theta_inverse,   //歸一化
                        angle_axis[1] * theta_inverse,
                        angle_axis[2] * theta_inverse};

        // Explicitly inlined evaluation of the cross product for
        // performance reasons.
        /*const T w_cross_pt[3] = { w[1] * pt[2] - w[2] * pt[1],
                                  w[2] * pt[0] - w[0] * pt[2],
                                  w[0] * pt[1] - w[1] * pt[0] };*/
        T w_cross_pt[3];
        CrossProduct(w, pt, w_cross_pt); //t 叉乘 n

        const T tmp = DotProduct(w, pt) * (T(1.0) - costheta); //t 點乘 n 
        //    (w[0] * pt[0] + w[1] * pt[1] + w[2] * pt[2]) * (T(1.0) - costheta);

        result[0] = pt[0] * costheta + w_cross_pt[0] * sintheta + w[0] * tmp;
        result[1] = pt[1] * costheta + w_cross_pt[1] * sintheta + w[1] * tmp;
        result[2] = pt[2] * costheta + w_cross_pt[2] * sintheta + w[2] * tmp;
    } else {
        // Near zero, the first order Taylor approximation of the rotation
        // matrix R corresponding to a vector w and angle w is
        //
        //   R = I + hat(w) * sin(theta)
        //
        // But sintheta ~ theta and theta * w = angle_axis, which gives us
        //
        //  R = I + hat(w)
        //
        // and actually performing multiplication with the point pt, gives us
        // R * pt = pt + w x pt.
        //
        // Switching to the Taylor expansion near zero provides meaningful
        // derivatives when evaluated using Jets.
        //
        // Explicitly inlined evaluation of the cross product for
        // performance reasons.
        /*const T w_cross_pt[3] = { angle_axis[1] * pt[2] - angle_axis[2] * pt[1],
                                  angle_axis[2] * pt[0] - angle_axis[0] * pt[2],
                                  angle_axis[0] * pt[1] - angle_axis[1] * pt[0] };*/
        T w_cross_pt[3];
        CrossProduct(angle_axis, pt, w_cross_pt);

        result[0] = pt[0] + w_cross_pt[0];
        result[1] = pt[1] + w_cross_pt[1];
        result[2] = pt[2] + w_cross_pt[2];
    }
}

#endif // rotation.h

5.CMakeLists.txt

cmake_minimum_required(VERSION 2.8)

project(bundle_adjustment)
set(CMAKE_BUILD_TYPE "Release")
set(CMAKE_CXX_FLAGS "-O3 -std=c++11")

LIST(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake)

Find_Package(G2O REQUIRED)
Find_Package(Eigen3 REQUIRED)
Find_Package(Ceres REQUIRED)
Find_Package(Sophus REQUIRED)
Find_Package(CSparse REQUIRED)

SET(G2O_LIBS g2o_csparse_extension g2o_stuff g2o_core cxsparse)

include_directories(${PROJECT_SOURCE_DIR} ${EIGEN3_INCLUDE_DIR} ${CSPARSE_INCLUDE_DIR})

add_library(bal_common common.cpp)
add_executable(bundle_adjustment_g2o bundle_adjustment_g2o.cpp)
add_executable(bundle_adjustment_ceres bundle_adjustment_ceres.cpp)

target_link_libraries(bundle_adjustment_ceres ${CERES_LIBRARIES} bal_common)
target_link_libraries(bundle_adjustment_g2o ${G2O_LIBS} bal_common)