文章目錄

• • 非線性優化
  • • 0 .case實戰
    • • 0.1求解思路
      • 0.2 g2o求解
    • 1. 狀態估計問題
    • • 1.1 最大后驗與最大似然
      • 1.2 最小二乘的引出
    • 2. 非線性最小二乘
    • • 2.1 一階和二階梯度法
      • 2.2 高斯牛頓法
      • 2.2 列文伯格-馬夸爾特方法（阻尼牛頓法)
    • 3 Ceres庫的使用
    • 4 g2o庫的使用

非線性優化

0 .case實戰

0.1求解思路

1. 對兩幅圖像img_1,img_2分別提取特征點

2. 特征匹配

3. 通過depth，獲得第一幅圖像匹配的特征點的深度，由相機內參K恢復這些特征點的三維坐標（相機坐標系）。

4. 由第一幅圖像中的特征點的三維坐標、第二幅圖像中特征點的2D像素坐標，以及相機內參K作為優化函數的輸入，分別采用如下方法進行優化

5. 牛頓高斯法
   （1）構建誤差方程,由相機位姿、相機內參獲得第一幅圖像特征點對應的三維坐標到第二幅圖像中的投影（像素坐標），與真實提取的第二幅圖像中特征點的像素坐標作差即重投影誤差：
   （2）關于相機位姿李代數的一階變化：雅可比矩陣
   

   （3）誤差關于空間點位置的導數（在這個實例代碼中沒用上，僅對位姿做了優化，并沒有優化空間點位置）：
   
   （4）構建增量方程，g=-J(x)f(x)，代碼中g對應e。
   

   （5）LDLT分解，求增量方程dx = H.ldlt().solve(b)。
   （6）更新變量pose = Sophus::SE3d::exp(dx) * pose。
   （7）在新的位姿下，進行新一輪迭代，重復（1）-（7）。

   0.2 g2o求解

   1. 構建定點（頂點代表優化變量），在3D2D的PnP位姿求解中，僅優化了位姿。
      （1）頂點（變量）_estimate = Sophus::SE3d();
      （2）變量更新_estimate = Sophus::SE3d::exp(update_eigen) * _estimate;

   2. (1)邊連接的頂點（在這里只有一個頂點，即相機位姿），邊為誤差項，即由頂點引起的誤差

      const VertexPose *v = static_cast<VertexPose *> (_vertices[0]);
      Sophus::SE3d T = v->estimate();
      Eigen::Vector3d pos_pixel = _K * (T * _pos3d);
      pos_pixel /= pos_pixel[2];
      _error = _measurement - pos_pixel.head<2>();
      

      (2)誤差對頂點求導，得到_jacobianOplusXi

   3. g2o的庫調用過程
      增加頂點optimizer.addVertex(vertex_pose);
      增加邊（是一個循環，每一個特征點就有一條邊）：optimizer.addEdge(edge);
      初始化 optimizer.initializeOptimization();
      求解 optimizer.optimize(10);
      獲取結果：pose = vertex_pose->estimate();

1. 狀態估計問題

1.1 最大后驗與最大似然

1. 經典 SLAM 模型：

