沉默杀手
OpenMP加速gmapping
|激光SLAMgmapping|
Word count: 362|Reading time: 1 min

在CMakeLists里做如下配置：

1
2
3
4
5
6
FIND_PACKAGE( OpenMP REQUIRED)
if(OPENMP_FOUND)
    message("OPENMP FOUND")
    set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}")
    set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}")
endif()

注意： OpenMP并不适合需要复杂的线程间同步和互斥的场合，这种情况下花的时间可能更长

gmapping使用OpenMP加速的语句: #pragma omp parallel for

  1. for循环的drawFromMotion之前

    1
    2
    3
    4
    5
    //#pragma omp parallel for
    for (unsigned int i = 0; i < m_particles.size(); i++) {

        m_particles[i].pose =  m_motionModel.drawFromMotion(m_particles[i].pose, relPose,m_odoPose);
    }

  2. invalidateActiveArea之前

    1
    2
    3
    //#pragma omp parallel for
    for (unsigned int i = 0; i < m_particles.size(); i++) {
        m_matcher.invalidateActiveArea();

  3. scanMatch函数开头，实际就是对整个scanMatch并行化

    1
    2
    3
    inline void scanMatch(double * plainReading) {
        #pragma omp parallel for
        for (unsigned int i = 0; i < m_particles.size(); i++) {

  4. updateMap函数中

1
2
3
4
5
#pragma omp parallel for
for(int x=0; x < smap.getMapSizeX(); x++)
{
    for(int y=0; y < smap.getMapSizeY(); y++)
    {
  1. 重采样resample函数

对于保留下来的粒子进行更新，在并行化操作里面m_particles.push_back()会报错，因此需要把push_back()提出来，在外面的的for循环进行

1
2
3
4
5
6
7
8
9
10
11
#pragma omp parallel for
    for(int i = 0; i<tmp_size;i++)
    {
        //对保留下来的粒子数据进行更新
        //每个粒子的权重都设置为相同的值
        temp[i].setWeight(0);
        //为每个粒子更新running_scans
        //增加了一帧激光数据 因此需要更新地图
        m_matcher.registerScan(temp[i].map,temp[i].pose,plainReading);
        //m_matcher.registerScan(temp[i].lowResolutionMap,temp[i].pose,plainReading);
    }

为每个粒子更新地图时，同样可以并行化