文章目录

voxblox 参考文档
- 示例展示
- 扩展文档
- ESDF工作流程
- - 算法
  - TSDF产生ESDF的流程图
  - Raise Wavefront流程图
  - Lower Wavefront流程图
- 安装
- 参与Voxblox项目的Contributing
- 性能
- - 积分性能
  - 与Octomap相比运行时间
- 运行Voxblox
- Voxblox的ROS节点
- - Topics话题服务
  - - 发布的话题
    - 订阅的话题
    - 服务Services
    - 参数
    - - 通用参数
      - TSDF 积分器参数
      - 快速 TSDF 积分器特定参数
      - ESDF 积分器参数
      - ICP细化位姿参数
      - 输入坐标变换参数
      - 输出参数
- Voxblox中的坐标变换
- - A t B C _At_{BC} AtBC
  - C A B C_{AB} CAB
  - T A B T_{AB} TAB
- 使用Voxblox做路径规划
- API调用

voxblox 参考文档

官网参考文档https://voxblox.readthedocs.io/en/latest/index.html

记录一下阅读过程，感觉有用的部分就翻译记录下

示例展示

就是在各种数据集上面运行的效果呗，就是没有些具体怎么设置运行获得的。不看也罢

扩展文档

怎么自己生成参考文档？？

ESDF工作流程

算法

算法描述自己看论文吧，官网居然没有给论文的跳转链接。
Helen Oleynikova, Zachary Taylor, Marius Fehr, Juan Nieto, and Roland Siegwart, “Voxblox: Incremental 3D Euclidean Signed Distance Fields for On-Board MAV Planning”, in IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), 2017.

TSDF产生ESDF的流程图

论文里面的算法描述不如这个图给的直观

Raise Wavefront流程图

Lower Wavefront流程图

安装

Voxblox最好直接使用ROS(ROS Indigo, ROS Kinetic ,ROS Melodic,咱也不知道ROS Noetic能使用不自己试吧)在ubuntu下进行使用。Voxblox 也将在 OS X 上运行，但靠自己的研究吧。

首先安装ROS参考之前自己整理的ROS安装与报错记录
安装一些编译工具，主要ROS版本melodic

sudo apt-get install python-wstool python-catkin-tools ros-melodic-cmake-modules protobuf-compiler autoconf

创建ROS工作空间，初始化

mkdir -p ~/catkin_ws/src
cd ~/catkin_ws
catkin init
catkin config --extend /opt/ros/kinetic
catkin config --cmake-args -DCMAKE_BUILD_TYPE=Release
catkin config --merge-devel

下载voxblox安装依赖

ssh or https 的方式下载都行，但是ssh我自己一直不成功，貌似是github的token验证方式的问题，反正很麻烦。不如直接https下载

cd ~/catkin_ws/src/
git clone https://github.com/ethz-asl/voxblox.git
wstool init . ./voxblox/voxblox_https.rosinstall #这里不好使的可以实时使用绝对路径
wstool update

编译安装

cd ~/catkin_ws/src/
catkin build voxblox_ros

参与Voxblox项目的Contributing

咱太菜了，算了吧。

性能

Voxblox 代码优先考虑可读性和易于扩展而不是性能。它还被设计用于在缺少 GPU 的系统上运行。创建 Voxblox 的主要驱动力之一是创建适合机器人规划需求的体积映射库，因此，与许多 TSDF 库不同，除了靠近表面的区域之外，所有可能的自由空间都被映射。这些设计决策限制了性能，但是仍然可以轻松实现大型环境的高质量实时映射。

积分性能

voxblox实现了三种不同形式的积分策略

Merged 应对大场景，将多个voxels捆绑在一起进行投影。
Fast
Simple 直接遍历的操作，很淳朴简单哈。

与Octomap相比运行时间

Octomap对每个voxel都进行映射，但是voxblox面对大规模场景时候可以(使用Merged策略)对voxel进行捆绑映射，能够在节省运行时间同时精度不产生明显下降。

运行Voxblox

可以在数据集上运行，数据集的启动文件放在项目的voxblox_ros/launch目录下

voxblox 自己的数据集

cow and lady dataset：修改cow_and_lady_dataset.launch文件中数据集的读取路径后，通过以下命令运行
```
roslaunch voxblox_ros cow_and_lady_dataset.launch
```
basement dataset：修改basement_dataset.launch文件中数据集的读取路径后，通过以下命令运行
```
   roslaunch voxblox_ros basement_dataset.launch
```
该数据集无GroundTruth，使用16线的雷达数据，并且使用ICP的方式获取有漂移的位姿估计。

