采用特征点法做VO存在耗时较大的问题，一般耗时情况：如下

        (1) 在图像中提取特征点并计算特征描述， 非常耗时  ~10+ms  ORB，shift耗时更多；
        (2) 在不同图像中寻找特征匹配，                非常耗时   O(n^2) 暴力匹配
        (3) 利用匹配点信息计算相机位姿，             比较快速<1ms

        因此，为了速度还有不使用特征匹配计算VO的方法，主要有两大类，分别是：

        (1) 通过其他方式寻找配对点;   光流法
                稀疏光流法：以Lucas-Kanade(LK)光流为代表
                稠密光流法：以Horn-Schunk(HS)光流为代表
        本质上是估计像素在不同时刻图像中的运动。
        光流法推导过程依赖灰度不变假设：
        t时刻位于x,y处像素点的灰度值为
        时刻位于x+dx,y+dy处的像素点灰度值为
        对t+dt时刻进行泰勒一级展开如下：
        ，由于灰度值两个时刻相等，所以可得：
，所以这里是希望求解dx/dt和dy/dt。

        注意：灰度不变是一种理想假设，对于高光、阴影等情况很可能不成立。

        由于上述的光流公式是一个二元一次方程，欠定的，所以需要引入额外的约束。假定一个窗口(w*w)内光度不变，则可以构建矩阵：

        通过超定最小二乘求解运动u，v

        ,

        

      具体代码如下：

