slam第六讲_g2o

编译安装g2o:

1、下载代码包：

https://github.com/RainerKuemmerle/g2o

2、安装依赖项

znkz@znkz:~$ sudo apt-get install libqt4-dev qt4-qmake libqglviewer-dev libsuitesparse-dev libcxsparse3.1.2 libcholmod-dev

E: 无法定位软件包 libcholmod-dev

最后一项需要Tab键补全：

znkz@znkz:~$ sudo apt-get install libqt4-dev qt4-qmake libqglviewer-dev libsuitesparse-dev libcxsparse3.1.2 libcholmod2.1.2

3、编译：

CMake Warning at cmake_modules/FindQGLViewer.cmake:1 (find_package):
  By not providing "FindQt5.cmake" in CMAKE_MODULE_PATH this project has
  asked CMake to find a package configuration file provided by "Qt5", but
  CMake did not find one.

  Could not find a package configuration file provided by "Qt5" with any of
  the following names:

    Qt5Config.cmake
    qt5-config.cmake

  Add the installation prefix of "Qt5" to CMAKE_PREFIX_PATH or set "Qt5_DIR"
  to a directory containing one of the above files.  If "Qt5" provides a
  separate development package or SDK, be sure it has been installed.
Call Stack (most recent call first):
  CMakeLists.txt:174 (find_package)


Qt5 not found. Install it and set Qt5_DIR accordingly
-- Found QGLVIEWER: /usr/include/QGLViewer  
-- Compiling g2o apps
-- Compiling g2o examples
-- Compiling with GCC
-- Found Eigen3: /usr/include/eigen3 (Required is at least version "2.91.0") 
-- Configuring done
-- Generating done
-- Build files have been written to: /home/znkz/下载/g2o-master/build

https://blog.csdn.net/lch_vison/article/details/80899039

一般情况下，ubuntu16.04可能默认qt4，但随着qt5更新，部分程序要求qt5的支持，卸载qt4再安装qt5会比较麻烦，所以可能需要同时安装qt4和qt5，两个版本的任意切换显得很有必要。

        首先，查看系统默认打qt版本，命令行输入

znkz@znkz:~/下载/g2o-master/build$ qmake -v
QMake version 2.01a
Using Qt version 4.8.6 in /usr/lib/x86_64-linux-gnu

将依赖项中qt4-qmake换成qt5-qmake

znkz@znkz:/usr/lib/x86_64-linux-gnu/qt-default/qtchooser$ sudo apt-get install libqt4-dev qt5-qmake libqglviewer-dev libsuitesparse-dev libcxsparse3.1.2 libcholmod2.1.2

再检查版本：

znkz@znkz:/usr/lib/x86_64-linux-gnu/qt-default/qtchooser$ qmake -v
QMake version 2.01a
Using Qt version 4.8.6 in /usr/lib/x86_64-linux-gnu

需要在文件路径   /usr/lib/x86_64-linux-gnu/qt-default/qtchooser的default.conf文件中修改默认版本

原来：

/usr/lib/x86_64-linux-gnu/qt4/bin
/usr/lib/x86_64-linux-gnu

修改为：

/usr/lib/x86_64-linux-gnu/qt5/bin
/usr/lib/x86_64-linux-gnu

再查看版本：

znkz@znkz:/usr/lib/x86_64-linux-gnu/qt-default/qtchooser$ qmake -v
QMake version 3.0
Using Qt version 5.2.1 in /usr/lib/x86_64-linux-gnu

但cmake仍找不到qt5:

-- Compiling on Unix
-- Found CHOLMOD and its dependencies
-- Compiling with OpenGL support
CMake Warning at cmake_modules/FindQGLViewer.cmake:1 (find_package):
  By not providing "FindQt5.cmake" in CMAKE_MODULE_PATH this project has
  asked CMake to find a package configuration file provided by "Qt5", but
  CMake did not find one.

  Could not find a package configuration file provided by "Qt5" with any of
  the following names:

    Qt5Config.cmake
    qt5-config.cmake

  Add the installation prefix of "Qt5" to CMAKE_PREFIX_PATH or set "Qt5_DIR"
  to a directory containing one of the above files.  If "Qt5" provides a
  separate development package or SDK, be sure it has been installed.
