cv::Exception Airsim implementation

Posted by Manuelo247 on
URL: http://official-rtab-map-forum.206.s1.nabble.com/cv-Exception-Airsim-implementation-tp10281.html

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.