[Question] From registered libfreenect2::Frame to pcl::Pointcloud<pcl::PointXYZRGBA>

[Tabjones]

Hello everyone, first of all thanks for the amazing work.

I'm looking at Protonect.cpp source code and i see that after registration->apply(rgb, depth, &undistorted, &registered); (line 203) I should have a color-depth registered image in registered Frame. In fact that same frame gets sent to the viewer for the user to see.

Now what i want to do is convert that frame into a PointCloud class (from pcl library), as far as i know it should contain all the necessary information to construct a point cloud: it has width, height and data.

My question is, how can I access the data pointer to extract euclidean coordinates (x,y,z) and the color associated with each point ?
Are there already some helper functions doing this that i'm missing ?

[xlz]

In principle:

For pixel xi=0..511, yi=0..423, normalized distorted coords (xd, yd) = ((xi+0.5-cx)/fx, (yi+0.5-cy)/fy). Normalized undistorted coords (xu, yu) are obtained from kinect2_undistort(). Then the point in camera coordinates are (X, Y, Z) = (xu*Z, yu*Z, Z=depth[yi*512+xi]). (The undistort function kinect2_undistort() was reverse engineered from the official Kinect SDK. See here #41 (comment). But that function was subsumed by a reverse pixel mapping method in the Registration class.)

To retrieve the color value for pixel xi,yi, use kinect2_depth_to_color(xu, yu, Z, &xc, &yc) to locate and sample the xc,yc point in the rgb image, with nearest, or linear or something else.

I'm also trying to figure what Registration::apply() does. It suddenly seems to be something not what I think it is.

undistorted returned by apply() is a rectified depth image. The above has shown how to directly derive (X,Y,Z) from distorted image. I don't think you have to use undistorted to construct the point cloud, as it loses some information from the originally distorted depth image. I guess the point of undistorted depth images is they are efficient data representation of a point cloud compared to a list of XYZRGB in that X and Y are implicitly coded in the pixel index. So if you want to reconstruct point clouds from saved depth images, you can do it from distorted depth image, apply undistortion, and derive X,Y, or do it from undistorted depth image, and directly derive X,Y. The first way makes it more difficult for later computation. I guess that's why undistorted/rectified depth images are passed around.

If I understand this correctly, for pixel xi=0..511, yi=0..423, (xu, yu) = ((xi+0.5-cx)/fx, (yi+0.5-cy)/fy), then the point coordinates you are looking for is (xu*undistorted[512*yi+xi], yu*undistorted[512*yi+xi], undistorted[512*yi+xi]) and the color is registered[512*yi+xi].

[floe]

@xlz, the second part of your answer is a better explanation IMHO; main reasoning for having undistorted images is that a) they are better suited to purely image-based processing, e.g. depth filtering, and b) they are a by-product of color-to-depth mapping anyway. In particular, the depth-mapped color image is also implicitly undistorted/rectified, so if you want the corresponding depth values for a color pixel from there, you need to use undistorted.

@Tabjones This shouldn't actually be very complex to implement, feel free to submit a pull request with a suitable helper function (although we probably won't add a dependency on PCL just for that). Maybe it can be implemented as a header-only template, to be included alongside PCL?

[Tabjones]

@xlz ok it think i get it. Only thing it's not yet clear to me: what are cx,cy and fx,fy ? Some kind of distortion parameters from intrinsic calibration? I should be able to access those from camera parameters?

@floe Yes most definitely, I could PR that, as soon as I grok it correctly. As for the PCL dep, I could avoid that by returning vectors of floats, i.e. the hypothetical helper function would return 4 vectors (coordinates and color). Then the end-user could convert those into whatever he wants and pulls in PCL deps into his program, if he wants. It also probably makes sense to put this function into Registration class. ok?

[Tabjones]

Ok I'm assuming cx,cy and fx,fy are from IrCameraParameters, so to summarize, a snippet would be:

• Extract cx,cy,fx,fy from Camera Parameters
• For xi in depth width:
  • For yi in depth height:
    1. xu = (xi+0.5-cx)/fx
    2. yu = (yi+0.5-cy)fy
    3. X = xu*undistorted[512*yi+xi]
    4. Y = yu*undistorted[512*yi+xi]
    5. Z = undistorted[512*yi+xi]
    6. RGB = registered[512*yi+xi]
  • End for yi
• End for xi

[floe]

Yes, that sounds correct. If you put it inside Registration class as a method, you can directly use depth.cx, depth.cy etc. as the class already has a copy of the camera parameters.

[moonsh]

I am novice at programming and doing same task. I know It's silly question but I can't understand this part X= xu* undistorted[512*yi+ xi]. Is there function called undistorted ? I have searched in Registration class to find way to use undistorted but still I don't know. Could anyone explain about this ? Thank you.

[xlz]

undistorted is the one of the output variables of Registration::apply().

[Tabjones]

@moonsh Both undistorted and registered are two instance of class libfreenect2::Frame and they are output variables of Registration::apply(), as @xlz said.

I called them the same as Protonect.cpp program for clarity, but of course you could call them mickey and mouse in your program (although i would not recommend that)!

The class has a public data member, which is float array, so actually the command would be:

X = xu*undistorted.data[512*yi+xi]

[Tabjones]

I have made a first prototype, although it doesn't work as expected! Look:

The color looks somewhat correct, but point coordinates are not. Maybe there's some funny type castings going on. Or xu, yu computations are wrong.

What is that 0.5 in xu computation? where does it come from?

[xlz]

0.5 specifies pixel centers.

[Tabjones]

I see. I think there's a wrong type casting from unsigned char (from Frame::data) to float (point coordinates).
I used reinterpret_cast<float> (undistorted.data[512*yi+xi]), but maybe that's not the correct way to do it.

[xlz]

That's definitely not correct. It it accessed like this, the same way in Registration::apply():

float *undistorted_data = (float *)undistorted->data;
depth = undistorted_data[512*yi+xi]...

[Tabjones]

Ahhh thanks, i totally missed that.