Call Stack (most recent call first):
  CMakeLists.txt:174 (find_package)


Qt5 not found. Install it and set Qt5_DIR accordingly
-- Compiling g2o apps
-- Compiling g2o examples
-- Compiling with GCC
-- Configuring done
-- Generating done
-- Build files have been written to: /home/znkz/下载/g2o-master/build

找不到qt5-config.cmake文件

尝试two：官网下载：

qt-opensource-linux-x64-5.8.0.run 安装于/home/znkz/qt5.8文件夹中

修改default.conf:

/home/znkz/qt5.8/5.8/gcc_64/bin
/home/znkz/qt5.8

 

znkz@znkz:~/下载$ qmake -v
QMake version 3.1
Using Qt version 5.8.0 in /home/znkz/qt5.8/5.8/gcc_64/lib

并且在~/.bashrc中添加路径：

PATH=${PATH}:/home/znkz/qt5.8/5.8/gcc_64/bin:/home/znkz/qt5.8/Tools/QtCreator/bin

再次编译g2o:

znkz@znkz:~/下载/g2o-master/build$ cmake ..
-- Compiling on Unix
-- Found CHOLMOD and its dependencies
-- Compiling with OpenGL support
-- Compiling g2o apps
-- Compiling g2o examples
-- Compiling with GCC
CMake Error at g2o/apps/g2o_viewer/CMakeLists.txt:7 (string):
  string does not recognize sub-command APPEND


-- Generating position indpendent code for slam2d because Qt5 was built with -reduce-relocations
-- Configuring incomplete, errors occurred!
See also "/home/znkz/下载/g2o-master/build/CMakeFiles/CMakeOutput.log".

如果编译的时候出现类似上面的问题，是由于cmake版本偏低的原因，所以安装cmake的时候最好安装官方推荐版本或者安装g2o历史版本

 

程序编译问题：

/home/znkz/slam/ch6/g2o_curve_fitting/main.cpp:82:52: error: cannot allocate an object of abstract type ‘CurveFittingVertex’
     CurveFittingVertex* v = new CurveFittingVertex();

原因：来自基类的基函数在派生类中没有继承并实现；检查派生类中函数，发现函数名书写错误；

/home/znkz/slam/ch6/g2o_curve_fitting/main.cpp:17:5: error: ‘static void* CurveFittingVertex::operator new(std::size_t)’ is private

因为派生类中指明为public，c++中class默认为privete

//main.cpp
#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 <eigen3/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() {}
    
    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 {}
    double _x;
};


int main(int argc, char** argv)
{
    double a = 1.0, b = 2.0, c = 1.0;
    int N = 100;
    double w_sigma = 1.0;
    cv::RNG rng;
    double abc[3] = {0, 0, 0};
    
    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;
	
    }
    
    typedef g2o::BlockSolver< g2o::BlockSolverTraits<3,1>> Block;
    Block::LinearSolverType* linearSolver = new g2o::LinearSolverDense<Block::PoseMatrixType>();
    Block* solver_ptr = new Block ( linearSolver);
    g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg( 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;
}

project(g2o_curve_fitting)


cmake_minimum_required(VERSION 3.2)
set ( CMAKE_CXX_FLAGS "-std=c++11")
#opencv
find_package(OpenCV REQUIRED)
include_directories( ${Opencv_INCLUDE_DIRS} )

#EIGEN
include_directories(" /usr/include/eigen3/")

#g2o
include_directories(${G2O_INCLUDE_DIRS})

