Implement a technique to estimate scan_ts when packets are missing at beginning of a lidar scan #67

Samahu · 2023-02-23T20:13:59Z

Related Issues & PRs

Closes #47
Related #46

Summary of Changes

Implement a technique to estimate frame_ts and scan_ts when packets are missing at beginning of a LIDAR scan
The technique utilizes linear interpolation to impute the value of the frame time
- The timestamp of the very first frame is extrapolated based on column time spacing

Note
The TIME_FROM_ROS_TIME case extrapolates frame time based on the reception time of the first packet/column to arrive of a given frame.

Validation

Launch with a live sensor and observe that the timestamp of point cloud messages stays timestamps consistent regardless of the existence of the missing packets.

… the beginning of a frame

ShepelIlya · 2023-02-27T14:47:26Z

Tested with 2 Ouster OS0-64 (fw 2.4), on Jetson AGX Xavier with CISCO IE-3300 switch. Before this fix with high network load some point clouds from driver had invalid timestamps (2-3 secs into future), now this problem is fixed (tested for an hour).

peci1 · 2023-02-27T15:18:29Z

I think it would be quite easy to create an artificial dataset on which this function can be tested. I started creating it, but have no time the following few days to finish it. Basically, you just record the lidar packets into a bag file, figure out which packet represents start of a scan, and drop a few packets around that place.

It would be quite interesting to know how does this behave if the first packet of the very first scan is missing.

Samahu · 2023-02-27T16:56:33Z

I think it would be quite easy to create an artificial dataset on which this function can be tested. I started creating it, but have no time the following few days to finish it. Basically, you just record the lidar packets into a bag file, figure out which packet represents start of a scan, and drop a few packets around that place.

We can add tests that mimic the condition

It would be quite interesting to know how does this behave if the first packet of the very first scan is missing.

Assuming the very fist frame had zero packets the imputation code will use the following values for last_scan:

static int last_scan_last_nonzero_idx =
            curr_scan_first_nonzero_idx - 1;  // initialize to a known state (x0)
static uint64_t last_scan_last_nonzero_value =
            curr_scan_first_nonzero_value;  // impute with ts_v(idx) (y0)

If you substitute these values into the linear interpolation equation y0 + (x - x0) * (y1 - y0) / (x1 - x0) then the denominator x1-x0 should evaluate to to 1, while the term y1 -y0 should evaluate to 0, thus you are left with y0 which is the first valid timestamp value. Technically I can estimate the timestamp of first frame but it won't be very accurate and the code will get far more complicated, that I wasn't sure if it is worth the effort.

peci1 · 2023-02-27T17:09:12Z

Hmm, isn't this a reason to switch to the approach where you compute the timestamp from known sensor parameters? I.e. you know the rotation speed and index of the first packet in the scan, so it is possible to compute the timestamp with pretty simple math. Its precision only depends on the stability of rotation speed, but I always thought this isn't an issue with Ouster lidars. Or did your tests show otherwise?

The benefit would be that the same code could also be used in case of timestamping with ROS time. And another benefit would be that it would work only on the current scan and would not depend on any preceding scan(s).

Samahu · 2023-02-27T17:20:37Z

I tried simple linear extrapolation curr_frame_ts - timestep * count(messed packets[cols]) but I did see instances where this could go past the last_msg timestamp from previous scan. that's why I resorted to this approach which is safe and more accurate (note that I save the value of the last nonzero of last frame so it would be the closest to the missed value). The only down side is you would need special code to handle the missed value of the very first frame (if this is of a real concern I can add the code, but I wasn't sure if it is worth the effort).

peci1 · 2023-02-27T17:32:47Z

Ok, if the tests show the imputation is more precise, then it is a good way to go. What about using the extrapolation for the first scan?

Samahu · 2023-02-27T17:50:47Z

Ok, if the tests show the imputation is more precise, then it is a good way to go. What about using the extrapolation for the first scan?

The first packets of the very first scan are almost always missed (by a good margin), so it makes sense to handle this case with extrapolation.

Samahu · 2023-02-28T20:32:16Z

I pushed an update to extrapolate the timestamp of the very first frame.

With regard to the TIME_FROM_ROS_TIME timestamp mode I am still thinking about it but I feel it would require some substantial (but beneficial) amount of changes to support it properly. The simple extrapolation wouldn't work since the scan timestamp is marked based on receive time of packets on the host machine. Since packets may come out of order you can not extrapolate the scan timestamp based on the current value. I may leave the case of TIME_FROM_ROS_TIME out of this PR and address it later.

Samahu · 2023-03-02T19:13:54Z

I did push an update to the PR that addresses the TIME_FROM_ROS_TIME case, it simply infers the frame timestamps by extrapolating the reception time of the first packet/column of the new frame.

src/os_cloud_nodelet.cpp

bexcite · 2023-03-02T23:25:07Z

