Lidar in python - Quaternions, Angular Velocity, Linear Accelleration?

[jttolleson]

Hello all: Jayson Here; and I have struggled with getting lidar implemented in python, now i have struck gold with my new UniTree lidar. It reads me data to python and puts it in lists....I just do not know how to make the list of quaternions an xyz list yet for graphing....it looks intimidating so i began this topic.

I i have code which i will share here saved to github which retrieves and graphs the lidar data, however....i get two scan messages which must be combined mathematically...one of distance points and one of imu which has the quaternions...with theta roe and phi or some odd bit.....any how as i approach the main question it is how to make an xyz point list in python from quaternions,angular velocity, linear accelleration.......

[b]would the only way to know theta (in the making of the list) from a lidar machine readout or is it compute-able someway.....[/b].

Ie.....(i do not have auxillaryData or , just rangeData.... and a list of quaternions, the imu message)..

import math

def parseRangeAuxiliaryDataToCloud(auxiliaryData, rangeData, scanCloud):
    if auxiliaryData.packet_id != rangeData.packet_id:
        return False
    
    rotate_yaw_bias = 0
    range_scale = 0.001
    z_bias = 0.0445
    points_num_of_scan = 120
    bias_laser_beam_ = auxiliaryData.b_axis_dist / 1000
    sin_theta = math.sin(auxiliaryData.theta_angle)
    cos_theta = math.cos(auxiliaryData.theta_angle)
    sin_ksi = math.sin(auxiliaryData.ksi_angle)
    cos_ksi = math.cos(auxiliaryData.ksi_angle)
    pitch_cur = auxiliaryData.sys_vertical_angle_start * math.pi / 180.0
    pitch_step = auxiliaryData.sys_vertical_angle_span * math.pi / 180.0
    yaw_cur = (auxiliaryData.com_horizontal_angle_start + rotate_yaw_bias) * math.pi / 180.0
    yaw_step = auxiliaryData.com_horizontal_angle_step / points_num_of_scan * math.pi / 180.0
    range_float = 0
    point = [0, 0, 0, 0]
    
    for i in range(0, points_num_of_scan * 2, 2):
        j = i // 2
        pitch_cur += pitch_step
        yaw_cur += yaw_step
        
        range = (rangeData.point_data[i + 1] << 8) | rangeData.point_data[i]
        if range == 0:
            continue
        
        range_float = range_scale * range
        
        sin_alpha = math.sin(pitch_cur)
        cos_alpha = math.cos(pitch_cur)
        sin_beta = math.sin(yaw_cur)
        cos_beta = math.cos(yaw_cur)
        A = (-cos_theta * sin_ksi + sin_theta * sin_alpha * cos_ksi) * range_float + bias_laser_beam_
        B = cos_alpha * cos_ksi * range_float
        point[0] = cos_beta * A - sin_beta * B
        point[1] = sin_beta * A + cos_beta * B
        point[2] = (sin_theta * sin_ksi + cos_theta * sin_alpha * cos_ksi) * range_float + z_bias
        point[3] = auxiliaryData.reflect_data[j]
        scanCloud.append(point)
    
    return True

this is my(working) code from unitree (mostly) that gets data via udp

import socket
import struct
import time
import sys
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import axes3d
import pydeck
import pandas as pd
import numpy as np
import subprocess
from subprocess import Popen, PIPE, STDOUT, run
# IP and Port
UDP_IP = "0.0.0.0"
UDP_PORT = 80
#cloud_scan_num = str(np.clip(int(sys.argv[1]), 0, 20000))
#subprocess.Popen('./unilidar_publisher_udp '+str(cloud_scan_num), shell=True, stdout=subprocess.PIPE, stderr=STDOUT)
##################################################################################
def plot(x,y,z,intensity):
    fig = plt.figure(figsize = (12, 10))
    ax = plt.axes(projection ="3d")
    ax.grid(visable = True, color ='grey',linestyle ='-.', linewidth = 0.3, alpha = 0.2)
    my_cmap = plt.get_cmap('tab20b')  
    sctt = ax.scatter3D(x, y, z, alpha = 1, c = z, cmap = my_cmap, marker ='.')
    plt.title("Lidar Scan 3D scatter plot")
    fig.colorbar(sctt, ax = ax, shrink = 0.5, aspect = 5)
    plt.savefig('/home/jay/Documents/unitree-lidar/WorkingUnilidar/static/xyz.jpg')
#############################################################################
def plot2(pointlist):
    pointlist2 = pd.DataFrame(pointlist, columns=['x', 'y', 'z','r','g','b','i'])
    target = [pointlist2.x.mean(), pointlist2.y.mean(), pointlist2.z.mean()]
    point_cloud_layer = pydeck.Layer(    "PointCloudLayer",    data=pointlist2,    get_position=["x", "y", "z"],    get_color=["r", "g", "b"],    get_normal=[0, 0, 15],    auto_highlight=True,    pickable=True,    point_size=3,)
    view_state = pydeck.ViewState(target=target, controller=True, rotation_x=15, rotation_orbit=30, zoom=10)
    view = pydeck.View(type="OrbitView", controller=True)
    r = pydeck.Deck(point_cloud_layer, initial_view_state=view_state, views=[view])
    r.to_html("/home/jay/Documents/unitree-lidar/WorkingUnilidar/static/point_cloud_layer.html", css_background_color="#add8e6")
