Is RTABMAP suitable for creating a 3D Model of a Room?

classic Classic list List threaded Threaded
5 messages Options
Lex
Reply | Threaded
Open this post in threaded view
|

Is RTABMAP suitable for creating a 3D Model of a Room?

Lex
Hi, I am doing a project where the goal is to create a 3D Model of an enviroment using multiple Kinects. After doing some testing with RTABMap and using the built-in export for creating a 3D Mesh I can't manage to get good results (the models that I get have low detail). I am assuming that this is because I am not using good parameters in the export options or a I am using a low quality map; no problem with this, I will be doing more tests.

But it also came to my mind that maybe using Kinects (and visual odometry) with Rtabmap is not good enough for this purpose? Or should it be capable?
Reply | Threaded
Open this post in threaded view
|

Re: Is RTABMAP suitable for creating a 3D Model of a Room?

matlabbe
Administrator
Lex wrote
(the models that I get have low detail)
If it is a level of details problem, this could be fixed by tuning export options. However, if it is a drift problem (double-walls, double-objects...), then it could be a odometry parameter tuning. Note sure if you are referring to kinect 360, kinect One, or Kienct azure, for the later you can get relatively good tracking and get results similar to RTAB-Map on iOS or Tango. If you can share the database and show some results that you have, it could help to give more advices.

Lex
Reply | Threaded
Open this post in threaded view
|

Re: Is RTABMAP suitable for creating a 3D Model of a Room?

Lex
Hi, Right now this is the results of a mapping session. I use Kinect 360 mounted on a Pioneer robot; right now I am not using the odometry from the robot and I just use visual odometry from the kinects.

This is the map I get doing a sessions of a couple minutes:

As you can see the map is diverged, at some point it begins to create another corridor insted of mapping the same, thinking that the previous corridor is a new one. I assume this is due to lost odometry in the corridors and can only be fixed if a I do another session and avoid lost odemetry or bad pose calibration of the cameras right?
Also, I do not know the exact pose of my cameras, right now I just set the poses manually by eye, I am trying to create a system that automatically determines the poses between cameras using 3 planes of a room and an intersect point. Meanwhile I just measure the distance manually, I asume that if I can get a better calibration the results would be better.
This is my bag file and the database: https://drive.google.com/drive/folders/18n7jMgByeYapPN96baX0X9sG3RvxeTCX?usp=sharing
And this is my launch:
<launch>
   
  <!-- Multi-cameras demo with 2 Kinects -->
   <param name="use_sim_time" type="bool" value="True"/>


  <!-- Script to play a sound if odometry is lost -->
  <node name="lost_odometry_sound" pkg="kinect_model" type="lost_odometry_sound.py" output="screen"/>


  <!-- Frames: Kinects are placed at 90 degrees, clockwise -->
  <node pkg="tf2_ros" type="static_transform_publisher" name="base_to_camera1_tf"
      args="0.0 -0.3 0.0 0.3 0.1 0.0 /base_link /camera3_link" />
  <node pkg="tf2_ros" type="static_transform_publisher" name="base_to_camera2_tf"
      args="0.0 0.4 0.0 -0.4 0.2 0.0 /base_link /camera2_link" />
  <node pkg="tf2_ros" type="static_transform_publisher" name="base_to_camera3_tf"
      args="0.0 0.0 0.0 0.0 0.0 0.0 /base_link /camera1_link" />

 
  <!-- Publish tf for camera1 -->
  <include file="$(find rgbd_launch)/launch/kinect_frames.launch">
    <arg name="camera" value="camera1" />
  </include>

  <!-- Publish tf for camera2 -->
  <include file="$(find rgbd_launch)/launch/kinect_frames.launch">
    <arg name="camera" value="camera2" />
  </include>

  <!-- Publish tf for camera3 -->
  <include file="$(find rgbd_launch)/launch/kinect_frames.launch">
    <arg name="camera" value="camera3" />
  </include>


   <!-- Choose visualization -->
   <arg name="rviz"       default="false" />
   <arg name="rtabmapviz" default="true" /> 
   
   <!-- ODOMETRY MAIN ARGUMENTS: 
        -"strategy"        : Strategy: 0=Frame-to-Map 1=Frame-to-Frame
        -"feature"         : Feature type: 0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK
        -"nn"              : Nearest neighbor strategy : 0=Linear, 1=FLANN_KDTREE, 2=FLANN_LSH, 3=BRUTEFORCE 
                             Set to 1 for float descriptor like SIFT/SURF                  
                             Set to 3 for binary descriptor like ORB/FREAK/BRIEF/BRISK  
        -"max_depth"       : Maximum features depth (m)  
        -"min_inliers"     : Minimum visual correspondences to accept a transformation (m)  
        -"inlier_distance" : RANSAC maximum inliers distance (m)  
        -"local_map"       : Local map size: number of unique features to keep track 
        -"odom_info_data"  : Fill odometry info messages with inliers/outliers data.
    -->
   <arg name="strategy"        default="0" />
   <arg name="feature"         default="6" />
   <arg name="nn"              default="3" />
   <arg name="max_depth"       default="4.0" />
   <arg name="min_inliers"     default="20" />
   <arg name="inlier_distance" default="0.02" />
   <arg name="local_map"       default="1000" />
   <arg name="odom_info_data"  default="true" />
   <arg name="wait_for_transform"  default="true" />


    
    <group ns="camera1">
      <node name="republish_rgbd_image"  type="rgbd_relay" pkg="rtabmap_ros">
           <remap     from="rgbd_image" to="rgbd_image/compressed"/>
           <remap     from="rgbd_image/compressed_relay" to="rgbd_image"/>
           <param name="uncompress" value="true"/>
      </node>
    </group>

    <group ns="camera2">
      <node name="republish_rgbd_image"  type="rgbd_relay" pkg="rtabmap_ros">
           <remap     from="rgbd_image" to="rgbd_image/compressed"/>
           <remap     from="rgbd_image/compressed_relay" to="rgbd_image"/>
           <param name="uncompress" value="true"/>
      </node>
    </group>
   

    <group ns="camera3">
      <node name="republish_rgbd_image"  type="rgbd_relay" pkg="rtabmap_ros">
           <remap     from="rgbd_image" to="rgbd_image/compressed"/>
           <remap     from="rgbd_image/compressed_relay" to="rgbd_image"/>
           <param name="uncompress" value="true"/>
      </node>
    </group>
   
   
   <!-- sync rgb/depth images per camera 
   <group ns="camera1">
    <node pkg="nodelet" type="nodelet" name="rgbd_sync" args="standalone rtabmap_ros/rgbd_sync">
      <remap from="rgb/image"         to="rgb/image_rect_color"/>
      <remap from="depth/image"       to="depth_registered/image_raw"/>
      <remap from="rgb/camera_info"   to="rgb/camera_info"/>
      <remap from="rgbd_image"      to="rgbd_image"/>
     
