How to downsample a PCLPointCloud2 keeping it organized

classic Classic list List threaded Threaded
4 messages Options
Reply | Threaded
Open this post in threaded view
|

How to downsample a PCLPointCloud2 keeping it organized

dhindhimathai
Hi everyone,

I'm trying to downsample a *pcl::PCLPointCloud2 points* that I get from a
Kinect One (which is an organized point cloud: points.width = 512 and
points.height = 424) to 320x240. By downsampling using a *pcl::VoxelGrid*
the resulting downsampled point cloud is no more organized
(output_points.height = 1) but I need it to be organized for fast Normal
Estimation using Integral Images. I can't change anything in the module of
pointcloud elaboration and I really need to downsample.

Is there any way to downsample preserving the organized structure or at
least is it possible to re-organize the unorganized output of the VoxelGrid?

Thank you in advance.



--
Sent from: http://www.pcl-users.org/
_______________________________________________
[hidden email] / http://pointclouds.org
http://pointclouds.org/mailman/listinfo/pcl-users
Reply | Threaded
Open this post in threaded view
|

Re: How to downsample a PCLPointCloud2 keeping it organized

Sérgio Agostinho
After having a look at the files in the filter module, I don't think
there's anything which performs what you need i.e., image and point
cloud resizing/resampling, preserving its organized structure.

Cheers




_______________________________________________
[hidden email] / http://pointclouds.org
http://pointclouds.org/mailman/listinfo/pcl-users

signature.asc (836 bytes) Download Attachment
Reply | Threaded
Open this post in threaded view
|

Re: How to downsample a PCLPointCloud2 keeping it organized

Stephen McDowell
Currently, you are trying to do this:

1. Acquire a range image which is 512x424
2. Create a point cloud (convert to cartesian coordinates)
3. Down-sample.

(3) cannot be done this way.  What you want to do is an “image pyramid”, only I’ll note that acquiring 320x240 using an image pyramid is not typical.  It can be done, just not normal to do.  Instead, what you would typically do is

1. Acquire a range image which is 512x424
2. Down-sample this image using a gaussian filter into an 256x212 image.  Sometimes you’ll also see people combine the bilateral and gaussian filters, this is particularly relevant for range images since the bilateral filter helps reduce noise while still preserving edges.
3. Create a point cloud by converting to cartesian coordinates, from the down-sampled image.

- - - - - -
I am including everything you need here, but it’s kind of long.  Note that I frequently mess-up width x height indexing vs row x col indexing, so definitely double check the (uncompiled) code I am writing here, you may need to switch things!
- - - - - -

Even if you are not using CUDA, you can still convert this into usable code:


Above that in the same file there is also a variant that both down-samples and uses a bilateral filter.  I’ll just be walking through down-sampling.

Without going through all of the code, what you need to pay attention to when adapting the code to your purposes is the indexing.  I will also briefly explain how you would write it in CPU code in case you do not know CUDA syntax.

    pyrDownKernel (const PtrStepSz<ushort> src, PtrStepSz<ushort> dst, float sigma_color)

`src` here is the image data being filtered down, it is 512x424 in your case.  `dst` is where the down-sampled image is getting stored, it would be 256x212 in your case.  The PtrStepSz is a construct for using STL-like containers in CUDA instead of managing data memory manually, the important note about indexing:

    int center = src.ptr (2 * y)[2 * x];

since our source image is two times the size of the destination image, that’s why this index is used.  If you were using a hand-coded one-dimensional array allocated like

    ushort *src = (ushort *)malloc(512 * 424 * sizeof(ushort));

then the one-dimensional indexing for this would be

     int center = src[(2 * y * 512) + (2 * x)];

So now comes how to write this as a CPU loop.  The method linked above, for all intensive purposes, is the inner-most loop body.  So you could think of it as doing

    for every row
        for every col
            innerLoop(row, col, src, dst, sigma_color)

At the beginning of the loop, there is this check that we won’t need to do on the CPU

    if (x >= dst.cols || y >= dst.rows)
        return;

