[AUTONAVx][lec3]3D Geometry and Sensors

3D Geometry

Rotation matrices and quaternions can simply be concatenated by multiplication.

Euler angles use three variables to describe three degrees of freedom and thus are minimal.

Angle-Axis can be minimal if the rotation about the angle is encoded in the length of the axis vector.

Rotation matrices can simply be transposed to invert them.Angle Axis can be inverted by negating angle or axis.

Quaternions can be inverted by flipping the sign of v or w.

Only rotation matrices are unique. The other representations allow to express the same rotation in multiple ways.

implement the position inverse and multiply operations.

import numpy as np

class Pose3D:
    def __init__(self, rotation, translation):
        self.rotation = rotation
        self.translation = translation

    def inv(self):
        '''
        Inversion of this Pose3D object

        :return inverse of self
        '''
        # TODO: implement inversion
        inv_rotation = self.rotation.T
        inv_translation = -np.dot(inv_rotation, self.translation)

        return Pose3D(inv_rotation, inv_translation)

    def __mul__(self, other):
        '''
        Multiplication of two Pose3D objects, e.g.:
            a = Pose3D(...) # = self
            b = Pose3D(...) # = other
            c = a * b       # = return value

        :param other: Pose3D right hand side
        :return product of self and other
        '''
        return Pose3D(np.dot(self.rotation, other.rotation), np.dot(self.rotation, other.translation) + self.translation)

    def __str__(self):
        return "rotation:\n" + str(self.rotation) + "\ntranslation:\n" + str(self.translation.transpose())

def compute_quadrotor_pose(global_marker_pose, observed_marker_pose):
    '''
    :param global_marker_pose: Pose3D 
    :param observed_marker_pose: Pose3D

    :return global quadrotor pose computed from global_marker_pose and observed_marker_pose
    '''
    # TODO: implement global quadrotor pose computation
    global_quadrotor_pose = global_marker_pose * observed_marker_pose.inv()

    return global_quadrotor_pose