Re: rtabmap_ros outdoor 3D mapping

Posted by acp on
URL: http://official-rtab-map-forum.206.s1.nabble.com/rtabmap-ros-outdoor-3D-mapping-tp6104p6347.html

Hi Mathew.

I have used Kalibr  to calibrate the imu topic /mavros/imu/data  from the pixhawk 4 mini  and the  /stereo_camera/left/image_mono /stereo_camera/right/image_mono from  the zed camera. I have gotten the following two rotational matrices:

"T_cam_imu: rostopic: /stereo_camera/left/image_mono
  - [-0.01172298941818703, -0.9917474694886735, 0.12766960594406557, 0.0009733116714396002]
  - [-0.2241398884968312, -0.12182314042441611, -0.9669128362172881, -0.001738025514640773]
  - [0.9744864708674659, -0.039950960188021634, -0.22086203584212016, 0.0017137757927055978]
  - [0.0, 0.0, 0.0, 1.0]"

T_cam_imu: rostopic: /stereo_camera/right/image_mono
  - [-0.0030002316421554875, -0.9920830562172235, 0.12554763309910266, -0.11063708293669737]
  - [-0.22017184308270676, -0.1218120127551974, -0.9678254972163587, -0.0018464550183625145]
  - [0.9754564870478116, -0.030545754454934293, -0.21806329072340788, 0.0019577491538307953]
  - [0.0, 0.0, 0.0, 1.0]

I have used the cam_imu_left rotational and the 3D Rotation Converter  to obtain the [xyzw] quaternion angles. Then  I have used the getRPY() ROS function to obtain the roll, pitch, yaw angles  Roll: -3.00242, Pitch: -1.34878, Yaw: -1.58442. Then   I have inserted them  into the zed.urdf file as you can see.

<robot name="zed_camera">


    <link name="zed_camera_center">
        <visual>
            <origin xyz="0 0 0" rpy="0 0 0"/>
            <geometry>
                <mesh filename="package://zed_wrapper/urdf/ZED.stl" />
            </geometry>
            <material name="light_grey">
                <color rgba="0.8 0.8 0.8 0.8"/>
            </material>
        </visual>
    </link>



    <link name="base_link" />


    <joint name="zed_base_link_camera_center_joint" type="fixed">
        <parent link="zed_left_camera_optical_frame"/>
        <child link="base_link"/>
        <origin xyz="0 0 0" rpy="-3.00242 -1.34878 -1.58442 " />
    </joint>


and I have inserted the imu and the Optimizer/GravitySigma as you can see:

<include file="$(find rtabmap_ros)/launch/rtabmap.launch">
      <arg name="rtabmap_args"        value="--delete_db_on_start --Optimizer/GravitySigma 0.3" />
      <arg name="imu_topic"                     value="/mavros/imu/data"/> 


You can also see the two frames in rviz

The question here if what I have done is correct and what do you think?

cheers