voidOpticalFlowSingleLevel( const Mat &img1, //为第一幅图 const Mat &img2, //为第二幅图 const vector<KeyPoint> &kp1, //第一幅图关键点 vector<KeyPoint> &kp2,//第二幅图关键点，待求，输入的时候是一个空的vector vector<bool> &success,//返回值，判断是否计算成功 bool inverse//是否为反向光流 ){
// parameters int half_patch_size = 4; int iterations = 10; bool have_initial = !kp2.empty(); // 对每一个关键点进行计算 for (size_t i = 0; i < kp1.size(); i++) { auto kp = kp1[i]; // 如果是第一次计算，dx=dy=0 double dx = 0, dy = 0; // dx,dy need to be estimated // 若不是第一次计算，则为两幅图对应关键点的差值 if (have_initial) { dx = kp2[i].pt.x - kp.pt.x; dy = kp2[i].pt.y - kp.pt.y; }
double cost = 0, lastCost = 0; // NOTE this J does not change when dx, dy is updated, so we can store it and only compute error
bool succ = true; // indicate if this point succeeded
// 针对每一个点，都通过高斯牛顿法求解和他对应的在第二张图中的点。8x8的框 for (int iter = 0; iter < iterations; iter++) { Eigen::Matrix2d H = Eigen::Matrix2d::Zero(); Eigen::Vector2d b = Eigen::Vector开，然后我们寻找2d::Zero(); cost = 0;
if (kp.pt.x + dx <= half_patch_size || kp.pt.x + dx >= img1.cols - half_patch_size || kp.pt.y + dy <= half_patch_size || kp.pt.y + dy >= img1.rows - half_patch_size) { // go outside succ = false; break; }
// 循环8x8的框中的每一个点，其实在这里构造8x8的框，我认为是更好的求Delta x。可以更精确的找到下降的方向。 // 比如两个点也可以求直线，但是他不可以很好的代表所有点所表示的线，需要考虑求所有点，然后最小二乘，那就可以找到近似的线 for (int x = -half_patch_size; x < half_patch_size; x++) for (int y = -half_patch_size; y < half_patch_size; y++) {
// GetPixelValue函数是可以求任何值对应的亮度，通过插值计算得到 double error = GetPixelValue(img1, x + kp.pt.x, y + kp.pt.y) - GetPixelValue(img2, x + kp.pt.x + dx, y + kp.pt.y + dy); Eigen::Vector2d J; // Jacobian if (inverse == false) { // f(x,y)_x = (f(x+1,y) - f(x-1,y))/2 J << -0.5 * (GetPixelValue(img2, x+kp.pt.x+dx+1, y+kp.pt.y+dy) - GetPixelValue(img2, x+kp.pt.x+dx - 1, y+kp.pt.y+dy)), -0.5 * (GetPixelValue(img2, x+kp.pt.x+dx, y+kp.pt.y+dy + 1) - GetPixelValue(img2, x+kp.pt.x+dx, y+kp.pt.y+dy - 1)); } else { // Inverse Jacobian // NOTE this J does not change when dx, dy is updated, so we can store it and only compute error J << -0.5 * (GetPixelValue(img1, x+kp.pt.x+dx+1, y+kp.pt.y+dy) - GetPixelValue(img1, x+kp.pt.x+dx - 1, y+kp.pt.y+dy)), -0.5 * (GetPixelValue(img1, x+kp.pt.x+dx, y+kp.pt.y+dy + 1) - GetPixelValue(img1, x+kp.pt.x+dx, y+kp.pt.y+dy - 1)); } // compute H, b and set cost; H += J * J.transpose(); b += -error *J; cost += error * error; // TODO END YOUR CODE HERE } // compute update // TODO START YOUR CODE HERE (~1 lines) Eigen::Vector2d update = H.ldlt().solve(b); // TODO END YOUR CODE HERE
if (isnan(update[0])) { // sometimes occurred when we have a black or white patch and H is irreversible cout << "update is nan" << endl; succ = false; break; } if (iter > 0 && cost > lastCost) { cout << "cost increased: " << cost << ", " << lastCost << endl; break; }
// update dx, dy dx += update[0]; dy += update[1]; lastCost = cost; succ = true; }