add_executable( g2o_curve_fitting main.cpp)
target_link_libraries(g2o_curve_fitting ${OpenCV_LIBS} g2o_core g2o_stuff )
install(TARGETS g2o_curve_fitting RUNTIME DESTINATION bin)

generating data: 
0 2.71828
0.01 2.93161
0.02 2.12942
0.03 2.46037
0.04 4.18814
0.05 2.73368
0.06 2.42751
0.07 3.44729
0.08 3.72543
0.09 2.1358
0.1 4.12333
0.11 3.38199
0.12 4.81164
0.13 1.62582
0.14 1.76862
0.15 3.21555
0.16 3.0922
0.17 5.82752
0.18 4.29855
0.19 2.74081
0.2 5.75724
0.21 3.53729
0.22 1.95514
0.23 2.99195
0.24 3.28739
0.25 4.70749
0.26 6.24365
0.27 5.81645
0.28 4.88402
0.29 4.75991
0.3 7.25246
0.31 5.92933
0.32 7.00306
0.33 5.22286
0.34 5.16179
0.35 7.26191
0.36 6.40545
0.37 6.25549
0.38 6.56094
0.39 6.53523
0.4 8.14891
0.41 7.77616
0.42 7.40141
0.43 8.75638
0.44 7.20606
0.45 7.57795
0.46 8.21564
0.47 9.84032
0.48 6.96725
0.49 9.90619
0.5 9.27125
0.51 9.87567
0.52 10.3412
0.53 9.55315
0.54 11.3635
0.55 10.8815
0.56 13.0648
0.57 11.4756
0.58 11.337
0.59 13.2393
0.6 13.5299
0.61 14.0441
0.62 13.31
0.63 13.672
0.64 14.8504
0.65 14.2599
0.66 14.7724
0.67 17.4339
0.68 17.4632
0.69 17.7598
0.7 16.8223
0.71 19.9468
0.72 20.5446
0.73 21.3767
0.74 20.1435
0.75 20.3088
0.76 23.2543
0.77 23.4349
0.78 22.8706
0.79 24.094
0.8 25.4183
0.81 25.5237
0.82 27.9738
0.83 28.5861
0.84 29.5703
0.85 29.6744
0.86 32.667
0.87 34.2698
0.88 33.5124
0.89 36.1479
0.9 39.2485
0.91 40.988
0.92 41.5716
0.93 41.3686
0.94 44.285
0.95 42.8312
0.96 47.7941
0.97 48.5931
0.98 51.8487
0.99 51.0258
start optimization
iteration= 0	 chi2= 34160.645534	 time= 0.0057172	 cumTime= 0.0057172	 edges= 100	 schur= 0	 lambda= 699.050482	 levenbergIter= 7
iteration= 1	 chi2= 19064.548305	 time= 0.00396034	 cumTime= 0.00967754	 edges= 100	 schur= 0	 lambda= 233.016827	 levenbergIter= 1
iteration= 2	 chi2= 17463.228578	 time= 0.00313083	 cumTime= 0.0128084	 edges= 100	 schur= 0	 lambda= 77.672276	 levenbergIter= 1
iteration= 3	 chi2= 17445.410049	 time= 0.00220924	 cumTime= 0.0150176	 edges= 100	 schur= 0	 lambda= 25.890759	 levenbergIter= 1
iteration= 4	 chi2= 17445.404907	 time= 0.00242345	 cumTime= 0.0174411	 edges= 100	 schur= 0	 lambda= 17.260506	 levenbergIter= 1
iteration= 5	 chi2= 17445.404907	 time= 0.00201721	 cumTime= 0.0194583	 edges= 100	 schur= 0	 lambda= 11.507004	 levenbergIter= 1
iteration= 6	 chi2= 17445.404907	 time= 0.00294174	 cumTime= 0.0224	 edges= 100	 schur= 0	 lambda= 3088887817.306430	 levenbergIter= 7
solve time cost = 0.0239291 seconds. 
estimated model:       0       0 2.69483
*** 正常退出 ***