视觉里程计(三)

1.特征点法分析及改进

特征法在视觉里程计中虽占主流，但是依旧有其缺点：
1.关键点的提取与描述子的计算非常耗时。
2.使用特征点时，忽略了出特征以外的所有信息。一副图像有几十万个像素，而特征点只有几百个，可能忽略了大部分有用的图像。
3.相机会运动到特征缺失的地方，如一堵墙一个空荡荡的走廊等。

解决思路：
1.保留特征点，但只计算关键点，不计算描述子，同时使用光流法跟踪特征点运动。
2.计算关键点，不计算描述子，使用直接法来计算特征点在下一幅图像的位置。
3，不计算关键点也不计算描述子，直接根据像素灰度计算相机运动。
第一种方法，将描述子换成光流法跟踪，估计相机运动还是用对积几何、PnP或ICP。
第二种方法，根据图像的—像素灰度信息—计算相机运动，称为直接法。

根据使用像素的数量，直接法分为：稀疏、稠密、半稠密三种。特征点法：只能重构稀疏特征点法，直接法还具有恢复稠密和半稠密结构的能力。

直接法是从光流演变而来的。光流描述了像素在图像中的运动，而直接法则附带一个相机运动模型。

2.光流法

光流是一种描述像素随时间在图像之间运动的方法。 随时间的流逝，同一个像素会在图像中运动，追踪其运动。计算部分像素运动称为稀疏光流，计算所有像素称为稠密光流。LK光流是典型的稀疏光流，且可以在SLAM中用于跟踪特征点位置。

通过像素在俩图像间，分别在x轴y轴的速度，以及同个像素灰度不变得到下一个特征点。

—LK光流(稀疏光流)—-

来自相机的图像是随时间变化的，图像是时间的函数：I (t),在t时刻，位于(x,y)的像素，灰度为 I (x,y,t).把图像看做是位置和时间的函数，值域就是图像中像素的灰度。

当一个空间点，在t时刻的像素坐标为(x,y)，那么我们需要知道在其他时刻该点在图像上的像素坐标(x1,y1).

引入光流法的基本假设：灰度不变假设：同一个空间点的像素灰度值，在各个图像中是固定不变的。

