视觉里程计-光流法(optical flow)

光流是一种描述像素随着时间,在图像之间运动的方法:
在这里插入图片描述
基于灰度不变假设:
I ( x 1 , y 1 , z 1 ) = I ( x 2 , y 2 , z 2 ) = I ( x 3 , y 3 , z 3 ) \begin{aligned} I(x_{1},y_{1},z_{1})=I(x_{2},y_{2},z_{2})=I(x_{3},y_{3},z_{3}) \end{aligned} I(x1,y1,z1)=I(x2,y2,z2)=I(x3,y3,z3)
Lucas-Kanade 光流
一个在t时间,位于 ( x , y ) (x,y) (x,y)处的像素,灰度可以表示成位置和时间的函数:
I ( x , y , t ) \begin{aligned} I(x,y,t) \end{aligned} I(x,y,t)
基于灰度不变假设:
I ( x + d x , y + d y , t + d t ) = I ( x , y , t ) \begin{aligned} I(x+dx,y+dy,t+dt)=I(x,y,t) \end{aligned} I(x+dx,y+dy,t+dt)=I(x,y,t)
对左边进行泰勒展开,保留一阶项:
I ( x + d x , y + d y , t + d t ) ≈ I ( x , y , t ) + ∂ I ∂ x d x + ∂ I ∂ y d y + ∂ I ∂ t d t \begin{aligned} I(x+dx,y+dy,t+dt)\approx I(x,y,t)+\frac{\partial\textbf{I}}{\partial x}dx+\frac{\partial\textbf{I}}{\partial y}dy+\frac{\partial\textbf{I}}{\partial t}dt \end{aligned} I(x+dx,y+dy,t+dt)I(x,y,t)+xIdx+yIdy+tIdt
从而:
∂ I ∂ x d x + ∂ I ∂ y d y + ∂ I ∂ t d t = 0 ∂ I ∂ x d x d t + ∂ I ∂ y d y d t = − ∂ I ∂ t \begin{aligned} \frac{\partial\textbf{I}}{\partial x}dx+\frac{\partial\textbf{I}}{\partial y}dy+\frac{\partial\textbf{I}}{\partial t}dt=0\\ \frac{\partial\textbf{I}}{\partial x}\frac{dx}{dt}+\frac{\partial\textbf{I}}{\partial y}\frac{dy}{dt}=-\frac{\partial\textbf{I}}{\partial t} \end{aligned} xIdx+yIdy+tIdt=0xIdtdx+yIdtdy=tI
d x / d t dx/dt dx/dt是像素沿x轴方向的运动速度, d y / d t dy/dt dy/dt是像素沿着y轴的运动速度,记为 u , v u,v u,v.同时 ∂ I / ∂ x \partial \textbf{I}/\partial x I/x是图像沿 x x x方向的梯度,另一项是 y y y方向上的梯度,记为 I x , I y I_{x},I_{y} Ix,Iy。写成紧凑的矩阵形式:
[ I x I y ] [ u v ] = − I t \begin{aligned} \left[\begin{matrix} \textbf{I}_{x}&\textbf{I}_{y} \end{matrix}\right]\left[\begin{matrix} u\\ v \end{matrix}\right]=-\textbf{I}_{t} \end{aligned} [IxIy][uv]=It
考虑一个 w × w w\times w w×w大小的窗口,它含有 w 2 w^{2} w2数量的像素。假设一个窗口内的像素具有相同的运动,因此一共有 w 2 w_{2} w2个方程:
[ I x I y ] k [ u v ] = − I t k , k = 1 , . . . . , w 2 \begin{aligned} \left[\begin{matrix} \textbf{I}_{x}&\textbf{I}_{y} \end{matrix}\right]_{k}\left[\begin{matrix} u\\ v \end{matrix}\right]=-\textbf{I}_{tk}, \quad k=1,....,w^{2} \end{aligned} [IxIy]k[uv]=Itk,k=1,....,w2

A = [ [ I x I y ] 1 ⋮ [ I x I y ] k ] , b = [ I t 1 ⋮ I t k ] \begin{aligned} \textbf{A}=\left[\begin{matrix} \left[\begin{matrix} \textbf{I}_{x}&\textbf{I}_{y} \end{matrix}\right]_{1}\\ \vdots \\ \left[\begin{matrix} \textbf{I}_{x}&\textbf{I}_{y} \end{matrix}\right]_{k} \end{matrix}\right],\textbf{b}=\left[\begin{matrix} I_{t1}\\ \vdots\\ I_{tk} \end{matrix}\right] \end{aligned} A=[IxIy]1[IxIy]k,b=It1Itk
于是整个方程可以表示为:
A [ u v ] = − b \begin{aligned} A\left[\begin{matrix} u\\ v \end{matrix}\right]=-b \end{aligned} A[uv]=b
超定方程组可以用块的最小二乘法通过求伪逆得到最优解:
[ u v ] ∗ = − ( A T A ) − 1 A T b \begin{aligned} \left[\begin{matrix} u\\ v \end{matrix}\right]^{*}=-(\textbf{A}^{T}\textbf{A})^{-1}\textbf{A}^{T}\textbf{b} \end{aligned} [uv]=(ATA)1ATb
LK光流通常用来跟踪角点的运动

Tum公开数据集
在这里插入图片描述
解压数据集:

tar -zxvf data.tar.gz

在这里插入图片描述
1.rgb.txt和depth.txt记录了各文件的采集时间和对应的文件名
2.rgb/和depth/目录存放着采集到的png格式图像文件。彩色图为八位三通道,深度图为16位单通道图像。文件名即采集时间
3.groundtruth.txt为外部运动系统采集到的相机位姿,格式为:
( t i m e , t x , t y , t z , q x , q y , q z , q w ) \begin{aligned} (time,t_{x},t_{y},t_{z},q_{x},q_{y},q_{z},q_{w}) \end{aligned} (time,tx,ty,tz,qx,qy,qz,qw)
4.associate.py 可以匹配深度图和彩色图
运行

python associate.py rgb.txt depth.txt > associate.txt
#如果没有numpy  sudo apt-get install python-numpy

生成了associate.txt

useLK.cpp 内容

#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 );
    if ( !fin ) 
    {
        cerr<<"I cann't find associate.txt!"<<endl;
        return 1;
    }
    
    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::FastFeatureDetector> detector = cv::FastFeatureDetector::create();
            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<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;
}

CMakeLists.txt 内容

cmake_minimum_required( VERSION 2.8 )
project( useLK )

set( CMAKE_BUILD_TYPE Release )
set( CMAKE_CXX_FLAGS "-std=c++11 -O3" )

find_package( OpenCV )
include_directories( ${OpenCV_INCLUDE_DIRS} )

add_executable( useLK useLK.cpp )
target_link_libraries( useLK ${OpenCV_LIBS} )

终端:

cd slambook/ch8/LKFlow
mkdir build
cd build
cmake ..
make
./useLK ../../data
#这里需要指定数据集的位置

在这里插入图片描述
从终端可以清楚看到特征点的丢失
在这里插入图片描述

©️2020 CSDN 皮肤主题: 数字20 设计师:CSDN官方博客 返回首页