Euroc 数据集
参考voxblox的数据集运行。
KITTI 数据集需要使用转化工具转化为rosbag
参考voxblox的数据集运行。

可以使用rviz工具显示mesh可视化效果。

订阅话题：/voxblox_node/mesh
使用world静态坐标
mesh的可视化更新频率可在launch文件中更改

Voxblox的ROS节点

Topics话题服务

发布的话题

mesh 消息类型:voxblox_msgs::MeshBlock
一个可视化主题，以可在 rviz 中看到的形式显示从 tsdf 生成的网格。设置 update_mesh_every_n_sec 控制更新速率。
surface_pointcloud 消息类型:pcl::PointCloud<pcl::PointXYZRGB>
靠近表面的体素的彩色点云。
tsdf_pointcloud 消息类型:pcl::PointCloud<pcl::PointXYZI>
显示所有已分配体素的点云。
mesh_pointcloud 消息类型:pcl::PointCloud<pcl::PointXYZRGB>
仅当 output_mesh_as_pointcloud 为真时出现，输出包含生成网格顶点的点云。
mesh_pcl 消息类型:pcl_msgs::PolygonMesh
仅当 output_mesh_as_pcl_mesh 为真时出现，输出由 generate_mesh 服务生成的所有网格。
tsdf_slice 消息类型:pcl::PointCloud<pcl::PointXYZI>
输出按照存储的距离值着色的 TSDF 的 2D 水平切片。
esdf_pointcloud 消息类型:pcl::PointCloud<pcl::PointXYZI>
显示所有分配的 ESDF 体素值的点云。仅在使用 esdf_server 服务时出现。
esdf_slice 消息类型:pcl::PointCloud<pcl::PointXYZI>
输出由存储的距离值着色的 ESDF 的 2D 水平切片。仅在使用 esdf_server服务时出现。
occupied_nodes 消息类型:visualization_msgs::MarkerArray
可视化 TSDF 中分配的体素的位置。
tsdf_map_out 消息类型:voxblox_msgs::Layer
发布整个 TSDF 层以更新其他节点（侦听 tsdf_layer_in）。仅当 publish_tsdf_map 设置为 true 时才发布。
esdf_map_out消息类型:voxblox_msgs::Layer
发布整个 ESDF 层以更新其他节点（侦听 esdf_layer_in）。仅当 publish_esdf_map 设置为 true 时才发布。
traversable消息类型:pcl::PointCloud<pcl::PointXYZI>
（仅限 ESDF 服务器）输出地图中被认为可遍历的所有点，由 publish_traversable 和 traversability_radius 参数控制。

订阅的话题

transform消息类型:geometry_msgs::TransformStamped
仅在 use_tf_transforms 为 false 时出现。从世界frame到当前传感器frame的坐标转换。
pointcloud消息类型:sensor_msgs::PointCloud2
要积分的输入点云。
freespace_pointcloud消息类型:sensor_msgs::PointCLoud2
仅当 use_freespace_pointcloud 为真时才会出现。与给定点位于表面上的点云主题不同，freespace_pointcloud 中的点被视为漂浮在空白空间中。这些点可以帮助在地图中生成更完整的自由空间信息。
tsdf_map_in消息类型:voxblox_msgs::Layer
用本Topic中的替换当前 TSDF 层。体素大小和每侧体素应匹配。
esdf_map_in消息类型:voxblox_msgs::Layer
用本Topic中的替换当前 ESDF 层。体素大小和每侧体素应匹配。
icp_transform消息类型:geometry_msgs::TransformStamped
如果启用了 ICP，这是当前世界坐标系和 ICP 坐标系之间的校正变换。

服务Services

tsdf_server 和 esdf_server 节点具有以下服务:

generate_mesh
此服务有一个空的请求和响应。调用此服务将生成一个新的网格。除非 mesh_filename 设置为“”，否则mesh将保存为.ply文件。如果 output_mesh_as_pointcloud 为true，mesh网格发布在 mesh_pointcloud 话题，如果output_mesh_as_pcl_mesh为true，则在发布在mesh_pcl话题。
generate_esdf
此服务有一个空的请求和响应。可用于触发 esdf 地图更新。
save_map
此服务具有 voxblox_msgs::FilePath::Request 和voxblox_msgs::FilePath::Response。服务调用将 tsdf 层保存到 .vxblx 文件中。
load_map
此服务具有 voxblox_msgs::FilePath::Request 和voxblox_msgs::FilePath::Response。服务调用从 .vxblx 文件加载 tsdf 层。
publish_map
此服务有一个空的请求和响应。在 tsdf_map_out 和 esdf_map_out 话题上发布 TSDF 和 ESDF 层。
publish_pointclouds
此服务有一个空的请求和响应。发布 TSDF 和 ESDF 点云和切片。

