voxblox建图教程

之前一直在摸索港科大的vins + fiest做定位和建图，但是实际上由于标定等多种原因，vins mono和vins fusion定位效果都不太好，然后orbslam系列对于快速运动也经常会跑丢。最近我在实验使用realsense的t265做定位，然后用d455做深度估计，然后使用voxblox来进行见图，效果好得一批，这篇博客对这个进行记录。

1. 定位和深度估计

将t265和d455同时连接到计算机，然后启用同时工作的节点

roslaunch realsense2_camera rs_d400_and_t265.launch

那么就可以获取下面的两个数据，一个是点云：/d400/depth/color/points, 另外一个就是t265的坐标系：t265_odom_frame

2.利用voxblox来进行建图，之前的blog已经说了这个库的安装方式，添加一个launch节点

<launch>
  <arg name="robot_name" default="my_robot" />
  <arg name="voxel_size" default="0.10" />
  <arg name="voxels_per_side" default="16" />
  <arg name="world_frame" default="t265_odom_frame" />
  <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="/d400/depth/color/points"/>
      <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="0.5" />
      <param name="clear_sphere_for_planning" value="true" />
      <param name="world_frame" value="$(arg world_frame)" />
    </node>
  </group>
</launch>

运行之后就可以获得地图

3. rviz的显示

fixed_frame需要设置为t265_odom_frame，然后在rviz中添加占据地图就可以显示了