SR300Camera::fillInZCoords

/***
Reads the depth data from the sensor and fills in the matrix
***/
void SR300Camera::fillInZCoords()
{

    Image::ImageData depthImage;
    Image *depthMap = sample->depth;
    depthMap->AcquireAccess(Image::ACCESS_READ, &depthImage);
    Image::ImageInfo imgInfo = depthMap->QueryInfo();
    int depth_stride = depthImage.pitches[0] / sizeof(pxcU16);
    Projection * projection = device->CreateProjection();


    unsigned int wxhDepth = depth_width * depth_height;
    // create the array of depth coordinates + depth value (posUVZ) within the defined ROI
    Point3DF32* posUVZ = new Point3DF32[wxhDepth];
    pxcU16 *dpixels = (pxcU16*)depthImage.planes[0];
    unsigned int dpitch = depthImage.pitches[0] / sizeof(pxcU16); /* aligned width */

    for (unsigned int yy = 0, k = 0; yy < depth_height; yy++)
    {
        for (unsigned int xx = 0; xx < depth_width; xx++, k++)
        {
            posUVZ[k].x = (pxcF32)xx*depth_width;
            posUVZ[k].y = (pxcF32)yy *depth_height;
            //posUVZ[k].z = (pxcF32)dpixels[yy * dpitch + xx];
            //posUVZ[k].z = static_cast<pxcF32>(reinterpret_cast<pxcI16*>(depthImage.planes[0]
      //+ static_cast<int>(posUVZ[k].y) * depthImage.pitches[0])[static_cast<int>(posUVZ[k].x)]);
            posUVZ[k].z = static_cast<pxcF32>(reinterpret_cast<pxcI16*>(depthImage.planes[0]
                + static_cast<int>(yy) * depthImage.pitches[0])[static_cast<int>(xx)]);
            cout << "xx is " << posUVZ[k].x << endl;
            cout << "yy is " << posUVZ[k].y << endl;
            cout << "zz is " << posUVZ[k].z<< endl;
        }
    }

    vector<PointF32> depthPoints;
    vector<PointF32> projPoints;



    //CImageGUI::DEPTH_IMG = 1




    // convert the array of depth coordinates + depth value (posUVZ) into the world coordinates (pos3D) in mm
    Point3DF32* pos3D = new Point3DF32[wxhDepth];
    //projection->DepthToWorld(depthMap, depthPoints, projPoints);
    /*
    for (unsigned int yy = 0, k = 0; yy < depth_height; yy++)
    {
        for (unsigned int xx = 0; xx < depth_width; xx++, k++)
        {
            cout << "xx is " << [k] << endl;
            //cout << "yy is " << pos3D[k].y*1000.0 << endl;
            //cout << "zz is " << pos3D[k].z*1000.0 << endl;
            //xyzBuffer.push_back(cv::Point3f(pos3D[k].x, pos3D[k].y, pos3D[k].z));
        }
    }
    */
    int dsize = depth_height*depth_width;
    vector<Point3DF32> dcordsAbs;
    /*
    for (auto i = 0; i < ; i++) {
        dcordsAbs[i].x = dcords[i].x * dwidth, dcordsAbs[i].y = dcords[i].y * dheight;
        dcordsAbs[i].z = static_cast<pxcF32>(reinterpret_cast<pxcI16*>(ddata.planes[0] + static_cast<int>(dcordsAbs[i].y) * ddata.pitches[0])[static_cast<int>(dcordsAbs[i].x)]);
    }
    */
    //depth->ReleaseAccess(&ddata);
    //projection->DepthToWorld(depthMap, depthPoints, pos3D);

    //projection->ProjectDepthToCamera(wxhDepth, posUVZ, pos3D);
    /*
    if (projection->ProjectDepthToCamera(wxhDepth, posUVZ, pos3D) < PXC_STATUS_NO_ERROR)
    {
        delete[] posUVZ;
        delete[] pos3D;
        cout << "projection unsucessful";
        return;
    }
    */
    /*
    for (unsigned int yy = 0, k = 0; yy < depth_height; yy++)
    {
        for (unsigned int xx = 0; xx < depth_width; xx++, k++)
        {
            cout << "xx is " << pos3D[k].x*1000.0 << endl;
            cout << "yy is " << pos3D[k].y*1000.0 << endl;
            cout << "zz is " << pos3D[k].z*1000.0 << endl;
            xyzBuffer.push_back(cv::Point3f(pos3D[k].x, pos3D[k].y, pos3D[k].z));
        }
    }
    */

    /*
    for (int idx = 0; idx < wxhDepth; idx++) {

        cout << "x is " << pos3D[idx].x*1000.0 << endl;
        cout << "y is " << pos3D[idx].y*1000.0 << endl;
        cout << "z is " << pos3D[idx].z*1000.0 << endl;
        xyzBuffer.push_back(cv::Point3f(pos3D[idx].x, pos3D[idx].y, pos3D[idx].z));
    }
    */

    //xyzMap = cv::Mat(xyzMap.size(), xyzMap.type, &pos3D);
    //xyzMap = cv::Mat(xyzBuffer);
    //cout << "xyzMap = " << endl << " " << xyzMap << endl << endl;

    projection->Release();
    delete[] posUVZ;
    delete[] pos3D;

};