视觉SLAM ch8代码总结_视觉slam第八章代码解析

目录

一、LK光流

1.使用LK光流

 2.使用高斯牛顿实现光流

 3.多层光流

光流法实践总结

 二、直接法

1.单层直接法

2.多层稀疏直接法

三、课后题

---

一、LK光流

1.使用LK光流

用OpenCV的光流追踪特征点的运动。

第一张图像中提取角点，利用光流追踪角点在第二张图像中的位置。

使用cv::calcOpticalFlowPyrLK函数可以得到追踪后的点。

void cv::calcOpticalFlowPyrLK	(	
InputArray 	prevImg,
InputArray 	nextImg,
InputArray 	prevPts,
InputOutputArray 	nextPts,
OutputArray 	status,
OutputArray 	err,
Size 	winSize = Size(21, 21),
int 	maxLevel = 3,
TermCriteria 	criteria = TermCriteria(TermCriteria::COUNT+TermCriteria::EPS, 30, 0.01),
int 	flags = 0,
double 	minEigThreshold = 1e-4 
)		

使用具有金字塔迭代的LK计算稀疏特征集的光流

参数：
prevImg ：buildOpticalFlowPyramid构造的第一个输入图像或金字塔。

nextImg ：与prevImg相同大小和相同类型的第二个输入图像或金字塔。

prevPts ：图1中的关键点（单精度浮点型Point2f）

nextPts ：输出的对应在图2中的关键点，当flags = OPTFLOW_USE_INITIAL_FLOW时，
nextPts将作为光流法初值，此时其维度必须与prevPts相同。

status ：输出状态向量（uchar类型）;如果找到相应特征的流，则向量的每个元素设置为1，否则设置为0。

err ：输出每个点的匹配误差; 误差度量的类型可以在flags参数中设置; 如果未找到流，则未定义错误。

winSize ：每个金字塔等级的搜索窗口的winSize大小。

maxLevel ：基于0的最大金字塔等级数;如果设置为0，则不使用金字塔（单级），如果设置为1，则使用两个级别，依此类推;如果将金字塔传递给输入，那么算法将使用与金字塔一样多的级别，但不超过maxLevel。

criteria ：参数，指定迭代搜索算法的终止条件（在指定的最大迭代次数criteria.maxCount之后或当搜索窗口移动小于criteria.epsilon时）。

flags ：操作标志：
OPTFLOW_USE_INITIAL_FLOW使用初始估计，存储在nextPts中;如果未设置标志，则将prevPts复制到nextPts并将其视为初始估计。
OPTFLOW_LK_GET_MIN_EIGENVALS使用最小特征值作为误差测量（参见minEigThreshold描述）;如果没有设置标志，则将原稿周围的色块和移动点之间的L1距离除以窗口中的像素数，用作误差测量。

minEigThreshold ：算法计算光流方程的2x2正常矩阵的最小特征值，除以窗口中的像素数;
如果此值小于minEigThreshold，则过滤掉相应的功能并且不处理其流程，因此它允许删除坏点并获得性能提升。

 OpenCV颜色空间转换：cvtColor()函数

void cvtColor（InputArray src，outputArray dst，int code，int dstCn=0）

src:为输入图像;
dst:为输出图像;
code:为颜色空间转换的标识符。程序中使用的是CV_GRAY2BGR,转化为BGR格式的图

dstCn:为目标图像的通道数，若该参数是0，表示目标图像取源图像的通道数。

 绘制圆circle()函数

void circle(InputOutputArray img, Point center, int radius,
                       const Scalar& color, int thickness = 1,
                       int lineType = LINE_8, int shift = 0);

 img:图像                           center:圆心

radius:半径                        color:颜色，比如cv::Sclar(0,0,250) 表示红色

thickness:圆线条的粗细       lineType:线条的类型，默认是8

 绘制两个点线段line()函数

void line(InputOutputArray img, Point pt1, Point pt2, const Scalar& color,
                     int thickness = 1, int lineType = LINE_8, int shift = 0);

img:图像                               pt1:线段的第一个端点

pt2:线段的第一个端点          color:线段颜色

thickness：线段粗细            lineType:线段类型

 2.使用高斯牛顿实现光流

光流可以看作一个优化问题，通过最小化灰度误差估计最优的像素偏移。（也就是像素在x、y轴上的速度）

误差项：error = I(x,y)(图1) - I(x+dx,y+dy)(图2)

雅可比矩阵： ①正向光流： 也就是图二点的灰度 负梯度

                        dx(i,j) = [I(i+1,j) - I(i-1,j)]/2;    i = kp.pt.x + dx + x ， j =  kp.pt.y + dy + y
                        dy(i,j) = [I(i,j+1) - I(i,j-1)]/2;

                        ②反向光流：图一的灰度负梯度

                        dx(i,j) = [I(i+1,j) - I(i-1,j)]/2;    i = kp.pt.x + x， j =  kp.pt.y + y
                        dy(i,j) = [I(i,j+1) - I(i,j-1)]/2;

