D455+VINS-Fusion+surfelmapping dense mapping (2)

Continuing from the previous article, start dense mapping

1. Install surfel

Source code address: densesurfelmapping-VINS-supported

Be careful to choose the VINS-supported version

cd ~/catkin_ws/src
git clone https://github.com/HKUST-Aerial-Robotics/DenseSurfelMapping/tree/VINS-supported
cd ..
catkin_make

Then we configure the adaptation parameters of the D455 camera, enter densesurfelmapping-VINS-supported, create a new vins_realsense.launch file, and fill in the following content:

<launch>
    <node pkg="surfel_fusion" type="surfel_fusion" name="surfel_fusion" clear_params="true" output="screen">

        <!-- camera parameter -->
        <param name="cam_width"  value="640" />
        <param name="cam_height" value="480" />

        <!--input grey_image info-->
        <param name="cam_fx" value="392.2211560299197" />
        <param name="cam_fy" value="392.77289841050305" />
        <param name="cam_cx" value="324.0902022905199" />
        <param name="cam_cy" value="243.14325635089884" />

        <!-- fusion parameter, all in meter -->
        <param name="fuse_far_distence"  value="5.0" />
        <param name="fuse_near_distence" value="0.01" />

        <!-- for deform the map -->
        <param name="drift_free_poses" value="100" />

        <!-- for data save -->
<!--        <remap from="~image" to="/camera/infra1/image_rect_raw" />-->
<!--        <remap from="~depth" to="/camera/depth/image_rect_raw" />-->
        <remap from="~image" to="/camera/color/image_raw" />
        <remap from="~depth" to="/camera/depth/image_rect_raw" />
        <remap from="~loop_path" to="/loop_fusion/pose_graph_path" />
        <remap from="~extrinsic_pose" to="/vins_estimator/extrinsic" />
        <param name="save_name" value="$(find surfel_fusion)/../../../output_map"/>

    </node>

    <node type="rviz" name="rviz" pkg="rviz" args="-d $(find surfel_fusion)/launch/surfel.rviz" />

</launch>

It can be seen that surfel dense mapping requires the following topics of the camera:

/camera/color/image_raw: rgb information

/camera/depth/image_rect_raw: Depth information

/loop_fusion/pose_graph_path: loopback information

/vins_estimator/extrinsic: pose information

Start the camera:

roslaunch realsense2_camera stereo-imu.launch

Turn on the tracking node:

rosrun vins vins_node ~/catkin_ws/src/VINS-Fusion/config/euroc/euroc_stereo_imu_config.yaml 

Open the closed loop:

rosrun loop_fusion loop_fusion_node ~/catkin_ws/src/VINS-Fusion/config/euroc/euroc_stereo_imu_config.yaml 

Start Surfel Fusion

roslaunch surfel_fusion vins_realsense.launch

The actual measured results are as follows: