【slam十四讲第二版】【课本例题代码向】【第九讲~后端Ⅰ】【安装Meshlab】【BAL数据集格式】【ceres求解BA】【g2o求解BA】

0 前言

• 参考：视觉SLAM十四讲CH9代码解析及课后习题详解

1 安装Meshlab: 三维几何网格处理

• 参考：视觉SLAM十四讲学习1 环境配置
• 参考：添加链接描述视觉SLAM十四讲第三方库
• 安装命令为：

sudo add-apt-repository ppa:zarquon42/meshlab
sudo apt-get update
sudo apt-get install meshlab

• 用来打开.ply文件

2 BAL数据集

2.1 BAL数据集格式

• 详细格式请见：https://grail.cs.washington.edu/projects/bal/
• 数据格式如下：

<num_cameras> <num_points> <num_observations> 
<camera_index_1> <point_index_1> <x_1> <y_1> 
... 
<camera_index_num_observations> <point_index_num_observations> <x_num_observations> <y_num_observations> 
<camera_1> 
... 
<camera_num_cameras> 
<point_1> 
.. . 
<point_num_points>

其中，相机和点索引从 0 开始。每个相机是一组 9 个参数 - R、t、f、k1 和 k2。旋转 R 被指定为Rodrigues 向量。

1. 其中，第一行的数据是相机数量、路标点的数量、观察到路标点的总数量，其中观察到路标点的总数量是所有观察到的观测点，每个相机观测到的路标点都算一个
2. 然后后面是每行有4个数据，分别为相机标号、路标点标号、x坐标、y坐标，总共有观察到路标点的总数量行
3. 下面每一行只有一个数据，这里分为两部分，第一部分是相机的数据，每九行算作一组数据，分别是- R(3个)、t(3个)、f、k1 和 k2，总共相机数量 * 9行
4. 然后是第二部分，是路标点的点坐标，每三行算作一组数据，总共路标点的数量 * 3

3 使用ceres实现BA

• 工程所需的cmake文件自取：链接: https://pan.baidu.com/s/1AIFaH4O5IMNoHeR0vW7vfQ 提取码: b0k3
• 整个工程自取链接: https://pan.baidu.com/s/117KshglyrDldFV59I8b0-A 提取码: tlbm

3.1 bundle_adjustment_ceres.cpp

#include 
#include 
#include "common.h"//使用common.h中定义的BALProblem类读入该文件的内容
#include "SnavelyReprojectionError.h"

using namespace std;

void SolveBA(BALProblem &bal_problem);//定义SolveBA函数

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);//通过Perturb函数给数据加入噪声
    bal_problem.WriteToPLYFile("initial.ply");//存储最初点云
    SolveBA(bal_problem);//BA求解
    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();

    // 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]);//调用SnavelyReprojectionError.h c文件
        //等价于下式
        //ceres::CostFunction *cost_function = new ceres::AutoDiffCostFunction(
        //        new SnavelyReprojectionError(observed_x, observed_y));

        // If enabled use Huber's loss function. 如果启用，请使用Huber的损失函数。
        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.
        // 每个观测点对应于一对相机和一个点，分别由camera_index()[i]和point_index()[i]标识。
        double *camera = cameras + camera_block_size * bal_problem.camera_index()[i];
        //camera_block_size = bal_problem.camera_block_size();
        //*camera = cameras + bal_problem.camera_block_size() * bal_problem.camera_index()[i]
        double *point = points + point_block_size * bal_problem.point_index()[i];//得到的是对应路标点在世界坐标系下的三维坐标
        //point_block_size = bal_problem.point_block_size();
        //*point = points + bal_problem.point_block_size() * bal_problem.point_index()[i]
        problem.AddResidualBlock(cost_function, loss_function, camera, point); // 向问题中添加误差项
        //CostFunction* : 描述最小二乘的基本形式即代价函数
        //LossFunction* : 描述核函数的形式
    }

    // show some information here ...
    std::cout << "bal problem file loaded..." << std::endl;//输出bal problem file loaded...
    std::cout << "bal problem have " << bal_problem.num_cameras() << " cameras and "
              << bal_problem.num_points() << " points. " << std::endl;//bal_problem.num_cameras()表示相机位姿个数
    //bal_problem.num_points()表示路标点数
    std::cout << "Forming " << bal_problem.num_observations() << " observations. " << std::endl;// bal_problem.num_observations()表示观测点数

    std::cout << "Solving ceres BA ... " << endl;//BA求解
    ceres::Solver::Options options; // 这里有很多配置项可以填Options类嵌入在Solver类中 ，在Options类中可以设置关于求解器的参数
    options.linear_solver_type = ceres::LinearSolverType::SPARSE_SCHUR;
    // 增量方程如何求解 这里的linear_solver_type 是一个Linear_solver_type的枚举类型的变量
    //使用Schur消元
    options.minimizer_progress_to_stdout = true;
    ceres::Solver::Summary summary;// 优化信息
    ceres::Solve(options, &problem, &summary);
    std::cout << summary.FullReport() << "\n";
}

