cv::Exception Airsim implementation

[Manuelo247]

I am working on a project that involves integrating the AirSim simulator with RTAB-Map. The simulated drone is a quadcopter that provides both odometry data and RGB and depth images.

My problem is that I have never used RTAB-Map before, and I haven't found other projects that implement it with the AirSim simulator. Consequently, I have encountered several errors throughout my project, which I have been solving one by one. However, I am now stuck on an issue that I can't move past.

[ INFO] [1721460479.375829476]: rtabmap 0.21.5 started... terminate called after throwing an instance of 'cv::Exception' what(): OpenCV(4.2.0) ../modules/core/src/matrix.cpp:209: error: (-215:Assertion failed) 0 <= _dims && _dims <= CV_MAX_DIM in function 'setSize'

[rtabmap-1] process has died [pid 49137, exit code -6, cmd /home/manuelo247/catkin_ws/devel/lib/rtabmap_slam/rtabmap --delete_db_on_start udebug /rgb/camera_info:=/airsim/image/camera_info /odom_info:=/airsim/odom_info /odom:=/airsim/odom /depth/image:=/airsim/image/depth /rgb/image:=/airsim/image/rgb __name:=rtabmap __log:=/home/manuelo247/.ros/log/98c6ab9e-4656-11ef-9083-5dc43197f92a/rtabmap-1.log]. log file: /home/manuelo247/.ros/log/98c6ab9e-4656-11ef-9083-5dc43197f92a/rtabmap-1*.log

This error is thrown by RTAB-Map itself. After debugging my code a bit, I realized that the issue arises when publishing to "/airsim/image/camera_info". I know the error message mentions something about image sizes, but I have verified that the sizes of the images I am publishing match those in the camera info topic. Additionally, some sections of my topic publication were generated by AI, and I don't fully understand their purpose or functionality. Here is a section of the code that generates the message.

def get_cameraInfo(size):
    camera_info = client.simGetCameraInfo("0")
    current_time = rospy.Time.now()

    cam_info_msg = CameraInfo()
    cam_info_msg.header.seq = 0  # Placeholder
    cam_info_msg.header.stamp = current_time  # Usar el tiempo actual
    cam_info_msg.header.frame_id = 'camera_link'
    cam_info_msg.width = size[0]  # Ajusta según sea necesario
    cam_info_msg.height = size[1]  # Ajusta según sea necesario

    if camera_info.fov <= 0:
        rospy.logwarn("FOV is non-positive or zero. Adjust the camera settings.")
        fx = size[0] / 2.0
        fy = size[1] / 2.0
    else:
        fx = size[0] / (2 * np.tan(camera_info.fov / 2.0))
        fy = size[1] / (2 * np.tan(camera_info.fov / 2.0))
        
    cx = size[0] / 2
    cy = size[1] / 2
    
    cam_info_msg.K = [
        fx, 0, cx,
        0, fy, cy,
        0, 0, 1
    ]
    cam_info_msg.P = [
        fx, 0, cx, 0,
        0, fy, cy, 0,
        0, 0, 1, 0
    ]
    
    cam_info_msg.distortion_model = 'plumb_bob'  # Modelo típico de distorsión
    cam_info_msg.D = [0, 0, 0, 0, 0]  # Parámetros de distorsión (k1, k2, t1, t2, k3)
    cam_info_msg.R = [1, 0, 0, 0, 1, 0, 0, 0, 1]  # Matriz de rotación (identidad aquí)
    cam_info_msg.binning_x = 0
    cam_info_msg.binning_y = 0
    cam_info_msg.roi.x_offset = 0
    cam_info_msg.roi.y_offset = 0
    cam_info_msg.roi.height = 0
    cam_info_msg.roi.width = 0
    cam_info_msg.roi.do_rectify = False
}

I also tried searching for the log files mentioned in the error, but I couldn't find any log files corresponding to RTAB-Map, so I couldn't get a more detailed breakdown of the error.

Here is my launch file:

