double tau;
double lambda;
double theta;
+ double gamma;
int nscales;
int warps;
double epsilon;
std::vector<Mat_<float> > I0s;
std::vector<Mat_<float> > I1s;
std::vector<Mat_<float> > u1s;
- std::vector<Mat_<float> > u2s;
+ std::vector<Mat_<float> > u2s;
+ std::vector<Mat_<float> > u3s;
Mat_<float> I1x_buf;
Mat_<float> I1y_buf;
Mat_<float> p11_buf;
Mat_<float> p12_buf;
Mat_<float> p21_buf;
- Mat_<float> p22_buf;
+ Mat_<float> p22_buf;
+ Mat_<float> p31_buf;
+ Mat_<float> p32_buf;
Mat_<float> div_p1_buf;
- Mat_<float> div_p2_buf;
+ Mat_<float> div_p2_buf;
+ Mat_<float> div_p3_buf;
Mat_<float> u1x_buf;
Mat_<float> u1y_buf;
Mat_<float> u2x_buf;
- Mat_<float> u2y_buf;
+ Mat_<float> u2y_buf;
+ Mat_<float> u3x_buf;
+ Mat_<float> u3y_buf;
} dm;
struct dataUMat
{
nscales = 5;
warps = 5;
epsilon = 0.01;
+ gamma = 1.;
innerIterations = 30;
outerIterations = 10;
useInitialFlow = false;
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)
{
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)
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)
{
// 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]);
}
// 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
}
}
}
Mat_<float> I1wx;
Mat_<float> I1wy;
Mat_<float> u1;
- Mat_<float> u2;
+ Mat_<float> u2;
+ Mat_<float> u3;
Mat_<float> grad;
Mat_<float> rho_c;
mutable Mat_<float> v1;
- mutable Mat_<float> v2;
+ mutable Mat_<float> v2;
+ mutable Mat_<float> v3;
float l_t;
+ float gamma;
};
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<float>::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_<float>& I1wx, const Mat_<float>& I1wy, const Mat_<float>& u1, const Mat_<float>& u2, const Mat_<float>& grad, const Mat_<float>& rho_c,
- Mat_<float>& v1, Mat_<float>& v2, float l_t)
+void estimateV(const Mat_<float>& I1wx, const Mat_<float>& I1wy, const Mat_<float>& u1, const Mat_<float>& u2, const Mat_<float>& u3, const Mat_<float>& grad, const Mat_<float>& rho_c,
+ Mat_<float>& v1, Mat_<float>& v2, Mat_<float>& 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);
}
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);
}
}
Mat_<float> p11 = dm.p11_buf(Rect(0, 0, I0.cols, I0.rows));
Mat_<float> p12 = dm.p12_buf(Rect(0, 0, I0.cols, I0.rows));
Mat_<float> p21 = dm.p21_buf(Rect(0, 0, I0.cols, I0.rows));
- Mat_<float> p22 = dm.p22_buf(Rect(0, 0, I0.cols, I0.rows));
+ Mat_<float> p22 = dm.p22_buf(Rect(0, 0, I0.cols, I0.rows));
+ Mat_<float> p31 = dm.p31_buf(Rect(0, 0, I0.cols, I0.rows));
+ Mat_<float> 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_<float> div_p1 = dm.div_p1_buf(Rect(0, 0, I0.cols, I0.rows));
- Mat_<float> div_p2 = dm.div_p2_buf(Rect(0, 0, I0.cols, I0.rows));
+ Mat_<float> div_p2 = dm.div_p2_buf(Rect(0, 0, I0.cols, I0.rows));
+ Mat_<float> div_p3 = dm.div_p2_buf(Rect(0, 0, I0.cols, I0.rows));
Mat_<float> u1x = dm.u1x_buf(Rect(0, 0, I0.cols, I0.rows));
Mat_<float> u1y = dm.u1y_buf(Rect(0, 0, I0.cols, I0.rows));
Mat_<float> u2x = dm.u2x_buf(Rect(0, 0, I0.cols, I0.rows));
- Mat_<float> u2y = dm.u2y_buf(Rect(0, 0, I0.cols, I0.rows));
+ Mat_<float> u2y = dm.u2y_buf(Rect(0, 0, I0.cols, I0.rows));
+ Mat_<float> u3x = dm.u3x_buf(Rect(0, 0, I0.cols, I0.rows));
+ Mat_<float> u3y = dm.u3y_buf(Rect(0, 0, I0.cols, I0.rows));
const float l_t = static_cast<float>(lambda * theta);
const float taut = static_cast<float>(tau / theta);
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<float>::max();