\left{\begin{array}{c}
x_{k}=f\left(x_{k-1}, u_{k}\right)+w_{k} \
z_{k, j}=f\left(y_{j}, x_{k}\right)+v_{k, j}
\end{array}\right.

其中x{k}為相機的位姿，u為輸入數據，即為采集到的數據

假如我們在x{k} 處觀測到路標 y_{j} 對應到圖像上的像素位置 z_{k, j}， 那么我們的觀測方程可以表示為：

s z_{k, j}=K \exp \left(\xi^{\wedge}\right) y_{i}

2. 在運動和觀測方程中，通常假設兩個噪聲項 w_{k}, v_{k} 滿足零均值的高斯分布：

w_{k} N\left(0, R_{k}\right), v_{k} N\left(0, Q_{k, j}\right)

3. 狀態估計問題：通過帶噪聲的數據 z和u , 推斷位姿 x和地圖y （以及它們的概率分布），其中有優化方法有：

（1）擴展卡爾曼濾波器（EKF）求解，關心的是當前時刻的狀態估計，而對直之前的狀態沒有多加考慮。
（2）非線性優化，使用所有時刻采集到的數據進行狀態估計，被認為優于傳統濾波器，成為當前視覺slam的主流。
非線性優化把所待估計的變量都放在一個狀態變量中：

x=\left{x_{1}, \ldots, x_{N}, y_{1}, \ldots, y_{M}\right}

已知：輸入讀數u，觀測值z。（帶噪聲）

目標： P(x \mid z, u)，只考慮z時：P(x \mid z)

4. 貝葉斯法則：

   P(x \mid z)=\frac{P(z \mid x) P(x)}{P(z)} \rightarrow P(z \mid x) P(x)

5. 貝葉斯法則左側通常稱為后驗概率。它右側的 P(z \mid x) 稱為似然，另一部分P ( x ) 稱為先驗

6. 直接求后驗分布較困難，轉而求一個狀態最優估計，使得在該狀態下，后驗概率最大化(MPA):

x^{*} M A P=\operatorname{argmax} P(x \mid z)=\operatorname{argmax} P(z)

若不知道機器人位姿大概在什么地方，此時就沒有了先驗。進而可以求解x x*x*的最大似然估計： 

x_{M L E}^{*}=\operatorname{argmax} P(z \mid x)

最大似然估計，可以理解成：“**在什么樣的狀態下，最可能產生現在觀測到的數據**” 

1.2 最小二乘的引出

1. 對于某一次觀測：

z_{k, j}=h\left(y_{j}, x_{k}\right)+v_{k, j}

假設噪聲項 v_{k} \sim N\left(0, Q_{k, j}\right), 所以觀測數據的條件概率為： 

P\left(z_{k, j} \mid x_{k}, y_{j}\right)=N\left(h\left(y_{i}, x_{k}\right), Q_{k, j}\right)

2. 為了計算使它最大化的 x_{k}, y_{k},使用最小化負對數的方式，來求一個高斯分布的最大似然。考慮一個任意的高維高斯分布v_{k} \sim N(\mu, \Sigma), 它的概率密度函數展開形式為:

P(x)=\frac{1}{\sqrt{(2 \pi)^{N} \operatorname{det}\left(\sum\right)}} \exp \left(-\frac{1}{2}(x-\mu)^{T} \Sigma^{-1}(x-\mu)\right)

**取負對數：** 

-\ln (P(x))=\frac{1}{2} \ln \left((2 \pi)^{N} \operatorname{det}(\Sigma)\right)+\frac{1}{2}(x-\mu)^{T} \Sigma^{-1}(x-\mu)

3. 第一項與 x 無關，可以略去。代入 SLAM 的觀測模型，相當于在求：

x^{*}=\operatorname{argmin}\left(\left(z_{k, j}-h\left(x_{k}, y_{j}\right)\right)^{T} Q_{k, j}^{-1}\left(z_{k, j}-h\left(x_{k}, y_{j}\right)\right)\right)

該式等價于最小化噪聲項（即誤差）的平方（Σ ΣΣ 范數意義下） 

4. 對于所有的運動和任意的觀測，定義數據與估計值之間的誤差：

\begin{array}{c}
e_{v, k}=x_{k}-f\left(x_{k-1}, u_{k}\right) \
e_{v, j, k}=z_{k, j}-f\left(x_{k}, y\right)
\end{array}

并求該誤差的平方之和： 

J(x)=\sum_{k} e_{T}^{v, k} R_{k}^{-1} e_{v, k}+\sum_{k} \sum_{j} e_{y, k, j}^{T} Q_{k, j}^{-1} e_{y, k, j}

這樣就得到一個總體意義下的最小二乘問題，它的最優解等價于狀態的最大似然估計。由于噪聲的存在，當我們把估計的軌跡與地圖代入SLAM的運動、觀測方程時，并不會很完美，可以對狀態進行微調，使得整體的誤差下降到一個極小值。

5. SLAM中的最小二乘問題具有一些特定的結構：
   (1)整個問題的目標函數由許多個誤差的（加權的）平方和組成。雖然總體的狀態變量維數很高，但每個誤差項都是簡單的，僅與一倆個狀態變量有關，運動誤差只與x_{k-1}, x_{k} 有關，觀測誤差只與 x_{k}, y_{j} 有關，每個誤差是個小規模約束，把這誤差項的小雅克比矩陣放在整體的雅克比矩陣中。稱每個誤差項對應優化變量為參數快。
   整體誤差由很多小誤差項之和組成的問題，其增量方程具有一定的稀疏性。
   (2)如果使用李代數表示，那么該問題轉換成**無約束最小二乘問題*，用旋轉矩陣描述姿態會引入自身的約束
   (3)使用二范數度量誤差，相當于歐式空間中距離的平方。

2. 非線性最小二乘

最簡單的最小二乘問題:

\min {x} \frac{1}{2}|f(x)|{2}^{2}

為求其最小值，則需要求其導數，然后得到其求解 x的最優解
對于不方便求解的最小二乘問題，可以用迭代的方法，從初始值出發，不斷的跟新當前的優化變量，使目標函數下降，具體步驟有：
（1）給定某個初始值。
（2）對于第k次迭代，尋找增量 \Delta x_{k} , 使得 f\left(x_{k}+\Delta x_{k}\right) |_{2}^{2} 這里應該是二范數）達到最小。

（3）若\Delta x_{k}足夠小，則停止。

（4）否則,令x_{k+1}=x_{k}+\Delta x_{k} 返回第2步。

2.1 一階和二階梯度法

1. 將目標函數在 x xx 附近進行泰勒展開：

   |f(x+\Delta x)|{2}^{2} \approx|f(x)|{2}^{2}+J(x) \Delta x+\frac{1}{2} \Delta x^{T} H \Delta x

   J是|f(x)|_{2}^{2} 關于x xx的導數（雅克比矩陣），H HH是二階導數（海塞矩陣）

2. 最速梯度下降法：只保留一階梯度，增量的方向為：

\Delta x^{*}=-J{I}(x)

3. 牛頓法：保留二階梯度，增量方程為：

\Delta x^{*}=-J{I}(x)\Delta x^{{*}=\operatorname{argmin}|f(x)|_{2}}{2}+J(x) \Delta x+\frac{1}{2} x^{T} H \Delta x

求右側等式關于 ?x的導數并令它為零，得到了增量的解： 

H \Delta X=-J^{T}

4. 兩種方法的問題：
   （1）最速下降法過于貪心，容易走出鋸齒路線，反而增加了迭代次數。
   （2）牛頓法則需要計算目標函數的 H矩陣，這在問題規模較大時非常困難，通常傾向于避免 H 的計算。

2.2 高斯牛頓法

1. 高斯牛頓法，它的思想是將f(x) 進行泰勒展開（目標函數不是f(x) )

