private void DepthScanToXYZ(int[] depth, ros.sensor_msgs.CameraInfo info, out double[] x, out double[] y, out double[] z)
{
x = new double[depth.Length];
y = new double[depth.Length];
z = new double[depth.Length];
int[] u;
int[] v;
DepthScanToXYZ(depth, info, out x, out y, out z, out u, out v);
}
private void DepthScanToXYZ(int[] depth, ros.sensor_msgs.CameraInfo info, out double[] x, out double[] y, out double[] z, out int[] u, out int[] v)
{
x = new double[depth.Length];
y = new double[depth.Length];
z = new double[depth.Length];
u = new int[depth.Length];
v = new int[depth.Length];
// Get z values
for (int i = 0; i < depth.Length; i++)
{
z[i] = depth[i] * 0.001;
// Set 0 values to 'far away'
if (depth[i] == 0)
{
z[i] = 10;
}
}
// Get x y values (intrinsic camera matrix)
Debug.Log(info);
Debug.Log(info.width);
Debug.Log(info.K);
double u0 = info.K[2];
double v0 = info.K[5];
double fxinv = 1.0 / info.K[0];
double fyinv = 1.0 / info.K[4];
int width = (int)info.width;
for (int i = 0; i < z.Length; i++)
{
int ui = (i % width);
int vi = (i / width);
u[i] = ui;
v[i] = vi;
x[i] = z[i] * (ui - u0) * fxinv;
y[i] = z[i] * (vi - v0) * fyinv;
}
}