<launch>
  <arg name="rviz" default="true" />
  <arg name="rtabmap" default="true" />
  <arg name="rtabmap_viz" default="true" />
  <arg name="gnome" default="false" />

  <arg unless="$(arg gnome)" name="gnome_terminal" default=""/>
  <arg if="$(arg gnome)" name="gnome_terminal" default="gnome-terminal -- bash -c "/>

  <rosparam command="load" file="$(find drone_slam_simulation)/config/params.yaml"/>
  
  <node if="$(arg rtabmap)" pkg="rtabmap_slam" type="rtabmap" name="rtabmap" output="screen"  args="--delete_db_on_start udebug">
    <param name="frame_id" type="string" value="base_link"/>

    <!-- <param name="subscribe_rgbd" value="true"/> -->
    <param name="subscribe_rgb" value="true"/>
    <param name="subscribe_depth" value="true"/>
    <param name="subscribe_odom" value="true"/>

    <remap from="/rgb/camera_info" to="/airsim/image/camera_info"/>
    <remap from="/odom_info" to="/airsim/odom_info"/>
    <remap from="/odom" to="/airsim/odom"/> 
    <remap from="/depth/image" to="/airsim/image/depth"/>
    <remap from="/rgb/image" to="/airsim/image/rgb"/>

    <param name="gdb" value="true"/>
    <param name="StatisticLogged" value="true"/>
    <param name="approx_sync" value="true"/>
    <param name="sync_queue_size" value="100"/>
    <param name="topic_queue_size" value="100"/>
  </node>

  <node if="$(eval arg('rtabmap') == true and arg('rtabmap_viz') == true)" pkg="rtabmap_viz" type="rtabmap_viz" name="rtabmap_viz" output="screen">
    <remap from="/odom" to="/airsim/odom"/>
    <param name="frame_id" value="camera_link"/>
    <param name="sync_queue_size" value="50"/>
    <param name="topic_queue_size" value="50"/>
  </node>

  <node if="$(arg rviz)" pkg="rviz" type="rviz" name="rviz" output="screen" args="-d $(find drone_slam_simulation)/config/topics.rviz"/>

  <node pkg="rqt_graph" type="rqt_graph" name="rqt_graph" required="false"/>

  <node pkg="drone_slam_simulation" type="move_dron.py" name="move_dron" output="screen" launch-prefix="$(arg gnome_terminal)"/>
  <node pkg="drone_slam_simulation" type="publish_topics.py" name="publish_topics" output="screen" launch-prefix="$(arg gnome_terminal)"/>
  
</launch>

You can find the repository for my project here, in case you want to look at any specific files in more detail. I will be actively monitoring this thread for any questions.

[matlabbe]

Can you record and share a small rosbag of your data?

rosbag record /tf /tf_static /airsim/image/camera_info /airsim/odom /airsim/image/depth /airsim/image/rgb

[Manuelo247]

Here is the rosbag file: https://drive.google.com/file/d/1n-h2SiTNGSChCZ1TSa-apHJSA2qX5a9k/view?usp=sharing

[matlabbe]

I could not reproduce the crash. You built rtabmap from source, if you have more than one opencv installed, that may be the cause of the crash. Try rtabmap_ros binaries.



Otherwise than that, the depth image looks wrong. Here a closer look:



1. The grid I show is 50 meters x 50 meters, so there is clearly a scale issue in depth values
2. The flat surfaces look curvy in the point cloud, but not in the image, maybe a mismatch between the output calibration values and the ones actually used by the simulator to generate the depth image.
3. The camera is looking up, but frame_id is set to base_link. It means there is a missing optical rotation (RPY: -1.57 0 -1.57) between base_link and the frame_id inside the image.

You can debug these kind of stuff just with RVIZ by using DepthCloud display (the grid there is 10x10 meters):

[Manuelo247]

Thank you very much for your response and your help! With RViz, I was able to visualize that there was something strange with the depth image, but since I still haven't managed to run RTAB-Map, I haven't been worrying about that aspect yet. Thanks for the clarifications; once I get to that part, I will try what you mentioned.

Regarding the original error, I have resolved the cv::Exception. It seems that the dependencies were not downloaded correctly, so I simply re-ran sudo apt install ros-$ROS_DISTRO-rtabmap* to install them. Additionally, I added the line find_package(OpenCV 4 REQUIRED) in my CMakeLists.txt to ensure that it uses the version of OpenCV I have installed. Unfortunately, I still haven't managed to run RTAB-Map. When I try to run it along with the rosbag, I see the following:

