【SLAM十四讲-9.3 实践Ceres BA-BAL数据集problem-16-22106-pre.txt分析】

数据集Dubrovnik Dataset 杜布罗夫尼克数据集的链接：Bundle Adjustment in the Large https://grail.cs.washington.edu/projects/bal/

cpp 复制代码

problem-16-22106-pre.txt.bz2

cpp 复制代码

16 22106 83718（这里是第1行）
0 0     -3.859900e+02 3.871200e+02（这里是第2行）
1 0     -3.844000e+01 4.921200e+02
2 0     -6.679200e+02 1.231100e+02
7 0     -5.991800e+02 4.079300e+02
12 0     -7.204300e+02 3.143400e+02
13 0     -1.151300e+02 5.548999e+01
0 1     3.838800e+02 -1.529999e+01
1 1     5.597500e+02 -1.061500e+02
10 1     3.531899e+02 1.649500e+02
0 2     5.915500e+02 1.364400e+02
1 2     8.638600e+02 -2.346997e+01
2 2     4.947200e+02 1.125200e+02
6 2     4.087800e+02 2.846700e+02
7 2     4.246100e+02 3.101700e+02
9 2     2.848900e+02 1.928900e+02
省略省略省略省略省略省略省略省略
省略省略省略省略省略省略省略省略
省略省略省略省略省略省略省略省略
14 22101     8.408002e+01 -4.590699e+02
15 22101     -3.668100e+02 5.134200e+02
14 22102     6.578003e+01 -4.800800e+02
15 22102     -3.920800e+02 4.843700e+02
14 22103     1.808000e+02 -5.225200e+02
15 22103     -2.593700e+02 4.393600e+02
14 22104     8.235999e+01 -5.621600e+02
15 22104     -3.864900e+02 3.803600e+02
14 22105     2.212600e+02 -4.689200e+02
15 22105     -1.963200e+02 5.126800e+02 （这里是第83719行，总共83718个观测数据）
-1.6943983532198115e-02（这里是第83720行）
1.1171804676513932e-02
2.4643508831711991e-03
7.3030995682610689e-01
-2.6490818471043420e-01
-1.7127892627337182e+00
1.4300319432711681e+03
-7.5572758535864072e-08
3.2377569465570913e-14
1.5049725341485708e-02
-1.8504564785154357e-01
-2.9278402790141456e-01
-1.0590476152349551e+00
-3.6017862414345798e-02
省略省略省略省略省略省略省略省略
省略省略省略省略省略省略省略省略
省略省略省略省略省略省略省略省略
-4.6810959170777309e+01
-5.1658843840370494e+00
8.7190890000602259e+00
-4.3856247105773697e+01
-7.9343522447974468e+00
7.3540260405955173e+00
-4.2005555023142122e+01
-3.7719680164031812e+00
1.1022501903417394e+01
-4.7349911639646528e+01（这是第150181行，总共16个相机 * 9个参数 + 22106个路标点 * 3个参数 = 144 +66318 = 66462个参数）

Data Format 数据格式

每个问题都以 bzip2 压缩文本文件的形式提供，格式如下。

cpp 复制代码

<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 指定为罗德里格斯矢量。
problem-16-22106-pre.txt 数据格式说明

cpp 复制代码

<num_cameras> <num_points> <num_observations>
16 22106 83718      - `<num_cameras>`：表示相机的数量，这里为16。
                    - `<num_points>`：表示路标点（三维点）的数量，这里为22106。
                    - `<num_observations>`：表示观测的数量，指的是所有相机观测到的路标点的数量，这里为83718。 

                    这些数据指示了整个问题的规模和数据集中的相机、路标点以及相机观测的总数目。                        
                                                      
         
