SLAM十四讲ch9代码注释

int main(int argc, char *argv[])的使用

argc是命令行参数的数目。
argv是字符串数组，其元素分别是各个命令行参数。

例：

• 题目中假如编译后生成的exe文件名为CaclFabonacci，在Dos提示符下输入

• CaclFabonacci 15

则argc = 2，argv[0] = "CaclFabonacci"，argv[1] = "15"

common.h

创建了一个BALProblem类，来管理读取到的数据，头文件便于查找一些指针

/// 从文件读入BAL dataset
class BALProblem {
public:
    /// 加载文件数据
    explicit BALProblem(const std::string &filename, bool use_quaternions = false);//explicit是防止类构造函数的隐式自动转换
 
    ~BALProblem() {
        delete[] point_index_;
        delete[] camera_index_;
        delete[] observations_;
        delete[] parameters_;
    }
 
    /// 存储数据到.txt
    void WriteToFile(const std::string &filename) const;
    /// 存储点云文件
    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; }
    /// 特征点参数数目：3
    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_; }
    /// 特征点参数首地址=相机位姿参数首地址+相机位姿参数维度（9 or 10）*相机个数
    const double *points() const { return parameters_ + camera_block_size() * num_cameras_; }
    /// mutable 易变的 （相机位姿参数或特征点首地址）
    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();
    }//第i个相机参数首地址
 
    double *mutable_point_for_observation(int i) {
        return mutable_points() + point_index_[i] * point_block_size();
    }//第i个特征点首地址
 
    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_;
};

random.h

得到服从标准正态分布的样本

#ifndef RAND_H
#define RAND_H

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