f(x+\Delta x) \approx f(x)+J(x) \Delta x

f(J) 是一個m ? n mnm?n*雅克比矩陣，當前的目標是為了尋找下降矢量 ?x，使得 \left|f(x+\Delta x)^{2}\right| 達到最小。

2. 為了求 \Delta x_{k} 需要解一個線性的最小二乘問題

\Delta x^{*}=\arg \min _{\Delta x} \frac{1}{2}|f(x)+J(x) \Delta x|^{2}

考慮的是 \Delta x_{k} 的導數（而不是x x*x*） ，先展開目標函數的平方項 

\begin{array}{c}
\frac{1}{2}|f(x)+J(x) \Delta x|^{2}=\frac{1}{2}(f(x)+J(x) \Delta x)^{T}(f(x)+J(x) \Delta x) \
\left.=\frac{1}{2}(|f(x)|)_{2}^{2}+2 f(x)^{T} J(x) \Delta x+\Delta x J(x)^{T} J(x) \Delta x\right)
\end{array}

上式關于\Delta x的導數，并令其為零：

J(x)^{T} J(x) \Delta x=-J(x)^{T} f(x)

這個方程稱之為增量方程，也稱之為高斯牛頓方程或正規方程，將左邊的系數設為H，右邊的系數設為g ，則有

H \Delta x=g

3. 高斯牛頓法求解的算法步驟可寫成：
   （1）給定初始值x_{0}

（2）對于第k 次迭代，求出當前的雅克比矩陣J\left(x_{k}\right) 和誤差 f\left(x_{k}\right)（3）求解增量方程： H \Delta x_{k}=g（4）若\Delta x 足夠小，則停止。否則，令 x_{k+1}=x_{k}+\Delta x_{k} 返回第二步 

4. 高斯牛頓法的缺點：
   （1）要求H 是可逆的，而且是正定的，如果出現H 矩陣奇異或者病態，此時增量的穩定性較差，導致算法不收斂
   （2）步長問題，若求出來的 \Delta x_{k}太長，則可能出現局部近似不夠準確，無法保證迭代收斂。

2.2 列文伯格-馬夸爾特方法（阻尼牛頓法)

1. 信賴區域方法 (Trust Region Method)：給 \Delta x添加一個信賴區域（Trust Region），不能讓它太大而使得近似不準確

2. 考慮使用如下公式來判斷泰勒近似是否夠好

\rho=\frac{f(x+\Delta x)-\Delta x}{J(x) \Delta x}

（1）如果 ρ ρρ 接近于 1，則近似是好的。
（2）如果 ρ ρρ太小，說明實際減小的值遠少于近似減小的值，則認為近似比較差，需要縮小近似范圍。
（3）如果 ρ ρρ 比較大，則說明實際下降的比預計的更大，我們可以放大近似范圍。

3. 改良版的非線性優化框架

[外鏈圖片轉存失敗,源站可能有防盜鏈機制,建議將圖片保存下來直接上傳(img-i5um1muo-1610464509672)(C:\Users\guoqi\AppData\Roaming\Typora\typora-user-images\1610039803776.png)]

4. 由于是有約束優化，可以利用拉格朗日乘子將其轉化為一個無約束優化問題：

