How to convert a 3D point cloud to a depth image? - c++

Question

For my work I have to convert a point cloud to a grey scale (depth) image meaning that the z coordinate of each XYZ point in the cloud represents a shade of grey. For mapping a Z coordinate from the [z_min, z_max] interval to the [0..255] interval I used the map function of Arduino:
float map(float x, float in_min, float in_max, float out_min, float out_max)
{ return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min; }
With that done I need to write the result to an image, the problem being that the clouds that I have can have millions of points so I can't just write them 1 by 1 to an image in order. Let's say that I have 3000x1000 ordered XY points. How would I do if I wanted to write them to a 700x300 pixels image? I hope the question is clear, thanks in advance for answering.

Answer 1

I have managed to find a solution to my problem. It is a fairy long algorithm for stack overflow but bear with me. The idea is write a vector of XY grey scale points as a pgm file.
Step 1: cloud_to_greyscale - function that converts an XYZ Point Cloud into a vector of XY grey scale points and that receives a cloud as a parameter:
for each point pt in cloud
point_xy_greyscale.x <- pt.x
point_xy_greyscale.y <- pt.y
point_xy_greyscale.greyscale <- map(pt.z, z_min, z_max, 0, 255)
greyscale_vector.add(point_xy_greyscale)
loop
return greyscale_vector
Step 2: greyscale_to_image - function that writes the previously returned vector as a greyscale_image, a class that has a width, a height and a _pixels member corresponding to a double dimensional array of unsigned short usually. The function receives the following parameters: a greyscale_vector (to be turned into the image) and an x_epsilon that will help us delimit the x pixel coordinates for our points, knowing that the x point coordinates are floats (and thus not suitable as array indices).
A little background info: I work on something called widop clouds so in my 3D space x is the width, y is the depth and z is the height. Also worth noting is the fact that y is an integer so for my problem, the height of the image is easy to find: it's y_max - y_min. To find the width of the image, follow the algorithm below and if it isn't clear I will answer any questions and I'm open to suggestions.
img_width <- 0; // image width
img_height <- y_max - y_min + 1 // image height
// determining image width
for each point greyscale_xy_point in greyscale_vector
point_x_cell <- (pt.x - x_min) * x_epsilon * 10
if point_x_cell > img_width
img_width <- point_x_cell + 1
loop
// defining and initializing image with the calculated height and width
greyscale_img(img_width, img_height)
// initializing greyscale image points
for y <- 0 to greyscale_img.height
for x <- 0 to greyscale_img.width
greyscale_img[y][x] = 0
loop
loop
// filling image with vector data
for each point point_xy_greyscale in greyscale_vector
image_x = (point_xy_greyscale.x - x_min) * x_epsilon * 10
image_y = point_xy_greyscale.y - y_min
greyscale_image[image_y][image_x] = point_xy_greyscale.greyscale
loop
return greyscale_image
The only thing left to do is to write the image to the file, but that is easy to do, you can just find the format rules in the previous link related to the pgm format. I hope this helps someone.
EDIT_1: I added a picture of the result. It is supposed to be a railway and the reason it's fairly dark is that there are some objects that are tall so ground objects are darker.
depth image of railway