<camera_index_1> <point_index_1> <x_1> <y_1>
0 0     -3.859900e+02 3.871200e+02          共83718行观测：每一行包含一个观测数据点，其中包括相机索引 0-15 、路标点索引 0-22105 以及对应的 x 和 y 值。                  
1 0     -3.844000e+01 4.921200e+02          共83718行观测：每一行包含一个观测数据点，其中包括相机索引 0-15 、路标点索引 0-22105 以及对应的 x 和 y 值。         
2 0     -6.679200e+02 1.231100e+02          共83718行观测：每一行包含一个观测数据点，其中包括相机索引 0-15 、路标点索引 0-22105 以及对应的 x 和 y 值。         
7 0     -5.991800e+02 4.079300e+02          共83718行观测：每一行包含一个观测数据点，其中包括相机索引 0-15 、路标点索引 0-22105 以及对应的 x 和 y 值。         
12 0     -7.204300e+02 3.143400e+02         共83718行观测：每一行包含一个观测数据点，其中包括相机索引 0-15 、路标点索引 0-22105 以及对应的 x 和 y 值。           
13 0     -1.151300e+02 5.548999e+01         共83718行观测：每一行包含一个观测数据点，其中包括相机索引 0-15 、路标点索引 0-22105 以及对应的 x 和 y 值。                
0 1     3.838800e+02 -1.529999e+01          共83718行观测：每一行包含一个观测数据点，其中包括相机索引 0-15 、路标点索引 0-22105 以及对应的 x 和 y 值。 
1 1     5.597500e+02 -1.061500e+02          共83718行观测：每一行包含一个观测数据点，其中包括相机索引 0-15 、路标点索引 0-22105 以及对应的 x 和 y 值。
10 1     3.531899e+02 1.649500e+02          共83718行观测：每一行包含一个观测数据点，其中包括相机索引 0-15 、路标点索引 0-22105 以及对应的 x 和 y 值。
.....................................       共83718行观测：每一行包含一个观测数据点，其中包括相机索引 0-15 、路标点索引 0-22105 以及对应的 x 和 y 值。    
14 22102     6.578003e+01 -4.800800e+02     共83718行观测：每一行包含一个观测数据点，其中包括相机索引 0-15 、路标点索引 0-22105 以及对应的 x 和 y 值。                   
15 22102     -3.920800e+02 4.843700e+02     共83718行观测：每一行包含一个观测数据点，其中包括相机索引 0-15 、路标点索引 0-22105 以及对应的 x 和 y 值。                   
14 22103     1.808000e+02 -5.225200e+02     共83718行观测：每一行包含一个观测数据点，其中包括相机索引 0-15 、路标点索引 0-22105 以及对应的 x 和 y 值。                   
15 22103     -2.593700e+02 4.393600e+02     共83718行观测：每一行包含一个观测数据点，其中包括相机索引 0-15 、路标点索引 0-22105 以及对应的 x 和 y 值。                   
14 22104     8.235999e+01 -5.621600e+02     共83718行观测：每一行包含一个观测数据点，其中包括相机索引 0-15 、路标点索引 0-22105 以及对应的 x 和 y 值。                   
15 22104     -3.864900e+02 3.803600e+02     共83718行观测：每一行包含一个观测数据点，其中包括相机索引 0-15 、路标点索引 0-22105 以及对应的 x 和 y 值。                   
14 22105     2.212600e+02 -4.689200e+02     共83718行观测：每一行包含一个观测数据点，其中包括相机索引 0-15 、路标点索引 0-22105 以及对应的 x 和 y 值。                   
15 22105     -1.963200e+02 5.126800e+02     共83718行观测：每一行包含一个观测数据点，其中包括相机索引 0-15 、路标点索引 0-22105 以及对应的 x 和 y 值。                   
<camera_index_num_observations> <point_index_num_observations> <x_num_observations> <y_num_observations>