参数

tsdf_server 和 esdf_server 参数:

通用参数

参数名称	默认值	含义
min_time_between_msgs_sec	0.0	积分消息后在接受新消息之前等待的最短时间。
pointcloud_queue_size	1	用于订阅点云的队列大小。
verbose	true	打印附加的调试和计时信息。
max_block_distance_from_body	3.40282e+38	与最新机器人姿势超过此距离的块将被删除，从而节省内存。

TSDF 积分器参数

参数名称	参数(积分方法)	描述
method	“simple”	最直接的积分。点云中的每个点都有从原点通过它的光线投射。每条光线通过的每个体素都会单独更新。一种非常缓慢而准确的方法。
method	“merged”	在同一个体素中开始和结束的光线被捆绑成一条光线。合并点的属性并添加它们的权重，因此不会丢失任何信息。近似意味着一些体素将接收原本用于相邻体素的更新。这种方法适用于大型体素（10 厘米或更大），并且可以比简单的积分器提高一个数量级。
method	“fast”	尝试更新已被来自同一点云的其他光线更新的体素的光线会提前终止并被丢弃。一种近似方法，旨在以丢弃大量信息为代价提供尽可能快的结果。速度和信息丢失之间的权衡可以通过 `start_voxel_subsampling_factor`和`max_consecutive_ray_collisions` 参数进行调整。对于体素小于 5 厘米的实时应用，该方法是目前唯一可行的积分器。

参数名称	默认值	含义
tsdf_voxel_size	0.2	tsdf 体素的大小
tsdf_voxels_per_side	16	分配块每侧的 TSDF 体素。必须是 2 的幂
voxel_carving_enabled	true	如果为真，则积分光线的整个长度，如果为假，则仅使用截断距离内的区域。
truncation_distance	2*`tsdf_voxel_size`	TSDF 的截断距离
max_ray_length_m	5.0	光线投射到的最大范围
min_ray_length_m	0.1	光线投射开始的点
max_weight	10000.0	分配给体素的权重上限
use_const_weight	false	如果为`true`，沿射线的所有点具有相同的权重
allow_clear	true	如果超过 `max_ray_length_m` 的真实点将被整合到这个距离
use_freespace_pointcloud	false	如果为 `true`，则会出现第二个订阅主题 `freespace_pointcloud`。清除光线从该主题点的截断距离之外投射，以帮助清除自由空间体素

快速 TSDF 积分器特定参数

method设置为fast时使用的参数：

参数名称	默认值	含义
start_voxel_subsampling_factor	2	在将积分点插入子体素之前，每个子体素只允许一个点。这可以被认为是对点云进行二次采样。子体素的边长是体素边长除以 `start_voxel_subsampling_factor`。
max_consecutive_ray_collisions	2	当此积分器投射光线时，它会检测是否有任何其他光线已通过此扫描的当前体素。如果它通过的 `max_consecutive_ray_collisions` 体素超过其他射线连续看到的，则认为没有添加新信息并且投射停止。
max_integration_time_s	3.40282e+38	帧集成的时间预算，如果超过此时间，则提前停止光线投射。用于保证实时性能。
clear_checks_every_n_frames	1	控制指示子体素是否已满或体素是否有光线通过的集合的清除频率。

ESDF 积分器参数

参数名称	默认值	含义
esdf_max_distance_m	2.0	将计算 esdf 的最大距离。
esdf_default_distance_m	2.0	为未知值和值设置的默认距离 >`esdf_max_distance_m`。
clear_sphere_for_planning	false	允许将未知空间设置为在传感器的当前位姿附近释放，并在远离传感器的位置设置未知空间。由以下两个参数控制。用来应对初始化时候环境位置，机器人无法自主探测的情况。
clear_sphere_radius	1.5	未知设置为自由的内球体半径，以米为单位。
occupied_sphere_radius	5.0	将未知设置为占用的外球体半径，以米为单位。

ICP细化位姿参数

