SLAM Eigen/Sophus/ceres/g2o/PCL開發庫安裝與使用 --持續更新

文章目錄

0. 寫在前面

SLAM這個經典體系不是說某種具體的算法，而是研究者通過長期研究工作、總結出來的一個比較完善的框架，在這個框架中設計了許多算法以及算法的運行，所以在進行具體的SLMA代碼的編寫時，我們要充分運行現行的開發庫與代碼庫，使我們的代碼更簡潔更高效，下面持續更新一些SLAM開發庫的安裝與使用。

Link libGL.so for TX1/TX2 compile bug
sudo ln -sf /usr/lib/aarch64-linux-gnu/tegra/libGL.so /usr/lib/aarch64-linux-gnu/libGL.so

1. Eigen 矩陣、向量的運算庫

Eigen is a C++ template library for linear algebra: matrices, vectors, numerical solvers, and related algorithms.

- INSTALL Eigen

Eigen庫是一個Ｃ＋＋線性代數開源庫，它提供了有關線性代數、矩陣和矢量運算、數值分析及相關的算法。許多上層的軟件庫也使用Eigen進行矩陣運算，包括g2o、Sophus等。Eigen庫由Ubuntu軟件源中提供，通過apt命令可以很方便的安裝Eigen

sudo apt-get install libeigen3-dev

Eigen與其他庫不同，它是一個由頭文件搭建起來的庫，Eigen頭文件的默認安裝位置在/usr/include/eigen3/中。我們在使用時，只需引入Eigen頭文件，不需要鏈接它的庫文件，在CMakeLists.txt裏添加Eigen頭文件的目錄

include_dircetories("/usr/include/eigen3")

- Eigen use-case

#include <iostream>
using namespace std;
#include <ctime>
#include <Eigen/Core>  //普通矩陣運算
#include <Eigen/Dense>　// 稠密矩陣的代數運算（逆，特徵值等）

// 矩陣的定義
	// Eigen 中所有向量和矩陣都是Eigen::Matrix，它是一個模板類。
	//它的前三個參數爲：數據類型，行，列
	// 聲明一個2*3的float矩陣
    Eigen::Matrix<float, 2, 3> matrix_23;
	//特殊形狀的矩陣
	Eigen::Vector3d v_3d;　　//等價於Eigen::Matrix<float,3,1> vd_3d
	// Matrix3d 或 Matrix3f double & float
	Eigen::Matrix3d matrix_33 = Eigen::Matrix3d::Zero(); //初始化爲零
	
//矩陣元素的訪問
	matrix_23 << 1, 2, 3, 4, 5, 6;　//直接訪問
	cout<<matrix_23(i,j)<<"\t"; //下標訪問
//矩陣的運算
	//１、注意矩陣的形狀
	//２、注意不同精度的矩陣的顯示轉換
	 Eigen::Matrix<double, 2, 1> result = matrix_23.cast<double>() * v_3d;//注意double / float轉換

	//矩陣的基本運算
	matrix_33 = Eigen::Matrix3d::Random();      // 隨機數矩陣
	matrix_33 = Eigen::Matrix3d::Zero();      // 零矩陣矩陣
	matrix_33 = Eigen::Matrix3d::Identity();  //單位矩陣
    cout << matrix_33.transpose() << endl;      // 轉置
    cout << matrix_33.sum() << endl;            // 各元素和
    cout << matrix_33.trace() << endl;          // 跡
    cout << 10*matrix_33 << endl;               // 數乘
    cout << matrix_33.inverse() << endl;        // 逆
    cout << matrix_33.determinant() << endl;    // 行列式

    // 特徵值與特徵向量
    // 實對稱矩陣可以保證對角化成功
    Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> eigen_solver ( matrix_33.transpose()*matrix_33 );
    cout << "Eigen values = \n" << eigen_solver.eigenvalues() << endl;
    cout << "Eigen vectors = \n" << eigen_solver.eigenvectors() << endl;

	//解矩陣方程
 clock_t time_stt = clock(); // 計時
    // 直接求逆
    Eigen::Matrix<double,MATRIX_SIZE,1> x = matrix_NN.inverse()*v_Nd;
    cout <<"time use in normal inverse is " << 1000* (clock() - time_stt)/(double)CLOCKS_PER_SEC << "ms"<< endl;
    
	// 通常用矩陣分解來求，例如QR分解，速度會快很多
   	 time_stt = clock();
    	x = matrix_NN.colPivHouseholderQr().solve(v_Nd);
    	cout <<"time use in Qr decomposition is " <<1000*  (clock() - time_stt)/(double)CLOCKS_PER_SEC <<"ms" << endl;

