pointcloud shows old data

[madgrizzle]

When I view the pointcloud in rviz2 with rgb8 as the color transformer everything looks good. When I switch to any other transformer (e.g., flat color) then I see lots of old point cloud data hanging around. I looked at the code and it seems that when it performs the conversion to generate the pointcloud, if the distance to the point is greater than the maximum distance (I think 3m) then it just changes the alpha value to 0 (meaning it just makes it transparent). The point data from the previous calculation is still there (the x, y, z data) so when you look at it in flat color mode, you see the old data. The problem this introduces is that my obstacle layer is treating all that old data as obstacles, and calculating the cost map accordingly.

I changed the code to go ahead and calculate new x, y, z data even though its beyond max distance and it seems to clear everything up, but it's odd that this hasn't come up before unless I'm actually the first trying to use it to generate a costmap.

[GilMe]

Thanks for sharing your find. It probably saved me a lot of time. I am trying to do lidar odometery and am getting some funky behaviour which I suspect is due to this issue.
I want to make sure I understand what your solution was. If I understand correctly what you did was not to remove the points from the pointcloud but to give them the distance that they report (which is 4001 which is actually an error code). That way these points appear very far in the cost map and this solves your problem. Is this correct?

[madgrizzle]

which topic (scan_3D, scan_2D, or scan_laser) are you trying to perform lidar odometry on? I only made mods to scan_3D since that's all I use at the moment (I have a separate 360 degree laser scanner I use for odometry)

[GilMe]

I did it on the scan_3D. That is where the bug is. I did managed to eventually fix the bug. Since scan_3D's is_dense variable is set to false then points that need to be ignored their x,y and z variables need to be set to std::numeric_limits<float>::quiet_NaN()

Basically I added these lines to both places where the alpha is set to zero and changed the alpha value back to 255

scan_3D.get()->points[BufferIndex].x = std::numeric_limits<float>::quiet_NaN();
scan_3D.get()->points[BufferIndex].y = std::numeric_limits<float>::quiet_NaN();
scan_3D.get()->points[BufferIndex].z = std::numeric_limits<float>::quiet_NaN();
scan_3D.get()->points[BufferIndex].a = 255

[madgrizzle]

Thanks! You know much more about this stuff than I. I'll try your solution, but to answer your original question, I just set it to some large distance (100m or something). However, and I don't know if it's related, I was experiencing issues with clearing of the costmap when I did so. I've since set it to be just a few meters, but beyond the distance of the local costmap's max range.

[cygbot]

We also noticed a while ago that pointcloud remains uninitialized.

I've been very busy these days, so I haven't been able to update. ;(

thank you

[madgrizzle]

Unfortunately, the use of quiet_NaN() didn't help my situation. Since I'm using it for developing a costmap, I believe I need valid data points for everything to accomplish the clearing function. As an example, I stand in front of the lidar and I get marked on the cost map but when I move away nothing gets detected in the same spot (i.e., wall is too far away), there is no valid data for that particular point and therefore nothing to base the clearing on. I've got an idea to try and that is to go ahead and call PointCloud3D.calcPointCloud() while manually setting the distance for a particular sample to the maximum distance.
If so, that should return valid x, y, z points that are points that lie on a sphere with the radius equal to the maximum distance. Maybe that will allow the costmap to be cleared..?

[madgrizzle]

So the costmap now clears. The code I'm using sets the distance to CygLiDARD1::Distance::Mode3D::Maximum_Depth_3D if its greater than that and then it calls PointCloud3D.calcPointCloud() to get x, y, z values. Pretty straightforward except I noticed there was a lot of noise at the very edge/outline of the pointcloud... perhaps its an interpolation issue. So, for the very edges (i.e., x=0, y=0, x=Width-1, or y=Height-1) I set the x, y, z to quiet_NaN() values to effectively remove it from the pointcloud. Doing this seemed to make my clearing function work... sigh.. finally.