<camera_1>
-1.6943983532198115e-02                 这部分数据表示16个相机 * 每个相机的9个参数 = 144个参数                        
1.1171804676513932e-02                  这部分数据表示16个相机 * 每个相机的9个参数 = 144个参数                        
2.4643508831711991e-03                  这部分数据表示16个相机 * 每个相机的9个参数 = 144个参数                        
7.3030995682610689e-01                  这部分数据表示16个相机 * 每个相机的9个参数 = 144个参数                        
-2.6490818471043420e-01                 这部分数据表示16个相机 * 每个相机的9个参数 = 144个参数                        
-1.7127892627337182e+00                 这部分数据表示16个相机 * 每个相机的9个参数 = 144个参数                        
1.4300319432711681e+03                  这部分数据表示16个相机 * 每个相机的9个参数 = 144个参数                        
-7.5572758535864072e-08                 这部分数据表示16个相机 * 每个相机的9个参数 = 144个参数                        
3.2377569465570913e-14                  这部分数据表示16个相机 * 每个相机的9个参数 = 144个参数                        
1.5049725341485708e-02                  这部分数据表示16个相机 * 每个相机的9个参数 = 144个参数                        
-1.8504564785154357e-01                 这部分数据表示16个相机 * 每个相机的9个参数 = 144个参数                        
.....................                   这部分数据表示16个相机 * 每个相机的9个参数 = 144个参数                        
<camera_num_cameras>                    这部分数据表示16个相机 * 每个相机的9个参数 = 144个参数                       
 
 


<point_= 144个参数1>
.....................
8.7190890000602259e+00                这部分数据表示22106个路标点 * 每个路标点的3个参数 = 66318个参数  
-4.3856247105773697e+01               这部分数据表示22106个路标点 * 每个路标点的3个参数 = 66318个参数   
-7.9343522447974468e+00               这部分数据表示22106个路标点 * 每个路标点的3个参数 = 66318个参数   
7.3540260405955173e+00                这部分数据表示22106个路标点 * 每个路标点的3个参数 = 66318个参数  
-4.2005555023142122e+01               这部分数据表示22106个路标点 * 每个路标点的3个参数 = 66318个参数   
-3.7719680164031812e+00               这部分数据表示22106个路标点 * 每个路标点的3个参数 = 66318个参数   
1.1022501903417394e+01                这部分数据表示22106个路标点 * 每个路标点的3个参数 = 66318个参数  
-4.7349911639646528e+01               这部分数据表示22106个路标点 * 每个路标点的3个参数 = 66318个参数   
<point_num_points>


文件总行数：150181 = 整个问题的规模和数据集中的相机、路标点以及相机观测的总数目 1行
                  + 观测数据 83718行 
                  + 16个相机 * 每个相机的9个参数   144行
                  + 22106个路标点 * 每个路标点的3个参数  66318行

在文本文件 `problem-16-22106-pre.txt` 中，`<camera_1>` 到 `<camera_num_cameras>` 和 `<point_1>` 到 `<point_num_points>` 部分表示相机和路标点的参数信息。
在这里，`<camera_1>` 到 `<camera_num_cameras>` 部分包含了相机的参数数据，而 `<point_1>` 到 `<point_num_points>` 部分包含了路标点的参数数据。
这些部分可能包含相机和路标点的内部参数，例如内参矩阵、畸变系数（如果有的话），或者其他相机和路标点相关的参数。这取决于问题的具体设置和使用的算法，这些参数通常是用来描述相机或者路标点在优化过程中的特性和约束。
通常情况下，这些参数会以一定的格式保存在文本文件中，并在算法中被读取和处理。需要根据问题的具体背景和文档中的说明来解释这些参数的含义。

在这个上下文中，每个相机的9个参数代表了相机的内部参数和外部参数。具体而言，这9个参数通常指的是：
1. **R (Rotation)**：旋转矩阵或旋转向量。在这种情况下，描述相机的旋转通常使用 Rodrigues' vector（罗德里格斯向量）来表示。Rodrigues' vector 是一个三维向量，用来表示旋转轴乘以旋转角度的结果。
2. **t (Translation)**：平移向量。它表示相机在世界坐标系中的位置。
3. **f (Focal Length)**：焦距，即相机的焦距参数。
4. **k1, k2 (Distortion Parameters)**：径向畸变参数，通常使用k1和k2来表示畸变的程度。这些参数用于校正由于相机镜头特性而引起的径向畸变。

