admin管理员组文章数量:1660166
文章目录
- 1.运行报错
- 1.1.工程编译报错
- 1.2.ceres到达迭代次数优化不收敛
- 2.ceres进行BA优化
- 2.1.SnavelyReprojection.h
- 2.1.1.文件作用
- 2.1.2.文件内容
- 2.2mon.cpp和common.h
- 2.3.budle_adjustment_ceres.cpp
- 3.g2o进行BA优化
1.运行报错
1.1.工程编译报错
视觉SLAM十四讲ch9代码关于fmt的报错
这个博客中和我遇到的错误一模一样，主要两个错误：
1.struct HessianTupleType<std::index_sequence<Ints...>> { error: 'index_sequence' is not a member of 'std'
这个博客中也说了主要是最新版的g2o使用的是C++14的编译标准，而高博原来的程序中写的是C++11编译，所以需要把CMakeLists.txt文件中改为C++14编译。但是修改之后还报错：
2.undefined reference to 'fmt::v8::datail::basic_data<void>::left_padding_shifts'
这个是因为g2o库更新，需要使用fmt库，因此需要安装fmt库。所以在CmakeList.txt中添加如下内容：
find_package(fmt REQUIRED)
set(FMT_LIBRARIES fmt::fmt)
target_link_libraries(bundle_adjustment_g2o ${G2O_LIBS} ${FMT_LIBRARIES} bal_common))
1.2.ceres到达迭代次数优化不收敛
ceres的默认迭代次数是50次，使用这个默认的迭代次数运行最后不收敛，原因是达到迭代次数。此时把迭代次数修改提高一下：
ceres::Solver::Options options;
options.max_num_iterations = 100;
比如这里提高为100次，再次运行优化就收敛了，并且从结果看这个时候迭代了61次，实际上只比50多了十多次。
2.ceres进行BA优化
参考博客：
非线性优化-ceres-solver- Bundle Adjustment
2.1.SnavelyReprojection.h
2.1.1.文件作用
首先这个文件的命名中Sanvely是一个人的名字，这个数据集使用的是Noah Snavely’s Bundler assumes，即Noah Snavely假设的相机模型，其中相机坐标系具有负的Z轴，也就是高博书中说的相机投影平面在光心之后，所以按照书中的投影模型最后得到的Z轴数值还要×-1。
上面的博客中写的解释：
给定一组测量的图像特征位置和对应关系，束调整的目标是找到最小化重投影误差的 3D 点位置和相机参数。这个优化问题通常被表述为非线性最小二乘问题，其中误差是观察到的特征位置与相机图像平面上相应3D点的投影之间差异的平方 L2L2范数。 Ceres 为解决束调整问题提供了广泛的支持。
像往常一样，第一步是定义一个计算重投影误差/残差的模板化函子。函子的结构类似ExponentialResidual（即指数残差），因为每个图像观察都有一个该对象的实例。
BAL 问题中的每个残差取决于一个三维点和一个九参数相机。定义相机的九个参数是：三个用于作为 Rodriques轴角矢量的旋转，三个用于平移，一个用于焦距，两个用于径向畸变。该相机型号的详细信息可以在 Bundler 主页和 BAL 主页上找到。
2.1.2.文件内容
这是一个头文件，主要就是定义了ceres自动求导需要用到的模板类，并且在模板类的重载()运算符中实现残差的计算。另外注意在for循环中添加残差的时候，其中的一个参数是ceres::CostFunction这个对象的指针，这里在这个类中一并实现了，如下所示。所以后面在添加残差的时候直接调用这个类成员函数即可，这样也是提高程序的紧凑性吧。
static ceres::CostFunction *Create(const double observed_x, const double observed_y) {
return (new ceres::AutoDiffCostFunction<SnavelyReprojectionError, 2, 9, 3>( // 残差二维，两个优化变量分别是9维和3维
new SnavelyReprojectionError(observed_x, observed_y)));
}
另外好像没有发现ceres中有定义更新的函数？所以这里使用的优化变量是旋转向量，而不是旋转矩阵。这样对于迭代得到的值直接加上就可以，无需像优化T那样从李代数转换到李群上再乘。
对于这个程序，有几个不明白的问题如下：
- 为什么相机参数中把畸变也当成了优化参数？这个属于相机内参不应该是确定的吗？
- 类中根据相机参数（内参和外参）计算估计的投影像素坐标的函数
static inline bool CamProjectionWithDistortion(const T *camera, const T *point, T *predictions)，声明为了static inline，为什么要这样？
解释： - static：静态成员在所有类的对象之间共享，更像是一个全局函数。静态成员函数或变量的设计思路是将和某些类紧密相关的全局变量或函数写在类里面，使其看上去像一个整体，易于理解和维护。在静态成员函数中不能访问非静态成员变量，也不能调用非静态成员函数。 所以这里就是因为这个计算投影点的函数只是给这个类调用，所以声明为static更加紧凑。
- inline：内联函数，函数被调用的时候直接就地展开，而无需进行入栈在CALL函数在出栈。其实内联函数比较适合很简单的函数，这种函数通常执行的时间比调用的时间要断，所以声明为内联可以提高执行速度。当然内联会导致代码急剧膨胀，在函数内进行了复杂操作的时候更是如此，一般尽量不要声明为内联。此外，并非声明了inline就是内联，这可能还和编译器有关。
详细可以参考：C++类的const, static 和inline成员函数（变量）
#ifndef SnavelyReprojection_H
#define SnavelyReprojection_H
#include <iostream>
#include "ceres/ceres.h"
#include "rotation.h"
class SnavelyReprojectionError {
public:
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); // 把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;
}
// 注意这里就是之前在主程序中定义的添加残差的时候加入的对象指针
// 另外这里为什么要声明为static?
static ceres::CostFunction *Create(const double observed_x, const double observed_y) {
return (new ceres::AutoDiffCostFunction<SnavelyReprojectionError, 2, 9, 3>( // 残差二维，两个优化变量分别是9维和3维
new SnavelyReprojectionError(observed_x, observed_y)));
}
private:
double observed_x;
double observed_y;
};
#endif // SnavelyReprojection.h
2.2mon.cpp和common.h
这两个文件主要是定义了一个BALProblem类，用于读取数据集中的数据。
对于这个数据集，其格式内容如下。但是一个比较困惑的内容是里面的像素点是一个小数，这个很奇怪，可能是由于这个数据已经是去掉了cx/cy的原因？（但是cx, cy也是整数啊）
// 第1行：分别是相机的位姿个数16，观测点的个数22106，观测得到的像素点个数83718
16 22106 83718
// 第2行 - 83719行：
//分别描述了当前这儿观测数据的 相机位姿序号， 路标点序号， 观测的像素u值，观测的像素v值
0 0 -3.859900e+02 3.871200e+02
1 0 -3.844000e+01 4.921200e+02
2 0 -6.679200e+02 1.231100e+02
7 0 -5.991800e+02 4.079300e+02
................................
// 第83720行 - 150181行：
// 前9 * 16行存放相机的16个位姿的数据，比如前9行是相机0号位姿数据，第9-18行是相机1号位姿数据
// 从9 * 16行到最后，共3 * 22106行存放所有的路标点的数据，比如前3行是0号路标点的位置
// 所以一共是9 * 16 + 3 * 22106 = 150181 - 83720 + 1 = 66462行
这里面最重要的类的构造函数，实现了从数据文件中读取数据存到相关变量里，其函数实现如下。
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_);
// 16 22106 83718
std::cout << "Header: " << num_cameras_ // 16个相机位姿
<< " " << num_points_ // 22106个路标点
<< " " << num_observations_; // 观测数据（像素点）个数83718,因为一个位姿的相机不可能看到所有的观测点，所以这里观测数据<16 * 22106
point_index_ = new int[num_observations_]; // 路标点的序号数组，说明当前的像素点对应的路标点的序号 0 - 22105
camera_index_ = new int[num_observations_]; // 相机位姿序号数组，说明当前的像素点对应的相机位姿的序号 0 - 15
observations_ = new double[2 * num_observations_]; // 观测的像素值，可是为什么是小数？
num_parameters_ = 9 * num_cameras_ + 3 * num_points_; // 对每一次观测的误差，参数的个数：9个相机参数 + 3个路标点参数
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) { // 对于16个相机位姿和22106个路标点，都有一个初始值，这里依次读取出来
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.3.budle_adjustment_ceres.cpp
这个文件中就是实现BA优化了，ceres部分主要的就是添加残差块。但是这其中需要和2.2节的分析结合，对每一个观测的像素点，要根据相机位姿序号和路标点序号正确提取出对应的9维相机位姿和3位路标点数据。
其次就是残差的添加，这很简单，就是对于每一个观测数据，都要添加一个残差，所以for循环遍历即可。
#include <iostream>
#include <ceres/ceres.h>
#include "common.h"
#include "SnavelyReprojectionError.h"
using namespace std;
void SolveBA(BALProblem &bal_problem);
int main(int argc, char **argv) {
if (argc != 2) {
cout << "usage: bundle_adjustment_ceres 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;
}
void SolveBA(BALProblem &bal_problem) {
const int point_block_size = bal_problem.point_block_size(); // 路标点维度，3
const int camera_block_size = bal_problem.camera_block_size(); // 相机参数维度，9
// mutable 可变的
double *points = bal_problem.mutable_points(); // 22106个路标点的位置数据的头部指针
double *cameras = bal_problem.mutable_cameras(); // 16个相机位姿数据的头部指针
// 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;
// bal_problem.num_observations()返回观测到的像素的个数，这里是83718
// 也就是对每个观测的值，都计算残差
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的定义在 误差的模板类 中实现了，本质上这里和程序中直接生成是一样的
cost_function = SnavelyReprojectionError::Create(observations[2 * i + 0], observations[2 * i + 1]);
// If enabled use Huber's loss function.
ceres::LossFunction *loss_function = new ceres::HuberLoss(1.0); // 这个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.
/* cameras - 16个相机位姿数据的头部指针； bal_problem.camera_index()返回存放相机位姿序号
的数组的指针，i就是第i个观测数据对应的相机位姿。在cameras偏移camera_block_size（相机位姿
参数维度，9），得到的就是当前观测数据对应的相机位姿的参数中指向第一个的指针。
*/
double *camera = cameras + camera_block_size * bal_problem.camera_index()[i];
double *point = points + point_block_size * bal_problem.point_index()[i]; // 关于路标点的这个同理
/* 添加残差块，对于每个观测的像素点来说，需要优化的是相机参数和路标点参数,
最后要优化的损失函数是所有的观测点的误差的平方和。
loss_function是核函数，防止噪声数据导致过大的梯度影响.
*/
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;
ceres::Solver::Options options;
options.max_num_iterations = 100;
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优化
这一部分的程序比较简单，主要编程思路就是根据g2o编程的套路来。主要分为以下几步：
- 定义顶点类：根据顶点数据类型的不同，可能会要先定义好自己顶点的数据类型，然后把这个数据类型作为参数，传入到定义的顶点类中定义顶点类（一般都是继承自基础顶点）。顶点里要实现顶点数据的初始化（设置_estimate成员变量），顶点数据的更新规则（oplusImpl函数）
- 定义边类：边的参数就是前面定义的顶点类，边这个类一般都是继承自二元边。在这个边里要实现计算误差的函数（computeError），实现解析雅克比函数（linearizeOplus）。如果不实现这个函数的话，会使用数值雅克比进行求导。
- 根据数据添加所有的顶点：包括设置顶点的Id（vertex->setId），设置顶点的初始估计值（vertex->setEstimate），在优化器中添加顶点（optimizer.addVertex）。需要注意的是，对于SLAM问题存在稀疏的结构，可以使用边缘化来加速求解，一般而言都是把路标点的数据边缘化，转化到相机位姿中计算。当需要使用边缘化的时候，g2o要求手动设置待边缘化的顶点，也就是vertex->setMarginalized(true)。
- 根据数据添加所有的边：边需要添加它链接的顶点，即edge->setVertex。其次需要设置观测值，即edge->setMessurement。设置信息矩阵，表明各个数据的方差，即edge->setInformation。设置鲁邦核函数，防止噪声数据造成误梯度。把这个边的数据都设置好之后，在优化器中添加这个边，即optimizer.addEdge。
- 开始优化：此时已经一切准备就绪，可以进行优化了。这个即调用
optimizer.initializeOptimization();和optimizer.optimize(40);进行优化，其中第二个函数中的参数是最大迭代次数，当不收敛的时候可以适当的增加这个迭代次数。 - 把优化结果更新到内存（非必要）：在ch9的程序中，使用了BLA这个数据集，数据的存取都是使用这个类中的函数，而且这个类中可以实现把优化前后的数据生成.ply类型的点云数据以便查看。由于使用g2o优化得到的顶点都是在自定义的数据类型中，原来BLA中读出来的数据只是对自定义的顶点数据类型进行了初始化。所以要想让BLA生成优化后的结果的点云数据，就要把优化结果从自定义的顶点数据类型中存到BLA类的数据中。也就是高博的程序中最后的那一部分。但是对于Ceres来说，它的优化是直接在原来的数据内存上进行优化的，没有一定再定义数据类型（内存）存储要优化的数据，所以不需要进行更新内存的操作。
问题：看程序中g2o好像必须指定一个迭代次数，也就是这一句optimizer.optimize(10);，如果这个函数中不传参的话会报错，也就是必须指定迭代次数。并且从运行结果来看，它就是会迭代这么多次，难道不会自己判定是否收敛吗？
#include <g2o/core/base_vertex.h>
#include <g2o/core/base_binary_edge.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/solvers/csparse/linear_solver_csparse.h>
#include <g2o/core/robust_kernel_impl.h>
#include <iostream>
#include "common.h"
#include "sophus/se3.hpp"
using namespace Sophus;
using namespace Eigen;
using namespace std;
/// 姿态和内参的结构 Intrinsics内参
struct PoseAndIntrinsics {
PoseAndIntrinsics() {}
/// set from given data address
// 在构造函数之前显式地声明explicit，防止隐式转换。比如类的构造函数形参为int，外部一个函数的形参为这个类，如果
// 调用这个外部函数的时候传入int类型，那么编译器不会报错，而是会进行隐式的类型转化，把int作为类的构造函数的形参，
// 然后把生成一个类的对象传给外部调用的这个函数
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) {
auto 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;
};
/// 位姿加相机内参的顶点，9维，前三维为so3，接下去为t, f, k1, k2
// 这里设置顶点优化变量维度为9，要优化的变量类型是PoseAndIntrinsics这个类
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) { // 把3D点进行投影，得到估计的投影点位置
Vector3d pc = _estimate.rotation * point + _estimate.translation;
pc = -pc / pc[2];
double r2 = pc.squaredNorm();
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) {}
virtual bool write(ostream &out) const {}
};
// 顶点：路标点
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) {}
virtual bool write(ostream &out) const {}
};
// 投影的边
// 边的维度为2，数据类型是Vecor2d，连接的两个顶点类型分别是VertexPoseAndIntrinsics, VertexPoint
class EdgeProjection :
public g2o::BaseBinaryEdge<2, Vector2d, VertexPoseAndIntrinsics, VertexPoint> {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
virtual void computeError() override {
auto v0 = (VertexPoseAndIntrinsics *) _vertices[0]; // 强制类型转换，_vertices[0]中存放的就是第一个输入的数据
auto v1 = (VertexPoint *) _vertices[1];
auto proj = v0->project(v1->estimate()); // 计算投影值
_error = proj - _measurement; // 计算误差值
}
// use numeric derivatives
// 这里没有声明雅克比的函数，所以是数值求导
virtual bool read(istream &in) {}
virtual bool write(ostream &out) const {}
};
void SolveBA(BALProblem &bal_problem);
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;
}
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();
// pose dimension 9, landmark is 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);
/// build g2o problem
const double *observations = bal_problem.observations();
// vertex 存放顶点指针的容器
vector<VertexPoseAndIntrinsics *> vertex_pose_intrinsics;
vector<VertexPoint *> vertex_points;
// 对于16个相机位姿，添加16个顶点
for (int i = 0; i < bal_problem.num_cameras(); ++i) {
VertexPoseAndIntrinsics *v = new VertexPoseAndIntrinsics(); // 这个顶点类型继承自基础顶点类，所以后面有setId等成员函数
double *camera = cameras + camera_block_size * i; // 当前这个相机位姿的指针，索引9个数
v->setId(i); // 设置顶点Id
v->setEstimate(PoseAndIntrinsics(camera)); // 设置顶点的估计初始值
optimizer.addVertex(v); // 在优化器中添加这个顶点
vertex_pose_intrinsics.push_back(v);
}
// 对于22106个路标点，添加22106个顶点
for (int i = 0; i < bal_problem.num_points(); ++i) {
VertexPoint *v = new VertexPoint();
double *point = points + point_block_size * i;
v->setId(i + bal_problem.num_cameras());
v->setEstimate(Vector3d(point[0], point[1], point[2]));
// g2o在BA中需要手动设置待Marg的顶点
v->setMarginalized(true); // 手动设置稀疏求解
optimizer.addVertex(v);
vertex_points.push_back(v);
}
// edge 每一个观测值都是一条边
for (int i = 0; i < bal_problem.num_observations(); ++i) {
EdgeProjection *edge = new EdgeProjection;
// bal_problem.camera_index()[i]得到的是当前观测数据对应的相机位姿的序号，
// 作为数组索引恰好得到vector容器中存放的相机位姿顶点的指针
edge->setVertex(0, vertex_pose_intrinsics[bal_problem.camera_index()[i]]); // 设置这条边的第一个顶点
// bal_problem.point_index()[i]得到的是当前观测数据对应的路标点的序号，
// 作为数组索引恰好得到vector容器中存放的路标点顶点的指针
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); //添加边
}
optimizer.initializeOptimization();
optimizer.optimize(40);
// set to bal problem
// 程序执行到这里的时候，已经优化完毕了，为了后面
// BLAproblem这个类把优化的结果存盘使用，所以还需要把这个结果
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]; // 这里位姿顶点的数据类型是Vector3d，所以这里返回的估计值也是Vector3d
for (int k = 0; k < 3; ++k) point[k] = vertex->estimate()[k];
}
}
版权声明：本文标题：视觉SLAM十四讲——第九讲后端1 内容由热心网友自发贡献，该文观点仅代表作者本人， 转载请联系作者并注明出处：https://www.elefans.com/dianzi/1729850578a1215354.html， 本站仅提供信息存储空间服务，不拥有所有权，不承担相关法律责任。如发现本站有涉嫌抄袭侵权/违法违规的内容，一经查实，本站将立刻删除。
发表评论