inline double RandDouble()
{
    double r = static_cast<double>(rand());//类型转换;rand()随机数发生器，int rand(void),头文件stdil.h，随机数在0～RAND_MAX之间
    return r / RAND_MAX;//返回0～1之间的数
}
/*
Marsaglia polar method算法：  得到满足N（0,1）正态分布的随机点
  x和y是圆： x*x+y*y <=1内的随机点
  RandNormal()处理后  x*w是服从标准正态分布的样本
*/
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 : 
/*
角轴[a0,a1,a2]变四元数[q0,q1,q2,q3]：
[q1,q2,q3]=[a0,a1,a2]*sin(theta/2)/theta 其中[a0,a1,a2]/theta=[n0,n1,n2] 单位长度向量n 
这里theta=sqrt(a0*a0+a1*a1+a2*a2) 角轴模长为旋转角度

定义sin(theta/2)/theta为k
当角不为0的时候，如常计算k. q0=cos（theta/2）

当角趋近于0的时候,k趋近于0.5.q0趋近于1
*/
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;
    /*四元数[q0,q1,q2,q3]变角轴[a0,a1,a2]:
    [a0,a1,a2]=[q1,q2,q3]*theta/sin(theta/2) 
    [a0,a1,a2]/theta=[n0,n1,n2]->对上式取模->得到sin(theta/2),为sqrt(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);//sin(theta/2)
        const T &cos_theta = quaternion[0];//cos_theta指针指向q0，若q0=cos(theta/2)<0,则theta/2>pi/2,theta>pi
        
        const T two_theta = T(2.0) * ((cos_theta < 0.0)
                                      ? atan2(-sin_theta, -cos_theta)
                                      : atan2(sin_theta, cos_theta));//2*(theta/2)
    //atan2(y,x)函数:计算(x,y)与原点连线与x轴正方向夹角，返回值为弧度，在几何意义上，atan2(y,x)=atan2(y/x),y>0时，夹角为逆时针，y<0时，夹角为顺时针得到；值域为（-pi,pi]
        const T k = two_theta / sin_theta; //定义theta/sin(theta/2)为k 

        angle_axis[0] = q1 * k;//a[0]/theta=q1/sin(theta/2)
        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);//theta=0,k取2
        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);//向量点乘，结果即||a||2,而||a||=theta
    //依旧是根据theta是否为0来讨论
    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;

        //角轴单位向量n
        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 = pt . costheta +
        //            (w x pt) * sintheta +
        //            w (w . pt) (1 - 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

 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;//map(指向数据的指针，构造矩阵的行数和列数),map相当于引用普通的c++数组，进行矩阵操作，而不用copy数据
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);//int fscanf(文件指针,格式字符串,输入列表),返回值为读取的数据个数；fscanf函数，完成读取后文件指针自动向下移
    if (num_scanned != 1)
        std::cerr << "Invalid UW data file. ";//由下方调用可知，读的是一个double或long float数据，所以返回值是1;读取失败则返回-1，数据输入不匹配则返回0
}

//给一个三维向量加入噪声，后面的Perturb()函数对路标点，相机的旋转，相机的平移分别加入噪声
void PerturbPoint3(const double sigma, double *point) {
    for (int i = 0; i < 3; ++i)
        point[i] += RandNormal() * sigma;//RandNormal()生成标准正态分布样本，x*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());
    //nth_element(first, nth, last, compare),求[first, last]这个区间中第n大小的元素，如果参数加入了compare函数，就按compare函数的方式比较。array[first, last)元素区间，排序后，array[nth]就是第n大的元素（从0开始),但是[first, nth) 和 [nth,last)区间的大小顺序不一定。
    return *mid_point;
}

//BALProblem功能：读取txt信息,并将其中的相机旋转信息由角轴->四元数
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_);//txt第一行信息：相机个数，观测点个数，观测次数，依次读取输出
    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_];//二维坐标（x,y）

    num_parameters_ = 9 * num_cameras_ + 3 * num_points_;//估计参数维度：相机9，空间坐标3
    parameters_ = new double[num_parameters_];

    //从第2行开始，到83719行结束，信息是：相机序号，观测点序号，x坐标，y坐标
    //共观测了22106个点，记录每个点在不同（可以看到该点的）相机下的坐标(x,y)
    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);
        }
    }

    //83720开始，读取相机参数16*9，空间坐标22106*3，共66462个数据
    for (int i = 0; i < num_parameters_; ++i) {
        FscanfOrDie(fptr, "%lf", parameters_ + i);
    }

    fclose(fptr);

    use_quaternions_ = use_quaternions;//标识符，值为1，使用四元数
    if (use_quaternions) {
        // Switch the angle-axis rotations to quaternions.
        num_parameters_ = 10 * num_cameras_ + 3 * num_points_;//4+3+3，3
        double *quaternion_parameters = new double[num_parameters_];//参数数组10，3
        double *original_cursor = parameters_;//原先的参数数组9，3，将读取的参数赋值
        double *quaternion_cursor = quaternion_parameters;
        for (int i = 0; i < num_cameras_; ++i) {
            AngleAxisToQuaternion(original_cursor, quaternion_cursor);//相机参数前3维(旋转)，角轴->四元数，指针访问
            quaternion_cursor += 4;
            original_cursor += 3;
            for (int j = 4; j < 10; ++j) {
                *quaternion_cursor++ = *original_cursor++;//读取剩余6个相机参数(平移3，焦距1，畸变系数2)
            }
        }
        // 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;//更新
    }
}

//WriteToFiles功能：将已知数据写入文件，其中包括四元数->角轴转换，和上面相反
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_);
    
    //把上面txt观测数据重写一遍
    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));//将剩余6个参数拷贝到角轴数组中
            //原型：extern void *memcpy(void *dest, void *src, unsigned int count);由src所指内存区域复制count个字节到dest所指内存区域,函数返回指向dest的指针,void *表示未定义类型指针
        } 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
//将优化后的数据写入新建的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();
}

//C to AC功能：已知camera，取出相机旋转，相机中心信息
void BALProblem::CameraToAngelAxisAndCenter(const double *camera,
                                            double *angle_axis,
                                            double *center) const {
    VectorRef angle_axis_ref(angle_axis, 3);//VectorRef自定义Map类型,参数：指向数据的指针，矩阵行数或列数
    if (use_quaternions_) {
        QuaternionToAngleAxis(camera, angle_axis);
    } else {
        angle_axis_ref = ConstVectorRef(camera, 3);
    }

    // c = -R't
    //对平移量t进行旋转
    /*如何计算相机中心center的世界坐标
    PW_center:世界坐标系下的相机坐标
    PC_center:相机坐标系下的相机原点坐标（0,0,0）
    根据相机坐标系与世界坐标系的转换关系：PW_center×R+t=PC_center
    PW_center= -R't */
    Eigen::VectorXd inverse_rotation = -angle_axis_ref;//角轴添加负号代表旋转方向相反,即为逆
    AngleAxisRotatePoint(inverse_rotation.data(),
                         camera + camera_block_size() - 6,
                         center);
    //camera：旋转3/4维，平移3维，内参3维，需要t，所以大小-6
    //camara和camara_block_size在common.h，参数parameter起始位，10或9
    VectorRef(center, 3) *= -1.0;
}

