search

introlab / rtabmap

RTAB-Map library and standalone application
https://introlab.github.io/rtabmap
Other
2.72k stars 775 forks source link

using D435i with T265 #439

Open fightforge opened 5 years ago

fightforge commented 5 years ago

Hi, i have both t265 and d435i, and i want to use both for increase accuracy in rtabmap. Is that possible? Will i get better result using both sensors?

thanks

rorygarlandAnyVision commented 5 years ago

Using both is perfectly possible and it seems to get better results. You will need to have the libraries for the cameras, realsense2. You can check they work by running realsense-viewer

After that, you can start the two cameras via:

  1. d435i: detection - selection source - rgbd camera - realsense d435
  2. t265: detection - selection source - stereo camera - realsense t265

Everything should work from there.

fightforge commented 5 years ago

Hi, i got it work together in ubuntu 16.04 and kinect with ros and rviz but could you suggest me a launch file just for hendheld mapping?

Thanks

JoeyLeeNPU commented 5 years ago

I have a test on 265 and 435, can we have a discuss with more details?

fightforge commented 5 years ago

Hi, searching online i made a mix of lauch files in one file. I'm using ros kinect on ubuntu 16.04, and as first command i start rs_d400_and_t265.launch that i found in librealsense2 on github, i change just the serial number and camera name (i set camera1 name to t265 and camera2 to d400) sections from my sensors:

roslaunch realsense2_camera rs_d400_and_t265.launch align_depth:=true unite_imu_method:=linear_interpolation

then i start the imu_filter_magic as decribed in http://wiki.ros.org/rtabmap_ros/Tutorials/HandHeldMapping:

rosrun imu_filter_madgwick imu_filter_node _use_mag:=false _publish_tf:=false _world_frame:="enu" /imu/data_raw:=/t265/imu /imu/data:=/rtabmap/imu

and after all i started my modified rtabmap.launch using the same wiki about hendheldmapping for d435i:

roslaunch rtabmap_ros rtabmap.launch rtabmap_args:=--delete_db_on_start --Optimizer/GravitySigma 0.3 depth_topic:=/d400/aligned_depth_to_color/image_raw rgb_topic:=/d400/color/image_raw camera_info_topic:=/camera/color/camera_info approx_sync:=false wait_imu_to_init:=true imu_topic:=/rtabmap/imu

and this is my rtabmap.launch:

