OctoMap开启多线程

1. 前言

OctoMap是一个对三维空间进行体素化展示的工具包，其不仅以概率的形式刻画每个三维空间体素是障碍物的几率(这对机器人导航和运动规划非常重要)，同时优化了数据结构，极大的减少了三维体素地图对内存的消耗．

库链接: https://github.com/OctoMap/octomap
论文链接: http://www.arminhornung.de/Research/pub/hornung13auro.pdf

2. 单线程版本

2.1 库安装

直接使用ros发行的版本:

sudo apt-get install ros-kinetic-octomap

或者源码安装:

git clone https://github.com/OctoMap/octomap.git
cd octomap
mkdir build & cd build
cmake ..
make
sudo make install

2.2 测试demo

#include <ros/ros.h>
#include <sstream>

#include <octomap/octomap.h>
#include <octomap/math/Utils.h>
#include <omp.h>

using namespace std;
using namespace octomap;

int main (int argc, char** argv)
{
    ros::init(argc, argv, "generate_octomap_node");
    ros::NodeHandle nh, pnh("~");

    OcTree tree (0.05);  

    point3d origin (0.01f, 0.01f, 0.02f);
    point3d point_on_surface (4.01f, 0.01f, 0.01f);

    cout << "generating spherical scan at " << origin << " ..." << endl;

    Pointcloud cloud;

    for (int i=-100; i<101; i++) {
        for (int j=-100; j<101; j++) {
        point3d rotated = point_on_surface;
        rotated.rotate_IP(0, DEG2RAD(i*0.5), DEG2RAD(j*0.5));
        cloud.push_back(rotated);
        }
    }  

    // insert in global coordinates:
    float insert_start = omp_get_wtime();
    for (int i = 0; i < 10; i++)
        tree.insertPointCloud(cloud, origin);
    float insert_end = omp_get_wtime();
    cout << "insert cloud cost time " << (insert_end - insert_start) << " [sec]" << endl;

    cout << "done." << endl;
    cout << "writing to spherical_scan.bt..." << endl;
    tree.writeBinary("spherical_scan.bt");

    return 0;
}

结果:

generating spherical scan at (0.01 0.01 0.02) ...
insert cloud cost time 14.3125 [sec]
done.
writing to spherical_scan.bt...
Writing 66907 nodes to output stream...

cpu使用率:

结论: 从cpu使用率来看，只使用了单线程进行点云的插入，总耗时14.3125

3. 多线程版本

3.1 多线程版本库安装

如果先前安装的是ros的发行版本，该版本不支持多线程，因此需要卸载掉，否则使用catkin_make编译时会优先找到该非多线程版本:

sudo apt-get remove ros-kinetic-octomap

源码安装多线程版本:

cd octomap
mkdir build & cd build
cmake -DOCTOMAP_OMP=true ..
make
sudo make install

3.2 多线程版本测试结果

测试代码与单线程版本一致，测试结果如下:

generating spherical scan at (0.01 0.01 0.02) ...
insert cloud cost time 11.375 [sec]
done.
writing to spherical_scan.bt...
Writing 66907 nodes to output stream...

cpu使用率:

结论: 从cpu使用率可以看出插入点云的过程使用了多线程，总耗时11.375秒，只比之前单线程版本提升几秒，并没有按cpu使用率那样成比例减少运行时间? 正在与原作者交流原因…

4. 关于octovis问题

由于卸载了ros-kinetic-octomap，对其有依赖的ros-kinetic-octovis也被卸载了，因此要想通过octovis查看测试demo生成的octomap.bt二进制文件，只有通过源码安装octovis:

cd octomap/octovis
mkdir build & cd build
cmake ..
make
sudo make install

查看生成文件:

octovis spherical_scan.bt

出现如下qt5版本过高问题:

*** Error in `octovis': realloc(): invalid pointer: 0x00007ff6795f2820 ***
======= Backtrace: =========
/lib/x86_64-linux-gnu/libc.so.6(+0x777f5)[0x7ff67796a7f5]
/lib/x86_64-linux-gnu/libc.so.6(+0x85df0)[0x7ff677978df0]
/lib/x86_64-linux-gnu/libc.so.6(realloc+0x24f)[0x7ff67797795f]
/usr/lib/x86_64-linux-gnu/libQt5Core.so.5(_ZN9QListData7reallocEi+0x1f)[0x7ff67239a9cf]
/usr/lib/x86_64-linux-gnu/libQt5Core.so.5(_ZN9QListData6appendEi+0x81)[0x7ff67239aaa1]
/usr/lib/x86_64-linux-gnu/libQt5Core.so.5(+0x1d6d78)[0x7ff672466d78]
/usr/lib/x86_64-linux-gnu/libQt5Core.so.5(_Z21qRegisterResourceDataiPKhS0_S0_+0x2e6)[0x7ff672462b16]
/usr/lib/x86_64-linux-gnu/libQt5Core.so.5(+0x7bcc3)[0x7ff67230bcc3]
/lib64/ld-linux-x86-64.so.2(+0x106fa)[0x7ff67a5fa6fa]
/lib64/ld-linux-x86-64.so.2(+0x1080b)[0x7ff67a5fa80b]
/lib64/ld-linux-x86-64.so.2(+0xc6a)[0x7ff67a5eac6a]
======= Memory map: ========

安装qt4:

sudo apt-get install libqglviewer-dev-qt4

然后查看生成文件:

octovis spherical_scan.bt

结果: