第五章作業
作者：曾是少年
二 ORB特征點
ORB(Oriented FAST and BRIEF) 特征是 SLAM 中一種很常用的特征，由於其二進制特性，使得它可以非常快速地提取與計算 [1]。下面，你將按照本題的指導，自行書寫 ORB 的提取、描述子的計算以及匹配的代碼。
代碼框架參照 computeORB.cpp 文件，圖像見 1.png 文件和 2.png。
2.1 ORB提取
ORB 即 Oriented FAST 簡稱。它實際上是 FAST 特征再加上一個旋轉量。本習題將使用 OpenCV 自帶的 FAST 提取算法，但是你要完成旋轉部分的計算。旋轉的計算過程描述如下 [2]：在一個小圖像塊中，先計算質心。質心是指以圖像塊灰度值作為權重的中心。
- 在一個小的圖像塊 B 中，定義圖像塊的矩為：
- 通過矩可以找到圖像塊的質心：
- 連接圖像塊的幾何中心 O 與質心 C，得到一個方向向量\(\rightarrow_{OC}\)，於是特征點的方向可以定義為：
實際上只需計算 \(m_{01}\) 和 \(m_{10}\) 即可。習題中取圖像塊大小為 16x16，即對於任意點 \((u, v)\)，圖像塊從$ (u − 8, v − 8)$ 取到 $(u + 7, v + 7) $即可。請在習題的 computeAngle 中，為所有特征點計算這個旋轉角。
提示：
1. 由於要取圖像 16x16 塊，所以位於邊緣處的點（比如 u < 8 的）對應的圖像塊可能會出界，此時需要判斷該點是否在邊緣處，並跳過這些點。
2. 由於矩的定義方式，在畫圖特征點之后，角度看起來總是指向圖像中更亮的地方。
3. std::atan 和 std::atan2 會返回弧度制的旋轉角，但 OpenCV 中使用角度制，如使用 std::atan 類函數，請轉換一下。
作為驗證，第一個圖像的特征點如圖 1 所示。看不清可以放大看。
圖 1: 帶有旋轉的 FAST
答：計算角度的過程在題目中已經講的很清楚了，代碼如下：
void computeAngle(const cv::Mat &image, vector<cv::KeyPoint> &keypoints) {
int half_patch_size = 8,count=0;
for (auto &kp : keypoints) {
// START YOUR CODE HERE (~7 lines)
kp.angle = 0; // compute kp.angle
float u = kp.pt.x,v = kp.pt.y;;
double m01=0,m10=0;
if((u>=half_patch_size) && (u+half_patch_size)<=image.cols &&(v>=half_patch_size)&& (v+half_patch_size)<=image.rows){
for (int i = -half_patch_size; i < half_patch_size; ++i) {
for (int j = -half_patch_size; j < half_patch_size; ++j) {
m01 += (j) * image.at<uchar>(v + j, u + i) ;
m10 += (i) * image.at<uchar>(v + j, u + i) ;
}
}
kp.angle = atan2(m01 , m10) / pi *180;
}
}
return;
}
其中，需要注意的點如下(學習自網絡)：
-
關於
KeyPoint，其內含一個Point2f對象存儲xy值，用.pt獲取，含有一個angle值也可直接獲取。另外還有size，response，octave，class_id等其他特征點或者分類任務會用到的域。 -
math.h庫中：atan給斜率求正切，僅對-90°，-90°有效atan2給兩點值，值域為整個圓-180°——180°，(本題中應該使用atan2)- 另外注意角度弧度轉換，
angle存儲度數，atan使用弧度
-
質心
m10和m01:- 累加時乘在前面的值可以是絕對坐標
p.x+j或p.y+i，也可以用j和i的相對坐標。 - 使用相對坐標的質心有優勢：在求
cx和cy時，如果是絕對坐標，那么m00每一步也必須乘上p.x和p.y，並且算出來是絕對質心計算角度還需要做一次差，相對坐標可以避免多步乘法和減法運算。
- 累加時乘在前面的值可以是絕對坐標
2.2 ORB 描述
ORB 描述即帶旋轉的 BRIEF 描述。所謂 BRIEF 描述是指一個 0-1 組成的字符串（可以取 256 位或128 位），每一個 bit 表示一次像素間的比較。算法流程如下：
-
給定圖像 I 和關鍵點 (u, v)，以及該點的轉角 θ。以 256 位描述為例，那么最終描述子
\[d = [d_1, d_2, ..., d_{256}]. \] -
對任意 \(i = 1, . . . , 256\)，\(d_i\) 的計算如下。取 (u, v) 附近任意兩個點 p, q，並按照 θ 進行旋轉：
\[\left[ \begin{array}{ccc} u'_p\\ v_p' \end{array} \right] = \left[ \begin{array}{ccc} cos \theta & -sin\theta\\ sin \theta & cos\theta \end{array} \right] \left[ \begin{array}{ccc} u_p\\ v_p \end{array} \right] \]其中 \(u_p\),$ v_p$ 為 p 的坐標，對 q 亦然。記旋轉后的 p, q 為 p′, q′，那么比較 $I(p′) $和 \(I(q′)\)，若前者大，記 \(d_i = 0\)，反之記 \(d_i = 1\)。
這樣我們就得到了 ORB 的描述。我們在程序中用 256 個 bool 變量表達這個描述2。請你完成 computeORBDesc 函數，實現此處計算。注意，通常我們會固定 p, q 的取法（稱為 ORB 的 pattern），否則每次都重新隨機選取，會使得描述不穩定。我們在全局變量 ORB_pattern 中定義了 p, q 的取法，格式為\(u_p, v_p, u_q, v_q\)。請你根據給定的 pattern 完成 ORB 描述的計算。
提示
- p, q 同樣要做邊界檢查，否則會跑出圖像外。如果跑出圖像外，就設這個描述子為空。
- 調用
cos和sin時同樣請注意弧度和角度的轉換。
答：
源代碼如下所示：
void computeORBDesc(const cv::Mat &image, vector<cv::KeyPoint> &keypoints, vector<DescType> &desc) {
for (auto &kp: keypoints) {
DescType d(256, false);
for (int i = 0; i < 256; i++) {
// START YOUR CODE HERE (~7 lines)
d[i] = 0; // if kp goes outside, set d.clear()
double cos_ = cos(kp.angle /180 * pi),sin_ = sin(kp.angle /180 * pi);
cv::Point2f up_t(cos_ * ORB_pattern[4 * i] - sin_ * ORB_pattern[4 * i + 1],
sin_ * ORB_pattern[4 * i] + cos_ * ORB_pattern[4 * i + 1]);
cv::Point2f uq_t(cos_ * ORB_pattern[4 * i + 2] - sin_ * ORB_pattern[4 * i + 3],
sin_ * ORB_pattern[4 * i + 2] + cos_* ORB_pattern[4 * i + 3]);
//求作比較兩點的坐標
cv::Point2f up = up_t + kp.pt;
cv::Point2f uq = uq_t + kp.pt;
//出界點把特征向量清空,並且不計入總數
if (up.x < 0 || up.y < 0 || up.x > image.cols || up.y > image.rows ||
uq.x < 0 || uq.y < 0 || uq.x > image.cols || uq.y > image.rows) {
d.clear();
break;// if kp goes outside, set d.clear()
}
d[i] = image.at<uchar>(up) > image.at<uchar>(uq) ? 0 : 1;
// END YOUR CODE HERE
}
desc.push_back(d);
}
int bad = 0;
for (auto &d: desc) {
if (d.empty()) bad++;
}
cout << "bad/total: " << bad << "/" << desc.size() << endl;
return;
}
2.3 暴力匹配
在提取描述之后，我們需要根據描述子進行匹配。暴力匹配是一種簡單粗暴的匹配方法，在特征點不多時很有用。下面你將根據習題指導，書寫暴力匹配算法。
所謂暴力匹配思路很簡單。給定兩組描述子 P = [p_1, . . . , p_M] 和 Q = [q_1, . . . , q_N ]。那么，對 P 中意一個點，找到 Q 中對應最小距離點，即算一次匹配。但是這樣做會對每個特征點都找到一個匹配，所以我們通常還會限制一個距離閾值 dmax，即認作匹配的特征點距離不應該大於 dmax。下面請你根據上述描述，實現函數 bfMatch，返回給定特征點的匹配情況。實踐中取 dmax = 50。
提示：
- 你需要按位計算兩個描述子之間的漢明距離。
OpenCV的DMatch結構，queryIdx為第一圖的特征ID，trainIdx為第二個圖的特征ID。- 作為驗證，匹配之后輸出圖像應如圖 2 所示。
圖 2 匹配圖像
答：
源代碼如下所示：
void bfMatch(const vector<DescType> &desc1, const vector<DescType> &desc2, vector<cv::DMatch> &matches) {
int d_max = 50;
// START YOUR CODE HERE (~12 lines)
// find matches between desc1 and desc2.
for (size_t i = 0; i < desc1.size(); ++i){
if(desc1[i].empty()) continue;
int d_min=256;
int index2=-1;
for(size_t j=0;j < desc2.size();j++){
if(desc2[j].empty())continue;
int dist=0;
for(size_t k=0;k<256;k++){
dist+=desc1[i][k]^desc2[j][k];
}
if(dist<d_max&&dist<d_min){
d_min=dist;
index2=j;
}
}
if(d_min<d_max){
matches.push_back(cv::DMatch(i,index2,d_min));
}
}
}
運行結果，截圖：
當最大閾值設為50的時候，得到特征點如下圖：
匹配結果如下圖：
提示: 暴力匹配的優化可以使用快速查找就是因為 由於兩個連續幀之間的那個相機運動可能不會太大，所以各個物體在圖片中的位置也應該變化不是太大，這樣的話就可以針對那個第一個圖中第一個圖中特征點，在第二個圖中快速進行查找。
最后，請結合實驗，回答下面幾個問題：
1. 為什么說 ORB 是一種二進制特征？
答：ORB使用二進制向量描述了像素點與周圍區域的關系（即BRIEF）描述子，根據實驗可以看出這一點。
2. 為什么在匹配時使用 50 作為閾值，取更大或更小值會怎么樣？
答：可以看到閾值為50時，檢測的特征點對依然存在部分誤匹配。
當閾值為50的時候，可以檢測出的特征對有95個匹配的特征對。但存在一些誤匹配的點對。
當閾值為40的時候，可以檢測到39個特征點對,得到的特征點對較少；
當閾值為60的時候，可以檢測到matches: 174的特征點對，同時誤匹配也增加了一些.
當閾值設置為100時,可以檢測到593個點對,誤匹配很多.
3. 暴力匹配在你的機器上表現如何？你能想到什么減少計算量的匹配方法嗎？
答：暴力匹配在機器上運行時間較長，在真正的SLAM過程中可能導致效率低下的問題。
運行截圖如下:
兩幀之間的特征匹配使用了大約3000ms,用時比較長.
- 針對效率提升,可以想到的方法包括：對各個特征點限制匹配搜索的區域；使用光流或者直接法進行計算等。
- 針對誤匹配,可以使用RANSAC等算法進行后處理去除部分無匹配.
三 從 E 恢復 R, t
我們在書中講到了單目對極幾何部分，可以通過本質矩陣 E，得到旋轉和平移 R, t，但那時直接使用了OpenCV 提供的函數。本題中，請你根據數學原理，完成從 E 到 R, t 的計算。程序框架見 code/E2Rt.cpp.
設 Essential 矩陣 E 的取值為（與書上實驗數值相同）：
. 請計算對應的 R, t，流程如下：
- 對
E作SVD分解：
-
處理 Σ 的奇異值。設 \(Σ = diag(σ_1, σ_2, σ_3)\) 且 \(σ_1 ≥ σ_2 ≥ σ_3\)，那么處理后的 \(Σ\) 為：
\[Σ = diag(\frac{σ_1 + σ_2}{2},\frac{σ_1 + σ_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(\pi/2)\)沿
Z軸旋轉90度得到的旋轉矩陣。同時，由於−E和E等價，所以對任意一個t或R取負號，也會得到同樣的結果。因此，從 E 分解到 t, R 時，一共存在四個可能的解。請打印這四個可能的 R, t。提示：用
AngleAxis或Sophus::SO3計算 \(R_Z(π/2)\)。
注：實際當中，可以利用深度值判斷哪個解是真正的解，不過本題不作要求，只需打印四個可能的解即可。同時，你也可以驗證 \(t^∧R\) 應該與 E 只差一個乘法因子，並且與書上的實驗結果亦只差一個乘法因子。
答：解答本題的過程在題目中已經很清楚了，需要注意的點如下：
-
Eigen庫中調用SVD分解的方法如下所示：Eigen::JacobiSVD<Eigen::Matrix3d> svd(E,ComputeThinU|ComputeThinV); Matrix3d V=svd.matrixV(),U=svd.matrixU(); Matrix3d un_S=U.inverse()* E*V.transpose().inverse(); -
使用旋轉向量對旋轉矩陣進行賦值的方法：
AngleAxisd V1(M_PI / 2, Vector3d(0, 0, 1)); -
根據線性方程解出的 E，可能不滿足 E 的內在性質——它的奇異值不一定為
σ, σ, 0的形式。這時，我們會刻意地把 Σ 矩陣調整成上面的樣子。通常的做法是，對八點法求得的 E 進行 SVD 分解后，會得到奇異值矩陣Σ = diag(σ1, σ2, σ3)，不妨設\(\delta_1\geq\delta_2\geq\delta_3\), 要取設置為\[Σ = diag(\frac{σ_1 + σ_2}{2},\frac{σ_1 + σ_2}2, 0). \]這相當於是把求出來的矩陣投影到了 E 所在的流形上。當然，更簡單的做法是將奇異值矩陣取成
diag(1, 1, 0)，因為 E 具有尺度等價性，所以這樣做也是合理的。
源代碼如下：
// 給定Essential矩陣
Matrix3d E;
E << -0.0203618550523477, -0.4007110038118445, -0.03324074249824097,
0.3939270778216369, -0.03506401846698079, 0.5857110303721015,
-0.006788487241438284, -0.5815434272915686, -0.01438258684486258;
// 待計算的R,t
Matrix3d R;
Vector3d t;
// SVD and fix sigular values
// START YOUR CODE HERE
Eigen::JacobiSVD<Eigen::Matrix3d> svd(E,ComputeThinU|ComputeThinV);
Matrix3d V=svd.matrixV(),U=svd.matrixU();
Matrix3d un_S=U.inverse()* E*V.transpose().inverse(); //類型不要搞混
//計算后的Sigma矩陣
double delta_1=un_S(0,0),delta_2=un_S(1,1);
Matrix3d S = Matrix3d::Zero();
S(0,0)=(delta_1+delta_2)/2;
S(1,1)=(delta_1+delta_2)/2;
// END YOUR CODE HERE
// set t1, t2, R1, R2
// START YOUR CODE HERE
Matrix3d t_wedge1;
Matrix3d t_wedge2;
Matrix3d R1;
Matrix3d R2;
// 使用旋轉的角度和旋轉軸向量（此向量為單位向量）來初始化角軸
AngleAxisd V1(M_PI / 2, Vector3d(0, 0, 1));
AngleAxisd V2(- M_PI / 2, Vector3d(0, 0, 1));
Matrix3d Rz_pos = V1.toRotationMatrix();
Matrix3d Rz_neg = V2.toRotationMatrix();
t_wedge1 = U*Rz_pos*S*U.transpose();
t_wedge2 = U*Rz_neg*S*U.transpose();
R1 = U*Rz_pos*V.transpose();
R2 = U*Rz_neg*V.transpose();
// END YOUR CODE HERE
cout << "R1 = " << R1 << endl;
cout << "R2 = " << R2 << endl;
cout << "t1 = " << Sophus::SO3::vee(t_wedge1) << endl;
cout << "t2 = " << Sophus::SO3::vee(t_wedge2) << endl;
計算結果：
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
通過以上結果，可以看出\(t^{\wedge}R=-E\),相差了一個乘法因子。
四 用 G-N 實現 Bundle Adjustment 中的位姿估計
Bundle Adjustment 並不神秘，它僅是一個目標函數為重投影誤差的最小二乘。我們演示了 BundleAdjustment 可以由 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)，內參矩陣為：
請你根據上述條件，用 G-N 法求出最優位姿，初始估計為 \(T_0 = I\)。程序 GN-BA.cpp 文件提供了大致的框架，請填寫剩下的內容。
在書寫程序過程中，回答下列問題：
1. 如何定義重投影誤差？
第i個投影點的誤差為：
其中，\(u_i\)是3D空間點\(P_i\)在歸一化平面上的投影點，K是內參矩陣，\(\xi\)是位資的李代數表示.
整體的重投影誤差如下：
程序中，重投影誤差的計算代碼如下：
Vector3d P = T_esti.rotation_matrix()*p3d[i]+T_esti.translation();
// Vector3d P = T_esti*p3d[i];
double x = P[0];
double y = P[1];
double z = P[2];
Vector3d P_e = K*P/z;
Vector2d e(p2d[i].x()-P_e.x(),p2d[i].y()-P_e.y());
cost += e.transpose()*e;
計算重投影誤差的過程如下：
- 先把世界坐標系中的3d空間點投影到像素坐標系；
- 把像素坐標點與2d點做差以計算每個點的重投影誤差；
- 累加誤差得到所有點的重投影誤差。
2.該誤差關於自變量的雅可比矩陣是什么？
自變量是位姿的李代數 \(\xi\)。
計算過程
- 線性化重投影誤差
-
重投影誤差如下：
\[\begin{aligned} e &= \frac{1}{2}\sum^n_{i=1}||e_i||^2 \\&=\frac{1}{2}\sum^n_{i=1}||u_i-\frac{1}{s_i}K\exp(\xi^{\wedge})P_i||^2 \end{aligned} \] -
雅克比矩陣計算如下：
程序中計算雅克比矩陣的源代碼如下W：
Matrix<double, 2, 6> J;
// START YOUR CODE HERE
J(0,0) = fx/z;
J(0,1) = 0;
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,0) = 0;
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;
J = -J;
// END YOUR CODE HERE
3. 解出更新量之后，如何更新至之前的估計上？
作為驗證，最后估計得到的位姿應該接近：
這和書中使用 g2o優化的結果很接近。
答：將得到的更新量dx
源代碼如下：
Vector6d dx;
// START YOUR CODE HERE
dx = H.ldlt().solve(b);
// END YOUR CODE HERE
T_esti *= Sophus::SE3::exp(dx); //更新估計
注意點：
-
使用
Vetor6d得到李代數的方法Sophus::se3 T = Sophus::SE3::exp(dx);
最后的程序運行截圖如下：
結果與題目給出的驗證結果相近。
附加題* 五 用 ICP 實現軌跡對齊
在實際當中，我們經常需要比較兩條軌跡之間的誤差。第三節課習題中，你已經完成了兩條軌跡之間的 RMSE 誤差計算。但是，由於 ground-truth 軌跡與相機軌跡很可能不在一個參考系中，它們得到的軌跡並不能直接比較。這時，我們可以用 ICP 來計算兩條軌跡之間的相對旋轉與平移，從而估計出兩個參考系之間的差異。
圖 3: vicon 運動捕捉系統，部署於場地中的多個紅外相機會捕捉目標球的運動軌跡，實現快速定位。
設真實軌跡為 \(T_g\)，估計軌跡為 \(T_e\)，二者皆以 \(T_{WC}\) 格式存儲。但是真實軌跡的坐標原點定義於外部某參考系中（取決於真實軌跡的采集方式，如 Vicon 系統可能以某攝像頭中心為參考系，見圖 3），而估計軌跡則以相機出發點為參考系（在視覺 SLAM 中很常見）。由於這個原因，理論上的真實軌跡點與估計軌跡點應滿足：
其中 i 表示軌跡中的第 i 條記錄，Tge ∈ SE(3) 為兩個坐標系之間的變換矩陣，該矩陣在整條軌跡中保持
不變。Tge 可以通過兩條軌跡數據估計得到，但方法可能有若干種：
- 認為初始化時兩個坐標系的差異就是 Tge，即：
- 在整條軌跡上利用最小二乘計算 Tge：
- 把兩條軌跡的平移部分看作點集，然后求點集之間的 ICP，得到兩組點之間的變換。
其中第三種也是實踐中用的最廣的一種。現在請你書寫 ICP 程序，估計兩條軌跡之間的差異。軌跡文件在compare.txt文件中，格式為：timee, te, qe,timeg, tg, qg。
其中 t 表示平移，q 表示單位四元數。請計算兩條軌跡之間的變換，然后將它們統一到一個參考系，並畫在 pangolin 中。軌跡的格式與先前相同，即以時間，平移，旋轉四元數方式存儲。
本題不提供代碼框架，你可以利用之前的作業完成本題。圖 4 顯示了對准前與對准后的兩條軌跡。
答：代碼過程如下：
- 讀取文件得到軌跡
vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>> esti_poses;
vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>> ground_truth_poses;
//讀取文件
fstream INFILE("compare.txt");
double time,t_x,t_y,t_z,q_x,q_y,q_z,q_w;
while(!INFILE.eof())
{
INFILE>>time;
INFILE>>t_x;
INFILE>>t_y;
INFILE>>t_z;
INFILE>>q_x;
INFILE>>q_y;
INFILE>>q_z;
INFILE>>q_w;
esti_poses.push_back(Sophus::SE3(Eigen::Quaterniond(q_w,q_x,q_y,q_z),Eigen::Vector3d(t_x,t_y,t_z)));
INFILE>>time;
INFILE>>t_x;
INFILE>>t_y;
INFILE>>t_z;
INFILE>>q_x;
INFILE>>q_y;
INFILE>>q_z;
INFILE>>q_w;
ground_truth_poses.push_back(Sophus::SE3(Eigen::Quaterniond(q_w,q_x,q_y,q_z),Eigen::Vector3d(t_x,t_y,t_z)));
}
- 計算質心及去質心坐標
Eigen::Vector3d center_et(0,0,0);//估計出軌跡的質心
Eigen::Vector3d center_gd(0,0,0);//groundtruth的質心
for(int i = 0;i<esti_poses.size();i++)
{
center_et+=esti_poses[i].translation();
center_gd+=ground_truth_poses[i].translation();
}
center_et /= s;
center_gd /= s;
vector<Vector3d> t_esti;
vector<Vector3d> t_gd;
for(int i = 0;i<s;i++)
{
t_esti.push_back(esti_poses[i].translation()-center_et);
t_gd.push_back(ground_truth_poses[i].translation()-center_gd);
}
- 根據優化問題計算旋轉矩陣R（使用SVD分解進行計算得到）
Matrix3d R; //待估計R
Matrix3d W;
W.setZero();
cout<<W<<endl;
//A.SVD方法
for(int i = 0;i<s;i++)
W+=t_gd[i]*t_esti[i].transpose();
Eigen::JacobiSVD<Eigen::Matrix3d> svd(W,ComputeThinU|ComputeThinV);
Matrix3d V=svd.matrixV(),U=svd.matrixU();
R=U*V.transpose();
cout<<"R:"<<endl<<R<<endl;
- 根據R計算t
Vector3d t;
t = center_gd - R*center_et;
- 用計算出的R，t將軌跡投影后，畫出軌跡
vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>> truth_poses_rt;
R=R.inverse();
t=-R*t;
for(int i = 0;i<s;i++)
{
Sophus::SE3 p_rt;
p_rt = Sophus::SE3(R,t)*ground_truth_poses[i];
truth_poses_rt.push_back(p_rt);
}
DrawTrajectory(truth_poses_rt,esti_poses);
得到的結果如下圖所示：
對齊前的結果如下圖所示：
-
紅色表示groundtruth軌跡
-
藍色表示估計出的軌跡
對齊后的結果如下所示（本次實驗中，將groundtruth軌跡與估計出的軌跡對齊，）
