My question is regarding the Opencv CV::calcOpticalFlowPyrLK implementation. I found in their docs ( OpenCV: Object Tracking) that they should use an Affine Lucas Kanade implementation, but when I’m looking in their github I cannot find any affine matrix in their optimisation (opencv/modules/video/src/lkpyramid.cpp at 4.x · opencv/opencv · GitHub). However, I implemented a standart sparse LK with and without affine optimisation, but still cannot get the same result like in OpenCV. I wonder if I did something wrong in my code. I used the Scharr operator to calculate the derivative and used a Gauß-Newton optimisation to solve it. I hope that anyone of you had the same problem or has more knowledge and can help me to get a similar or better result like in OpenCV.
Some things I should mention. If my window size is not at least twice as much as in OpenCV my display movement will be very short, except I set the iteration to 2000 or higher. It seems like a gradient descent method then.
Below is my code for the standart LK with Scharr Filter:
#include <opencv2/opencv.hpp>
#include <string>
#include <chrono>
#include <vector>
#include <Eigen/Core>
#include <Eigen/Dense>
std::string file_1 = "../000001.png"; // first image
std::string file_2 = "../000005.png"; // second image
namespace opticalFlow {
void calcOpticalFlowLKPyr(const cv::Mat img1, std::vector<cv::Point2f> pt1,const cv::Mat img2, std::vector<cv::Point2f> &pt2, cv::Size winSize, int pyr_lvl, int iteration_max);
int create_pyramid(const cv::Mat img, std::vector<cv::Mat> &pyramide, cv::Size winSize,int num_level, bool withDerivatives=false, int pyrBorder = cv::BORDER_REFLECT_101, int derivBorder=cv::BORDER_CONSTANT, bool tryReuseInputImage=false);
inline float GetPixelValue(const cv::Mat &img, float x, float y);
double getError(const cv::Mat img1, cv::Point2f pt1, int x, const cv::Mat img2, cv::Point2f pt2, int y, Eigen::Matrix2d A);
void OpticalFlowSingleLevel(const cv::Mat img1, std::vector<cv::Point2f> pt1, const cv::Mat img2, std::vector<cv::Point2f> &pt2, cv::Size winSize, int iteration_max);
double ScharrFilterX(const cv::Mat img, float x, float y);
double ScharrFilterY(const cv::Mat img, float x, float y);
Eigen::Vector2d calcDerivative(const cv::Mat img1, cv::Point2f p1, float x, float y);
int create_pyramid(const cv::Mat img, std::vector<cv::Mat> &pyramide, cv::Size winSize,int num_level, bool withDerivatives, int pyrBorder, int derivBorder, bool tryReuseInputImage) {
// return cv::buildOpticalFlowPyramid(img, pyramide, winSize, num_level, withDerivatives, pyrBorder, derivBorder, tryReuseInputImage);
pyramide.resize(num_level);
img.convertTo(pyramide[0],CV_32FC1, 1.0/255.0);
for ( int i = 1; i < num_level; i++ ) cv::pyrDown(pyramide[i-1],pyramide[i]);
return num_level;
}
double getError(const cv::Mat img1, cv::Point2f pt1, int x, const cv::Mat img2, cv::Point2f pt2, int y, Eigen::Matrix2d A) {
return (double) (GetPixelValue(img1, pt1.x + x, pt1.y + y) - GetPixelValue(img2, pt2.x + x, pt2.y + y)); //GetPixelValue(img2, pt2.x + (A(0,0)*x + A(0,1)*y), pt2.y + (A(1,0)*x + A(1,1)*y) ));
}
double ScharrFilterX(const cv::Mat img, float x, float y) {
return -3 * GetPixelValue(img, x-1, y-1) + 0 * GetPixelValue(img, x, y-1) + 3 * GetPixelValue(img, x+1, y-1) +
-10 * GetPixelValue(img, x-1, y) + 0 * GetPixelValue(img, x, y) + 10 * GetPixelValue(img, x+1, y) +
-3 * GetPixelValue(img, x-1, y+1) + 0 * GetPixelValue(img, x, y+1) + 3 * GetPixelValue(img, x+1, y+1);
}
double ScharrFilterY(const cv::Mat img, float x, float y) {
return 3 * -GetPixelValue(img, x-1, y-1) + 10 * -GetPixelValue(img, x, y-1) + 3 * -GetPixelValue(img, x+1, y-1) +
0 * GetPixelValue(img, x-1, y) + 0 * GetPixelValue(img, x, y) + 0 * GetPixelValue(img, x+1, y) +
3 * GetPixelValue(img, x-1, y+1) + 10 * GetPixelValue(img, x, y+1) + 3 * GetPixelValue(img, x+1, y+1);
}
Eigen::Vector2d calcDerivative(const cv::Mat img1, cv::Point2f p1, float x, float y) {
return -1 *Eigen::Vector2d(1 * ScharrFilterX(img1, p1.x + x, p1.y + y), 1 * ScharrFilterY(img1, p1.x + x, p1.y + y));
}
void OpticalFlowSingleLevel(const cv::Mat img1, std::vector<cv::Point2f> pt1, const cv::Mat img2, std::vector<cv::Point2f> &pt2, cv::Size winSize, int iteration_max) {
std::vector<Eigen::Matrix2d> Affine(pt1.size(), Eigen::Matrix2d::Identity());
int half_patch_size_x = std::floor(winSize.width / 2);
int half_patch_size_y = std::floor(winSize.height / 2);
for(int pt_id=0; pt_id < pt1.size(); pt_id++) {
cv::Point2f p1 = pt1[pt_id];
cv::Point2f p2 = pt2[pt_id];
Eigen::Matrix2d A = Affine[pt_id];
Eigen::Matrix2d H = Eigen::Matrix2d::Zero();
Eigen::Vector2d g;
Eigen::Vector2d J;
for(int iter=0; iter < iteration_max; iter++) {
double cost = 0;
g = Eigen::Vector2d::Zero();
J = Eigen::Vector2d::Zero();
for (int x = -half_patch_size_x; x < half_patch_size_x; x++) {
for(int y = -half_patch_size_y; y < half_patch_size_y; y++) {
double error = getError(img1, p1, x, img2, p2, y, A);
J = calcDerivative(img1, p1, x, y);
cost += error * error;
g += Eigen::Vector2d(-J(0) * error,
-J(1) * error);
if(iter == 0) {
double Ix = J(0);
double Iy = J(1);
Eigen::Matrix2d tmpM;
tmpM << Ix * Ix, Ix * Iy,
Iy * Ix, Iy * Iy;
H += tmpM;
}
}
}
Eigen::Vector2d update = H.ldlt().solve(g);
p2.x += update(0);
p2.y += update(1);
std::cout << img1.size() << " " << pt_id << "\t"<< iter << "\t" << std::sqrt(update(0)*update(0) + update(1)*update(1)) << "\t" << std::sqrt(cost) << std::endl;
}
pt2[pt_id] = p2;
}
}
void calcOpticalFlowLKPyr(const cv::Mat img1, std::vector<cv::Point2f> pt1 ,const cv::Mat img2, std::vector<cv::Point2f> &pt2, cv::Size winSize, int pyr_lvl, int iteration_max) {
std::vector<cv::Mat> pyr_img1;
std::vector<cv::Mat> pyr_img2;
std::vector<cv::Point2f> pyr_pt1 = pt1;
std::vector<cv::Point2f> pyr_pt2 = pt2;
int lvl1 = create_pyramid(img1, pyr_img1, winSize, pyr_lvl);
int lvl2 = create_pyramid(img2, pyr_img2, winSize, pyr_lvl);
if(lvl1 != lvl2) {
std::cerr << "Different pyramid lvl" << std::endl;
exit(-1);
}
if(pyr_pt2.size() != pyr_pt1.size())
pyr_pt2 = pyr_pt1;
float scale = pow(2.f,-(pyr_lvl-1));
for(int i = 0; i < pt1.size(); i++) {
pyr_pt2[i] = pt1[i] * scale;
}
for(int level = pyr_lvl - 1; level >= 0; level--) {
float scale = pow(2.f,-level);
for ( int i = 0; i < pyr_pt1.size(); i++ )
{
pyr_pt1[i].x = pt1[i].x*scale;
pyr_pt1[i].y = pt1[i].y*scale;
}
OpticalFlowSingleLevel(pyr_img1[level], pyr_pt1, pyr_img2[level], pyr_pt2, winSize, iteration_max);
if ( level > 0 ) {
for ( int i = 0; i < pyr_pt2.size(); i++ )
{
pyr_pt2[i].x *= 2;
pyr_pt2[i].y *= 2;
}
}
}
pt2 = pyr_pt2;
}
inline float GetPixelValue(const cv::Mat &img, float x, float y) {
// boundary check
if (x < 0) x = 0;
if (y < 0) y = 0;
if (x >= img.cols - 1) x = img.cols - 2;
if (y >= img.rows - 1) y = img.rows - 2;
float xx = x - floor(x);
float yy = y - floor(y);
int x_a1 = std::min(img.cols - 1, int(x) + 1);
int y_a1 = std::min(img.rows - 1, int(y) + 1);
return (1 - xx) * (1 - yy) * img.at<float>(y, x)
+ xx * (1 - yy) * img.at<float>(y, x_a1)
+ (1 - xx) * yy * img.at<float>(y_a1, x)
+ xx * yy * img.at<float>(y_a1, x_a1);
}
};
int main(int argc, char* argv[]) {
std::cout << "Start" << std::endl;
int winx = std::atoi(argv[1]);
int winy = std::atoi(argv[2]);
int pyr_lvl = std::atoi(argv[3]);
int iteration_max = std::atoi(argv[4]);
int feature_num = std::atoi(argv[5]);
cv::Mat img1 = cv::imread(file_1);
cv::Mat img2 = cv::imread(file_2);
cv::Mat gray_img1, gray_img2;
cv::cvtColor(img1, gray_img1, cv::COLOR_BGR2GRAY);
cv::cvtColor(img2, gray_img2, cv::COLOR_BGR2GRAY);
std::vector<cv::Point2f> pt1;
std::vector<cv::Point2f> pt2;
cv::Size winSize = cv::Size(winx,winy);
int lvl = pyr_lvl;
std::vector<cv::KeyPoint> kp1;
cv::Ptr<cv::GFTTDetector> detector = cv::GFTTDetector::create(feature_num, 0.01, 20); // maximum 500 keypoints
detector->detect(gray_img1, kp1);
for(auto ptt:kp1) {
cv::Point2f tmp = ptt.pt;
pt1.push_back(tmp);
pt2.push_back(tmp);
}
opticalFlow::calcOpticalFlowLKPyr(gray_img1,pt1, gray_img2, pt2, winSize, lvl, iteration_max);
std::cout << "End" << std::endl;
cv::Mat img2_CV2;
img2_CV2 = img2.clone();
for (int i = 0; i < pt2.size(); i++) {
if (true) {
cv::circle(img2_CV2, pt1[i], 2, cv::Scalar(0, 250, 0), 2);
cv::line(img2_CV2, pt1[i], pt2[i], cv::Scalar(0, 250, 0));
}
}
cv::imshow("tracked by myself", img2_CV2);
cv::waitKey(0);
}
I hope someone can help me with that.