3.2 CMakeLists.txt

cmake_minimum_required(VERSION 2.8)

project(bundle_adjustment_ceres)
set(CMAKE_BUILD_TYPE "Release")
set(CMAKE_CXX_STANDARD 14)

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 src/common.cpp)
add_executable(bundle_adjustment_ceres src/bundle_adjustment_ceres.cpp)

target_link_libraries(bundle_adjustment_ceres ${CERES_LIBRARIES} bal_common)

3.3 其他文件

3.3.1 残差函数SnaveReprojectionError.h

#ifndef SnavelyReprojection_H
#define SnavelyReprojection_H

#include 
#include "ceres/ceres.h"
#include "rotation.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 camera[0,1,2]是轴角旋转
        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。 camera参数，[6]焦距，[7-8]二阶和四阶径向畸变
    // point : 3D location. 3D定位。
    // 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);//内联函数作用是是point经过轴角camera旋转后得到的点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;
};

#endif // SnavelyReprojection.h

3.3.2 旋转变换转换rotation.h

#ifndef ROTATION_H
#define ROTATION_H

#include 
#include 
#include 

//
// 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]) {//内联函数作用是是pt经过轴角angle_axis旋转后得到的点result
    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);

        const T tmp = DotProduct(w, pt) * (T(1.0) - costheta);
        //    (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

3.3.3 BAL数据集处理的库函数

3.3.3.1 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_;
};

3.3.3.2 common.cpp

#include 
#include 
#include 
#include 
#include 
#include 
#include 

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

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

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. ";
}

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_];
    camera_index_ = new int[num_observations_];
    observations_ = new double[2 * num_observations_];

    num_parameters_ = 9 * num_cameras_ + 3 * num_points_;
    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);
        }
    }

    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);
            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;
    }
}

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);
}

// 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();
}

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);
    }

    // c = -R't
    Eigen::VectorXd inverse_rotation = -angle_axis_ref;
    AngleAxisRotatePoint(inverse_rotation.data(),
                         camera + camera_block_size() - 6,
                         center);
    VectorRef(center, 3) *= -1.0;
}

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;
    }

    // t = -R * c
    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();//获取路标点坐标开始的指针位置
    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>();
    }

    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);
    }

    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;
        CameraToAngelAxisAndCenter(camera, angle_axis, center);
        // center = scale * (center - median)
        VectorRef(center, 3) = scale * (VectorRef(center, 3) - median);
        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);
        }
    }

    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);
    }
}

3.4 输出