cmake_minimum_required( VERSION 2.8 )
project( geometry )

# 添加Eigen頭文件
include_directories( "/usr/include/eigen3" )

add_executable( eigenGeometry   eigenGeometry.cpp )

- Eigen/Geometry(Eigen的幾何變換模塊進行旋轉變換)

Eigen Geometry module API

#include <cmath>  //定義的PI常量
#include <Eigen/Core>　//Eigen　基本庫
#include <Eigen/Geometry> 　//Eigen 幾何旋轉庫

//0 定義旋轉矩陣　旋轉向量
Eigen::Matrix3d rotation_matrix = Eigen::Matrix3d::Identity();
Eigen::AngleAxisd rotation_vector ( M_PI/4, Eigen::Vector3d ( 0,0,1 ) );     //(角/軸)沿 Z 軸旋轉 45 度

//1 旋轉向量與旋轉矩陣的轉換
	//旋轉向量轉旋轉矩陣
	rotation_matrix = rotation_vector.toRotationMatrix();　
	rotation_matrix = rotation_vector.matrix(); //兩種方式一樣的

//使用旋轉矩陣/旋轉向量進行座標變換
    // 用 AngleAxis 可以進行座標變換
    Eigen::Vector3d v ( 1,0,0 );
    Eigen::Vector3d v_rotated = rotation_vector * v;
    cout<<"(1,0,0) after rotation = "<<v_rotated.transpose()<<endl;
    // 用旋轉矩陣進行座標變換
    v_rotated = rotation_matrix * v;
    cout<<"(1,0,0) after rotation = "<<v_rotated.transpose()<<endl;

//2 旋轉矩陣與歐拉角的轉換
    // 旋轉矩陣＝＝>歐拉角
    Eigen::Vector3d euler_angles = rotation_matrix.eulerAngles ( 2,1,0 ); // ZYX順序，即ywa pitch roll順序
    cout<<"yaw pitch roll = "<<euler_angles.transpose()<<endl;	
    	
//3 變換矩陣Ｔ　(歐式變換矩陣的使用)
    // 歐氏變換矩陣使用 Eigen::Isometry
    Eigen::Isometry3d T=Eigen::Isometry3d::Identity();     // 雖然寫的3d，實質上是4＊4的矩陣(3d　指的是Ｒ)
    T.rotate ( rotation_vector );                        　 // R 按照rotation_vector進行旋轉
    T.pretranslate ( Eigen::Vector3d ( 1,3,4 ) );          //t 把平移向量設成(1,3,4)
    cout << "Transform matrix = \n" << T.matrix() <<endl;
//使用變換矩陣進行座標變換
    Eigen::Vector3d v_transformed = T*v;          // 相當於R*v+t
    cout<<"v tranformed = "<<v_transformed.transpose()<<endl;

//４　旋轉矩陣/向量與四元素的轉換
	//旋轉向量轉四元素
    Eigen::Quaterniond q = Eigen::Quaterniond ( rotation_vector );
    cout<<"quaternion = \n"<<q.coeffs() <<endl;   // 請注意coeffs的順序是(x,y,z,w),w爲實部，前三者爲虛部
    // 旋轉矩陣轉四元素
    q = Eigen::Quaterniond ( rotation_matrix );
    cout<<"quaternion = \n"<<q.coeffs() <<endl;
    // 使用四元數旋轉一個向量，使用重載的乘法即可
    v_rotated = q*v; // 注意數學上是qvq^{-1}
    cout<<"(1,0,0) after rotation = "<<v_rotated.transpose()<<endl;

旋轉矩陣，旋轉向量和四元數的初始化和相互轉換

2. Pangolin (OpenGL 顯示)

install Pangolin

https://github.com/stevenlovegrove/Pangolin
仔細看README文檔
添加依賴
sudo apt-get install libglew-dev
git clone https://github.com/stevenlovegrove/Pangolin.git
cd Pangolin
mkdir build
cd build
cmake …
cmake --build .

sudo make install

3.Sophus李代數庫