在基于假设的前提下有方程：$I ( x+dx,y+dx,t+dt）=I (x,y,t)$

对上述进行泰勒展开，得到公式有：$\frac{\partial I}{\partial x}dx+\frac{\partial I}{\partial y}dy+\frac{\partial I}{\partial t}dt=0$

对以上等式分别除以 dt 得到：$\frac{\partial I}{\partial x}\frac{dx}{dt}+\frac{\partial I}{\partial y}\frac{dy}{dt}=-\frac{\partial I}{\partial t}$

其中 $\frac{dy}{dt}"$为像素在轴y的速度，$\frac{\partial I}{\partial x}$为灰度在x轴的梯度，另一下为y轴的梯度。

由于同一张图片像素的运动是一致的，即认为在x轴和y轴有一致的运动速度，公式为：$\begin{bmatrix} I_{x}& I_{y} \end{bmatrix}\begin{bmatrix} u\\ v \end{bmatrix}=-I_{t}$，这里面的元素代替了上述商式，如$I_{x}=\frac{\partial I}{\partial x}$

假设一个$w^{2}"$数量的像素，则有公式：$A=\begin{bmatrix} \begin{bmatrix} I _{x}& I _{y} \end{bmatrix}_{1}\\ \vdots \\ \begin{bmatrix} I _{x}& I _{y} \end{bmatrix}_{k} \end{bmatrix}$ , $b=\begin{bmatrix} I_{t1}\\ \vdots \\ I_{tk} \end{bmatrix}$ , 于是有：$A\begin{bmatrix} u\\ v \end{bmatrix}=-b$, 这是一个关于u，v的超定线性方程，则得到一个最小二乘问题：$\begin{bmatrix} u\\ v \end{bmatrix}^{*}=-(A^{T}A)^{-1}A^{T}b$ , 没有零解，优化解。在SLAM中，LK光流被用来跟踪角点运动。

TUM数据集：由慕尼黑工业大学(TUM)提供的公开RGB-D数据集。
1.rgb.txt 和 depth.txt 记录了各个文件的采集时间和对应的文件名。
2.rbg/和depth/目录存放着采集到的PNG格式图像文件。彩色为8位3通道，深度图为16位单通道，文件名为采集时间。
3,。groundtrhth.txt为相机位姿，格式为(time,tx,ty,tz,qx,qy,qz,qw)

在这里我们贴出代码并对其进行分析

int main(int argc, char **argv)
{
   string path_to_dataset="/home/hansry/Slam_Book/src/Test_lk/data";
   string associate_file = path_to_dataset+"/associate.txt";
   ifstream fin(associate_file);
   if(!fin)
   {
     cout<<"i cant find associate.txt!"<<endl;
     return 1;
   }

   string rgb_file,depth_file,time_rgb,time_depth;
   list<Point2f> keypoints;
   Mat color,depth,last_color;

   for(int index=0;index<100;index++)
   {
     fin>>time_rgb>>rgb_file>>time_depth>>depth_file;
     color = imread(path_to_dataset+"/"+rgb_file);
     depth = imread(path_to_dataset+"/"+depth_file,-1);
     if(index == 0)
     {
       // 对第一帧提取FAST特征点
       vector<KeyPoint> kps;
       Ptr<FastFeatureDetector> detector = FastFeatureDetector::create("FAST");//用的是FAST关键点
       detector->detect(color,kps);
       for (auto kp:kps)
         keypoints.push_back(kp.pt);
       last_color=color;
       continue;
     }

     if(color.data==nullptr||depth.data==nullptr)
       continue;

     // 对其他帧用LK跟踪特征点
     vector<Point2f>  next_keypoints;
     vector<Point2f>  prev_keypoints;
     for (auto kp:keypoints)
       prev_keypoints.push_back(kp); //为第一帧提取的关键点
     vector<unsigned char> status;
     vector<float> error;
     chrono::steady_clock::time_point t1=chrono::steady_clock::now();
  calcOpticalFlowPyrLK(last_color,color,prev_keypoints,next_keypoints,status,error);
     chrono::steady_clock::time_point t2=chrono::steady_clock::now();
     chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>( t2-t1 );
     cout<<"LK Flow use time："<<time_used.count()<<" seconds."<<endl;

     // 把跟丢的点删掉
     int i=0;
     for(auto iter=keypoints.begin();iter!=keypoints.end();i++)
     {
       if(status[i]==0)
       {
         iter=keypoints.erase(iter);
         continue;
       }
       *iter=next_keypoints[i];
       iter++;
     }
     cout<<"tracked Keypoints:"<<keypoints.size()<<endl;
     if(keypoints.size()==0)
     {
       cout<<"all keypoints are lost."<<endl;
       break;
     }

     Mat img_show=color.clone();
     //for(auto kp:keypoints),keypoints是一个序列，kp是一个用于访问keypoints中基本元素的变量，每次迭代都会用keypoints中的下一个元素来初始化s；
     for ( auto kp:keypoints )
                cv::circle(img_show, kp, 10, cv::Scalar(0, 240, 0), 1);
            cv::imshow("corners", img_show);
            cv::waitKey(0);
    last_color = color;
   }
   return 0;
}

通过运行程序之后我们可得，位于物体角点处的特征更加稳定。边缘处的特征会沿着边缘“滑动”，主要是沿着边缘移动时特征块的内容基本不变。一般而言，LK光流实际上只会碰到特征点跟丢的情况，而较少出现误匹配。

光流：计算量小，会失去特征点，但是误匹配程度低，要求相机的运动是微小的。
描述子：较大的运动可以识别出，容易误匹配。