\min {\Delta x{k}} \frac{1}{2}\left|f\left(x_{k}+J\left(x_{k} \Delta x_{k}\right)\right)\right|^{2}+\frac{\lambda}{2}|D \Delta x|^{2}

類似高斯牛頓法展開，可得其增量的線性方程： 

\left(H+\lambda D^{T} D\right) \Delta X=g

考慮它的簡化形式，即D=I，那么相當于求解： 

(H+\lambda I) \Delta x=g

（1）當參數λ λ*λ*較小時，H 占主導地位，說明二次近似模型在該范圍內是比較好的，該方法接近于高斯牛頓法。

（2）當參數λ λλ較大時，列文伯格-馬夸爾特方法接近于一階梯度下降法，說明二次近似不好。
（3）該方法可在一定程度上避免線性方程組的系數矩陣的非奇異和病態問題)，提供更穩定、更準確的增量\Delta x。

5. 非線性優化的框架分為Line Search和Trust Region兩類。
   （1）Line Search：先固定搜索方向，后尋找步長，以最速梯度法和高斯牛頓法為代表。
   （2）Trust Region：先固定搜索區域，再考慮找該區域的最優點，以列文伯格-馬夸爾特方法為代表

3 Ceres庫的使用

ceres庫的使用教程這份google的資料很詳細，足夠解決我們遇到的問題。

ceres example 見鏈接。

3.1 實例

