视觉SLAM入门 -- 学习笔记 - Part6
2 LK 光流
2.1 光流文献综述
我们课上演示了Lucas-Kanade 稀疏光流，用OpenCV 函数实现了光流法追踪特征点。实际上，光流法有很长时间的研究历史，直到现在人们还在尝试用Deep learning 等方法对光流进行改进[1, 2]。本题将指导你完成基于Gauss-Newton 的金字塔光流法。首先，请阅读文献（paper 目录下提供了pdf），回答下列问题。
问题：
1. 按此文的分类，光流法可分为哪几类？
We categorize algorithms as either additive or compositional, and as either forwards or inverse.
答：加性算法(additive)和组合算法(compositional)，或正向算法(forwards)和逆算法(inverse)
2. 在compositional 中，为什么有时候需要做原始图像的wrap？该wrap 有何物理意义？
答：需要在当前位姿估计之前引入增量式 warp（incremental warp）以建立半群约束要求（the semi-group requirement）。
3. forward 和inverse 有何差别？
The key to efficiency is switching the role of the image and the template
The only differences between the forwards and inverse compositional algorithms are:
(1) the error image is computed after switching the roles of I and T
(2) Steps 3,5, and 6 use the gradient of T rather than the gradient of I and can be precomputed
(3) Eq. (35) is used to compute Δp rather than Eq. (10), and finally
(4)the incremental warp is inverted before it is composed with the current estimate in Step 9.
答：
forward方法对于输入图像进行参数化(包括仿射变换及放射增量);
inverse方法则同时输入图像参数和模板图像, 其中输入图像参数化仿射变换, 模板图像参数化仿射增量. 因此反向方法的计算量显著降低，计算效率较高.。由于图像灰度值和运动参数非线性, 整个优化过程为非线性。
forward方法和inverse方法在目标函数上不太一样，虽然都是把运动向量 p 都是跟着 I（被匹配图像），但是forward方法中的迭代的微小量 Δp 使用 I 计算，inverse方法中的 Δp 使用T计算，从而减小了计算量。
这篇文献属实太难读了，目前基础太薄弱了，暂时先不翻译论文原文部分……
2.2 forward-addtive Gauss-Newton 光流的实现
接下来我们来实现最简单的光流，即上文所说的forward-addtive。我们先考虑单层图像的LK 光流，然后再推广至金字塔图像。按照教材的习惯，我们把光流法建模成一个非线性优化问题，再使用Gauss-Newton 法迭代求解。
设有图像1.png，2.png，我们在1.png 中提取了GFTT 角点( 一种角点提取算法，没有描述子。经常与光流配合使用，但是计算量比FAST 更大)，然后希望在2.png中追踪这些关键点。设两个图分别为 I 1 I_{1} I1, I 2 I_{2} I2，第一张图中提取的点集为 P = p i P = {p_{i}} P=pi，其中 p i = [ x i ， y i ] T p_{i} = [x_{i}， y_{i}]^{T} pi=[xi，yi]T 为像素坐标值。考虑第i 个点，我们希望计算 Δ x i Δx_{i} Δxi， Δ y i Δy_{i} Δyi，满足：
即最小化二者灰度差的平方，其中 ∑ W \sum_{W} ∑W表示我们在某个窗口（Window）中求和（而不是单个像素，因为问题有两个未知量，单个像素只有一个约束，是欠定的）。
实践中，取此window 为8x8 大小的小块，即从 x i − 4 x_{i} - 4 xi−4 取到 x i + 3 x_{i} + 3 xi+3，y 坐标亦然。显然，这是一个forward-addtive 的光流，而上述最小二乘问题可以用Gauss-Newton 迭代求解。
回答下列问题，并根据你的回答，实code/optical_flow.cpp 文件中的OpticalFlowSingleLevel 函数。
1. 从最小二乘角度来看，每个像素的误差怎么定义？
答： e i = I i ( x i , y i ) − I 2 ( x i + Δ x i , y i + Δ y i ) e_{i} = I_{i}(x_{i},y_{i}) - I_{2}(x_{i} + \Delta x_{i},y_{i} +\Delta y_{i} ) ei=Ii(xi,yi)−I2(xi+Δxi,yi+Δyi)
2. 误差相对于自变量的导数如何定义？
答：可用图像2在 ( x + Δ x , y + Δ y ) (x + \Delta x,y + \Delta y ) (x+Δx,y+Δy)处的梯度定义
下面是有关实现过程中的一些提示：
- 同上一次作业，你仍然需要去除那些提在图像边界附近的点，不然你的图像块可能越过边界。
- 该函数称为单层的光流，下面我们要基于这个函数来实现多层的光流。在主函数中，我们对两张图 像分别测试单层光流、多层光流，并与OpenCV 结果进行对比。作为验证，正向单层光流结果应 该如图1 所示，它结果不是很好，但大部分还是对的。
图1: 光流结果示意图，多层光流结果应该和OpenCV 光流类似。
- 在光流中，关键点的坐标值通常是浮点数，但图像数据都是以整数作为下标的。之前我们直接取了 浮点数的整数部分，即把小数部分归零。但是在光流中，通常的优化值都在几个像素内变化，所以我们还用浮点数的像素插值。函数GetPixelValue 为你提供了一个双线性插值方法（这也是常用的图像插值法之一），你可以用它来获得浮点的像素值。
forward-addtive下代码输出：（代码见2.3）
2.3 反向法
在你实现了上述算法之后，就会发现，在迭代开始时，Gauss-Newton 的计算依赖于 I 2 I_{2} I2 在 ( x i + Δ x i , y i + Δ y i ) (x_{i}+Δx_{i}, y_{i}+Δy_{i}) (xi+Δxi,yi+Δyi)处的梯度信息。然而，角点提取算法仅保证了 I 1 ( x i , y i ) I_{1}(x_{i}, y_{i}) I1(xi,yi) 处是角点（可以认为角度点存在明显梯度），但对于 I 2 I_{2} I2，我们并没有办法假设 I 2 I_{2} I2 在 x i x_{i} xi, y i y_{i} yi 处亦有梯度，从而Gauss-Newton 并不一定成立。反向的光流法（inverse）则做了一个巧妙的技巧，即用 I 1 ( x i , y i ) I_{1}(x_{i}, y_{i}) I1(xi,yi)处的梯度，替换掉原本要计算的 I 2 ( x i + Δ x i , y i + Δ y i ) I_{2}(x_{i}+Δx_{i}, y_{i}+Δy_{i}) I2(xi+Δxi,yi+Δyi)的梯度。
这样做的好处有：
• I 1 ( x i , y i ) I_{1}(x_{i}, y_{i}) I1(xi,yi) 是角点，梯度总有意义；
• I 1 ( x i , y i ) I_{1}(x_{i}, y_{i}) I1(xi,yi) 处的梯度不随迭代改变，所以只需计算一次，就可以在后续的迭代中一直使用，节省了大量计算时间。
我们为OpticalFlowSingleLevel 函数添加一个bool inverse 参数，指定要使用正常的算法还是反向的算法。请你根据上述说明，完成反向的LK 光流法。
OpticalFlowSingleLevel（）
void OpticalFlowSingleLevel(
const Mat &img1,
const Mat &img2,
const vector<KeyPoint> &kp1,
vector<KeyPoint> &kp2,
vector<bool> &success,
bool inverse
) {
// parameters
int half_patch_size = 4;
int iterations = 10;
bool have_initial = !kp2.empty();
for (size_t i = 0; i < kp1.size(); i++) {
auto kp = kp1[i];
double dx = 0, dy = 0; // dx,dy need to be estimated
if (have_initial) {
dx = kp2[i].pt.x - kp.pt.x;
dy = kp2[i].pt.y - kp.pt.y;
}
double cost, lastCost = 0;
bool succ = true; // indicate if this point succeeded
// Gauss-Newton iterations
for (int iter = 0; iter < iterations; iter++) {
Eigen::Matrix2d H = Eigen::Matrix2d::Zero();
Eigen::Vector2d b = Eigen::Vector2d::Zero();
cost = 0;
if (kp.pt.x + dx <= half_patch_size || kp.pt.x + dx >= img1.cols - half_patch_size ||
kp.pt.y + dy <= half_patch_size || kp.pt.y + dy >= img1.rows - half_patch_size) {
// go outside
succ = false;
break;
}
// compute cost and jacobian
for (int x = -half_patch_size; x < half_patch_size; x++)
for (int y = -half_patch_size; y < half_patch_size; y++) {
// TODO START YOUR CODE HERE (~8 lines)
double error = GetPixelValue(img1, kp.pt.x+x, kp.pt.y+y) -
GetPixelValue(img2, kp.pt.x+x+dx, kp.pt.y+y+dy);
Eigen::Vector2d J; // Jacobian
if (!inverse) {
// Forward Jacobian
J = -1.0 * Eigen::Vector2d(
0.5 *(GetPixelValue(img2, kp.pt.x+x+dx+1, kp.pt.y+dy+y)-
GetPixelValue(img2, kp.pt.x+dx+x - 1, kp.pt.y+dy+y)),
0.5*(GetPixelValue(img2, kp.pt.x+dx+x, kp.pt.y+dy+y+1)-
GetPixelValue(img2, kp.pt.x+dx+x, kp.pt.y+dy+y-1))
);
} else if(iter==0){
// Inverse Jacobian
// NOTE this J does not change when dx, dy is updated, so we can store it and only compute error
J = -1.0 * Eigen::Vector2d(
0.5 *(GetPixelValue(img1, kp.pt.x+x+1, kp.pt.y+y)-
GetPixelValue(img1, kp.pt.x+x - 1, kp.pt.y+y)),
0.5*(GetPixelValue(img1, kp.pt.x+x, kp.pt.y+y+1)-
GetPixelValue(img1, kp.pt.x+x, kp.pt.y+y-1))
);
}
// compute H, b and set cost;
if(!inverse || iter==0)H += J*J.transpose();
b += - J*error;
cost = error*error;
// TODO END YOUR CODE HERE
}
// compute update
// TODO START YOUR CODE HERE (~1 lines)
Eigen::Vector2d update = H.ldlt().solve(b);
// TODO END YOUR CODE HERE
if (isnan(update[0])) {
// sometimes occurred when we have a black or white patch and H is irreversible
cout << "update is nan" << endl;
succ = false;
break;
}
if (iter > 0 && cost > lastCost) {
cout << "cost increased: " << cost << ", " << lastCost << endl;
break;
}
// update dx, dy
dx += update[0];
dy += update[1];
lastCost = cost;
succ = true;
}
success.push_back(succ);
// set kp2
if (have_initial) {
kp2[i].pt = kp.pt + Point2f(dx, dy);
} else {
KeyPoint tracked = kp;
tracked.pt += cv::Point2f(dx, dy);
kp2.push_back(tracked);
}
}
}
代码输出：
2.4 推广至金字塔
通过实验，可以看出光流法通常只能估计几个像素内的误差。如果初始估计不够好，或者图像运动太大，光流法就无法得到有效的估计（不像特征点匹配那样）。但是，使用图像金字塔，可以让光流对图像运动不那么敏感。下面请你使用缩放倍率为2，共四层的图像金字塔，实现coarse-to-fine 的LK 光流。函数在OpticalFlowMultiLevel 中。
实现完成后，给出你的光流截图（正向、反向、金字塔正向、金字塔反向），可以和OpenCV 作比较。
然后回答下列问题：
1. 所谓coarse-to-fine 是指怎样的过程？
答：以原始图像为底层，每向上一层，便对下层图像进行一定倍率的缩放，得到金字塔，计算光流时，先从顶层图像开始计算，然后把上一层的追踪结果作为下一层光流的初始值。因为上层图像缩放了很多倍，最为粗超，所以称为由粗至精（coarse-to-fine）
2. 光流法中的金字塔用途和特征点法中的金字塔有何差别？
答：光流法中的图像金字塔通过逐层迭代寻找最佳的关键点位置和光流方向，目的在解决光流在快速运动中难以检测的问题。(p215)
特征点法中的图像金字塔通过检测每一层上的角点来增添尺度的描述，通过匹配不同层上的图像解决了特征点的尺度不变性。(p156)
提示：你可以使用上面写的单层光流来帮助你实现多层光流。
OpticalFlowMultiLevel（）
void OpticalFlowMultiLevel(
const Mat &img1,
const Mat &img2,
const vector<KeyPoint> &kp1,
vector<KeyPoint> &kp2,
vector<bool> &success,
bool inverse) {
// parameters
int pyramids = 4;
double pyramid_scale = 0.5;
double scales[] = {
1.0, 0.5, 0.25, 0.125};
// create pyramids
vector<Mat> pyr1(pyramids), pyr2(pyramids); // image pyramids
pyr1[0] = img1;
pyr2[0] = img2;
// TODO START YOUR CODE HERE (~8 lines)
for (int i = 1; i < pyramids; i++) {
Mat img1_pyr, img2_pyr;
cv::resize(img1, img1_pyr,
cv::Size((int)(pyr1[i-1].cols * pyramid_scale), (int)(pyr1[i-1].rows * pyramid_scale)));
cv::resize(img2, img2_pyr,
cv::Size((int)(pyr2[i-1].cols * pyramid_scale), (int)(pyr2[i-1].rows * pyramid_scale)));
pyr1[i] = img1_pyr;
pyr2[i] = img2_pyr;
}
// TODO END YOUR CODE HERE
// coarse-to-fine LK tracking in pyramids
// TODO START YOUR CODE HERE
vector<KeyPoint> kpts1_pyr, kpts2_pyr;
for(auto &kp:kp1){
auto kp_top = kp;
kp_top.pt *=scales[pyramids-1];
kpts1_pyr.emplace_back(kp_top);
kpts2_pyr.emplace_back(kp_top);
}
for (int level = pyramids-1; level >0 ; level--) {
success.clear();
OpticalFlowSingleLevel(pyr1[level], pyr2[level], kpts1_pyr,kpts2_pyr, success, inverse);
if(level>0){
for (int i = 0; i < kpts1_pyr.size(); ++i) {
kpts1_pyr[i].pt /= pyramid_scale;
kpts2_pyr[i].pt /= pyramid_scale;
}
}
}
for(auto &kp:kpts2_pyr){
kp2.emplace_back(kp);
}
// TODO END YOUR CODE HERE
// don't forget to set the results into kp2
}
2.5 讨论
现在你已经自己实现了光流，看到了基于金字塔的LK 光流能够与OpenCV 达到相似的效果（甚至更好）。根据光流的结果，你可以和上讲一样，计算对极几何来估计相机运动。下面针对本次实验结果，谈谈你对下面问题的看法：
• 我们优化两个图像块的灰度之差真的合理吗？哪些时候不够合理？你有解决办法吗？
不合理，一般情况下由于相机的曝光、环境光影变化、物体材质等，何难保证图像灰度保持不变。另外当运动过快的时候，灰度变化大，光流难以追踪。
解决办法：前者暂未想到解决办法，后者可以建立光流金字塔
• 图像块大小是否有明显差异？取16x16 和8x8 的图像块会让结果发生变化吗？
建立图像金字塔时，窗口固定，在每一层金字塔上都用同样大小的窗口进行计算所以图像块大小不会有明显差异
在使用单层光流时，大窗口增大光流鲁棒性，小窗口增强光流精确性。
• 金字塔层数对结果有怎样的影响？缩放倍率呢？
金字塔层数一般越多越好，但在4-5层之后，图像会变得很小，特征点像素会因太过紧密而容易引起误追踪。
缩放倍率减小，金字塔层数增加，迭代层数增加，效果理应变好
3 直接法
3.1 单层直接法
我们说直接法是光流的直观拓展。在光流中，我们估计的是每个像素的平移（在additive 的情况下）。而在直接法当中，我们最小化光流误差，来估计相机的旋转和平移（以李代数的形式）。现在我们将使用和前一个习题非常相似的做法来实现直接法，请同学体现二者之间的紧密联系。
本习题中，你将使用Kitti 数据集中的一些图像。给定left.png 和disparity.png，我们知道，通过这两个图可以得到left.png 中任意一点的3D 信息。现在，请你使用直接法，估计图像000001.png 至000005.png的相机位姿。我们称 left.png 为参考图像（reference，简称ref），称000001.png -000005.png 中任意一图为当前图像（current，简称cur），如图2 所示。
图2: 本题中待估计位姿的图像
设待估计的目标为 T c u r , r e f T_{cur,ref} Tcur,ref，那么在ref 中取一组点 p i {p_{i}} pi，位姿可以通过最小化下面的目标函数求解：
其中N 为点数， π \pi π函数为针孔相机的投影函数 R 3 ↦ R 2 R^{3}\mapsto R^{2} R3↦R2， W i W_{i} Wi 为第i 个点周围的小窗口。同光流法，该问题可由Gauss-Newton 函数求解。请回答下列问题，然后实现code/direct_method.cpp 中的DirectPoseEstimationSingleLayer函数。
1. 该问题中的误差项是什么？
答： e （ T ） = I r e f ( π ( p ) ) − I c u r ( π ( T c u r , r e f p ) ) e（T） = I_{ref}(\pi(p)) - I_{cur}(\pi(T_{cur,ref}p)) e（T）=Iref(π(p))−Icur(π(Tcur,refp))
2. 误差相对于自变量的雅可比维度是多少？如何求解？
答：首先要确定咱们的自变量是 相机的李代数姿态ξ（6自由度），然后下面算雅可比：
令 q = T c u r , r e f p q =T_{cur,ref}p q=Tcur,refp ， u = π ( q ) = 1 Z K q u = \pi(q) = \frac{1}{Z}Kq u=π(q)=Z1Kq根据视觉SLAM入门 – 学习笔记 - Part5 —— 4 用G-N 实现Bundle Adjustment 中的位姿估计的第二问得到
那么自变量的雅可比有 J = ∂ e ∂ T = − ∂ I c u r ∂ u ∂ u ∂ q ∂ q ∂ δ ξ = − ∂ I c u r ∂ u ∂ u ∂ δ ξ J = \frac{\partial e}{\partial T} = -\frac{\partial I_{cur}}{\partial u}\frac{\partial u}{\partial q}\frac{\partial q}{\partial \delta \xi }= -\frac{\partial I_{cur}}{\partial u}\frac{\partial u}{\partial \delta \xi } J=∂T∂e=−∂u∂Icur∂q∂u∂δξ∂q=−∂u∂Icur∂δξ∂u
∂ I c u r ∂ u \frac{\partial I_{cur}}{\partial u} ∂u∂Icur为1x2， ∂ u ∂ δ ξ \frac{\partial u}{\partial \delta \xi } ∂δξ∂u为2x6，
所以 J J J为 1x6
3. 窗口可以取多大？是否可以取单个点？
答：
① 窗口可以取3x3, 4x4等大小；
② 可以取单个点但会降低鲁棒性
下面是一些实现过程中的提示：
- 这次我们在参考图像中随机取1000 个点，而不是取角点。请思考为何不取角点，直接法也能工作。
- 由于相机运动，参考图像中的点可能在投影之后，跑到后续图像的外部。所以最后的目标函数要对投影在内部的点求平均误差，而不是对所有点求平均。程序中我们以good 标记出投影在内部的点。
- 单层直接法的效果不会很好，但是你可以查看每次迭代的目标函数都会下降。
DirectPoseEstimationSingleLayer（）
void DirectPoseEstimationSingleLayer(
const cv::Mat &img1,
const cv::Mat &img2,
const VecVector2d &px_ref,
const vector<double> &depth_ref,
Sophus::SE3d &T21
) {
// parameters
int half_patch_size = 4;
int iterations = 100;
double cost = 0, lastCost = 0;
int nGood = 0; // good projections
VecVector2d goodProjection;
VecVector2d goodref;
for (int iter = 0; iter < iterations; iter++) {
nGood = 0;
goodProjection.clear();
goodref.clear();
// Define Hessian and bias
Matrix6d H = Matrix6d::Zero(); // 6x6 Hessian
Vector6d b = Vector6d::Zero(); // 6x1 bias
for (size_t i = 0; i < px_ref.size(); i++) {
// compute the projection in the second image
// TODO START YOUR CODE HERE
Eigen::Vector3d point_ref = depth_ref[i] *
Eigen::Vector3d ((px_ref[i][0]-cx)/fx, (px_ref[i][1]-cy)/fy, 1);
Eigen::Vector3d point_cur = T21 * point_ref;
if(point_cur[2]<0) {
continue;}
float u = fx * point_cur[0]/point_cur[2] + cx, v = fy*point_cur[1]/point_cur[2] + cy;
if(u < half_patch_size || u > img2.cols - half_patch_size || v < half_patch_size ||
v > img2.rows - half_patch_size){
continue;
}
nGood++;
goodProjection.emplace_back(Eigen::Vector2d(u, v));
goodref.emplace_back(Eigen::Vector2d(px_ref[i][0], px_ref[i][1]));
// and compute error and jacobian
double X = point_cur[0], Y = point_cur[1], Z = point_cur[2];
for (int x = -half_patch_size; x < half_patch_size; x++)
for (int y = -half_patch_size; y < half_patch_size; y++) {
double error =GetPixelValue(img1, px_ref[i][0]+x, px_ref[i][1]+y) -
GetPixelValue(img2, u+x, v+y);
Matrix26d J_pixel_xi; // pixel to \xi in Lie algebra
Eigen::Vector2d J_img_pixel; // image gradients
J_pixel_xi << fx/Z, 0, -fx*X/(Z*Z), -fx*X*Y/(Z*Z), fx+fx*X*X/(Z*Z), -fx*Y/Z,
0, fy/Z, -fy*Y/(Z*Z), -fy-fy*Y*Y/(Z*Z), fy*X*Y/(Z*Z), fy*X/Z;
J_img_pixel = Eigen::Vector2d (
0.5*(GetPixelValue(img2, u+1+x, v+y) - GetPixelValue(img2, u-1+x, v+y)),
0.5*(GetPixelValue(img2, u+x, v+1+y) - GetPixelValue(img2,u+x, v-1+y))
);
// total jacobian
Vector6d J= -1.0 * (J_img_pixel .transpose()* J_pixel_xi).transpose();
H += J * J.transpose();
b += -error * J;
cost += error * error;
}
// END YOUR CODE HERE
}
// solve update and put it into estimation
// TODO START YOUR CODE HERE
Vector6d update;
update = H.ldlt().solve(b);
T21 = Sophus::SE3d::exp(update) * T21;
// END YOUR CODE HERE
cost /= nGood;
if (isnan(update[0])) {
// sometimes occurred when we have a black or white patch and H is irreversible
cout << "update is nan" << endl;
break;
}
if (iter > 0 && cost > lastCost) {
cout << "cost increased: " << cost << ", " << lastCost << endl;
break;
}
if(update.norm() < 1e-3){
// converge
break;
}
lastCost = cost;
cout << "iteration " << iter << " cost = " << cost << ", good = " << nGood << endl;
}
cout << "good projection: " << nGood << endl;
cout << "T21 = \n" << T21.matrix() << endl;
// in order to help you debug, we plot the projected pixels here
cv::Mat img1_show, img2_show;
cv::cvtColor(img1, img1_show, CV_GRAY2BGR);
cv::cvtColor(img2, img2_show, CV_GRAY2BGR);
// for (auto &px: px_ref) {
// cv::rectangle(img1_show, cv::Point2f(px[0] - 2, px[1] - 2), cv::Point2f(px[0] + 2, px[1] + 2),
// cv::Scalar(0, 250, 0));
// }
// for (auto &px: goodProjection) {
// cv::rectangle(img2_show, cv::Point2f(px[0] - 2, px[1] - 2), cv::Point2f(px[0] + 2, px[1] + 2),
// cv::Scalar(0, 250, 0));
// }
for (int i = 0; i < goodref.size(); ++i) {
auto p_ref = goodref[i];
cv::rectangle(img1_show, cv::Point2f(p_ref[0] - 2, p_ref[1] - 2),
cv::Point2f(p_ref[0] + 2, p_ref[1] + 2),
cv::Scalar(0, 250, 0));
auto p_cur = goodProjection[i];
if(p_cur[0]>0 && p_cur[1]>0){
cv::circle(img2_show, cv::Point2f(p_cur[0], p_cur[1]),
2, cv::Scalar(0,250,0), 2);
cv::line(img2_show, cv::Point2f(p_ref[0], p_ref[1]), cv::Point2f(p_cur[0], p_cur[1]),
cv::Scalar(0,250,0));
}
}
// cv::imwrite("reference.bmp", img1_show);
// cv::imwrite("current.bmp", img2_show);
cv::imshow("reference", img1_show);
cv::imshow("current", img2_show);
cv::waitKey(0);
}
代码输出：
reference
000001.png
000002.png
000003.png
000004.png
000005.png
3.2 多层直接法
下面，类似于光流，我们也可以把直接法以coarse-to-fine 的过程，拓展至多层金字塔。多层金字塔的直接法允许图像在发生较大运动时仍能追踪到所有点。下面我们使用缩放倍率为2 的四层金字塔，实现金字塔上的直接法。请实现DirectPoseEstimationMultiLayer 函数，下面是一些提示：
- 在缩放图像时，图像内参也需要跟着变化。那么，例如图像缩小一倍，fx, fy, cx, cy 应该如何变化？
- 根据coarse-to-fine 的过程，上一层图像的位姿估计结果可以作为下一层图像的初始条件。
- 在调试期间，可以画出每个点在ref 和cur 上的投影，看看它们是否对应。若准确对应，则说明位 姿估计是准确的。
作为验证，图像000001 和000005 的位姿平移部分应该接近(根据各人实现不同，此题结果可能有所差异，但不应该大于0.5 米)：
可以看出车辆基本是笔直向前开的。
DirectPoseEstimationMultiLayer（）
void DirectPoseEstimationMultiLayer(
const cv::Mat &img1,
const cv::Mat &img2,
const VecVector2d &px_ref,
const vector<double> &depth_ref,
Sophus::SE3d &T21
) {
// parameters
int pyramids = 4;
double pyramid_scale = 0.5;
double scales[] = {
1.0, 0.5, 0.25, 0.125};
// create pyramids
vector<cv::Mat> pyr1(pyramids), pyr2(pyramids); // image pyramids
// TODO START YOUR CODE HERE
pyr1[0] = img1;
pyr2[0] = img2;
for (int i = 1; i < pyramids; ++i) {
cv::resize(img1, pyr1[i],
cv::Size(pyr1[i-1].cols*pyramid_scale, pyr1[i-1].rows*pyramid_scale));
cv::resize(img2, pyr2[i],
cv::Size(pyr2[i-1].cols*pyramid_scale, pyr2[i-1].rows*pyramid_scale));
}
// END YOUR CODE HERE
double fxG = fx, fyG = fy, cxG = cx, cyG = cy; // backup the old values
for (int level = pyramids - 1; level >= 0; level--) {
VecVector2d px_ref_pyr; // set the keypoints in this pyramid level
for (auto &px: px_ref) {
px_ref_pyr.push_back(scales[level] * px);
}
// TODO START YOUR CODE HERE
// scale fx, fy, cx, cy in different pyramid levels
fx = fxG * scales[level];
fy = fyG * scales[level];
cx = cxG * scales[level];
cy = cyG * scales[level];
// END YOUR CODE HERE
DirectPoseEstimationSingleLayer(pyr1[level], pyr2[level], px_ref_pyr, depth_ref, T21);
}
}
代码输出：
000001.png
后面还有000002.png - 000005.png, 跟上面类似，就不贴了……
3.3 * 延伸讨论
现在你已经实现了金字塔上的Gauss-Newton 直接法。你可以调整实验当中的一些参数，例如图像点数、每个点周围小块的大小等等。请思考下面问题：
1. 直接法是否可以类似光流，提出inverse, compositional 的概念？它们有意义吗？
答：可以类似。光流法中优化的是dx、dy，inverse使用参考图片在(x, y)处的梯度代替目标图片可以减少计算量通过增量的表示方式来区分方法； 迭代更新运动参数的时候，如果迭代的结果是在原始的值(6个运动参数)上增加一个微小量，那么称之为Additive，如果在仿射矩阵上乘以一个矩阵(增量运动参数形成的增量仿射矩阵)，这方法称之为Compositional。(the compositional approach iteratively solves for an incremental warp W (x ; Δ p ) \bm W(\bm x; \Delta \bm p)W(x;Δp) rather than an additive update to the parameters Δ p \Delta \bm pΔp.)（讲解：Compositional即同时估计仿射变换参数）
但直接法的inverse, compositional 似乎没有什么实际意义。
2. 请思考上面算法哪些地方可以缓存或加速？
答：可以针对每一个像素点并行计算其光流
3. 在上述过程中，我们实际假设了哪两个patch 不变？
答：假设了对应点处的灰度值以及深度信息不变
4. 为何可以随机取点？而不用取角点或线上的点？那些不是角点的地方，投影算对了吗？
答：
① 因为直接法是通过对比图像灰度值匹配的，并不需要角点的描述子进行匹配，因而可以随机取点。
② 非角点的地方投影在误差允许范围内
5. 请总结直接法相对于特征点法的异同与优缺点。
答： 优点： p230 ① 可省去计算特征点、描述子的时间 ② 只要求像素有梯度，不要求特征点。 ③ 可以构建半稠密乃至稠密的地图
缺点：p231 ① 非凸性。 ② 单个像素没有区分度 ③ 灰度值不变是很强的假设
4 * 使用光流计算视差
在上一题中我们已经实现了金字塔LK 光流。光流有很多用途，它给出了两个图像中点的对应关系，所以我们可以用光流进行位姿估计，或者计算双目的视差。回忆第四节课的习题中，我们介绍了双目可以通过视差图得出点云，但那时直接给出了视差图，而没有进行视差图的计算。现在，给定图像left.png, right.png，请你使用上题的结果，计算left.png 中的GFTT 点在right.png 中的（水平）视差，然后与disparity.png进行比较。这样的结果是一个稀疏角点组成的点云。请计算每个角点的水平视差，然后对比视差图比较结果。
本程序不提供代码框架，请你根据之前习题完成此内容。
#include 代码输出：
参考：
https://blog.csdn.net/hitljy/article/details/107145834
https://github.com/gaoxiang12/slambook2
高翔 张涛等 《视觉SLAM十四讲 第二版》