2D光流

最新推荐文章于 2022-04-03 20:57:22 发布

磊磊哈哈

最新推荐文章于 2022-04-03 20:57:22 发布

阅读量201

点赞数 1

原文链接：https://editor.csdn.net/md?articleId=110423168

版权

SLAM十四讲专栏收录该内容

37 篇文章 8 订阅

订阅专栏

特征点法缺点：
（1）：关键点提取耗时过长，（ORB：20ms，SIFT：无法实时获取）
（2）：忽略特征点以外信息，丢弃了大部分信息
（3）：找不到足够特征点匹配

2D光流基本概念

稀疏光流：计算部分像素运动：LK(Lucas-Kanade)
稠密光流：计算全部像素运动:(Horn-Schunck光流)
灰度不变假设：同一个点的像素灰度值，在各个图像中是固定不变的。（实际中很有可能不成立）

LK推导

灰度不变假设可以得到：
$I (x + d x, y + d y, t + d t) = I (x, y, t)$
对 $I (x + d x, y + d y, t + d t)$ 进行一阶泰勒展开：
$I(x+dx,y+dy,t+dt)\approx I(x,y,t)+\frac{\partial I}{\partial x}dx+\frac{\partial I}{\partial y}dy+\frac{\partial I}{\partial t}dt=I(x,y,t)$
得到：
$I:灰度值\\ \frac{\partial I}{\partial x}:x方向梯度\\ \frac{\partial I}{\partial y}:y方向梯度\\ \frac{\partial I}{\partial t}:灰度对时间方向的变化$
$\frac{\partial I}{\partial x}dx+\frac{\partial I}{\partial y}dy+\frac{\partial I}{\partial t}dt=0$
除以 $d t$ ，得到：
$\frac{\partial I}{\partial x}\frac{dx}{dt}+\frac{\partial I}{\partial y}\frac{d y}{dx}=-\frac{\partial I}{\partial t}$
再令 $u=\frac{dx}{dy},v=\frac{dy}{dt}，I_x=\frac{\partial I}{\partial x},I_y=\frac{\partial I}{\partial y}$
得到： $\left[\begin{matrix}I_x&I_y\end{matrix}\right]\left[\begin{matrix}u\\v\end{matrix}\right]=-I_t$
加入新的约束：假设某一窗口内的像素具有相同运动，这里假设窗口大小为 $w \times w$ ,得到：
$\left[\begin{matrix}I_x&I_y\end{matrix}\right]_k\left[\begin{matrix}u\\v\end{matrix}\right]=-I_{tk},k=1,...,w^2$
再令 $A=\left[\begin{matrix}[I_x,I_y]_1\\ ...\\ [I_x,I_y]_n\end{matrix}\right],b=\left[\begin{matrix}I_{t1}\\...\\I_{t_k}\end{matrix}\right],X=\left[\begin{matrix}u\\v\end{matrix}\right]$
最小2乘求解线性方程：
$X=(A^TA)^{-1}A^Tb$

实现

双线性内差法
资料1
资料2
$f(x,y)=\frac{1}{(x_2-x_1)(y_2-y_1)}(f(Q_{11})(x_2-x)(y_2-y)+f(Q_{21}(x-x_1)(y_2-y))+f(Q_{12})(x_2-x)(y-y_1))+f(Q_{22})(x-x_1)(y-y_1)$
图像梯度(中值差分)

$\frac{\partial (x,y)}{\partial x}=lim_{\xi->0}\frac{f(x+\xi,y)-f(x,y)}{\xi}=（f(x+1,y)-f(x,y)-(f(x,y)-f(x-1,y))）/2=f(x+1,y)-f(x-1,y)/2$
$\frac{\partial (x,y)}{\partial x}=lim_{\xi->0}\frac{f(x,y+\xi)-f(x,y)}{\xi}=（f(x,y+1)-f(x,y)-(f(x,y)-f(x,y-1))）/2=f(x,y+1)-f(x,y-1)/2$
parallel_for函数
并行处理，加速计算
parallel_for详解

