Building Map from RBG images

[iamnewtoslam]

Hi Matthew,

I am currently working on my project and I have a little problem with that. I am using the 2D lidar to make the occupancy map and it works now.
Thus, I would like to use the RGB images (only RGB images for assignment) to extract the lines using YOLO and generate the point cloud on those segments (z=0) and merge to the 2D map. Could you suggest me some idea? Thank you so much.

[matlabbe]

Can you have a stereo camera? That could help to get depth of the lines detected in 2D images. Once you have depth corresponding to pixels on the lines, you can use the camera calibration to project those pixels in a 3D point cloud relative to the camera optical frame.

If you have only single-camera RGB images, if the robot is moving on a flat ground and you detect only lines on the ground, then you may estimate the depth of each pixel in the image based on camera height and angle to horizon.

[iamnewtoslam]

Hi @matlabbe,

Thank you for your replies, I have written another node to create the line and generated the point cloud on that lines based on the IPM principle. However, the point clouds only display on the current frame. My question is that possible to combine the generated point clouds by RGB camera to the global 2D occupancy grid map?

I saw there is the topic /cloud_map generated from rtabmap is that the point cloud that could be merged between camera and lidar?

Thank you.

[matlabbe]

You could use pointcloud_to_depthimage (with approx_sync:=false) to convert your point cloud into a depth image. You can feed that depth image with RGB image to rtabmap node. Then set Grid/Sensor=2 (with Grid/3D=false) parameter so that the occupancy grid is created from both lidar and camera's depth.

One issue that could happen if you use Grid/RayTracing=true at the same time is that "lines" going out of FOV of the camera may be ray traced by the lidar.

[maekjuplease]

Hi @matlabbe,

I have tried this in the morning. However there is nothing output from pointcloud_to_depthimage node. After checking, this could be the problem from input format where my camera_info input is Sensor_msgs/camera_info and the default is stereo_msgs/camera_info.

Thus, I tried another way is that convert 2D scan to 3D point cloud to merge those point clouds together. However, there is also no output from ICP_odometry node. Could you give me some guidance? Thank you so much.

Here is the default launch file that I create:

