從零開始學SLAM: Ceres/G2O求解優化問題
阿新 • • 發佈：2018-11-08
Ceres求解優化問題
從《視覺SLAM十四講》和ceres的tutorial開始學起，同時複習一下C++中的語法。
struct CURVE_FITTING_COST { CURVE_FITTING_COST ( double x,double y): _x(x),_y(y) {} template <typename T> bool operator()( const T* const abc, T* residual )const { residual[0]=T(_y)-ceres::exp(abc[0]*T(_x)*T(_x)+abc[1]*T(_x)+abc[2]); return true; } const double _x,_y; }
要使用Ceres，首先要定義求解問題的cost function<CURVE_FITTING_COST>。
在結構體中首先構造建構函式：
CURVE_FITTING_COST ( double x,double y): _x(x),_y(y) {} //結構體的建構函式初始化列表
//相當於CURVE_FITTING_COST ( double x,double y) {_x=x; _y=y}
接下來，定義模板template
template <typename T>
template的使用是為了簡化不同型別的函式和類的重複定義，模版例項化時可以替換任意型別
bool operator()( const T* const abc, T* residual )const
{
residual[0]=T(_y)-ceres::exp(abc[0]*T(_x)*T(_x)+abc[1]*T(_x)+abc[2]);
return true;
}
過載（）符號，仿函式的小技巧，使結構體的一個例項具有類似一個函式的性質，在程式碼編寫過程中能當做一個函式一樣來使用。
對結構體、類的一個例項，比如Myclass類的一個例項Obj1，如果Myclass裡對（）進行了過載，那Obj1被建立之後，就可以將Obj1這個例項當做函式來用，比如Obj（x）。在這裡CURVE_FITTING_COST（）中共傳入了兩個變數，包括3維引數向量abc以及殘差residual。
int main ( int argc, char** argv )
{
double a=1.0, b=2.0, c=1.0; // 真實引數值
int N=100; // 資料點
double w_sigma=1.0; // 噪聲Sigma值
cv::RNG rng; // OpenCV隨機數產生器
double abc[3] = {0,0,0}; // abc引數的估計值
vector<double> x_data, y_data; // 資料
cout<<"generating data: "<<endl;
for ( int i=0; i<N; i++ )
{
double x = i/100.0;
x_data.push_back ( x );
y_data.push_back (
exp ( a*x*x + b*x + c ) + rng.gaussian ( w_sigma )
);
cout<<x_data[i]<<" "<<y_data[i]<<endl;
}
// 構建最小二乘問題
ceres::Problem problem;
for ( int i=0; i<N; i++ )
{
problem.AddResidualBlock ( // 向問題中新增誤差項
// 使用自動求導，模板引數：誤差型別，輸出維度，輸入維度，維數要與前面struct中一致
new ceres::AutoDiffCostFunction<CURVE_FITTING_COST, 1, 3> (
new CURVE_FITTING_COST ( x_data[i], y_data[i] )
),
nullptr, // 核函式，這裡不使用，為空
abc // 待估計引數
);
}
// 配置求解器
ceres::Solver::Options options; // 這裡有很多配置項可以填
options.linear_solver_type = ceres::DENSE_QR; // 增量方程如何求解
options.minimizer_progress_to_stdout = true; // 輸出到cout
ceres::Solver::Summary summary; // 優化資訊
chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
ceres::Solve ( options, &problem, &summary ); // 開始優化
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 time cost = "<<time_used.count()<<" seconds. "<<endl;
// 輸出結果
cout<<summary.BriefReport() <<endl;
cout<<"estimated a,b,c = ";
for ( auto a:abc ) cout<<a<<" ";
cout<<endl;
return 0;
}
這裡主要對之前我們構建的costfunction是怎麼使用的進行講解，也就是下面的程式碼
problem.AddResidualBlock (
new ceres::AutoDiffCostFunction<CURVE_FITTING_COST, 1, 3> (
new CURVE_FITTING_COST ( x_data[i], y_data[i] )
),
nullptr,
abc
);
這一段程式碼可以寫成另外一種簡單的形式
CostFunction* cost_function =
new AutoDiffCostFunction<CURVE_FITTING_COST, 1, 3>(new CURVE_FITTING_COST( x_data[i], y_data[i] ));
problem.AddResidualBlock (cost_function， nullptr, abc );
其中，我們第一段程式碼new CURVE_FITTING_COST( x_data[i], y_data[i] )呼叫了結構體的建構函式，而<CURVE_FITTING_COST, 1, 3>中，1代表殘差維度，3代表待優化的引數維度。回顧之前的建構函式，過載（）時傳入的引數分別是優化變數和殘差（殘差就是代價函式的輸出）。然後將CURVE_FITTING_COST傳入AutoDiffCostFunction方法來構建尋優問題。
第二段是向問題中新增誤差項，nullptr表示空指標（相較於NULL，優點在於在編譯器進行解釋程式時，NULL會被直接解釋成0，變為一個數字，而nullptr不會），abc為待尋優引數。
for ( auto a:abc )
表示的則是基於範圍的for迴圈，例如
#include<iostream>
#include<stdlib.h>
#include<string>
using namespace std;
int main()
{
int arr[] = { 1, 2, 3, 4, 5, 6, 7, 8, 9, 10 };
for (auto val : arr)
{ cout << val << " "; }
system("pause");
return 0;
}
//輸出結果： 1 2 3 4 5 6 7 8 9 10
CMakeList程式碼為
cmake_minimum_required(VERSION 2.8)
project(ceres)
find_package(Ceres REQUIRED)
include_directories(${CERES_INCLUDE_DIRS})
add_executable(use_ceres use_ceres.cpp)
target_link_libraries(use_ceres ${CERES_LIBRARIES})
參考
https://blog.csdn.net/cqrtxwd/article/details/78956227 一文助你Ceres 入門——Ceres Solver新手向全攻略
https://blog.csdn.net/lixiaogang_theanswer/article/details/79969012 基於範圍的for迴圈
https://blog.csdn.net/fightingforcv/article/details/51472586 C++中的模板template
http://www.ceres-solver.org/nnls_tutorial.html Ceres: Tutorial: Non-linear Least Squares
https://blog.csdn.net/tianshuai1111/article/details/7687983 【C++ STL】深入解析神祕的 — 仿函式
G2O解決優化問題
在程式碼的編譯過程中，出現了無法找到FindG2O.cmake的問題，解決這個問題的方法可以將g2o下載檔案中cmake_modules資料夾下的對應cmake檔案拷進usr/share/cmake-3.10/Modules下，在自己package中加入
list( APPEND CMAKE_MODULE_PATH /home/g2o/cmake_modules)
set(G2O_ROOT /home/g2o)
可以解決這個問題。
在解決這個問題之後，由於使用的ubuntu版本為18.04，高翔的程式碼需要作出修改，詳情可以參考 https://blog.csdn.net/robinhjwy/article/details/78084210
修改後程式碼為
#include <iostream>
#include <g2o/core/base_vertex.h>
#include <g2o/core/base_unary_edge.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/core/optimization_algorithm_gauss_newton.h>
#include <g2o/core/optimization_algorithm_dogleg.h>
#include <g2o/solvers/dense/linear_solver_dense.h>
#include <g2o/solvers/csparse/linear_solver_csparse.h>
#include <Eigen/Core>
#include <opencv2/core/core.hpp>
#include <cmath>
#include <chrono>
using namespace std;
// 曲線模型的頂點，模板引數：優化變數維度和資料型別
class CurveFittingVertex: public g2o::BaseVertex<3, Eigen::Vector3d>
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
virtual void setToOriginImpl() // 重置
{
_estimate << 0,0,0;
}
virtual void oplusImpl( const double* update ) // 更新
{
_estimate += Eigen::Vector3d(update);
}
// 存檔和讀盤：留空
virtual bool read( istream& in ) {}
virtual bool write( ostream& out ) const {}
};
// 誤差模型 模板引數：觀測值維度，型別，連線頂點型別
class CurveFittingEdge: public g2o::BaseUnaryEdge<1,double,CurveFittingVertex>
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
CurveFittingEdge( double x ): BaseUnaryEdge(), _x(x) {}
// 計算曲線模型誤差
void computeError()
{
const CurveFittingVertex* v = static_cast<const CurveFittingVertex*> (_vertices[0]);
const Eigen::Vector3d abc = v->estimate();
_error(0,0) = _measurement - std::exp( abc(0,0)*_x*_x + abc(1,0)*_x + abc(2,0) ) ;
}
virtual bool read( istream& in ) {}
virtual bool write( ostream& out ) const {}
public:
double _x; // x 值， y 值為 _measurement
};
int main( int argc, char** argv )
{
double a=1.0, b=2.0, c=1.0; // 真實引數值
int N=100; // 資料點
double w_sigma=1.0; // 噪聲Sigma值
cv::RNG rng; // OpenCV隨機數產生器
double abc[3] = {0,0,0}; // abc引數的估計值
vector<double> x_data, y_data; // 資料
cout<<"generating data: "<<endl;
for ( int i=0; i<N; i++ )
{
double x = i/100.0;
x_data.push_back ( x );
y_data.push_back (
exp ( a*x*x + b*x + c ) + rng.gaussian ( w_sigma )
);
cout<<x_data[i]<<" "<<y_data[i]<<endl;
}
// 初始化g2o
typedef g2o::BlockSolver< g2o::BlockSolverTraits<3,1> > Block;
//std::unique_ptr<Block::LinearSolverType> linearSolver ( new g2o::LinearSolverDense<Block::PoseMatrixType>());
std::unique_ptr<Block::LinearSolverType> linearSolver ( new g2o::LinearSolverDense<Block::PoseMatrixType>());
std::unique_ptr<Block> solver_ptr ( new Block ( std::move(linearSolver)));
g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg ( std::move(solver_ptr));
g2o::SparseOptimizer optimizer;
optimizer.setAlgorithm (solver);
optimizer.setVerbose( true ); // 開啟除錯輸出
// 往圖中增加頂點
CurveFittingVertex* v = new CurveFittingVertex();
v->setEstimate( Eigen::Vector3d(0,0,0) );
v->setId(0);
optimizer.addVertex( v );
// 往圖中增加邊
for ( int i=0; i<N; i++ )
{
CurveFittingEdge* edge = new CurveFittingEdge( x_data[i] );
edge->setId(i);
edge->setVertex( 0, v ); // 設定連線的頂點
edge->setMeasurement( y_data[i] ); // 觀測數值
edge->setInformation( Eigen::Matrix<double,1,1>::Identity()*1/(w_sigma*w_sigma) ); // 資訊矩陣：協方差矩陣之逆
optimizer.addEdge( edge );
}
// 執行優化
cout<<"start optimization"<<endl;
chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
optimizer.initializeOptimization();
optimizer.optimize(100);
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 time cost = "<<time_used.count()<<" seconds. "<<endl;
// 輸出優化值
Eigen::Vector3d abc_estimate = v->estimate();
cout<<"estimated model: "<<abc_estimate.transpose()<<endl;
return 0;
}
CMakeLists.txt為
cmake_minimum_required( VERSION 2.8 )
project( g2o_curve_fitting )
set( CMAKE_BUILD_TYPE "Release" )
set( CMAKE_CXX_FLAGS "-std=c++11 -O3" )
# 新增cmake模組以使用ceres庫
list( APPEND CMAKE_MODULE_PATH /home/g2o/cmake_modules)
set(G2O_ROOT /home/g2o)
find_package(G2O REQUIRED)
include_directories(
${G2O_INCLUDE_DIRS}
"/usr/include/eigen3"
)
find_package( OpenCV REQUIRED )
# OpenCV
include_directories( ${OpenCV_DIRS} )
add_executable( curve_fitting main.cpp )
# 與G2O和OpenCV連結
target_link_libraries( curve_fitting
${OpenCV_LIBS}
g2o_core g2o_stuff
)