步骤：

①在第一张图中检测角点

②利用光流追踪第一张图中的角点在第二张图中的位置

OpticalFlowSingleLevel()单层光流函数的实现

③当迭代dx、dy不是整数时，采用双线性插值方法获取像素灰度值 

参考：双线性插值

        图像处理之双线性插值法

④计算光流

calculateOpticalFlow()函数实现

程序中遇到的一些问题：

cv::Range类用于可以用来表示矩阵的多个连续的行或列,有两个元素 start 和 end。表示范围从start到end，包含start，但不包含end

class CV_EXPORTS Range
{
public:
    Range();
    Range(int _start, int _end);
    int size() const; //得到元素数量
    bool empty() const; //判断是否为空
    static Range all(); //可以用在任何需要获得对象可用范围的时候
 
    int start, end;
};

cv::parallel_for_(参数1， 参数2 )函数

使用parallel_for_ 可以并行计算节约时间

parallel_for_(Range(0,kp1.size()),
              std::bind::(&OpticalflowTracker::calculateOpticalFlow,&tracker,placeholders::_1));

Range表示一个范围，即并行计算哪些需要追踪的点，kp1是图一的角点；

bind是一个绑定函数，表示调用OpticalFlowTracker的实例tracker中的calculateOpticalFlow()，即计算光流；std::placeholders::_1是占位符，表示传入的参数是tracker.calculateOpticalFlow()的第一个参数。

 floor()函数：

返回一个小于传入参数的最大整数

例如：floor(2.6)的结果是2 ； floor(-2.6)结果是 -3   

---

 3.多层光流

如果相机运动速度快，两张图像差异明显，单层光流会达到局部极小值，通过引入图像金字塔来改善。

图像金字塔：对同一个图像进行缩放，得到不同分辨率下的图像。

缩放是从下往上，计算光流从上往下。

调整图像大小函数resize()

cv::resize(InputArray src, OutputArray dst, Size dsize, double fx=0, 
            double fy=0, int interpolation=INTER_LINEAR )

参数依次为：输入图像，输出图像，输出图像大小，横向缩放倍数，纵向缩放倍数，插值方式。

其中横、纵缩放倍数仅在输出图像大小(dsize)设为0时有用；插值默认双线性插值。

光流法实践总结

光流法取代描述子计算与匹配环节，后续对于相机位姿的估计以及路标点空间坐标的估计，仍需要对极几何、PnP、ICP等算法。

总之光流法比特征点法快，能够避免描述子计算与匹配，但要求相机运动平滑或采样率高。

 二、直接法

1.单层直接法

实现的是双目稀疏直接法

①RNG是opencv里c++的随机数产生器，可以产生三种随机数：

RNG(int seed)   使用种子seed产生一个64位随机整数
RNG::uniform( )      产生一个均匀分布的随机数
RNG::gaussian( )    产生一个高斯分布的随机数
 
RNG::uniform(a, b )  返回一个[a,b)范围的均匀分布的随机数，a,b的数据类型要一致，而且必须是int、float、double中的一种，默认是int。
 
RNG::gaussian( σ)   返回一个均值为0，标准差为σ的随机数。
如果要产生均值为λ，标准差为σ的随机数，可以λ+ RNG::gaussian( σ)

 ②std::mutex 介绍

std::mutex 是C++11 中最基本的互斥量，std::mutex 对象提供了独占所有权的特性——即不支持递归地对 std::mutex 对象上锁，而 std::recursive_lock 则可以递归地对互斥量对象上锁。

std::mutex 的成员函数：

构造函数，std::mutex不允许拷贝构造，也不允许 move 拷贝，最初产生的 mutex 对象是处于 unlocked 状态的。

lock()：调用线程将锁住该互斥量。线程调用该函数会发生下面 3 种情况：(1). 如果该互斥量当前没有被锁住，则调用线程将该互斥量锁住，直到调用 unlock之前，该线程一直拥有该锁。(2). 如果当前互斥量被其他线程锁住，则当前的调用线程被阻塞住。(3). 如果当前互斥量被当前调用线程锁住，则会产生死锁(deadlock)。

unlock()： 解锁，释放对互斥量的所有权。

try_lock()：尝试锁住互斥量，如果互斥量被其他线程占有，则当前线程也不会被阻塞。线程调用该函数也会出现下面 3 种情况，(1). 如果当前互斥量没有被其他线程占有，则该线程锁住互斥量，直到该线程调用 unlock 释放互斥量。(2). 如果当前互斥量被其他线程锁住，则当前调用线程返回 false，而并不会被阻塞掉。(3). 如果当前互斥量被当前调用线程锁住，则会产生死锁(deadlock)。

