refactor(nebula_decoders_hesai): implement generic Hesai decoder #42
Conversation
mojomex
changed the title
mojomex
changed the title
Implement a templated class HesaiDecoder which can handle all currently supported Hesai sensors.
mojomex
force-pushed
the
generic-hesai-decoder
branch
from
9f438d0
to
d75299f
Compare
Unit tests now expect a `dual_return_distance_threshold == 0.1` and the `<=` resp. `>=` operators for excluding points under/over a limit.
|
Since all unit tests are passing now (*with some caveats, see below), I will mark this PR as ready and open the discussion! ✔️ Unit testsThe unit test reference point clouds had some issues that prevented this new decoder from passing tests, see #48 and #49.
Has been reverted back, now that unit tests pass. ✔️ Angle correction differences
This has been due to float vs. double precision and multiplication order of floats in distance calculations. Fixed. ✔️ Performance
|
mojomex
changed the title
If a scan is completed mid-packet, the new scan's timestamp is neither that of the current, nor that of the next packet. Rather, it is the current packet's timestamp plus the minimum time offset of any point in the remainder of the packet.
Instead of separating azimuth/elevation correction and sin/cos lookups, these values are now all returned by `getCorrectedAngleData`. The `return_units` vector is now allocated once per return group, instead of every unit. `findField` for AT128 now contains not modulos or arithmetic and thus is significantly faster.
Codecov ReportPatch coverage:
❗ Your organization needs to install the Codecov GitHub app to enable full functionality. Additional details and impacted files@@ Coverage Diff @@
## main #42 +/- ##
==========================================
- Coverage 13.35% 12.00% -1.36%
==========================================
Files 111 61 -50
Lines 10989 7830 -3159
Branches 1725 1517 -208
==========================================
- Hits 1468 940 -528
+ Misses 8345 5789 -2556
+ Partials 1176 1101 -75
Flags with carried forward coverage won't be shown. Click here to find out more.
☔ View full report in Codecov by Sentry. |
mojomex
force-pushed
the
generic-hesai-decoder
branch
from
e5b78d3
to
e8fa779
Compare
Implement a generic Hesai decoder that can handle all Hesai sensor models.
Fixes #48, fixes #49 and closes #38.
PR Type
Related Links
#38 -- the issue for this PR
#48 -- the first issue that made the unit tests of this PR fail
#49 -- the second issue that made the unit tests of this PR fail
Description
Since sensors from the same vendor often follow similar conventions when it comes to packet structure and data processing steps, a generic decoder can be used for most of the decoding work.
This PR propses such a generic decoder for all Hesai sensors currently supported.
Potential for generalizaiton
Packet formats
For all handled Hesai sensors, the packet structure follows this rough format:
Decoding steps
For all handled Hesai sensors, decoding a packet follows these steps:
The steps marked with * are model-specific:
Angle correction
There are two approaches between all the supported sensors:
For both approaches, the same sin/cos lookup tables can be computed and used.
However, the resolution and calculation of these tables is different.
Calibration based
For each laser channel, a fixed elevation angle and azimuth angle offset are defined in the calibration file.
sin/cos lookup tables are computed at 0.001 deg resolution (the resolution observed in the calibration files) for the whole 360 deg range .
This yields a (360 deg / 0.001 deg) * sizeof(float) = 360000 * 4B = 1.4MB memory footprint per lookup table (of which there are two: sin, cos).
Correction based
While azimuth and elevation correction also have a per-channel component, an additional component depending on azimuth AND channel is present.
This leads to more individual azimuth/elevation values than the calibration-based approach, and for the case of the AT128, the lookup table resolution is around 0.00004 deg.
This yields a memory footprint of 36.9 MB per lookup table.
Timing correction
Timing correction for all sensors follows the same underlying formula:$T_{s_i}$ , packet start timestamp $T_{p_j}$ , block offset $o_{b_k}$ within the packet and channel offset $o_{c_l}$ within the block, the point identified by $(i, j, k, l)$ has the relative scan timestamp $t_{i,j,k,l} = T_{p_j} - T_{s_i} + o_{b_k} + o_{c_l}$ .
Given a scan start timestamp
The block offset follows a formula linear in the block index for all sensor models which addidionally depends on the number of returns of the currently active
return_mode.The channel offset is given as a formula, table or set of tables for all sensors. A few sensors' formula is influenced by factors such as high resolution mode (128E3X, 128E4X), alternate firing sequences (QT128) and near/farfield firing (128E3X).
Return types
While there is a wide range of different supported return modes (e.g. single (first), single (strongest), dual (first, last), etc.) their handling is largely the same.
Differences only arise in multi-return (dual or triple) in the output order of the returns, and in the handling of some returns being duplicates (e.g. in dual(first, strongest), the first return coincides with the strongest one).
Here is an exhaustive list of differences:
0x3B, 128E3X, 128E4X and XT32 reverse the output order (Last, First)0x39, all sensors except XT32M place the second strongest return in the even block if last == strongest0x3c, the same as for0x39holds.For all other return modes, duplicate points are output if the two returns coincide.
The implementation
HesaiPacketPackets are defined as packed structs for effortless parsing.
The sensor-specific layout for sensor XYZ is defined in
PacketXYZand usually employs an ownTailXYZstruct.The header formats are largely shared between sensors.
The packet body (i.e. point data) is mainly parameterized by bytes per point, points per block, and blocks per body. Thus, parameterized generic structs are used. A few skews such as fine azimuth blocks and blocks with a start-of-block (SOB) header exist and are implemented as their own structs.
HesaiSensorEach sensor model has its own class
PandarXYZ : HesaiSensor<...>that defines packet type and timing, and return mode handling logic. Angle correction is the same for 90% of sensors and thus outsourced intoAngleCorrectorand subclasses. These are template arguments forHesaiSensor.Return mode handling has a default implementation that is supplemented by additional logic only in 3 sensors.
AngleCorrectorThe angle corrector has three main tasks:
hasScanCompleted()logic that decides where one scan ends and the next startsThe two angle correction types are calibration-based and correction-based. In both approaches, a file from the sensor is used to extract the angle correction for each azimuth/channel.
For all approaches, cos/sin lookup tables in the appropriate size (360deg) and resolution (either 1/1000th deg or 1/25600th deg) are generated.
These resolutions come from the resolutions of the calibration file angle offsets (1/1000th deg) and the fine azimuth resolution of AT128 (1/15600th deg).
HesaiDecoder<SensorT>The decoder is in charge of the control flow and shared decoding steps of all sensors.
It is a template class taking a sensor type
SensorTfrom which packet type, angle correction etc. are deducted at compile time.Thus, this unified decoder is an almost zero-cost abstraction.
Its tasks are:
SensorTwhere necessaryReview Procedure
Remarks
Current progress
Pre-Review Checklist for the PR Author
PR Author should check the checkboxes below when creating the PR.
Checklist for the PR Reviewer
Reviewers should check the checkboxes below before approval.
Post-Review Checklist for the PR Author
PR Author should check the checkboxes below before merging.
CI Checks