Working with Orientations and Transformations

This document introduces essential 3D mathematical functions used to handle orientations and transformations in 3D space. It covers the basics of converting between Euler angles, quaternions, and transformation matrices.

Euler to quaternion conversion 

This Python function converts Euler angles (yaw, pitch, roll) to a quaternion, and the result is returned as a list [qx, qy, qz, qw]

def euler_to_quaternion(yaw, pitch, roll):
    qx = np.sin(roll / 2) * np.cos(pitch / 2) * np.cos(yaw / 2) - np.cos(roll / 2) * np.sin(pitch / 2) * np.sin(yaw / 2)
    qy = np.cos(roll / 2) * np.sin(pitch / 2) * np.cos(yaw / 2) + np.sin(roll / 2) * np.cos(pitch / 2) * np.sin(yaw / 2)
    qz = np.cos(roll / 2) * np.cos(pitch / 2) * np.sin(yaw / 2) - np.sin(roll / 2) * np.sin(pitch / 2) * np.cos(yaw / 2)
    qw = np.cos(roll / 2) * np.cos(pitch / 2) * np.cos(yaw / 2) + np.sin(roll / 2) * np.sin(pitch / 2) * np.sin(yaw / 2)
    return [qx, qy, qz, qw]

Matrix to quaternion and translation

This Python code for a function that converts a 4x4 transformation matrix into a quaternion and translation vector. It returns both the quaternion and the translation

from transforms3d.quaternions import mat2quat
def matrix_to_quaternion_translation(matrix):
    quaternion = mat2quat(matrix[:3, :3])
    translation = matrix[:3, 3]
    return quaternion, translation

 Quaternion and translation to matrix

This function converts a quaternion and a translation vector into a 4x4 transformation matrix. The translation vector is added to the last column of the matrix.

from transforms3d.quaternions import quat2mat
def quaternion_translation_to_matrix(quaternion, translation):
    rotation_matrix = quat2mat(quaternion)
    matrix = np.eye(4)
    matrix[:3, :3] = rotation_matrix
    matrix[:3, 3] = translation
    return matrix

💡 In most cases, transformation matrices are based on a common anchor point, often the back axle of a vehicle. Converting these matrices into x, y, z translations and quaternions allows you to format them correctly for calibration files.

When working with dependent matrices, such as baselink -> Lidar and Lidar -> Camera, you can determine each component's position relative to the base. For example, to find the camera's position, first get the lidar position with baselink -> Lidar, then multiply this by Lidar -> Camera.

Lens Distortion Types and Supported Models 

This section illustrates three common types of lens distortion: Barrel, Pincushion, and Mustache. Each type uniquely affects the appearance of a grid, demonstrating how straight lines can appear curved or warped due to the optical characteristics of the lens.

• Barrel Distortion: Causes lines to bow outward from the center, creating a "bulging" effect.
• Pincushion Distortion: Lines bend inward towards the center, giving a "pinched" appearance.
• Mustache Distortion: A combination of barrel and pincushion effects, with lines bending outward in the center and inward towards the edges.

Barrel

Pincushion

Mustache

Supported Distortion Models 

• Kannala-Brandt: Also referred to as Fisheye in OpenCV, this model is commonly used for wide-angle lenses. 
• Mei: Known as Omnidirectional in OpenCV, it is suitable for lenses with more complex distortion patterns, such as those in 360-degree cameras.