互斥量是一个可以处于两态之一的变量:解锁和加锁。这样，只需要一个二进制位表示它，不过实际上，常常使用一个整型量，0表示解锁，而其他所有的值则表示加锁。互斥量使用两个过程。当一个线程(或进程)需要访问临界资源的临界区时，它调用mutex_lock。如果该互斥量当前是解锁的(即临界区可用)，此调用成功，调用线程可以自由进入该临界区。

参考：mutex详解

        mutex简单介绍 

2.多层稀疏直接法

多层直接法与多层光流法大同小异，不同在于：当图像分辨率变为原来的k倍，对应相机内参也应该变为原来的k倍。

只要当相机运动很小，图像中的梯度没有很强的非凸性，直接法才成立。

 三、课后题

1.除了LK光流还有哪些光流方法？各有什么特点？

光流的概念是Gibson在1950年首先提出来的。它是空间运动物体在观察成像平面上的像素运动的瞬时速度，是利用图像序列中像素在时间域上的变化以及相邻帧之间的相关性来找到上一帧跟当前帧之间存在的相应关系，从而计算出相邻帧之间物体的运动信息的一种方法。一般而言，光流是因为场景中前景目标本身的移动、相机的运动，或者两者的共同运动所产生的。

从本质上说，光流就是你在这个运动着的世界里感觉到的明显的视觉运动。例如，当你坐在火车上，然后往窗外看。你可以看到树、地面、建筑等等，他们都在往后退。这个运动就是光流。而且，我们都会发现，他们的运动速度居然不一样，这就给我们提供了一个挺有意思的信息：通过不同目标的运动速度判断它们与我们的距离。一些比较远的目标，例如云、山，它们移动很慢，感觉就像静止一样。但一些离得比较近的物体，例如建筑和树，就比较快的往后退，然后离我们的距离越近，它们往后退的速度越快。一些非常近的物体，例如路面的标记啊，草地啊等等，快到好像在我们耳旁发出嗖嗖的声音。

假设条件：（光流法成立的条件）

（1）亮度恒定，就是同一点随着时间的变化，其亮度不会发生改变。这是基本光流法的假定（所有光流法变种都必须满足），用于得到光流法基本方程；

（2）小运动，这个也必须满足，就是时间的变化不会引起位置的剧烈变化，这样灰度才能对位置求偏导（换句话说，小运动情况下我们才能用前后帧之间单位位置变化引起的灰度变化去近似灰度对位置的偏导数），这也是光流法不可或缺的假定；

（3）空间一致，一个场景上邻近的点投影到图像上也是邻近点，且邻近点速度一致。这是Lucas-Kanade光流法特有的假定，因为光流法基本方程约束只有一个，而要求x，y方向的速度，有两个未知变量。我们假定特征点邻域内做相似运动，就可以连立n多个方程求取x，y方向的速度（n为特征点邻域总点数，包括该特征点）。

其他光流：

参考：光流的介绍

2.在本节的程序的求图像梯度过程中，我们简单地求了u+1 和u-1 的灰度之差除2，作为u 方向上的梯度值。这种做法有什么缺点？提示：对于距离较近的特征，变化应该较快；而距离较远的特征在图像中变化较慢，求梯度时能否利用此信息？

这种做法得到的梯度可能整体较小，因为除了角点以外，大部分像素点周围点都与之相似，从而对整体的优化求解贡献不大，可以考虑采取双边滤波的思想，在求梯度时加上距离权重与灰度权重，从而使得在求梯度时利用更多信息。

可以加上一个与深度值成比例的权重系数，对于较近的特征，速度较快，像素跨度大，人为地给它的像素梯度降低，使得求解时它的增量较少，反之，对于较远的特征，速度较慢，像素跨度小，人为地让它的像素梯度提高，使得求解时它的增量较大，这样做不仅能使得迭代加快，还能一定程度上避免进入局部最优。

3.直接法能否和光流一样，提出反向法的概念？

雅可比矩阵计算公式中修改：    为   

雅可比矩阵函数中修改代码：

//使用img1的梯度作为第二个图像的梯度
J_img_pixel = Eigen::Vector2d(
    0.5 * (GetPixelValue(img1, px_ref[i][0] + 1 + x, px_ref[i][1] + y) - GetPixelValue(img1, px_ref[i][0] - 1 + x, px_ref[i][1] + y)),
    0.5 * (GetPixelValue(img1, px_ref[i][0] + x, px_ref[i][1] + 1 + y) - GetPixelValue(img1, px_ref[i][0] + x, px_ref[i][1] - 1 + y))
);