/home/bupo/my_study/slam14/slam14_my/cap9/bundle_adjustment_ceres/cmake-build-release/bundle_adjustment_ceres src/problem-16-22106-pre.txt
Header: 16 22106 83718bal problem file loaded...
bal problem have 16 cameras and 22106 points. 
Forming 83718 observations. 
Solving ceres BA ... 
iter      cost      cost_change  |gradient|   |step|    tr_ratio  tr_radius  ls_iter  iter_time  total_time
   0  1.842900e+07    0.00e+00    2.04e+06   0.00e+00   0.00e+00  1.00e+04        0    5.30e-02    1.66e-01
   1  1.449093e+06    1.70e+07    1.75e+06   2.16e+03   1.84e+00  3.00e+04        1    1.18e-01    2.85e-01
   2  5.848543e+04    1.39e+06    1.30e+06   1.55e+03   1.87e+00  9.00e+04        1    1.03e-01    3.88e-01
   3  1.581483e+04    4.27e+04    4.98e+05   4.98e+02   1.29e+00  2.70e+05        1    1.02e-01    4.90e-01
   4  1.251823e+04    3.30e+03    4.64e+04   9.96e+01   1.11e+00  8.10e+05        1    1.02e-01    5.91e-01
   5  1.240936e+04    1.09e+02    9.78e+03   1.33e+01   1.42e+00  2.43e+06        1    1.02e-01    6.93e-01
   6  1.237699e+04    3.24e+01    3.91e+03   5.04e+00   1.70e+00  7.29e+06        1    1.01e-01    7.95e-01
   7  1.236187e+04    1.51e+01    1.96e+03   3.40e+00   1.75e+00  2.19e+07        1    1.01e-01    8.96e-01
   8  1.235405e+04    7.82e+00    1.03e+03   2.40e+00   1.76e+00  6.56e+07        1    1.02e-01    9.98e-01
   9  1.234934e+04    4.71e+00    5.04e+02   1.67e+00   1.87e+00  1.97e+08        1    1.02e-01    1.10e+00
  10  1.234610e+04    3.24e+00    4.31e+02   1.15e+00   1.88e+00  5.90e+08        1    1.02e-01    1.20e+00
  11  1.234386e+04    2.24e+00    3.27e+02   8.44e-01   1.90e+00  1.77e+09        1    1.02e-01    1.30e+00
  12  1.234232e+04    1.54e+00    3.44e+02   6.69e-01   1.82e+00  5.31e+09        1    1.05e-01    1.41e+00
  13  1.234126e+04    1.07e+00    2.21e+02   5.45e-01   1.91e+00  1.59e+10        1    1.06e-01    1.51e+00
  14  1.234047e+04    7.90e-01    1.12e+02   4.84e-01   1.87e+00  4.78e+10        1    1.03e-01    1.62e+00
  15  1.233986e+04    6.07e-01    1.02e+02   4.22e-01   1.95e+00  1.43e+11        1    1.02e-01    1.72e+00
  16  1.233934e+04    5.22e-01    1.03e+02   3.82e-01   1.97e+00  4.30e+11        1    1.02e-01    1.82e+00
  17  1.233891e+04    4.25e-01    1.07e+02   3.46e-01   1.93e+00  1.29e+12        1    1.01e-01    1.92e+00
  18  1.233855e+04    3.59e-01    1.04e+02   3.15e-01   1.96e+00  3.87e+12        1    1.03e-01    2.03e+00
  19  1.233825e+04    3.06e-01    9.27e+01   2.88e-01   1.98e+00  1.16e+13        1    1.02e-01    2.13e+00
  20  1.233799e+04    2.61e-01    1.17e+02   2.16e-01   1.97e+00  3.49e+13        1    1.02e-01    2.23e+00
  21  1.233777e+04    2.18e-01    1.22e+02   1.15e-01   1.97e+00  1.05e+14        1    1.01e-01    2.33e+00
  22  1.233760e+04    1.73e-01    1.10e+02   9.67e-02   1.89e+00  3.14e+14        1    1.02e-01    2.43e+00
  23  1.233746e+04    1.37e-01    1.14e+02   1.11e-01   1.98e+00  9.41e+14        1    1.02e-01    2.54e+00
  24  1.233735e+04    1.13e-01    1.17e+02   1.96e-01   1.96e+00  2.82e+15        1    1.02e-01    2.64e+00
  25  1.233735e+04    0.00e+00    1.17e+02   0.00e+00   0.00e+00  1.41e+15        1    3.65e-02    2.67e+00
WARNING: Logging before InitGoogleLogging() is written to STDERR
W0725 09:28:28.405835  5092 levenberg_marquardt_strategy.cc:115] Linear solver failure. Failed to compute a step: CHOLMOD warning: Matrix not positive definite.
  26  1.233725e+04    9.50e-02    1.20e+02   3.98e-01   1.99e+00  4.24e+15        1    1.02e-01    2.78e+00
  27  1.233725e+04    0.00e+00    1.20e+02   0.00e+00   0.00e+00  2.12e+15        1    3.71e-02    2.81e+00
