Generate point cloud from depth image

[saikns]

I am using the depth image to detect any irregularities or obstacles in the path of an AGV with ifm o3d303 mounted. The process involves removing any depth data other than obstacles from the image through a mask created by gradient based processing. I would like to convert this selective depth image to a point cloud. Is it possible to do using this package?

P.S I have a working version of this using openni to convert selective depth image to point cloud, I am looking for a similar functionality.

[tpanzarella]

Yes. To convert the depth image to a point cloud, you also need the rotated unit vectors and the translation vector from the extrinsics. All of this is exposed via aptly named topics published to by the camera node. To save myself some typing, here is an example in C++ (from the underlying libo3d3xx sensor interface library) and here (see the compute_cartesian function) is an example in Python from the ROS unit tests.

You will of course have to port this to your specific scenario, but, the basis for how to do this computation is in the linked to code.

[saikns]

By using the exact framework in ex-cartesian.cpp, I was able to create a point cloud. About half of the points are turning out right but the other half looks like they are having an issue with some co-ordinate system. You can look at the images below, green cloud is original reference cloud & white cloud is recreated using ex-cartesian.cpp. This is the code I am using

// rotated unit vectors
cv::Mat ex, ey, ez;
std::vector<cv::Mat> uvec_channels(3);
cv::split(m_unitVectors, uvec_channels);
ex = uvec_channels[0];
ey = uvec_channels[1];
ez = uvec_channels[2];

// hold a reference to the radial distance image and convert to float
cv::Mat rdis = m_depthImg;
cv::Mat rdis_f;
rdis.convertTo(rdis_f, CV_32FC1);

// get a copy of the extrinsics
std::vector<float> extrinsics = m_extrinsics;

// NOTE: The unit vectors are already rotated, so, we only need to extract
// out the translation vector. The units are in mm.
float tx = extrinsics[0];
float ty = extrinsics[1];
float tz = extrinsics[2];

// Compute the cartesian data
cv::Mat x_f = ex.mul(rdis_f) + tx;
cv::Mat y_f = ey.mul(rdis_f) + ty;
cv::Mat z_f = ez.mul(rdis_f) + tz;

cv::Mat x, y, z;
x = z_f/1000.0f;
y = -x_f/1000.0f;
z = -y_f/1000.0f;

int width = x.cols;
int height = x.rows;
for(int i = 0; i < width; ++i) {
    for(int j = 0; j < height; ++j) {
        PointT point;
        point.x = x.at<float>(i,j);
        point.y = y.at<float>(i,j);
        point.z = z.at<float>(i,j);
        cloud.points.push_back(point);
    }
}

I am not sure what's throwing off the conversion. Let me know if you see anything funny.

Another quick question about converting cartesian co-ordinates back to image co-ordinates, is there a way to use something similar to a pinhole camera model like
projected_point_x = f_x * point_x / point_z + c_x
projected_point_x = f_y * point_y / point_z + c_y.

[graugans]

@saikns What software version are you using on the camera?

[saikns]

libo3d3xx - 0.4.4
o3d3xx-ros - 0.2.4
firmware - 1.4.1639

[tpanzarella]

I am not sure what's throwing off the conversion. Let me know if you see anything funny.

The code looks OK upon inspection. The image certainly looks funny -- no computed points in -y space, like the points are rotated around z 180 degrees and also y by 90 (or something???). Not clear why this is affecting only some of the points. We need to dig deeper.

Can you run this code outside of ROS? Probably easiest to generate your point clouds, write them to a PCD, then load them in pcl_viewer. This could give us a quick visual check. I'd like to rule out anything in the ROS interface causing this issues (e.g., maybe something getting stamped with the wrong tf frame? I'm speculating). It is curious, because, there is a unit test in libo3d3xx that explicitly performs this transformation and checks that the off-board computation matches the on-board computation to w/in 1 cm tolerance.

Another quick question about converting cartesian co-ordinates back to image co-ordinates, is there a way to use something similar to a pinhole camera model like

Not that I am aware of -- i.e., the camera firmware does not expose the intrinsics in this way.

[tpanzarella]

I'm currently working on a patch version of the library that will be compatible with the latest IFM firmware release. While doing this development, the only unit test that is failing is the Cartesian computation test. I believe something is incorrect somewhere. I am not sure if it is how I am doing the computation or the data that are being returned by the sensor. @graugans I can use your help here and perhaps we should tie in the quant team (Christian's group). Something is wrong.

I am doing this testing outside of ROS, so, that is not an issue. In the attached PNG screenshots, the green cloud is the Cartesian data as computed by the sensor. The white cloud is the Cartesian data as computed by my code. I have attached three views of the same data from a scene taken in my office. I have also attached a zip file that contains:

1. ASCII versions of the point data as PCD files (so exact pixel data can be inspected).
2. The camera configuration as a JSON dump. Please NOTE that I have enabled on-camera rectification as without the rectification the data are qualitatively worse.

The code that generated the data is here

pcd_and_json.zip

[cfreundl]

I tried to reproduce the issue at my place. For that purpose, I have extended the live viewer example of libo3d3xx with the display of the off-board computed cartesian data and the unit vectors.
This archive contains the source code as well as the following screenshots.

I could not see a problem with firmwares 1.4.1639 and 1.6.2038, all tested with a camera application without rectification:

Firmware 1.4.1639:

Firmware 1.6.2038:

However, with the lastest development firmware 1.5.698 I could basically reproduce Tom's results:

There is a general translation error visible, also there is a somewhat larger error on the left margin of the image.

@tpanzarella, is it possible that the errors we can see with the 1.5.698 firmware might be a result of the extended image chunk headers such that due to an offset error we get those large displacement at the beginning of each image row? Also, might this cause the extrinsic calibration to be wrong within the library?

@saikns, unfortunately this does not help with your initial problem. Have you already tried to reproduce your issue with the sample code provided by @tpanzarella? Is it possible that you provide a dump of your camera configuration for us to reproduce the error you are seeing?

[tpanzarella]

@tpanzarella, is it possible that the errors we can see with the 1.5.698 firmware might be a result of the extended image chunk headers such that due to an offset error we get those large displacement at the beginning of each image row?

@cfreundl This is a good thought. I plan to implement the code to deal with the new chunk headers today. Once I have that done, I will test again and report back.

Also, might this cause the extrinsic calibration to be wrong within the library?

The extrinsics have always been a sticky issue. Basically, if you store an extrinsic calibration on the camera, all bets are off. However, apply the extrinsics (as stored by the factory) to the radial distance image and the unit vectors to compute the cartesian data then transforming into the libo3d3xx / o3d3xx-ros camera frame should result in correct data.

[tpanzarella]

@cfreundl I believe your assertions were correct. I updated the code to correctly handle the v2 image chunk headers in this commit. The Cartesian Computation unit tests are passing again -- against both v2 and v1 headers. Additionally, I re-ran my test from above, and as you can see, the data look good:

Many thanks for your assistance with this. Not sure this helps @saikns so much -- feels like something else is going on there.

The updated code is still living on my dev-0.4.9 branch. I still have a few updates to put in before I release and merge it to the HEAD.

[tpanzarella]

As a side note to my last comment, you can clearly see that I am not rectifying the images either, so, I am pretty confident that this is the fix that was necessary to deal with the issue that was uncovered while looking into @saikns issue.