imho, I would try to make the function for estimating the first column timestamp that operates on a single LidarScan as:

find first valid ts (+ first_m_id)
find last_valid_ts ( + last_m_id)
subtract first_valid_ts from both (first/last values) and make the values in double (we will move to 100k-200k int values, i.e. removing many bits and be safer on precision size)
fit the line col_ts = a + b * m_id
calculate col_ts for m_id set to zero, i.e. just a value (which should be negative because we've substracted first_valid_ts before fitting the line)
add first_valid_ts back to the calculated col_ts for m_id zero, thus moving everything back to huge nanosecs timestamp space.

I've not tried it, but if it will work without losing precision it would be a single standalone function that will work for any LidarScan without keeping any other state around.

bexcite · 2023-03-02T21:51:32Z

src/os_cloud_nodelet.cpp

+        if (predicate(*p)) return p - array.data();
+    } while (p-- != array.data());
+    return -1;
+}


I'm curious would it possible to use a standard std::find_if with Eigen reverse() to get a reverse iterator? https://eigen.tuxfamily.org/dox/classEigen_1_1DenseBase.html#a38ea394036d8b096abf322469c80198f

~~Initially had it like this but found it that does not cross compile.~~

Sorry I thought you meant something else

I wouldn't want to create a reverse of the array just so I could find the last element. while you could easily find the item by iterating the array backwards in-place..

Originally, I thought you were suggesting me to use std::reverse_iterator, this would very appropriate and you can pass it to std::find_if and it would traverse the array in-place backwards. The only issue it wouldn't cross-compile (failed on gcc-9).

src/os_cloud_nodelet.cpp

Samahu · 2023-03-03T00:13:45Z

imho, I would try to make the function for estimating the first column timestamp that operates on a single LidarScan as:

find first valid ts (+ first_m_id)

find last_valid_ts ( + last_m_id)

subtract first_valid_ts from both (first/last values) and make the values in double (we will move to 100k-200k int values, i.e. removing many bits and be safer on precision size)

fit the line col_ts = a + b * m_id

