3D reconstruction scanning using d435+t265 is messy

[mhaboali]

Hi and thanks for sharing this great project,

I'm trying to create a launch file similar to opensource_tracking that's offered as an example. This launch file should use both d435 + t265 cameras, when I ran the system I got very messy results on /rtabmap/scan_map as shown below

And here's the TF

And here's rqt_graph

And here's my launch files _demortab.launch

<launch>
    <arg name="offline"          default="false"/>
    <include unless="$(arg offline)" 
        file="$(find realsense2_camera)/launch/demo_265_d435.launch">  
        <arg name="publish_odom_tf"          value="true"/>   
        <arg name="view_rviz"                value="false"/>

    </include>

    <node pkg="imu_filter_madgwick" type="imu_filter_node" name="ImuFilter">
        <param name="use_mag" type="bool" value="false" />
        <param name="_publish_tf" type="bool" value="false" />
        <param name="_world_frame" type="string" value="enu" />
        <remap from="/imu/data_raw" to="/camera/imu"/>
    </node>

    <include file="$(find rtabmap_ros)/launch/rtabmap.launch">
        <arg name="args" value="--delete_db_on_start"/>
        <arg name="frame_id"                value="d435_link"/>
        <arg name="rgb_topic" value="/d435/color/image_raw"/>
        <arg name="depth_topic" value="/d435/aligned_depth_to_color/image_raw"/>
        <arg name="camera_info_topic" value="/d435/color/camera_info"/>
        <arg name="depth_camera_info_topic" value="/d435/aligned_depth_to_color/camera_info"/>
        <arg name="rtabmapviz" value="false"/>
        <arg name="rviz" value="true"/>
    </include>

    <include file="$(find robot_localization)/launch/ukf_template.launch"/>
    <param name="/ukf_se/frequency" value="300"/>
    <param name="/ukf_se/base_link_frame" value="t265_pose_frame"/>
    <param name="/ukf_se/odom0" value="/t265/odom/sample"/>
    <rosparam param="/ukf_se/odom0_config">[true,true,true,
                                            true,true,true,
                                            true,true,true,
                                            true,true,true,
                                            true,true,true]
    </rosparam>
    <param name="/ukf_se/odom0_relative" value="true"/>
    <param name="/ukf_se/odom0_pose_rejection_threshold" value="10000000"/>
    <param name="/ukf_se/odom0_twist_rejection_threshold" value="10000000"/>

    <param name="/ukf_se/imu0" value="/imu/data"/>
    <rosparam param="/ukf_se/imu0_config">[false, false, false,
                                           true,  true,  true,
                                           true,  true,  true,
                                           true,  true,  true,
                                           true,  true,  true]
    </rosparam>
    <param name="/ukf_se/imu0_differential" value="true"/>
    <param name="/ukf_se/imu0_relative" value="false"/>
    <param name="/ukf_se/use_control" value="false"/>
    <!-- <param name="/ukf_se/odom0_config" value="{true,true,true,}"/> -->
<node pkg="tf2_ros" type="static_transform_publisher" name="tf2_odom_t265_broadcaster" args="0.0 0.00 0 0 0 0 odom t265_odom_frame "/>

</launch>

_demo_265_d435.launch_