//
// Created by Xiang on 2017/12/19.
//#include <opencv2/opencv.hpp>
#include <string>
#include <chrono>
#include <Eigen/Core>
#include <Eigen/Dense>using namespace std;
using namespace cv;string file_1 = "./LK1.png";  // first image
string file_2 = "./LK2.png";  // second image/// Optical flow tracker and interface
class OpticalFlowTracker {
public:OpticalFlowTracker(const Mat &img1_,const Mat &img2_,const vector<KeyPoint> &kp1_,vector<KeyPoint> &kp2_,vector<bool> &success_,bool inverse_ = true, bool has_initial_ = false) :img1(img1_), img2(img2_), kp1(kp1_), kp2(kp2_), success(success_), inverse(inverse_),has_initial(has_initial_) {}void calculateOpticalFlow(const Range &range);private:const Mat &img1;const Mat &img2;const vector<KeyPoint> &kp1;vector<KeyPoint> &kp2;vector<bool> &success;bool inverse = true;bool has_initial = false;
};/*** single level optical flow* @param [in] img1 the first image* @param [in] img2 the second image* @param [in] kp1 keypoints in img1* @param [in|out] kp2 keypoints in img2, if empty, use initial guess in kp1* @param [out] success true if a keypoint is tracked successfully* @param [in] inverse use inverse formulation?*/
void OpticalFlowSingleLevel(const Mat &img1,const Mat &img2,const vector<KeyPoint> &kp1,vector<KeyPoint> &kp2,vector<bool> &success,bool inverse = false,bool has_initial_guess = false
);/*** multi level optical flow, scale of pyramid is set to 2 by default* the image pyramid will be create inside the function* @param [in] img1 the first pyramid* @param [in] img2 the second pyramid* @param [in] kp1 keypoints in img1* @param [out] kp2 keypoints in img2* @param [out] success true if a keypoint is tracked successfully* @param [in] inverse set true to enable inverse formulation*/
void OpticalFlowMultiLevel(const Mat &img1,const Mat &img2,const vector<KeyPoint> &kp1,vector<KeyPoint> &kp2,vector<bool> &success,bool inverse = false
);/*** get a gray scale value from reference image (bi-linear interpolated)* @param img* @param x* @param y* @return the interpolated value of this pixel*/inline float GetPixelValue(const cv::Mat &img, float x, float y) {// boundary checkif (x < 0) x = 0;if (y < 0) y = 0;if (x >= img.cols - 1) x = img.cols - 2;if (y >= img.rows - 1) y = img.rows - 2;float xx = x - floor(x);float yy = y - floor(y);int x_a1 = std::min(img.cols - 1, int(x) + 1);int y_a1 = std::min(img.rows - 1, int(y) + 1);return (1 - xx) * (1 - yy) * img.at<uchar>(y, x)+ xx * (1 - yy) * img.at<uchar>(y, x_a1)+ (1 - xx) * yy * img.at<uchar>(y_a1, x)+ xx * yy * img.at<uchar>(y_a1, x_a1);
}int main(int argc, char **argv) {// images, note they are CV_8UC1, not CV_8UC3Mat img1 = imread(file_1, 0);Mat img2 = imread(file_2, 0);// key points, using GFTT here.vector<KeyPoint> kp1;Ptr<GFTTDetector> detector = GFTTDetector::create(500, 0.01, 20); // maximum 500 keypointsdetector->detect(img1, kp1);// now lets track these key points in the second image// first use single level LK in the validation picturevector<KeyPoint> kp2_single;vector<bool> success_single;OpticalFlowSingleLevel(img1, img2, kp1, kp2_single, success_single);// then test multi-level LKvector<KeyPoint> kp2_multi;vector<bool> success_multi;chrono::steady_clock::time_point t1 = chrono::steady_clock::now();OpticalFlowMultiLevel(img1, img2, kp1, kp2_multi, success_multi, true);chrono::steady_clock::time_point t2 = chrono::steady_clock::now();auto time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);cout << "optical flow by gauss-newton: " << time_used.count() << endl;// use opencv's flow for validationvector<Point2f> pt1, pt2;for (auto &kp: kp1) pt1.push_back(kp.pt);vector<uchar> status;vector<float> error;t1 = chrono::steady_clock::now();cv::calcOpticalFlowPyrLK(img1, img2, pt1, pt2, status, error);t2 = chrono::steady_clock::now();time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);cout << "optical flow by opencv: " << time_used.count() << endl;// plot the differences of those functionsMat img2_single;cv::cvtColor(img2, img2_single, CV_GRAY2BGR);for (int i = 0; i < kp2_single.size(); i++) {if (success_single[i]) {cv::circle(img2_single, kp2_single[i].pt, 2, cv::Scalar(0, 250, 0), 2);cv::line(img2_single, kp1[i].pt, kp2_single[i].pt, cv::Scalar(0, 250, 0));}}Mat img2_multi;cv::cvtColor(img2, img2_multi, CV_GRAY2BGR);for (int i = 0; i < kp2_multi.size(); i++) {if (success_multi[i]) {cv::circle(img2_multi, kp2_multi[i].pt, 2, cv::Scalar(0, 250, 0), 2);cv::line(img2_multi, kp1[i].pt, kp2_multi[i].pt, cv::Scalar(0, 250, 0));}}Mat img2_CV;cv::cvtColor(img2, img2_CV, CV_GRAY2BGR);for (int i = 0; i < pt2.size(); i++) {if (status[i]) {cv::circle(img2_CV, pt2[i], 2, cv::Scalar(0, 250, 0), 2);cv::line(img2_CV, pt1[i], pt2[i], cv::Scalar(0, 250, 0));}}cv::imshow("tracked single level", img2_single);cv::imshow("tracked multi level", img2_multi);cv::imshow("tracked by opencv", img2_CV);cv::waitKey(0);return 0;
}void OpticalFlowSingleLevel(const Mat &img1,const Mat &img2,const vector<KeyPoint> &kp1,vector<KeyPoint> &kp2,vector<bool> &success,bool inverse, bool has_initial) {kp2.resize(kp1.size());success.resize(kp1.size());OpticalFlowTracker tracker(img1, img2, kp1, kp2, success, inverse, has_initial);parallel_for_(Range(0, kp1.size()),std::bind(&OpticalFlowTracker::calculateOpticalFlow, &tracker, placeholders::_1));
}void OpticalFlowTracker::calculateOpticalFlow(const Range &range) {// parametersint half_patch_size = 4;int iterations = 10;for (size_t i = range.start; i < range.end; i++) {auto kp = kp1[i];double dx = 0, dy = 0; // dx,dy need to be estimatedif (has_initial) {dx = kp2[i].pt.x - kp.pt.x;dy = kp2[i].pt.y - kp.pt.y;}double cost = 0, lastCost = 0;bool succ = true; // indicate if this point succeeded// Gauss-Newton iterationsEigen::Matrix2d H = Eigen::Matrix2d::Zero();    // hessianEigen::Vector2d b = Eigen::Vector2d::Zero();    // biasEigen::Vector2d J;  // jacobianfor (int iter = 0; iter < iterations; iter++) {if (inverse == false) {H = Eigen::Matrix2d::Zero();b = Eigen::Vector2d::Zero();} else {// only reset bb = Eigen::Vector2d::Zero();}cost = 0;// compute cost and jacobianfor (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, kp.pt.x + x, kp.pt.y + y) -GetPixelValue(img2, kp.pt.x + x + dx, kp.pt.y + y + dy);;  // Jacobianif (inverse == false) {J = -1.0 * Eigen::Vector2d(0.5 * (GetPixelValue(img2, kp.pt.x + dx + x + 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) {// in inverse mode, J keeps same for all iterations// NOTE this J does not change when dx, dy is updated, so we can store it and only compute errorJ = -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;b += -error * J;cost += error * error;if (inverse == false || iter == 0) {// also update HH += J * J.transpose();}}// compute updateEigen::Vector2d update = H.ldlt().solve(b);if (std::isnan(update[0])) {// sometimes occurred when we have a black or white patch and H is irreversiblecout << "update is nan" << endl;succ = false;break;}if (iter > 0 && cost > lastCost) {break;}// update dx, dydx += update[0];dy += update[1];lastCost = cost;succ = true;if (update.norm() < 1e-2) {// convergebreak;}}success[i] = succ;// set kp2kp2[i].pt = kp.pt + Point2f(dx, dy);}
}void OpticalFlowMultiLevel(const Mat &img1,const Mat &img2,const vector<KeyPoint> &kp1,vector<KeyPoint> &kp2,vector<bool> &success,bool inverse) {// parametersint pyramids = 4;double pyramid_scale = 0.5;double scales[] = {1.0, 0.5, 0.25, 0.125};// create pyramidschrono::steady_clock::time_point t1 = chrono::steady_clock::now();vector<Mat> pyr1, pyr2; // image pyramidsfor (int i = 0; i < pyramids; i++) {if (i == 0) {pyr1.push_back(img1);pyr2.push_back(img2);} else {Mat img1_pyr, img2_pyr;cv::resize(pyr1[i - 1], img1_pyr,cv::Size(pyr1[i - 1].cols * pyramid_scale, pyr1[i - 1].rows * pyramid_scale));cv::resize(pyr2[i - 1], img2_pyr,cv::Size(pyr2[i - 1].cols * pyramid_scale, pyr2[i - 1].rows * pyramid_scale));pyr1.push_back(img1_pyr);pyr2.push_back(img2_pyr);}}chrono::steady_clock::time_point t2 = chrono::steady_clock::now();auto time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);cout << "build pyramid time: " << time_used.count() << endl;// coarse-to-fine LK tracking in pyramidsvector<KeyPoint> kp1_pyr, kp2_pyr;for (auto &kp:kp1) {auto kp_top = kp;kp_top.pt *= scales[pyramids - 1];kp1_pyr.push_back(kp_top);kp2_pyr.push_back(kp_top);}for (int level = pyramids - 1; level >= 0; level--) {// from coarse to finesuccess.clear();t1 = chrono::steady_clock::now();OpticalFlowSingleLevel(pyr1[level], pyr2[level], kp1_pyr, kp2_pyr, success, inverse, true);t2 = chrono::steady_clock::now();auto time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);cout << "track pyr " << level << " cost time: " << time_used.count() << endl;if (level > 0) {for (auto &kp: kp1_pyr)kp.pt /= pyramid_scale;for (auto &kp: kp2_pyr)kp.pt /= pyramid_scale;}}for (auto &kp: kp2_pyr)kp2.push_back(kp);
}

        (2) 无配对点方法；     直接法