Skip to content

Commit

Permalink
Add individual point timing offset calculations
Browse files Browse the repository at this point in the history 
Finding the precise timestamp for each individual lidar measurement
by calculating the timing offset from the timestamp of the packet.
  • Loading branch information
@christian-larsson
Christian Larsson authored and christian-larsson committed Nov 23, 2018
1 parent e8b8994 commit 47bd261
Show file tree
Hide file tree
Showing 4 changed files with 141 additions and 58 deletions.
3 changes: 3 additions & 0 deletions spherical_point_cloud.go
View file
Original file line number Diff line number Diff line change
Expand Up @@ -17,6 +17,7 @@ type SphericalPoint struct {
Elevation float64
Reflectivity Reflectivity
LastReflection bool
TimingOffset float64
}

func (cloud *SphericalPointCloud) UnmarshalPacket(packet *Packet) error {
Expand All @@ -32,6 +33,7 @@ func (cloud *SphericalPointCloud) UnmarshalPacket(packet *Packet) error {

func (cloud *SphericalPointCloud) parseBlock(blockIndex int, packet *Packet) error {
azimuth := packet.Blocks[blockIndex].Azimuth
timingOffsets := calculateTimingOffset(packet.ReturnMode)
for j := 0; j < len(packet.Blocks[0].Channels); j++ {
if j == 16 {
azimuth = interpolateAzimuth(blockIndex, packet)
Expand All @@ -57,6 +59,7 @@ func (cloud *SphericalPointCloud) parseBlock(blockIndex int, packet *Packet) err
point.Elevation = verticalAngle(j)
point.Reflectivity = packet.Blocks[blockIndex].Channels[j].Reflectivity
point.LastReflection = lastReturn
point.TimingOffset = timingOffsets[j][blockIndex]

cloud.SphericalPoints = append(cloud.SphericalPoints, point)
}
Expand Down
140 changes: 85 additions & 55 deletions spherical_point_cloud_test.go
View file
Original file line number Diff line number Diff line change
@@ -1,10 +1,11 @@
package vlp16
package vlp16_test

import (
"io"
"os"
"testing"

"github.com/einride/vlp-16-go"
"github.com/stretchr/testify/assert"
"github.com/stretchr/testify/require"
)
Expand All @@ -14,10 +15,10 @@ func TestSimulateRead(t *testing.T) {
testData, err := os.Open(testDataFile)

require.NoError(t, err)
var packet Packet
var packet vlp16.Packet

for {
cloud := SphericalPointCloud{}
cloud := vlp16.SphericalPointCloud{}
err = packet.Read(testData)
if err == io.EOF {
break
Expand All @@ -36,20 +37,48 @@ func TestSimulateRead(t *testing.T) {
func TestLastReflection(t *testing.T) {
test := assert.New(t)

cloud := SphericalPointCloud{}
cloud := vlp16.SphericalPointCloud{}
err := cloud.UnmarshalPacket(&examplePacketLastReflection)
if err != nil {
test.Error(err)
}
test.Equal(cloud.SphericalPoints[0].LastReflection, true)
test.Equal(cloud.SphericalPoints[24].LastReflection, false)
test.Equal(cloud.SphericalPoints[21].LastReflection, false)
}

var examplePacketLastReflection = Packet{
Blocks: [12]Block{
func TestTimingOffset(t *testing.T) {
test := assert.New(t)

eps := 0.005

cloud := vlp16.SphericalPointCloud{}
err := cloud.UnmarshalPacket(&examplePacketLastReflection)
if err != nil {
test.Error(err)
}
test.InDelta(2.304, cloud.SphericalPoints[0].TimingOffset, eps)
test.InDelta(34.560, cloud.SphericalPoints[10].TimingOffset, eps)
test.InDelta(89.856, cloud.SphericalPoints[20].TimingOffset, eps)
test.InDelta(642.816,
cloud.SphericalPoints[len(cloud.SphericalPoints)-1].TimingOffset, eps)

cloud.SphericalPoints = cloud.SphericalPoints[:0]
err = cloud.UnmarshalPacket(&examplePacket)
if err != nil {
test.Error(err)
}
test.InDelta(2.304, cloud.SphericalPoints[0].TimingOffset, eps)
test.InDelta(34.560, cloud.SphericalPoints[10].TimingOffset, eps)
test.InDelta(89.856, cloud.SphericalPoints[20].TimingOffset, eps)
test.InDelta(1306.37,
cloud.SphericalPoints[len(cloud.SphericalPoints)-1].TimingOffset, eps)
}

var examplePacketLastReflection = vlp16.Packet{
Blocks: [12]vlp16.Block{
{
Azimuth: 0x2866,
Channels: [32]Channel{
Channels: [32]vlp16.Channel{
{Distance: 0x0, Reflectivity: 0x2},
{Distance: 0x2ff, Reflectivity: 0x3}, // First valid point, last return
{Distance: 0x0, Reflectivity: 0x3},
Expand All @@ -65,7 +94,7 @@ var examplePacketLastReflection = Packet{
{Distance: 0x2a9, Reflectivity: 0x23},
{Distance: 0x2c5, Reflectivity: 0x32},
{Distance: 0x2c3, Reflectivity: 0x1d},
{Distance: 0x2d9, Reflectivity: 0x4a},
{Distance: 0x2d9, Reflectivity: 0x4a}, // 11th valid point, Laser ID 15
{Distance: 0x0, Reflectivity: 0x2},
{Distance: 0x30f, Reflectivity: 0x7},
{Distance: 0x0, Reflectivity: 0x2},
Expand All @@ -81,16 +110,16 @@ var examplePacketLastReflection = Packet{
{Distance: 0x2b5, Reflectivity: 0x26},
{Distance: 0x2d6, Reflectivity: 0x39},
{Distance: 0x2c6, Reflectivity: 0x18},
{Distance: 0x2d3, Reflectivity: 0x32},
{Distance: 0x2d3, Reflectivity: 0x32}, // 21st valid point, Laser ID 31
},
},
{
Azimuth: 0x288e,
Channels: [32]Channel{
Channels: [32]vlp16.Channel{
{Distance: 0x0, Reflectivity: 0x2},
{Distance: 0x0, Reflectivity: 0x64},
{Distance: 0x0, Reflectivity: 0x3},
{Distance: 0x318, Reflectivity: 0x4c}, // 25th valid point, not last return
{Distance: 0x318, Reflectivity: 0x4c}, // 22nd valid point, not last return
{Distance: 0x0, Reflectivity: 0x2},
{Distance: 0x2ad, Reflectivity: 0x3e},
{Distance: 0x0, Reflectivity: 0x3},
Expand Down Expand Up @@ -123,7 +152,7 @@ var examplePacketLastReflection = Packet{
},
{
Azimuth: 0x28b6,
Channels: [32]Channel{
Channels: [32]vlp16.Channel{
{Distance: 0x0, Reflectivity: 0x2},
{Distance: 0x31c, Reflectivity: 0x45},
{Distance: 0x0, Reflectivity: 0x2},
Expand Down Expand Up @@ -160,7 +189,7 @@ var examplePacketLastReflection = Packet{
},
{
Azimuth: 0x28de,
Channels: [32]Channel{
Channels: [32]vlp16.Channel{
{Distance: 0x0, Reflectivity: 0x2},
{Distance: 0x321, Reflectivity: 0x41},
{Distance: 0x0, Reflectivity: 0x2},
Expand Down Expand Up @@ -197,7 +226,7 @@ var examplePacketLastReflection = Packet{
},
{
Azimuth: 0x2906,
Channels: [32]Channel{
Channels: [32]vlp16.Channel{
{Distance: 0x0, Reflectivity: 0x2},
{Distance: 0x322, Reflectivity: 0x43},
{Distance: 0x0, Reflectivity: 0x2},
Expand Down Expand Up @@ -232,44 +261,45 @@ var examplePacketLastReflection = Packet{
{Distance: 0x2d7, Reflectivity: 0x3b},
},
},
{Azimuth: 0x292e, Channels: [32]Channel{
{Distance: 0x0, Reflectivity: 0x2},
{Distance: 0x32b, Reflectivity: 0x41},
{Distance: 0x0, Reflectivity: 0x2},
{Distance: 0x324, Reflectivity: 0x4b},
{Distance: 0x0, Reflectivity: 0x2},
{Distance: 0x2bf, Reflectivity: 0x40},
{Distance: 0x0, Reflectivity: 0x3},
{Distance: 0x2c0, Reflectivity: 0x3f},
{Distance: 0x0, Reflectivity: 0x1},
{Distance: 0x2c3, Reflectivity: 0x41},
{Distance: 0x2d1, Reflectivity: 0x2a},
{Distance: 0x2ca, Reflectivity: 0x3c},
{Distance: 0x2c4, Reflectivity: 0x43},
{Distance: 0x2cd, Reflectivity: 0x35},
{Distance: 0x318, Reflectivity: 0x3b},
{Distance: 0x2dd, Reflectivity: 0x35},
{Distance: 0x0, Reflectivity: 0x2},
{Distance: 0x32f, Reflectivity: 0x3d},
{Distance: 0x0, Reflectivity: 0x2},
{Distance: 0x326, Reflectivity: 0x4c},
{Distance: 0x0, Reflectivity: 0x4},
{Distance: 0x2be, Reflectivity: 0x3b},
{Distance: 0x0, Reflectivity: 0x3},
{Distance: 0x2c6, Reflectivity: 0x35},
{Distance: 0x0, Reflectivity: 0x1},
{Distance: 0x2c5, Reflectivity: 0x3c},
{Distance: 0x2d1, Reflectivity: 0x2d},
{Distance: 0x2cc, Reflectivity: 0x3c},
{Distance: 0x2ca, Reflectivity: 0x43},
{Distance: 0x2cf, Reflectivity: 0x31},
{Distance: 0x310, Reflectivity: 0x12},
{Distance: 0x2da, Reflectivity: 0x30},
},
{Azimuth: 0x292e,
Channels: [32]vlp16.Channel{
{Distance: 0x0, Reflectivity: 0x2},
{Distance: 0x32b, Reflectivity: 0x41},
{Distance: 0x0, Reflectivity: 0x2},
{Distance: 0x324, Reflectivity: 0x4b},
{Distance: 0x0, Reflectivity: 0x2},
{Distance: 0x2bf, Reflectivity: 0x40},
{Distance: 0x0, Reflectivity: 0x3},
{Distance: 0x2c0, Reflectivity: 0x3f},
{Distance: 0x0, Reflectivity: 0x1},
{Distance: 0x2c3, Reflectivity: 0x41},
{Distance: 0x2d1, Reflectivity: 0x2a},
{Distance: 0x2ca, Reflectivity: 0x3c},
{Distance: 0x2c4, Reflectivity: 0x43},
{Distance: 0x2cd, Reflectivity: 0x35},
{Distance: 0x318, Reflectivity: 0x3b},
{Distance: 0x2dd, Reflectivity: 0x35},
{Distance: 0x0, Reflectivity: 0x2},
{Distance: 0x32f, Reflectivity: 0x3d},
{Distance: 0x0, Reflectivity: 0x2},
{Distance: 0x326, Reflectivity: 0x4c},
{Distance: 0x0, Reflectivity: 0x4},
{Distance: 0x2be, Reflectivity: 0x3b},
{Distance: 0x0, Reflectivity: 0x3},
{Distance: 0x2c6, Reflectivity: 0x35},
{Distance: 0x0, Reflectivity: 0x1},
{Distance: 0x2c5, Reflectivity: 0x3c},
{Distance: 0x2d1, Reflectivity: 0x2d},
{Distance: 0x2cc, Reflectivity: 0x3c},
{Distance: 0x2ca, Reflectivity: 0x43},
{Distance: 0x2cf, Reflectivity: 0x31},
{Distance: 0x310, Reflectivity: 0x12},
{Distance: 0x2da, Reflectivity: 0x30},
},
},
{
Azimuth: 0x2956,
Channels: [32]Channel{
Channels: [32]vlp16.Channel{
{Distance: 0x0, Reflectivity: 0x2},
{Distance: 0x331, Reflectivity: 0x3d},
{Distance: 0x0, Reflectivity: 0x2},
Expand Down Expand Up @@ -306,7 +336,7 @@ var examplePacketLastReflection = Packet{
},
{
Azimuth: 0x297d,
Channels: [32]Channel{
Channels: [32]vlp16.Channel{
{Distance: 0x0, Reflectivity: 0x2},
{Distance: 0x333, Reflectivity: 0x3d},
{Distance: 0x0, Reflectivity: 0x2},
Expand Down Expand Up @@ -343,7 +373,7 @@ var examplePacketLastReflection = Packet{
},
{
Azimuth: 0x29a5,
Channels: [32]Channel{
Channels: [32]vlp16.Channel{
{Distance: 0x0, Reflectivity: 0x2},
{Distance: 0x33a, Reflectivity: 0x38},
{Distance: 0x0, Reflectivity: 0x2},
Expand Down Expand Up @@ -380,7 +410,7 @@ var examplePacketLastReflection = Packet{
},
{
Azimuth: 0x29cc,
Channels: [32]Channel{
Channels: [32]vlp16.Channel{
{Distance: 0x0, Reflectivity: 0x2},
{Distance: 0x339, Reflectivity: 0x3c},
{Distance: 0x0, Reflectivity: 0x2},
Expand Down Expand Up @@ -417,7 +447,7 @@ var examplePacketLastReflection = Packet{
},
{
Azimuth: 0x29f5,
Channels: [32]Channel{
Channels: [32]vlp16.Channel{
{Distance: 0x0, Reflectivity: 0x2},
{Distance: 0x344, Reflectivity: 0x3a},
{Distance: 0x0, Reflectivity: 0x3},
Expand Down Expand Up @@ -454,7 +484,7 @@ var examplePacketLastReflection = Packet{
},
{
Azimuth: 0x2a1d,
Channels: [32]Channel{
Channels: [32]vlp16.Channel{
{Distance: 0x0, Reflectivity: 0x2},
{Distance: 0x346, Reflectivity: 0x38},
{Distance: 0x0, Reflectivity: 0x2},
Expand Down
26 changes: 23 additions & 3 deletions vlp16.go
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,9 +5,11 @@ import (
)

const (
distanceFactor = 0.002 // 2/1000. A reported value of 51154 represents 102.308 meter
azimuthFactor = 0.01 // Azimuth is uint16 representing an angle in one hundredth of a degree
maxAzimuth = 35999 // Azimuth max value as binary
distanceFactor = 0.002 // 2/1000. A reported value of 51154 represents 102.308 meter
azimuthFactor = 0.01 // Azimuth is uint16 representing an angle in one hundredth of a degree
maxAzimuth = 35999 // Azimuth max value as binary
fullFiringTime = 55.296 // Total time for laser firings plus recharge (µs)
singleFiringTime = 2.304 // Time for one laser firing (µs)
)

var verticalAngles = [16]float64{
Expand All @@ -28,6 +30,24 @@ var verticalAngles = [16]float64{
deg2Rad(-1),
deg2Rad(15)}

func calculateTimingOffset(returnMode ReturnMode) [32][12]float64 {
var timingOffsets [32][12]float64
var dataBlockIndex int
for y, inner := range timingOffsets {
for x := range inner {
if returnMode == ReturnModeDualReturn {
dataBlockIndex = (x - (x % 2)) + (y / 16)
} else {
dataBlockIndex = (x * 2) + (y / 16)
}
dataPointIndex := y % 16
timingOffsets[y][x] = fullFiringTime*float64(dataBlockIndex) +
singleFiringTime*float64(dataPointIndex)
}
}
return timingOffsets
}

func spherical2XYZ(laserID int, azimuth uint16, distance uint16) (float64, float64, float64) {
omega := verticalAngle(laserID)
r := float64(distance) * distanceFactor
Expand Down
30 changes: 30 additions & 0 deletions vlp16_test.go
View file
Original file line number Diff line number Diff line change
Expand Up @@ -14,6 +14,36 @@ const (
testDataFile = "test/testdata"
)

func TestCalculateTimeOffset(t *testing.T) {
test := assert.New(t)

eps := 0.005

// Expected values taken from Figure 9-9 Single Return Mode Timing Offsets (µs), VLP-16 Manual
timingOffsetsLastReturn := calculateTimingOffset(ReturnModeLastReturn)
test.InDelta(0.0, timingOffsetsLastReturn[0][0], eps)
test.InDelta(34.560, timingOffsetsLastReturn[15][0], eps)
test.InDelta(55.296, timingOffsetsLastReturn[16][0], eps)
test.InDelta(89.856, timingOffsetsLastReturn[31][0], eps)
test.InDelta(110.592, timingOffsetsLastReturn[0][1], eps)
test.InDelta(145.152, timingOffsetsLastReturn[15][1], eps)
test.InDelta(165.888, timingOffsetsLastReturn[16][1], eps)
test.InDelta(200.448, timingOffsetsLastReturn[31][1], eps)
test.InDelta(1306.37, timingOffsetsLastReturn[31][11], eps)

// Expected values taken from Figure 9-10 Dual Return Mode Timing Offsets (µs), VLP-16 Manual
timingOffsetsDualReturn := calculateTimingOffset(ReturnModeDualReturn)
test.InDelta(0.0, timingOffsetsDualReturn[0][0], eps)
test.InDelta(34.560, timingOffsetsDualReturn[15][0], eps)
test.InDelta(55.296, timingOffsetsDualReturn[16][0], eps)
test.InDelta(89.856, timingOffsetsDualReturn[31][0], eps)
test.InDelta(0.0, timingOffsetsDualReturn[0][1], eps)
test.InDelta(34.560, timingOffsetsDualReturn[15][1], eps)
test.InDelta(55.296, timingOffsetsDualReturn[16][1], eps)
test.InDelta(89.856, timingOffsetsDualReturn[31][1], eps)
test.InDelta(642.816, timingOffsetsDualReturn[31][11], eps)
}

func TestInterpolateAzimuth(t *testing.T) {
test := assert.New(t)
testData, err := os.Open(testDataFile)
Expand Down

0 comments on commit 47bd261

Please sign in to comment.