Sophus庫支持三維運動的SO(3)、SE(3)，此外還支持二維運動的SO(2)、SE(2)和相似變換Sim(3)等內容。它是直接在Eigen庫基礎上開發的，因此我們不需要安裝額外的依賴庫。可以直接從github上獲取Sophus庫，Sophus庫有模板類庫和非模板類庫兩個版本，非模板類庫使用相對簡單。模板類的Sophus庫使用相對複雜一些。

可以通過輸入以下命令獲得非模板類的Sophus庫
https://github.com/strasdat/Sophus

install Sophus

git clone http://github.com/strasdat/Sophus.git
cd Sophus
git checkout a621ff

cd Sophus
mkdir build
cd build
cmake …
make

李羣 Sophus::SO3 可以由旋轉矩陣、旋轉向量、四元素來定義
李代數 Eigen::Vector3d （實際物理意義就是旋轉向量）
log 李羣–>李代數的映射 so3 = SO3_R.log()
exp 李代數–>李羣的映射 Sophus::SO3::exp(update_so3)
hat ^ 李代數(也就是旋轉向量的反對稱矩陣) Sophus::SO3::hat(so3)
vee 反對稱矩陣到向量的轉換 Sophus::SO3::vee()
擾動即乘上一個小量

Sophus use-case

#include <iostream>
#include <cmath>
using namespace std; 
#include <Eigen/Core>
#include <Eigen/Geometry>
#include "sophus/so3.h"
#include "sophus/se3.h"

// 1 定義旋轉矩陣與平移向量　R t
   // 沿Z軸轉90度的旋轉矩陣
    Eigen::Matrix3d R = Eigen::AngleAxisd(M_PI/2, Eigen::Vector3d(0,0,1)).toRotationMatrix();
    //定義平移向量
    Eigen::Vector3d t(1,0,0);           // 沿X軸平移1
    
// SO(3) 旋轉矩陣李羣與李代數
	//旋轉矩陣李羣SO(3)可以由　旋轉矩陣/旋轉向量/四元素得到,並且都是等效的　
	//(注意李羣的表示形式    Sophus::SO3)

    Sophus::SO3 SO3_R(R);               // Sophus::SO(3)可以直接從旋轉矩陣構造
    Sophus::SO3 SO3_v( 0, 0, M_PI/2 );  // 亦可從旋轉向量構造(注意此時旋轉變量的形式)
    Eigen::Quaterniond q(R);            // 或者四元數
    Sophus::SO3 SO3_q( q );
    	
    cout<<"SO(3) from vector: "<<SO3_V<<endl;
    cout<<"SO(3) from VECTOR: "<<SO3_R<<endl;
    cout<<"SO(3) from quaternion :"<<SO3_q<<endl;    

	//旋轉矩陣李代數　（李羣的對數映射）
	//(SO(3)李代數表示形式    Eigen::Vector3d 
    Eigen::Vector3d 　so3 = SO3_R.log();
    cout<<"so3 = "<<so3.transpose()<<endl;
    // hat 爲向量==>反對稱矩陣 (李代數向量　對應的反對稱矩陣)
    cout<<"so3 hat=\n"<<Sophus::SO3::hat(so3)<<endl;
    // 相對的，vee爲反對稱==>向量
    cout<<"so3 hat vee= "<<Sophus::SO3::vee( Sophus::SO3::hat(so3) ).transpose()<<endl; // transpose純粹是爲了輸出美觀一些
    
    //旋轉矩陣李代數的 增量擾動模型的更新
    Eigen::Vector3d update_so3(1e-4, 0, 0); //假設更新量爲這麼多
    Sophus::SO3 SO3_updated = Sophus::SO3::exp(update_so3)*SO3_R;
    cout<<"SO3 updated = "<<SO3_updated<<endl;

//SE(3) 變換矩陣李羣與李代數

	//變換矩陣李羣SE(3)可以由　旋轉矩陣/四元素 + 平移向量得到,並且都是等效的　
    Sophus::SE3 SE3_Rt(R, t);           // 從R,t構造SE(3)
    Sophus::SE3 SE3_qt(q,t);            // 從q,t構造SE(3)
    cout<<"SE3 from R,t= "<<endl<<SE3_Rt<<endl;
    cout<<"SE3 from q,t= "<<endl<<SE3_qt<<endl;

	//變換矩陣李代數　（李羣的對數映射）
	//(SE(3)李代數表示形式    Eigen::Matrix<double,6,1>   sophus中旋轉在前，平移在後
	typedef Eigen::Matrix<double,6,1> Vector6d;
	Vector6d se3 = SE3_Rt.log();
    cout<<"se3 = "<<se3.transpose()<<endl;
    
    //向量的反對稱矩陣表示形式的變換
    cout<<"se3 hat = "<<endl<<Sophus::SE3::hat(se3)<<endl;
    cout<<"se3 hat vee = "<<Sophus::SE3::vee( Sophus::SE3::hat(se3) ).transpose()<<endl;
    