//与上面方向相反 已知相机旋转，相机中心空间坐标，获得camera参数
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;
    }

    // 0=R*Cw+t ；t = -R * c 获得平移参数
    AngleAxisRotatePoint(angle_axis, center, camera + camera_block_size() -6);
    VectorRef(camera + camera_block_size() - 6, 3) *= -1.0;
}

//Normalize()功能：对points,camera中的center进行处理；
//对原始数据进行归一化，将所有路标点的中心置零，然后做一个合适尺度的缩放。这会使优化过程中数值更稳定，防止在极端情况下处理很大或者有很大偏移的BA问题

void BALProblem::Normalize() {
    // Compute the marginal median of the geometry
    std::vector<double> tmp(num_points_);
    Eigen::Vector3d median;
    double *points = mutable_points();//返回parameters_中的空间点信息起始位置
    for (int i = 0; i < 3; ++i) {
        for (int j = 0; j < num_points_; ++j) {
            tmp[j] = points[3 * j + i];//所有点的X，Y，Z
        }
        median(i) = Median(&tmp);//计算X/Y/Z中位数
    }

    for (int i = 0; i < num_points_; ++i) {
        VectorRef point(points + 3 * i, 3);
        tmp[i] = (point - median).lpNorm<1>();//空间点和均值之差，用1范数(绝对值之和)
    }

    const double median_absolute_deviation = Median(&tmp);//异常值检测——MAD（Median absolute deviation, 中位数绝对偏差）是单变量数据集中样本差异性的稳健度量。mad是一个健壮的统计量，对于数据集中异常值的处理比标准差更具有弹性，可以大大减少异常值对于数据集的影响。

    // 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);//n个point->tmp为n维，每维point-median(3维偏差)的1范数->MAD:tmp(n维)的中位数值，感觉像是points在新的坐标系的坐标point
    }

    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);//更新center
        AngleAxisAndCenterToCamera(angle_axis, center, camera);
    }
}

//Perturb()功能：对空间点，相机旋转和平移加入噪声
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);
    }
}

 g2o

CMakeLists.txt

cmake_minimum_required(VERSION 3.12)

project(bundle_adjustment_g2o)
set(CMAKE_BUILD_TYPE "Release")
set(CMAKE_CXX_FLAGS "-std=c++14 -O3")

list(APPEND CMAKE_MODULE_PATH /home/mia/g2o1/cmake_modules)

find_package(fmt REQUIRED)
find_package(G2O REQUIRED)
find_package(Sophus REQUIRED)
find_package(CSparse REQUIRED)
#find_package(Eigen3 REQUIRED)

set(FMT_LIBRARIES fmt::fmt)#这里好像和Sophus有关，要记得用fmt，要不然找不到fmt下的函数
set(G2O_LIBS cxsparse g2o_csparse_extension)#单独添加stuff core添加不上，在最后直接加的

include_directories("/usr/include/eigen3")
include_directories(${PROJECT_SOURCE_DIR} ${CSPARSE_INCLUDE_DIR} ${G2O_INCLUDE_DIRS})

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

target_link_libraries(bundle_adjustment_g2o ${FMT_LIBRARIES} bal_common ${G2O_STUFF_LIBRARY} ${G2O_CORE_LIBRARY} ${G2O_LIBS})