SLAM14讲ch10代码实践

一、g2o solver位姿优化

       其pose_graph_g2o_SE3.cpp代码部分可在github下在完整版，这里具体讲在运行过程中可能出现的问题。

        1.在build里面make编译时会出现一大片报红的错误，具体如下图所示。    

 出现这一问题关键在于下载的源代码中使用的是c++11标准，在编译时就会出现一大片错误，具体解决方法是将c++11改为c++14标准即可，具体改为：

set(CMAKE_CXX_FLAGS "-std=c++14 -O3")

    2.改完标准，再次进行make编译又出现如下图错误：

    

由错误我们可以看到是缺乏fmt文件导致，具体做法分为两种。一是还没有下载fmt文件的可以在CSDN上寻找fmt安装具体方法；二是你已经安装了fmt文件 但是仍然报上述错误，原因在于CMaklist.txt文件中在target_link_libraries后面没有加fmt，加上再次编译就可以解决上述错误。这部分具体加好后如下所示：

target_link_libraries(pose_graph_g2o_SE3
        g2o_core g2o_stuff g2o_types_slam3d ${CHOLMOD_LIBRARIES}
        fmt)

       整个CMakeList.txt代码如下：

cmake_minimum_required(VERSION 2.8)
project(pose_graph)

set(CMAKE_BUILD_TYPE "Release")
# set(CMAKE_CXX_FLAGS "-std=c++11 -O2")
set(CMAKE_CXX_FLAGS "-std=c++14 -O3")
list(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake_modules)

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

# sophus 
find_package(Sophus REQUIRED)
include_directories(${Sophus_INCLUDE_DIRS})

# g2o 
find_package(G2O REQUIRED)
include_directories(${G2O_INCLUDE_DIRS})

add_executable(pose_graph_g2o_SE3 pose_graph_g2o_SE3.cpp)
target_link_libraries(pose_graph_g2o_SE3
        g2o_core g2o_stuff g2o_types_slam3d ${CHOLMOD_LIBRARIES}
        fmt)

add_executable(pose_graph_g2o_lie pose_graph_g2o_lie_algebra.cpp)
target_link_libraries(pose_graph_g2o_lie
        g2o_core g2o_stuff
        ${CHOLMOD_LIBRARIES}
        ${Sophus_LIBRARIES}
        fmt)

        3.出现下图所示则表明编译成功

      在运行 下面代码时出现file sphere.g2o does not exist.的错误，原因在于没有将  sphere.g2o文件放入新建的 build文件下，因为你执行文件时是在build文件下执行，如果没有将其放入则会出现file sphere.g2o does not exist.的错误，故正确编译好后的build下文件应该如下图所示

./pose_graph_g2o_SE3 sphere.g2o

       4.原始没有优化的图为（使用命令g2o_viewer sphere.g2o就是未优化图）

     使用命令g2o_viewer result.g2o 可得优化后的图，如下所示

二、李代数上的位姿图优化 

   具体操作过程及可能出现的问题见上面一所示，其完整CMakeList.txt文件也如上。这里主要讲讲pose_graph_g2o_lie_algebra.cpp单独出现的问题。在完成编译及执行代码时出现下面所示错误：

        Sophus ensure failed in function 'void Sophus::SO3Base<Derived>::normalize() [with Derived = Sophus::SO3<double>]', file '/usr/local/include/sophus/so3.hpp', line 320.
Quaternion (   0.706662 4.32706e-17    0.707551 -4.3325e-17) should not be close to zero!
已放弃 (核心已转储)

 ./pose_graph_g2o_lie sphere.g2o 

        出现上述错误原因在于在顶点类VertexSE3LieAlgebra的读入函数virtual bool read(istream& is)缺少返回值。具体解决方法是在pose_graph_g2o_lie_algebra.cpp代码的VertexSE3LieAlgebra函数最后加上return true；再次编译即可解决。具体加法如下图代码所示：

class VertexSE3LieAlgebra : public g2o::BaseVertex<6, SE3d> {
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    virtual bool read(istream &is) override {
        double data[7];
        for (int i = 0; i < 7; i++)
            is >> data[i];
        setEstimate(SE3d(
            Quaterniond(data[6], data[3], data[4], data[5]),
            Vector3d(data[0], data[1], data[2])
        ));
        return true;
    }

再次执行可得result_lie.g2o的可视化图像：

         以上便是我在运行ch10相关代码时所遇到的一些问题及解决方法，希望对你有帮助！！！！！