@foxglove/velodyne-cloud
Version:
TypeScript library for converting Velodyne LIDAR packet data to point clouds
324 lines • 16 kB
JavaScript
"use strict";
// This Source Code Form is subject to the terms of the Mozilla Public
// License, v2.0. If a copy of the MPL was not distributed with this
// file, You can obtain one at http://mozilla.org/MPL/2.0/
Object.defineProperty(exports, "__esModule", { value: true });
exports.Transformer = void 0;
const RawPacket_1 = require("./RawPacket");
const VelodyneTypes_1 = require("./VelodyneTypes");
const VLP16_SCANS_PER_FIRING = 16;
const VLP16_BLOCK_TDURATION = 110.592; // [µs]
const VLP16_DSR_TOFFSET = 2.304; // [µs]
const VLP16_FIRING_TOFFSET = 55.296; // [µs]
const VLS128_DISTANCE_RESOLUTION = 0.004; // [m]
class Transformer {
constructor(calibration, { minRange, maxRange, minAngle, maxAngle } = {}) {
const [defMinRange, defMaxRange] = defaultRange(calibration.model);
this.calibration = calibration;
this.minRange = minRange ?? defMinRange;
this.maxRange = maxRange ?? defMaxRange;
this.minAngle = minAngle ?? 0;
this.maxAngle = maxAngle ?? 35999;
}
/**
* Unpack a raw Velodyne packet into a destination PointCloud class.
* @param raw Velodyne packet
* @param scanStamp Timestamp of the first packet in the point cloud, as
* fractional UNIX epoch seconds
* @param packetStamp Optional timestamp of the current packet in the point
* cloud, as fractional UNIX epoch seconds. In unspecified, raw.timestamp()
* will be used
*/
unpack(raw, scanStamp, packetStamp, output) {
packetStamp ?? (packetStamp = raw.timestamp());
switch (raw.factoryId) {
case VelodyneTypes_1.FactoryId.VLP16:
case VelodyneTypes_1.FactoryId.VLP16HiRes:
return this._unpackVLP16(raw, scanStamp, packetStamp, output);
case VelodyneTypes_1.FactoryId.VLS128:
case VelodyneTypes_1.FactoryId.VLS128Old:
return this._unpackVLS128(raw, scanStamp, packetStamp, output);
default:
return this._unpackGeneric(raw, scanStamp, packetStamp, output);
}
}
_unpackGeneric(raw, scanStamp, packetStamp, output) {
const timeDiffStartToThisPacket = packetStamp - scanStamp;
const xyz = [0, 0, 0];
for (let i = 0; i < RawPacket_1.RawPacket.BLOCKS_PER_PACKET; i++) {
const block = raw.blocks[i];
const rawRotation = block.rotation;
// Discard blocks that are outside the area of interest
if (!angleInRange(rawRotation, this.minAngle, this.maxAngle)) {
continue;
}
const timingOffsetsRow = this.calibration.timingOffsets[i] ?? [];
// upper bank lasers are numbered [0..31], lower bank lasers are [32..63]
const bankOrigin = block.isUpperBlock() ? 32 : 0;
for (let j = 0; j < RawPacket_1.RawPacket.SCANS_PER_BLOCK; j++) {
// Discard returns with invalid (zero) distance values
if (!block.isValid(j)) {
continue;
}
const laserNumber = j + bankOrigin;
const corrections = this.calibration.laserCorrections[laserNumber];
// Position
const rawDistance = block.distance(j);
const distance = rawDistance * this.calibration.distanceResolution + corrections.distCorrection;
if (rawDistance === 0 || !distanceInRange(distance, this.minRange, this.maxRange)) {
continue;
}
computePosition(distance, rawRotation, this.calibration, corrections, xyz);
// Intensity
const rawIntensity = block.intensity(j);
const intensity = computeIntensity(rawIntensity, rawDistance, corrections);
// Time
const offsetSec = timeDiffStartToThisPacket + (timingOffsetsRow[j] ?? 0);
output.addPoint(
// Use standard ROS coordinate system (right-hand rule)
xyz[1], -xyz[0], xyz[2], distance, intensity, corrections.laserId, block.rotation, offsetSec * 1e9);
}
}
}
_unpackVLS128(raw, scanStamp, packetStamp, output) {
const timeDiffStartToThisPacket = packetStamp - scanStamp;
const dualReturn = raw.returnMode === VelodyneTypes_1.ReturnMode.DualReturn ? 1 : 0;
const blockCount = RawPacket_1.RawPacket.BLOCKS_PER_PACKET - 4 * dualReturn;
let azimuth = 0;
let azimuthNext = 0;
let azimuthDiff = 0;
let lastAzimuthDiff = 0;
const xyz = [0, 0, 0];
for (let i = 0; i < blockCount; i++) {
const block = raw.blocks[i];
const rawRotation = block.rotation;
const timingOffsetsRow = this.calibration.timingOffsets[~~(i / 4)] ?? [];
const bankOrigin = bankOriginForBlock(block.blockId);
// Calculate difference between current and next block's azimuth angle.
if (i === 0) {
azimuth = rawRotation;
}
else {
azimuth = azimuthNext;
}
if (i < RawPacket_1.RawPacket.BLOCKS_PER_PACKET - (1 + dualReturn)) {
const nextBlock = raw.blocks[i + (1 + dualReturn)];
// Get the next block rotation to calculate how far we rotate between
// blocks
azimuthNext = nextBlock.rotation;
// Finds the difference between two successive blocks
azimuthDiff = (36000 + azimuthNext - azimuth) % 36000;
// This is used when the last block is next to predict rotation amount
lastAzimuthDiff = azimuthDiff;
}
else {
// This makes the assumption the difference between the last block and the
// next packet is the same as the last to the second to last. Assumes RPM
// doesn't change much between blocks
azimuthDiff = i === RawPacket_1.RawPacket.BLOCKS_PER_PACKET - 4 * dualReturn - 1 ? 0 : lastAzimuthDiff;
}
for (let j = 0; j < RawPacket_1.RawPacket.SCANS_PER_BLOCK; j++) {
const rawDistance = block.distance(j);
const distance = rawDistance * VLS128_DISTANCE_RESOLUTION;
if (!distanceInRange(distance, this.minRange, this.maxRange)) {
continue;
}
// Offset the laser in this block by which block it's in
const laserNumber = j + bankOrigin;
// VLS-128 fires 8 lasers at a time
const firingOrder = ~~(laserNumber / 8);
const corrections = this.calibration.laserCorrections[laserNumber];
// correct for the laser rotation as a function of timing during the
// firings
const azimuthCorrection = this.calibration.vls128LaserAzimuthCache[firingOrder];
const azimuthCorrectedF = azimuth + azimuthDiff * azimuthCorrection;
const azimuthCorrected = Math.round(azimuthCorrectedF) % 36000;
if (!angleInRange(azimuthCorrected, this.minAngle, this.maxAngle)) {
continue;
}
// Position
computePosition(distance, rawRotation, this.calibration, corrections, xyz);
// Intensity. VLS-128 intensity values can be used directly
const intensity = block.intensity(j);
// Time
const timingIndex = firingOrder + ~~(laserNumber / 64);
const offsetSec = timeDiffStartToThisPacket + (timingOffsetsRow[timingIndex] ?? 0);
output.addPoint(
// Use standard ROS coordinate system (right-hand rule)
xyz[1], -xyz[0], xyz[2], distance, intensity, corrections.laserId, block.rotation, offsetSec * 1e9);
}
}
}
_unpackVLP16(raw, scanStamp, packetStamp, output) {
const timeDiffStartToThisPacket = packetStamp - scanStamp;
let azimuthDiff = 0;
let lastAzimuthDiff = 0;
const xyz = [0, 0, 0];
for (let i = 0; i < RawPacket_1.RawPacket.BLOCKS_PER_PACKET; i++) {
const block = raw.blocks[i];
const rawRotation = block.rotation;
const timingOffsetsRow = this.calibration.timingOffsets[i] ?? [];
// Calculate difference between current and next block's azimuth angle
if (i < RawPacket_1.RawPacket.BLOCKS_PER_PACKET - 1) {
const nextBlock = raw.blocks[i + 1];
const rawAzimuthDiff = nextBlock.rotation - block.rotation;
azimuthDiff = (36000 + rawAzimuthDiff) % 36000;
// some packets contain an angle overflow where azimuthDiff < 0
if (rawAzimuthDiff < 0) {
azimuthDiff = lastAzimuthDiff;
}
lastAzimuthDiff = azimuthDiff;
}
else {
azimuthDiff = lastAzimuthDiff;
}
for (let j = 0; j < RawPacket_1.RawPacket.SCANS_PER_BLOCK; j++) {
const azimuthCorrected = vlp16AzimuthCorrected(block, j, azimuthDiff);
// Discard points which are not in the area of interest
if (!angleInRange(azimuthCorrected, this.minAngle, this.maxAngle)) {
continue;
}
const dsr = j % VLP16_SCANS_PER_FIRING;
const corrections = this.calibration.laserCorrections[dsr];
// Position
const rawDistance = block.distance(j);
const distance = rawDistance * this.calibration.distanceResolution + corrections.distCorrection;
if (!distanceInRange(distance, this.minRange, this.maxRange)) {
continue;
}
computePosition(distance, rawRotation, this.calibration, corrections, xyz);
// Intensity
const rawIntensity = block.intensity(j);
const intensity = computeIntensity(rawIntensity, rawDistance, corrections);
// Time
const offsetSec = timeDiffStartToThisPacket + (timingOffsetsRow[j] ?? 0);
output.addPoint(
// Use standard ROS coordinate system (right-hand rule)
xyz[1], -xyz[0], xyz[2], distance, intensity, corrections.laserId, block.rotation, offsetSec * 1e9);
}
}
}
}
exports.Transformer = Transformer;
// Check if a given angle is within [min..max], handling wraparound
function angleInRange(angle, min, max) {
if (min <= max) {
return angle >= min && angle <= max;
}
else {
return angle >= min || angle <= max;
}
}
// Check if a given distance is within [min..max]
function distanceInRange(distance, min, max) {
return distance >= min && distance <= max;
}
// Clamp a value in the range of [min..max]
function clamp(value, min, max) {
return value <= min ? min : value >= max ? max : value;
}
// Return the default [min, max] range of valid distances for a given hardware model
function defaultRange(model) {
switch (model) {
case VelodyneTypes_1.Model.VLP16:
case VelodyneTypes_1.Model.VLP16HiRes:
case VelodyneTypes_1.Model.HDL32E:
return [0.4, 100];
case VelodyneTypes_1.Model.HDL64E:
case VelodyneTypes_1.Model.HDL64E_S21:
case VelodyneTypes_1.Model.HDL64E_S3:
return [0.4, 120];
case VelodyneTypes_1.Model.VLP32C:
return [0.4, 200];
case VelodyneTypes_1.Model.VLS128:
return [0.4, 300];
}
}
// Get the first block index for a bank of 32 lasers
function bankOriginForBlock(blockId) {
switch (blockId) {
case VelodyneTypes_1.BlockId.Block_0_To_31:
return 0;
case VelodyneTypes_1.BlockId.Block_32_To_63:
return 32;
case VelodyneTypes_1.BlockId.Block_64_To_95:
return 64;
case VelodyneTypes_1.BlockId.Block_96_To_127:
return 96;
default:
return 0;
}
}
// Faster Math.pow(x, 2)
function sqr(x) {
return x * x;
}
// Correct for the laser rotation as a function of timing during the firings
function vlp16AzimuthCorrected(block, laserIndex, azimuthDiff) {
const dsr = laserIndex % VLP16_SCANS_PER_FIRING;
const firing = ~~(laserIndex / VLP16_SCANS_PER_FIRING);
const azimuthCorrectedF = block.rotation +
(azimuthDiff * (dsr * VLP16_DSR_TOFFSET + firing * VLP16_FIRING_TOFFSET)) /
VLP16_BLOCK_TDURATION;
return Math.round(azimuthCorrectedF) % 36000;
}
// Given an adjusted distance and raw azimuth reading from a laser return and
// calibration data, returns a 3D position
function computePosition(distance, rawRotation, calibration, corrections, output) {
const cosVertAngle = corrections.cosVertCorrection;
const sinVertAngle = corrections.sinVertCorrection;
const cosRotCorrection = corrections.cosRotCorrection;
const sinRotCorrection = corrections.sinRotCorrection;
const cosRot = calibration.cosRotTable[rawRotation];
const sinRot = calibration.sinRotTable[rawRotation];
// cos(a-b) = cos(a)*cos(b) + sin(a)*sin(b)
const cosRotAngle = cosRot * cosRotCorrection + sinRot * sinRotCorrection;
// sin(a-b) = sin(a)*cos(b) - cos(a)*sin(b)
const sinRotAngle = sinRot * cosRotCorrection - cosRot * sinRotCorrection;
const horizOffset = corrections.horizOffsetCorrection;
const vertOffset = corrections.vertOffsetCorrection;
// Compute the distance in the xy plane (w/o accounting for rotation)
let xyDistance = distance * cosVertAngle - vertOffset * sinVertAngle;
// Calculate temporal X, use absolute value
let xx = xyDistance * sinRotAngle - horizOffset * cosRotAngle;
// Calculate temporal Y, use absolute value
let yy = xyDistance * cosRotAngle + horizOffset * sinRotAngle;
if (xx < 0) {
xx = -xx;
}
if (yy < 0) {
yy = -yy;
}
// Get 2 point calibration values, linear interpolation to get distance
// correction for X and Y. This means distance correction uses different
// values at different distances
let distanceCorrX = 0;
let distanceCorrY = 0;
if (corrections.twoPtCorrectionAvailable) {
distanceCorrX =
((corrections.distCorrection - corrections.distCorrectionX) * (xx - 2.4)) / (25.04 - 2.4) +
corrections.distCorrectionX;
distanceCorrX -= corrections.distCorrection;
distanceCorrY =
((corrections.distCorrection - corrections.distCorrectionY) * (yy - 1.93)) / (25.04 - 1.93) +
corrections.distCorrectionY;
distanceCorrY -= corrections.distCorrection;
}
const distanceX = distance + distanceCorrX;
xyDistance = distanceX * cosVertAngle - vertOffset * sinVertAngle;
output[0] = xyDistance * sinRotAngle - horizOffset * cosRotAngle;
const distanceY = distance + distanceCorrY;
xyDistance = distanceY * cosVertAngle - vertOffset * sinVertAngle;
output[1] = xyDistance * cosRotAngle + horizOffset * sinRotAngle;
output[2] = distanceY * sinVertAngle + vertOffset * cosVertAngle;
}
// Given raw intensity and distance readings from a laser return and calibration
// data, returns a corrected intensity reading
function computeIntensity(rawIntensity, rawDistance, corrections) {
const minIntensity = corrections.minIntensity;
const maxIntensity = corrections.maxIntensity;
const focalOffset = 256 * (1 - corrections.focalDistance / 13100) * (1 - corrections.focalDistance / 13100);
const focalSlope = corrections.focalSlope;
return clamp(rawIntensity + focalSlope * Math.abs(focalOffset - 256 * sqr(1 - rawDistance / 65535)), minIntensity, maxIntensity);
}
//# sourceMappingURL=Transformer.js.map