一、ORB 特征點
ORB(Oriented FAST and BRIEF) 特征是 SLAM 中一種很常用的特征，由於其二進制特性，使得它 可以非常快速地提取與計算 [1]。下面，你將按照本題的指導，自行書寫 ORB 的提取、描述子的計算以及 匹配的代碼。代碼框架參照 computeORB.cpp 文件，圖像見 1.png 文件和 2.png。
1.1 ORB 提取
ORB 即 Oriented FAST 簡稱。它實際上是 FAST 特征再加上一個旋轉量。本習題將使用 OpenCV 自 帶的 FAST 提取算法，但是你要完成旋轉部分的計算。旋轉的計算過程描述如下 [2]：
在一個小圖像塊中，先計算質心。質心是指以圖像塊灰度值作為權重的中心。
- 在一個小的圖像塊 B 中，定義圖像塊的矩為：
\(m_{pq} = \sum_{x,y \in B} x^py^qI(x,y), p,q = \{0,1\}\)
- 通過矩可以找到圖像塊的質心：
\(C=(\frac{m_{10}}{m_{00}},\frac{m_{01}}{m_{00}})\)
- 連接圖像塊的幾何中心 O 與質心 C，得到一個方向向量 \(\overrightarrow{OC}\)，於是特征點的方向可以定義為：
\(\theta = \arctan(m_{01}/m_{10})\)
實際上只需計算 \(m_{01}\) 和 \(m_{10}\) 即可。習題中取圖像塊大小為 16x16，即對於任意點 (u,v)，圖像塊從 (u−8,v−8) 取到 (u + 7,v + 7) 即可。請在習題的 computeAngle 中，為所有特征點計算這個旋轉角。
提示：
-
由於要取圖像 16x16 塊，所以位於邊緣處的點（比如 u < 8 的）對應的圖像塊可能會出界，此時 需要判斷該點是否在邊緣處，並跳過這些點。
-
由於矩的定義方式，在畫圖特征點之后，角度看起來總是指向圖像中更亮的地方。
-
std::atan 和 std::atan2 會返回弧度制的旋轉角，但 OpenCV 中使用角度制，如使用 std::atan 類 函數，請轉換一下。 作為驗證，第一個圖像的特征點如圖 1 所示。看不清可以放大看。
// compute the angle
void computeAngle(const cv::Mat &image, vector<cv::KeyPoint> &keypoints) {
int half_patch_size = 8;
for (auto &kp : keypoints) {
// START YOUR CODE HERE (~7 lines)
kp.angle = 0; // compute kp.angle
int u = kp.pt.x;
int v = kp.pt.y; //height y => image row
if( u - 8 >= 0 && u + 7 <= image.cols && v - 8 >= 0 && v + 7 <= image.rows){ // vaild keypoint
int m01 = 0;
int m10 = 0;
// (row, col) <=> (h,w) <=> (v,u)
for(int j = -8; j < 8; j++){
for(int i = -8; i < 8; i++){
m01 += j*image.at<uchar>(v + j, u + i); // y*Iuv
m10 += i*image.at<uchar>(v + j, u + i); // x*Iuv
}
}
kp.angle = (float)atan2(m01, m10)*180/pi;
}
// END YOUR CODE HERE
}
return;
}
1.2 ORB 描述
ORB 描述即帶旋轉的 BRIEF 描述。所謂 BRIEF 描述是指一個 0-1 組成的字符串（可以取 256 位或 128 位），每一個 bit 表示一次像素間的比較。算法流程如下：
- 給定圖像 I 和關鍵點 (u,v)，以及該點的轉角 θ。以 256 位描述為例，那么最終描述子
\(d=[d_1, d_2, \cdots, d_{256}]\)
- 對任意 i = 1,...,256，\(d_i\) 的計算如下。取 (u,v) 附近任意兩個點 p,q，並按照 θ 進行旋轉：
\(\begin{bmatrix} u_p'\\ v_p' \end{bmatrix}= \begin{bmatrix} \cos \theta & -\sin\theta\\ \sin\theta & \cos \theta \end{bmatrix} \begin{bmatrix} u_p\\ v_p \end{bmatrix}\)
這樣我們就得到了 ORB 的描述。我們在程序中用 256 個 bool 變量表達這個描述2。請你完成 computeORBDesc 函數，實現此處計算。注意，通常我們會固定 p,q 的取法（稱為 ORB 的 pattern），否則每 次都重新隨機選取，會使得描述不穩定。我們在全局變量 ORB_pattern 中定義了 p,q 的取法，格式為 up,vp,uq,vq。請你根據給定的 pattern 完成 ORB 描述的計算。
提示：
-
p,q 同樣要做邊界檢查，否則會跑出圖像外。如果跑出圖像外，就設這個描述子為空。
-
調用 cos 和 sin 時同樣請注意弧度和角度的轉換。
// START YOUR CODE HERE (~7 lines)
d[i] = 0; // if kp goes outside, set d.clear()
int pattern[4], pattern_R[4]; // up, vp, uq, vq;
for(int j = 0; j < 4; j++){
pattern[j] = ORB_pattern[i*4 + j];
}
// rotation
pattern_R[0] = pattern[0]*cos(kp.angle/180*pi) - pattern[1]*sin(kp.angle/180*pi);
pattern_R[1] = pattern[0]*sin(kp.angle/180*pi) + pattern[1]*cos(kp.angle/180*pi);
pattern_R[2] = pattern[2]*cos(kp.angle/180*pi) - pattern[3]*sin(kp.angle/180*pi);
pattern_R[3] = pattern[2]*sin(kp.angle/180*pi) + pattern[3]*cos(kp.angle/180*pi);
// point
pattern_R[0] += kp.pt.x;
pattern_R[1] += kp.pt.y;
pattern_R[2] += kp.pt.x;
pattern_R[3] += kp.pt.y;
if(pattern_R[0] < 0 || pattern_R[0] >= image.cols || pattern_R[1] < 0 || pattern_R[1] >= image.rows
|| pattern_R[2] < 0 || pattern_R[2] >= image.cols || pattern_R[3] < 0 || pattern_R[3] >= image.rows){
d.clear();
break;
}else{
d[i] = (image.at<uchar>(pattern_R[1], pattern_R[0]) > image.at<uchar>(pattern_R[3], pattern_R[2])) ? 0 : 1;
}
// END YOUR CODE HERE
1.3 暴力匹配
在提取描述之后，我們需要根據描述子進行匹配。暴力匹配是一種簡單粗暴的匹配方法，在特征點不多時很有用。下面你將根據習題指導，書寫暴力匹配算法。
所謂暴力匹配思路很簡單。給定兩組描述子 \(P = [p_1,...,p_M]\) 和 \(Q = [q_1,...,q_N]\)。那么，對 \(P\) 中任意一個點，找到 \(Q\) 中對應最小距離點，即算一次匹配。但是這樣做會對每個特征點都找到一個匹配，所以我們通常還會限制一個距離閾值 \(d_{max}\)，即認作匹配的特征點距離不應該大於 \(d_{max}\)。下面請你根據上述描述，實現函數 bfMatch，返回給定特征點的匹配情況。實踐中取 \(d_{max} = 50\)。
提示：
-
你需要按位計算兩個描述子之間的漢明距離。
-
OpenCV 的 DMatch 結構，queryIdx 為第一圖的特征 ID，trainIdx 為第二個圖的特征 ID。
-
作為驗證，匹配之后輸出圖像應如圖 2 所示。
// find matches between desc1 and desc2.
int d1_id = -1;
for(auto &d1: desc1){
d1_id++;
if(!d1.empty()){
vector<vector<int> > d1_match(0, vector<int>(2)); // record [hanmming, d1_id]
int d2_id = -1;
for(auto &d2: desc2){
d2_id++;
if(!d2.empty()){
int hamming = 0;// init
for(int i = 0; i < 256; i++){
hamming += (d1[i] == d2[i]) ? 0 : 1;
}
d1_match.push_back({hamming, d2_id});
}
}
sort(d1_match.begin(), d1_match.end()); //hamming, order lowest to sort
if(d1_match[0][0] < d_max){
cv::DMatch m;
m.queryIdx = d1_id;
m.trainIdx = d1_match[0][1]; //d2_id
m.distance = d1_match[0][0];
matches.push_back(m);
}
}
}
// END YOUR CODE HERE
最后，請結合實驗，回答下面幾個問題：
-
為什么說 ORB 是一種二進制特征？
因為其描述子是以二進制表示的
-
為什么在匹配時使用 50 作為閾值，取更大或更小值會怎么樣？
閾值取得過大會導致誤匹配率增加；閾值取得過小會導致匹配對過少。這兩種情況會導致后續的位姿求解不穩定。
-
暴力匹配在你的機器上表現如何？你能想到什么減少計算量的匹配方法嗎？
在我的機器上秒出，后續可以采用快速最近鄰匹配法。
二、從 E 恢復 R,t
我們在書中講到了單目對極幾何部分，可以通過本質矩陣 E，得到旋轉和平移 R,t，但那時直接使用了 OpenCV 提供的函數。本題中，請你根據數學原理，完成從 E 到 R,t 的計算。程序框架見 code/E2Rt.cpp.
設 Essential 矩陣 E 的取值為（與書上實驗數值相同）：
\(E=\begin{bmatrix} −0.0203618550523477 & −0.4007110038118445 & −0.03324074249824097\\ 0.3939270778216369 & −0.03506401846698079 & 0.5857110303721015\\ −0.006788487241438284 & −0.5815434272915686 & −0.01438258684486258 \end{bmatrix}\)
請計算對應的 R,t，流程如下：
- 對 E 作 SVD 分解：
\(E=U\sum V^T\)
- 處理 Σ 的奇異值。設 Σ = diag(σ1,σ2,σ3) 且 σ1 ≥ σ2 ≥ σ3，那么處理后的 Σ 為：
\(\sum = diag(\frac{\sigma_1 + \sigma_2}{2}, \frac{\sigma_1 + \sigma_2}{2}, 0)\)
\(t^∧_1 = UR_Z(\frac{π}{2})ΣU^T\), \(R_1 = UR^T_Z(\frac{π}{2})V^T\)
\(t^∧_2 = UR_Z(−\frac{π}{2})ΣU^T\), \(R_2 = UR^T_Z(−\frac{π}{2})V^T.\)
其中 \(R_Z(\frac{π}{2})\) 表示沿 Z 軸旋轉 90 度得到的旋轉矩陣。同時，由於 −E 和 E 等價，所以對任意一 個 t 或 R 取負號，也會得到同樣的結果。因此，從 E 分解到 t,R 時，一共存在四個可能的解。請 打印這四個可能的 R,t。
提示：用 AngleAxis 或 Sophus::SO3 計算 \(R_Z(\frac{π}{2})\)。
注：實際當中，可以利用深度值判斷哪個解是真正的解，不過本題不作要求，只需打印四個可能的解 即可。同時，你也可以驗證 t∧R 應該與 E 只差一個乘法因子，並且與書上的實驗結果亦只差一個乘法因子。
// START YOUR CODE HERE
JacobiSVD<MatrixXd> svd(E, ComputeThinU | ComputeThinV);
Matrix3d U = svd.matrixU();
Matrix3d V = svd.matrixV();
Matrix3d Sigma = U.inverse()*E*V.transpose().inverse();
vector<double> diag = {Sigma(0,0), Sigma(1,1), Sigma(2,2)};
sort(diag.begin(), diag.end());
double tau = (diag[2] + diag[1])*0.5;
Matrix3d Sigma_tau = Matrix3d::Zero();
Sigma_tau(0,0) = tau;
Sigma_tau(1,1) = tau;
Matrix3d R_Z1 = AngleAxisd(M_PI/2, Vector3d(0,0,1)).matrix();
Matrix3d R_Z2 = AngleAxisd(-M_PI/2, Vector3d(0,0,1)).matrix();
// END YOUR CODE HERE
// set t1, t2, R1, R2
// START YOUR CODE HERE
Matrix3d t_wedge1 = U*R_Z1*Sigma_tau*U.transpose(); //t_hat
Matrix3d t_wedge2 = U*R_Z2*Sigma_tau*U.transpose();
Matrix3d R1 = U*R_Z1*V.transpose();
Matrix3d R2 = U*R_Z2*V.transpose();
// END YOUR CODE HERE
運行結果：
kieranych@kieranych-ThinkPad-Edge-E431:~/vslam/hw5/L5/code/E2Rt/build$ ./E2Rt
R1 = -0.998596 0.0516992 -0.0115267
-0.0513961 -0.99836 -0.0252005
0.0128107 0.0245727 -0.999616
R2 = -0.365887 -0.0584576 0.928822
-0.00287462 0.998092 0.0616848
0.930655 -0.0198996 0.365356
t1 = -0.581301
-0.0231206
0.401938
t2 = 0.581301
0.0231206
-0.401938
t^R = 0.0203619 0.400711 0.0332407
-0.393927 0.035064 -0.585711
0.00678849 0.581543 0.0143826
三、用 G-N 實現 Bundle Adjustment 中的位姿估計
Bundle Adjustment 並不神秘，它僅是一個目標函數為重投影誤差的最小二乘。我們演示了 Bundle Adjustment 可以由 Ceres 和 g2o 實現，並可用於 PnP 當中的位姿估計。本題，你需要自己書寫一個高斯牛頓法，實現用 Bundle Adjustment 優化位姿的功能，求出相機位姿。嚴格來說，這是 Bundle Adjustment 的一部分，因為我們僅考慮了位姿，沒有考慮點的更新。完整的 BA 需要用到矩陣的稀疏性，我們留到第 七節課介紹。
假設一組點的 3D 坐標為 \(P = {p_i}\)，它們在相機中的坐標為 \(U = {u_i}\)，\(∀i = 1,...n\)。在文件 p3d.txt 和 p2d.txt 中給出了這兩組點的值。同時，設待估計的位姿為 T∈SE(3)，內參矩陣為：
\(K=\begin{bmatrix} 520.9 & 0 & 325.1\\ 0& 521.0 & 249.7\\ 0 & 0 & 1 \end{bmatrix}\)
請你根據上述條件，用 G-N 法求出最優位姿，初始估計為 \(T_0 = I\)。程序 GN-BA.cpp 文件提供了大致的 框架，請填寫剩下的內容。 在書寫程序過程中，回答下列問題：
- 如何定義重投影誤差？
- 該誤差關於自變量的雅可比矩陣是什么？
- 解出更新量之后，如何更新至之前的估計上？
作為驗證，最后估計得到的位姿應該接近：
\(T^* =\begin{bmatrix} 0.9978 & −0.0517 & 0.0399 & −0.1272\\ 0.0506 & 0.9983 &0.0274 & −0.007\\ −0.0412 & −0.0253 & 0.9977 & 0.0617\\ 0& 0 & 0 & 1 \end{bmatrix}\)
int main(int argc, char **argv) {
VecVector2d p2d;
VecVector3d p3d;
Matrix3d K;
double fx = 520.9, fy = 521.0, cx = 325.1, cy = 249.7;
K << fx, 0, cx, 0, fy, cy, 0, 0, 1;
// load points in to p3d and p2d
// START YOUR CODE HERE
ifstream p2d_fin; //(uv)
p2d_fin.open(p2d_file);
string s;
while(getline(p2d_fin, s) && !s.empty()){
istringstream p2d_data(s);
Vector2d p2d_temp;
p2d_data >> p2d_temp[0] >> p2d_temp[1];
p2d.push_back(p2d_temp);
}
p2d_fin.close();
ifstream p3d_fin; //(xyz)
p3d_fin.open(p3d_file);
while(getline(p3d_fin, s) && !s.empty()){
istringstream p3d_data(s);
Vector3d p3d_temp;
p3d_data >> p3d_temp[0] >> p3d_temp[1] >> p3d_temp[2];
p3d.push_back(p3d_temp);
}
p3d_fin.close();
// END YOUR CODE HERE
assert(p3d.size() == p2d.size());
int iterations = 100;
double cost = 0, lastCost = 0;
int nPoints = p3d.size();
cout << "points: " << nPoints << endl;
Sophus::SE3 T_esti; // estimated pose
for (int iter = 0; iter < iterations; iter++) {
Matrix<double, 6, 6> H = Matrix<double, 6, 6>::Zero();
Vector6d b = Vector6d::Zero();
cost = 0;
// compute cost
for (int i = 0; i < nPoints; i++) {
// compute cost for p3d[I] and p2d[I]
// START YOUR CODE HERE
Vector3d p_cam = T_esti*p3d[i];
Vector3d e_3d = Vector3d(p2d[i][0], p2d[i][1], 1) - K*p_cam/p_cam[2]; // (x/z, y/z, 1)
Vector2d e(e_3d[0], e_3d[1]); //e_3d[2] = 0
cost += 0.5*e.transpose()*e; //0.5*e^2
// END YOUR CODE HERE
// compute jacobian
Matrix<double, 2, 6> J;
// START YOUR CODE HERE
double x = p_cam[0], y = p_cam[1], z = p_cam[2];
J(0, 0) = -fx/z;
J(0, 2) = fx*x/(z*z);
J(0, 3) = fx*x*y/(z*z);
J(0, 4) = -fx - fx*(x*x)/(z*z);
J(0, 5) = fx*y/z;
J(1, 1) = -fy/z;
J(1, 2) = fy*y/(z*z);
J(1, 3) = fy + fy*(y*y)/(z*z);
J(1, 4) = -fy*x*y/(z*z);
J(1, 5) = -fy*x/z;
// END YOUR CODE HERE
// J^T J dx = -J^T e
H += J.transpose() * J;
b += -J.transpose() * e;
}
// solve dx
Vector6d dx;
// START YOUR CODE HERE
dx = H.ldlt().solve(b);
// END YOUR CODE HERE
if (isnan(dx[0])) {
cout << "result is nan!" << endl;
break;
}
if (iter > 0 && cost >= lastCost) {
// cost increase, update is not good
cout << "cost: " << cost << ", last cost: " << lastCost << endl;
break;
}
// update your estimation
// START YOUR CODE HERE
T_esti = Sophus::SE3::exp(dx)*T_esti;
// END YOUR CODE HERE
lastCost = cost;
cout << "iteration " << iter << " cost=" << cout.precision(12) << cost << endl;
}
cout << "estimated pose: \n" << T_esti.matrix() << endl;
return 0;
}
運行結果：
kieranych@kieranych-ThinkPad-Edge-E431:~/vslam/hw5/L5/code/GN-BA/build$ ./gn-ba
points: 76
iteration 0 cost=622769.1141257
iteration 1 cost=12206.604278533
iteration 2 cost=12150.675954556
iteration 3 cost=12150.6753269
iteration 4 cost=12150.6753269
cost: 150.6753269, last cost: 150.6753269
estimated pose:
0.997866186837 -0.0516724392948 0.0399128072711 -0.127226621
0.050595918872 0.998339770315 0.0275273682291 -0.00750679765351
-0.0412689491074 -0.0254492048098 0.998823914318 0.061386084881
0 0 0 1
四、用 ICP 實現軌跡對齊
在實際當中，我們經常需要比較兩條軌跡之間的誤差。第三節課習題中，你已經完成了兩條軌跡之間 的 RMSE 誤差計算。但是，由於 ground-truth 軌跡與相機軌跡很可能不在一個參考系中，它們得到的軌 跡並不能直接比較。這時，我們可以用 ICP 來計算兩條軌跡之間的相對旋轉與平移，從而估計出兩個參考 系之間的差異。
設真實軌跡為 \(T_g\)，估計軌跡為 \(T_e\)，二者皆以 \(T_{WC}\) 格式存儲。但是真實軌跡的坐標原點定義於外部 某參考系中（取決於真實軌跡的采集方式，如 Vicon 系統可能以某攝像頭中心為參考系，見圖 3），而估計 軌跡則以相機出發點為參考系（在視覺 SLAM 中很常見）。由於這個原因，理論上的真實軌跡點與估計軌 跡點應滿足：
\(T_{g,i} = T_{ge}T_{e,i}\)
其中 i 表示軌跡中的第 i 條記錄，\(T_{ge} ∈SE(3)\) 為兩個坐標系之間的變換矩陣，該矩陣在整條軌跡中保持 不變。\(T_{ge}\) 可以通過兩條軌跡數據估計得到，但方法可能有若干種：
- 認為初始化時兩個坐標系的差異就是 \(T_{ge}\)，即：
\(T_{ge} = T_{g,1}T^{−1}_{e,1}\).
\(T_{ge} = \arg \underset{T_{ge}}{ min} \sum_{i=1}{n}||log(T^{−1}_{gi}T_{ge}T_{e,i})^∨ ||_2\)
- 把兩條軌跡的平移部分看作點集，然后求點集之間的 ICP，得到兩組點之間的變換。 其中第三種也是實踐中用的最廣的一種。現在請你書寫 ICP 程序，估計兩條軌跡之間的差異。軌跡文 件在 compare.txt 文件中，格式為：
\(time_e,t_e,q_e,time_g,t_g,q_g,\)
其中 t 表示平移，q 表示單位四元數。請計算兩條軌跡之間的變換，然后將它們統一到一個參考系，並畫 在 pangolin 中。軌跡的格式與先前相同，即以時間，平移，旋轉四元數方式存儲。
本題不提供代碼框架，你可以利用之前的作業完成本題。圖 4 顯示了對准前與對准后的兩條軌跡。
運行結果：
kieranych@kieranych-ThinkPad-Edge-E431:~/vslam/hw5/L5/code/compare/build$ ./draw_gt_est
3d-3d pairs: 612
W= 476.887 31.8495 -128.09
88.5167 -39.3434 34.6791
-3.32456 -8.22012 -1.70694
U= 0.988585 -0.150113 -0.0128977
0.150527 0.987718 0.0418997
-0.0064496 0.0433628 -0.999039
V= 0.968777 0.221248 0.111896
0.051191 -0.620082 0.782865
-0.242592 0.752694 0.612047
ICP via SVD results:
R = 0.923063 0.133592 -0.360706
0.369047 -0.571958 0.732576
-0.108443 -0.809331 -0.577255
t = 1.54022
0.932743
1.44744
(poses_gt, poses_est): RMSE: 3.74174
(poses_gt, poses_est_cor): RMSE: 0.041552
1）坐標系未對齊： RMSE: 3.74174
- 坐標系對齊： RMSE: 0.041552