这些参数的组合构成了相机的内部参数（焦距、畸变）以及外部参数（旋转、平移），并且在相机标定或三维重建问题中经常使用。

common.cpp文件的部分代码

cpp 复制代码

// 从文件中读取数据，并检查读取是否成功的函数模板
template<typename T>
void FscanfOrDie(FILE *fptr, const char *format, T *value) {
    // 使用 fscanf 从文件中按照给定的格式读取数据到 value 中
    int num_scanned = fscanf(fptr, format, value);
    // 如果读取的数据数量不符合预期（不是 1 个），则输出错误信息
    if (num_scanned != 1)
        std::cerr << "Invalid UW data file. ";
}

// BALProblem 类的构造函数
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;
    };

    // 读取文件头信息：相机数量、路标点数量、观测数量 读取第1行的3个数：16 22106 83718
    FscanfOrDie(fptr, "%d", &num_cameras_);
    // 每次调用 FscanfOrDie 函数只会读取并解析一组数据。
    // 例如，如果你的调用是 FscanfOrDie(fptr, "%d", &num_cameras_);，那么它会从文件中读取一个整数，存储到 num_cameras_ 变量中。
    FscanfOrDie(fptr, "%d", &num_points_);
    FscanfOrDie(fptr, "%d", &num_observations_);

    std::cout << "Header: " << num_cameras_
              << " " << num_points_
              << " " << num_observations_<<std::endl;//Header: 16 22106 83718
    // 分配存储相机索引、路标点索引和观测数据的数组空间
    point_index_ = new int[num_observations_];//point_index_ 是一个 int 类型的指针，指向一个包含 num_observations_ 个整数的数组。
    camera_index_ = new int[num_observations_];//camera_index_ 也是一个 int 类型的指针，指向一个包含 num_observations_ 个整数的数组。
    observations_ = new double[2 * num_observations_];//observations_ 是一个 double 类型的指针，指向一个包含 2 * num_observations_ 个双精度浮点数的数组。
    // observations_指向 [x1,y1,   x2,y2  ,...,  x83718,y83718] 

    // 计算参数总数并分配存储参数的数组空间
    num_parameters_ = 9 * num_cameras_ + 3 * num_points_;//num_parameters_ = 9*16+ 3*22106 =66462
    // problem-16-22106-pre.txt  150181行 - 83719行   = 66462行
    parameters_ = new double[num_parameters_];

    // 从文件中读取相机索引、路标点索引和观测数据，并存储到相应的数组中
    /* `camera_index_ + i`、`point_index_ + i`、`observations_ + 2 * i + j` 是指针算术的用法。
    1. `camera_index_ + i` 和 `point_index_ + i`：这些是指针算术的形式，用于访问数组中的特定元素。
    `camera_index_` 和 `point_index_` 是指向 `int` 类型数组的指针。
    `camera_index_ + i` 和 `point_index_ + i` 分别表示数组的第 `i` 个元素的地址。
    由于这些是指针算术，因此它们会根据数组元素的大小移动指针，使得指针指向相应元素的地址。
    2. `observations_ + 2 * i + j`：这里也是指针算术的用法，`observations_` 是指向 `double` 类型数组的指针。
    `observations_ + 2 * i + j` 表示数组中第 `(2 * i + j)` 个元素的地址。
    在这个循环中，`j` 的值为 `0` 或 `1`，因此 `observations_ + 2 * i + j` 分别访问了 `observations_` 数组中的奇数或偶数索引位置的元素。 */
    
    // 读取第2行到83719行 ，共83718行观测
    for (int i = 0; i < num_observations_; ++i) { // i < 83718
        FscanfOrDie(fptr, "%d", camera_index_ + i);
        FscanfOrDie(fptr, "%d", point_index_ + i);
        for (int j = 0; j < 2; ++j) {// [x1,y1,   x2,y2  ,...,  x83718,y83718] 
            FscanfOrDie(fptr, "%lf", observations_ + 2 * i + j);
        }
    }

    // 读取 83720行 到 最后180181行，共144 * 9 + 22106 * 3  =66462行参数
    // 读取并存储所有参数
    // 16个相机 * 每个相机的9个参数   144行
    // 22106个路标点 * 每个路标点的3个参数  66318行
    for (int i = 0; i < num_parameters_; ++i) {
        FscanfOrDie(fptr, "%lf", parameters_ + i);
    }