W0725 09:28:28.545459  5092 levenberg_marquardt_strategy.cc:115] Linear solver failure. Failed to compute a step: CHOLMOD warning: Matrix not positive definite.
  28  1.233725e+04    0.00e+00    1.20e+02   0.00e+00   0.00e+00  5.30e+14        1    3.47e-02    2.85e+00
W0725 09:28:28.580186  5092 levenberg_marquardt_strategy.cc:115] Linear solver failure. Failed to compute a step: CHOLMOD warning: Matrix not positive definite.
  29  1.233718e+04    6.92e-02    5.76e+01   1.59e-01   1.70e+00  1.59e+15        1    9.89e-02    2.95e+00
  30  1.233714e+04    3.65e-02    5.80e+01   6.25e-01   1.93e+00  4.77e+15        1    1.06e-01    3.05e+00
W0725 09:28:28.822146  5092 levenberg_marquardt_strategy.cc:115] Linear solver failure. Failed to compute a step: CHOLMOD warning: Matrix not positive definite.
  31  1.233714e+04    0.00e+00    5.80e+01   0.00e+00   0.00e+00  2.38e+15        1    3.72e-02    3.09e+00
  32  1.233714e+04    0.00e+00    5.80e+01   0.00e+00   0.00e+00  5.96e+14        1    3.50e-02    3.13e+00
W0725 09:28:28.857184  5092 levenberg_marquardt_strategy.cc:115] Linear solver failure. Failed to compute a step: CHOLMOD warning: Matrix not positive definite.
  33  1.233711e+04    3.32e-02    5.99e+01   2.01e-01   2.00e+00  1.79e+15        1    9.95e-02    3.22e+00
  34  1.233708e+04    3.14e-02    6.02e+01   3.64e-01   2.00e+00  5.36e+15        1    1.05e-01    3.33e+00
  35  1.233708e+04    0.00e+00    6.02e+01   0.00e+00   0.00e+00  2.68e+15        1    3.72e-02    3.37e+00
W0725 09:28:29.098881  5092 levenberg_marquardt_strategy.cc:115] Linear solver failure. Failed to compute a step: CHOLMOD warning: Matrix not positive definite.
  36  1.233708e+04    0.00e+00    6.02e+01   0.00e+00   0.00e+00  6.70e+14        1    3.50e-02    3.40e+00
W0725 09:28:29.133908  5092 levenberg_marquardt_strategy.cc:115] Linear solver failure. Failed to compute a step: CHOLMOD warning: Matrix not positive definite.
  37  1.233705e+04    2.50e-02    2.04e+01   1.37e-01   1.68e+00  2.01e+15        1    1.01e-01    3.50e+00
  38  1.233704e+04    1.58e-02    2.01e+01   4.77e-01   1.95e+00  6.03e+15        1    1.08e-01    3.61e+00
W0725 09:28:29.379166  5092 levenberg_marquardt_strategy.cc:115] Linear solver failure. Failed to compute a step: CHOLMOD warning: Matrix not positive definite.
  39  1.233704e+04    0.00e+00    2.01e+01   0.00e+00   0.00e+00  3.02e+15        1    3.70e-02    3.65e+00
  40  1.233704e+04    0.00e+00    2.01e+01   0.00e+00   0.00e+00  7.54e+14        1    3.51e-02    3.68e+00
W0725 09:28:29.414316  5092 levenberg_marquardt_strategy.cc:115] Linear solver failure. Failed to compute a step: CHOLMOD warning: Matrix not positive definite.
  41  1.233702e+04    1.51e-02    2.07e+01   3.02e-01   2.00e+00  2.26e+15        1    9.98e-02    3.78e+00
  42  1.233701e+04    1.48e-02    2.00e+01   1.14e+00   1.99e+00  6.79e+15        1    1.07e-01    3.89e+00
  43  1.233701e+04    0.00e+00    2.00e+01   0.00e+00   0.00e+00  3.39e+15        1    3.75e-02    3.93e+00
W0725 09:28:29.658689  5092 levenberg_marquardt_strategy.cc:115] Linear solver failure. Failed to compute a step: CHOLMOD warning: Matrix not positive definite.
  44  1.233701e+04    0.00e+00    2.00e+01   0.00e+00   0.00e+00  8.48e+14        1    3.53e-02    3.96e+00
