π Robotics/Computer Vision Notes: Coordinate Transformations
This notebook translates handwritten notes on coordinate transformations, focusing on rotation, translation, and properties of rotation matrices, often used in robotics and computer vision (e.g., in systems utilizing ROS 2 and Aruco markers ).
Setup and Data Snippet (ROS 2 Aruco Detection) ΒΆ The notes include a snippet of data from an Aruco detection topic in a ROS 2 environment, which provides the pose (position and orientation) of a detected marker.
The markerβs pose is given relative to a frame identified as /v41_frame.
Rigid Body Transformation Concepts ΒΆ A Rigid body is defined as a set of points where the distance between any two points remains constant after a transformation.
The transformation involves a Rotation (R R R Translation (t t t
The transformation of a point p βΎ J \underline{p}_J p β J β J J J p βΎ W \underline{p}_W p β W β W W W
p βΎ W = W R J p βΎ J + W t βΎ J (1) \underline{p}_W = {}^W R_J \underline{p}_J + {}^W \underline{t}_J \tag{1} p β W β = W R J β p β J β + W t β J β ( 1 ) p βΎ W \underline{p}_W p β W β W W W
p βΎ J \underline{p}_J p β J β J J J
W R J {}^W R_J W R J β Rotation matrix from frame J J J W W W
W t βΎ J {}^W \underline{t}_J W t β J β Translation vector from frame J J J W W W
In 2D, this equation expands to:
( x W y W ) = ( r 11 r 12 r 21 r 22 ) ( x J y J ) + ( t 1 t 2 ) \begin{pmatrix} x_W \\ y_W \end{pmatrix} = \begin{pmatrix} r_{11} & r_{12} \\ r_{21} & r_{22} \end{pmatrix} \begin{pmatrix} x_J \\ y_J \end{pmatrix} + \begin{pmatrix} t_1 \\ t_2 \end{pmatrix} ( x W β y W β β ) = ( r 11 β r 21 β β r 12 β r 22 β β ) ( x J β y J β β ) + ( t 1 β t 2 β β ) 2D Rotation Derivation ΒΆ The notes derive the transformation for a 2D rotation of a point p βΎ = ( x y ) \underline{p} = \begin{pmatrix} x \\ y \end{pmatrix} p β = ( x y β ) ΞΈ \theta ΞΈ p βΎ β² = ( x β² y β² ) \underline{p}' = \begin{pmatrix} x' \\ y' \end{pmatrix} p β β² = ( x β² y β² β )
Given:
x = r cos β‘ Ξ± x = r \cos\alpha x = r cos Ξ±
y = r sin β‘ Ξ± y = r \sin\alpha y = r sin Ξ±
x β² = r cos β‘ ( ΞΈ + Ξ± ) x' = r \cos(\theta+\alpha) x β² = r cos ( ΞΈ + Ξ± )
y β² = r sin β‘ ( ΞΈ + Ξ± ) y' = r \sin(\theta+\alpha) y β² = r sin ( ΞΈ + Ξ± )
Using the angle addition formulas (cos β‘ ( A + B ) = cos β‘ A cos β‘ B β sin β‘ A sin β‘ B \cos(A+B) = \cos A \cos B - \sin A \sin B cos ( A + B ) = cos A cos B β sin A sin B sin β‘ ( A + B ) = sin β‘ A cos β‘ B + cos β‘ A sin β‘ B \sin(A+B) = \sin A \cos B + \cos A \sin B sin ( A + B ) = sin A cos B + cos A sin B
x β² = r cos β‘ ΞΈ cos β‘ Ξ± β r sin β‘ ΞΈ sin β‘ Ξ± x' = r \cos\theta \cos\alpha - r \sin\theta \sin\alpha x β² = r cos ΞΈ cos Ξ± β r sin ΞΈ sin Ξ± Substituting x x x y y y
x β² = x cos β‘ ΞΈ β y sin β‘ ΞΈ x' = x \cos\theta - y \sin\theta x β² = x cos ΞΈ β y sin ΞΈ y β² = r sin β‘ ΞΈ cos β‘ Ξ± + r cos β‘ ΞΈ sin β‘ Ξ± y' = r \sin\theta \cos\alpha + r \cos\theta \sin\alpha y β² = r sin ΞΈ cos Ξ± + r cos ΞΈ sin Ξ± Substituting x x x y y y
y β² = x sin β‘ ΞΈ + y cos β‘ ΞΈ y' = x \sin\theta + y \cos\theta y β² = x sin ΞΈ + y cos ΞΈ In matrix form, the 2D Rotation Matrix R ( ΞΈ ) R(\theta) R ( ΞΈ )
p βΎ β² = R ( ΞΈ ) p βΎ \underline{p}' = R(\theta)\underline{p} p β β² = R ( ΞΈ ) p β ( x β² y β² ) = ( cos β‘ ΞΈ β sin β‘ ΞΈ sin β‘ ΞΈ cos β‘ ΞΈ ) ( x y ) \begin{pmatrix} x' \\ y' \end{pmatrix} = \begin{pmatrix} \cos\theta & -\sin\theta \\ \sin\theta & \cos\theta \end{pmatrix} \begin{pmatrix} x \\ y \end{pmatrix} ( x β² y β² β ) = ( cos ΞΈ sin ΞΈ β β sin ΞΈ cos ΞΈ β ) ( x y β ) To represent rotation and translation (Equation 1) as a single matrix multiplication, homogeneous coordinates are used. A 2D point p βΎ = ( x y ) \underline{p} = \begin{pmatrix} x \\ y \end{pmatrix} p β = ( x y β ) ( x y 1 ) \begin{pmatrix} x \\ y \\ 1 \end{pmatrix} β β β x y 1 β β β β
The Transformation Matrix T T T J J J W W W
W T J = ( W R J W t βΎ J 0 βΎ T 1 ) {}^W T_J = \begin{pmatrix} {}^W R_J & {}^W \underline{t}_J \\ \underline{0}^T & 1 \end{pmatrix} W T J β = ( W R J β 0 β T β W t β J β 1 β ) Where 0 βΎ T \underline{0}^T 0 β T ( 0 0 ) \begin{pmatrix} 0 & 0 \end{pmatrix} ( 0 β 0 β )
The transformation becomes:
( p βΎ W 1 ) = W T J ( p βΎ J 1 ) \begin{pmatrix} \underline{p}_W \\ 1 \end{pmatrix} = {}^W T_J \begin{pmatrix} \underline{p}_J \\ 1 \end{pmatrix} ( p β W β 1 β ) = W T J β ( p β J β 1 β ) Properties of a Rotation Matrix ΒΆ A Rotation Matrix (R R R orthonormal matrix . An orthonormal matrix U U U
1. Inverse and Transpose ΒΆ For any orthonormal matrix U U U
U β 1 = U T U^{-1} = U^T U β 1 = U T Therefore, for a rotation matrix R R R
R β 1 = R T R^{-1} = R^T R β 1 = R T This property is geometrically intuitive: transforming a point back from P β² P' P β² P P P ΞΈ \theta ΞΈ β ΞΈ -\theta β ΞΈ
R β 1 ( ΞΈ ) = R ( β ΞΈ ) = ( cos β‘ ( β ΞΈ ) β sin β‘ ( β ΞΈ ) sin β‘ ( β ΞΈ ) cos β‘ ( β ΞΈ ) ) = ( cos β‘ ΞΈ sin β‘ ΞΈ β sin β‘ ΞΈ cos β‘ ΞΈ ) R^{-1}(\theta) = R(-\theta) = \begin{pmatrix} \cos(-\theta) & -\sin(-\theta) \\ \sin(-\theta) & \cos(-\theta) \end{pmatrix} = \begin{pmatrix} \cos\theta & \sin\theta \\ -\sin\theta & \cos\theta \end{pmatrix} R β 1 ( ΞΈ ) = R ( β ΞΈ ) = ( cos ( β ΞΈ ) sin ( β ΞΈ ) β β sin ( β ΞΈ ) cos ( β ΞΈ ) β ) = ( cos ΞΈ β sin ΞΈ β sin ΞΈ cos ΞΈ β ) Which is R T R^T R T
R T = ( cos β‘ ΞΈ sin β‘ ΞΈ β sin β‘ ΞΈ cos β‘ ΞΈ ) R^T = \begin{pmatrix} \cos\theta & \sin\theta \\ -\sin\theta & \cos\theta \end{pmatrix} R T = ( cos ΞΈ β sin ΞΈ β sin ΞΈ cos ΞΈ β ) Also, R R T = R T R = I R R^T = R^T R = I R R T = R T R = I
2. Determinant ΒΆ For an orthonormal matrix U U U det β‘ ( U ) β { β 1 , + 1 } \det(U) \in \{-1, +1\} det ( U ) β { β 1 , + 1 }
A pure Rotation matrix R R R
det β‘ ( R ) = 1 \det(R) = 1 det ( R ) = 1 For the 2D rotation matrix:
det β‘ ( ( cos β‘ ΞΈ β sin β‘ ΞΈ sin β‘ ΞΈ cos β‘ ΞΈ ) ) = cos β‘ 2 ΞΈ β ( β sin β‘ 2 ΞΈ ) = cos β‘ 2 ΞΈ + sin β‘ 2 ΞΈ = 1 \det\left(\begin{pmatrix} \cos\theta & -\sin\theta \\ \sin\theta & \cos\theta \end{pmatrix}\right) = \cos^2\theta - (-\sin^2\theta) = \cos^2\theta + \sin^2\theta = 1 det ( ( cos ΞΈ sin ΞΈ β β sin ΞΈ cos ΞΈ β ) ) = cos 2 ΞΈ β ( β sin 2 ΞΈ ) = cos 2 ΞΈ + sin 2 ΞΈ = 1 If an orthonormal matrix has det β‘ ( U ) = β 1 \det(U) = -1 det ( U ) = β 1 Reflection matrix . Thus, not all orthonormal matrices are rotation matrices.
The notes also demonstrate how to find the inverse transformation, J T W = ( W T J ) β 1 {}^J T_W = ({}^W T_J)^{-1} J T W β = ( W T J β ) β 1
Starting from the point transformation:
p βΎ W = W R J p βΎ J + W t βΎ J \underline{p}_W = {}^W R_J \underline{p}_J + {}^W \underline{t}_J p β W β = W R J β p β J β + W t β J β 1. Isolate the rotational term:
p βΎ W β W t βΎ J = W R J p βΎ J \underline{p}_W - {}^W \underline{t}_J = {}^W R_J \underline{p}_J p β W β β W t β J β = W R J β p β J β Left-multiply by the transpose of the rotation matrix, W R J T {}^W R_J^T W R J T β J R W {}^J R_W J R W β
W R J T ( p βΎ W β W t βΎ J ) = ( W R J T W R J ) p βΎ J {}^W R_J^T (\underline{p}_W - {}^W \underline{t}_J) = \left( {}^W R_J^T {}^W R_J \right) \underline{p}_J W R J T β ( p β W β β W t β J β ) = ( W R J T β W R J β ) p β J β Since W R J T W R J = I {}^W R_J^T {}^W R_J = I W R J T β W R J β = I
W R J T p βΎ W β W R J T W t βΎ J = p βΎ J {}^W R_J^T \underline{p}_W - {}^W R_J^T {}^W \underline{t}_J = \underline{p}_J W R J T β p β W β β W R J T β W t β J β = p β J β This shows the inverse transformation is:
p βΎ J = W R J T β J R W p βΎ W + ( β W R J T W t βΎ J ) β J t βΎ W \underline{p}_J = \underbrace{{}^W R_J^T}_{{}^J R_W} \underline{p}_W + \underbrace{\left( -{}^W R_J^T {}^W \underline{t}_J \right)}_{{}^J \underline{t}_W} p β J β = J R W β W R J T β β β p β W β + J t β W β ( β W R J T β W t β J β ) β β The inverse transformation matrix J T W {}^J T_W J T W β
J T W = ( W R J T β W R J T W t βΎ J 0 βΎ T 1 ) = ( J R βΎ W J t βΎ W 0 βΎ T 1 ) {}^J T_W = \begin{pmatrix} {}^W R_J^T & -{}^W R_J^T {}^W \underline{t}_J \\ \underline{0}^T & 1 \end{pmatrix} = \begin{pmatrix} {}^J\underline{R}_W & {}^J\underline{t}_W \\ \underline{0}^T & 1 \end{pmatrix} J T W β = ( W R J T β 0 β T β β W R J T β W t β J β 1 β ) = ( J R β W β 0 β T β J t β W β 1 β )