　　 //變換矩陣李代數的 增量擾動模型的更新
    Vector6d update_se3; //更新量
    update_se3.setZero();
    update_se3(0,0) = 1e-4d;
    cout<< "se3_update =  " << update_se3.transpose() <<endl;
    Sophus::SE3 SE3_updated = Sophus::SE3::exp(update_se3)*SE3_Rt;
    cout<<"SE3 updated = "<<endl<<SE3_updated.matrix()<<endl;

cmake_minimum_required( VERSION 2.8 )
project( useSophus )

find_package( Sophus REQUIRED )
message(" ${Sophus_INCLUDE_DIRS}")
include_directories( ${Sophus_INCLUDE_DIRS} )

add_executable( useSophus useSophus.cpp )
target_link_libraries( useSophus ${Sophus_LIBRARIES} )

4-1 ceres非線性優化庫

Ceres solver 是谷歌開發的一款用於非線性優化的庫，在谷歌的開源激光雷達slam項目cartographer中被大量使用。Ceres官網上的文檔非常詳細地介紹了其具體使用方法，相比於另外一個在slam中被廣泛使用的圖優化庫G2O，ceres的文檔相對來說比較詳細。

install ceres庫

git clone https://ceres-solver.googlesource.com/ceres-solver
cd ceres-solver
mkdir build && cd build
cmake …
make
遇到一個問題 //usr/lib/libblas.so.3: undefined reference to `gotoblas’
解決辦法 sudo apt-get remove libopenblas-base

use-case of ceres-solver 曲線擬合

ceres 求解非線性優化的三個步驟

構建cost function代價函數，也就是尋優的目標式
通過代價函數構建待求解的優化問題 ceres::Problem problem.AddResidualBlock
配置求解器參數，並求解問題ceres::Solver::Options

#include <iostream>
#include <opencv2/core/core.hpp>
#include <ceres/ceres.h>
#include <chrono>

using namespace std;

// 代價函數的計算模型
struct CURVE_FITTING_COST
{
   //cost function 
   CURVE_FITTING_COST ( double x, double y ) : _x ( x ), _y ( y ) {}
   // 殘差的計算
   template <typename T>
   bool operator() (
       const T* const abc,     // (輸入)帶估計的模型參數，有3維
       T* residual ) const     // (輸出)殘差
   {
       residual[0] = T ( _y ) - ceres::exp ( abc[0]*T ( _x ) *T ( _x ) + abc[1]*T ( _x ) + abc[2] ); // y-exp(ax^2+bx+c)
       return true;
   }
   const double _x, _y;    // x,y數據
};

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;
   //100組帶有噪聲的觀測數據
   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;
}

cmake_minimum_required( VERSION 2.8 )
project( ceres_curve_fitting )
set( CMAKE_BUILD_TYPE "Release" )
set( CMAKE_CXX_FLAGS "-std=c++11 -O3" )

# 添加cmake模塊以使用ceres庫
# list( APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake_modules )

# 尋找Ceres庫並添加它的頭文件
find_package( Ceres REQUIRED )
include_directories( ${CERES_INCLUDE_DIRS} )

# OpenCV
find_package( OpenCV REQUIRED )
include_directories( ${OpenCV_DIRS} )

add_executable( curve_fitting main.cpp )
# 與Ceres和OpenCV鏈接
target_link_libraries( curve_fitting ${CERES_LIBRARIES} ${OpenCV_LIBS} )

4-2 圖優化 g2o庫(General Graph Optimization)

優化問題的三個要素：目標函數、優化變量和優化約束。簡單說來，優化問題就是在給定約束條件下，求使目標函數最小的優化變量的值。
圖優化是一種將非線性理論與圖論結合起來的理論，在圖優化中將頂點表示優化變量，邊表示誤差項，從而將非線性最小二乘問題轉化成構建一直對應的一個圖。
g2o是一個基於圖優化的庫, 利用g2o庫，可以利用圖優化來求解非線性優化問題。

例如在SLAM中，可以用圖優化的圖模型來表達一個對相機運動和地圖的非線性最小二乘的優化問題。