W0725 09:28:29.694017  5092 levenberg_marquardt_strategy.cc:115] Linear solver failure. Failed to compute a step: CHOLMOD warning: Matrix not positive definite.
  45  1.233700e+04    1.42e-02    2.09e+01   2.20e-01   1.99e+00  2.54e+15        1    1.01e-01    4.06e+00
  46  1.233698e+04    1.39e-02    1.88e+01   1.65e+00   2.00e+00  7.63e+15        1    1.05e-01    4.17e+00
W0725 09:28:29.937575  5092 levenberg_marquardt_strategy.cc:115] Linear solver failure. Failed to compute a step: CHOLMOD warning: Matrix not positive definite.
  47  1.233698e+04    0.00e+00    1.88e+01   0.00e+00   0.00e+00  3.82e+15        1    3.72e-02    4.21e+00
  48  1.233698e+04    0.00e+00    1.88e+01   0.00e+00   0.00e+00  9.54e+14        1    3.50e-02    4.24e+00
W0725 09:28:29.972617  5092 levenberg_marquardt_strategy.cc:115] Linear solver failure. Failed to compute a step: CHOLMOD warning: Matrix not positive definite.
  49  1.233697e+04    1.36e-02    2.14e+01   3.81e-01   2.00e+00  2.86e+15        1    1.00e-01    4.34e+00
W0725 09:28:30.109880  5092 levenberg_marquardt_strategy.cc:115] Linear solver failure. Failed to compute a step: CHOLMOD warning: Matrix not positive definite.
  50  1.233697e+04    0.00e+00    2.14e+01   0.00e+00   0.00e+00  1.43e+15        1    3.69e-02    4.38e+00

Solver Summary (v 2.0.0-eigen-(3.3.9)-lapack-suitesparse-(5.1.2)-cxsparse-(3.1.9)-eigensparse-no_openmp)

                                     Original                  Reduced
Parameter blocks                        22122                    22122
Parameters                              66462                    66462
Residual blocks                         83718                    83718
Residuals                              167436                   167436

Minimizer                        TRUST_REGION

Sparse linear algebra library    SUITE_SPARSE
Trust region strategy     LEVENBERG_MARQUARDT

                                        Given                     Used
Linear solver                    SPARSE_SCHUR             SPARSE_SCHUR
Threads                                     1                        1
Linear solver ordering              AUTOMATIC                 22106,16
Schur structure                         2,3,9                    2,3,9

Cost:
Initial                          1.842900e+07
Final                            1.233697e+04
Change                           1.841667e+07

Minimizer iterations                       51
Successful steps                           37
Unsuccessful steps                         14

Time (in seconds):
Preprocessor                         0.113155

  Residual only evaluation           0.447670 (36)
  Jacobian & residual evaluation     1.553284 (37)
  Linear solver                      1.851589 (50)
Minimizer                            4.267012

Postprocessor                        0.005809
Total                                4.385978

Termination:                   NO_CONVERGENCE (Maximum number of iterations reached. Number of iterations: 50.)


进程已结束,退出代码0

• initial.ply
  

• final.ply

4 实践：g2o求解BA

• 工程所需的cmake文件自取：链接: https://pan.baidu.com/s/1AIFaH4O5IMNoHeR0vW7vfQ 提取码: b0k3
• 整个工程自取：链接: https://pan.baidu.com/s/1a7DQtQLh7hBHSTYeS1UPjQ 提取码: 52s3

4.1 bundle_adjustment_g2o.cpp

#include //g2o顶点（Vertex）头文件 视觉slam十四讲p141用顶点表示优化变量，用边表示误差项
#include //g2o边（edge）头文件
#include //求解器头文件
#include //列文伯格——马尔夸特算法头文件
#include 
#include //鲁棒核函数
#include 

#include "common.h"//使用common.h中定义的BALProblem类读入该文件的内容
#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)
    {
        //每个相机一共有一共有6维的姿态
        rotation = Sophus::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];//1维焦距
        //2维畸变参数
        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;//1维焦距
        data_addr[7] = k1;//2维畸变参数
        data_addr[8] = k2;//2维畸变参数
    }

    Sophus::SO3d rotation;
    Vector3d translation = Vector3d::Zero();//置0
    double focal = 0;//初始化为0
    double k1 = 0, k2 = 0;//将2维畸变参数初始化为0
};

