Point Cloud Processing

I’m having serious problems with point cloud processing in rtmaps. Our code base is in simulink and we’ve been using the RTMaps TLC to create rtmaps components from our simulink models. This has worked great for us, except when dealing with point cloud data. As an example I created a very basic point cloud filter in simulink that trims down a point cloud using a specified radius. In this example I set the radius to 30 meters and viewed the point cloud before and after the filter. Instead of the point cloud being trimmed down I find data is shifted greatly from the truth and even missing in some cases. I provided an image below of the point cloud before and after and I’ll provide the few lines of MATLAB code that were code generated.

Note: According to mathworks all of the functions used in the script below are C/C++ code generation compatible.


function mergedPointCloud = fcn(os2_data, os0Adjustments, os2Adjustments)

os2PointCloud = pointCloud(os2_data);
indices2 = findPointsInCylinder(os2PointCloud,[3 30]);
os2restricted = select(os2PointCloud,indices2,OutputSize=“full”);
mergedPointCloud = os2restricted.Location;

Image from RTMaps Diagram

Make sure that your component is set up to parse the pointcloud data correctly. A “pointcloud” in RTMaps is simply a 1-D vector of floats set up as [x0, y0, z0, x1, y1, z1 … xN, yN, zN].