工程复现 -- 占据栅格地图 G-VOM_py占据栅格地图

工程复现 – G-VOM

参考：
1.G-VOM
2.catkin-tools官方文档
3.G-VOM论文地址
4. 本文工程地址GVOM_Nuscenes

一. 效果（主要查看voxel_map和height_map）

二. 文章解析

1. 摘要

提出了一个带有GPU加速的局部三维体素地图框架，用于越野路径规划和导航。我们的方法提供硬和软正面障碍物检测，负面障碍物（坑洞、悬崖）检测、斜率估计和粗糙度估计。10hz更新频率，软障碍物包括灌木、高草。硬障碍物包括树木，石头，建筑物等。

2. 方法

地图存贮
处理流程

• 点云处理：点云和odom创建体素地图，并加入队列。
• 地图处理：把所有buffer中地图融合，产生一张输出地图。

3. 重点梳理

3.1 对每一帧点云的处理操作

• 建立3个一维数组，大小[self.x_size × self.y_size × self.z_size]，分别为index_map，hit_count，total_count。
  • index_map ： 为存在点的位置，建立索引计数，由0起增大；针对没有点的位置，-1-Nm（total_count）
  • hit_count ： 记录点云每一个点所在voxel的被ray穿过的计数变量
  • total_count ： 所有通过该voxel的整个ray的切分数目
• 点云坐标变换 ： lidar2odom
• Trace ray 投影地图： 统计 total_count、hit_count、index_map; 当前坐标系为odom坐标系
• 剔除empty voxel，生成small index map
• 计算metrics (共10维度)
  • 三个方向归一化均值，26邻域局部坐标计算出x_mean, y_mean, z_mean
  • 协方差xx，xy，xz，yy，yz，zz
  • 每一个voxel内最低高度min_height

3.2 对Buffer内地图的处理操作

• 重新组织建立combined_index_map
  对buffer中的多张map进行融合，使用上一轮次index_map再进行融合，因而会出现定位信息不准的话，index数目增加且不更新，除非边界超限map消失。
• 重新统计三个一维数组index_map，hit_count，total_count和metrics
• 计算每个voxel特征值
• 制作各类地图
  • 高程地图 height_map，取一个pillar中最高点
  • 推测高程地图 inferred_height_map
  • 坡度计算
  • 猜测高度计算: 按一定的搜索方式，进行四周高程查找，得到4个方向比较后的最大、最小值，由此得到高差。
  • positive_obstacle_map
  • negative_obstacle_map
  • visability_map
    
    

三. 下载安装工程

3.1 下载工程

git clone https://github.com/unmannedlab/G-VOM.git
1

• ROS环境

3.2 安装工程

详细参考install.md

mkdir -p gvom_ws/src
cd gvom_ws
catkin init
## 创建虚拟环境
conda create -n GVOM python=3.6
conda activate GVOM
## 安装第三方库
pip install catkin_pkg
cd src && git clone https://github.com/unmannedlab/G-VOM.git && cd ..
catkin build
1
2
3
4
5
6
7
8
9
10

注意：安装过程中的问题请参考文章最后的答疑。

3.3 安装依赖

pip install pyyaml numba rospkg matplotlib
1

3.4 数据准备

参考nuscenes2bag, 将nuscenes数据集生成bag

四. 运行

roslaunch gvom gvom_node.launch
1

注意： 若参考答疑Q1 之后仍报错tf2_ros，只需要重新开启一个终端，重复编译和运行步骤即可。

五.答疑

Q1： ImportError: dynamic module does not define module export function (PyInit__tf2)
A1：
参考：
ROS tf使用报错：ImportError: dynamic module does not define module export function
ImportError: dynamic module does not define module export function (PyInit__tf2)

问题在于ros melodic目前只支持Python2，一些主要的功能包如tf2等都是在Python2的环境下运行的，而Ubuntu习惯配置和优先使用Python3，故导致这类问题出现。
ROS melodic 使用python3环境

sudo apt update
sudo apt install python3-catkin-pkg-modules python3-rospkg-modules python3-empy

mkdir -p ~/catkin_ws/src; cd ~/catkin_ws
catkin_make
source devel/setup.bash
wstool init
wstool set -y src/geometry2 --git https://github.com/ros/geometry2 -v 0.6.5
wstool up
rosdep install --from-paths src --ignore-src -y -r

## 编译时候添加参数，不需要根据自己实际虚拟环境路径进行修改
catkin build --cmake-args \
            -DCMAKE_BUILD_TYPE=Release \
            -DPYTHON_EXECUTABLE=/usr/bin/python3 \
            -DPYTHON_INCLUDE_DIR=/usr/include/python3.6m \
            -DPYTHON_LIBRARY=/usr/lib/x86_64-linux-gnu/libpython3.6m.so
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17

