Running FLOAM with Realsense L515 data

[MrMixit]

Hi!

I have tried to modify the laser processing to be able to process data from the Realsense L515 lidar by adding the following lines in laserProcessingClass.cpp (in the featureExtraction() function):

    else if (N_SCANS == 480)
    {
        scanID = int((angle-45)*480/55); // Assuming angles span from 45 to 100 degrees vertically
        if (angle > 100 || angle < 45 || scanID > (N_SCANS - 1) || scanID < 0)
        {
        continue;
        }
    }

I assumed that by recording depth with 640x480 resolution I would get 480 scan lines. The L515 has a vertical FOV of 55, but Intel does not say anything about "absolute angular limits" such as what we have for velodyne lidars, so it is hard to tell exactly how I should sort it into scanIDs, but I did some print debugging and these limits I chose did not seem to exclude any measurements.

My main issue is that the filtered point clouds have circular "holes" in the middle of the FOV, see the attached screenshots. This Lidar is not recording in sweeps of 360 degrees, so this might be one of the reasons we are getting bad results. I could not understand from the code if/where assumptions about this are made.

I have been running the launch file with these parameters:

    <param name="/use_sim_time" value="true" />
    <param name="scan_period" value="1.0/30.0" /> 
    <param name="vertical_angle" type="double" value="55.0" />
    <param name="max_dis" type="double" value="9.0" />
    <param name="min_dis" type="double" value="0.25" />
    <param name="map_resolution" type="double" value="0.02" />

Again, I do not really know what to set the vertical angle to, but we have been streaming with 30 fps and the Lidar has a range from 0.25 to 9 meters. The entire rosbag I ran it with is around 8 GB and contains images as well as imu measurements, but I sent you an 18 second sample through WeTransfer. Let me know if I can provide you with any additional information.

[wh200720041]

Hi @MrMixit

Thanks for your question here.

I tested your rosbag and I re-opened a new project floam-ssl because the sensor configuration as well as the algorithm is much different from velodyne lidar

https://github.com/wh200720041/FLOAM_ssl.git

You may have a quick test with that algorithm. Unfortunately, I dont have solid state lidar on hand and I cant carry out further evaluation.

According to my test, I think one problem with L515 is the range. The detection range is too short, subsequently, there are little geometric features. I think it may reduce localization accuracy.

Han

[MrMixit]

Wow, thank you! I'll test that out and let you know how it works. We managed to get FLOAM working pretty well on another recording without any other modifications to the code, but in this data, most of the points/features more on the sides rather than in the center of the FOV. The accuracy of the map seemed pretty good when we measured the objects in rviz and compared to real life measurements.

As you did, I also noticed that the points are recorded in a frame that is not the base link frame L515_link, but rather L515_depth_optical_frame, which is rotated in relation to the base link. This was probably causing issues when calculating angles, which could in turn ruin the scan line classification, so your new version should make it work better. Thank you again!

Adam

[wh200720041]

Hi @MrMixit

for the frame problem, you can check with floam_ssl, I did some modification on the frame as I noticed that for depth camera, the Z-axis is different from the Z-axis in velodyne.

Han

[MrMixit]

Hi again! Thank you for the suggestion, I tried to use this modification for the original FLOAM:

distance = sqrt(pc_in->points[i].x * pc_in->points[i].x + pc_in->points[i].z * pc_in->points[i].z);
angle = atan(pc_in->points[i].y / distance) * 180 / M_PI;

and then still do "classification" of points with scanIDs based on vertical angle (angle between z-x plane and point) with these lines:

scanID = int((27.5 - angle)*480/55);
if (angle > 27.5 || angle < -27.5 || scanID > (N_SCANS - 1) || scanID < 0) {
    continue;
}

I also decreased the threshold from 131 to 50 points needed in a scan line for it to be processed (if(laserCloudScans[i]->points.size()<50) { continue; }). It works decently for both odometry and mapping accuracy, but I am not sure if we are getting the expected behavior. I've attached a screenshot of a filtered point cloud where you can see that some "lines" are excluded, but the curvature/non-straightness of the excluded lines suggests there is something more going on in the laser processing that I haven't understood.

Can you think of an explanation for this? Is it because floam is assuming 360 degree data somewhere, or maybe because L515 data is different in some other way?

[wh200720041]

Hi @MrMixit

I am not so sure about your question. Perhaps you may want to compare floam_ssl with floam. There are some changes in the feature extraction.

Personally I feel that the data from L515 is noisy and I think use floam for L515 is not a good idea, though it seems still work in your case. Right now I dont have L515 on hand, unfortunately I can't help to test floam on L515.

Thanks Han

[MrMixit]

Sorry if I was very unclear!

From the Velodyne Lidars, we get horizontal scan lines, and in the feature extraction in floam, you filter all incoming points into these lines by calculating the vertical angle in relation to the Lidar, then extract features from lines that have enough points. If a line does not have enough points it would be removed, and then the filtered point cloud would have clearly visible missing horizontal lines where we had too few points. In my picture, these missing lines are not horizontal, but rather circular, so I was wondering why this could be happening, but I am guessing I missed some of the changes you made in the floam_ssl version. I will look more at the feature extraction differences!

I agree that the data seems really noisy. We can set confidence thresholds when recording to remove a bit of the noise in the data (but probably also some useful data), so I might experiment with that for future recordings.

Thank you! Adam

[wh200720041]

Hi @MrMixit

Have you changed the z axis? the z axis of L515 and Velodyne is different.

And in your code scanID = int((27.5 - angle)*480/55); is 27.5 from L515 manual?

Thanks Han

[MrMixit]

Hi!

Yes, I have switched around the axes in laserProcessing such that we calculate distance in z-x plane and then compare that to the y axis to calculate the angle, instead of getting distance in x-y plane and comparing to z axis as for velodyne.

Sadly, no such value can be found in the manual, only that the FOV is 55 degrees. That value was determined through testing different limits, and printing the angles when they were outside those limits.

Best, Adam

[wh200720041]

Hi @MrMixit

Perhaps, in your code,

angle = atan(pc_in->points[i].y / distance) * 180 / M_PI; angle is derived by y/sqrt(x,z)

You may try with angle = atan (y/z) or atan(z/y)

mathematically your equation is correct, but I doubt intel does not use that equation for distance estimation.

I am not sure if this works

Regards Han

[MrMixit]

Oh, right, I noticed you did something like that in floam_ssl too. That is a good point, I will try that!

Best, Adam

[wh200720041]

If there is no question I shall close this issue. For further questions related to L515, you can post question in floam_SSL