void Calc::computeVelocity(const cv::Mat& old, const cv::Mat& current)
{
// Smooth
// cv::GaussianBlur(current, current, cv::Size(3, 3), 0, 0);
// cv::dilate(current, current, cv::Mat());
// Moments
cv::Mat_<double> It = current - old;
cv::Mat_<double> Ix, Iy;
cv::Sobel(old, Ix, CV_64F, 1, 0);
cv::Sobel(old, Iy, CV_64F, 0, 1);
cv::Mat_<double> Ixy = Ix.mul(Iy);
cv::Mat_<double> Ixt = Ix.mul(It);
cv::Mat_<double> Iyt = Iy.mul(It);
Ix = Ix.mul(Ix);
Iy = Iy.mul(Iy);
// Convolution
cv::GaussianBlur(Ix , Ix , _window_size, 0, 0);
cv::GaussianBlur(Iy , Iy , _window_size, 0, 0);
cv::GaussianBlur(Ixy, Ixy, _window_size, 0, 0);
cv::GaussianBlur(Ixt, Ixt, _window_size, 0, 0);
cv::GaussianBlur(Iyt, Iyt, _window_size, 0, 0);
// Computation
for (size_t i = 0; i < _ny; ++i)
{
for (size_t j = 0; j < _nx; ++j)
{
const size_t ii = boost::math::round(It.size().height * (i + 0.5) / _ny);
const size_t jj = boost::math::round(It.size().width * (j + 0.5) / _nx);
cv::Matx22d M;
M(0, 0) = Ix(ii, jj);
M(0, 1) = Ixy(ii, jj);
M(1, 0) = Ixy(ii, jj);
M(1, 1) = Iy(ii, jj);
cv::Matx21d b, x;
b(0) = -Ixt(ii, jj);
b(1) = -Iyt(ii, jj);
cv::solve(M, b, x);
_out.velocity.at<float>(i, j, 0) = _scale * x(0);
_out.velocity.at<float>(i, j, 1) = _scale * x(1);
}
}
// Smooth
// cv::GaussianBlur(_out.velocity, _out.velocity, cv::Size(3, 3), 0, 0);
}
Point2f OpticalFlowLK(const Mat& f1, const Mat& f2) {
It it = It(f1, f2);
Ixy ixy = Ixy(f2);
Matrix mat;
mat.calcInvATA(ixy);
mat.calcMat(it, ixy);
Point2f resUV = mat.calcD();
return resUV;
}
