Generating point cloud from depth & image values

[Bradyk27]

Hi @NishanthJKumar ! Thanks for sharing this info about your project here!
I am looking forward to see the videos and images!

We did not publish our code for the turtlebot application open source. It would be too much effort to to make the code readable, document, fix bugs and administrated it properly. I hope you understand.

For the 3D point cloud we proceeded as follows:

• We used opencv library to uncompress the CompressedImage data.
• We used a script like this to get the points in 3D from the two 2D images.
• We then put this data into a mesh with a simple shader that illustrates point data only.
• By only updating parts of the mesh (e.g. 20%) in each frame you can improve the performance of your application.
  Originally posted by @MartinBischoff in What are you using ROS# for? #20 (comment)

[Bradyk27]

@MartinBischoff, could you expand upon this? I'm currently trying to do the same thing without much luck.

I elected not to use opencv library to uncompress the data and instead just subscribed directly to the raw_image. This very well might be the source of my problem: did you run opencv on the Ubuntu side and then transfer the uncompressed images over? Or did you somehow run opencv within Unity?

Also, could you clarify this line?

float depth = 0;// depthImage[u, v];

What would depthImage[u,v] be? depthImage.data[u*v]?

Finally, where would one find / calculate the focal variable?

Thank you so much for your help--pulling my hair out trying to get this to work.

[viliwonka]

I managed to sucessfully send point cloud through RosSharp to RViz, but it was very slow, it took like 20 seconds. But it was HD pointcloud, mind that, and I had to use BSON serialization since JSON itself doesn't support NaN values.

[sterlingm]

@Bradyk27 Here is what worked for me to use OpenCV to decompress images coming from an Asus Xtion Pro on a Turtlebot2.

C++ side: https://pastebin.com/Ah7qL5yE
I could not get the image_transport codecs to compile in VS17 so I copied in code from that package to do the quantization for depth images. If your images are not encoded by image_transport then you may need to change some things. I wasn't sure if it was appropriate to add this code to the repo so for now I left it out. Also, keep in mind the Mat type and imread mode may be different depending on your image type. For my case, I was using jpg for RGB and png for depth.

C# side: I created a script to eventually merge the 2D images, but right now it only decompresses (should be helpful for your question about decompressing though): RGBDMerger.cs. This script uses the DLL Import that I linked above.
I had to make some minor changes to ImageSubscriber, and create a new script DepthImageSubscriber. The DepthImageSubscriber script is just a copy of ImageSubscriber, but it's necessary in order to have one component that can subscribe to two images. ImageSubscriber,DepthImageSubscriber

The decompression takes between ~5-10ms for each type of image. You'll need to manually set the width, height, and msg fields of your images based on your camera+robot. Hope this helps.

[sterlingm]

@Bradyk27 To get the focal length of your camera, check for sensor_msgs/CameraInfo data to be published in ROS. When working with a Turtlebot 2, for instance, the camera information is published on topics /camera/X/camera_info where X is depth, rgb, etc.