<?xml version="1.0"?>
<!-- -->
<launch>

  <!-- HECTOR MAPPING VERSION: use this with ROS bag demo_mapping_no_odom.bag generated  -->
  <!--                         from demo_mapping.bag with:                               -->
  <!-- rosbag filter demo_mapping.bag demo_mapping_no_odom.bag 'topic != "/tf" or topic == "/tf" and m.transforms[0].header.frame_id != "/odom"' -->
  <!-- WARNING : Database is automatically deleted on each startup                       -->
  <!--           See "delete_db_on_start" option below...                                -->

  <!-- Choose visualization -->
  <arg name="rviz" default="true" />
  <arg name="rtabmap_viz" default="false" />

  <!-- Choose hector_slam or icp_odometry for odometry -->
  <arg name="hector" default="false" />

  <!-- If "hector" above is false, this option feeds wheel odometry to
       icp_odometry as guess ( to be more robust to corridor-like environments).
       If so, use original demo_mapping.bag containing wheel odometry! -->
  <arg name="odom_guess" default="false" />


  <node name="pointCloudFromRGB_test" pkg="rtabmap_odom" type="pointCloudFromRGB_test" output="screen">
  </node>

  <node name="rgb_to_intensity_converter" pkg="rtabmap_odom" type="rgb_to_intensity_converter" output="screen">
  </node>

  <!-- Example with camera or not -->
  <arg name="camera" default="true" />

  <!-- Limit lidar range if > 0 (has effect only when hector:=false, better with odom_guess:=true) -->
  <arg name="max_range" default="0" />

  <!-- Point to Plane ICP? (has effect only when hector:=false) -->
  <arg name="p2n" default="true" />

  <!-- Use libpointmatcher for ICP? (has effect only when hector:=false) -->
  <arg name="pm" default="true" />

  <param name="use_sim_time" type="bool" value="True"/>

  <!-- Odometry from laser scans -->
  <!-- If argument "hector" is true, we use Hector mapping to generate odometry for us -->
  <node if="$(arg hector)" pkg="hector_mapping" type="hector_mapping" name="hector_mapping" output="screen">

    <!-- Frame names -->
    <param name="map_frame" value="hector_map" />
    <param name="base_frame" value="base_link" />
    <param name="odom_frame" value="odom" />

    <!-- Tf use -->
    <param name="pub_map_odom_transform" value="false"/>
    <param name="pub_map_scanmatch_transform" value="true"/>
    <param name="pub_odometry" value="true"/>

    <!-- Map size / start point -->
    <param name="map_resolution" value="0.050"/>
    <param name="map_size" value="2048"/>
    <param name="map_multi_res_levels" value="2" />

    <!-- Map update parameters -->
    <param name="map_update_angle_thresh" value="0.06" />

    <!-- Advertising config -->
    <param name="scan_topic" value="/scan_multi"/>
  </node>

  <!-- If argument "hector" is false, we use rtabmap's icp odometry to generate odometry for us -->
  <node unless="$(arg hector)" pkg="rtabmap_odom" type="icp_odometry" name="icp_odometry" output="screen" >
     <!-- <remap from="scan"      to="/scan_multi"/> -->
     <remap from="scan_cloud"      to="/merged_cloud"/>
     <remap from="odom"      to="/scanmatch_odom"/>
     <remap from="odom_info"      to="/rtabmap/odom_info"/>

     <param name="frame_id"        type="string" value="base_link"/>
     <param name="deskewing"       type="string" value="true"/>

     <param if="$(arg odom_guess)" name="odom_frame_id"   type="string" value="icp_odom"/>
     <param name="guess_frame_id"  type="string" value="odom"/>

     <param name="Icp/VoxelSize"     type="string" value="0.05"/>
     <param name="Icp/RangeMax"      type="string" value="$(arg max_range)"/>
     <param name="Icp/Epsilon"       type="string" value="0.001"/>
     <param unless="$(arg odom_guess)" name="Icp/MaxTranslation" type="string" value="0"/> <!-- can be set to reject large ICP jumps -->
     <param if="$(arg p2n)" name="Icp/PointToPlane"  type="string" value="true"/>
     <param if="$(arg p2n)" name="Icp/PointToPlaneK"  type="string" value="5"/>
     <param if="$(arg p2n)" name="Icp/PointToPlaneRadius"  type="string" value="0.3"/>
     <param unless="$(arg p2n)" name="Icp/PointToPlane"  type="string" value="false"/>
     <param unless="$(arg p2n)" name="Icp/PointToPlaneK"  type="string" value="0"/>
     <param unless="$(arg p2n)" name="Icp/PointToPlaneRadius"  type="string" value="0"/>
     <param name="Icp/MaxCorrespondenceDistance" type="string" value="0.1"/>
     <param name="Icp/PM"             type="string" value="$(arg pm)"/> <!-- use libpointmatcher to handle PointToPlane with 2d scans-->
     <param name="Icp/PMOutlierRatio" type="string" value="0.85"/>
     <param name="Odom/Strategy"        type="string" value="0"/>
     <param name="Odom/GuessMotion"     type="string" value="true"/>
     <param name="Odom/ResetCountdown"  type="string" value="0"/>
     <param name="Odom/ScanKeyFrameThr"  type="string" value="0.75"/>
  </node>


  <!-- <node pkg="rtabmap_util" type="pointcloud_to_depthimage" name="pointcloud_to_depthimage">
    <remap from="cloud" to="/cameraCloudFrame"/>
    <remap from="rgb/camera_info" to="/camera0/color/camera_info"/>
    <param name="Grid/RangeMax" type="double" value="100"/>
    <param name="Grid/DepthDecimation" type="double" value="0.1"/>
    <param name="approx_sync" type="double" value="100"/>
  </node> -->

  <group ns="rtabmap">
    <node if="$(arg camera)" pkg="rtabmap_sync" type="nodelet" name="rgbd_sync" args="standalone rtabmap_sync/rgbd_sync" output="screen">
      <remap from="rgb/image"       to="/data_throttled_image"/>
      <remap from="depth/image"     to="/data_throttled_image_depth"/>
      <remap from="rgb/camera_info" to="/data_throttled_camera_info"/>
      <param name="rgb/image_transport"   type="string" value="compressed"/>
      <param name="depth/image_transport" type="string" value="compressedDepth"/>
    </node>

    <!-- SLAM -->
    <!-- args: "delete_db_on_start" and "udebug" -->
    <node name="rtabmap" pkg="rtabmap_slam" type="rtabmap" output="screen">
      <param name="frame_id" type="string" value="base_link"/>

      <param name="subscribe_rgb"   type="bool" value="false"/>
      <param name="subscribe_depth" type="bool" value="false"/>
      <param name="subscribe_rgbd"  type="bool" value="$(arg camera)"/>
      <param name="subscribe_scan"  type="bool" value="false"/>
      <param name="subscribe_scan_cloud"  type="bool" value="true"/>
      <!-- <remap from="grid_map" to="/map"/> -->

      <!-- <remap from="scan" to="/scan_multi"/> -->
      <remap from="scan_cloud"      to="/merged_cloud"/>

      <param name="Grid/Sensor"      type="int" value="0"/>
      <param name="database_path"       type="string" value="~/.ros/mapping0923_test1.db"/>
      <param name="Mem/IncrementalMemory" type="string" value="false"/>
      <param name="Mem/InitWMWithAllNodes" type="string" value="false"/>


      <!-- As hector doesn't provide compatible covariance in the odometry topic, don't use the topic and fix the covariance -->
      <param name="odom_frame_id"            type="string" value="icp_odom"/>
      <param name="odom_tf_linear_variance"  type="double" value="0.0005"/>
      <param name="odom_tf_angular_variance" type="double" value="0.0005"/>

      <remap from="odom" to="/scanmatch_odom"/>
      <param unless="$(arg hector)" name="subscribe_odom_info" type="bool" value="true"/>

      <!-- RTAB-Map's parameters -->
      <param name="Reg/Strategy"       type="string" value="1"/>    <!-- 0=Visual, 1=ICP, 2=Visual+ICP -->
      <param name="Reg/Force3DoF"      type="string" value="true"/>
      <param name="RGBD/ProximityBySpace"    type="string" value="true"/>
      <param name="Icp/CorrespondenceRatio"  type="string" value="0.2"/>
      <param name="Icp/VoxelSize"            type="string" value="0.05"/>
      <param name="Icp/RangeMax"             type="string" value="$(arg max_range)"/>
      <param name="Grid/RangeMax"            type="string" value="$(arg max_range)"/>
    </node>

    <!-- Visualisation RTAB-Map -->
    <node if="$(arg rtabmap_viz)" pkg="rtabmap_viz" type="rtabmap_viz" name="rtabmap_viz" args="-d $(find rtabmap_demos)/launch/config/rgbd_gui.ini" output="screen">
      <param name="subscribe_rgbd"      type="bool" value="$(arg camera)"/>
      <param name="subscribe_laserScan" type="bool" value="true"/>
      <param name="frame_id"            type="string" value="base_link"/>

      <!-- <remap from="scan"            to="/scan_multi"/> -->

      <!-- As hector doesn't provide compatible covariance in the odometry topic -->
      <param if="$(arg hector)" name="odom_frame_id" type="string" value="hector_map"/>

      <remap unless="$(arg hector)" from="odom" to="/scanmatch_odom"/>
      <param unless="$(arg hector)" name="subscribe_odom_info" type="bool" value="true"/>
    </node>

  </group>

  <!-- Visualisation RVIZ -->
  <node if="$(arg rviz)" pkg="rviz" type="rviz" name="rviz" args="-d $(find rtabmap_demos)/launch/config/demo_robot_mapping.rviz" output="screen"/>
   <node if="$(arg camera)" pkg="nodelet" type="nodelet" name="points_xyzrgb" args="standalone rtabmap_util/point_cloud_xyzrgb">
    <remap from="rgbd_image"      to="/rtabmap/rgbd_image"/>
    <remap from="cloud"           to="voxel_cloud" />

    <param name="voxel_size" type="double" value="0.01"/>
  </node>