#############################################################################
# Point Type
class PointUnitree:
    def __init__(self, x, y, z, intensity, time, ring):
        self.x = x
        self.y = y
        self.z = z
        self.intensity = intensity
        self.time = time
        self.ring = ring

# Scan Type
class ScanUnitree:
    def __init__(self, stamp, id, validPointsNum, points):
        self.stamp = stamp
        self.id = id
        self.validPointsNum = validPointsNum
        self.points = points

# IMU Type
class IMUUnitree:
    def __init__(self, stamp, id, quaternion, angular_velocity, linear_acceleration):
        self.stamp = stamp
        self.id = id
        self.quaternion = quaternion
        self.angular_velocity = angular_velocity
        self.linear_acceleration = linear_acceleration

# Create UDP socket
sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
sock.bind((UDP_IP, UDP_PORT))

# Calculate Struct Sizes
imuDataStr = "=dI4f3f3f"
imuDataSize = struct.calcsize(imuDataStr)

pointDataStr = "=fffffI"
pointSize = struct.calcsize(pointDataStr)

scanDataStr = "=dII" + 120 * "fffffI"
scanDataSize = struct.calcsize(scanDataStr)

print("pointSize = " +str(pointSize) + ", scanDataSize = " + str(scanDataSize) + ", imuDataSize = " + str(imuDataSize))

while True:
    # Recv data
    data, addr = sock.recvfrom(10000)
    print(f"Received data from {addr[0]}:{addr[1]}")

    msgType = struct.unpack("=I", data[:4])[0]
    print("msgType =", msgType)

    if msgType == 101:  # IMU Message
        length = struct.unpack("=I", data[4:8])[0]
        imuData = struct.unpack(imuDataStr, data[8:8+imuDataSize])
        imuMsg = IMUUnitree(imuData[0], imuData[1], imuData[2:6], imuData[6:9], imuData[9:12])
        quaternion = imuMsg.quaternion
        ang_vel = imuMsg.angular_velocity
        lin_acc = imuMsg.linear_acceleration
        print ('quaternion: ', quaternion,'angularVelocity: ',ang_vel,'linearAcceleration: ',lin_acc)
        time.sleep(14)
    elif msgType == 102:  # Scan Message
        length = struct.unpack("=I", data[4:8])[0]
        stamp = struct.unpack("=d", data[8:16])[0]
        id = struct.unpack("=I", data[16:20])[0]
        validPointsNum = struct.unpack("=I", data[20:24])[0]
        scanPoints = []
        pointStartAddr = 24
        for i in range(validPointsNum):
            pointData = struct.unpack(pointDataStr, data[pointStartAddr: pointStartAddr+pointSize])
            pointStartAddr = pointStartAddr + pointSize
            point = PointUnitree(*pointData)
            scanPoints.append(point)
        scanMsg = ScanUnitree(stamp, id, validPointsNum, scanPoints)

        print("A Scan msg is parsed!")
        print("\tstamp =", scanMsg.stamp, "id =", scanMsg.id)
        print("\tScan size =", scanMsg.validPointsNum)
        time.sleep(8)
        print("\tfirst 10 points (x, y, z, intensity, time, ring) =")
        x = []
        y = []
        z = []
        r = []
        g = []
        b = []
        intensity = []
        for i in range(1,len(scanMsg.points)):
            point = scanMsg.points[i]
            print("\t", point.x, point.y, point.z, point.intensity, point.time, point.ring)
            x.append(point.x)
            y.append(point.y)
            z.append(point.z)
            intensity.append(point.intensity)
            r.append(64)
            g.append(130)
            b.append(109)
        print("\n")
        pointlist = list(zip(x,y,z,r,g,b,intensity))
        print (pointlist[:10])
        plot(x,y,z,intensity)
        plot2(pointlist)
###############################################################
sock.close()

the whole code is available @
https://github.com/Jayson-Tolleson/UniTr...on-Example

[RockBlok]

I'm not a surveyor by any means, but I assuming the lidar data set you have has some sort of georeferencing. The manner in which the georeference is set up will affect what the xyz points actually are, and how you would use them.

Find the formula for the georeferencing and use that to make the xyz points? Trying to work backwards and find the formula sounds like a massive headache. If I were you, then I'd take a look at the data collection device and find what that formula is. Most lidar scanners will also output data directly to a csv file with the xyz points, or just get a point cloud file directly and generate whatever points you need from there. Those files are usually so massive that they require smoothing or other simplification schemes (I like meshes), but that might allow you better control over how whatever you are doing calculates.

This is the basic format of the formula you are using though if you were wondering.

https://en.wikipedia.org/wiki/Georeferencing

[jttolleson]

soo, i solved it finally, what i need is quaternions to x,y,z here is the code i scalped...

import math

def quaternion_to_euler_angle(w, x, y, z):
ysqr = y * y

t0 = +2.0 * (w * x + y * z)
t1 = +1.0 - 2.0 * (x * x + ysqr)
X = math.degrees(math.atan2(t0, t1))

t2 = +2.0 * (w * y - z * x)
t2 = +1.0 if t2 > +1.0 else t2
t2 = -1.0 if t2 < -1.0 else t2
Y = math.degrees(math.asin(t2))

t3 = +2.0 * (w * z + x * y)
t4 = +1.0 - 2.0 * (ysqr + z * z)
Z = math.degrees(math.atan2(t3, t4))

return X, Y, Z