4.使用Ceres或g2o实现稀疏直接法和半稠密直接法

 代码：HW-of-SLAMBOOK2/hw8 at main · Philipcjh/HW-of-SLAMBOOK2 · GitHubhttps://github.com/Philipcjh/HW-of-SLAMBOOK2/tree/main/hw8

Ceres：

参数块：李代数，维度是6

残差块：某个点的光度误差，维度是1

覆写残差块雅克比矩阵计算方式

创建所有点的残差块实例，添加到problem求解。

①Ceres代码中没有用之前使用过的自动求导类AutoDiffCostFunction，而是使用SizedCostFunction（指定大小的代价函数）

该类继承自CostFunction类。如果参数块和残差块的大小在编译时已知，那么用户可以把它们指定为模板参数，并且使用SizeCostFunction。这样用户只需要编程实现CostFunction::Evaluate()即可。下面是SizedCostFunction的代码：

template<int kNumResiduals,
         int N0 = 0, int N1 = 0, int N2 = 0, int N3 = 0, int N4 = 0,
         int N5 = 0, int N6 = 0, int N7 = 0, int N8 = 0, int N9 = 0>
class SizedCostFunction : public CostFunction {
 public:
  virtual bool Evaluate(double const* const* parameters,
                        double* residuals,
                        double** jacobians) const = 0;
};

 CostFunction类代码：

class CostFunction {
 public:
  virtual bool Evaluate(double const* const* parameters,
                        double* residuals,
                        double** jacobians) = 0;
  const vector<int32>& parameter_block_sizes();
  int num_residuals() const;
 
 protected:
  vector<int32>* mutable_parameter_block_sizes();
  void set_num_residuals(int num_residuals);
};

参数块的数量和大小被记录在CostFunction::parameter_block_sizes_。输出残差的个数被记录在CostFunction::num_residuals_。从此类继承的用户代码将使用相应的访问器设置这两个成员。 

CostFunction::Evaluate用于计算残差向量和雅可比矩阵，

• parameters是一个数组的数组(二维数组)。它包含了个数等于参数块个数parameter_block_sizes_.size()的子数组。每个子数组parameters[i]都存储着第i个参数块内的参数，大小等于该参数块内参数的个数parameter_block_sizes_[i]。该数组永远不为Null。
• residuals是一个大小等于残差个数num_residuals_的数组。它也永远不为Null。
• jacobians也是一个数组的数组。大小等于参数块的个数parameter_block_sizes_.size()。 如果它为Null就意味着，用户只希望计算残差。每一个元素都对应一个子数组jacobians[i]。每个子数组都是大小为num_residuals x parameter_block_sizes_[i]的行优先数组。如果某个子数组jacobians[i]不是Null，那说明用户要求计算对应parameters[i]的残差向量的雅可比矩阵，并且存在这个子数组。
• 返回值反映了计算残差或者雅可比矩阵是否成功。

②Eigen::Map       参考：eigen Map - 知乎 (zhihu.com)

Eigen中定义了一系列的vector和matrix，相比copy数据，更一般的方式是复用数据的内存，将它们转变为Eigen类型。Map类很好地实现了这个功能，可以用它避免很多不必要的内存拷贝。

它的定义是：

 template<typename PlainObjectType, int MapOptions, typename StrideType>
 class Eigen::Map< PlainObjectType, MapOptions, StrideType >

参数： 

PlainObjectType：映射后的Eigen数据类型

MapOptions：指针所指对象的内存对齐方式，可选对齐方式如下，默认为Eigen::Unaligned ( = 0)

StrideType：跨度类型，默认情况下map在数组的内存中是连续取得映射元素，可以通过该参数设置按照一定的跨度映射元素。可选的跨度有三种：Stride, InnerStride和OuterStride

OuterStride指的是，如果我们构造的矩阵为列优先（column-major），那么每两个连续的列之间的指针就会跳跃一个步幅；如果为InnerStride，那么矩阵或者向量内部每两个连续的元素之间就会跳跃一个步幅。

Eigen中Map是一个模板类，用于将顺序容器中的元素（或者说是一段连续内存）表达成Eigen中矩阵类型如Matrix或者Vector，而不会造成任何内存和时间上的开销。其操作的对象是顺序容器、数组等能获得指向一段连续内存的指针。

注意:Map没有默认的构造函数，需要传递一个指针来初始化对象。例如，定义一个float类型的矩阵： Eigen::Map<MatrixXf> mf(pf,rows,cols);   pf是一个数组指针float *。

---

---

g2o：

顶点：李代数

边：每个点都对应一条边（一元边），定义代价函数计算方式，误差计算方式，雅克比矩阵

设置求解器

连接顶点和所有边，求解。

最后从结果来看，ceres和g2o优化的结果差别不是很大。