calculate col_ts for m_id set to zero, i.e. just a value (which should be negative because we've substracted first_valid_ts before fitting the line)

add first_valid_ts back to the calculated col_ts for m_id zero, thus moving everything back to huge nanosecs timestamp space.

I've not tried it, but if it will work without losing precision it would be a single standalone function that will work for any LidarScan without keeping any other state around.

The whole idea of my approach is to avoid having the estimated timestamp of the scan interleave with values from the previous scan. Imagine the case where you receive a frame with nonzero values only in the range of [1000, 1010]. So a very short range of valid value and you try to fit the line into it. It does not guarantee that the projected/estimated value of first timestamp can't go past previous frame. The approach used here does guarantee that you won't have values interfere/interleave between two different scans/frames.

bexcite · 2023-03-03T06:41:04Z

@Samahu

Imagine the case where you receive a frame with nonzero values only in the range of [1000, 1010]

yeah, agree. I've tried some of these cases and they indeed can go past the previous frame, i've even played with least squares fit that uses not just 2 points, but all of those available and still in some LidarScans it may have error with ground truth's zero column timestamp to the extent of 150k - 250k nanosecs in some cases ... so probably we can't get rid of keeping the state of the previous frame and need to have this filter like approach.

src/os_cloud_nodelet.cpp

… beginning of a lidar scan (ouster-lidar#67) * Implement a technique to estimate scan_ts when packets are missing at the beginning of a frame * Be consistent with variable naming * Add an assert statement for find_if_reverse * Extrapolate the time for the first scan * Implement the extrapolation for TIME_FROM_ROS_TIME timestamp option * Address the issue with ulround function not working for integral types * Perform type conversion on the difference * Take always positive difference and compute sign * Use lround instead of invoking cast to integer/long * Update CHANGELOG

… missing at beginning of a lidar scan (ouster-lidar#67) * Implement a technique to estimate scan_ts when packets are missing at the beginning of a frame * Be consistent with variable naming * Add an assert statement for find_if_reverse * Extrapolate the time for the first scan * Implement the extrapolation for TIME_FROM_ROS_TIME timestamp option * Address the issue with ulround function not working for integral types * Perform type conversion on the difference * Take always positive difference and compute sign * Use lround instead of invoking cast to integer/long * Update CHANGELOG

Implement a technique to estimate scan_ts when packets are missing at…

d4fdc44

… the beginning of a frame

Samahu added the enhancement New feature or request label Feb 23, 2023

Samahu self-assigned this Feb 23, 2023

Samahu mentioned this pull request Feb 23, 2023

Scans with missing packets at the beginning have wrong timestamp #47

Closed

Samahu added 2 commits February 23, 2023 12:26

Be consistent with variable naming

55ac84d

Add an assert statement for find_if_reverse

8158a4d

Extrapolate the time for the first scan

ddf7586

Implement the extrapolation for TIME_FROM_ROS_TIME timestamp option

99e30e2

Samahu requested review from kairenw, chrisbayruns and bexcite and removed request for kairenw and chrisbayruns March 2, 2023 19:14

Samahu marked this pull request as ready for review March 2, 2023 19:18

Samahu requested a review from twslankard March 2, 2023 20:09

twslankard reviewed Mar 2, 2023

View reviewed changes

src/os_cloud_nodelet.cpp Show resolved Hide resolved

twslankard reviewed Mar 2, 2023

View reviewed changes

src/os_cloud_nodelet.cpp Show resolved Hide resolved

twslankard reviewed Mar 2, 2023

View reviewed changes

src/os_cloud_nodelet.cpp Outdated Show resolved Hide resolved

twslankard approved these changes Mar 2, 2023

View reviewed changes

bexcite reviewed Mar 2, 2023

View reviewed changes

Samahu mentioned this pull request Mar 3, 2023

Non-monotonic time results in damaged t field of pointclouds #46

Open

kairenw approved these changes Mar 6, 2023

View reviewed changes

src/os_cloud_nodelet.cpp Show resolved Hide resolved

Samahu added 5 commits March 6, 2023 12:49

Address the issue with ulround function not working for integral types

da6363e

Perform type conversion on the difference

19f18e5

Take always positive difference and compute sign

caada4f

Use lround instead of invoking cast to integer/long

c98decb

Update CHANGELOG

26d0bbd

Samahu merged commit bf86365 into master Mar 7, 2023

Samahu deleted the SW-4502-scans-with-missing-packets-at-the-beginning-have-wrong-timestamps branch March 7, 2023 19:55

Samahu mentioned this pull request Mar 29, 2023

ROS2 how is header.stamp assigned in each /ouster/points message? #89

Closed

Samahu mentioned this pull request Apr 29, 2023

Publish time with Azimuth window enabled and timestamping #114

Open

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Implement a technique to estimate scan_ts when packets are missing at beginning of a lidar scan #67

Implement a technique to estimate scan_ts when packets are missing at beginning of a lidar scan #67

Samahu commented Feb 23, 2023 •

edited

Loading

ShepelIlya commented Feb 27, 2023 •

edited

Loading

peci1 commented Feb 27, 2023

Samahu commented Feb 27, 2023

peci1 commented Feb 27, 2023

Samahu commented Feb 27, 2023 •

edited

Loading

peci1 commented Feb 27, 2023

Samahu commented Feb 27, 2023

Samahu commented Feb 28, 2023 •

edited

Loading

Samahu commented Mar 2, 2023 •

edited

Loading

bexcite commented Mar 2, 2023

bexcite Mar 2, 2023

Samahu Mar 2, 2023 •

edited

Loading

Samahu Mar 2, 2023 •

edited

Loading

Samahu Mar 2, 2023 •

edited

Loading

Samahu Mar 2, 2023 •

edited

Loading

Samahu commented Mar 3, 2023 •

edited

Loading

bexcite commented Mar 3, 2023

Implement a technique to estimate scan_ts when packets are missing at beginning of a lidar scan #67

Implement a technique to estimate scan_ts when packets are missing at beginning of a lidar scan #67

Conversation

Samahu commented Feb 23, 2023 • edited Loading

Related Issues & PRs

Summary of Changes

Validation

ShepelIlya commented Feb 27, 2023 • edited Loading

peci1 commented Feb 27, 2023

Samahu commented Feb 27, 2023

peci1 commented Feb 27, 2023

Samahu commented Feb 27, 2023 • edited Loading

peci1 commented Feb 27, 2023

Samahu commented Feb 27, 2023

Samahu commented Feb 28, 2023 • edited Loading

Samahu commented Mar 2, 2023 • edited Loading

bexcite commented Mar 2, 2023

bexcite Mar 2, 2023

Choose a reason for hiding this comment

Samahu Mar 2, 2023 • edited Loading

Choose a reason for hiding this comment

Samahu Mar 2, 2023 • edited Loading

Choose a reason for hiding this comment

Samahu Mar 2, 2023 • edited Loading

Choose a reason for hiding this comment

Samahu Mar 2, 2023 • edited Loading

Choose a reason for hiding this comment

Samahu commented Mar 3, 2023 • edited Loading

bexcite commented Mar 3, 2023

Samahu commented Feb 23, 2023 •

edited

Loading

ShepelIlya commented Feb 27, 2023 •

edited

Loading

Samahu commented Feb 27, 2023 •

edited

Loading

Samahu commented Feb 28, 2023 •

edited

Loading

Samahu commented Mar 2, 2023 •

edited

Loading

Samahu Mar 2, 2023 •

edited

Loading

Samahu Mar 2, 2023 •

edited

Loading

Samahu Mar 2, 2023 •

edited

Loading

Samahu Mar 2, 2023 •

edited

Loading

Samahu commented Mar 3, 2023 •

edited

Loading