頂點表示待優化的變量，位姿節點和路標節點
邊表示模型的目標函數的誤差這裏用運動方程和觀測方程來描述

install g2o

安裝依賴
sudo apt-get install libqt4-dev qt4-qmake libqglviewer-dev libsuitesparse-dev libcxsparse3.1.2
libcholmod-dev (注：最後一個依賴項需要table鍵來填充名稱)

git clone https://github.com/RainerKuemmerle/g2o.git
cd g2o
mkdir build && cd build
cmake …
make -j12
sudo make install

use-case : g2o曲線擬合

首先根據優化問題，表示出圖的形式
在程序中定義頂點和邊
構建圖優化，設置求解器
在圖中添加頂點和邊，以及他們的對應關係
執行優化並輸出優化結果

問題描述：根據100組帶有噪聲的觀測值，來擬合曲線

構建圖 一個頂點和100條邊

#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 <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 {}
};

// 誤差模型 模板參數：觀測值維度，類型，連接頂點類型
// 這個模型是一元邊，所以從BaseUnaryEdge派生， 誤差是1維的double類型，所連接的頂點
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;  // 每個誤差項優化變量維度爲3，誤差值維度爲1
    std::unique_ptr<Block::LinearSolverType> linearSolver ( new g2o::LinearSolverDense<Block::PoseMatrixType>());
    //Block::LinearSolverType* linearSolver = new g2o::LinearSolverDense<Block::PoseMatrixType>(); // 線性方程求解器
    //Block* solver_ptr = new Block( linearSolver );      // 矩陣塊求解器
    std::unique_ptr<Block> solver_ptr ( new Block ( std::move(linearSolver))); 
    // 梯度下降方法，從GN, LM, DogLeg 中選
    //g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg( std::move(solver_ptr));
    //g2o::OptimizationAlgorithmGaussNewton* solver = new g2o::OptimizationAlgorithmGaussNewton( std::move(solver_ptr));
    g2o::OptimizationAlgorithmDogleg* solver = new g2o::OptimizationAlgorithmDogleg( std::move(solver_ptr));

    //g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg( solver_ptr );
    // g2o::OptimizationAlgorithmGaussNewton* solver = new g2o::OptimizationAlgorithmGaussNewton( solver_ptr );
    // g2o::OptimizationAlgorithmDogleg* solver = new g2o::OptimizationAlgorithmDogleg( 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;
}

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 ${PROJECT_SOURCE_DIR}/cmake_modules )

# 尋找G2O
find_package( G2O REQUIRED )
include_directories( 
    ${G2O_INCLUDE_DIRS}
    "/usr/include/eigen3"
)

# OpenCV
find_package( OpenCV REQUIRED )
include_directories( ${OpenCV_DIRS} )

add_executable( curve_fitting main.cpp )
# 與G2O和OpenCV鏈接
target_link_libraries( curve_fitting    ${OpenCV_LIBS}    g2o_core g2o_stuff)

點雲PCL庫

install pcl庫與工具

sudo apt-get install libpcl_dev
sudo apt-get install pcl_tools

使用 pcl_viewer map.pcd

SLAM Eigen/Sophus/ceres/g2o/PCL開發庫安裝與使用 --持續更新

文章目錄

0. 寫在前面

1. Eigen 矩陣、向量的運算庫

- INSTALL Eigen

- Eigen use-case

- Eigen/Geometry(Eigen的幾何變換模塊進行旋轉變換)

2. Pangolin (OpenGL 顯示)

install Pangolin

3.Sophus李代數庫

install Sophus

Sophus use-case

4-1 ceres非線性優化庫

install ceres庫

use-case of ceres-solver 曲線擬合

4-2 圖優化 g2o庫(General Graph Optimization)

install g2o

use-case : g2o曲線擬合

點雲PCL庫

install pcl庫與工具

end 參考文檔

ORB/BRISK/AKAZE特徵點提取、特徵匹配的性能比較

python os用法以及os.path路徑的聚合－分割－遍歷方法

tensorflow pip 安裝以及nvdia-docker-tensorflow安裝

SLAM Eigen/Sophus/ceres/g2o/PCL開發庫安裝與使用 --持續更新

使用evo工具可視化和評測SLAM算法性能

Mac下配置sublime實現LaTeX

https://yachay.unat.edu.pe/blog/index.php?comment_area=format_blog&comment_component=blog&comment_co

linux以太網驅動總結