Project Tango: Depthmap Transformation from XYZij data

Question

I'm currently trying to filter the depth information using OpenCV. For that reason I need to transform Project Tango's depth information XYZij into a image like depthmap. (Like the output of Microsoft Kinect) Unfortunately the official APIs lacking the ij part of XYZij. That's why I'm trying to project the XYZ part using the camera intrinsics projection, wich is explained in the official C API Dokumentation. My current approach looks like this:

    float fx = static_cast<float>(ccIntrinsics.fx);
    float fy = static_cast<float>(ccIntrinsics.fy);
    float cx = static_cast<float>(ccIntrinsics.cx);
    float cy = static_cast<float>(ccIntrinsics.cy);

    float k1 = static_cast<float>(ccIntrinsics.distortion[0]);
    float k2 = static_cast<float>(ccIntrinsics.distortion[1]);
    float k3 = static_cast<float>(ccIntrinsics.distortion[2]);

    for (int k = 0; k < xyz_ij->xyz_count; ++k) {

        float X = xyz_ij->xyz[k][0];
        float Y = xyz_ij->xyz[k][1];
        float Z = xyz_ij->xyz[k][2];

        float ru = sqrt((pow(X, 2) + pow(Y, 2)) / pow(Z, 2));
        float rd = ru + k1 * pow(ru, 3) + k2 * pow(ru, 5) + k3 * pow(ru, 7);

        int x = X / Z * fx * rd / ru + cx;
        int y = X / Z * fy * rd / ru + cy;

        // drawing into OpenCV Mat in red
        depth.at<cv::Vec3b>(x, y)[0] = 240;

    }

The resulting depthmap can be seen in the lower right corner. But it seems that this calculation result in a linear representation ... Does anyone has already done something similar? Are the XYZ points already correct positioned for this projection?

Answer 1

I have actually found a solution ... Just skipped the distortion calculation like they do in the rgb-depth-sync-example. My code now looks like this:

    float fx = static_cast<float>(ccIntrinsics.fx);
    float fy = static_cast<float>(ccIntrinsics.fy);
    float cx = static_cast<float>(ccIntrinsics.cx);
    float cy = static_cast<float>(ccIntrinsics.cy);

    int width = static_cast<int>(ccIntrinsics.width);
    int height = static_cast<int>(ccIntrinsics.height);

    for (int k = 0; k < xyz_ij->xyz_count; ++k) {

        float X = xyz_ij->xyz[k * 3][0];
        float Y = xyz_ij->xyz[k * 3][1];
        float Z = xyz_ij->xyz[k * 3][2];

        int x = static_cast<int>(fx * (X / Z) + cx);
        int y = static_cast<int>(fy * (Y / Z) + cy);

        uint8_t depth_value = UCHAR_MAX - ((Z * 1000) * UCHAR_MAX / 4500);

        cv::Point point(y % height, x % width);
        line(depth, point, point, cv::Scalar(depth_value, depth_value, depth_value), 4.5);

    }

And the working OpenCV result looks like this: