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

[FApon]

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;

}

[MartyG]

RealSense expert Samontab published a YouTube video about getting raw camera data into OpenCV. I don't know if it will be useful to your particular issue but it may be worth a look.

https://www.youtube.com/watch?v=wIkIdjN6Oyw Accessing data from the Intel RealSense F200 - YouTube

[FApon]

Hello MartyG

Thank you for your reply, but I saw that video at the beginning of everything (maybe 4 months ago ), it was really helpful because it gave me the relationship between FPS and what I was able to stream.

Unfortunately that information is not useful anymore, it is way different having the depth raw image from having the distance and the world coordinates.

Thanks any way

Kind regards

Fabian

[MartyG]

I did some further research. In this link, a person gets the world coordinates from the image coordinates in OpenCV with a non-RealSense camera using a function called solvePnP.

http://answers.opencv.org/question/62779/image-coordinate-to-world-coordinate-opencv/ image coordinate to world coordinate opencv - OpenCV Q&A Forum

And on another YouTube video, somebody gets coordinates in mm using OpenCV's triangulatePoints function. The useful point on this video is the detailed text description's explanation of the process under 'Show More', not the video itself.

https://www.youtube.com/watch?v=FrYBhLJGiE4 OpenCV triangulate points - YouTube

This next video is about getting the image coordinates, which isn't what you asked for. But it has a link to source code that still works, and in the comments the author of the video says to someone who asked about world coordinates that they can get them with the same script by changing the script to use camera capture.

https://www.youtube.com/watch?v=TYeNGnAQNt0 opencv tutorial:program to find the coordinates of a pixel on an image - YouTube

That was all I was able to find that seemed relevant. Sorry I could not be of more help in this case.

[jb455 · Accepted solution]

So, you want to know how to get the vertex for a specific pixel in the image, right?

If the pixel you're interested in is in the depth image, that's easy as the vertices array is aligned to the depth image by default. For a point (u,v) in the depth image, the corresponding world point is v = vertices[u + v * depthImage.info.width].

If it's in the colour image, it's a bit more complicated. You'll first have to map the colour points to the depth image. There are a couple of ways to do this, depending on your situation. The easiest is probably by using https://software.intel.com/sites/landingpage/realsense/camera-sdk/v1.1/documentation/html/index.html?mapcolortodepth_pxcprojection.html MapColourToDepth, but this is only efficient if you have a "small number" of pixels you want to map. If you need to map anything approaching the whole image and you're at all concerned about performance, it'd be best to use either https://software.intel.com/sites/landingpage/realsense/camera-sdk/v1.1/documentation/html/index.html?projectcolortocamera_pxcprojection.html ProjectColourToCamera or http://https//software.intel.com/sites/landingpage/realsense/camera-sdk/v1.1/documentation/html/index.html?queryinvuvmap_pxcprojection.html QueryInvUVMap. Once you have your colour pixel mapped to the depth image, the case is reduced to the depth image method above.

Note, for help with any of the methods linked, search this forum/google for the method name and you should find some snippets to get you started.

[idata]

Hello fhfonsecaa,

Thank you for your interest in the Intel® RealSense™ Technology.I was wondering if you could check the suggestion provided by jb455.If you have any other question or update, don't hesitate to contact us.Regards,Andres V.