[ INFO] [1722532010.979733391, 1722528810.284685780]: rtabmap 0.21.5 started...
[ WARN] (2024-08-01 11:06:51.160) util3d.cpp:606::cloudFromDepthRGB() Cloud with only NaN values created!
[ INFO] [1722532011.168262229, 1722528810.466757323]: rtabmap (1): Rate=1.00s, Limit=0.000s, Conversion=0.0074s, RTAB-Map=0.0324s, Maps update=0.0024s pub=0.0015s (local map=1, WM=1)
[ INFO] [1722532012.221922422, 1722528811.519834617]: rtabmap (2): Rate=1.00s, Limit=0.000s, Conversion=0.0004s, RTAB-Map=0.0384s, Maps update=0.0002s pub=0.0000s (local map=2, WM=2)
[ INFO] [1722532013.312328784, 1722528812.611937877]: rtabmap (3): Rate=1.00s, Limit=0.000s, Conversion=0.0001s, RTAB-Map=0.0168s, Maps update=0.0000s pub=0.0000s (local map=3, WM=3)
[ WARN] (2024-08-01 11:06:54.301) util3d.cpp:606::cloudFromDepthRGB() Cloud with only NaN values created!
[ INFO] [1722532014.302363932, 1722528813.602527825]: rtabmap (4): Rate=1.00s, Limit=0.000s, Conversion=0.0001s, RTAB-Map=0.0062s, Maps update=0.0001s pub=0.0001s (local map=4, WM=4)

I am running RTAB-Map as follows:

/home/manuelo247/catkin_ws/devel/lib/rtabmap_slam/rtabmap /rgb/camera_info:=/airsim/image/camera_info /odom_info:=/airsim/odom_info /odom:=/airsim/odom /depth/image:=/airsim/image/depth /rgb/image:=/airsim/image/rgb __name:=rtabmap --delete_db_on_start

This is what I see in RTAB-Map, and it closes after a few seconds.

[matlabbe]

These is kind of something in the 3D view, you may have to zoom out to see everything. As I said, there is a missing optical rotation for the image topic, so the cloud appears up instead of in front of the frame.

it closes after a few seconds.

is there an error on the terminal?

[Manuelo247]

I'm sorry for the delayed response, i was able to resolve some issues that were hindering my progress.
I managed to fix the error you mentioned regarding the distorted data. This issue was due to an error in the publication of the depth image, where the data was not being converted correctly. Here are the new results.



The point cloud seems to be generated correctly based on my tests, but when sending this data to RTAB-Map, it still crashes.
I'm not sure if this could be due to the way I'm executing it or if I'm missing a dependency. What I get is an image like the one in the previous message with the same errors. Additionally, when interacting with the 3D map, it closes and gives me the message that the process died, which I'll share below.

[ WARN] (2024-08-12 22:09:32.575) Rtabmap.cpp:4560::process() Republishing data of requested node(s) 2 1 (Rtabmap/MaxRepublished=2)
[ WARN] (2024-08-12 22:09:42.306) util3d.cpp:606::cloudFromDepthRGB() Cloud with only NaN values created!
[ INFO] [1723522182.308177993]: rtabmap (13): Rate=1.00s, Limit=0.000s, Conversion=0.0003s, RTAB-Map=0.0341s, Maps update=0.0000s pub=0.0000s (local map=13, WM=13)
[rtabmap_viz-7] process has died [pid 14562, exit code -11, cmd /opt/ros/noetic/lib/rtabmap_viz/rtabmap_viz /odom:=/airsim_node/drone_1/odom_local_ned __name:=rtabmap_viz __log:=/home/manuelo247/.ros/log/d3fde6ba-5929-11ef-adb7-c57683ed6876/rtabmap_viz-7.log].
log file: /home/manuelo247/.ros/log/d3fde6ba-5929-11ef-adb7-c57683ed6876/rtabmap_viz-7*.log

I've included a small rosbag with my data and, lastly, my launch file.

