From 6a6f24b1704951d395ae3bda3fb16d894f57dd1f Mon Sep 17 00:00:00 2001 From: Ernest Galbrun Date: Fri, 18 Apr 2014 16:36:34 +0200 Subject: [PATCH] adding new ali's feature --- modules/video/src/tvl1flow.cpp | 110 +++++++++++++++++++++++---------- 1 file changed, 77 insertions(+), 33 deletions(-) diff --git a/modules/video/src/tvl1flow.cpp b/modules/video/src/tvl1flow.cpp index fad73ef65d..426ff9f0a5 100644 --- a/modules/video/src/tvl1flow.cpp +++ b/modules/video/src/tvl1flow.cpp @@ -100,6 +100,7 @@ protected: double tau; double lambda; double theta; + double gamma; int nscales; int warps; double epsilon; @@ -120,7 +121,8 @@ private: std::vector > I0s; std::vector > I1s; std::vector > u1s; - std::vector > u2s; + std::vector > u2s; + std::vector > u3s; Mat_ I1x_buf; Mat_ I1y_buf; @@ -141,15 +143,20 @@ private: Mat_ p11_buf; Mat_ p12_buf; Mat_ p21_buf; - Mat_ p22_buf; + Mat_ p22_buf; + Mat_ p31_buf; + Mat_ p32_buf; Mat_ div_p1_buf; - Mat_ div_p2_buf; + Mat_ div_p2_buf; + Mat_ div_p3_buf; Mat_ u1x_buf; Mat_ u1y_buf; Mat_ u2x_buf; - Mat_ u2y_buf; + Mat_ u2y_buf; + Mat_ u3x_buf; + Mat_ u3y_buf; } dm; struct dataUMat { @@ -343,6 +350,7 @@ OpticalFlowDual_TVL1::OpticalFlowDual_TVL1() nscales = 5; warps = 5; epsilon = 0.01; + gamma = 1.; innerIterations = 30; outerIterations = 10; useInitialFlow = false; @@ -367,13 +375,15 @@ void OpticalFlowDual_TVL1::calc(InputArray _I0, InputArray _I1, InputOutputArray dm.I0s.resize(nscales); dm.I1s.resize(nscales); dm.u1s.resize(nscales); - dm.u2s.resize(nscales); + dm.u2s.resize(nscales); + dm.u3s.resize(nscales); I0.convertTo(dm.I0s[0], dm.I0s[0].depth(), I0.depth() == CV_8U ? 1.0 : 255.0); I1.convertTo(dm.I1s[0], dm.I1s[0].depth(), I1.depth() == CV_8U ? 1.0 : 255.0); dm.u1s[0].create(I0.size()); - dm.u2s[0].create(I0.size()); + dm.u2s[0].create(I0.size()); + dm.u3s[0].create(I0.size()); if (useInitialFlow) { @@ -400,15 +410,20 @@ void OpticalFlowDual_TVL1::calc(InputArray _I0, InputArray _I1, InputOutputArray dm.p11_buf.create(I0.size()); dm.p12_buf.create(I0.size()); dm.p21_buf.create(I0.size()); - dm.p22_buf.create(I0.size()); + dm.p22_buf.create(I0.size()); + dm.p31_buf.create(I0.size()); + dm.p32_buf.create(I0.size()); dm.div_p1_buf.create(I0.size()); - dm.div_p2_buf.create(I0.size()); + dm.div_p2_buf.create(I0.size()); + dm.div_p3_buf.create(I0.size()); dm.u1x_buf.create(I0.size()); dm.u1y_buf.create(I0.size()); dm.u2x_buf.create(I0.size()); - dm.u2y_buf.create(I0.size()); + dm.u2y_buf.create(I0.size()); + dm.u3x_buf.create(I0.size()); + dm.u3y_buf.create(I0.size()); // create the scales for (int s = 1; s < nscales; ++s) @@ -433,16 +448,16 @@ void OpticalFlowDual_TVL1::calc(InputArray _I0, InputArray _I1, InputOutputArray else { dm.u1s[s].create(dm.I0s[s].size()); - dm.u2s[s].create(dm.I0s[s].size()); + dm.u2s[s].create(dm.I0s[s].size()); } + dm.u3s[s].create(dm.I0s[s].size()); } - if (!useInitialFlow) { dm.u1s[nscales - 1].setTo(Scalar::all(0)); dm.u2s[nscales - 1].setTo(Scalar::all(0)); - } - + } + dm.u3s[nscales - 1].setTo(Scalar::all(0)); // pyramidal structure for computing the optical flow for (int s = nscales - 1; s >= 0; --s) { @@ -457,9 +472,10 @@ void OpticalFlowDual_TVL1::calc(InputArray _I0, InputArray _I1, InputOutputArray // zoom the optical flow for the next finer scale resize(dm.u1s[s], dm.u1s[s - 1], dm.I0s[s - 1].size()); - resize(dm.u2s[s], dm.u2s[s - 1], dm.I0s[s - 1].size()); + resize(dm.u2s[s], dm.u2s[s - 1], dm.I0s[s - 1].size()); + resize(dm.u3s[s], dm.u3s[s - 1], dm.I0s[s - 1].size()); - // scale the optical flow with the appropriate zoom factor + // scale the optical flow with the appropriate zoom factor (don't scale u3!) multiply(dm.u1s[s - 1], Scalar::all(1 / scaleStep), dm.u1s[s - 1]); multiply(dm.u2s[s - 1], Scalar::all(1 / scaleStep), dm.u2s[s - 1]); } @@ -872,6 +888,10 @@ void CalcGradRhoBody::operator() (const Range& range) const // compute the constant part of the rho function rhoRow[x] = (I1wRow[x] - I1wxRow[x] * u1Row[x] - I1wyRow[x] * u2Row[x] - I0Row[x]); + //It = I1wRow[x] - I0Row[x] + //(u - u0)*i_X = I1wxRow[x] * u1Row[x] + //(v - v0)*i_Y = I1wyRow[x] * u2Row[x] + // gamma * w = gamma * u3 } } } @@ -911,12 +931,15 @@ struct EstimateVBody : ParallelLoopBody Mat_ I1wx; Mat_ I1wy; Mat_ u1; - Mat_ u2; + Mat_ u2; + Mat_ u3; Mat_ grad; Mat_ rho_c; mutable Mat_ v1; - mutable Mat_ v2; + mutable Mat_ v2; + mutable Mat_ v3; float l_t; + float gamma; }; void EstimateVBody::operator() (const Range& range) const @@ -926,65 +949,77 @@ void EstimateVBody::operator() (const Range& range) const const float* I1wxRow = I1wx[y]; const float* I1wyRow = I1wy[y]; const float* u1Row = u1[y]; - const float* u2Row = u2[y]; + const float* u2Row = u2[y]; + const float* u3Row = u3[y]; const float* gradRow = grad[y]; const float* rhoRow = rho_c[y]; float* v1Row = v1[y]; - float* v2Row = v2[y]; + float* v2Row = v2[y]; + float* v3Row = v3[y]; for (int x = 0; x < I1wx.cols; ++x) { - const float rho = rhoRow[x] + (I1wxRow[x] * u1Row[x] + I1wyRow[x] * u2Row[x]); + const float rho = rhoRow[x] + (I1wxRow[x] * u1Row[x] + I1wyRow[x] * u2Row[x]) + gamma * u3Row[x]; float d1 = 0.0f; float d2 = 0.0f; - + float d3 = 0.0f; +// add d3 for 3 cases if (rho < -l_t * gradRow[x]) { d1 = l_t * I1wxRow[x]; d2 = l_t * I1wyRow[x]; + d3 = l_t * gamma; } else if (rho > l_t * gradRow[x]) { d1 = -l_t * I1wxRow[x]; - d2 = -l_t * I1wyRow[x]; + d2 = -l_t * I1wyRow[x]; + d3 = -l_t * gamma; } else if (gradRow[x] > std::numeric_limits::epsilon()) { float fi = -rho / gradRow[x]; d1 = fi * I1wxRow[x]; d2 = fi * I1wyRow[x]; + d3 = fi * gamma; } v1Row[x] = u1Row[x] + d1; - v2Row[x] = u2Row[x] + d2; + v2Row[x] = u2Row[x] + d2; + v3Row[x] = u3Row[x] + d3; } } } -void estimateV(const Mat_& I1wx, const Mat_& I1wy, const Mat_& u1, const Mat_& u2, const Mat_& grad, const Mat_& rho_c, - Mat_& v1, Mat_& v2, float l_t) +void estimateV(const Mat_& I1wx, const Mat_& I1wy, const Mat_& u1, const Mat_& u2, const Mat_& u3, const Mat_& grad, const Mat_& rho_c, + Mat_& v1, Mat_& v2, Mat_& v3, float l_t, float gamma) { CV_DbgAssert( I1wy.size() == I1wx.size() ); CV_DbgAssert( u1.size() == I1wx.size() ); CV_DbgAssert( u2.size() == I1wx.size() ); + CV_DbgAssert( u3.size() == I1wx.size() ); CV_DbgAssert( grad.size() == I1wx.size() ); CV_DbgAssert( rho_c.size() == I1wx.size() ); CV_DbgAssert( v1.size() == I1wx.size() ); CV_DbgAssert( v2.size() == I1wx.size() ); + CV_DbgAssert( v3.size() == I1wx.size() ); EstimateVBody body; body.I1wx = I1wx; body.I1wy = I1wy; body.u1 = u1; - body.u2 = u2; + body.u2 = u2; + body.u3 = u3; body.grad = grad; body.rho_c = rho_c; body.v1 = v1; - body.v2 = v2; - body.l_t = l_t; + body.v2 = v2; + body.v3 = v3; + body.l_t = l_t; + body.gamma = gamma; parallel_for_(Range(0, I1wx.rows), body); } @@ -1019,6 +1054,8 @@ float estimateU(const Mat_& v1, const Mat_& v2, const Mat_& u1Row[x] = v1Row[x] + theta * divP1Row[x]; u2Row[x] = v2Row[x] + theta * divP2Row[x]; + //u3 + error += (u1Row[x] - u1k) * (u1Row[x] - u1k) + (u2Row[x] - u2k) * (u2Row[x] - u2k); } } @@ -1217,19 +1254,26 @@ void OpticalFlowDual_TVL1::procOneScale(const Mat_& I0, const Mat_ Mat_ p11 = dm.p11_buf(Rect(0, 0, I0.cols, I0.rows)); Mat_ p12 = dm.p12_buf(Rect(0, 0, I0.cols, I0.rows)); Mat_ p21 = dm.p21_buf(Rect(0, 0, I0.cols, I0.rows)); - Mat_ p22 = dm.p22_buf(Rect(0, 0, I0.cols, I0.rows)); + Mat_ p22 = dm.p22_buf(Rect(0, 0, I0.cols, I0.rows)); + Mat_ p31 = dm.p31_buf(Rect(0, 0, I0.cols, I0.rows)); + Mat_ p32 = dm.p32_buf(Rect(0, 0, I0.cols, I0.rows)); p11.setTo(Scalar::all(0)); p12.setTo(Scalar::all(0)); p21.setTo(Scalar::all(0)); - p22.setTo(Scalar::all(0)); + p22.setTo(Scalar::all(0)); + p31.setTo(Scalar::all(0)); + p32.setTo(Scalar::all(0)); Mat_ div_p1 = dm.div_p1_buf(Rect(0, 0, I0.cols, I0.rows)); - Mat_ div_p2 = dm.div_p2_buf(Rect(0, 0, I0.cols, I0.rows)); + Mat_ div_p2 = dm.div_p2_buf(Rect(0, 0, I0.cols, I0.rows)); + Mat_ div_p3 = dm.div_p2_buf(Rect(0, 0, I0.cols, I0.rows)); Mat_ u1x = dm.u1x_buf(Rect(0, 0, I0.cols, I0.rows)); Mat_ u1y = dm.u1y_buf(Rect(0, 0, I0.cols, I0.rows)); Mat_ u2x = dm.u2x_buf(Rect(0, 0, I0.cols, I0.rows)); - Mat_ u2y = dm.u2y_buf(Rect(0, 0, I0.cols, I0.rows)); + Mat_ u2y = dm.u2y_buf(Rect(0, 0, I0.cols, I0.rows)); + Mat_ u3x = dm.u3x_buf(Rect(0, 0, I0.cols, I0.rows)); + Mat_ u3y = dm.u3y_buf(Rect(0, 0, I0.cols, I0.rows)); const float l_t = static_cast(lambda * theta); const float taut = static_cast(tau / theta); @@ -1241,7 +1285,7 @@ void OpticalFlowDual_TVL1::procOneScale(const Mat_& I0, const Mat_ remap(I1, I1w, flowMap1, flowMap2, INTER_CUBIC); remap(I1x, I1wx, flowMap1, flowMap2, INTER_CUBIC); remap(I1y, I1wy, flowMap1, flowMap2, INTER_CUBIC); - + //calculate I1(x+u0) and its gradient calcGradRho(I0, I1w, I1wx, I1wy, u1, u2, grad, rho_c); float error = std::numeric_limits::max();