<launch>
  <arg name="gui_cfg"           default="~/.ros/rtabmap_gui.ini" />
  <arg name="launch_prefix"     default=""/>
  <arg name="output"            default="screen"/>
  <arg name="imu_topic"         default="/imu/data"/>
  <!-- <node pkg="tf" type="static_transform_publisher" name="base_to_t265_tf" args="0.0 0.0 0.0 0.0 0.0 0.0 /base_link /t265_link 100" /> -->
  <node pkg="tf" type="static_transform_publisher" name="d400_to_t265_tf" args="0.0 0.0 0.0 -3.14 0.0 0.0 /t265_link /d400_link 100" />

  <group ns="rtabmap">
    <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="--delete_db_on_start">
        <param name="odom_frame_id"        type="string" value="t265_odom_frame"/> <!-- the fixed frame used by robot_localization -->
        <param name="odom_tf_angular_variance" type="double" value="0.0005"/>
        <param name="odom_tf_linear_variance"  type="double" value="0.0001"/> <!-- 1 cm stddev error -->

      <param name="frame_id" type="string" value="t265_link"/>
      <param name="subscribe_depth" type="bool" value="true"/>
      <param name="subscribe_odom_info" type="bool" value="false"/>
        <!-- Add param name="subscribe_odom_info" type="bool" value="true"/> to fix problems with rtabmap subscribing to odom_info -->
      <!--  <remap from="odom" to="/odometry/filtered"/> -->
      <!--remap from="rgb/image" to="/camera/color/image_raw"/-->
        <!--remap from="depth/image" to="/camera/aligned_depth_to_color/image_raw"/-->
        <!--remap from="rgb/camera_info" to="/camera/color/camera_info"/-->
      <remap from="rgb/image" to="/d400/color/image_raw"/>
      <remap from="depth/image" to="/d400/aligned_depth_to_color/image_raw"/>
      <remap from="rgb/camera_info" to="/d400/color/camera_info"/>
        <param name="queue_size" type="int" value="100"/>
      <!-- RTAB-Map's parameters -->
        <param name="RGBD/NeighborLinkRefining" type="string" value="true"/>
        <param name="RGBD/ProximityBySpace"     type="string" value="true"/>
        <param name="RGBD/AngularUpdate"        type="string" value="0.01"/>
        <param name="RGBD/LinearUpdate"         type="string" value="0.01"/>
        <param name="RGBD/OptimizeFromGraphEnd" type="string" value="false"/>
        <param name="Reg/Force3DoF"         type="string" value="true"/> <!--same as optimizer/slam2D -->
        <param name="Reg/Strategy"              type="string" value="1"/> <!-- 1=ICP -->
        <param name="Reg/Force3DoF"             type="string" value="true"/>
        <param name="Vis/MinInliers"            type="string" value="5"/>
        <param name="Vis/InlierDistance"        type="string" value="0.1"/>
        <param name="Rtabmap/TimeThr"           type="string" value="700"/>
        <param name="Mem/RehearsalSimilarity"   type="string" value="0.45"/>
        <!-- <param name="Grid/CellSize" type="string" value="0.05"/> -->
        <param name="Kp/MaxFeatures" type="string" value="400"/>
      <param name="Optimizer/Strategy" type="string" value="2"/> <!-- 0 is TORO, Default G20-->
      <param name="RGBD/OptimizeMaxError" type="string" value="0"/> <!--When using TORO, seto to 0,otherwise set to 1-->
      <param name="Kp/DetectorStrategy" type="string" value="6"/> <!--0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF(default) 7=BRISK 8=GFTT/ORB 9=KAZE-->
      <param name="RGBD/LoopClosureReextractFeatures" type="string" value="true"/> <!--extract features even if there are some already in the node-->
      <!--  <param name="Grid/FromDepth" type="string" value="false"/> -->
      <!-- <param name="RGBD/ProximityPathMaxNeighbors" type="string" value="10"/> -->
      <param name="Vis/FeatureType" type="string" value="6"/> <!--suppress warning,want to be same as KP/detectorstrategy -->
    </node>

    <!-- Visualisation RTAB-Map -->
    <node pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" args="-d $(arg gui_cfg)" output="$(arg output)" launch-prefix="$(arg launch_prefix)">
      <!-- <node pkg="rviz" type="rviz" name="rtabmapviz" args="-d $(find rtabmap_ros)/launch/config/rgbd.rviz"/> -->
      <!-- <node pkg="nodelet" type="nodelet" name="points_xyzrgb" args="standalone rtabmap_ros/point_cloud_xyzrgb" output="$(arg output)"> -->
      <param name="subscribe_rgbd"      type="bool"   value="false"/>
      <param name="subscribe_odom_info" type="bool" value="false"/>
      <!-- <param name="frame_id"           type="string" value="d400_link"/> -->
      <param name="odom_frame_id"       type="string" value="t265_link"/>
      <param name="wait_for_transform_duration" type="double"   value="0.4"/>
      <param name="queue_size"          type="int"  value="50"/>
      <param name="approx_sync"         type="bool"   value="true"/>

      <!--remap from="rgb/image" to="/camera/rgb/image_rect_color"/-->
      <!--remap from="depth/image" to="/camera/depth_registered/image_raw"/-->
      <!--remap from="rgb/camera_info" to="/camera/rgb/camera_info"/-->
      <!--remap from="rgbd_image"   to="/camera/rgb/image_rect_color"/-->
      <remap from="rgb/image" to="/d400/color/image_raw"/>
      <remap from="depth/image" to="/d400/aligned_depth_to_color/image_raw"/>
      <remap from="rgb/camera_info" to="/d400/color/camera_info"/>
      <remap from="odom"      to="/odometry/filtered"/>
    </node>
  </group>
</launch>

I think you can use the same launch file becouse i'm not using d435i imu. I get always scan error/warning becouse, if i understood well, i'm using optimizer/strategy as 2, but if i change it to 1 or 0 i get bad result in 3d mapping. With this configuration i get slow good result in 3d mapping, but i want to know if it's all correct or if i made some mistakes.

Thanks

JoeyLeeNPU commented 5 years ago

My test is set as following: t265's /odom to rtab's /odom, and 435 only provides rgb-d, 2d laser provides pointcloud. The back-end optimizer is default, i think it works good for localization.

BogdanLazarescu commented 4 years ago

I am trying to run rtabmap demo with bot 435 and 265, but it seems that the transforms i am using are somehow wrong(the pointcloud is always under the map frame). Can you share your launch file with the configuration you are using? Thank you!

JoeyLeeNPU commented 4 years ago

I am trying to run rtabmap demo with bot 435 and 265, but it seems that the transforms i am using are somehow wrong(the pointcloud is always under the map frame). Can you share your launch file with the configuration you are using? Thank you!

I think you can read “rtab.launch” about using D435 and T265 in the realsense SDK, the TFs in camera is a little different from other camera like ZED which has a 90 degrees rotation.

BogdanLazarescu commented 4 years ago

@JoeyLeeNPU do you happen to be able to share the launchfile you have used?

tmichals commented 4 years ago

Hi... I was wondering about the imu_filter_magic ... I seem to be stuck waiting for data on the /imu/data_raw... is there anything you can say about this? I do not see very much information about using imu_filter_magic, so what am I missing?