All that is doing is making sure the memory access stays in bounds, because when we launch the “outer loop” in CUDA, more threads than are necessary may be run (it has to do with how CUDA works).  So really, the loop is just

    // NOTE: double check bounds here, this is what I always mess up
    for (int y = 0; y < 212; ++y) {// 212 is dst.rows (the image height)
        for (int x = 0; x < 256; ++x) {// 256 is dst.cols (the image width)
            // inner loop body
        }
    }

Where, in one-dimensional indexing, the line

    dst.ptr (y)[x] = sum / count;

would now be

    dst.ptr[y * 256 + x] = sum / count;

There is another step!!!

Now that you have a down-sampled image, you need to compute the coordinates in 3D space.  However, since we down-sampled to exactly (width / 2) x (height / 2), computing the cartesian coordinates must be done with this in mind.  It’s actually quite straight-forward, you just divide the focal lengths and center point by 2!

I am not familiar with the PCL grabber API, I do this stuff manually.  The classical way of computing the 3D coordinates is as follows (this is just for one coordinate (int x, int y) in a loop):

    ushort depth = range_image[y * width + x];
    float pz = static_cast<float>(depth);// assuming the driver is giving you millimeters (I think it is)
    float px = pz * (static_cast<float>(x) - cx) / fx;
    float py = pz * (static_cast<float>(y) - cy) / fy;
    // set cloud x, y, z for this point

I assume you know how to get the fx, fy, cx, and cy for your camera.  You can either do this by calibrating the camera, or since you’re using the Kinect v2 you can troll through this issue:


As I said before, I don’t know the PCL grabber API, I do it all manually.  It might provide a way to get the camera intrinsics.  What you can see in this code is that it doesn’t matter whether we’re doing the original image, or the down-sampled image.  The signature might look like

    void computeCartesian(PointCloud *cloud, ushort *range_image, int width, int height, float fx, float fy, float cx, float cy)

For the original image, you just specify 512, 424, and the original fx, fy, cx, and cy.  For the down-sampled image, your width is now 256, height is 212.  Then you use the original fx, fy, cx, and cy, all divided by two.

As a final remark, at the beginning of the post I stated that doing 512x424 -> 320x240 is not common.  The reason is because in order to create this size image, the down-sampling code becomes much more complicated.  512 / 320 = 1.6, so you would be dividing fx and cx by 1.6 in the cartesian code.  But 424 / 240 = 1.76!  So doing something like

    int center = src[(1.76 * y * width) + (1.6 * x)];

will probably not be correct, you’d have to be considering pixel boundaries at this point and probably averaging things?  The correct way of doing that is likely possible, but unknown to me.  The point: dividing by two is well defined, but asymmetric down-sampling of 1.6 in the width and 1.76 in the height is guaranteed much more difficult to do correctly.

I hope that is clear and equips you with all the information you need.  It seems like a lot, but it’s not tooooo difficult since the code is there for you :)

Happy down-sampling!

-Stephen

P.S. Third time is a charm, I know nothing about the grabber API.  The libfreenect2 driver provides float data (not ushort), so that may be relevant.  But I think PCL may be using OpenNI2, in which case it might be ushort?  I really don’t know, sorry!


_______________________________________________
[hidden email] / http://pointclouds.org
http://pointclouds.org/mailman/listinfo/pcl-users
Reply | Threaded
Open this post in threaded view
|

Re: How to downsample a PCLPointCloud2 keeping it organized

Stephen McDowell
One more thing.  You originally said you wanted 320x240.  Though I don’t know your motive for this size, you are probably working off of some code somewhere that was working with the original kinect.  The original kinect produces frame sizes of 640x480, meaning the next “pyramid down” is 320x240.

So whatever it was that you wanted 320x240 for, will almost certainly work the same using 256x212 instead!
_______________________________________________
[hidden email] / http://pointclouds.org
http://pointclouds.org/mailman/listinfo/pcl-users