十四讲里面的g2o优化

Caleb tam

已于 2023-05-31 19:55:41 修改

阅读量290

点赞数

CC 4.0 BY-SA版权

分类专栏： slam 文章标签： c++ 开发语言

于 2022-11-30 17:47:44 首次发布

本文链接：https://blog.youkuaiyun.com/t_home/article/details/128119124

slam 专栏收录该内容

5 篇文章

订阅专栏

该博客主要介绍了一个C++程序，它读取特定的数据格式文件，用于构建相机姿态和空间点的优化问题。程序首先检查文件是否正确打开，然后读取文件头信息，包括相机、点和观测的数量。接着，它分配内存来存储观测数据，包括相机索引、点索引和观测坐标。最后，程序读取姿态和点的参数，为求解器准备数据。整个过程涉及文件I/O、内存管理和数据解析。

摘要生成于 C知道，由 DeepSeek-R1 满血版支持，前往体验 >

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);
    bal_problem.WriteToPLYFile("initial.ply");
    SolveBA(bal_problem);
    bal_problem.WriteToPLYFile("final.ply");

    return 0;
}

1 分析构造函数（部分代码）

数据格式

Data Format

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

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_);
    // 顶层三个值  相机(Pose + Intrinsics)个数  空间点(位置)个数  观测边的条数
    std::cout << "Header: " << num_cameras_
              << " " << num_points_
              << " " << num_observations_ << std::endl;

    // 根据数量 开辟指针类型变量空间  int *p = new int[10] 是创建10个int型的内存
    point_index_ = new int[num_observations_];
    camera_index_ = new int[num_observations_];
    observations_ = new double[2 * num_observations_];
    // 输入边的描述  index  1对1
    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);
        }
    }

    // 非四元数表示姿态  R+t+Intrinsics ： 3 + 3 + 3    空间点   3  
    num_parameters_ = 9 * num_cameras_ + 3 * num_points_;
    parameters_ = new double[num_parameters_];
    // 输入两类顶点， （姿态和内参） （空间点）
    for (int i = 0; i < num_parameters_; ++i) {
        FscanfOrDie(fptr, "%lf", parameters_ + i);
    }

    fclose(fptr);
}