费雪信息场增量建场实际实验

写在前面

上一阶段的工作是基础是在张子潮大佬的费雪信息场这几篇论文的基础上进行的
Beyond Point Clouds: Fisher Information Field for Active Visual Localization, Zichao Zhang, Davide Sacaramuzza 和 Fisher Information Field:an Efficient and Differentiable Map for Perception-ware Planning. Zichao Zhang, Davide Sacaramuzza
然后想用这个费雪信息场（FIF）做实际的增量建场的实验，在实验中遇到了一些问题，现在记录一下。

准备工作

费雪信息场代码来建一个场，计算任意位姿下的费雪信息大小，用于具有感知意识的规划，前端用SVO提供路标点信息，用Voxblox提供地图的可视化显示。用D455相机提供图像信息来跑SVO算法，定位我没有用SVO的定位，用的是t265的定位。定位我也试了VinS的定位，可能是由于相机参数不准确，因为我没标相机，直接用终端输出的相机参数，所以效果都没有t265的好。这一部分存在不合理的地方，因为路标点信息用的是svo，svo输出的pointcloud也是和位姿联合优化来的，因此，所以这部分用t265的位姿信息不太合理，但是voxblox建图的定位消息是t265提供的，比较准确，建出来的场能用，所以就都用t265消息了，如何觉得不合理，把建费雪信息场需要的位姿信息改成SVO提供就可以。

用D455提供图像信息，跑SVO算法

1. 在realsense目录下编写一个launch文件

首先，我们需要D455的图像信息，因此要在realsense目录下编写一个launch文件，启动相机节点。后续还需要t265的定位消息，因此在realsense代码自带的文件rs_d400_and_t265.launch就可以改就可以。将图像的宽度和高度改一下，d455默认的应该是width为848，height为480。然后需要两个图像分别的信息和imu信息，因此在相机2的代码中加入如下语句。

      <arg name="enable_infra"        default="true"/>
      <arg name="enable_infra1"       default="true"/>
      <arg name="enable_infra2"       default="true"/>
      <arg name="enable_gyro"         default="true"/>
      <arg name="enable_accel"        default="true"/>
      <arg name="unite_imu_method"    default="linear_interpolation"/>

2. 在SVO目录下编写一个launch文件

SVO需要提供的话题就是双目相机的两个图像信息和imu信息，因此把这两个信息改成d455输出的信息。

    <param name="cam0_topic" value="/d400/infra1/image_rect_raw" type="str" /> 
    <param name="cam1_topic" value="/d400/infra2/image_rect_raw" type="str" /> 
    <param name="imu_topic" value="/d400/imu" type="str" />

同时，标定文件也要改成d455对应的标定文件，相机参数我是直接在终端输出的相机信息中填的，也没有对相机进行标定。

3. 启动SVO节点

这时，启动两个节点，就能在rviz中看到SVO输出的图像，带有路标点的信息，这时候在终端输出话题/svo/pointcloud就能看到路标点的输出，但是SVO默认的是输出关键帧的路标点，所以输出的比较少，因为后续还要用路标点信息做建场，所以去SVO里改了下判定条件，输出每一帧的路标点信息。

用t265提供定位信息

这一部分其实启动realsense节点，t265就能提供定位信息，但是t265提供的是nav_msgs/odometry, 而FIF建场所需要的是,因此在t265的代码中做了一下修改，输出了的消息。

FIF增量建场

1. 编写建场launch文件

首先，用SVO输出的点云信息提供路标点，用t265输出的定位信息提供位姿，使用t265_odom_frame作为参考坐标系。因此在文件中添加信息

     <param name="features3d_topic_name" value="/svo/pointcloud" />
     <param name="body_pose_topic_name" value="/t265/odom/geometry" />
     <param name="cam_frame0" value="t265_odom_frame" />

建场的kernel类型，我选择的是quad_trace，info的好像在代码中没实现，然后gp_trace的我试了一下建场的速度特别慢，所以在文件中添加

   <node name="act_map" pkg="act_map_ros" type="trace_map_server_node" output="screen" args="-alsologtostderr --v=1" clear_params="true">

同时，world和t265_odom_frame没有建立联系，因此在文件中添加

     <node pkg="tf" type="static_transform_publisher" name="t265_to_world" args="0 0 0 0 0 0 /world /t265_odom_frame  100"/>

2. 编写voxblox文件

把需要的点云信息改成d400提供的点云信息，然后把坐标系改一下

    <arg name="world_frame" default="t265_odom_frame" />
    <remap from="pointcloud" to="/d400/depth/color/points"/>

3. 启动建场节点

这时候启动节点，开始移动建场，就能看到费雪信息场的可视化显示了，也就是论文中提到的每一个体素的best_views显示。

4.保存建的场

这个服务的名称我记不住了，大概就是这个，用 rosservice list 看一下就可以了

rosservice call /act_map/save_map_layers "file_path: "

执行完这个语句之后，就会在你提供的这个文件路径下保存两个文件，如果你想用费雪信息场做规划的话需要用到这两个文件。

遇到的各种问题

1. 找不到d455的陀螺仪

这个问题很奇怪，给几个电脑都装过realsense，都没有遇到过找不到陀螺仪的情况，具体表现就是，终端输出

sensor gyro_3d not found

然后用realsense-viewer看，点陀螺仪也看不到，最终在一个讨论中看到了解决方案，文章提供了两种方法，方法1：把系统 SECURITY BOOT关闭，我用的是这个方法，可行。
方法2：换内核，具体的方法见上面文章，我没尝试。

2. 各种进程终止

这个大概率是launch或者yaml文件有问题，比如说图片的分辨率和算法的分辨率对应不上，比如d455提供的图片480848，但svo算法里默认参数是480752，那么就需要改一下。

3. 建不出来场

这块可能的问题有很多，我最开始的时候也是建不出来，最开始我输出SVO的pointcloud，发现它输出的特别慢，可能几秒才输出一次，看SVO代码发现它那个是输出关键帧的点云信息，然后我把它改成了输出每一帧的点云信息，还有可能是体素大小过小，FIF每个体素的尺寸我设置为0.2，当过小的时候，建的就非常慢，然后SVO的特征点数量也让我改小了，这样也能提升建场的速度。