参数名称	默认值	含义
enable_icp	false	是否使用 ICP 将所有传入的点云与现有结构对齐。
icp_refine_roll_pitch	true	应用 6 自由度姿势校正为真，应用 4 自由度（x、y、z、偏航）校正为假。
accumulate_icp_corrections	true	是否在所有点云上累积来自 ICP 的变换校正。如果为`false`，则在每个新的点云处重置。
icp_corrected_frame	icp_corrected	将 ICP 校正输出到的 TF 帧。
pose_corrected_frame	pose_corrected	用于输出相对于 `icp_corrected_frame` 的 ICP 校正位姿的 TF 帧。
icp_mini_batch_size	20	每批点匹配校正中使用的点数。
icp_subsample_keep_ratio 0.5	将使用随机子采样来减少用于匹配的点数。
icp_min_match_ratio	0.8	对于要接受的小批量细化，至少点云中的点比率必须落在现有 TSDF 层的截断距离内。
icp_inital_translation_weighting	100.0	粗略衡量系统对提供的初始姿势的置信度。 ICP 中使用的每个点都会为坐标变换贡献 1 个权重信息点。
icp_inital_rotation_weighting	100.0	粗略衡量系统对提供的初始姿势的置信度。 ICP 中使用的每个点都为旋转贡献了 2 个权重信息点。

输入坐标变换参数

参数名称	默认值	含义
use_tf_transforms	true	如果为 `true`，则 ros tf 树将用于获取传感器相对于世界的位姿（将使用 `sensor_frame` 和 `world_frame`）。如果为 `false`，则必须通过变换主题给出姿势。
world_frame	“world”	查找 tf 变换时使用的基本框架。这也是给出大多数输出的框架。
sensor_frame	“”	查找 tf 变换时使用的传感器框架。如果设置为“”，则将使用输入点云消息的框架。
T_B_D		从基座base到将应用的动态系统的静态转换。
invert_T_B_D	false	如果给定的 T_B_D 应该在使用之前反转。
T_B_C		从底座base到将应用的传感器的静态转换。
invert_T_B_C	false	如果给定的 T_B_C 应该在使用之前反转。

输出参数

参数名称	默认值	含义
output_mesh_as_pointcloud	false	如果为 `tru`e，则每当调用 `generate_mesh` 服务时，生成的网格的顶点将作为主题 `mesh_pointcloud` 上的点云输出。
output_mesh_as_pcl_mesh	false	如果为 `true`，每当调用`generate_mesh` 服务时，生成的网格将作为主题 `mesh_pcl` 上的 `pcl::PolygonMesh` 输出。
slice_level	0.5	生成的 tsdf 和 esdf 切片的高度。
color_ptcloud_by_weight	false	点云是否由体素加权着色。
mesh_filename	“”	文件名输出网格将被保存到，如果不应该生成文件则留空。
color_mode	“color”	将用于为网格着色的方法。选项有 “color”, “height”, “normals”,“lambert”,“gray”。
mesh_min_weight	1e-4	将点包含在网格中所需的最小权重。
update_mesh_every_n_sec	0.0	`mesh`主题将被发布到的速率，值为 0 禁用。请注意，这不会触发任何其他网格操作，例如生成层文件。
publish_tsdf_map	false	是否定期在 ROS 话题上发布完整的 TSDF 图。
publish_esdf_map	false	是否定期在 ROS 话题上发布完整的 ESDF 地图。
publish_tsdf_info	false	启用 `tsdf_pointcloud`、`surface_pointcloud` 和`occupied_node`的发布。`publish_pointclouds` 如果为 true，则 tsdf 和 esdf（如果生成）在网格更新时发布为点云。`intensity_colormap`“rainbow”。如果传入的点云是强度（不是 RGB）点云，例如来自激光，这将设置强度将如何映射到颜色。有效选项是 `rainbow`、`inverse_rainbow`、`grayscale`、`inverse_grayscale`、`ironbow`。
intensity_max_value	100.0	用于强度映射的最大值。最小值始终为 0。
publish_traversable	false	是否显示来自 ESDF 服务器的可遍历性点云。
traversability_radius	1.0	一个点被认为是可遍历的最小半径。

Voxblox中的坐标变换

使用wiki中介绍的方式进行坐标变换，平移向量+旋转四元数的方式

结合wiki里面的图片与符号理解一些相关内容。那是每个符号可以有不同的理解方式，这里记录一下我的理解方式。不过我比较细化 T A 2 B T_{A2B} TA2B这样的符号来表示从A坐标系下变换到B坐标系下。

A t B C _At_{BC} AtBC

平移向量

在A坐标系下从B坐标系原点指向C坐标系的矢量
在A坐标系下从B位位置变换到C位置
从C坐标系下变换到B坐标系下

你可以假设一开始在C坐标系下位姿(0,0,0)去理解变换方式。

C A B C_{AB} CAB

旋转四元数