// 打印数组大小
std::cout << "Size of camera_index_: " << num_observations_ << std::endl;
std::cout << "Size of point_index_: " << num_observations_ << std::endl;
std::cout << "Size of observations_: " << 2 * num_observations_ << std::endl;
std::cout << "Size of parameters_: " << num_parameters_ << std::endl;
// Size of camera_index_: 83718
// Size of point_index_: 83718
// Size of observations_: 167436
// Size of parameters_: 66462
// Size of camera_index_: 83718

// 打印相机索引数组 camera_index_
for (int i = 0; i < 10; ++i) {
    std::cout << "Camera index " << i << ": " << camera_index_[i] << std::endl;
}

// 打印路标点索引数组 point_index_
for (int i = 0; i < 10; ++i) {
    std::cout << "Point index " << i << ": " << point_index_[i] << std::endl;
}

// 打印观测数据数组 observations_
for (int i = 0; i < 10; ++i) {
    std::cout << "Observations for " << i << ": " << observations_[2 * i] << " " << observations_[2 * i + 1] << std::endl;
}

// 打印参数数组 parameters_
for (int i = 0; i < 10; ++i) {
    std::cout << "Parameter " << i << ": " << parameters_[i] << std::endl;
}

// Camera index 0: 0
// Camera index 1: 1
// Camera index 2: 2
// Camera index 3: 7
// Camera index 4: 12
// Camera index 5: 13
// Camera index 6: 0
// Camera index 7: 1
// Camera index 8: 10
// Camera index 9: 0
// Point index 0: 0
// Point index 1: 0
// Point index 2: 0
// Point index 3: 0
// Point index 4: 0
// Point index 5: 0
// Point index 6: 1
// Point index 7: 1
// Point index 8: 1
// Point index 9: 2
// Observations for 0: -385.99 387.12
// Observations for 1: -38.44 492.12
// Observations for 2: -667.92 123.11
// Observations for 3: -599.18 407.93
// Observations for 4: -720.43 314.34
// Observations for 5: -115.13 55.49
// Observations for 6: 383.88 -15.3
// Observations for 7: 559.75 -106.15
// Observations for 8: 353.19 164.95
// Observations for 9: 591.55 136.44
// Parameter 0: -0.016944
// Parameter 1: 0.0111718
// Parameter 2: 0.00246435
// Parameter 3: 0.73031
// Parameter 4: -0.264908
// Parameter 5: -1.71279
// Parameter 6: 1430.03
// Parameter 7: -7.55728e-08
// Parameter 8: 3.23776e-14
// Parameter 9: 0.0150497



    fclose(fptr); // 关闭文件

    // 根据 use_quaternions 的值，将角轴表示的旋转转换为四元数表示
    use_quaternions_ = use_quaternions;
    if (use_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++;
            }
        }
        // 复制剩余的路标点信息
        for (int i = 0; i < 3 * num_points_; ++i) {
            *quaternion_cursor++ = *original_cursor++;
        }
        // 使用四元数参数替换角轴参数
        delete[] parameters_;
        parameters_ = quaternion_parameters;
    }
}

终端输出部分信息：

cpp 复制代码

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

参数块：16个相机 + 22106 个路标点 = 22122

参数：16个相机 * 9个参数 + 22106个路标点 * 3个参数 = 144 +66318 = 66462个参数

残差块：83718次观测

残差：[x1,y1, x2,y2 ,..., x83718,y83718] ，数组大小为2*83718 =167436