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 求解非线性优化的三个步骤

1. 构建cost function代价函数，也就是寻优的目标式
2. 通过代价函数构建待求解的优化问题 ceres::Problem problem.AddResidualBlock
3. 配置求解器参数，并求解问题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

end 参考文档

• 《视觉SLAM十四讲》之开源库的安装　高翔
• Ceres入门
• 深入理解图优化与g2o：图优化篇