<param name="rgb/image_transport"   type="string" value="compressed"/>
      <param name="depth/image_transport" type="string" value="compressedDepth"/>

    </node>

   </group>

   <group ns="camera2">
    <node pkg="nodelet" type="nodelet" name="rgbd_sync" args="standalone rtabmap_ros/rgbd_sync">
      <remap from="rgb/image"         to="rgb/image_rect_color"/>
      <remap from="depth/image"       to="depth_registered/image_raw"/>
      <remap from="rgb/camera_info"   to="rgb/camera_info"/>
      <remap from="rgbd_image"      to="rgbd_image"/>

<param name="rgb/image_transport"   type="string" value="compressed"/>
      <param name="depth/image_transport" type="string" value="compressedDepth"/> 
    </node>
   </group>

   <group ns="camera3">
    <node pkg="nodelet" type="nodelet" name="rgbd_sync" args="standalone rtabmap_ros/rgbd_sync">
      <remap from="rgb/image"         to="rgb/image_rect_color"/>
      <remap from="depth/image"       to="depth_registered/image_raw"/>
      <remap from="rgb/camera_info"   to="rgb/camera_info"/>
      <remap from="rgbd_image"      to="rgbd_image"/>

<param name="rgb/image_transport"   type="string" value="compressed"/>
      <param name="depth/image_transport" type="string" value="compressedDepth"/> 
    </node>
   </group>
        
        -->
  <group ns="rtabmap">
  
    <!-- Odometry -->
    <node pkg="rtabmap_ros" type="rgbd_odometry" name="rgbd_odometry" output="screen">
      <remap from="rgbd_image0"       to="/camera1/rgbd_image/"/>
      <remap from="rgbd_image1"       to="/camera2/rgbd_image/"/>
      <remap from="rgbd_image2"       to="/camera3/rgbd_image/"/>




	        <param name="Grid/CellSize" type="string" value="0.01"/>

	  <param name="subscribe_rgbd"           type="bool"   value="true"/>
	  <param name="frame_id"                 type="string" value="base_link"/>
	  <param name="rgbd_cameras"             type="int"    value="3"/>
	  <param name="wait_for_transform"       type="bool"   value="$(arg wait_for_transform)"/>
	  <param name="Odom/Strategy"            type="string" value="$(arg strategy)"/> 
	  <param name="OdomF2M/BundleAdjustment" type="string" value="0"/> <!-- should be 0 for multi-cameras -->
	  <param name="Vis/EstimationType"      type="string" value="0"/> <!-- should be 0 for multi-cameras -->
	  <param name="Vis/FeatureType"         type="string" value="$(arg feature)"/> 
	  <param name="Vis/CorGuessWinSize"     type="string" value="0"/> 
	  <param name="Vis/CorNNType"           type="string" value="$(arg nn)"/>
	  <param name="Vis/MaxDepth"            type="string" value="$(arg max_depth)"/>  
	  <param name="Vis/MinInliers"          type="string" value="$(arg min_inliers)"/> 
	  <param name="Vis/InlierDistance"      type="string" value="$(arg inlier_distance)"/>       
      <param name="OdomF2M/MaxSize" type="string" value="$(arg local_map)"/> 
      <param name="Odom/FillInfoData"        type="string" value="$(arg odom_info_data)"/>   
    </node>
  
    <!-- Visual SLAM (robot side) -->
    <!-- args: "delete_db_on_start" and "udebug" -->
    <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="--delete_db_on_start">
      <param name="subscribe_depth"  type="bool"   value="false"/>
      <param name="subscribe_rgbd"   type="bool"   value="true"/>
      <param name="rgbd_cameras"    type="int"    value="3"/>
	  <param name="frame_id"         type="string" value="base_link"/>
	  <param name="gen_scan"         type="bool"   value="true"/>
	  <param name="wait_for_transform" type="bool"   value="$(arg wait_for_transform)"/>
	  <param name="map_negative_poses_ignored" type="bool"   value="false"/>        <!-- refresh grid map even if we are not moving-->
	  <param name="map_negative_scan_empty_ray_tracing" type="bool" value="false"/> <!-- don't fill empty space between the generated scans-->
	
      <remap from="rgbd_image0"       to="/camera1/rgbd_image/"/>
      <remap from="rgbd_image1"       to="/camera2/rgbd_image/"/>
      <remap from="rgbd_image2"       to="/camera3/rgbd_image/"/>
 <!-- 
      <param name="Grid/RangeMax"     type="string" value="5"/>
      <param name="Grid/MaxObstacleHeight"     type="string" value="2"/> -->
      <param name="Grid/CellSize" type="string" value="0.01"/>
      <param name="Grid/3D"     type="string" value="true"/>
      <param name="Grid/RayTracing"     type="string" value="true"/>


      <param name="Grid/FromDepth"     type="string" value="false"/>
      <param name="Vis/EstimationType" type="string" value="0"/> <!-- should be 0 for multi-cameras -->
      <param name="Vis/MinInliers"     type="string" value="10"/>
      <param name="Vis/InlierDistance" type="string" value="$(arg inlier_distance)"/>


    </node>
  
    <!-- Visualisation RTAB-Map -->
    <node if="$(arg rtabmapviz)" pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" args="-d $(find rtabmap_ros)/launch/config/rgbd_gui.ini" output="screen">
      <param name="subscribe_depth"  type="bool"   value="false"/>
      <param name="subscribe_rgbd"   type="bool"   value="true"/>
      <param name="subscribe_odom_info" type="bool"   value="$(arg odom_info_data)"/>
      <param name="frame_id"            type="string" value="base_link"/>
      <param name="rgbd_cameras"       type="int"    value="3"/>
      <param name="wait_for_transform"  type="bool"   value="$(arg wait_for_transform)"/>
    
      <remap from="rgbd_image0"       to="/camera1/rgbd_image/"/>
      <remap from="rgbd_image1"       to="/camera2/rgbd_image/"/>
      <remap from="rgbd_image2"       to="/camera3/rgbd_image/"/>


    </node>
  
  </group>
  
  <!-- Visualization RVIZ -->
  <node if="$(arg rviz)" pkg="rviz" type="rviz" name="rviz" args="-d $(find rtabmap_ros)/launch/config/rgbd.rviz"/>

