SR300Camera::fillInZCoords()

/***
Reads the depth data from the sensor and fills in the matrix
***/
void SR300Camera::fillInZCoords()
{
    //int res = pmdGet3DCoordinates(hnd, dists, 3 * numPixels * sizeof(float)); //store x,y,z coordinates dists (type: float*)
    ////float * zCoords = new float[1]; //store z-Coordinates of dists in zCoords
    //xyzMap = cv::Mat(xyzMap.size(), xyzMap.type(), dists);

    /*
    Projection *projection = pp->QueryCaptureManager()->QueryDevice()->CreateProjection();
    std::vector<Point3DF32> vertices;
    vertices.resize(depth_width * depth_height);
    projection->QueryVertices(sample->depth, vertices.data());
    xyzBuffer.clear();
    //for (int i = 0; i < bufferSize.width*bufferSize.height; i++) {
    for (int i=0; i< depth_width*depth_height; i++) {
        //cv::Point3f p;
        //p.x = vertices[i].x;
        //p.y = vertices[i].y;
        //p.z = vertices[i].z;
        //xyzBuffer.push_back(p);
        cout << "x= " << vertices[i].x << endl;
        cout << "y= " << vertices[i].y << endl;
        cout << "z= " << vertices[i].z << endl;

        xyzBuffer.push_back(cv::Point3f(vertices[i].x, vertices[i].y, vertices[i].z));
    }



    xyzMap = cv::Mat(xyzBuffer);
    cout << "xyzMap = " << endl << " " << xyzMap << endl << endl;

    projection->Release();

    */

    PXCImage::ImageData depthImage;
    PXCImage *depthMap = sample->depth;
    depthMap->AcquireAccess(PXCImage::ACCESS_READ, &depthImage);
    PXCPoint3DF32 * pos2d = new PXCPoint3DF32[depth_width*depth_height];
    int depth_stride = depthImage.pitches[0] / sizeof(pxcU16);

    for (int y = 0, k = 0; y < depth_height; y++) {
        for (int x = 0; x < depth_width; x++, k++) {
            pos2d[k].x = (pxcF32)x;
            pos2d[k].y = (pxcF32)y;
            pos2d[k].z = ((short *)depthImage.planes[0])[y* depth_stride + x];
        }
    }


    //Point3DF32 * pos3d = NULL;
    Point3DF32 * pos3d = new Point3DF32[depth_width*depth_height];
    PXCPoint3DF32 * vertices = new PXCPoint3DF32[depth_width * depth_height];
    Projection * projection = device->CreateProjection();
    projection->ProjectDepthToCamera(depth_width*depth_height, pos2d, pos3d);
    cout << "x is " << pos3d->x << endl;
    cout << "y is " << pos3d->y << endl;
    cout << "z is " << pos3d->z << endl;
    /*
    projection->QueryVertices(sample->depth, vertices);

    xyzBuffer.clear();
    for (int i = 0; i< depth_width*depth_height; i++) {
        //cv::Point3f p;
        //p.x = vertices[i].x;
        //p.y = vertices[i].y;
        //p.z = vertices[i].z;
        //xyzBuffer.push_back(p);
        cout << "x= " << vertices[i].x << endl;
        cout << "y= " << vertices[i].y << endl;
        cout << "z= " << vertices[i].z << endl;

        xyzBuffer.push_back(cv::Point3f(vertices[i].x, vertices[i].y, vertices[i].z));
    }



    xyzMap = cv::Mat(xyzBuffer);
    */

}