视觉里程计2 高翔

视觉里程计2

直接法根据像素的亮度信息估计相机的运动，可以完全不用计算关键点和描述子，能够避免特征的计算时间，特征缺失的情况，只要常见中存在明暗变化（可以渐变，不形成局部的图像梯度），直接法就能工作。
直接法分为稀疏，稠密和半稠密三种。稀疏光流以Lucas-Kanade光流为代表（LK光流）

LK光流实践部分

• TUM数据集定义的格式
  – rgb.txt和depth.txt记录了各文件的采集时间和对应的文件名。
  – rgb 和 depth目录存放着采集到的PNG格式图像文件。彩色图像为8位3通道，深度图为16位单通道图像。文件名即采集时间。
  – groundtruth.txt为外部运动捕捉系统采集到相机位姿，格式为
  （time, tx, ty, tz, qx, qy, qz, qw）
  应当注意的是彩色图、深度图、和标准轨迹的采集都是独立的，轨迹的采集频率比图像高很多。在使用数据前，需要根据根据采集时间对数据进行一次时间上的对齐，以便对彩色图和深度图进行配对。原则上，可以把采集时间相近于一个阈值的数据，看成是一对图像。并把相近时间的位姿，看作是该图像的真实采集位置。
  需要在数据集中进行如下操作：

python associate.py rgb.txt depth.txt > associate.txt

这段脚本会根据输入的两个文件中的采集时间进行配对，最后输出到文件associate.txt。输出文件含有配对后的两幅图像的时间，文件名信息，可以作为后续处理的来源。

LK光流程序

该程序的主要任务：使用LK的目的是跟踪特征点。对第一幅图像提取FAST角点，然后用LK光流跟踪它们，并画在图 中。

#include <iostream>
#include <fstream>
#include <list>
#include <vector>
#include <chrono>

using namespace std;

#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/video/tracking.hpp>

int main(int argc, char** argv) {

    if ( argc != 2)
    {
        cout << "usage: useLK path_to_dataset" << endl;
        return 1;
    }
    string path_to_dataset = argv[1];
    string associate_file = path_to_dataset + "/associate.txt";
    ifstream fin( associate_file );
    string rgb_file, depth_file, time_rgb, time_depth;
    list< cv::Point2f > keypoints; //因为要删除跟踪失败的点，使用list
    cv::Mat color, depth, last_color;
    for (int index = 0; index < 100; index++)
    {
        fin >> time_rgb >> rgb_file >> time_depth >> depth_file;
        color = cv::imread(path_to_dataset + "/" + rgb_file);
        depth = cv::imread(path_to_dataset + "/" + depth_file, -1);
        if(index == 0)
        {
            //--对第一帧提取FAST特征点
            vector<cv::KeyPoint> kps;
            cv::Ptr<cv::ORB> orb = cv::ORB::create();
            orb -> 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<cv::Point2f> next_keypoints;
        vector<cv::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();
        cv::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;
        }
        // 画出keypoints
        cv::Mat img_show = color.clone();
        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;
}

容器的比较

请查看该链接

直接法

---

直接法的思路是根据当前相机的位姿估计值寻找p2的位置

若相机位姿不够好，p2的外观和p1会有明显的差别，为了减小这个差别，进行优化相机的位姿，来寻找与p1更相似的p2。这同样可以通过一个优化问题完成，但此时最小化的不是重投影误差，而是光度误差。
程序没有搞明白，先不写了·