How to convert local radar laser to global position？

Question

I need to transform the radar sensor data from local coordinates to global coordinates. My approach is to first convert the radar laser data to local coordinates, then obtain the rotation quaternion through /tf, convert the quaternion to yaw angle, and obtain the rotated coordinates through the rotation matrix. Finally, based on the robot's position obtained from /odom, the global coordinates are calculated.

def getPos(self):
    pos = self.odom_data.pose.pose.position
    rotation = self.tf_data.transforms[0].transform.rotation
    roll, pitch, yaw = euler_from_quaternion((rotation.x, rotation.y,
                                                  rotation.z, rotation.w))
    return [pos.x, pos.y, yaw]

def getScan(self):
    scan = self.scan_data.ranges
    rpos = self.getPos()
    a_start = self.scan_data.angle_min
    a_increment = self.scan_data.angle_increment
    scan_xy = []
    for i in range(len(scan)):
        if scan[i] < 0.1:
           continue
        x = scan[i] * math.cos(a_start + i * a_increment)
        y = scan[i] * math.sin(a_start + i * a_increment)
        x = x * math.cos(rpos[2]) - y * math.sin(rpos[2]) + rpos[0]
        y = y * math.cos(rpos[2]) + x * math.sin(rpos[2]) + rpos[1]
        scan_xy.append([x, y])
     return np.array(scan_xy)

The above code is my calculation process. I want to transform the local coordinate positions of the LiDAR into global coordinate positions, but now, after the conversion, it does not achieve what I want. Where is the problem with the code above?

Answer 1

The issue with your method is that when you update x and then use the new x to calculate y. It is the incorrect wayt to do it, because the calculation of y depends on the original x value, not the updated one.

So, to solve this, you need to temporarily store the new x value in a separate variable before updating x. I actually did the same mistake you did in a previous project. You weren't far off, it you just missed a little detail.

Here is a suggested code with dummy data:

import math
import numpy as np

def euler_from_quaternion(quat):
    x, y, z, w = quat
    return (0, 0, math.atan2(2.0*(y*z + w*x), w*w - x*x - y*y + z*z))

class SensorDataTransform:
    def __init__(self, odom_data, scan_data, tf_data):
        self.odom_data = odom_data
        self.scan_data = scan_data
        self.tf_data = tf_data

    def getPos(self):
        pos = self.odom_data['pose']['pose']['position']
        rotation = self.tf_data['transforms'][0]['transform']['rotation']
        _, _, yaw = euler_from_quaternion((rotation['x'], rotation['y'], rotation['z'], rotation['w']))
        return [pos['x'], pos['y'], yaw]

    def getScan(self):
        scan = self.scan_data['ranges']
        rpos = self.getPos()
        a_start = self.scan_data['angle_min']
        a_increment = self.scan_data['angle_increment']
        scan_xy = []
        for i in range(len(scan)):
            if scan[i] < 0.1: 
                continue
            x_local = scan[i] * math.cos(a_start + i * a_increment)
            y_local = scan[i] * math.sin(a_start + i * a_increment)
            x_global = x_local * math.cos(rpos[2]) - y_local * math.sin(rpos[2]) + rpos[0]
            y_global = y_local * math.cos(rpos[2]) + x_local * math.sin(rpos[2]) + rpos[1]
            scan_xy.append([x_global, y_global])
        return np.array(scan_xy)

odom_data = {'pose': {'pose': {'position': {'x': 1, 'y': 2}}}}
tf_data = {'transforms': [{'transform': {'rotation': {'x': 0, 'y': 0, 'z': 0.7071068, 'w': 0.7071068}}}]}

scan_data = {
    'ranges': [1.0, 1.5, 2.0],
    'angle_min': -math.pi / 4,
    'angle_increment': math.pi / 4 
}

transformer = SensorDataTransform(odom_data, scan_data, tf_data)
print(transformer.getScan())

which gives you:

[[1.70710678 1.29289322]
 [2.5        2.        ]
 [2.41421356 3.41421356]]