Rodrigues Rotation Formula

Axis–angle is a minimal 3-parameter representation of 3D rotations: a unit vector (rotation axis) and a scalar angle, while a rotation matrix is a 3×3 orthonormal matrix in SO(3). Rodrigues’ formula gives a direct, smooth mapping from axis–angle to a rotation matrix using sines and cosines of the rotation angle and a skew-symmetric matrix built from the axis. This is heavily used in SfM/SLAM because it is both numerically stable and easy to differentiate for optimization.

In practice, you often store a rotation as a 3D vector $$\mathbf{r}$\in \mathbb{R}^3$, where its direction is the rotation axis and its magnitude is the rotation angle: $$\theta$= \|$\mathbf{r}$\|,\$\mathbf{k}$=$\mathbf{r}$/$\theta$$. Rodrigues’ formula then produces a 3×3 matrix $R$ that you can use to rotate 3D points, compose poses, and compute Jacobians with respect to the three parameters.

---

1. Background Knowledge (concepts/theory)

• SO(3) and rotation matrices

• 3D rotations form a Lie group called SO(3): the set of $3 \times 3$ real matrices $R$ such that $R^\top R = I$ and $\det(R) = 1$.

• Rotation matrices are redundant (9 entries with 6 constraints) and can be awkward in optimization.

• Axis–angle and the Lie algebra so(3)

• Any 3D rotation can be represented by a unit axis $\mathbf{k}$ and an angle $\theta$.

• The Lie algebra so(3) is the space of skew-symmetric $3\times 3$ matrices; each such matrix corresponds to a 3D vector via the hat operator:

$$\mathbf{k} = (k_x,k_y,k_z) \;\Longrightarrow\; [\mathbf{k}]_\times = \begin{pmatrix} 0 & -k_z & k_y \\ k_z & 0 & -k_x \\ -k_y & k_x & 0 \end{pmatrix}.$$

• The exponential map $\exp: \mathfrak{so}(3) \to$\text{SO}$(3)$ is what turns an axis–angle (a vector in $\mathbb{R}^3$) into a rotation matrix.

• Rodrigues as the matrix exponential

• Rodrigues’ formula is exactly the closed-form expression of $\exp($\theta$[$\mathbf{k}$]_\times)$:

$$R = I + \sin\theta\,[\mathbf{k}]_\times + (1-\cos\theta)\,[\mathbf{k}]_\times^2.$$

• This is preferred in SfM/SLAM because:
• It uses 3 parameters.
• It is smooth and avoids gimbal lock (unlike Euler angles).
• It has nice derivatives for gradient-based optimization.

---

2. Algorithm / Approach

The pattern for this type of problem:

1. Interpret the input:

• Decide whether the input is:
• A unit axis $\mathbf{k}$ and angle $\theta$, or
• A 3D vector $\mathbf{r}$ where $\mathbf{r} =$\theta$\mathbf{k}$.

2. Normalize and extract axis/angle:

• Compute $\theta = \|$\mathbf{r}$\|$ and $\mathbf{k} =$\mathbf{r}$/\theta$ if needed.
• Handle the small-angle case separately.

3. Build the skew-symmetric matrix:

• Construct $[$\mathbf{k}$]_\times$ from $k_x, k_y, k_z$.

4. Apply Rodrigues’ formula:

• Use the given formula with $\sin\theta$, $\cos\theta$, and $[$\mathbf{k}$]_\times^2$.

5. Return the 3×3 rotation matrix.

This is a direct formula implementation problem with attention to numerical stability.

---

3. Step-by-Step Strategy

Assume your function receives a 3D vector r (axis–angle):

1. Compute angle and early exit for no rotation

theta = np.linalg.norm(r)
if theta < eps:
return np.eye(3) # approximately zero rotation

2. Compute unit axis

k = r / theta # shape (3,)
kx, ky, kz = k

3. Construct skew-symmetric matrix [k]_x

K = np.array([[ 0, -kz, ky],
[ kz, 0, -kx],
[ -ky, kx, 0]])

4. Precompute trig functions

c = np.cos(theta)
s = np.sin(theta)

5. Compute K^2

K2 = K @ K

6. Assemble rotation matrix using Rodrigues

I = np.eye(3)
R = I + s * K + (1 - c) * K2
return R

7. Optional small-angle refinement

• For very small $\theta$, using the above may be fine, but you can improve stability by using the series:
• $\sin$\theta$\approx \theta$
• $1-\cos$\theta \approx \tfrac{\theta^2}{2}
• Or directly approximate:

$$R \approx I + [\mathbf{r}]_\times$$

when $\|$\mathbf{r}$\|$ is tiny.

---

4. Common Pitfalls

• Not handling the zero / tiny angle case

• When $\theta \approx 0$, dividing by $\theta$ to get $\mathbf{k}$ is unstable.

• Always add a small $\varepsilon$ check to:

• Return $I$, or

• Use the series expansions.

• Axis not normalized

• If the problem gives you $\mathbf{k}$ and $\theta$ separately, ensure $\mathbf{k}$ is unit length before using Rodrigues; otherwise the result is not a valid rotation.

• Skew-symmetric matrix sign mistakes

• Carefully follow the definition:

$$[\mathbf{k}]_\times = \begin{pmatrix} 0 & -k_z & k_y \\ k_z & 0 & -k_x \\ -k_y & k_x & 0 \end{pmatrix}.$$

• A swapped sign will flip the rotation direction.

• Floating-point drift

• Due to numerical error, $R^\top R$ might not be exactly $I$.

• Usually acceptable, but if needed, you can re-orthogonalize $R$ in downstream code.

• Confusing degrees and radians

• Most math libraries use radians; convert from degrees if needed.

---

5. Time & Space Complexity

• Time complexity:

• All operations are on fixed-size 3×3 matrices and 3D vectors: constant number of multiplications, additions, and trig calls.

• So the time complexity is $O(1)$.

• Space complexity:

• You store a few 3D vectors and 3×3 matrices, independent of input size.

• So the space complexity is also $O(1)$.

This problem is mainly about correctly implementing a small, constant-time linear algebra formula with good numerical hygiene.