視覺十四講:第八講_光流法(特徵點追蹤)
1.直接法的引出
特徵點估計相機運動的方法,主要是在關鍵點和描述子的計算非常耗時;而且在紋理資訊比較少的情況下,特徵點的數量會明顯減少。
解決方案:
1.保留特徵點,只計算關鍵點,不計算描述子,然後使用光流法跟蹤特徵點的運動,從而實現特徵點的匹配。
2.只計算關鍵點,不計算描述子。使用直接法計算下一時刻特徵點的位置,從而實現特徵點的匹配。
第一種方法,是把特徵點匹配換成光流法,估計相機運動時仍然採用對極幾何、PnP或ICP演算法。仍然需要計算角點。
第二種方法,是通過畫素的灰度資訊,同時估計相機運動和點的投影,不要求提取到的點必須為角點,甚至可以是隨機的選點。
2.LK光流法
光流法基於灰度不變強假設。
對於t時刻位於(x,y)處的畫素,設t+dt時刻它運動到了(x+dx,y+dy)處,由於灰度不變,所以有:I(x,y,t) = I(x+dx,y+dy,t+dt).
將右邊進行泰勒展開,保留一階項:
\(I(x+dx,y+dy,t+dt) \approx I(x,y,t)+ \frac{\partial I}{\partial x}dx + \frac{\partial I}{\partial y}dy + \frac{\partial I}{\partial t}dt\)
由於I(x,y,t) = I(x+dx,y+dy,t+dt),所以:\(\frac{\partial I}{\partial x}dx + \frac{\partial I}{\partial y}dy + \frac{\partial I}{\partial t}dt = 0\)
兩邊除dt,得:
\(\frac{\partial I}{\partial x} \frac{dx}{dt} + \frac{\partial I}{\partial y} \frac{dy}{dt} =- \frac{\partial I}{\partial t}\)
其中dx/dt為畫素在x軸上的運動速度,dy/dt為y軸上的速度,記為u,v。同時\(\frac{\partial I}{\partial x}\)
我們計算的是u、v,所以一個點無法計算,故假設該點附近的一個視窗內的畫素都具有相同的運動。
考慮一個大小為w*w的視窗,共有\(w^{2}\)數量的畫素,有\(w^{2}\)個方程:
這是一個關於u,v的超定方程,傳統解法是求最小二乘解
這樣就得到畫素在影象間的運動速度u,v。由於畫素梯度僅在區域性有效,如果一次迭代不夠好,可以多迭代幾次這個方程。
影象梯度:
影象梯度一般也可以用中值差分:
dx(i,j) = [I(i+1,j) - I(i-1,j)]/2;
dy(i,j) = [I(i,j+1) - I(i,j-1)]/2;
LK光流程式:
vector<KeyPoint> kp1;
//通過GFTT來獲取角點
Ptr<GFTTDetector> detector = GFTTDetector::create(500, 0.01, 20); // maximum 500 keypoints
detector->detect(img1, kp1);
vector<Point2f> pt1, pt2;
for (auto &kp: kp1) pt1.push_back(kp.pt);
vector<uchar> status;
vector<float> error;
//直接呼叫LK光流函式
cv::calcOpticalFlowPyrLK(img1, img2, pt1, pt2, status, error);
3.高斯牛頓法實現光流
該問題可以看成一個優化問題:通過最小化灰度誤差來估計最優的畫素偏差。
相關程式碼:
GetPixelValue:採用雙線性內插法,來估計一個點的畫素:
公式為:
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) x = img.cols - 1;
if (y >= img.rows) y = img.rows - 1;
uchar *data = &img.data[int(y) * img.step + int(x)];
float xx = x - floor(x);
float yy = y - floor(y);
return float(
(1 - xx) * (1 - yy) * data[0] +
xx * (1 - yy) * data[1] +
(1 - xx) * yy * data[img.step] +
xx * yy * data[img.step + 1]
);
}
class OpticalFlowTracker {
public:
OpticalFlowTracker(
const Mat &img1_,
const Mat &img2_,
const vector<KeyPoint> &kp1_,
vector<KeyPoint> &kp2_,
vector<bool> &success_,
bool inverse_ = true, bool has_initial_ = false) :
img1(img1_), img2(img2_), kp1(kp1_), kp2(kp2_), success(success_), inverse(inverse_),
has_initial(has_initial_) {}
void calculateOpticalFlow(const Range &range);
private:
const Mat &img1;
const Mat &img2;
const vector<KeyPoint> &kp1;
vector<KeyPoint> &kp2;
vector<bool> &success;
bool inverse = true;
bool has_initial = false;
};
void OpticalFlowSingleLevel(
const Mat &img1,
const Mat &img2,
const vector<KeyPoint> &kp1,
vector<KeyPoint> &kp2,
vector<bool> &success,
bool inverse, bool has_initial) {
kp2.resize(kp1.size());
success.resize(kp1.size());
OpticalFlowTracker tracker(img1, img2, kp1, kp2, success, inverse, has_initial);
//通過cv::parallel_for_並行呼叫了calculateOpticalFlow函式,迴圈次數為kp1.size(),傳入了tracker資料。
parallel_for_(Range(0, kp1.size()),
std::bind(&OpticalFlowTracker::calculateOpticalFlow, &tracker, placeholders::_1));
}
void OpticalFlowTracker::calculateOpticalFlow(const Range &range) {
// parameters
int half_patch_size = 4; //視窗的大小為8*8
int iterations = 10; //每一個角點迭代10次
//總共的迴圈次數為 kp1.size()
for (size_t i = range.start; i < range.end; i++) {
auto kp = kp1[i];
double dx = 0, dy = 0; // dx,dy need to be estimated
if (has_initial) {
dx = kp2[i].pt.x - kp.pt.x;
dy = kp2[i].pt.y - kp.pt.y;
}
double cost = 0, lastCost = 0;
bool succ = true; // indicate if this point succeeded
// Gauss-Newton iterations
Eigen::Matrix2d H = Eigen::Matrix2d::Zero(); // hessian
Eigen::Vector2d b = Eigen::Vector2d::Zero(); // bias
Eigen::Vector2d J; // jacobian
for (int iter = 0; iter < iterations; iter++) {
if (inverse == false) {
H = Eigen::Matrix2d::Zero();
b = Eigen::Vector2d::Zero();
} else { //反向光流,H矩陣不變,只計算一次,每次迭代計算殘差b
// only reset b
b = Eigen::Vector2d::Zero();
}
cost = 0;
// compute cost and jacobian
//對視窗的所有畫素進行計算
for (int x = -half_patch_size; x < half_patch_size; x++)
for (int y = -half_patch_size; y < half_patch_size; y++) {
//計算誤差
double error = GetPixelValue(img1, kp.pt.x + x, kp.pt.y + y) -
GetPixelValue(img2, kp.pt.x + x + dx, kp.pt.y + y + dy);;
if (inverse == false) { //雅可比矩陣,為第二個影象在x+dx,y+dy處的梯度。梯度上面已講
J = -1.0 * Eigen::Vector2d(
0.5 * (GetPixelValue(img2, kp.pt.x + dx + x + 1, kp.pt.y + dy + y) -
GetPixelValue(img2, kp.pt.x + dx + x - 1, kp.pt.y + dy + y)),
0.5 * (GetPixelValue(img2, kp.pt.x + dx + x, kp.pt.y + dy + y + 1) -
GetPixelValue(img2, kp.pt.x + dx + x, kp.pt.y + dy + y - 1))
);
} else if (iter == 0) { //反向光流,計算第一張影象的雅可比矩陣,10次迭代只計算一次
// in inverse mode, J keeps same for all iterations
// NOTE this J does not change when dx, dy is updated, so we can store it and only compute error
J = -1.0 * Eigen::Vector2d(
0.5 * (GetPixelValue(img1, kp.pt.x + x + 1, kp.pt.y + y) -
GetPixelValue(img1, kp.pt.x + x - 1, kp.pt.y + y)),
0.5 * (GetPixelValue(img1, kp.pt.x + x, kp.pt.y + y + 1) -
GetPixelValue(img1, kp.pt.x + x, kp.pt.y + y - 1))
);
}
// compute H, b and set cost;
b += -error * J;
cost += error * error;
if (inverse == false || iter == 0) {
// also update H
H += J * J.transpose();
}
}
// compute update
Eigen::Vector2d update = H.ldlt().solve(b); //求解方程u,v
if (std::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) { //找到最小
break;
}
// 更新 dx, dy
dx += update[0];
dy += update[1];
lastCost = cost;
succ = true;
if (update.norm() < 1e-2) { //誤差範數已較小,直接退出
// converge
break;
}
}
success[i] = succ;
// kp2,找到了kp2和kp1的匹配點座標
kp2[i].pt = kp.pt + Point2f(dx, dy);
}
}