</launch>

[matlabbe]

To make pointcloud_to_depthimage works correctly, it requires a valid sensor_msgs/CameraInfo using camera optical frame_id, with TF/URDF between camera and lidar accurate. In your case, the point cloud seems to be generated already in camera optical frame, so TF/URDF is not required, just make sure the point cloud's frame_id is the same than the frame_id of the camera_info topic.

However, there is also no output from ICP_odometry node

What is /merged_cloud? is it published and have points? Are there warnings?

[maekjuplease]

To make pointcloud_to_depthimage works correctly, it requires a valid sensor_msgs/CameraInfo using camera optical frame_id, with TF/URDF between camera and lidar accurate. In your case, the point cloud seems to be generated already in camera optical frame, so TF/URDF is not required, just make sure the point cloud's frame_id is the same than the frame_id of the camera_info topic.

I will further check it and reply to you soon.

What is /merged_cloud? is it published and have points? Are there warnings?
/merged_cloud is the topic that merged two scan data (front and rear scans) and converted them to point cloud. However I have tried to input that to the ICP odometry node and the error shown as below:

I even tried to use one scan and converted to point cloud but the result still shown as the above image.

[matlabbe]

Set Icp/PointToPlaneRadius to 0 for icp_odometry node. I think that configuration was for laser scan topic, not point cloud topic. Or if the point cloud is 2D (which looks like your case), set scan_cloud_is_2d to true for icp_odometry node so you don't need to change Icp/PointToPlaneRadius.