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