#include <iostream>
#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <chrono>
#include <ceres/ceres.h>
#include <ceres/rotation.h>
using namespace std;
using namespace cv;void find_feature_matches ( const Mat& img_1, const Mat& img_2,std::vector<KeyPoint>& keypoints_1,std::vector<KeyPoint>& keypoints_2,std::vector< DMatch >& matches )
{//-- 初始化Mat descriptors_1, descriptors_2;// used in OpenCV3Ptr<FeatureDetector> detector = ORB::create();Ptr<DescriptorExtractor> descriptor = ORB::create();// use this if you are in OpenCV2// Ptr<FeatureDetector> detector = FeatureDetector::create ( "ORB" );// Ptr<DescriptorExtractor> descriptor = DescriptorExtractor::create ( "ORB" );Ptr<DescriptorMatcher> matcher  = DescriptorMatcher::create ( "BruteForce-Hamming" );//-- 第一步:檢測 Oriented FAST 角點位置detector->detect ( img_1,keypoints_1 );detector->detect ( img_2,keypoints_2 );//-- 第二步:根據角點位置計算 BRIEF 描述子descriptor->compute ( img_1, keypoints_1, descriptors_1 );descriptor->compute ( img_2, keypoints_2, descriptors_2 );//-- 第三步:對兩幅圖像中的BRIEF描述子進行匹配，使用 Hamming 距離vector<DMatch> match;// BFMatcher matcher ( NORM_HAMMING );matcher->match ( descriptors_1, descriptors_2, match );//-- 第四步:匹配點對篩選double min_dist=10000, max_dist=0;//找出所有匹配之間的最小距離和最大距離, 即是最相似的和最不相似的兩組點之間的距離for ( int i = 0; i < descriptors_1.rows; i++ ){double dist = match[i].distance;if ( dist < min_dist ) min_dist = dist;if ( dist > max_dist ) max_dist = dist;}printf ( "-- Max dist : %f \n", max_dist );printf ( "-- Min dist : %f \n", min_dist );//當描述子之間的距離大于兩倍的最小距離時,即認為匹配有誤.但有時候最小距離會非常小,設置一個經驗值30作為下限.for ( int i = 0; i < descriptors_1.rows; i++ ){if ( match[i].distance <= max ( 2*min_dist, 30.0 ) ){matches.push_back ( match[i] );}}
}Point2d pixel2cam ( const Point2d& p, const Mat& K )
{return Point2d(( p.x - K.at<double> ( 0,2 ) ) / K.at<double> ( 0,0 ),( p.y - K.at<double> ( 1,2 ) ) / K.at<double> ( 1,1 ));
}//Eigen::Matrix<double,3,3> camera_matrix;
//camera_matrix <<520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1;
Mat K = ( Mat_<double> ( 3,3 ) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1 );struct CurveFittingCost {CurveFittingCost(Point2d point2d, Point3d point3d) : _point2d(point2d), _point3d(point3d) {}template<typename T>bool operator() (const T *const camera_pose,T *residual)const{T point[3];point[0] = T(_point3d.x);point[1] = T(_point3d.y);point[2] = T(_point3d.z);T p[3];ceres::AngleAxisRotatePoint(camera_pose, point, p);p[0] += camera_pose[3]; p[1] += camera_pose[4]; p[2] +=camera_pose[5];//cam2world//p[0] / p[2],p[1] / p[2] is norm coordinate，the real depth id p[2]T x_p = p[0] / p[2];T y_p = p[1] / p[2];                                                 T predicted_x = x_p * K.at<double>(0,0)+K.at<double>(0,2);T predicted_y = y_p * K.at<double>(1,1)+K.at<double>(1,2);residual[0] = T(_point2d.x) - predicted_x;residual[1] = T(_point2d.y) - predicted_y;return true;}static ceres::CostFunction* Create(Point2d point2d, Point3d point3d) {return (new ceres::AutoDiffCostFunction<CurveFittingCost, 2, 6>(new CurveFittingCost(point2d, point3d)));}const Point2d _point2d;const Point3d _point3d;
};int main ( int argc, char** argv )
{if ( argc != 5 ){cout<<"usage: pose_estimation img1 img2 depth1 depth2"<<endl;return 1;}//-- 讀取圖像Mat img_1 = imread ( argv[1], CV_LOAD_IMAGE_COLOR );Mat img_2 = imread ( argv[2], CV_LOAD_IMAGE_COLOR );vector<KeyPoint> keypoints_1, keypoints_2;vector<DMatch> matches;find_feature_matches ( img_1, img_2, keypoints_1, keypoints_2, matches );cout<<"一共找到了"<<matches.size() <<"組匹配點"<<endl;// 建立3D點Mat d1 = imread ( argv[3], CV_LOAD_IMAGE_UNCHANGED ); // 深度圖為16位無符號數，單通道圖像Mat K = ( Mat_<double> ( 3,3 ) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1 );vector<Point3f> pts_3d;vector<Point2f> pts_2d;for ( DMatch m:matches ){ushort d = d1.ptr<unsigned short> (int ( keypoints_1[m.queryIdx].pt.y )) [ int ( keypoints_1[m.queryIdx].pt.x ) ];if ( d == 0 )   // bad depthcontinue;float dd = d/5000.0;Point2d p1 = pixel2cam ( keypoints_1[m.queryIdx].pt, K );pts_3d.push_back ( Point3f ( p1.x*dd, p1.y*dd, dd ) );pts_2d.push_back ( keypoints_2[m.trainIdx].pt );std::cout<<"point look "<<std::endl;std::cout<<keypoints_2[m.trainIdx].pt.x<<"  "<<keypoints_2[m.trainIdx].pt.y<<std::endl;std::cout<<p1.x*dd<<"  "<<p1.y*dd<<"   "<< dd <<std::endl;getchar();}double r_x = 1.0, r_y = 1.0, r_z = 1.0;double t_x = 1.0, t_y = 1.0, t_z = 1.0;double camera[6] = { r_x,r_y,r_z,t_x,t_y,t_z};ceres::Problem problem;for (size_t i = 0; i < pts_2d.size(); i++){ceres::CostFunction* cost_function =CurveFittingCost::Create(pts_2d[i],pts_3d[i]);problem.AddResidualBlock(cost_function,nullptr	/* squared loss */,camera);}ceres::Solver::Options options;options.linear_solver_type = ceres::DENSE_SCHUR;options.minimizer_progress_to_stdout = true;ceres::Solver::Summary summary;ceres::Solve(options, &problem, &summary);std::cout << summary.FullReport() << std::endl;std::cout << "**************" << std::endl;Mat R_vec = (Mat_<double>(3, 1) << camera[0], camera[1], camera[2]);std::cout << "camera: " << std::endl;std::cout << camera[0] << " " << camera[1] << " " << camera[2] << std::endl;std::cout << camera[3] << " " << camera[4] << " " << camera[5] << std::endl;std::cout << "**************" << std::endl;Mat R_cvest;Rodrigues(R_vec, R_cvest);//羅德里格斯公式，旋轉向量轉旋轉矩陣std::cout << "R_cvest=" << std::endl << R_cvest << std::endl;Eigen::Matrix3d R_est;Eigen::Vector3d t_est(camera[3], camera[4], camera[5]);std::cout << "t_est=" << std::endl<< t_est << std::endl;std::cout << "**************" << std::endl;Eigen::Isometry3d T(R_est);//構造變換矩陣與輸出T.pretranslate(t_est);std::cout << T.matrix() << std::endl;
}

4 g2o庫的使用

這里參考高翔博士的代碼，舉個栗子。詳見slambook2 ch7 pose_estimation_2d2d.cpp部分

#include <iostream>
#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <Eigen/Core>
#include <g2o/core/base_vertex.h>
#include <g2o/core/base_unary_edge.h>
#include <g2o/core/sparse_optimizer.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/solver.h>
#include <g2o/core/optimization_algorithm_gauss_newton.h>
#include <g2o/solvers/dense/linear_solver_dense.h>
#include <sophus/se3.hpp>
#include <chrono>using namespace std;
using namespace cv;void find_feature_matches(const Mat &img_1, const Mat &img_2,std::vector<KeyPoint> &keypoints_1,std::vector<KeyPoint> &keypoints_2,std::vector<DMatch> &matches);// 像素坐標轉相機歸一化坐標
Point2d pixel2cam(const Point2d &p, const Mat &K);// BA by g2o
typedef vector<Eigen::Vector2d, Eigen::aligned_allocator<Eigen::Vector2d>> VecVector2d;
typedef vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d>> VecVector3d;void bundleAdjustmentG2O(const VecVector3d &points_3d,const VecVector2d &points_2d,const Mat &K,Sophus::SE3d &pose
);// BA by gauss-newton
void bundleAdjustmentGaussNewton(const VecVector3d &points_3d,const VecVector2d &points_2d,const Mat &K,Sophus::SE3d &pose
);int main(int argc, char **argv) {if (argc != 5) {cout << "usage: pose_estimation_3d2d img1 img2 depth1 depth2" << endl;return 1;}//-- 讀取圖像Mat img_1 = imread(argv[1], CV_LOAD_IMAGE_COLOR);Mat img_2 = imread(argv[2], CV_LOAD_IMAGE_COLOR);assert(img_1.data && img_2.data && "Can not load images!");vector<KeyPoint> keypoints_1, keypoints_2;vector<DMatch> matches;find_feature_matches(img_1, img_2, keypoints_1, keypoints_2, matches);cout << "一共找到了" << matches.size() << "組匹配點" << endl;// 建立3D點Mat d1 = imread(argv[3], CV_LOAD_IMAGE_UNCHANGED);       // 深度圖為16位無符號數，單通道圖像Mat K = (Mat_<double>(3, 3) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1);vector<Point3f> pts_3d;vector<Point2f> pts_2d;for (DMatch m:matches) {ushort d = d1.ptr<unsigned short>(int(keypoints_1[m.queryIdx].pt.y))[int(keypoints_1[m.queryIdx].pt.x)];if (d == 0)   // bad depthcontinue;float dd = d / 5000.0;Point2d p1 = pixel2cam(keypoints_1[m.queryIdx].pt, K);pts_3d.push_back(Point3f(p1.x * dd, p1.y * dd, dd));pts_2d.push_back(keypoints_2[m.trainIdx].pt);}cout << "3d-2d pairs: " << pts_3d.size() << endl;chrono::steady_clock::time_point t1 = chrono::steady_clock::now();Mat r, t;solvePnP(pts_3d, pts_2d, K, Mat(), r, t, false); // 調用OpenCV 的 PnP 求解，可選擇EPNP，DLS等方法Mat R;cv::Rodrigues(r, R); // r為旋轉向量形式，用Rodrigues公式轉換為矩陣chrono::steady_clock::time_point t2 = chrono::steady_clock::now();chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);cout << "solve pnp in opencv cost time: " << time_used.count() << " seconds." << endl;cout << "R=" << endl << R << endl;cout << "t=" << endl << t << endl;VecVector3d pts_3d_eigen;VecVector2d pts_2d_eigen;for (size_t i = 0; i < pts_3d.size(); ++i) {pts_3d_eigen.push_back(Eigen::Vector3d(pts_3d[i].x, pts_3d[i].y, pts_3d[i].z));pts_2d_eigen.push_back(Eigen::Vector2d(pts_2d[i].x, pts_2d[i].y));}cout << "calling bundle adjustment by gauss newton" << endl;Sophus::SE3d pose_gn;t1 = chrono::steady_clock::now();bundleAdjustmentGaussNewton(pts_3d_eigen, pts_2d_eigen, K, pose_gn);t2 = chrono::steady_clock::now();time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);cout << "solve pnp by gauss newton cost time: " << time_used.count() << " seconds." << endl;cout << "calling bundle adjustment by g2o" << endl;Sophus::SE3d pose_g2o;t1 = chrono::steady_clock::now();bundleAdjustmentG2O(pts_3d_eigen, pts_2d_eigen, K, pose_g2o);t2 = chrono::steady_clock::now();time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);cout << "solve pnp by g2o cost time: " << time_used.count() << " seconds." << endl;return 0;
}void find_feature_matches(const Mat &img_1, const Mat &img_2,std::vector<KeyPoint> &keypoints_1,std::vector<KeyPoint> &keypoints_2,std::vector<DMatch> &matches) {//-- 初始化Mat descriptors_1, descriptors_2;// used in OpenCV3Ptr<FeatureDetector> detector = ORB::create();Ptr<DescriptorExtractor> descriptor = ORB::create();// use this if you are in OpenCV2// Ptr<FeatureDetector> detector = FeatureDetector::create ( "ORB" );// Ptr<DescriptorExtractor> descriptor = DescriptorExtractor::create ( "ORB" );Ptr<DescriptorMatcher> matcher = DescriptorMatcher::create("BruteForce-Hamming");//-- 第一步:檢測 Oriented FAST 角點位置detector->detect(img_1, keypoints_1);detector->detect(img_2, keypoints_2);//-- 第二步:根據角點位置計算 BRIEF 描述子descriptor->compute(img_1, keypoints_1, descriptors_1);descriptor->compute(img_2, keypoints_2, descriptors_2);//-- 第三步:對兩幅圖像中的BRIEF描述子進行匹配，使用 Hamming 距離vector<DMatch> match;// BFMatcher matcher ( NORM_HAMMING );matcher->match(descriptors_1, descriptors_2, match);//-- 第四步:匹配點對篩選double min_dist = 10000, max_dist = 0;//找出所有匹配之間的最小距離和最大距離, 即是最相似的和最不相似的兩組點之間的距離for (int i = 0; i < descriptors_1.rows; i++) {double dist = match[i].distance;if (dist < min_dist) min_dist = dist;if (dist > max_dist) max_dist = dist;}printf("-- Max dist : %f \n", max_dist);printf("-- Min dist : %f \n", min_dist);//當描述子之間的距離大于兩倍的最小距離時,即認為匹配有誤.但有時候最小距離會非常小,設置一個經驗值30作為下限.for (int i = 0; i < descriptors_1.rows; i++) {if (match[i].distance <= max(2 * min_dist, 30.0)) {matches.push_back(match[i]);}}
}Point2d pixel2cam(const Point2d &p, const Mat &K) {return Point2d((p.x - K.at<double>(0, 2)) / K.at<double>(0, 0),(p.y - K.at<double>(1, 2)) / K.at<double>(1, 1));
}void bundleAdjustmentGaussNewton(const VecVector3d &points_3d,const VecVector2d &points_2d,const Mat &K,Sophus::SE3d &pose) {typedef Eigen::Matrix<double, 6, 1> Vector6d;const int iterations = 10;double cost = 0, lastCost = 0;double fx = K.at<double>(0, 0);double fy = K.at<double>(1, 1);double cx = K.at<double>(0, 2);double cy = K.at<double>(1, 2);for (int iter = 0; iter < iterations; iter++) {Eigen::Matrix<double, 6, 6> H = Eigen::Matrix<double, 6, 6>::Zero();Vector6d b = Vector6d::Zero();cost = 0;// compute costfor (int i = 0; i < points_3d.size(); i++) {Eigen::Vector3d pc = pose * points_3d[i];double inv_z = 1.0 / pc[2];double inv_z2 = inv_z * inv_z;Eigen::Vector2d proj(fx * pc[0] / pc[2] + cx, fy * pc[1] / pc[2] + cy);Eigen::Vector2d e = points_2d[i] - proj;cost += e.squaredNorm();Eigen::Matrix<double, 2, 6> J;J << -fx * inv_z,0,fx * pc[0] * inv_z2,fx * pc[0] * pc[1] * inv_z2,-fx - fx * pc[0] * pc[0] * inv_z2,fx * pc[1] * inv_z,0,-fy * inv_z,fy * pc[1] * inv_z2,fy + fy * pc[1] * pc[1] * inv_z2,-fy * pc[0] * pc[1] * inv_z2,-fy * pc[0] * inv_z;H += J.transpose() * J;b += -J.transpose() * e;}Vector6d dx;dx = H.ldlt().solve(b);if (isnan(dx[0])) {cout << "result is nan!" << endl;break;}if (iter > 0 && cost >= lastCost) {// cost increase, update is not goodcout << "cost: " << cost << ", last cost: " << lastCost << endl;break;}// update your estimationpose = Sophus::SE3d::exp(dx) * pose;lastCost = cost;cout << "iteration " << iter << " cost=" << std::setprecision(12) << cost << endl;if (dx.norm() < 1e-6) {// convergebreak;}}cout << "pose by g-n: \n" << pose.matrix() << endl;
}/// vertex and edges used in g2o ba
class VertexPose : public g2o::BaseVertex<6, Sophus::SE3d> {
public:EIGEN_MAKE_ALIGNED_OPERATOR_NEW;virtual void setToOriginImpl() override {_estimate = Sophus::SE3d();}/// left multiplication on SE3virtual void oplusImpl(const double *update) override {Eigen::Matrix<double, 6, 1> update_eigen;update_eigen << update[0], update[1], update[2], update[3], update[4], update[5];_estimate = Sophus::SE3d::exp(update_eigen) * _estimate;}virtual bool read(istream &in) override {}virtual bool write(ostream &out) const override {}
};class EdgeProjection : public g2o::BaseUnaryEdge<2, Eigen::Vector2d, VertexPose> {
public:EIGEN_MAKE_ALIGNED_OPERATOR_NEW;EdgeProjection(const Eigen::Vector3d &pos, const Eigen::Matrix3d &K) : _pos3d(pos), _K(K) {}virtual void computeError() override {const VertexPose *v = static_cast<VertexPose *> (_vertices[0]);Sophus::SE3d T = v->estimate();Eigen::Vector3d pos_pixel = _K * (T * _pos3d);pos_pixel /= pos_pixel[2];_error = _measurement - pos_pixel.head<2>();}virtual void linearizeOplus() override {const VertexPose *v = static_cast<VertexPose *> (_vertices[0]);Sophus::SE3d T = v->estimate();Eigen::Vector3d pos_cam = T * _pos3d;double fx = _K(0, 0);double fy = _K(1, 1);double cx = _K(0, 2);double cy = _K(1, 2);double X = pos_cam[0];double Y = pos_cam[1];double Z = pos_cam[2];double Z2 = Z * Z;_jacobianOplusXi<< -fx / Z, 0, fx * X / Z2, fx * X * Y / Z2, -fx - fx * X * X / Z2, fx * Y / Z,0, -fy / Z, fy * Y / (Z * Z), fy + fy * Y * Y / Z2, -fy * X * Y / Z2, -fy * X / Z;}virtual bool read(istream &in) override {}virtual bool write(ostream &out) const override {}private:Eigen::Vector3d _pos3d;Eigen::Matrix3d _K;
};void bundleAdjustmentG2O(const VecVector3d &points_3d,const VecVector2d &points_2d,const Mat &K,Sophus::SE3d &pose) {// 構建圖優化，先設定g2otypedef g2o::BlockSolver<g2o::BlockSolverTraits<6, 3>> BlockSolverType;  // pose is 6, landmark is 3typedef g2o::LinearSolverDense<BlockSolverType::PoseMatrixType> LinearSolverType; // 線性求解器類型// 梯度下降方法，可以從GN, LM, DogLeg 中選auto solver = new g2o::OptimizationAlgorithmGaussNewton(g2o::make_unique<BlockSolverType>(g2o::make_unique<LinearSolverType>()));g2o::SparseOptimizer optimizer;     // 圖模型optimizer.setAlgorithm(solver);   // 設置求解器optimizer.setVerbose(true);       // 打開調試輸出// vertexVertexPose *vertex_pose = new VertexPose(); // camera vertex_posevertex_pose->setId(0);vertex_pose->setEstimate(Sophus::SE3d());optimizer.addVertex(vertex_pose);// KEigen::Matrix3d K_eigen;K_eigen <<K.at<double>(0, 0), K.at<double>(0, 1), K.at<double>(0, 2),K.at<double>(1, 0), K.at<double>(1, 1), K.at<double>(1, 2),K.at<double>(2, 0), K.at<double>(2, 1), K.at<double>(2, 2);// edgesint index = 1;for (size_t i = 0; i < points_2d.size(); ++i) {auto p2d = points_2d[i];auto p3d = points_3d[i];EdgeProjection *edge = new EdgeProjection(p3d, K_eigen);edge->setId(index);edge->setVertex(0, vertex_pose);edge->setMeasurement(p2d);edge->setInformation(Eigen::Matrix2d::Identity());optimizer.addEdge(edge);index++;}chrono::steady_clock::time_point t1 = chrono::steady_clock::now();optimizer.setVerbose(true);optimizer.initializeOptimization();optimizer.optimize(10);chrono::steady_clock::time_point t2 = chrono::steady_clock::now();chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);cout << "optimization costs time: " << time_used.count() << " seconds." << endl;cout << "pose estimated by g2o =\n" << vertex_pose->estimate().matrix() << endl;pose = vertex_pose->estimate();
}

to p3d = points_3d[i];
EdgeProjection *edge = new EdgeProjection(p3d, K_eigen);
edge->setId(index);
edge->setVertex(0, vertex_pose);
edge->setMeasurement(p2d);
edge->setInformation(Eigen::Matrix2d::Identity());
optimizer.addEdge(edge);
index++;
}

chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
optimizer.setVerbose(true);
optimizer.initializeOptimization();
optimizer.optimize(10);
chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
chrono::duration time_used = chrono::duration_cast<chrono::duration>(t2 - t1);
cout << “optimization costs time: " << time_used.count() << " seconds.” << endl;
cout << “pose estimated by g2o =\n” << vertex_pose->estimate().matrix() << endl;
pose = vertex_pose->estimate();
}

總結

以上是生活随笔為你收集整理的【Smooth】非线性优化的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。