<launch>
  <arg name="rviz" default="true" />
  <arg name="rtabmap" default="true" />
  <arg name="rtabmap_viz" default="true" />
  <arg name="rqt_graph" default="true" />
  <arg name="gnome" default="false" />
  <arg name="oldproject" default="false" />

  <arg unless="$(arg gnome)" name="gnome_terminal" default=""/>
  <arg if="$(arg gnome)" name="gnome_terminal" default="gnome-terminal -- bash -c "/>

  <rosparam command="load" file="$(find drone_slam_simulation)/config/params.yaml"/>
  <include unless="$(arg oldproject)" file="$(find airsim_ros_pkgs)/launch/airsim_node.launch"/>
  <include unless="$(arg oldproject)" file="$(find airsim_tutorial_pkgs)/launch/front_stereo_and_center_mono/depth_to_pointcloud.launch"/> 
  <include if="$(arg oldproject)" file="$(find drone_slam_simulation)/launch/rtabmap_depth_to_pointcloud.launch"/>

  <node pkg="rtabmap_slam" type="rtabmap" name="rtabmap" output="screen"  args="--delete_db_on_start udebug" if="$(arg rtabmap)">
    <param name="frame_id" type="string" value="front_left_custom_optical"/>

    <!-- <param name="subscribe_rgbd" value="true"/> -->
    <param name="subscribe_rgb" value="true"/>
    <param name="subscribe_depth" value="true"/>
    <param name="subscribe_odom" value="true"/>

    <remap from="/rgb/camera_info" to="/airsim_node/drone_1/front_left_custom/Scene/camera_info"/>
    <!-- <remap from="/odom_info" to="/airsim/odom_info"/> -->
    <remap from="/odom" to="/airsim_node/drone_1/odom_local_ned"/> 
    <remap from="/depth/image" to="/airsim_node/drone_1/front_left_custom/DepthPerspective"/>
    <remap from="/rgb/image" to="/airsim_node/drone_1/front_left_custom/Scene"/>

    <param name="approx_sync" value="true"/>
    <param name="sync_queue_size" value="50"/>
    <param name="topic_queue_size" value="50"/>
  </node>

  <node pkg="rtabmap_viz" type="rtabmap_viz" name="rtabmap_viz" output="screen" if="$(eval arg('rtabmap') == true and arg('rtabmap_viz') == true)">
    <remap from="/odom" to="/airsim_node/drone_1/odom_local_ned"/>
    <param name="frame_id" value="front_left_custom_optical"/>
    <param name="sync_queue_size" value="50"/>
    <param name="topic_queue_size" value="50"/>
  </node>


  
  <node if="$(eval arg('rviz') == true and arg('oldproject') == false)" pkg="rviz" type="rviz" name="rviz" output="screen" args="-d $(find drone_slam_simulation)/config/topics.rviz"/>
  <node if="$(eval arg('rviz') == true and arg('oldproject') == true)" pkg="rviz" type="rviz" name="rviz" output="screen" args="-d $(find drone_slam_simulation)/config/topics_old.rviz"/>

  <node pkg="rqt_graph" type="rqt_graph" name="rqt_graph" required="false" if="$(arg rqt_graph)"/>

  <node pkg="drone_slam_simulation" type="move_dron.py" name="move_dron" output="screen" launch-prefix="$(arg gnome_terminal)"/>
  <node if="$(arg oldproject)" pkg="drone_slam_simulation" type="publish_topics.py" name="publish_topics" output="screen" launch-prefix="$(arg gnome_terminal)"/>
  
  

</launch>

[matlabbe]

The rosbag link seems not working. For the crash, is it just rtabmap_viz crashing? maybe there is a GPU issue for some reason on that machine. If you can use rviz instead, that could unblock you.

[Manuelo247]

Here is the rosbag again. I made some changes, such as adding the following line to my launcher:

  <node pkg="tf2_ros" type="static_transform_publisher" name="base_link_to_map" args="0 0 0 0 0 0 front_left_custom_optical map" />

As you suggested, I was able to fully visualize the data with RViz. However, I must be publishing something incorrectly or using RTAB-Map the wrong way because it looks like this:
The points from the map generated by RTAB-Map appear in the wrong places, and they are published very sporadically, never in the same location.

[matlabbe]

Sorry to be a bummer, but the camera_info topic is missing in the bag.

[Manuelo247]

You are absolutely right, my apologies. Here is another rosbag with the complete data.

[matlabbe]

1) don't add:

<node pkg="tf2_ros" type="static_transform_publisher" name="base_link_to_map" args="0 0 0 0 0 0 front_left_custom_optical map" />

2) it seems that the depth topic is better, though either the scale is wrong, or the world is very huge.

3) the TF tree doesn't look right. There is no base frame on the drone and from_left_custom_optical looks upside down. The drone_1 frame is actually a fixed frame, not the base frame:


4) The environment doesn't have a lot of visual textures, so visual odometry cannot work properly. I made this launch using the TF between drone_1 and from_left_custom_optical as odometry:

