视觉SLAM十四讲作业练习（7）BA

BA后端优化

第七次作业的第2题BAL-dataset与书中所给示例代码类似，于是就学习了一下，让我写肯定写不出来。。。
整体思路如下：
文件结构：

.
├── bundle_adjustment_ceres.cpp
├── bundle_adjustment_g2o.cpp
├── cmake
│   ├── CeresConfig.cmake.in
│   ├── FindBLAS.cmake
│   ├── FindCSparse.cmake
│   ├── FindEigen3.cmake
│   ├── FindG2O.cmake
│   └── FindLAPACK.cmake
├── CMakeLists.txt
├── common.cpp
├── common.h
├── problem-16-22106-pre.txt
├── random.h
├── rotation.h
└── SnavelyReprojectionError.h

1.通用部分

（1）读取数据

1）了解读入文件的格式

//第一部分 header
<相机数量> <3D点数量> <观测2D点数量>
//第二部分 某个相机观测某个点得到像素点坐标
<相机索引_1> <3D点索引_1> <观测2D点_x_1> <观测2D点_y_1>
...
<相机索引_1> <3D点索引_1> <观测2D点_x_1> <观测2D点_y_1>
//第三部分相机参数(9) 点参数(3)
<相机参数_1--1>
<相机参数_1--2>
...
<相机参数_1--8>
<相机参数_1--9>
...
...
<相机参数_n--1>
<相机参数_n--2>
...
<相机参数_n--8>
<相机参数_n--9>
<3D点_1_x>
<3D点_1_y>
<3D点_1_z>
...
<3D点_m_x>
<3D点_m_y>
<3D点_m_z>

2）fscanf()函数

fscanf()函数的头文件是，函数原型 为 int fscanf(FILE *stream, const char *format, [argument…]);

其功能为根据数据格式(format)从输入流(stream)中写入数据(argument)；

【参数】stream为文件指针，format为格式化字符串，argument 为格式化控制符对应的参数。

从文件指针stream指向的文件中，按format中对应的控制格式读取数据，并存储在agars对应的变量中

void FscanfOrDie(FILE *fptr, const char *format, T *value) {
    //从stream流中连续读取能匹配format格式的字符串到参数列表中对应的变量里
    int num_scanned = fscanf(fptr, format, value);
    if (num_scanned != 1)
        std::cerr << "Invalid UW data file. ";
}

3）构造函数

​ 1读出文件第一行相机数，点的个数以及观测个数

​ 2读出哪一个相机[0,15]观测哪一个点[0,22105]得到观测的像素坐标值[x,y]

​ 总观测数：83718

​ 3参数依次读取

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

（2）处理数据

1）Normalize归一化

$$X_{new}=\frac{X_i-meanX}{X_{max}-X_{min}}$$

​ 1求中位数

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

2更新tmp

​ 3绝对标准差 = 个别测定值 - 多次测定平均值之差

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

​ 4对于相机的归一化，需要将文件中的相机有关数据提取出轴角，以及相机中心点在世界坐标系下的坐标
$$P_w=R{P}_c+t$$
其中 $P_c=(0,0,0)$，所以
$$P_c=-R^{-1}t$$
R为旋转矩阵，若想通过轴角得到R，需使用罗德里格斯公式：
$$R=cos\theta I+(1-cos\theta )n{n}^T+sin\theta n^{\Lambda }$$
所以
$$P_c=-cos\theta It+(1-cos\theta )n(n\cdot t)+sin\theta n\times t$$

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(),       //data()函数返回一个指向矩阵或向量的首地址的指针
                         camera + camera_block_size() - 6,
                         center);
    VectorRef(center, 3) *= -1.0;
}

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

2）Purturb添加噪声

​ 1产生随机数

#ifndef RAND_H
#define RAND_H

#include 
#include 

inline double RandDouble()
{
    double r = static_cast<double>(rand());     //sttatic_cast转换运算符
    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

​ 2构建噪声函数

void PerturbPoint3(const double sigma, double *point) {     //添加方差为sigma的高斯噪声
    for (int i = 0; i < 3; ++i)
        point[i] += RandNormal() * sigma;
}

​ 3给相机旋转，平移以及点的坐标添加噪声

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）输出数据

1）写入普通文件

​ 1fprinft()函数

​ 2memcpy()函数 void *memcpy(void *str1, const void *str2, size_t n)

​ 从存储区 str2 复制 n 个字节到存储区 str1

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\n", 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);
}

