Opencv 光流法解析
```cpp
/** @brief Calculates an optical flow for a sparse feature set using the iterative Lucas-Kanade method with
pyramids.
@param prevImg first 8-bit input image or pyramid constructed by buildOpticalFlowPyramid.
@param nextImg second input image or pyramid of the same size and the same type as prevImg.
@param prevPts vector of 2D points for which the flow needs to be found; point coordinates must be
single-precision floating-point numbers.
@param nextPts output vector of 2D points (with single-precision floating-point coordinates)
containing the calculated new positions of input features in the second image; when
OPTFLOW_USE_INITIAL_FLOW flag is passed, the vector must have the same size as in the input.
@param status output status vector (of unsigned chars); each element of the vector is set to 1 if
the flow for the corresponding features has been found, otherwise, it is set to 0.
@param err output vector of errors; each element of the vector is set to an error for the
corresponding feature, type of the error measure can be set in flags parameter; if the flow wasn't
found then the error is not defined (use the status parameter to find such cases).
@param winSize size of the search window at each pyramid level.
@param maxLevel 0-based maximal pyramid level number; if set to 0, pyramids are not used (single
level), if set to 1, two levels are used, and so on; if pyramids are passed to input then
algorithm will use as many levels as pyramids have but no more than maxLevel.
@param criteria parameter, specifying the termination criteria of the iterative search algorithm
(after the specified maximum number of iterations criteria.maxCount or when the search window
moves by less than criteria.epsilon.
@param flags operation flags:
- **OPTFLOW_USE_INITIAL_FLOW** uses initial estimations, stored in nextPts; if the flag is
not set, then prevPts is copied to nextPts and is considered the initial estimate.
- **OPTFLOW_LK_GET_MIN_EIGENVALS** use minimum eigen values as an error measure (see
minEigThreshold description); if the flag is not set, then L1 distance between patches
around the original and a moved point, divided by number of pixels in a window, is used as a
error measure.
@param minEigThreshold the algorithm calculates the minimum eigen value of a 2x2 normal matrix of
optical flow equations (this matrix is called a spatial gradient matrix in @cite Bouguet00), divided
by number of pixels in a window; if this value is less than minEigThreshold, then a corresponding
feature is filtered out and its flow is not processed, so it allows to remove bad points and get a
performance boost.
The function implements a sparse iterative version of the Lucas-Kanade optical flow in pyramids. See
@cite Bouguet00 . The function is parallelized with the TBB library.
@note
- An example using the Lucas-Kanade optical flow algorithm can be found at
opencv_source_code/samples/cpp/lkdemo.cpp
- (Python) An example using the Lucas-Kanade optical flow algorithm can be found at
opencv_source_code/samples/python/lk_track.py
- (Python) An example using the Lucas-Kanade tracker for homography matching can be found at
opencv_source_code/samples/python/lk_homography.py
*/
CV_EXPORTS_W void calcOpticalFlowPyrLK( InputArray prevImg, InputArray nextImg,
InputArray prevPts, InputOutputArray nextPts,
OutputArray status, OutputArray err,
Size winSize = Size(21,21), int maxLevel = 3,
TermCriteria criteria = TermCriteria(TermCriteria::COUNT+TermCriteria::EPS, 30, 0.01),
int flags = 0, double minEigThreshold = 1e-4 );
1.建立金字塔
```cpp
/** @brief Constructs the image pyramid which can be passed to calcOpticalFlowPyrLK.
@param img 8-bit input image.
@param pyramid output pyramid.
@param winSize window size of optical flow algorithm. Must be not less than winSize argument of
calcOpticalFlowPyrLK. It is needed to calculate required padding for pyramid levels.
@param maxLevel 0-based maximal pyramid level number.
@param withDerivatives set to precompute gradients for the every pyramid level. If pyramid is
constructed without the gradients then calcOpticalFlowPyrLK will calculate them internally.
@param pyrBorder the border mode for pyramid layers.
@param derivBorder the border mode for gradients.
@param tryReuseInputImage put ROI of input image into the pyramid if possible. You can pass false to force data copying.
@return number of levels in constructed pyramid. Can be less than maxLevel.
*/
CV_EXPORTS_W int buildOpticalFlowPyramid( InputArray img, OutputArrayOfArrays pyramid,
Size winSize, int maxLevel, bool withDerivatives = true,
int pyrBorder = BORDER_REFLECT_101,
int derivBorder = BORDER_CONSTANT,
bool tryReuseInputImage = true );
参数设置:
MaxLevel = 3 //默认建立0-3层金字塔,0层是最底层(原图),3层是最高层
withDerivatives = false //就是是否在计算金字塔的时候 同时计算 图像梯度(差分),默认为false, 如果改为true,代码好像有一点点bug?
int main() {
//ScharrDerivInvoker();//一个微分函数,求梯度
test_calcOpticalFlowPyrLKdd();
return 1;
}
下面的程序从opencv中摘录出来,可以直接运行调试打印输出,方便理解calcOpticalFlowPyrLK的每一个步骤, 建议直接copy该代码进行阅读和运行
void test_calcOpticalFlowPyrLKdd() {
string file1 = "D:\\dataset\\0.png";
string file2 = "D:\\dataset\\24.png";
//file1 = "0.png";
//file2 = "5.png";
Mat source = imread(file1);
Mat target = imread(file2);
cv::Mat img0Gray = cv::Mat::zeros(source.rows, source.cols, CV_8UC1);
cv::Mat curImgGray = cv::Mat::zeros(target.rows, target.cols, CV_8UC1);
cvtColor(source, img0Gray, cv::COLOR_RGB2GRAY);
cvtColor(target, curImgGray, cv::COLOR_RGB2GRAY);
vector<cv::Point2f> featurePtSet0;
int maxNum = 10000;
goodFeaturesToTrack(img0Gray, featurePtSet0, maxNum, 0.05, 5);
cornerSubPix(img0Gray, featurePtSet0, cv::Size(15, 15), cv::Size(-1, -1), cv::TermCriteria(cv::TermCriteria::MAX_ITER | cv::TermCriteria::EPS, 20, 0.03));
printf("prepoints number: %d\n", int(featurePtSet0.size()));
vector<cv::Point2f> curfeaturePtSet;
vector<uchar> curFlag;
vector<float> curErrSet;
calcOpticalFlowPyrLKdd(img0Gray, curImgGray, featurePtSet0, curfeaturePtSet, curFlag, curErrSet);
for (int p = 0; p < curErrSet.size(); p++)
if (curErrSet[p] > 100 || curfeaturePtSet[p].x < 0 || curfeaturePtSet[p].y < 0 || curfeaturePtSet[p].x > img0Gray.cols || curfeaturePtSet[p].y > img0Gray.rows)
curFlag[p] = 0;
vector<cv::Point2f> sourceFeatures;
vector<cv::Point2f> targetFeatures;
for (int i = 0; i < curFlag.size(); i++) {
if (curFlag.at(i) == 1) {
sourceFeatures.push_back(featurePtSet0.at(i));
targetFeatures.push_back(curfeaturePtSet.at(i));
}
}
int np = sourceFeatures.size();
for (int i = 0; i < np; i++) {
circle(source, sourceFeatures[i], 3, Scalar(0, 255, 0), -1, 8);
circle(target, targetFeatures[i], 3, Scalar(0, 255, 0), -1, 8);
}
int w1 = source.cols; int h1 = source.rows;
int w2 = target.cols; int h2 = target.rows;
int width = w1 + w2; int height = max(h1, h2);
Mat resultImg = Mat(height, width, CV_8UC3, Scalar::all(0));
Mat ROI_1 = resultImg(Rect(0, 0, w1, h1));
Mat ROI_2 = resultImg(Rect(w1, 0, w2, h2));
source.copyTo(ROI_1);
target.copyTo(ROI_2);
imshow("result", resultImg);
waitKey(0);
}
// opencv_sample.cpp : 此文件包含 "main" 函数。程序执行将在此处开始并结束。
//
#include <iostream>
#include "demohist.h"
#include "opencv2/core/utility.hpp"
#include "opencv2/imgproc.hpp"
#include "opencv2/imgcodecs.hpp"
#include "opencv2/highgui.hpp"
#include <iostream>
using namespace cv;
using namespace std;
/*
* filter , 带权重的差分
t0
[-3, 0, 3;
-10,0, 10;
-3, 0, 3]
t1
[-3,-10,-3;
0,0,0;
3,10,3
*/
typedef short deriv_type;
void ScharrDerivInvoker(const cv::Mat& src, cv::Mat& dst)
{
// std::string file = "D:\\dataset\\dang_yingxiangzhiliangceshi\\gain\\2022-07-28-20-49-03_GAIN4_of_simu.png";
// Mat src = imread(file, 1);
int rows = src.rows, cols = src.cols, cn = src.channels(), colsn = cols * cn;
int x, y, delta = (int)alignSize((cols + 2) * cn, 16);
//printf("shape : %d,%d,%d %d\n", rows, cols, cn, delta);
AutoBuffer<deriv_type> _tempBuf(delta * 2 + 64);
deriv_type* trow0 = alignPtr(_tempBuf.data() + cn, 16);
deriv_type* trow1 = alignPtr(trow0 + delta, 16);
//Mat dst = Mat::zeros(Size(rows, cols*4), CV_32SC1);
#if CV_SIMD128
v_int16x8 c3 = v_setall_s16(3), c10 = v_setall_s16(10);
#endif
//printf("shape : %d,%d,%d %d\n", rows, cols, cn, delta);
for (y = 0; y < rows; y++)
{
const uchar* srow0 = src.ptr<uchar>(y > 0 ? y - 1 : rows > 1 ? 1 : 0);
const uchar* srow1 = src.ptr<uchar>(y);
const uchar* srow2 = src.ptr<uchar>(y < rows - 1 ? y + 1 : rows > 1 ? rows - 2 : 0); // reflect padding
deriv_type* drow = (deriv_type*)dst.ptr<deriv_type>(y);
//printf("shape :%d %d,%d,%d %d\n",y, rows, cols, cn, delta);
// do vertical convolution
x = 0;
#if CV_SIMD128
{
for (; x <= colsn - 8; x += 8)
{
v_int16x8 s0 = v_reinterpret_as_s16(v_load_expand(srow0 + x));
v_int16x8 s1 = v_reinterpret_as_s16(v_load_expand(srow1 + x));
v_int16x8 s2 = v_reinterpret_as_s16(v_load_expand(srow2 + x));
v_int16x8 t1 = s2 - s0;
v_int16x8 t0 = v_mul_wrap(s0 + s2, c3) + v_mul_wrap(s1, c10);
v_store(trow0 + x, t0);
v_store(trow1 + x, t1);
}
}
#endif
for (; x < colsn; x++)
{
int t0 = (srow0[x] + srow2[x]) * 3 + srow1[x] * 10;
int t1 = srow2[x] - srow0[x];
trow0[x] = (deriv_type)t0;
trow1[x] = (deriv_type)t1;
}
//x方向同样是reflect padding
// make border
int x0 = (cols > 1 ? 1 : 0) * cn, x1 = (cols > 1 ? cols - 2 : 0) * cn;
for (int k = 0; k < cn; k++)
{
trow0[-cn + k] = trow0[x0 + k]; trow0[colsn + k] = trow0[x1 + k];
trow1[-cn + k] = trow1[x0 + k]; trow1[colsn + k] = trow1[x1 + k];
}
// do horizontal convolution, interleave the results and store them to dst
x = 0;
#if CV_SIMD128
{
for (; x <= colsn - 8; x += 8)
{
v_int16x8 s0 = v_load(trow0 + x - cn);
v_int16x8 s1 = v_load(trow0 + x + cn);
v_int16x8 s2 = v_load(trow1 + x - cn);
v_int16x8 s3 = v_load(trow1 + x);
v_int16x8 s4 = v_load(trow1 + x + cn);
v_int16x8 t0 = s1 - s0;
v_int16x8 t1 = v_mul_wrap(s2 + s4, c3) + v_mul_wrap(s3, c10);
v_store_interleave((drow + x * 2), t0, t1);
}
}
#endif
for (; x < colsn; x++)
{
deriv_type t0 = (deriv_type)(trow0[x + cn] - trow0[x - cn]);
deriv_type t1 = (deriv_type)((trow1[x + cn] + trow1[x - cn]) * 3 + trow1[x] * 10);
drow[x * 2] = t0; drow[x * 2 + 1] = t1;
}
//if (x > colsn - 25) {
// printf("shape : %d,%d,%d %d\n", rows, cols, cn, delta);
//}
/*
* t0
[-3, 0, 3;
-10,0, 10;
-3, 0, 3]
t1
[-3,-10,-3;
0,0,0;
3,10,3
*/
}
//print result
//printf("\n");
//printf("\n");
/*
for (y = 0; y < 10; y++) {
uchar* srow1 = src.ptr<uchar>(y);
for (x = 0; x < 10; x++) {
printf("%d ", (int)srow1[3*x]);
}
printf("\n");
}
printf("\n\n");
for (y = 0; y < 10; y++) {
deriv_type* drow = (deriv_type*)dst.ptr<deriv_type>(y);
for (x = 0; x < 10; x++) {
printf("%d ",(int) drow[6*x]);
}
printf("\n");
}
*/
}
int buildOpticalFlowPyramiddd(InputArray _img, OutputArrayOfArrays pyramid, Size winSize, int maxLevel, bool withDerivatives,
int pyrBorder, int derivBorder, bool tryReuseInputImage)
{
//withDerivatives = true;
Mat img = _img.getMat();
CV_Assert(img.depth() == CV_8U && winSize.width > 2 && winSize.height > 2);
int pyrstep = withDerivatives ? 2 : 1;
pyramid.create(1, (maxLevel + 1) * pyrstep, 0 /*type*/, -1, true);
printf("\n pyramid size:%d\n", pyramid.size());
int derivType = CV_MAKETYPE(DataType<deriv_type>::depth, img.channels() * 2);
//level 0
bool lvl0IsSet = false;
printf("tryReuseInputImage :%d,%d,%d\n", tryReuseInputImage, img.isSubmatrix(), (pyrBorder & BORDER_ISOLATED));
if (tryReuseInputImage && img.isSubmatrix() && (pyrBorder & BORDER_ISOLATED) == 0)
{
Size wholeSize;
Point ofs;
img.locateROI(wholeSize, ofs);
if (ofs.x >= winSize.width && ofs.y >= winSize.height
&& ofs.x + img.cols + winSize.width <= wholeSize.width
&& ofs.y + img.rows + winSize.height <= wholeSize.height)
{
pyramid.getMatRef(0) = img;
lvl0IsSet = true;
}
printf("00000000000000000\n");
}
if (!lvl0IsSet)
{
Mat& temp = pyramid.getMatRef(0);
printf("0000000000temp.empty() %d, (%d,%d,%d), %d,%d\n", temp.empty(), temp.rows, temp.cols, temp.channels(), temp.type(), img.type());
if (!temp.empty())
temp.adjustROI(winSize.height, winSize.height, winSize.width, winSize.width);
if (temp.type() != img.type() || temp.cols != winSize.width * 2 + img.cols || temp.rows != winSize.height * 2 + img.rows)
temp.create(img.rows + winSize.height * 2, img.cols + winSize.width * 2, img.type());
if (pyrBorder == BORDER_TRANSPARENT)
img.copyTo(temp(Rect(winSize.width, winSize.height, img.cols, img.rows)));
else
copyMakeBorder(img, temp, winSize.height, winSize.height, winSize.width, winSize.width, pyrBorder);
printf("0000000000 %d, %d ,%d, %d, %d,%d\n", img.rows, img.cols, temp.channels(), temp.rows, temp.cols, temp.channels());
temp.adjustROI(-winSize.height, -winSize.height, -winSize.width, -winSize.width);
printf("0000000001 %d, %d ,%d, %d, %d,%d\n", img.rows, img.cols, temp.channels(), temp.rows, temp.cols, temp.channels());
}
Size sz = img.size();
Mat prevLevel = pyramid.getMatRef(0);
Mat thisLevel = prevLevel;
printf("init sz : %d,%d\n", sz.height, sz.width);
for (int level = 0; level <= maxLevel; ++level)
{
if (level != 0)
{
Mat& temp = pyramid.getMatRef(level * pyrstep);
printf("pyramid : %d,%d, type (%d,%d)\n", level, temp.empty(), temp.type(), img.type());
printf("pyramid : shape %d,%d, (%d,%d)\n", temp.rows, temp.cols, sz.height, sz.width);
if (!temp.empty())
temp.adjustROI(winSize.height, winSize.height, winSize.width, winSize.width);
if (temp.type() != img.type() || temp.cols != winSize.width * 2 + sz.width || temp.rows != winSize.height * 2 + sz.height)
temp.create(sz.height + winSize.height * 2, sz.width + winSize.width * 2, img.type());
thisLevel = temp(Rect(winSize.width, winSize.height, sz.width, sz.height));
pyrDown(prevLevel, thisLevel, sz);
if (pyrBorder != BORDER_TRANSPARENT)
copyMakeBorder(thisLevel, temp, winSize.height, winSize.height, winSize.width, winSize.width, pyrBorder | BORDER_ISOLATED);
temp.adjustROI(-winSize.height, -winSize.height, -winSize.width, -winSize.width);
}
if (withDerivatives)
{
Mat& deriv = pyramid.getMatRef(level * pyrstep + 1);
printf("withDerivatives :level %d,%d, type (%d,%d)\n", level, deriv.empty(), deriv.type(), derivType);
printf("withDerivatives :shape %d,%d, (%d,%d)\n", deriv.rows, deriv.cols, winSize.height, sz.height);
if (!deriv.empty()) // not run
deriv.adjustROI(winSize.height, winSize.height, winSize.width, winSize.width);
if (deriv.type() != derivType || deriv.cols != winSize.width * 2 + sz.width || deriv.rows != winSize.height * 2 + sz.height)
deriv.create(sz.height + winSize.height * 2, sz.width + winSize.width * 2, derivType);//创建deriv
Mat derivI = deriv(Rect(winSize.width, winSize.height, sz.width, sz.height));
printf("withDerivatives :shape %d,%d, (%d,%d)\n", derivI.rows, derivI.cols, deriv.rows, deriv.cols);
ScharrDerivInvoker(thisLevel, derivI);
if (derivBorder != BORDER_TRANSPARENT)
copyMakeBorder(derivI, deriv, winSize.height, winSize.height, winSize.width, winSize.width, derivBorder | BORDER_ISOLATED);
deriv.adjustROI(-winSize.height, -winSize.height, -winSize.width, -winSize.width);
}
sz = Size((sz.width + 1) / 2, (sz.height + 1) / 2);
printf("sz : %d,%d\n", sz.width , sz.height);
printf("\n\n");
if (sz.width <= winSize.width || sz.height <= winSize.height)
{
pyramid.create(1, (level + 1) * pyrstep, 0 /*type*/, -1, true);//check this
return level;
}
prevLevel = thisLevel;
}
return maxLevel;
}
enum {
OPTFLOW_USE_INITIAL_FLOW = 4,
OPTFLOW_LK_GET_MIN_EIGENVALS = 8,
OPTFLOW_FARNEBACK_GAUSSIAN = 256
};
typedef float acctype;
typedef float itemtype;
#define CV_DESCALE(x,n) (((x) + (1 << ((n)-1))) >> (n))
void LKTrackerInvoker(
const Mat& _prevImg, const Mat& _prevDeriv, const Mat& _nextImg,
const Point2f* _prevPts, Point2f* _nextPts,
uchar* _status, float* _err,
Size _winSize, TermCriteria _criteria,
int _level, int _maxLevel, int _flags, float _minEigThreshold,
int npoints)
{
const Mat* prevImg = &_prevImg;
const Mat* prevDeriv = &_prevDeriv;
const Mat* nextImg = &_nextImg;
const Point2f* prevPts = _prevPts;
Point2f* nextPts = _nextPts;
uchar* status = _status;
float* err = _err;
Size winSize = _winSize;
TermCriteria criteria = _criteria;
int level = _level;
int maxLevel = _maxLevel;
int flags = _flags;
float minEigThreshold = _minEigThreshold;
Point2f halfWin((winSize.width - 1) * 0.5f, (winSize.height - 1) * 0.5f);
const Mat& I = *prevImg;
const Mat& J = *nextImg;
const Mat& derivI = *prevDeriv;
int j, cn = I.channels(), cn2 = cn * 2;
cv::AutoBuffer<deriv_type> _buf(winSize.area() * (cn + cn2));//21*21 *(1+2) or (3+6)
printf("************************************************************************\n");
printf("auto buffer size : %d, %d,%d ",int(winSize.area() * (cn + cn2)), cn, cn2);
int derivDepth = DataType<deriv_type>::depth;
Mat IWinBuf(winSize, CV_MAKETYPE(derivDepth, cn), _buf.data());
Mat derivIWinBuf(winSize, CV_MAKETYPE(derivDepth, cn2), _buf.data() + winSize.area() * cn);
printf("win buffer dtype : %d, %d,%d \n", CV_MAKETYPE(derivDepth, cn), CV_MAKETYPE(derivDepth, cn2), npoints);
for (int ptidx = 0; ptidx < npoints; ptidx++)
{
Point2f prevPt = prevPts[ptidx] * (float)(1. / (1 << level));
Point2f nextPt;
if (level == maxLevel)
{
if (flags & OPTFLOW_USE_INITIAL_FLOW)
nextPt = nextPts[ptidx] * (float)(1. / (1 << level));
else
nextPt = prevPt;
}
else
nextPt = nextPts[ptidx] * 2.f;
nextPts[ptidx] = nextPt;
Point2i iprevPt, inextPt;
prevPt -= halfWin;//窗口向左上移动
iprevPt.x = cvFloor(prevPt.x);
iprevPt.y = cvFloor(prevPt.y);
if (iprevPt.x < -winSize.width || iprevPt.x >= derivI.cols ||
iprevPt.y < -winSize.height || iprevPt.y >= derivI.rows)
{
if (level == 0)
{
if (status)
status[ptidx] = false;
if (err)
err[ptidx] = 0;
}
continue;
}
float a = prevPt.x - iprevPt.x;
float b = prevPt.y - iprevPt.y;
const int W_BITS = 14, W_BITS1 = 14;
const float FLT_SCALE = 1.f / (1 << 20);
int iw00 = cvRound((1.f - a) * (1.f - b) * (1 << W_BITS));
int iw01 = cvRound(a * (1.f - b) * (1 << W_BITS));
int iw10 = cvRound((1.f - a) * b * (1 << W_BITS));
int iw11 = (1 << W_BITS) - iw00 - iw01 - iw10;
int dstep = (int)(derivI.step / derivI.elemSize1());
int stepI = (int)(I.step / I.elemSize1());
int stepJ = (int)(J.step / J.elemSize1());
acctype iA11 = 0, iA12 = 0, iA22 = 0;
float A11, A12, A22;
#if CV_SIMD128 && !CV_NEON
v_int16x8 qw0((short)(iw00), (short)(iw01), (short)(iw00), (short)(iw01), (short)(iw00), (short)(iw01), (short)(iw00), (short)(iw01));
v_int16x8 qw1((short)(iw10), (short)(iw11), (short)(iw10), (short)(iw11), (short)(iw10), (short)(iw11), (short)(iw10), (short)(iw11));
v_int32x4 qdelta_d = v_setall_s32(1 << (W_BITS1 - 1));
v_int32x4 qdelta = v_setall_s32(1 << (W_BITS1 - 5 - 1));
v_float32x4 qA11 = v_setzero_f32(), qA12 = v_setzero_f32(), qA22 = v_setzero_f32();
#endif
#if CV_NEON
float CV_DECL_ALIGNED(16) nA11[] = { 0, 0, 0, 0 }, nA12[] = { 0, 0, 0, 0 }, nA22[] = { 0, 0, 0, 0 };
const int shifter1 = -(W_BITS - 5); //negative so it shifts right
const int shifter2 = -(W_BITS);
const int16x4_t d26 = vdup_n_s16((int16_t)iw00);
const int16x4_t d27 = vdup_n_s16((int16_t)iw01);
const int16x4_t d28 = vdup_n_s16((int16_t)iw10);
const int16x4_t d29 = vdup_n_s16((int16_t)iw11);
const int32x4_t q11 = vdupq_n_s32((int32_t)shifter1);
const int32x4_t q12 = vdupq_n_s32((int32_t)shifter2);
#endif
// extract the patch from the first image, compute covariation matrix of derivatives
int x, y;
for (y = 0; y < winSize.height; y++)
{
const uchar* src = I.ptr() + (y + iprevPt.y) * stepI + iprevPt.x * cn;
const deriv_type* dsrc = derivI.ptr<deriv_type>() + (y + iprevPt.y) * dstep + iprevPt.x * cn2;
deriv_type* Iptr = IWinBuf.ptr<deriv_type>(y);//pre image
deriv_type* dIptr = derivIWinBuf.ptr<deriv_type>(y);//pre deri
x = 0;
#if CV_SIMD128 && !CV_NEON
for (; x <= winSize.width * cn - 8; x += 8, dsrc += 8 * 2, dIptr += 8 * 2)
{
v_int32x4 t0, t1;
v_int16x8 v00, v01, v10, v11, t00, t01, t10, t11;
v00 = v_reinterpret_as_s16(v_load_expand(src + x));
v01 = v_reinterpret_as_s16(v_load_expand(src + x + cn));
v10 = v_reinterpret_as_s16(v_load_expand(src + x + stepI));
v11 = v_reinterpret_as_s16(v_load_expand(src + x + stepI + cn));
v_zip(v00, v01, t00, t01);
v_zip(v10, v11, t10, t11);
t0 = v_dotprod(t00, qw0, qdelta) + v_dotprod(t10, qw1);
t1 = v_dotprod(t01, qw0, qdelta) + v_dotprod(t11, qw1);
t0 = t0 >> (W_BITS1 - 5);
t1 = t1 >> (W_BITS1 - 5);
v_store(Iptr + x, v_pack(t0, t1));
v00 = v_reinterpret_as_s16(v_load(dsrc));
v01 = v_reinterpret_as_s16(v_load(dsrc + cn2));
v10 = v_reinterpret_as_s16(v_load(dsrc + dstep));
v11 = v_reinterpret_as_s16(v_load(dsrc + dstep + cn2));
v_zip(v00, v01, t00, t01);
v_zip(v10, v11, t10, t11);
t0 = v_dotprod(t00, qw0, qdelta_d) + v_dotprod(t10, qw1);
t1 = v_dotprod(t01, qw0, qdelta_d) + v_dotprod(t11, qw1);
t0 = t0 >> W_BITS1;
t1 = t1 >> W_BITS1;
v00 = v_pack(t0, t1); // Ix0 Iy0 Ix1 Iy1 ...
v_store(dIptr, v00);
v00 = v_reinterpret_as_s16(v_interleave_pairs(v_reinterpret_as_s32(v_interleave_pairs(v00))));
v_expand(v00, t1, t0);
v_float32x4 fy = v_cvt_f32(t0);
v_float32x4 fx = v_cvt_f32(t1);
qA22 = v_muladd(fy, fy, qA22);
qA12 = v_muladd(fx, fy, qA12);
qA11 = v_muladd(fx, fx, qA11);
v00 = v_reinterpret_as_s16(v_load(dsrc + 4 * 2));
v01 = v_reinterpret_as_s16(v_load(dsrc + 4 * 2 + cn2));
v10 = v_reinterpret_as_s16(v_load(dsrc + 4 * 2 + dstep));
v11 = v_reinterpret_as_s16(v_load(dsrc + 4 * 2 + dstep + cn2));
v_zip(v00, v01, t00, t01);
v_zip(v10, v11, t10, t11);
t0 = v_dotprod(t00, qw0, qdelta_d) + v_dotprod(t10, qw1);
t1 = v_dotprod(t01, qw0, qdelta_d) + v_dotprod(t11, qw1);
t0 = t0 >> W_BITS1;
t1 = t1 >> W_BITS1;
v00 = v_pack(t0, t1); // Ix0 Iy0 Ix1 Iy1 ...
v_store(dIptr + 4 * 2, v00);
v00 = v_reinterpret_as_s16(v_interleave_pairs(v_reinterpret_as_s32(v_interleave_pairs(v00))));
v_expand(v00, t1, t0);
fy = v_cvt_f32(t0);
fx = v_cvt_f32(t1);
qA22 = v_muladd(fy, fy, qA22);
qA12 = v_muladd(fx, fy, qA12);
qA11 = v_muladd(fx, fx, qA11);
}
#endif
#if CV_NEON
for (; x <= winSize.width * cn - 4; x += 4, dsrc += 4 * 2, dIptr += 4 * 2)
{
uint8x8_t d0 = vld1_u8(&src[x]);
uint8x8_t d2 = vld1_u8(&src[x + cn]);
uint16x8_t q0 = vmovl_u8(d0);
uint16x8_t q1 = vmovl_u8(d2);
int32x4_t q5 = vmull_s16(vget_low_s16(vreinterpretq_s16_u16(q0)), d26);
int32x4_t q6 = vmull_s16(vget_low_s16(vreinterpretq_s16_u16(q1)), d27);
uint8x8_t d4 = vld1_u8(&src[x + stepI]);
uint8x8_t d6 = vld1_u8(&src[x + stepI + cn]);
uint16x8_t q2 = vmovl_u8(d4);
uint16x8_t q3 = vmovl_u8(d6);
int32x4_t q7 = vmull_s16(vget_low_s16(vreinterpretq_s16_u16(q2)), d28);
int32x4_t q8 = vmull_s16(vget_low_s16(vreinterpretq_s16_u16(q3)), d29);
q5 = vaddq_s32(q5, q6);
q7 = vaddq_s32(q7, q8);
q5 = vaddq_s32(q5, q7);
int16x4x2_t d0d1 = vld2_s16(dsrc);
int16x4x2_t d2d3 = vld2_s16(&dsrc[cn2]);
q5 = vqrshlq_s32(q5, q11);
int32x4_t q4 = vmull_s16(d0d1.val[0], d26);
q6 = vmull_s16(d0d1.val[1], d26);
int16x4_t nd0 = vmovn_s32(q5);
q7 = vmull_s16(d2d3.val[0], d27);
q8 = vmull_s16(d2d3.val[1], d27);
vst1_s16(&Iptr[x], nd0);
int16x4x2_t d4d5 = vld2_s16(&dsrc[dstep]);
int16x4x2_t d6d7 = vld2_s16(&dsrc[dstep + cn2]);
q4 = vaddq_s32(q4, q7);
q6 = vaddq_s32(q6, q8);
q7 = vmull_s16(d4d5.val[0], d28);
int32x4_t q14 = vmull_s16(d4d5.val[1], d28);
q8 = vmull_s16(d6d7.val[0], d29);
int32x4_t q15 = vmull_s16(d6d7.val[1], d29);
q7 = vaddq_s32(q7, q8);
q14 = vaddq_s32(q14, q15);
q4 = vaddq_s32(q4, q7);
q6 = vaddq_s32(q6, q14);
float32x4_t nq0 = vld1q_f32(nA11);
float32x4_t nq1 = vld1q_f32(nA12);
float32x4_t nq2 = vld1q_f32(nA22);
q4 = vqrshlq_s32(q4, q12);
q6 = vqrshlq_s32(q6, q12);
q7 = vmulq_s32(q4, q4);
q8 = vmulq_s32(q4, q6);
q15 = vmulq_s32(q6, q6);
nq0 = vaddq_f32(nq0, vcvtq_f32_s32(q7));
nq1 = vaddq_f32(nq1, vcvtq_f32_s32(q8));
nq2 = vaddq_f32(nq2, vcvtq_f32_s32(q15));
vst1q_f32(nA11, nq0);
vst1q_f32(nA12, nq1);
vst1q_f32(nA22, nq2);
int16x4_t d8 = vmovn_s32(q4);
int16x4_t d12 = vmovn_s32(q6);
int16x4x2_t d8d12;
d8d12.val[0] = d8; d8d12.val[1] = d12;
vst2_s16(dIptr, d8d12);
}
#endif
for (; x < winSize.width * cn; x++, dsrc += 2, dIptr += 2)
{
int ival = CV_DESCALE(src[x] * iw00 + src[x + cn] * iw01 +
src[x + stepI] * iw10 + src[x + stepI + cn] * iw11, W_BITS1 - 5);
int ixval = CV_DESCALE(dsrc[0] * iw00 + dsrc[cn2] * iw01 +
dsrc[dstep] * iw10 + dsrc[dstep + cn2] * iw11, W_BITS1);
int iyval = CV_DESCALE(dsrc[1] * iw00 + dsrc[cn2 + 1] * iw01 + dsrc[dstep + 1] * iw10 +
dsrc[dstep + cn2 + 1] * iw11, W_BITS1);
Iptr[x] = (short)ival; //
dIptr[0] = (short)ixval;// Ix
dIptr[1] = (short)iyval;// Iy
iA11 += (itemtype)(ixval * ixval);
iA12 += (itemtype)(ixval * iyval);
iA22 += (itemtype)(iyval * iyval);
}
}
#if CV_SIMD128 && !CV_NEON
iA11 += v_reduce_sum(qA11);
iA12 += v_reduce_sum(qA12);
iA22 += v_reduce_sum(qA22);
#endif
#if CV_NEON
iA11 += nA11[0] + nA11[1] + nA11[2] + nA11[3];
iA12 += nA12[0] + nA12[1] + nA12[2] + nA12[3];
iA22 += nA22[0] + nA22[1] + nA22[2] + nA22[3];
#endif
A11 = iA11 * FLT_SCALE;
A12 = iA12 * FLT_SCALE;
A22 = iA22 * FLT_SCALE;
float D = A11 * A22 - A12 * A12;
float minEig = (A22 + A11 - std::sqrt((A11 - A22) * (A11 - A22) +
4.f * A12 * A12)) / (2 * winSize.width * winSize.height);
if (err && (flags & OPTFLOW_LK_GET_MIN_EIGENVALS) != 0)
err[ptidx] = (float)minEig;
// 行列式的delta值 和 最小的特征值 可以判断 是否是 可逆的,如果都很小,判断为不可逆,该点跟踪失败 status[ptidx] = false;
if (minEig < minEigThreshold || D < FLT_EPSILON)
{
if (level == 0 && status)
status[ptidx] = false;
continue;
}
D = 1.f / D;
nextPt -= halfWin;
Point2f prevDelta;
// 最小二乘迭代maxCount 次, 或者收敛
for (j = 0; j < criteria.maxCount; j++)
{
inextPt.x = cvFloor(nextPt.x);
inextPt.y = cvFloor(nextPt.y);
if (inextPt.x < -winSize.width || inextPt.x >= J.cols ||
inextPt.y < -winSize.height || inextPt.y >= J.rows)
{
if (level == 0 && status)
status[ptidx] = false;
break;
}
a = nextPt.x - inextPt.x;
b = nextPt.y - inextPt.y;
iw00 = cvRound((1.f - a) * (1.f - b) * (1 << W_BITS));
iw01 = cvRound(a * (1.f - b) * (1 << W_BITS));
iw10 = cvRound((1.f - a) * b * (1 << W_BITS));
iw11 = (1 << W_BITS) - iw00 - iw01 - iw10;
acctype ib1 = 0, ib2 = 0;
float b1, b2;
#if CV_SIMD128 && !CV_NEON
qw0 = v_int16x8((short)(iw00), (short)(iw01), (short)(iw00), (short)(iw01), (short)(iw00), (short)(iw01), (short)(iw00), (short)(iw01));
qw1 = v_int16x8((short)(iw10), (short)(iw11), (short)(iw10), (short)(iw11), (short)(iw10), (short)(iw11), (short)(iw10), (short)(iw11));
v_float32x4 qb0 = v_setzero_f32(), qb1 = v_setzero_f32();
#endif
#if CV_NEON
float CV_DECL_ALIGNED(16) nB1[] = { 0,0,0,0 }, nB2[] = { 0,0,0,0 };
const int16x4_t d26_2 = vdup_n_s16((int16_t)iw00);
const int16x4_t d27_2 = vdup_n_s16((int16_t)iw01);
const int16x4_t d28_2 = vdup_n_s16((int16_t)iw10);
const int16x4_t d29_2 = vdup_n_s16((int16_t)iw11);
#endif
for (y = 0; y < winSize.height; y++)
{
const uchar* Jptr = J.ptr() + (y + inextPt.y) * stepJ + inextPt.x * cn;
const deriv_type* Iptr = IWinBuf.ptr<deriv_type>(y);
const deriv_type* dIptr = derivIWinBuf.ptr<deriv_type>(y);
x = 0;
#if CV_SIMD128 && !CV_NEON
for (; x <= winSize.width * cn - 8; x += 8, dIptr += 8 * 2)
{
v_int16x8 diff0 = v_reinterpret_as_s16(v_load(Iptr + x)), diff1, diff2;
v_int16x8 v00 = v_reinterpret_as_s16(v_load_expand(Jptr + x));
v_int16x8 v01 = v_reinterpret_as_s16(v_load_expand(Jptr + x + cn));
v_int16x8 v10 = v_reinterpret_as_s16(v_load_expand(Jptr + x + stepJ));
v_int16x8 v11 = v_reinterpret_as_s16(v_load_expand(Jptr + x + stepJ + cn));
v_int32x4 t0, t1;
v_int16x8 t00, t01, t10, t11;
v_zip(v00, v01, t00, t01);
v_zip(v10, v11, t10, t11);
t0 = v_dotprod(t00, qw0, qdelta) + v_dotprod(t10, qw1);
t1 = v_dotprod(t01, qw0, qdelta) + v_dotprod(t11, qw1);
t0 = t0 >> (W_BITS1 - 5);
t1 = t1 >> (W_BITS1 - 5);
diff0 = v_pack(t0, t1) - diff0;
v_zip(diff0, diff0, diff2, diff1); // It0 It0 It1 It1 ...
v00 = v_reinterpret_as_s16(v_load(dIptr)); // Ix0 Iy0 Ix1 Iy1 ...
v01 = v_reinterpret_as_s16(v_load(dIptr + 8));
v_zip(v00, v01, v10, v11);
v_zip(diff2, diff1, v00, v01);
qb0 += v_cvt_f32(v_dotprod(v00, v10));
qb1 += v_cvt_f32(v_dotprod(v01, v11));
}
#endif
#if CV_NEON
for (; x <= winSize.width * cn - 8; x += 8, dIptr += 8 * 2)
{
uint8x8_t d0 = vld1_u8(&Jptr[x]);
uint8x8_t d2 = vld1_u8(&Jptr[x + cn]);
uint8x8_t d4 = vld1_u8(&Jptr[x + stepJ]);
uint8x8_t d6 = vld1_u8(&Jptr[x + stepJ + cn]);
uint16x8_t q0 = vmovl_u8(d0);
uint16x8_t q1 = vmovl_u8(d2);
uint16x8_t q2 = vmovl_u8(d4);
uint16x8_t q3 = vmovl_u8(d6);
int32x4_t nq4 = vmull_s16(vget_low_s16(vreinterpretq_s16_u16(q0)), d26_2);
int32x4_t nq5 = vmull_s16(vget_high_s16(vreinterpretq_s16_u16(q0)), d26_2);
int32x4_t nq6 = vmull_s16(vget_low_s16(vreinterpretq_s16_u16(q1)), d27_2);
int32x4_t nq7 = vmull_s16(vget_high_s16(vreinterpretq_s16_u16(q1)), d27_2);
int32x4_t nq8 = vmull_s16(vget_low_s16(vreinterpretq_s16_u16(q2)), d28_2);
int32x4_t nq9 = vmull_s16(vget_high_s16(vreinterpretq_s16_u16(q2)), d28_2);
int32x4_t nq10 = vmull_s16(vget_low_s16(vreinterpretq_s16_u16(q3)), d29_2);
int32x4_t nq11 = vmull_s16(vget_high_s16(vreinterpretq_s16_u16(q3)), d29_2);
nq4 = vaddq_s32(nq4, nq6);
nq5 = vaddq_s32(nq5, nq7);
nq8 = vaddq_s32(nq8, nq10);
nq9 = vaddq_s32(nq9, nq11);
int16x8_t q6 = vld1q_s16(&Iptr[x]);
nq4 = vaddq_s32(nq4, nq8);
nq5 = vaddq_s32(nq5, nq9);
nq8 = vmovl_s16(vget_high_s16(q6));
nq6 = vmovl_s16(vget_low_s16(q6));
nq4 = vqrshlq_s32(nq4, q11);
nq5 = vqrshlq_s32(nq5, q11);
int16x8x2_t q0q1 = vld2q_s16(dIptr);
float32x4_t nB1v = vld1q_f32(nB1);
float32x4_t nB2v = vld1q_f32(nB2);
nq4 = vsubq_s32(nq4, nq6);
nq5 = vsubq_s32(nq5, nq8);
int32x4_t nq2 = vmovl_s16(vget_low_s16(q0q1.val[0]));
int32x4_t nq3 = vmovl_s16(vget_high_s16(q0q1.val[0]));
nq7 = vmovl_s16(vget_low_s16(q0q1.val[1]));
nq8 = vmovl_s16(vget_high_s16(q0q1.val[1]));
nq9 = vmulq_s32(nq4, nq2);
nq10 = vmulq_s32(nq5, nq3);
nq4 = vmulq_s32(nq4, nq7);
nq5 = vmulq_s32(nq5, nq8);
nq9 = vaddq_s32(nq9, nq10);
nq4 = vaddq_s32(nq4, nq5);
nB1v = vaddq_f32(nB1v, vcvtq_f32_s32(nq9));
nB2v = vaddq_f32(nB2v, vcvtq_f32_s32(nq4));
vst1q_f32(nB1, nB1v);
vst1q_f32(nB2, nB2v);
}
#endif
for (; x < winSize.width * cn; x++, dIptr += 2)
{
int diff = CV_DESCALE(Jptr[x] * iw00 + Jptr[x + cn] * iw01 +
Jptr[x + stepJ] * iw10 + Jptr[x + stepJ + cn] * iw11,
W_BITS1 - 5) - Iptr[x];// It
ib1 += (itemtype)(diff * dIptr[0]);
ib2 += (itemtype)(diff * dIptr[1]);
}
}
#if CV_SIMD128 && !CV_NEON
v_float32x4 qf0, qf1;
v_recombine(v_interleave_pairs(qb0 + qb1), v_setzero_f32(), qf0, qf1);
ib1 += v_reduce_sum(qf0);
ib2 += v_reduce_sum(qf1);
#endif
#if CV_NEON
ib1 += (float)(nB1[0] + nB1[1] + nB1[2] + nB1[3]);
ib2 += (float)(nB2[0] + nB2[1] + nB2[2] + nB2[3]);
#endif
b1 = ib1 * FLT_SCALE;
b2 = ib2 * FLT_SCALE;
Point2f delta((float)((A12 * b2 - A22 * b1) * D),
(float)((A12 * b1 - A11 * b2) * D));//得到该次最小二乘的迭代结果,这里计算的是 nextframe->preframe的光流
//delta = -delta;
nextPt += delta;
nextPts[ptidx] = nextPt + halfWin;
// 如果光流收敛了
if (delta.ddot(delta) <= criteria.epsilon)
break;
// 或者两次光流已经很小(光流收敛), 停止迭代
if (j > 0 && std::abs(delta.x + prevDelta.x) < 0.01 &&
std::abs(delta.y + prevDelta.y) < 0.01)
{
nextPts[ptidx] -= delta * 0.5f;
break;
}
prevDelta = delta;
}
//这里计算 误差,如果OPTFLOW_LK_GET_MIN_EIGENVALS==1,令最小特征值作为误差项,
// 否则使用 patch 的 abs差异来表示
CV_Assert(status != NULL);
if (status[ptidx] && err && level == 0 && (flags & OPTFLOW_LK_GET_MIN_EIGENVALS) == 0)
{
Point2f nextPoint = nextPts[ptidx] - halfWin;
Point inextPoint;
inextPoint.x = cvFloor(nextPoint.x);
inextPoint.y = cvFloor(nextPoint.y);
if (inextPoint.x < -winSize.width || inextPoint.x >= J.cols ||
inextPoint.y < -winSize.height || inextPoint.y >= J.rows)
{
if (status)
status[ptidx] = false;
continue;
}
float aa = nextPoint.x - inextPoint.x;
float bb = nextPoint.y - inextPoint.y;
iw00 = cvRound((1.f - aa) * (1.f - bb) * (1 << W_BITS));
iw01 = cvRound(aa * (1.f - bb) * (1 << W_BITS));
iw10 = cvRound((1.f - aa) * bb * (1 << W_BITS));
iw11 = (1 << W_BITS) - iw00 - iw01 - iw10;
float errval = 0.f;
for (y = 0; y < winSize.height; y++)
{
const uchar* Jptr = J.ptr() + (y + inextPoint.y) * stepJ + inextPoint.x * cn;
const deriv_type* Iptr = IWinBuf.ptr<deriv_type>(y);
for (x = 0; x < winSize.width * cn; x++)
{
int diff = CV_DESCALE(Jptr[x] * iw00 + Jptr[x + cn] * iw01 +
Jptr[x + stepJ] * iw10 + Jptr[x + stepJ + cn] * iw11,
W_BITS1 - 5) - Iptr[x];
errval += std::abs((float)diff);
}
}
err[ptidx] = errval * 1.f / (32 * winSize.width * cn * winSize.height);
}
}
}
typedef short deriv_type;
void calcOpticalFlowPyrLKdd(InputArray _prevImg, InputArray _nextImg,
InputArray _prevPts, InputOutputArray _nextPts,
OutputArray _status, OutputArray _err)
{
//default parameter
Size winSize = Size(21, 21);
int maxLevel = 3;
TermCriteria criteria = TermCriteria(TermCriteria::COUNT + TermCriteria::EPS, 30, 0.01);
int flags = 0;
double minEigThreshold = 1e-4;
Mat prevPtsMat = _prevPts.getMat();
const int derivDepth = DataType<deriv_type>::depth;
CV_Assert(maxLevel >= 0 && winSize.width > 2 && winSize.height > 2);
printf("prevPtsMat : %d,%d,%d\n", prevPtsMat.rows, prevPtsMat.cols, prevPtsMat.channels());
printf("derivDepth : %d,\n", derivDepth);
int level = 0, i, npoints;
CV_Assert((npoints = prevPtsMat.checkVector(2, CV_32F, true)) >= 0);
if (npoints == 0)
{
_nextPts.release();
_status.release();
_err.release();
return;
}
if (!(flags & 4))
_nextPts.create(prevPtsMat.size(), prevPtsMat.type(), -1, true);
printf("npoints : %d,\n", npoints);
printf("_nextPts size : %d,\n", _nextPts.size());
Mat nextPtsMat = _nextPts.getMat();
CV_Assert(nextPtsMat.checkVector(2, CV_32F, true) == npoints);
printf("nextPtsMat size : %d,%d,%d\n", nextPtsMat.rows, nextPtsMat.cols, nextPtsMat.channels());
const Point2f* prevPts = prevPtsMat.ptr<Point2f>();
Point2f* nextPts = nextPtsMat.ptr<Point2f>();
_status.create((int)npoints, 1, CV_8U, -1, true);
Mat statusMat = _status.getMat(), errMat;
CV_Assert(statusMat.isContinuous());
uchar* status = statusMat.ptr();
float* err = 0;
for (i = 0; i < npoints; i++)
status[i] = true;
if (_err.needed())
{
_err.create((int)npoints, 1, CV_32F, -1, true);
errMat = _err.getMat();
CV_Assert(errMat.isContinuous());
err = errMat.ptr<float>();
}
/***************************/
printf("\n 000 /*************************************/\n\n");
printf("size: %d,%d,%d,%d\n", _prevPts.size(), _nextPts.size(), _status.size(), _err.size());
printf("dtype: %d,%d,%d,%d,%d\n", _prevPts.type(), _nextPts.type(), _status.type(), _err.type(), CV_32FC2);
printf("\n 000 /*************************************/\n\n");
std::vector<Mat> prevPyr, nextPyr;
int levels1 = -1;
int lvlStep1 = 1;
int levels2 = -1;
int lvlStep2 = 1;
printf("_prevImg.kind() == _InputArray::STD_VECTOR_MAT : %d,%d", _prevImg.kind() == _InputArray::STD_VECTOR_MAT, _nextImg.kind() == _InputArray::STD_VECTOR_MAT);
if (_prevImg.kind() == _InputArray::STD_VECTOR_MAT)
{
_prevImg.getMatVector(prevPyr);
levels1 = int(prevPyr.size()) - 1;
CV_Assert(levels1 >= 0);
if (levels1 % 2 == 1 && prevPyr[0].channels() * 2 == prevPyr[1].channels() && prevPyr[1].depth() == derivDepth)
{
lvlStep1 = 2;
levels1 /= 2;
}
// ensure that pyramid has required padding
if (levels1 > 0)
{
Size fullSize;
Point ofs;
prevPyr[lvlStep1].locateROI(fullSize, ofs);
CV_Assert(ofs.x >= winSize.width && ofs.y >= winSize.height
&& ofs.x + prevPyr[lvlStep1].cols + winSize.width <= fullSize.width
&& ofs.y + prevPyr[lvlStep1].rows + winSize.height <= fullSize.height);
}
if (levels1 < maxLevel)
maxLevel = levels1;
//printf("size: %d,%d,%d,%d \n", _prevImg.size(), prevPyr.size(), levels1);
//printf("size: %d,%d,%d,%d \n", prevPyr[0].channels(), prevPyr[1].channels(), prevPyr[1].depth());
}
if (_nextImg.kind() == _InputArray::STD_VECTOR_MAT)
{
_nextImg.getMatVector(nextPyr);
levels2 = int(nextPyr.size()) - 1;
CV_Assert(levels2 >= 0);
if (levels2 % 2 == 1 && nextPyr[0].channels() * 2 == nextPyr[1].channels() && nextPyr[1].depth() == derivDepth)
{
lvlStep2 = 2;
levels2 /= 2;
}
// ensure that pyramid has required padding
if (levels2 > 0)
{
Size fullSize;
Point ofs;
nextPyr[lvlStep2].locateROI(fullSize, ofs);
CV_Assert(ofs.x >= winSize.width && ofs.y >= winSize.height
&& ofs.x + nextPyr[lvlStep2].cols + winSize.width <= fullSize.width
&& ofs.y + nextPyr[lvlStep2].rows + winSize.height <= fullSize.height);
}
if (levels2 < maxLevel)
maxLevel = levels2;
}
int pyrBorder = BORDER_REFLECT_101;
int derivBorder = BORDER_CONSTANT;
bool tryReuseInputImage = true;
if (levels1 < 0)
maxLevel = buildOpticalFlowPyramiddd(_prevImg, prevPyr, winSize, maxLevel, false, pyrBorder, derivBorder, tryReuseInputImage);
if (levels2 < 0)
maxLevel = buildOpticalFlowPyramiddd(_nextImg, nextPyr, winSize, maxLevel, false, pyrBorder, derivBorder, tryReuseInputImage);
printf("buildOpticalFlowPyramiddd : %d,%d,%d\n\n\n", maxLevel, prevPyr.size(), nextPyr.size());
if ((criteria.type & TermCriteria::COUNT) == 0)
criteria.maxCount = 30;
else
criteria.maxCount = std::min(std::max(criteria.maxCount, 0), 100);
if ((criteria.type & TermCriteria::EPS) == 0)
criteria.epsilon = 0.01;
else
criteria.epsilon = std::min(std::max(criteria.epsilon, 0.), 10.);
criteria.epsilon *= criteria.epsilon;
printf("criteria : %d,%.7f\n", criteria.maxCount, criteria.epsilon);
// dI/dx ~ Ix, dI/dy ~ Iy
Mat derivIBuf;
if (lvlStep1 == 1)
derivIBuf.create(prevPyr[0].rows + winSize.height * 2, prevPyr[0].cols + winSize.width * 2, CV_MAKETYPE(derivDepth, prevPyr[0].channels() * 2));
printf("derivIBuf : %d,%d,%d, %d\n", derivIBuf.rows, derivIBuf.cols, derivIBuf.channels(), lvlStep1);
for (level = maxLevel; level >= 0; level--)
{
printf("level : %d,%d,%d\n", maxLevel, level, lvlStep1);
Mat derivI;
if (lvlStep1 == 1)
{
Size imgSize = prevPyr[level * lvlStep1].size();
printf("imgSize : %d,%d\n", imgSize.width, imgSize.height);
Mat _derivI(imgSize.height + winSize.height * 2,
imgSize.width + winSize.width * 2, derivIBuf.type(), derivIBuf.ptr());
derivI = _derivI(Rect(winSize.width, winSize.height, imgSize.width, imgSize.height));
ScharrDerivInvoker(prevPyr[level * lvlStep1], derivI);
copyMakeBorder(derivI, _derivI, winSize.height, winSize.height, winSize.width, winSize.width, BORDER_CONSTANT | BORDER_ISOLATED);
}
else
derivI = prevPyr[level * lvlStep1 + 1];
CV_Assert(prevPyr[level * lvlStep1].size() == nextPyr[level * lvlStep2].size());
CV_Assert(prevPyr[level * lvlStep1].type() == nextPyr[level * lvlStep2].type());
LKTrackerInvoker(prevPyr[level * lvlStep1], derivI,
nextPyr[level * lvlStep2], prevPts, nextPts,
status, err,
winSize, criteria, level, maxLevel,
flags, (float)minEigThreshold, npoints);
}
return;
}
最终结果图示
log:
prepoints number: 613
prevPtsMat : 1,613,2
derivDepth : 3,
npoints : 613,
_nextPts size : 613,
nextPtsMat size : 1,613,2
000 /*************************************/
size: 613,613,613,613
dtype: 13,13,0,5,13
000 /*************************************/
_prevImg.kind() == _InputArray::STD_VECTOR_MAT : 0,0
pyramid size:4
tryReuseInputImage :1,0,0
0000000000temp.empty() 1, (0,0,1), 0,0
0000000000 288, 352 ,1, 330, 394,1
0000000001 288, 352 ,1, 288, 352,1
init sz : 288,352
sz : 176,144
pyramid : 1,1, type (0,0)
pyramid : shape 0,0, (144,176)
sz : 88,72
pyramid : 2,1, type (0,0)
pyramid : shape 0,0, (72,88)
sz : 44,36
pyramid : 3,1, type (0,0)
pyramid : shape 0,0, (36,44)
sz : 22,18
pyramid size:4
tryReuseInputImage :1,0,0
0000000000temp.empty() 1, (0,0,1), 0,0
0000000000 288, 352 ,1, 330, 394,1
0000000001 288, 352 ,1, 288, 352,1
init sz : 288,352
sz : 176,144
pyramid : 1,1, type (0,0)
pyramid : shape 0,0, (144,176)
sz : 88,72
pyramid : 2,1, type (0,0)
pyramid : shape 0,0, (72,88)
sz : 44,36
pyramid : 3,1, type (0,0)
pyramid : shape 0,0, (36,44)
sz : 22,18
buildOpticalFlowPyramiddd : 3,4,4
criteria : 30,0.0001000
derivIBuf : 330,394,2, 1
level : 3,3,1
imgSize : 44,36
************************************************************************
auto buffer size : 1323, 1,2 win buffer dtype : 3, 11,613
level : 3,2,1
imgSize : 88,72
************************************************************************
auto buffer size : 1323, 1,2 win buffer dtype : 3, 11,613
level : 3,1,1
imgSize : 176,144
************************************************************************
auto buffer size : 1323, 1,2 win buffer dtype : 3, 11,613
level : 3,0,1
imgSize : 352,288
************************************************************************
auto buffer size : 1323, 1,2 win buffer dtype : 3, 11,613
参考链接:
https://blog.csdn.net/findgeneralgirl/article/details/107919541 opencv光流代码理解
https://blog.csdn.net/tianqishi/article/details/118765928 opencv光流代码理解
https://zhuanlan.zhihu.com/p/88033287 LK光流原理https://blog.csdn.net/qq_30815237/article/details/87208319 LK光流原理
https://zhuanlan.zhihu.com/p/435949335 LK光流python复现