<launch>
  <arg name="serial_no_camera1"             default="11622110054"/>             <!-- Note: Replace with actual serial number -->
  <arg name="serial_no_camera2"             default="937422070531"/>            <!-- Note: Replace with actual serial number -->
  <arg name="camera1"                       default="t265"/>        <!-- Note: Replace with camera name -->
  <arg name="camera2"                       default="d435"/>        <!-- Note: Replace with camera name -->
  <arg name="tf_prefix_camera1"         default="$(arg camera1)"/>
  <arg name="tf_prefix_camera2"         default="$(arg camera2)"/>
  <arg name="initial_reset"             default="false"/>
  <arg name="view_rviz"                 default="true"/>

  <arg name="device_type"         default="t265"/>
  <arg name="json_file_path"      default=""/>
  <arg name="fisheye_width"       default="848"/> 
  <arg name="fisheye_height"      default="800"/>
  <arg name="enable_fisheye1"     default="false"/>       <!-- Note -->
  <arg name="enable_fisheye2"     default="false"/>       <!-- Note -->
  <arg name="fisheye_fps"         default="30"/>
  <arg name="gyro_fps"            default="200"/>
  <arg name="accel_fps"           default="62"/>
  <arg name="enable_gyro"         default="true"/>
  <arg name="enable_accel"        default="true"/>
  <arg name="enable_pose"         default="true"/>
  <arg name="enable_sync"           default="false"/>
  <arg name="linear_accel_cov"      default="0.01"/>
  <arg name="unite_imu_method"      default="linear_interpolation"/>
  <arg name="publish_odom_tf"     default="true"/>
  <arg name="topic_odom_in"       default="wheel_odom"/>

  <group ns="$(arg camera1)" if="$(eval serial_no_camera1 != '')">
    <include file="$(find realsense2_camera)/launch/includes/nodelet.launch.xml">
      <arg name="serial_no"             value="$(arg serial_no_camera1)"/>
      <arg name="tf_prefix"               value="$(arg tf_prefix_camera1)"/>
      <arg name="device_type"              value="$(arg device_type)"/>
      <arg name="json_file_path"           value="$(arg json_file_path)"/>
      <arg name="enable_sync"              value="$(arg enable_sync)"/>

      <arg name="fisheye_width"            value="$(arg fisheye_width)"/>
      <arg name="fisheye_height"           value="$(arg fisheye_height)"/>
      <arg name="enable_fisheye1"          value="$(arg enable_fisheye1)"/>
      <arg name="enable_fisheye2"          value="$(arg enable_fisheye2)"/>

      <arg name="fisheye_fps"              value="$(arg fisheye_fps)"/>
      <arg name="gyro_fps"                 value="$(arg gyro_fps)"/>
      <arg name="accel_fps"                value="$(arg accel_fps)"/>
      <arg name="enable_gyro"              value="$(arg enable_gyro)"/>
      <arg name="enable_accel"             value="$(arg enable_accel)"/>
      <arg name="enable_pose"              value="$(arg enable_pose)"/>

      <arg name="linear_accel_cov"         value="$(arg linear_accel_cov)"/>
      <arg name="initial_reset"            value="$(arg initial_reset)"/>
      <arg name="unite_imu_method"         value="$(arg unite_imu_method)"/>
      <arg name="topic_odom_in"            value="$(arg topic_odom_in)"/>
      <arg name="publish_odom_tf"          value="$(arg publish_odom_tf)"/>
    </include>
  </group>

  <arg name="color_width"               default="1280"/>
  <arg name="color_height"              default="720"/>
  <arg name="depth_width"               default="1280"/>
  <arg name="depth_height"              default="720"/>
  <arg name="clip_distance"             default="-2"/>

  <group ns="$(arg camera2)">
    <include file="$(find realsense2_camera)/launch/includes/nodelet.launch.xml">
      <arg name="serial_no"             value="$(arg serial_no_camera2)"/>
      <arg name="tf_prefix"                 value="$(arg tf_prefix_camera2)"/>
      <arg name="initial_reset"         value="$(arg initial_reset)"/>
      <arg name="enable_pointcloud"         value="true"/>
      <arg name="align_depth"           value="true"/>
      <!-- <arg name="filters"               value="pointcloud"/> -->
      <arg name="color_width"           value="$(arg color_width)"/>
      <arg name="color_height"          value="$(arg color_height)"/>
      <arg name="depth_width"           value="$(arg depth_width)"/>
      <arg name="depth_height"          value="$(arg depth_height)"/>
      <arg name="linear_accel_cov"         value="1"/>
      <!-- <arg name="clip_distance"         value="$(arg clip_distance)"/> -->
    </include>
  </group>

<node pkg="tf2_ros" type="static_transform_publisher" name="tf2_d435_d265_broadcaster" args="0.0 0.00 0 0 0 0 t265_link d435_link"/>
<node if="$(arg view_rviz)" name="rviz" pkg="rviz" type="rviz" args="-d $(find realsense2_camera)/rviz/d435_and_t265.rviz" required="true" />

</launch>

Any help with that?

[matlabbe]

I suggest to follow the new D400+T265 example on this page: http://wiki.ros.org/rtabmap_ros/Tutorials/HandHeldMapping

In your setup above, you have two visual odometry working at the same time. I would expect the D435 TF to be linked on T265 and just have one odometry. Unless the D435 and T265 are two separate entities moving independently.

[mhaboali]

Thanks a lot for your reply @matlabbe I've applied those modifications and it seems to work much better with simple dataset, I'll test it soon with a more complex one and I'll keep you posted.

[mhaboali]

@matlabbe

I've tested it with simple my own dataset and I have some issues here with the MapData/map_cloud data. As you can see below the wall looks pretty good but the nearer objects(black chair and some table) look very scattered.

Do you have any idea which parameters should I tune for that? I appreciate your great help!

[matlabbe]

Black and metallic objects are difficult to scan. It is more a sensor issue.