# 视觉里程计-光流法(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 ( x , y , t ) \begin{aligned} I(x,y,t) \end{aligned}

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 + 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 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}
d x / d t dx/dt 是像素沿x轴方向的运动速度， d y / d t dy/dt 是像素沿着y轴的运动速度，记为 u , v u,v .同时 ∂ I / ∂ x \partial \textbf{I}/\partial x 是图像沿 x x 方向的梯度，另一项是 y y 方向上的梯度，记为 I x , I y I_{x},I_{y} 。写成紧凑的矩阵形式：
[ 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}

[ 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}

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 [ u v ] = − b \begin{aligned} A\left[\begin{matrix} u\\ v \end{matrix}\right]=-b \end{aligned}

[ 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}
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}
4.associate.py 可以匹配深度图和彩色图

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


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;
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;
cv::calcOpticalFlowPyrLK( last_color, color, prev_keypoints, next_keypoints, status, error );
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
#这里需要指定数据集的位置


• 点赞
• 评论
• 分享
x

海报分享

扫一扫，分享海报

• 收藏
• 打赏

打赏

weixin_37857892

你的鼓励将是我创作的最大动力

C币 余额
2C币 4C币 6C币 10C币 20C币 50C币
• 举报
• 一键三连

点赞Mark关注该博主, 随时了解TA的最新博文
01-26
09-01 1007

04-23 2468
04-24 2万+
01-09 1万+
09-15 2万+
06-20 3311
11-24 562