（2）写入ply文件

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

2.ceres_BA

（1）构建代价函数重载（）运算符

（2）构建寻优问题

（3）配置并运行求解器

#ifndef SnavelyReprojection_H
#define SnavelyReprojection_H

#include 
#include "ceres/ceres.h"
#include "rotation.h"
//定义误差类，作用为在类中重载()运算符，写出误差的计算方法
class SnavelyReprojectionError {
public:
    //1构建代价函数重载（）运算符
    //构造函数，将观测坐标读进来
    SnavelyReprojectionError(double observation_x, double observation_y) : observed_x(observation_x),
                                                                           observed_y(observation_y) {}

    //此处定义了一个模板()函数，定义误差计算方法，重投影误差，预测值减去观测值的误差
    template<typename T>
    bool operator()(const T *const camera,
                    const T *const point,
                    T *residuals) const {
        // camera[0,1,2] are the angle-axis rotation
        T predictions[2];
        CamProjectionWithDistortion(camera, point, predictions);
        // 计算误差，实质上就是重投影误差模型
        residuals[0] = predictions[0] - T(observed_x);
        residuals[1] = predictions[1] - T(observed_y);

        return true;
    }

    // camera : 9 dims array
    // [0-2] : angle-axis rotation
    // [3-5] : translateion
    // [6-8] : camera parameter, [6] focal length, [7-8] second and forth order radial distortion
    // point : 3D location.
    // predictions : 2D predictions with center of the image plane.
    template<typename T>
    static inline bool CamProjectionWithDistortion(const T *camera, const T *point, T *predictions) {
        // Rodrigues' formula
        T p[3];
        AngleAxisRotatePoint(camera, point, p);
        // camera[3,4,5] are the translation
        p[0] += camera[3];
        p[1] += camera[4];
        p[2] += camera[5];

        // Compute the center fo distortion
        // 归一化平面，去除深度
        T xp = -p[0] / p[2];
        T yp = -p[1] / p[2];

        // Apply second and fourth order radial distortion
        const T &l1 = camera[7];
        const T &l2 = camera[8];

        // 径向畸变
        T r2 = xp * xp + yp * yp;
        T distortion = T(1.0) + r2 * (l1 + l2 * r2);

        // 投影得到像素坐标
        const T &focal = camera[6];
        predictions[0] = focal * distortion * xp;
        predictions[1] = focal * distortion * yp;

        return true;
    }
	//2构建寻优问题
    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

void SolveBA(BALProblem &bal_problem) {
    //读出路标和相机的维度
    const int point_block_size = bal_problem.point_block_size();
    const int camera_block_size = bal_problem.camera_block_size();
    //读出路标和相机数据的首位置
    double *points = bal_problem.mutable_points();
    double *cameras = bal_problem.mutable_cameras();
    
    // Observations is 2 * num_observations long array observations
    // [u_1, u_2, ... u_n], where each u_i is two dimensional, the x
    // and y position of the observation.
    const double *observations = bal_problem.observations();
    ceres::Problem problem;

    for (int i = 0; i < bal_problem.num_observations(); ++i) {
        ceres::CostFunction *cost_function;

        // Each Residual block takes a point and a camera as input
        // and outputs a 2 dimensional Residual
        cost_function = SnavelyReprojectionError::Create(observations[2 * i + 0], observations[2 * i + 1]);

        // If enabled use Huber's loss function.
        //定义problem->AddResidualBlock()函数中需要的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.
        //定义problem->AddResidualBlock()函数中需要的待估计参数
        double *camera = cameras + camera_block_size * bal_problem.camera_index()[i];
        double *point = points + point_block_size * bal_problem.point_index()[i];

        problem.AddResidualBlock(cost_function, loss_function, camera, point);
    }

    // show some information here ...
    std::cout << "bal problem file loaded..." << std::endl;
    std::cout << "bal problem have " << bal_problem.num_cameras() << " cameras and "
              << bal_problem.num_points() << " points. " << std::endl;
    std::cout << "Forming " << bal_problem.num_observations() << " observations. " << std::endl;

    std::cout << "Solving ceres BA ... " << endl;
    //设定求解办法，SPARSE_SCHUR会让ceres实时求解过程为：先对路标部分进行Schur边缘化，以加速的方式求解此问题。
    ceres::Solver::Options options;
    options.linear_solver_type = ceres::LinearSolverType::SPARSE_SCHUR;
    options.minimizer_progress_to_stdout = true;
    ceres::Solver::Summary summary;
    ceres::Solve(options, &problem, &summary);
    std::cout << summary.FullReport() << "\n";
}

3.g2o_BA

（1）构建相机参数的结构

/// 姿态和内参的结构 
struct PoseAndIntrinsics {      //包含 R T f k1 k2
    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) {
        Vector3d 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;
};

（2）定义顶点

因为顶点分为相机与观测点，所以需要两个顶点类。

/// 位姿加相机内参的顶点，9维，前三维为so3，接下去为t, f, k1, k2
class VertexPoseAndIntrinsics : public g2o::BaseVertex<9, PoseAndIntrinsics> {    //优化变量维度  数据类型
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;

