0
$\begingroup$

I'm currently dealing with a dataset with points clouds collected by a lidar sensor and radar sensor. Unlike datasets like kitti however, they only provided info about the positions of the sensor relative to the origin of a car. For example:

sensor x(m) y(m) z(m) yaw(°) pitch(°) roll(°)
Lidar -1.5 0 1.5 5 -2 0
Radar 0.05 -0.5 1 0 5 180

The problem is I'm not sure how I could calculate the homogenous transformation matrices for each sensor to transform points in each sensor frame into the origin (0,0,0) reference frame.

I tried finding the Denavit–Hartenberg parameters. But due to the two rotations of each sensor, I cant seem to match the coordinates frames and get the corresponding parameters. Any advice would be appreciated!

$\endgroup$

1 Answer 1

2
$\begingroup$

You can do this easily just by using homogeneous transformation.

Attach a local frame to each sensor , and make sure your sensor measurements can be represented in this frame. For example, I would suggest to align the z-axis w.r.t sensor axis. In your case sensors' positions and orientations are already given. But generally, get the positions and orientations of each of these frames w.r.t global frame. You can have a global frame anywhere you want , but ideally at the geometric center of the robot .

Now just by using matrix multiplication , you can represent the sensor values of local frame to global frame. For example, if your sensor measurements in local frame are defined by P , and homogeneous transformation of sensor frame in global frame by H . then the sensor readings in global frame are H.P .

Edit : I seem to have missed the explanation about computing the Homogeneous matrix. Updating that part here.

Homogeneous matrix is combined way of representing overall rotation and translation. For representing the rotation effectively , we need more information than what you have provided in the question. For now , I am assuming the ZYX Euler representation . That means , your overall rotation would be : $R_z \times R_y \times R_x $ . Here , X, Y ,Z rotations are roll , pitch and yaw respectively. Translation component is just added to your overall rotation matrix in the right , along with homogeneous component. Homogeneous matrices for each sensor can be computed as given in the following python program. You can read more about the theory behind it here.

from scipy.spatial.transform import Rotation
import numpy as np 

class Transformation:
    def __init__(self,x=0,y=0,z=0,alpha=0,beta=0,gamma=0) -> None:
        self.alpha =alpha  #rotation w.r.t x in degrees 
        self.beta = beta   #rotation w.r.t y in degrees
        self.gamma = gamma  # rotation w.r.t z  in degrees 
        self.x = x  # X translation 
        self.y = y  # Y translation
        self.z = z  # Z translation 
    def xRotation(self):
        return Rotation.from_euler("X",self.alpha,degrees=True).as_matrix()
    def yRotation(self):
        return Rotation.from_euler("Y",self.beta,degrees=True).as_matrix()
    def zRotation(self):
        return Rotation.from_euler("Z",self.gamma,degrees=True).as_matrix()
    def xyzRotation(self):
        return Rotation.from_euler("ZYX",[self.alpha,self.beta ,self.gamma],degrees = True).as_matrix()
        # return np.linalg.multi_dot([self.xRotation(),self.yRotation(),self.zRotation()])  #self.xRotation().dot(self.yRotation()).dot(self.zRotation())  #
    def translation(self):
        return np.array([[self.x],[self.y],[self.z]])
    def homogeneous(self):
        rot_trans = np.concatenate((self.xyzRotation(),self.translation()),1) #np.concatenate
        bottom_row = np.array([[0,0,0,1]])  # this is required to convert to homogeneous matrix 
        return np.concatenate((rot_trans,bottom_row),0)  # use this as result.dot([point,1]). Here result is  4x4 numpy array containing homogenous matrix 


if __name__ == "__main__":
    # First computing the homogeneous matrix for Lidar 
    x,y,z,alpha,beta,gamma = -1.5,0,1.5,0,-2,5 
    t = Transformation(x,y,z,alpha,beta,gamma)
    print("H matrix for Lidar sensor")
    print(np.round(t.homogeneous(),2))

    # Homogenous matrix for Radar 

    x,y,z,alpha,beta,gamma = 0.05,-0.5,1,180,5,0 
    t = Transformation(x,y,z,alpha,beta,gamma)
    print("H matirx for Radar Sensor")
    print(np.round(t.homogeneous(),2)) 

The result of it is :

H matrix for Lidar sensor
[[ 1.   -0.   -0.03 -1.5 ]
 [ 0.    1.   -0.09  0.  ]
 [ 0.03  0.09  1.    1.5 ]
 [ 0.    0.    0.    1.  ]]

H matirx for Radar Sensor
[[-1.   -0.   -0.09  0.05]
 [ 0.   -1.    0.   -0.5 ]
 [-0.09  0.    1.    1.  ]
 [ 0.    0.    0.    1.  ]]

After obtaining the H matrix , you can multiply the corresponding measurement with it to obtain the measurement in global frame.

For example, if your Lidar measurement is :$[1,1.5,-1.3]^T $ , then its representation in global frame would be:

H_lidar = np.round(t.homogeneous(),2) 
lidar_measurement = np.array([[1,1.5,-1.3,1]]).T
print(np.dot(H_lidar,lidar_measurement))

This prints :

[[-0.461]
 [ 1.617]
 [ 0.365]
 [ 1.   ]]

The one is added in the last row to represent the homogeneous transformation.

Concepts to be careful about:

  1. Multiplication of rotation matrices is not commutative
  2. Know the difference between fixed angle rotation and current ange rotation . They are not same!
$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.

Not the answer you're looking for? Browse other questions tagged or ask your own question.