特征点法缺点:
(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+dx,y+dy,t+dt)=I(x,y,t) I(x+dx,y+dy,t+dt)=I(x,y,t)
对 I ( x + d x , y + d y , t + d t ) I(x+dx,y+dy,t+dt) I(x+dx,y+dy,t+dt)进行一阶泰勒展开:
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 = I ( x , y , 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(x+dx,y+dy,t+dt)≈I(x,y,t)+∂x∂Idx+∂y∂Idy+∂t∂Idt=I(x,y,t)
得到:
I : 灰 度 值 ∂ I ∂ x : x 方 向 梯 度 ∂ I ∂ y : y 方 向 梯 度 ∂ I ∂ t : 灰 度 对 时 间 方 向 的 变 化 I:灰度值\\ \frac{\partial I}{\partial x}:x方向梯度\\ \frac{\partial I}{\partial y}:y方向梯度\\ \frac{\partial I}{\partial t}:灰度对时间方向的变化 I:灰度值∂x∂I:x方向梯度∂y∂I:y方向梯度∂t∂I:灰度对时间方向的变化
∂ I ∂ x d x + ∂ I ∂ y d y + ∂ I ∂ t d t = 0 \frac{\partial I}{\partial x}dx+\frac{\partial I}{\partial y}dy+\frac{\partial I}{\partial t}dt=0 ∂x∂Idx+∂y∂Idy+∂t∂Idt=0
除以 d t dt dt,得到:
∂ I ∂ x d x d t + ∂ I ∂ y d y d x = − ∂ I ∂ t \frac{\partial I}{\partial x}\frac{dx}{dt}+\frac{\partial I}{\partial y}\frac{d y}{dx}=-\frac{\partial I}{\partial t} ∂x∂Idtdx+∂y∂Idxdy=−∂t∂I -
再令 u = d x d y , v = d y d t , I x = ∂ I ∂ x , I y = ∂ I ∂ y u=\frac{dx}{dy},v=\frac{dy}{dt},I_x=\frac{\partial I}{\partial x},I_y=\frac{\partial I}{\partial y} u=dydx,v=dtdy,Ix=∂x∂I,Iy=∂y∂I
得到: [ I x I y ] [ u v ] = − I t \left[\begin{matrix}I_x&I_y\end{matrix}\right]\left[\begin{matrix}u\\v\end{matrix}\right]=-I_t [IxIy][uv]=−It -
加入新的约束:假设某一窗口内的像素具有相同运动,这里假设窗口大小为 w × w w×w w×w,得到:
[ I x I y ] k [ u v ] = − I t k , k = 1 , . . . , w 2 \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 [IxIy]k[uv]=−Itk,k=1,...,w2 -
再令 A = [ [ I x , I y ] 1 . . . [ I x , I y ] n ] , b = [ I t 1 . . . I t k ] , X = [ u v ] 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] A=⎣⎡[Ix,Iy]1...[Ix,Iy]n⎦⎤,b=⎣⎡It1...Itk⎦⎤,X=[uv]
-
最小2乘求解线性方程:
X = ( A T A ) − 1 A T b X=(A^TA)^{-1}A^Tb X=(ATA)−1ATb
实现
-
双线性内差法
资料1
资料2
f ( x , y ) = 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 ) 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) f(x,y)=(x2−x1)(y2−y1)1(f(Q11)(x2−x)(y2−y)+f(Q21(x−x1)(y2−y))+f(Q12)(x2−x)(y−y1))+f(Q22)(x−x1)(y−y1) -
图像梯度(中值差分)
∂ ( x , y ) ∂ x = l i m ξ − > 0 f ( x + ξ , y ) − f ( x , y ) ξ = ( 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+\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 ∂x∂(x,y)=limξ−>0ξf(x+ξ,y)−f(x,y)=(f(x+1,y)−f(x,y)−(f(x,y)−f(x−1,y)))/2=f(x+1,y)−f(x−1,y)/2
∂ ( x , y ) ∂ x = l i m ξ − > 0 f ( x , y + ξ ) − f ( x , y ) ξ = ( f ( x , y + 1 ) − f ( x , y ) − ( f ( x , y ) − f ( x , y − 1 ) ) ) / 2 = f ( x , y + 1 ) − f ( x , y − 1 ) / 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 ∂x∂(x,y)=limξ−>0ξf(x,y+ξ)−f(x,y)=(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);
}
}