5 Replies Latest reply on May 10, 2017 4:58 PM by Intel Corporation

    How to get the world coordinates from 3D image for OpenCV?

    fhfonsecaa

      Hey guys, it is me again. I finally managed to construct the world coordinates image using the QueryVertices function, but I really do not get it how to have access to the distance in mm.

       

      I have read lots of forums and the documentation, which says I need to give the location (x,y) to the vertices matrix and then I get the distance in mm (I am not sure if it is x,y position in the world's space or x,y pixel's position). Nevertheless, neither I find that matrix nor I know how to construct it.

       

      At the end of every thing, I only need the whole world coordinates (x,y,z) in the openCV's coordinate system.

       

      Again, I am working with OpenCV,  the R200 camera and VisualStudio C++

       

      PXCImage* R200Driver::DepthToWorldByQueryVertices() {
        PXCImage *depth = frameDepth;
        PXCPoint3DF32 ligKht = {};
      
      
        if (!drawVertices || !projection) return 0;
      
      
        PXCImage::ImageInfo drawVerticesInfo = drawVertices->QueryInfo();
        PXCImage::ImageData drawVerticesDat;
        if (PXC_STATUS_NO_ERROR > drawVertices->AcquireAccess(PXCImage::ACCESS_WRITE, drawVerticesInfo.format, &drawVerticesDat))
        return 0;
      
      
        /* Retrieve vertices */
        float brightness = 200.f;
        pxcStatus sts = projection->QueryVertices(depth, &vertices[0]);
        if (sts >= PXC_STATUS_NO_ERROR) {
        PXCImage::ImageInfo dinfo = depth->QueryInfo();
        pxcBYTE* pdrawVerticesDat = drawVerticesDat.planes[0];
        int hist, num;
        float avg, nz1, nz2;
        for (pxcI32 y = 0; y < dinfo.height - 1; y++) {
        for (pxcI32 x = 0; x < dinfo.width - 1; x++) {
        pdrawVerticesDat[4 * x] = pdrawVerticesDat[4 * x + 1] = pdrawVerticesDat[4 * x + 2] = 0;
        PXCPoint3DF32 *vertex = &vertices[y * dinfo.width + x];
        if (vertex->z) {
        hist = 255;
        nz1 = (vertex[0].z - vertex[1].z) + (vertex[dinfo.width].z - vertex[dinfo.width + 1].z);
        nz2 = (vertex[0].z - vertex[dinfo.width].z) + (vertex[1].z - vertex[dinfo.width + 1].z);
        if (nz1 > 1 || nz2 > 1) {
        hist = (int)(brightness / sqrt(abs(nz1) + abs(nz2)));
        if (hist > 255) hist = 255;
        if (hist < 0) hist = 0;
        }
        pdrawVerticesDat[4 * x] = pdrawVerticesDat[4 * x + 1] = pdrawVerticesDat[4 * x + 2] = hist;
        }
        pdrawVerticesDat[4 * x + 3] = MAXBYTE;
        }
        pdrawVerticesDat += drawVerticesDat.pitches[0];
        }
        }
        drawVertices->ReleaseAccess(&drawVerticesDat);
        return drawVertices;
      }