Integration of ArduPilot and VIO tracking camera (Part 4): non-ROS bridge to MAVLink in Python

Blog
, ,
45

Hi @decades, thank you for your interest in the code.

First, let’s discuss what we want to achieve before delving into the code. This is a snippet from the blog post, so you can refer to the main discussion in the blog as to why and how the calculations come about.

  • A homogenous transformation matrix (referred to as matrix from here on) combines both the rotation and position into one 4x4 matrix:

In short, what we do in the code is first setting up the matrices (1), (2), (3) then multiplying these matrices (dot product of arrays in numpy) in the right order.

Now for the code:

  • H_T265Ref_T265body corresponds to the matrix (2), which is:

    • the homogeneous transformation matrix (H)
    • in the reference frame of the camera (T265Ref)
    • of the T265’s tracking pose (T265body).

  • The matrices (1) and (3) are H_aeroRef_T265Ref and H_T265body_aeroBody, respectively, and are defined in the earlier part of the code. You can figure out the meaning of their names in a similar way to (2). Their values depend on the initial orientation of the camera and do not change throughout the operation.

  • The data variable is the 6-DoF pose data that we obtained from the T265 camera. Once the latest pose is available, we extract the relevant information (position + rotation) and stuff them into the matrix (2), or H_T265Ref_T265body:

# In transformations, Quaternions w+ix+jy+kz are represented as [w, x, y, z]!
H_T265Ref_T265body = tf.quaternion_matrix([data.rotation.w, data.rotation.x, data.rotation.y, data.rotation.z]) 
H_T265Ref_T265body[0][3] = data.translation.x * scale_factor
H_T265Ref_T265body[1][3] = data.translation.y * scale_factor
H_T265Ref_T265body[2][3] = data.translation.z * scale_factor

  • The quaternion_matrix function creates a 4x4 matrix with the rotation part, then the translation part is added in the last column.

  • The scale_factor is introduced in part 5 of the series. By default its value is 1 and you can ignore it here.

  • Finally, we can realize the equation (4)=(1)x(2)x(3) with a concatenated matrix multiplication (dot product). Read from left to right, it’s equivalent to (1)x(2)x(3):

H_aeroRef_aeroBody = H_aeroRef_T265Ref.dot( H_T265Ref_T265body.dot( H_T265body_aeroBody))
3 Likes