Q2： catkin：command not found
A2：

sudo apt-get install python-catkin-tools
1

Q3: Could not find a package configuration file provided by “ros_numpy” with any of the following names:
A3:

sudo apt-get install ros-melodic-ros-numpy
1

Q4: ModuleNotFoundError: No module named ‘yaml’
A4:

pip install pyyaml
1

Q5: ModuleNotFoundError: No module named ‘rospkg’
A5:

pip install rospkg
1

Q6: ModuleNotFoundError: No module named ‘numba’
A6:

pip install llvmlite==0.31.0 -i https://pypi.tuna.tsinghua.edu.cn/simple some-package
pip install numba==0.48.0
1
2

注意：本文环境python3.6，可以参看下面两个三方库版本，版本不对应会类似Failed building wheel for llvmlite的错误。

Q7: 试跑节点过程中， gvom.py: 1122 __transform_pointcloud函数中，ValueError: Specified type ‘f8’ (<class ‘str’>) is not a Numba type
A7:

Q8： Cmake编译c++ 库 ，用python调用。出现Cannot determine link language for target
A8：
参考：Python调用C/C++动态库的方法, 亲测可行！
add_library只支持 .c 或者 .cpp, 使用.hpp不支持。catin build生成的文件在devel/lib下面，推荐直接使用参考文方式只编译库文件

g++ -std=c++11 conversion.c -shared -fPIC -o conversion.so
1

Q9： 编译好的动态库，python调用时候，使用double类型，出错argument 1: <class ‘TypeError’>: Don’t know how to convert parameter 1
A9：
参考：python调用动态库数据类型映射关系

## 使用ctypes.c_double转换
sum = conversionlibrary.add_func(ctypes.c_double(100.0), ctypes.c_double(200.0))
1
2

测试文件参考写法

# python 使用C++ conversion
import ctypes
from ctypes import *
import numpy as np

if __name__ == '__main__':
    translation = np.zeros([3])
    a = c_double(0)
    b = c_double(0)
    libpath = "/home/cui/workspace/gvom_ws/src/G-VOM/common/conversion.so"
    conversionlibrary = cdll.LoadLibrary(libpath)    
    
    # test1 add_func, ok
    # conversionlibrary.add_func.argtypes = [c_int, c_int]
    # conversionlibrary.add_func.restype = c_int
    sum = conversionlibrary.add_func(ctypes.c_double(100.0), ctypes.c_double(200.0))
    
    # test2, 直接调用变量不过
    #print('conversionlibrary: ', ctypes.c_double(conversionlibrary.RADIANS_PER_DEGREE))
    
    # test3, ok
    conversionlibrary.charLLtoUTM(ctypes.c_double(22.9758030), ctypes.c_double(113.5204020), byref(a), byref(b))
    translation[2] = 1000 * 1e-3  # 单位m
    print("a: %.6f, b: %.6f" %(a.value, b.value))
    print("success")
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25

Q10： numba. NotImplementedError: (UniTuple(float64 x 3), (0.0, 0.0, 0.0))
A10：
参考：numba 入门到精通
参数类型原因，将ego_position参数拆分成三个普通值，传入。

Q11：File “…/workspace/gvom_ws/src/G-VOM/scripts/gvom.py”, line 743:
ValueError: Specified type ‘<class ‘numpy.float64’>’ (<class ‘type’>) is not a Numba type
def __calculate_slope(height_map,xy_size,xy_resolution,output_slope_map_x,output_slope_map_y, output_roughness_map):

if( height_map[x,y] > -1000 ):
pts[0,i] = x * xy_resolution
A11： 数据类型问题，numba不支持np.float64, 修改756行 pts的定义为如下：

pts = numba.cuda.local.array((3,9), numba.float64)
1

Q12： 工程中使用voxel_inf_hm = np.core.records.fromarrays([voxel_inf_hm[:,0],voxel_inf_hm[:,1],voxel_inf_hm[:,2]], names=‘x,y,z’) ，完成数据转换，但是Rviz看不到数据，提示No position transformer
A12: 原因在于数据类型转换问题，rostopic echo 查看正确帧与错误帧，发现Rviz不支持显示double类型，只支持float类型。可参照如下进行数据类型转换。

lidar_pc = np.core.records.fromarrays([np.float32(lidar_pc[:,0]),np.float32(lidar_pc[:,1]),np.float32(lidar_pc[:,2])], names='x,y,z')
1