工程复现 – 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

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

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

3.3 安装依赖

pip install pyyaml numba rospkg matplotlib

3.4 数据准备

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

四. 运行

roslaunch gvom gvom_node.launch

注意：若参考答疑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-empymkdir -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

Q2： catkin：command not found
A2：

sudo apt-get install python-catkin-tools

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

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

pip install pyyaml

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

pip install rospkg

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

注意：本文环境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

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))

测试文件参考写法

# python 使用C++ conversion
import ctypes
from ctypes import *
import numpy as npif __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_intsum = conversionlibrary.add_func(ctypes.c_double(100.0), ctypes.c_double(200.0))# test2, 直接调用变量不过#print('conversionlibrary: ', ctypes.c_double(conversionlibrary.RADIANS_PER_DEGREE))# test3, okconversionlibrary.charLLtoUTM(ctypes.c_double(22.9758030), ctypes.c_double(113.5204020), byref(a), byref(b))translation[2] = 1000 * 1e-3  # 单位mprint("a: %.6f, b: %.6f" %(a.value, b.value))print("success")

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)

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')

工程复现 -- 占据栅格地图 G-VOM相关推荐

占据栅格地图（Occupancy Grid Map）
转载自:https://zhuanlan.zhihu.com/p/21738718 写在前面:这篇文章是Coursera上的课程(Robotics: Estimation and Learning), ...
SLAM中有关占据栅格地图的的表示方法和利用激光传感器构建占据栅格地图的方法
SLAM中有关占据栅格地图的的表示方法和利用激光传感器构建占据栅格地图的方法: https://zhuanlan.zhihu.com/p/21738718
9 概率机器人 Probabilistic Robotics 二值贝叶斯滤波占据栅格地图 occupancy grid mapping
文章目录 1 前言 2 二值贝叶斯滤波 2.1 理论基础 2.2 算法流程 2.3 重要公式推导 3 实例:占据栅格地图(occupancy grid mapping) 4 参考文献 1 前言如果通 ...
占据栅格地图构建（Occupancy Grid Map）
移动机器人地图构建问题,主要以gmapping为例,讲解了地图构建的整个流程.看过前面文章的小伙伴肯定都知道,gmapping算法把SLAM问题分解成两个部分,定位问题和地图构建问题.而gmappin ...
机器人的地图——占据栅格地图
1. 机器人地图的分类地图有很多种表示方式,例如,用经纬度标识地方的世界地图,城市的地铁图,校园指引图. 第一种我们称为尺度地图(Metric Map),每一个地点都可以用坐标来表示,比如北京在东经 ...
机器人学习--栅格地图（occupancy grid map）构建（部分代码解析）
转自: 占据栅格地图构建(Occupancy Grid Map) - 知乎占据栅格地图构建(Occupancy Grid Map)_「小白学移动机器人」一个专注分享移动机器人相关知识的公众号!-CS ...
机器人学习--从路径规划侧面看栅格地图
移动机器人路径规划--很多种算法,如果有计算机学科基础的话,可以参考数据结构中的深度优先算法或者广度优先算法 https://zhuanlan.zhihu.com/p/51112799 https:/ ...
SLAM 建立局部二维栅格地图的一种方法
转载请说明出处:http://blog.csdn.net/zhubaohua_bupt/article/details/72923373 二维占据栅格地图(栅格地图)广泛应用于移动机器人导航领域中,比 ...
a*算法路径规划matlab_【优化求解】基于栅格地图——遗传算法的机器人最优路径规划...
采用栅格对机器人的工作空间进行划分,再利用优化算法对机器人路径优化,是采用智能算法求最优路径的一个经典问题.目前,采用蚁群算法在栅格地图上进行路径优化取得比较好的效果,而利用遗传算法在栅格地图上进行路 ...

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