Required Math to convert depth data into correct XYZ values for creating point cloud

[Nadia-D]

Hi,

My name is Nadia. Recently I have been using Intel Realsense D435i to scan an agricultural field. I have been collecting data with ROS to create a rosbag of the scanned area. Then I open the rosbag in MatLab to create the point cloud of each collected frame, register those point clouds and finally generate a 3D scan of the field. 

However, I have been struggling with registering these point clouds together over the last 2 months. The created point clouds do not register on top of each other correctly. This the method that I use to generate the XYZ data for my point clouds from depth information.

1. Projecting depth information from image coordinate to camera coordinate using the intrinsic matrix (K), from /camera/depth/camera_info' topic:

Xc = [Depth*(Xim-cx)]/fx

Yc = [Depth*(Yim-cy)]/fy

Zc = Depth

2. Since to be able to register the point clouds correctly I need to have the XYZ values in the World coordinate system, I need to project the created (Xc, Yc, Zc) to world coordinate, however, I am not sure exactly which rotation and translation matrices under which topic from rosbag should be used to do this projection correctly and finally register the point clouds. I desperately looking for any help or hints. I am getting POSE (position and orientation) information of the camera from '/rtabmap/odom' topic. Also, Transformation between two frames from '/rtabmap/odominfo'. But I am not sure which data should be used to get the correct XYZ values.

Also, I am wondering if I can directly get the point cloud data from ROS in the correct coordinate or not? Because I believe all of these math that I am doing do not make sense and the sensor is supposed to generate the correct output for me.

Thanks for your time in advance.

Regards,

Nadia.