视觉SLAM ch9代码总结

BAL数据集

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

rotation.h

旋转向量与四元数的相互转换    书60页有公式推导

q = cos (a/2) + i(x * sin(a/2)) + j(y * sin(a/2)) + k(z * sin(a/2))
//其中a表示旋转角度，(x,y,z)表示旋转轴(单位向量)

 AngleAxisRotatePoint(const T angle_axis[3], const T pt[3], T result[3])   这个函数通过旋转向量来旋转点pt，相当于求某一个点经过旋转矩阵后的位置，也就是 R·pt。

推导过程：先将旋转向量通过罗德里格斯公式变为旋转矩阵，然后在乘这个点坐标。

random.h

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

common.cpp
功能：读取信息，写入信息，写入点云可视化文件，对空间点、旋转、平移加入噪声

int fscanf(文件指针,格式字符串,输入列表),返回值为读取的数据个数；fscanf函数，完成读取后文件指针自动向下移

void FscanfOrDie(FILE *fptr, const char *format, T *value) {
    //fscanf:文件指针自动向下移动
    int num_scanned = fscanf(fptr, format, value);
    if (num_scanned != 1)
        std::cerr << "Invalid UW data file. ";
}

（1）最开始先读取前三个数：分别是相机视角数量、特征点数量、观测结果数量，也就是16张图像，共提取到22106个特征点，这些特征点共出现了83718次（同一特征点在多个视角下重复出现）。 

（2）观测结果：每行四个数据包括图像编号（相机视角编号）、特征点编号、像素坐标。例如“10 2     5.826200e+02 3.637200e+02”就表明2号特征点在10号图像内的成像坐标为(582.6,363.7)。 

（3）最后是相机参数及路标点世界坐标：每个视角相机参数共有9个，3轴旋转角度、3轴平移向量、焦距、2个畸变系数，如果转化为四元数表达旋转，就对应10个参数（4+3+1+2）。每个路标点世界坐标包括三个参数。

这部分代码的具体介绍参考 link

ceres实现BA

①定义残差块的类。方法是写一个类或结构体(ch6中定义的是结构体)，在类中定义带模板参数的（）运算符

②构建最小二乘问题，主要采用Ceres::Problem类进行，使用Problem::AddResidualBlock( )
AddResidualBlock()顾名思义主要用于向Problem类传递残差模块的信息，函数原型如下，传递的参数主要包括代价函数模块、损失函数模块和参数模块。

③最小二乘的求解，使用ceres::Solve

按照上述流程，先看SnavelyReprojectionError.h代码：

1.重点在于重载括号运算符，重载过程主要是计算残差块。残差 = 预测值 - 实际值。实际值是每个特征点的实际坐标，数据集中已经给出；然后我们需要根据特征点的世界坐标计算它对应的图像坐标，也就是预测值。

计算方法：（1）使用轴角旋转这个点 

AngleAxisRotatePoint(camera, point, p);

（2）再加上平移部分

p[0] += camera[3];
p[1] += camera[4];
p[2] += camera[5];

 此时得到了相机坐标系坐标，将他变到归一化坐标系坐标

注意：这里乘-1 的原因书253页有介绍

T xp = -p[0] / p[2];
T yp = -p[1] / p[2];

（3）计算畸变，只考虑k1和k2

const T &l1 = camera[7];
const T &l2 = camera[8];

T r2 = xp * xp + yp * yp;
//下面的式子展开就是p102 式5.12
T distortion = T(1.0) + r2 * (l1 + l2 * r2);

 （4）通过内参矩阵将畸变后的点投影到像素平面

const T &focal = camera[6];
//式5.13   这里是认为 fx=fy  然后不考虑 cx cy
predictions[0] = focal * distortion * xp;
predictions[1] = focal * distortion * yp;

2. 将代价函数cost_function封装

static ceres::CostFunction *Create(const double observed_x, const double observed_y)
{

     //这里面残差是二维 待优化的是相机以及3d点 所以自动求导模板中的数字2,9,3分别是它们的维度
     return (new ceres::AutoDiffCostFunction
            (
                 new SnavelyReprojectionError(observed_x, observed_y))
            );
}

最后看bundle_adjustment_ceres.cpp  ，可以参考ch6中关于ceres求解步骤

g2o实现BA

参考：link

①定义相机参数节点（九维）

② 定义路标点坐标节点（三维）

③定义二元边：计算重投影误差

④构建图优化 SolveBA函数

//1 配置块求解器  BlockSolver 
typedef g2o::BlockSolver> BlockSolverType;

//2 配置线性方程求解器 LinearSolver
typedef g2o::LinearSolverCSparse LinearSolverType;

//3 配置总求解器solver，并从GN,LM,Dogleg优化算法中选一个，再用上述块求解器BlockSolver初始化
auto solver = new g2o::OptimizationAlgorithmLevenberg(
            g2o::make_unique(g2o::make_unique()));

//4 配置稀疏优化器SparseOptimizer
g2o::SparseOptimizer optimizer;
optimizer.setAlgorithm(solver);//设置求解器
optimizer.setVerbose(true);//打开调试输出

//5 往图中增加顶点和边，并添加到SparseOptimizer 
//因为顶点有很多个，所以需要容器

//容器 vertex_pose_intrinsics 和 vertex_points存放两顶点的地址
vector vertex_pose_intrinsics;
vector vertex_points;

for(int i=0;isetId(i);//设置编号
     v->setEstimate(PoseAndIntrinsics(camera));//传入待优化的系数 此处为相机
     optimizer.addVertex(v);//加入顶点
     vertex_pose_intrinsics.push_back(v);
}

for(int i=0; isetId(i+bal_problem.num_cameras());
    v->setEstimate(Vector3d(point[0],point[1],point[2]));//传入待优化的系数 此处为路标点
        
    //g20需要手动设置待边缘化（Marg）的点
    v->setMarginalized(true);//设置边缘化
    optimizer.addVertex(v);//加入顶点
    vertex_points.push_back(v);//将顶点一个一个放回到容器里面
}

//加入边
for(int i=0;isetId(i);//设置编号
    edge->setVertex(0,vertex_pose_intrinsics[bal_problem.camera_index()[i]]);//加入顶点
    edge->setVertex(1,vertex_points[bal_problem.point_index()[i]]);//加入顶点
    edge->setMeasurement(Sophus::Vector2d(observations[2*i+0],observations[2*i+1]));//设置观测数据
    edge->setInformation(Eigen::Matrix2d::Identity());//设置信息矩阵
    edge->setRobustKernel(new g2o::RobustKernelHuber());//设置核函数
    ptimizer.addEdge(edge);//加入边
}

//6 启动优化 
optimizer.initializeOptimization();                       // 设置优化初始值
optimizer.optimize(10);                                   // 设置优化次数

课后题参考：

视觉SLAM十四讲（第二版）第9讲习题解答 - 知乎 (zhihu.com)