/// 位姿加相机内参的顶点，9维，前三维为so3，接下去为t, f, k1, k2
class VertexPoseAndIntrinsics : public g2o::BaseVertex<9, PoseAndIntrinsics> {
public://以下定义的成员变量和成员函数都是公有的
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;//解决Eigen库数据结构内存对齐问题

    VertexPoseAndIntrinsics() {}

    // 重置
    virtual void setToOriginImpl() override //virtual表示该函数为虚函数，override保留字表示当前函数重写了基类的虚函数
    {
        _estimate = PoseAndIntrinsics();
    }
    // 更新
    virtual void oplusImpl(const double *update) override {
        _estimate.rotation = Sophus::SO3d::exp(Vector3d(update[0], update[1], update[2])) * _estimate.rotation;
        _estimate.translation += Vector3d(update[3], update[4], update[5]);//更新量累加
        _estimate.focal += update[6];//1维焦距更新量累加
        _estimate.k1 += update[7];//2维畸变参数更新量累加
        _estimate.k2 += update[8];//2维畸变参数更新量累加
    }

    /// 根据估计值投影一个点
    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) {} //istream类是c++标准输入流的一个基类
    //可参照C++ Primer Plus第六版的6.8节
    virtual bool write(ostream &out) const {} //ostream类是c++标准输出流的一个基类
    //可参照C++ Primer Plus第六版的6.8节
};

class VertexPoint : public g2o::BaseVertex<3, Vector3d> {
public://以下定义的成员变量和成员函数都是公有的
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;//解决Eigen库数据结构内存对齐问题

    VertexPoint() {}
    // 重置
    virtual void setToOriginImpl() override //virtual表示该函数为虚函数，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) {} //istream类是c++标准输入流的一个基类
    //可参照C++ Primer Plus第六版的6.8节
    virtual bool write(ostream &out) const {} //ostream类是c++标准输出流的一个基类
    //可参照C++ Primer Plus第六版的6.8节
};
// 误差模型 模板参数：观测值维度，类型，连接顶点类型
class EdgeProjection :
        public g2o::BaseBinaryEdge<2, Vector2d, VertexPoseAndIntrinsics, VertexPoint> {
public://以下定义的成员变量和成员函数都是公有的
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;//解决Eigen库数据结构内存对齐问题

    // 计算误差
    virtual void computeError() override //virtual表示虚函数，保留字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) {}//istream类是c++标准输入流的一个基类
    //可参照C++ Primer Plus第六版的6.8节
    virtual bool write(ostream &out) const {}//ostream类是c++标准输出流的一个基类
    //可参照C++ Primer Plus第六版的6.8节

};

void SolveBA(BALProblem &bal_problem);//定义SolveBA函数

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);//通过Perturb函数给数据加入噪声
    bal_problem.WriteToPLYFile("initial.ply");//存储最初点云
    SolveBA(bal_problem);//BA求解
    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;// 每个误差项优化变量维度为9，误差值维度为3
    typedef g2o::LinearSolverCSparse<BlockSolverType::PoseMatrixType> LinearSolverType;
    // use LM
    auto solver = new g2o::OptimizationAlgorithmLevenberg(
            g2o::make_unique<BlockSolverType>(g2o::make_unique<LinearSolverType>()));
    //c++中的make_unique表示智能指针类型
    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));//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);// g2o 中必须设置 marg 参见第十讲内容，//设置边缘化
        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);//设置迭代次数为40

    // set to bal problem
    for (int i = 0; i < bal_problem.num_cameras(); ++i) {
        double *camera = cameras + camera_block_size * i;
        //camera_block_size = bal_problem.camera_block_size();
        //*camera = cameras + bal_problem.camera_block_size() * bal_problem.camera_index()[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];
    }
}

4.2 CMakeLists.txt

cmake_minimum_required(VERSION 2.8)

project(bundle_adjustment_ceres)
set(CMAKE_BUILD_TYPE "Release")
set(CMAKE_CXX_STANDARD 14)

LIST(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake)
include_directories(
        "/usr/include/eigen3"
)

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

SET(G2O_LIBS  g2o_core g2o_stuff g2o_types_sba g2o_csparse_extension g2o_types_slam3d cxsparse)

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

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