</launch>
Lex
Reply | Threaded
Open this post in threaded view
|

Re: Is RTABMAP suitable for creating a 3D Model of a Room?

Lex
In reply to this post by matlabbe
Hi, I have been working more on this and now I have new Set-Up:

* 2 Kinects
* Sick LMS100
* Odometry from Pioneer 3

This is my new database now has better quality, and the map overall is good. But when creating the mesh I can't a get one with good quality. The first one I created had a lot of holes in the floor and walls. After some param adjusting I get less holes but now the cloud is filled with balls and blocky (nothing is flat, seems very little quality).

This is what I am refering to:


This is my new database, the bag and the launch file: https://udcgal-my.sharepoint.com/:f:/g/personal/a_gosende_udc_es/Ejndgjq7AXVHnhr4AhfRbc4BV609eTadSX5ikysTrwSzsw?e=YYgBcR
Reply | Threaded
Open this post in threaded view
|

Re: Is RTABMAP suitable for creating a 3D Model of a Room?

matlabbe
Administrator
Hi,

Looking at the database, the TF between the cameras and the base frame are wrong. The ground is not flat when looking kinect point cloud in base frame (see below). This creates a very wavy floor, so you will never get a straight mesh with that data. I suggest to open RVIZ, set global fixed frame to base_link, show rviz grid, add point cloud of both cameras, then iteratively adjust your static transform so that the ground in the point clouds is flat in RVIZ.



cheers,
Mathieu