#include <opencv/cv.hpp>
#include <string>
#include <chrono>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <iostream>
using namespace std;
using namespace Eigen;
using namespace cv;
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);
//Range类:
//      1:Range对象可以用来表示矩阵的多个连续的行或者多个连续的列
//      2:.Range是OpenCV中新加入的一个类，该类有两个关键的变量start和end；
//      3:表示范围从start到end，包含start，但不包含end;
//      4:提供了一个静态方法all()，表示所有的行或者所有的列
//      exp:Mat B = A(Range::all(),Range(1,3));提取第1到3列
//              C= B(Range(5,9),Range::all());B的第5至9行（不包括9）
private:
    const Mat &img1;
    const Mat &img2;
    const vector<KeyPoint> &kp1;
    vector<KeyPoint> &kp2;
    vector<bool> &success;
    bool inverse = true;
    bool has_initial = false;
};
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
);

void OpticalFlowMultiLevel(
    const Mat &img1,
    const Mat &img2,
    const vector<KeyPoint> &kp1,
    vector<KeyPoint> &kp2,
    vector<bool> &success,
    bool inverse = false
);
int main(int argc, const char** argv) {
    Mat img1=imread("/home/n1/notes/LK/LK/LK1.png",CV_LOAD_IMAGE_GRAYSCALE);//读取灰度图
    Mat img2=imread("/home/n1/notes/LK/LK/LK2.png",CV_LOAD_IMAGE_GRAYSCALE);
    vector<KeyPoint> P1;
    Ptr<GFTTDetector> detector=GFTTDetector::create(500,0.01,20);//亚像素角点检测
    detector->detect(img1,P1);//获取特征点

    vector<KeyPoint> P2;//获取特征点
    vector<bool> success_single;
    chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
    OpticalFlowSingleLevel(img1, img2, P1, P2, success_single, true);
    chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
    vector<KeyPoint> kp2_multi;
    vector<bool> success_multi;
    auto time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
    
    cout << "optical flow by gauss-newton: " << time_used.count() << endl;
    t1 = chrono::steady_clock::now();
    OpticalFlowMultiLevel(img1,img2,P1,kp2_multi,success_multi,true);
    t2 = chrono::steady_clock::now();
    time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
    cout << "optical flow by gauss-newton: " << time_used.count() << endl;

    //opencv 库
    vector<Point2f> pt1, pt2;
    for (auto &kp: P1) pt1.push_back(kp.pt);
    vector<uchar> status;
    vector<float> error;
    t1 = chrono::steady_clock::now();
    cv::calcOpticalFlowPyrLK(img1, img2, pt1, pt2, status, error);
    //void cv::calcOpticalFlowPyrLK	(	
    // InputArray 	prevImg,buildOpticalFlowPyramid构造的第一个8位输入图像或金字塔
    // InputArray 	nextImg,与prevImg相同大小和相同类型的第二个输入图像或金字塔
    // InputArray 	prevPts,需要找到流的2D点的矢量(vector of 2D points for which the flow needs to be found;);点坐标必须是单精度浮点数。
    // InputOutputArray 	nextPts,输出二维点的矢量（具有单精度浮点坐标），包含第二图像中输入特征的计算新位置;当传递OPTFLOW_USE_INITIAL_FLOW标志时，向量必须与输入中的大小相同。
    // OutputArray 	status,输出状态向量（无符号字符）;如果找到相应特征的流，则向量的每个元素设置为1，否则设置为0。
    // OutputArray 	err,输出错误的矢量; 向量的每个元素都设置为相应特征的错误，错误度量的类型可以在flags参数中设置; 如果未找到流，则未定义错误（
    // Size 	winSize = Size(21, 21),每个金字塔等级的搜索窗口的winSize大小
    // int 	maxLevel = 3,基于0的最大金字塔等级数;如果设置为0，则不使用金字塔（单级），如果设置为1，则使用两个级别，依此类推;如果将金字塔传递给输入，那么算法将使用与金字塔一样多的级别，但不超过maxLevel。
    // TermCriteria 	criteria = TermCriteria(TermCriteria::COUNT+TermCriteria::EPS, 30, 0.01),参数，指定迭代搜索算法的终止条件（在指定的最大迭代次数criteria.maxCount之后或当搜索窗口移动小于criteria.epsilon时）。
    // int 	flags = 0,      1 OPTFLOW_USE_INITIAL_FLOW使用初始估计，存储在nextPts中;如果未设置标志，则将prevPts复制到nextPts并将其视为初始估计。
    //                      2 OPTFLOW_LK_GET_MIN_EIGENVALS使用最小特征值作为误差测量（参见minEigThreshold描述）;如果没有设置标志，则将原稿周围的色块和移动点之间的L1距离除以窗口中的像素数，用作误差测量。
    // double 	minEigThreshold = 1e-4 算法计算光流方程的2x2正常矩阵的最小特征值，除以窗口中的像素数;如果此值小于minEigThreshold，则过滤掉相应的功能并且不处理其流程，因此它允许删除坏点并获得性能提升。
    // )	
    //
    t2 = chrono::steady_clock::now();
    time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
    cout << "optical flow by opencv: " << time_used.count() << endl;
    Mat img2_single;
    cv::cvtColor(img2, img2_single, CV_GRAY2BGR);//转为灰度图
    for (int i = 0; i < P2.size(); i++) {
        if (success_single[i]) {
            cv::circle(img2_single, P2[i].pt, 2, cv::Scalar(0, 250, 0), 2);//显示特征点
            cv::line(img2_single, P1[i].pt, P2[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, P1[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;
}
inline float GetPixelValue(const Mat &img,float x,float y){//inline 类似#define 可以在类内成员函数调用
    if(x<0)x=0;//对x限值
    if(y<0)y=0;//对y限值
    if(x>=img.cols)x=img.cols-1;//对x限值
    if(x>=img.rows)y=img.rows-1;
    uchar *data=&img.data[int(y)*img.step+int(x)];//step:图像数据一行所占自己长：列数*通道数
    float xx=x-floor(x);//取小数为，floor函数向下取整数 3.13 取3 -1.1取-2
    float yy=y-floor(y);
    return float((1-xx)*(1-yy)*data[0]+xx*(1-yy)*data[1]+(1-xx)*yy*data[img.step]+xx*yy*data[img.step+1]);
};

void OpticalFlowTracker::calculateOpticalFlow(const Range &range){
    int half_patch_size = 4;//选择关键邻域大小为8×8的区域
    int iterations = 10;    //迭代次数
    for(int i=range.start;i<range.end;i++){
        auto kp=kp1[i];//读取一个特征点
        double dx=0,dy=0;//dy,dx初始化，当为0时应当给一个估计值
        if(has_initial){//默认值为False,当为True时Kp2,有输入值
            dx=kp2[i].pt.x-kp.pt.x;//两个对应关键点之间差值
            dy=kp2[i].pt.y-kp.pt.y;
        }
        double cost=0,lastcost=0;//损失值
        bool succ = true;//这个点是否求得误差
        Matrix2d H=Matrix2d::Zero();//Ax=b中的A
        Vector2d b=Vector2d::Zero();//Ax=b中的b
        Vector2d J;//一阶导,雅可比矩阵
        for(int iter=0;iter<iterations;iter++){
            if(inverse==false){//如果为True 使用inverse法计算，反向光流，H矩阵不变，只计算一次（第一次两幅图像的Hessen矩阵），每次迭代计算残差b
                H=Matrix2d::Zero();
                b=Vector2d::Zero();
            }
            else{
                b=Vector2d::Zero();
            }
            cost =0;

            //计算雅克比矩阵
            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, kp.pt.x + x, kp.pt.y + y) -
                                   GetPixelValue(img2, kp.pt.x + x + dx, kp.pt.y + y + dy);;  // Jacobian
                    if(inverse==false){//2次差值求梯度值
                        J=-1.0*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){//在反向光流中，雅克比矩阵是固定的，一直为第一次获取的雅可比矩阵
                        J=-1.0*Vector2d((0.5*GetPixelValue(img2,kp.pt.x+dx+x+1,kp.pt.y+dy+y)-
                            0.5*GetPixelValue(img1,kp.pt.x+dx+x-1,kp.pt.y+dy+y)),
                            (0.5*GetPixelValue(img1,kp.pt.x+dx+x,kp.pt.y+dy+y+1)-
                            0.5*GetPixelValue(img1,kp.pt.x+dx+x,kp.pt.y+dy+y-1)));
                    }
                    b+=-error*J;//Ax=b，的b
                    cost +=error*error;//平方误差
                    if(inverse==false||iter==0){
                            H+=J*J.transpose();
                    }
                }
            }
                Eigen::Vector2d update = H.ldlt().solve(b);//dt分解求解
                
                if (std::isnan(update[0])) {
                    // 在一块区域为全黑或者全白时，H矩阵不可逆
                    cout << "update is nan" << endl;
                    succ = false;
                    break; 
                }
                if (iter > 0 && cost > lastcost) {
                    break;
                }
                dx +=update[0];
                dy +=update[1];
                lastcost=cost;
                succ=true;
                if(update.norm()<1e-2){
                    break;
                }
             
        }
        success[i]=succ;
        kp2[i].pt=kp.pt+Point2f(dx,dy);
    }
}
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());//重塑大小与kp1一致
    success.resize(kp1.size());
    OpticalFlowTracker tracker(img1, img2, kp1, kp2, success, inverse, has_initial);
    parallel_for_(Range(0, kp1.size()),//parallel_for_并行计算，加速运算
                  std::bind(&OpticalFlowTracker::calculateOpticalFlow, &tracker, std::placeholders::_1));//std::bind绑定函数，placeholders::_1:占位符


}
void OpticalFlowMultiLevel(
    const Mat &img1,
    const Mat &img2,
    const vector<KeyPoint> &kp1,
    vector<KeyPoint> &kp2,
    vector<bool> &success,
    bool inverse ){
        int pyramids=4;//尺度空间层数
        double pyramid_scale=0.5;//每层的缩放比力
        double scales[]={1.0,0.5,0.25,0.125};//缩放因子
        chrono::steady_clock::time_point t1=chrono::steady_clock::now();//当期时间
        vector<Mat> pyr1,pyr2;//图像图层
        
        for(int i=0;i<pyramids;i++){
            if(i==0){
                pyr1.push_back(img1);
                pyr2.push_back(img2);
            }else
            {
                Mat Rsize_img1,Rsize_img2;
                Size dsize_scale_1=Size(pyr1[i-1].cols*pyramid_scale,pyr1[i-1].rows*pyramid_scale);
                Size dsize_scale_2=Size(pyr2[i-1].cols*pyramid_scale,pyr2[i-1].rows*pyramid_scale);
                resize(pyr1[i-1],Rsize_img1,dsize_scale_1);
                resize(pyr2[i-1],Rsize_img2,dsize_scale_2);
                pyr1.push_back(Rsize_img1);
                pyr2.push_back(Rsize_img2);
            }
            
        }
        chrono::steady_clock::time_point t2=chrono::steady_clock::now();
        auto time_used=chrono::duration_cast<chrono::duration<double>>(t2-t1);
        cout<<"建立尺度空间时间"<<time_used.count()<<endl;
        vector<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--){
                success.clear();
                t1 = chrono::steady_clock::now();
                OpticalFlowSingleLevel(pyr1[level],pyr2[level],kp1_pyr,kp2_pyr,success,inverse,true);//调用单层LK光流
                t2 = chrono::steady_clock::now();
                auto time_used2 = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
                cout << "track pyr " << level << " cost time: " << time_used2.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);
        }
}

磊磊哈哈

关注

1
点赞
踩
2

收藏

觉得还不错? 一键收藏
0
评论
2D光流

特征点法缺点：（1）：关键点提取耗时过长，（ORB：20ms，SIFT：无法实时获取）（2）：忽略特征点以外信息，丢弃了大部分信息（3）：找不到足够特征点匹配2D光流基本概念稀疏光流：计算部分像素运动：LK(Lucas-Kanade)稠密光流：计算全部像素运动:(Horn-Schunck光流)灰度不变假设：同一个点的像素灰度值，在各个图像中是固定不变的。（实际中很有可能不成立）LK推导灰度不变假设可以得到：I(x+dx,y+dy,t+dt)=I(x,y,t)I(x+dx,y+dy,t
复制链接

扫一扫

专栏目录