target_link_libraries(bundle_adjustment_ceres bal_common
        ${G2O_LIBS}
        ${CSPARSE_LIBRARY}
        ${Sophus_LIBRARIES} fmt
        )

4.3 其他文件

4.3.1 旋转变换转换rotation.h

• 参考3.3.2

4.3.2 BAL数据集处理的库函数

4.3.2.1 common.h

• 参考3.3.3.1

4.3.2.2 common.cpp

• 参考3.3.3.2

4.4 输出：

/home/bupo/my_study/slam14/slam14_my/cap9/bundle_adjustment_g2o/cmake-build-debug/bundle_adjustment_ceres ./src/problem-16-22106-pre.txt
Header: 16 22106 83718iteration= 0	 chi2= 8894423.022949	 time= 0.226057	 cumTime= 0.226057	 edges= 83718	 schur= 1	 lambda= 227.832660	 levenbergIter= 1
iteration= 1	 chi2= 1772145.050517	 time= 0.199289	 cumTime= 0.425347	 edges= 83718	 schur= 1	 lambda= 75.944220	 levenbergIter= 1
iteration= 2	 chi2= 752585.293391	 time= 0.199969	 cumTime= 0.625315	 edges= 83718	 schur= 1	 lambda= 25.314740	 levenbergIter= 1
iteration= 3	 chi2= 402814.243627	 time= 0.199741	 cumTime= 0.825056	 edges= 83718	 schur= 1	 lambda= 8.438247	 levenbergIter= 1
iteration= 4	 chi2= 284879.378894	 time= 0.199375	 cumTime= 1.02443	 edges= 83718	 schur= 1	 lambda= 2.812749	 levenbergIter= 1
iteration= 5	 chi2= 238356.214415	 time= 0.198267	 cumTime= 1.2227	 edges= 83718	 schur= 1	 lambda= 0.937583	 levenbergIter= 1
iteration= 6	 chi2= 193550.755079	 time= 0.198764	 cumTime= 1.42146	 edges= 83718	 schur= 1	 lambda= 0.312528	 levenbergIter= 1
iteration= 7	 chi2= 146859.909574	 time= 0.198121	 cumTime= 1.61958	 edges= 83718	 schur= 1	 lambda= 0.104176	 levenbergIter= 1
iteration= 8	 chi2= 122887.700218	 time= 0.199035	 cumTime= 1.81862	 edges= 83718	 schur= 1	 lambda= 0.069451	 levenbergIter= 1
iteration= 9	 chi2= 97810.139925	 time= 0.198906	 cumTime= 2.01753	 edges= 83718	 schur= 1	 lambda= 0.046300	 levenbergIter= 1
iteration= 10	 chi2= 80329.940265	 time= 0.198727	 cumTime= 2.21625	 edges= 83718	 schur= 1	 lambda= 0.030867	 levenbergIter= 1
iteration= 11	 chi2= 65663.994405	 time= 0.197927	 cumTime= 2.41418	 edges= 83718	 schur= 1	 lambda= 0.020578	 levenbergIter= 1
iteration= 12	 chi2= 55960.726637	 time= 0.198214	 cumTime= 2.61239	 edges= 83718	 schur= 1	 lambda= 0.013719	 levenbergIter= 1
iteration= 13	 chi2= 53275.547797	 time= 0.198422	 cumTime= 2.81082	 edges= 83718	 schur= 1	 lambda= 0.009146	 levenbergIter= 1
iteration= 14	 chi2= 35983.312124	 time= 0.243725	 cumTime= 3.05454	 edges= 83718	 schur= 1	 lambda= 0.006097	 levenbergIter= 2
iteration= 15	 chi2= 32091.891518	 time= 0.28862	 cumTime= 3.34316	 edges= 83718	 schur= 1	 lambda= 0.016259	 levenbergIter= 3
iteration= 16	 chi2= 31156.262647	 time= 0.242385	 cumTime= 3.58555	 edges= 83718	 schur= 1	 lambda= 0.021679	 levenbergIter= 2
iteration= 17	 chi2= 30773.139623	 time= 0.198416	 cumTime= 3.78396	 edges= 83718	 schur= 1	 lambda= 0.014453	 levenbergIter= 1
iteration= 18	 chi2= 29079.563460	 time= 0.245161	 cumTime= 4.02912	 edges= 83718	 schur= 1	 lambda= 0.012488	 levenbergIter= 2
iteration= 19	 chi2= 28484.154313	 time= 0.242993	 cumTime= 4.27212	 edges= 83718	 schur= 1	 lambda= 0.016651	 levenbergIter= 2
iteration= 20	 chi2= 28445.405201	 time= 0.197765	 cumTime= 4.46988	 edges= 83718	 schur= 1	 lambda= 0.011101	 levenbergIter= 1
iteration= 21	 chi2= 27170.592543	 time= 0.243497	 cumTime= 4.71338	 edges= 83718	 schur= 1	 lambda= 0.011118	 levenbergIter= 2
iteration= 22	 chi2= 26748.191194	 time= 0.243523	 cumTime= 4.9569	 edges= 83718	 schur= 1	 lambda= 0.014824	 levenbergIter= 2
iteration= 23	 chi2= 26675.118188	 time= 0.198495	 cumTime= 5.1554	 edges= 83718	 schur= 1	 lambda= 0.009883	 levenbergIter= 1
iteration= 24	 chi2= 26087.985781	 time= 0.242708	 cumTime= 5.3981	 edges= 83718	 schur= 1	 lambda= 0.010281	 levenbergIter= 2
iteration= 25	 chi2= 25875.818536	 time= 0.24327	 cumTime= 5.64137	 edges= 83718	 schur= 1	 lambda= 0.013708	 levenbergIter= 2
iteration= 26	 chi2= 25831.564925	 time= 0.197571	 cumTime= 5.83894	 edges= 83718	 schur= 1	 lambda= 0.009139	 levenbergIter= 1
iteration= 27	 chi2= 25568.344873	 time= 0.243723	 cumTime= 6.08267	 edges= 83718	 schur= 1	 lambda= 0.011118	 levenbergIter= 2
iteration= 28	 chi2= 25455.865005	 time= 0.243181	 cumTime= 6.32585	 edges= 83718	 schur= 1	 lambda= 0.011781	 levenbergIter= 2
iteration= 29	 chi2= 25454.942053	 time= 0.197301	 cumTime= 6.52315	 edges= 83718	 schur= 1	 lambda= 0.007854	 levenbergIter= 1
iteration= 30	 chi2= 25260.709796	 time= 0.244192	 cumTime= 6.76734	 edges= 83718	 schur= 1	 lambda= 0.009148	 levenbergIter= 2
iteration= 31	 chi2= 25171.392636	 time= 0.243542	 cumTime= 7.01089	 edges= 83718	 schur= 1	 lambda= 0.009425	 levenbergIter= 2
iteration= 32	 chi2= 25104.160294	 time= 0.243352	 cumTime= 7.25424	 edges= 83718	 schur= 1	 lambda= 0.008637	 levenbergIter= 2
iteration= 33	 chi2= 25042.986799	 time= 0.242833	 cumTime= 7.49707	 edges= 83718	 schur= 1	 lambda= 0.008765	 levenbergIter= 2
iteration= 34	 chi2= 24984.677998	 time= 0.243707	 cumTime= 7.74078	 edges= 83718	 schur= 1	 lambda= 0.005949	 levenbergIter= 2
iteration= 35	 chi2= 24943.879912	 time= 0.244234	 cumTime= 7.98501	 edges= 83718	 schur= 1	 lambda= 0.007933	 levenbergIter= 2
iteration= 36	 chi2= 24886.075504	 time= 0.243852	 cumTime= 8.22886	 edges= 83718	 schur= 1	 lambda= 0.005674	 levenbergIter= 2
iteration= 37	 chi2= 24868.088225	 time= 0.242916	 cumTime= 8.47178	 edges= 83718	 schur= 1	 lambda= 0.007565	 levenbergIter= 2
iteration= 38	 chi2= 24833.053138	 time= 0.244644	 cumTime= 8.71642	 edges= 83718	 schur= 1	 lambda= 0.008448	 levenbergIter= 2
iteration= 39	 chi2= 24815.047826	 time= 0.245233	 cumTime= 8.96166	 edges= 83718	 schur= 1	 lambda= 0.009766	 levenbergIter= 2

进程已结束,退出代码0

• initial.ply
  
• final.ply