    VertexPoseAndIntrinsics() {}

    virtual void setToOriginImpl() override {                                      //顶点初始化
        _estimate = PoseAndIntrinsics();
    }

    virtual void oplusImpl(const double *update) override {                      //顶点估计值更新
        _estimate.rotation = SO3d::exp(Vector3d(update[0], update[1], update[2])) * _estimate.rotation;
        _estimate.translation += Vector3d(update[3], update[4], update[5]);
        _estimate.focal += update[6];
        _estimate.k1 += update[7];
        _estimate.k2 += update[8];
    }

    /// 根据估计值投影一个点
    Vector2d project(const Vector3d &point) {
        Vector3d pc = _estimate.rotation * point + _estimate.translation;
        pc = -pc / pc[2];                                               //归一化平面  BAL数据集说明  
        double r2 = pc.squaredNorm();
        //去畸变 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) {return false;}

    virtual bool write(ostream &out) const {return false;}
};

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) {return false;}

    virtual bool write(ostream &out) const {return false;}
};

（3）定义边

class EdgeProjection :
	//<>内：2：误差项维数，Vector2d：输入数据的变量形式，与边相连的两点
    public g2o::BaseBinaryEdge<2, Vector2d, VertexPoseAndIntrinsics, VertexPoint> {
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;

    virtual void computeError() override {
        VertexPoseAndIntrinsics *pose = static_cast<VertexPoseAndIntrinsics*> (_vertices[0]);
        VertexPoint *point = static_cast<VertexPoint*> (_vertices[1]);
        Vector2d proj = pose->project(point->estimate());
        _error = proj - _measurement;                               //误差项
    }
    // 没有实现linearizeOplus（） 的重写时，   g2o会自动提供一个数值计算的雅克比

    // use numeric derivatives
    virtual bool read(istream &in) {}

    virtual bool write(ostream &out) const {}

};

（4）构建图优化

​ 1）实例化optimizer优化器，打开调试输出

// pose dimension 9, landmark is 3     T 6维 + point 3维  ，观测点 point  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);                                                         //打开调试输出

​ 2）实例化顶点

	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;
        PoseAndIntrinsics estimate (camera);
        v->setId(i);
        v->setEstimate(estimate);                                    					//设置初始估计值
        optimizer.addVertex(v);                                                      	//增加pose顶点
        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;
        Eigen::Vector3d estimate (point[0], point[1], point[2]);
        v->setId(i + bal_problem.num_cameras());
        v->setEstimate(estimate);
        // g2o在BA中需要手动设置待Marg的顶点
        v->setMarginalized(true);                                                  //手动设置需要边缘化的顶点
        optimizer.addVertex(v);                                                    //增加point顶点
        vertex_points.push_back(v);
    }

​ 3）实例化边

	// edge
    for (int i = 0; i < bal_problem.num_observations(); ++i) {                    //g2o的边是误差
        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);
    }

​ 4）设置优化器参数，进行优化

    optimizer.initializeOptimization();
    optimizer.optimize(40);

​ 5）存储优化后的值

    // set to bal problem    将优化后的值 储存到bal_problem
    for (int i = 0; i < bal_problem.num_cameras(); ++i) {
        double *camera = cameras + camera_block_size * i;
        auto vertex = vertex_pose_intrinsics[i];
        auto estimate = vertex->estimate();
        estimate.set_to(camera);
    }
    for (int i = 0; i < bal_problem.num_points(); ++i) {
        double *point = points + point_block_size * i;
        auto vertex = vertex_points[i];
        for (int k = 0; k < 3; ++k)
            point[k] = vertex->estimate()[k];
    }