roslaunch rtabmap_launch rtabmap.launch \
   args:="-d" \
   use_sim_time:=true \
   rgb_topic:=/airsim_node/drone_1/front_left_custom/Scene \
   depth_topic:=/airsim_node/drone_1/front_left_custom/DepthPlanar \
   camera_info_topic:=/airsim_node/drone_1/front_left_custom/Scene/camera_info \
   odom_frame_id:=drone_1 \
   frame_id:=front_left_custom_optical \
   visual_odometry:=false

rosbag play --clock 2024-08-15-21-37-21.bag

[Manuelo247]

I removed the static TF line, which I had added because there was a message indicating no connection between them. Instead, I changed the world name from world_ned to map, so I no longer see an error related to that.

As for the scale, there's no issue; it's the world's scale, so I'm not concerned about it.

Regarding the TF tree, I had to add some rotations to the camera to visualize it correctly because otherwise, the point cloud projection would move erratically. However, I couldn't fix the issue of it being upside down, so I added a rotation to drone_1 to correct this and ensure the projection looked right. Could this be causing problems? Here are the lines I added:
"front_left_custom_optical"

    cam_tf_body_msg.transform.rotation.x = img_response.camera_orientation.x();
    cam_tf_body_msg.transform.rotation.y = img_response.camera_orientation.y();
    cam_tf_body_msg.transform.rotation.z = img_response.camera_orientation.z();
    cam_tf_body_msg.transform.rotation.w = img_response.camera_orientation.w();

    tf2::Quaternion quat_cam_body;
    tf2::convert(cam_tf_body_msg.transform.rotation, quat_cam_body);

    tf2::Quaternion additional_rotation;
    additional_rotation.setRPY(-1.57, 3.14, -1.57);

    tf2::Quaternion final_rotation = quat_cam_body * additional_rotation;
    cam_tf_body_msg.transform.rotation.x = final_rotation.x();
    cam_tf_body_msg.transform.rotation.y = final_rotation.y();
    cam_tf_body_msg.transform.rotation.z = final_rotation.z();
    cam_tf_body_msg.transform.rotation.w = final_rotation.w();

"drone_1"

    vehicle_tf_msg.transform.translation.x = vehicle_setting.position.x();
    vehicle_tf_msg.transform.translation.y = vehicle_setting.position.y();
    vehicle_tf_msg.transform.translation.z = vehicle_setting.position.z();

    tf2::Quaternion quat;
    quat.setRPY(3.14, 0, 0);
    vehicle_tf_msg.transform.rotation.x = quat.x();
    vehicle_tf_msg.transform.rotation.y = quat.y();
    vehicle_tf_msg.transform.rotation.z = quat.z();
    vehicle_tf_msg.transform.rotation.w = quat.w();

I no longer have many visible errors in the terminal, just the ones that were already appearing before.

[ WARN] (2024-09-02 07:33:13.761) util3d.cpp:606::cloudFromDepthRGB() Cloud with only NaN values created!
[ WARN] (2024-09-02 07:33:13.871) Rtabmap.cpp:2796::process() Ignoring local loop closure with 73 because resulting transform is too large!? (53.385418m > 1.000000m)
[ WARN] (2024-09-02 07:33:13.918) Rtabmap.cpp:4560::process() Republishing data of requested node(s) 73 23 (Rtabmap/MaxRepublished=2)

Despite all this, I don't see RTAB-Map publishing anything, even though messages are being published to the topics and RViz is reading those messages. Also, RViz shows that a map is being generated, but as I mentioned, I'm not seeing anything that's being generated.


Here is the rosbag.

[matlabbe]

Hi,

To visualize in rviz with MapCloud display, you should set "Cloud max depth" to 0 and set "Cloud voxel size" to 0.

To fix your TF tree issues, try to get frames in ROS standard (see REP105), because it super hard to understand each other. Below is the current TF and the expected one based on ROS standard frames. Change "drone_1" to "odom" and make it x=forward, y=left, z=up. The base frame on the drone on which odom is attached should be in ROS coordinates x=forward, y=left and z=up (like odom and map frames). For the frame of the depth/rgb image topics, create a static transform of ypr=-Pi/2 0 -Pi/2 to link base_link to front_left_custom_optical.

rtabmap sohuld then use "base_link" as frame_id and "odom" as odom_frame_id. The result MapCloud will then appear correctly. Note that rtabmap will publish map->odom, you should not publish it yourself as fixed transform.