How to calculate corresponding points coordinates with RGBD image?

[StardustLu]

Dear all,
I try to calculate corresponding points coordinates in world coordinate system with RGBD image, but the result is not good. If I understand it right, the pos in airsim should have no error. So what's wrong with my calculation ?

I set intrinsic parameters with #issue269;
I set extrinsic parameters with airsim_rec.txt
here's my calculation codes:

import numpy as np
from scipy.spatial.transform import Rotation

def calPworld(u, v, d, q, t):
    fx = 959.661112
    fy = 959.693478
    cx = 959.385435
    cy = 539.533360
    # X Y Z W
    r_mat = Rotation.from_quat(q)
    R = np.array(r_mat.as_matrix())

    T = np.array([t]).transpose()
    Z = d
    X = (u - cx) * Z / fx
    Y = - (v - cy) * Z / fy
    cp = np.array([[X], [Y], [Z]])
    return np.dot(R.transpose(), cp - T)

where :

• q denotes (Q_X, Q_Y, Q_Z, Q_W) in airsim_rec.txt
• t denotes (POS_X, POS_Y, POS_Z) in airsim_rec.txt
• d denotes depth in depth image
• u,v denotes pixel coordinate in RGB image

here is my settings.json to record the RGBD image (the depth image seems to be fixed in 256 x 144):

{
  "SeeDocsAt": "https://github.com/Microsoft/AirSim/blob/master/docs/settings.md",
  "SettingsVersion": 1.2,
  "SimMode": "ComputerVision",
  "Recording": {
    "RecordInterval": 1,
    "Cameras": [
        { "CameraName": "front_center", "ImageType": 0, "PixelsAsFloat": false, "Compress": true },
        { "CameraName": "front_center", "ImageType": 1, "PixelsAsFloat": true, "Compress": true }
    ]
  },
  "CameraDefaults": {
    "CaptureSettings": [
      {
        "ImageType": 0,
        "Width": 1920,
        "Height": 1080,
        "FOV_Degrees": 90
      },
      {
        "ImageType": 1,
        "Width": 1920,
        "Height": 1080,
        "FOV_Degrees": 90
      }
    ]
  }
}

eg. set a pair of corresponding points: p1(214, 280), p2(654, 280) , and the pos recorded in airsim_rec.txt :

POS_X	POS_Y	POS_Z	Q_W	Q_X	Q_Y	Q_Z
38.0266	-57.7638	-43.7433	0.999962	-3.12201e-09	3.57614e-07	0.00873036	
38.3268	-74.9576	-43.7433	0.999962	-3.12201e-09	3.57614e-07	0.00873036	

I can't find depth metric which seems to be a little different with issue#1054. I test it and find the scale factor seems to be 100. So the depth of these corresponding points are 55.029296875 and 46.2890625.
But the results seem to be far away from each other: P1(-79.48, 74.04, 98.77), P2(-51.52, 88.38, 90.03)
What's wrong with my calculation?

[jonyMarino]

Hi @StardustLu! Are you sure you have the correct intrinsic parameters? Did you run the experiment posted here? I think now you can set the intrinsic parameters through our API, take a look at #3949