lcs@nano:~/catkin_ws$ rosrun imu_filter_madgwick imu_filter_node _use_mag:=false _publish_tf:=false _world_frame:="enu" /imu/data_raw:=/t265/imu /imu/data:=/rta bmap/imu [ INFO] [1577606877.060742272]: Starting ImuFilter [ INFO] [1577606877.071141595]: Using dt computed from message headers [ INFO] [1577606877.092772220]: Imu filter gain set to 0.100000 [ INFO] [1577606877.092867064]: Gyro drift bias set to 0.000000 [ INFO] [1577606877.092931126]: Magnetometer bias values: 0.000000 0.000000 0.00 0000 [ WARN] [1577606887.108913779]: Still waiting for data on topic /imu/data_raw... [ WARN] [1577606897.109419973]: Still waiting for data on topic /imu/data_raw...

matlabbe commented 4 years ago

That line seems wrong: "/imu/data_raw:=/t265/imu /imu/data:=/rta bmap/imu". in the post above, it is: rosrun imu_filter_madgwick imu_filter_node _use_mag:=false _publish_tf:=false _world_frame:="enu" /imu/data_raw:=/t265/imu /imu/data:=/rtabmap/imu

Is /t265/imu published when you start realsense camera?

Note it is not "magic", it is "magdwick".

tmichals commented 4 years ago

Thank you.....I will check what I typed into the nano and let you know. I really am not sure where to find the information on starting the magdwick IMU Tool. I copied the rosrun startup from another person, that maybe wrong what they posted.

NicoNorena commented 4 years ago

I am using the D435i and T265 on a jetson; from my understanding in Rtabmap, the IMU is only used for the odometry, so when disabling the param visual_odometry you rely on the odometry from the t265. Furthermore, the IMU does not need to be pass into rtabmap.

I am seeing that the map is not updating the registered map correctly, I tried many configuration for the loop closure, proximity detection, and graph optimization, but It does not seem to help. In the picture below, the wall is registered at different planes or drifting as I move around. Screenshot from 2020-06-23 13-30-28 diffent view: Screenshot from 2020-06-23 13-37-04

Does any one has any idea why this is happening? I subscribed to these topics /d400/color/image_raw /d400/aligned_depth_to_color/image_raw /d400/color/camera_info /t265/odom/sample

matlabbe commented 4 years ago

It seems a TF problem. What is frame_id set to? IS there a rotation between the D400 and T265 cameras?

gezabohus commented 4 years ago

That line seems wrong: "/imu/data_raw:=/t265/imu /imu/data:=/rta bmap/imu". in the post above, it is: rosrun imu_filter_madgwick imu_filter_node _use_mag:=false _publish_tf:=false _world_frame:="enu" /imu/data_raw:=/t265/imu /imu/data:=/rtabmap/imu

Is /t265/imu published when you start realsense camera?

Note it is not "magic", it is "magdwick".

/t265/imu is not a published topic. I have

$ rostopic list | grep imu /cam_2/accel/imu_info /cam_2/gyro/imu_info /d400/accel/imu_info /d400/gyro/imu_info /t265/accel/imu_info /t265/gyro/imu_info

Any ideas if how to get imu from these? (I have one T265 and one D435i. Cam_2 should be the same as the D435i, I'm not sure why those topics are twice.)

matlabbe commented 4 years ago

I updated this tutorial with a D400+T265 example: http://wiki.ros.org/rtabmap_ros/Tutorials/HandHeldMapping

$ roslaunch realsense2_camera rs_d400_and_t265.launch

$ roslaunch rtabmap_ros rtabmap.launch \
   args:="-d --Mem/UseOdomGravity true --Optimizer/GravitySigma 0.3" \
   odom_topic:=/t265/odom/sample \
   frame_id:=t265_link \
   rgbd_sync:=true \
   depth_topic:=/d400/aligned_depth_to_color/image_raw \
   rgb_topic:=/d400/color/image_raw \
   camera_info_topic:=/d400/color/camera_info \
   approx_rgbd_sync:=false \
   visual_odometry:=false
gezabohus commented 4 years ago

Thanks @matlabbe.

Answering my own question, I was using one of the RS launch files that neglected to use unite_imu_method, hence no united imu. Once I fixed this, imu started to get published. (There is a bit of a mixture of camera/d400/t265 in the conventions, but those are easy to fix.)

NicoNorena commented 4 years ago

@matlabbe I was not using rgbd_sync:=true ; After using it it was working better.I think I had the frame rate too high as well. What is the frame rate you have tested? I was trying to increase it , I tested at 5 Hz . is the parameter for time threshold managing the maximum time the process () function should take to process the nodes in the working memory? essentially, if I set the time threshold too small like 100 [milisecs] it would not have enough time to process the nodes.

matlabbe commented 4 years ago

In my example, the rtabmap update rate was the default 1 Hz (Rtabmap/DetectionRate=1), with camera images published at 30 Hz. By default, the maximum processing time is infinite (no memory management). If Rtabmap/TimeThr is set to 100 ms and processing one frame needs more than 100 ms, it will still process the frame even if it takes 200 ms to do so, the actual rate will be then 5 Hz max.