Referece to #Destaggerd Point Cloud using of the destaggered function for Point Clouds about ouster-ros

[Samahu]

@66lukas66 I think the issue is that the cartesian method(s) which project the lidarscan to 3D expect the input 2D image to be in staggered state so you can not pass a destaggered image to and then expect the right results.

So what you should do is first perform the cartesian then take the generated points and pass it to the destagger function. However, the destagger method expects ouster::img_t which is a 2D array with the same dimensions as the lidarscan {WxH}. Meanwhile, points is an array of the size {WxH, 3} so we beed to reshape it back to the WxH using the Eigen::Map function as follows:

    auto points_x_reshaped = Eigen::Map<const ouster::img_t<float>>(points.col(0).data(), H, W);

then you can apply the destaggering method to the reshaped points as follows:

    auto points_x_destaggered = ouster::destagger<float>(points_x_reshaped, info.format.pixel_shift_by_row);

You would need to do the same for y and z (use col(1) and col(2) respectively). The before passing back to the copy_scan_to_cloud you need to re-assemble the 3 arrays back into a PointsF object (in a nutshell).

[Samahu]

And what is the reason that only the range must be destaggerd and not also the reflectivity?

All the 2D images are being destaggered in os_image_nodelet.cpp, the destaggering is applied to 4 images at the same time within this code block:

ouster-ros/src/os_image_nodelet.cpp

Lines 139 to 154 in 927f216

	// copy data out of Cloud message, with destaggering
	for (size_t u = 0; u < H; u++) {
	for (size_t v = 0; v < W; v++) {
	const size_t vv = (v + W - px_offset[u]) % W;
	const auto& pt = cloud[u * W + vv];
	// 16 bit img: use 4mm resolution and throw out returns >
	// 260m
	auto r = (pt.range + 0b10) >> 2;
	range_image_map(u, v) = r > pixel_value_max ? 0 : r;
	signal_image_eigen(u, v) = pt.intensity;
	reflec_image_eigen(u, v) = pt.reflectivity;
	nearir_image_eigen(u, v) = pt.ambient;
	}
	}

Hope my answers help

[66lukas66]

Both of your ideas work, not perfectly, according to my implimentation. They don't always sort all items in the correct order. I have now written a function for detagging the cloud, which also transcribes the point cloud at the same time, which is based on the destagger function.

I'll make a pull request for it shortly.

[Samahu]

Ok, I will take a look at your PR.