从B坐标系下旋转到A坐标系下
B在A坐标系姿态

A t B C = C A B ∗ B t B C _At_{BC}=C_{AB}*_Bt_{BC} AtBC=CAB∗BtBC

T A B T_{AB} TAB

欧式变换矩阵

B在Az坐标系下的位姿
从B坐标系下变换到A坐标系下

A P = T A B ∗ B P _AP=T_{AB}*_BP AP=TAB∗BP

使用Voxblox做路径规划

完了数目文档给的链接一个都打不开。
反正我也不研究路径规划啥的

路径规划器的算法在Continuous-Time Trajectory Optimization for Online UAV Replanning,Safe Local Exploration for Replanning in Cluttered Unknown Environments for Micro-Aerial Vehicles和 Sparse 3D Topological Graphs for Micro-Aerial Vehicle Planning 中有描述说明。并且即将开源。

同时，使用 voxblox 进行规划的总体思路是运行两个节点：一个用于映射，它摄取点云数据并生成 TSDF 和 ESDF，另一个用于规划，订阅最新的 ESDF覆盖 ROS。

规划器应该有一个 voxblox::EsdfServer 作为成员，并简单地重新映射 esdf_map_out 和 esdf_map_in 话题去匹配。

示例启动launch文件如下所示：

<launch><arg name="robot_name" default="my_robot" /><arg name="voxel_size" default="0.20" /><arg name="voxels_per_side" default="16" /><arg name="world_frame" default="odom" /><group ns="$(arg robot_name)"><node name="voxblox_node" pkg="voxblox_ros" type="esdf_server" output="screen" args="-alsologtostderr" clear_params="true"><remap from="pointcloud" to="great_sensor/my_pointcloud"/><remap from="voxblox_node/esdf_map_out" to="esdf_map" /><param name="tsdf_voxel_size" value="$(arg voxel_size)" /><param name="tsdf_voxels_per_side" value="$(arg voxels_per_side)" /><param name="publish_esdf_map" value="true" /><param name="publish_pointclouds" value="true" /><param name="use_tf_transforms" value="true" /><param name="update_mesh_every_n_sec" value="1.0" /><param name="clear_sphere_for_planning" value="true" /><param name="world_frame" value="$(arg world_frame)" /></node><node name="my_voxblox_planner" pkg="voxblox_planner" type="my_voxblox_planner" output="screen" args="-alsologtostderr"><remap from="odometry" to="great_estimator/odometry" /><remap from="my_voxblox_planner/esdf_map_in" to="esdf_map" /><param name="tsdf_voxel_size" value="$(arg voxel_size)" /><param name="tsdf_voxels_per_side" value="$(arg voxels_per_side)" /><param name="update_mesh_every_n_sec" value="0.0" /><param name="world_frame" value="$(arg world_frame)" /></node></group>
</launch>

还有一些使用 ESDF 碰撞检查编写自己的规划器的框架：

class YourPlannerVoxblox {public:YourPlannerVoxblox(const ros::NodeHandle& nh,const ros::NodeHandle& nh_private);virtual ~YourPlannerVoxblox() {}double getMapDistance(const Eigen::Vector3d& position) const;private:ros::NodeHandle nh_;ros::NodeHandle nh_private_;// Map!voxblox::EsdfServer voxblox_server_;
};

您还可以启用/禁用可遍历点云，如果您将半径设置为机器人的碰撞检查半径，则可以向您显示路径规划器判断的可在点云中遍历的地图部分：

YourPlannerVoxblox::YourPlannerVoxblox(const ros::NodeHandle& nh,const ros::NodeHandle& nh_private): nh_(nh),nh_private_(nh_private),voxblox_server_(nh_, nh_private_) {// Optionally load a map saved with the save_map service call in voxblox.std::string input_filepath;nh_private_.param("voxblox_path", input_filepath, input_filepath);if (!input_filepath.empty()) {if (!voxblox_server_.loadMap(input_filepath)) {ROS_ERROR("Couldn't load ESDF map!");}}double robot_radius = 1.0;voxblox_server_.setTraversabilityRadius(robot_radius);voxblox_server_.publishTraversable();
}

然后要检查碰撞，您可以将地图距离与机器人半径进行比较：

double YourPlannerVoxblox::getMapDistance(const Eigen::Vector3d& position) const {if (!voxblox_server_.getEsdfMapPtr()) {return 0.0;}double distance = 0.0;if (!voxblox_server_.getEsdfMapPtr()->getDistanceAtPosition(position,&distance)) {return 0.0;}return distance;
}

API调用

具体参考API官网文档和相关代码注释