01 Mathematical foundations 3D Vision Visual SLAM Robotics Robotic Grasping

Mathematics of Rigid-Body Motion

From Euclidean Geometry
to Optimization on SE(3)

A rigid pose is a state on a six-dimensional manifold, rather than sixteen independent entries of a 4×4 array. Starting with points, vectors, bases, and homogeneous coordinates, this chapter develops a common language for SO(3), SE(3), exponential/logarithmic maps, twists, the Adjoint, and left/right perturbations.

Open the Rigid Geometry Lab Start with geometric entities

Affine Space Before Matrices

Identify the geometric entity before choosing its transformation rule

A point describes a position; a vector describes the difference between two points or a free direction; a normal encodes a tangent-plane constraint; a basis turns geometric entities into coordinates. All can be stored as arrays, but matching array shapes do not imply matching semantics.

Point, direction and normal

\tilde{\mathbf p}=\begin{bmatrix}\mathbf p\\1\end{bmatrix},\quad \tilde{\mathbf v}=\begin{bmatrix}\mathbf v\\0\end{bmatrix},\quad \mathbf p'=A\mathbf p+\mathbf t,\quad \mathbf v'=A\mathbf v,\quad \mathbf n'\propto A^{-\top}\mathbf n

w=1 makes points receive translation; w=0 makes free vectors ignore it. A normal is defined by nᵀd=0, so requiring n′ᵀd′=0 gives n′∝A⁻ᵀn.

Geometric entities	Homogeneous form / structure	Rigid rule	General affine rule	Invariants	Common pitfalls
Point	`[pᵀ, 1]ᵀ`	p′ = Rp + t	p′ = Ap + t	Rigid motion preserves distances between points; translation affects points	Points belong to an affine space: two points can be subtracted, but changing the origin changes their coordinates
Free vector / direction	`[vᵀ, 0]ᵀ`	v′ = Rv	v′ = Av	Rigid motion preserves length, angles, and dot products; translation has no effect	When converting velocity between rotating frames, distinguish the frame in which it is expressed; do not assign w=1 to a direction
Normal	`Covector / plane constraint`	n′ = Rn	n′ ∝ A⁻ᵀn	Remains perpendicular to transformed tangents; normalize as needed	Transforming normals as ordinary directions works only for orthogonal transforms or uniform scaling
Coordinate basis	`B = [b₁ b₂ b₃]`	B′ = RB	B′ = AB	Columns of R form an orthonormal basis; columns of a general A need not be orthogonal	Whether columns describe a new basis in the old frame or a linear map depends on active/passive semantics
Pose	`T = [[R,t],[0ᵀ,1]]`	T ∈ SE(3)	For T ∈ Aff(3), the upper-left block is a general A	Rigid motion preserves distances, angles, volume, and orientation	A 4×4 shape does not guarantee a rigid transform; check RᵀR and det(R)

DOT PRODUCT

The dot product defines length and angle

‖v‖²=vᵀv and cosθ=(aᵀb)/(‖a‖‖b‖). An orthogonal R satisfies (Ra)ᵀ(Rb)=aᵀb.

CROSS PRODUCT

The cross product has a skew-symmetric matrix form

[a]×b=a×b and [Ra]×=R[a]×Rᵀ. This connects 3D angular velocity, normals, and SE(3) Jacobians.

AFFINE COMBINATION

Weighted sums of points require weights summing to 1

Σᵢαᵢpᵢ is independent of the origin when Σᵢαᵢ=1; centroids, barycentric coordinates, and interpolation rely on this property.

What Is Preserved?

Rigid, similarity, affine, and reflection transforms preserve different properties

Matrix size alone is insufficient. A transform family is determined by constraints on the upper-left linear block, its determinant, and the geometric structures it preserves. These also determine degrees of freedom, applicable solvers, and normal transformation rules.

Transform families	Matrix form	DoF	Constraints	Invariants	Uses in 6D pose
SO(3)	`R ∈ ℝ³ˣ³`	3	RᵀR = I, det(R)=+1	Lengths, angles, volume, and orientation; the origin stays fixed	Orientation, angular-velocity integration, rotation error
SE(3)	`[[R,t],[0ᵀ,1]]`	6	R ∈ SO(3), t ∈ ℝ³	Euclidean distances, angles, volume, and orientation	6D rigid pose, camera trajectories, robot kinematics
Sim(3)	`[[sR,t],[0ᵀ,1]]`	7	R ∈ SO(3), s > 0	Angles and length ratios; uniform scaling in every direction	Monocular SLAM scale alignment, unknown uniform scale
Aff(3)	`[[A,t],[0ᵀ,1]]`	12	A ∈ GL(3), det(A) ≠ 0	Lines, parallelism, and affine combinations	Image/3D data transforms and nonrigid preprocessing; not a 6D pose
O(3) \ SO(3)	`Q, det(Q)=−1`	3 + a discrete branch	QᵀQ = I, det(Q)=−1	Distances and angles, but reverses orientation	Reflection failures in SVD registration; diagnosing mirrored axis conventions

projective⊃affine⊃similarity⊃rigid · SE(3)⊃rotation · SO(3) Sets farther to the right impose stronger constraints and preserve more metric structure; larger matrices do not imply greater sophistication.

SO(3) and SE(3)

Valid rigid transforms are closed under multiplication and inversion

Group structure supports reliable transform chains: the identity represents no motion, the inverse reverses coordinate conversion, and associativity permits regrouping. Matrix multiplication is generally noncommutative, so motion order cannot be swapped arbitrarily.

Special orthogonal group

SO(3)=\left\{R\in\mathbb R^{3\times3}\mid R^\top R=I,\ \det R=+1\right\}

The 9 entries of R satisfy 6 independent constraints, leaving 3 physical degrees of freedom. Orthogonal matrices with det=−1 represent reflections or rotations combined with reflections, and do not belong to SO(3).

Special Euclidean group

SE(3)=\left\{T=\begin{bmatrix}R&\mathbf t\\\mathbf0^\top&1\end{bmatrix}\ \middle|\ R\in SO(3),\ \mathbf t\in\mathbb R^3\right\}

SE(3) has a semidirect-product structure in which rotation acts on translation: rotation changes how subsequent translations are expressed. A 6D rigid motion therefore involves more than concatenating two unrelated 3D vectors.

Composition

T_1T_2=\begin{bmatrix}R_1R_2&R_1\mathbf t_2+\mathbf t_1\\\mathbf0^\top&1\end{bmatrix}

The rightmost transform acts first. Rotate the second translation t₂ with R₁ into the final target frame before adding t₁.

Inverse

T^{-1}=\begin{bmatrix}R^\top&-R^\top\mathbf t\\\mathbf0^\top&1\end{bmatrix}

Inverse translation is more than −t; it must also be re-expressed from the original target frame into the source frame.

MINIMUM NUMERICAL EXAMPLE

Rotate 90° about z, then translate by [1,2,3]ᵀ

R = [[0,−1,0],[1,0,0],[0,0,1]]
p = [1,0,0]ᵀ
p′ = Rp+t = [1,3,3]ᵀ

Recover with T⁻¹ by subtracting t, then multiplying by Rᵀ: Rᵀ(p′−t)=[1,0,0]ᵀ. Simply negating t and applying operations in the wrong order fails to recover the original point.

Interactive Rigid Geometry Lab

Make invariants, Exp / Log, and perturbation frames computable

The first view compares rigid, similarity, affine, and reflection transforms; the second computes SE(3) exponential/logarithmic maps; the third compares left/right updates and uses the Adjoint to show how two local coordinate systems describe the same physical increment.

Point p · w=1 Direction d · w=0 Normal n · A⁻ᵀn The same rules extend from 2D to 3D

Classification

Rigid transform

Preserves distances, angles, and orientation

det(A)

Volume and Orientation

+1.000orientation preserving

Length ratio

‖A d‖ / ‖d‖

1.000Direction length preserved

Normal check

(A d) · (A⁻ᵀ n)

0.00e+0The correct normal remains perpendicular to the tangent

Wrong rule

(A d) · (A n)

0.00e+0Also correct for rigid motion

Homogeneous coordinates do not append 1 to every 3D quantity. Points use [p;1] and receive translation; free vectors and directions use [d;0]. Normals are covectors constraining a tangent plane: under general affine transforms they require A⁻ᵀ, rather than A.

Lie Group and Lie Algebra

Build a tangent space for linear calculations beside the nonlinear pose manifold

SO(3) and SE(3) vary continuously and have both group and smooth-manifold structures. A Lie algebra is the tangent space at the identity: an ordinary vector space suited to velocities, small increments, optimization steps, and local Gaussian uncertainty.

GROUP

T ∈ SE(3)

Global, nonlinear, composable finite rigid motions.

Log ↓
↑ Exp

ALGEBRA

ξ̂ ∈ se(3)

Tangent vectors at the identity in 4×4 matrix form.

vee ⇄ hat

COORDINATES

ξ = [ρ;φ] ∈ R⁶

This site places the translational tangent component ρ before the rotation vector φ.

Hat operator

[\boldsymbol\phi]_\times=\begin{bmatrix}0&-\phi_z&\phi_y\\\phi_z&0&-\phi_x\\-\phi_y&\phi_x&0\end{bmatrix},\qquad \boldsymbol\xi^\wedge=\begin{bmatrix}[\boldsymbol\phi]_\times&\boldsymbol\rho\\\mathbf0^\top&0\end{bmatrix}

[φ]×v=φ×v. Some libraries order ξ as [φ;ρ]; this changes Jacobian, Adjoint, and covariance blocks, beyond the notation alone.

SO(3) exponential · Rodrigues

\operatorname{Exp}(\boldsymbol\phi)=I+\frac{\sin\theta}{\theta}\Phi+\frac{1-\cos\theta}{\theta^2}\Phi^2,\quad \Phi=[\boldsymbol\phi]_\times,\ \theta=\|\boldsymbol\phi\|

As θ→0, avoid direct division by θ; use sinc / cosc or Taylor series. For a principal-value Log, the rotation angle usually lies in [0,π].

SE(3) exponential

\operatorname{Exp}(\boldsymbol\xi)=\exp(\boldsymbol\xi^\wedge)=\begin{bmatrix}\operatorname{Exp}(\boldsymbol\phi)&J_l(\boldsymbol\phi)\boldsymbol\rho\\\mathbf0^\top&1\end{bmatrix}

Rotation and translation do not exponentiate independently: the semidirect-product structure maps ρ to the final t through the SO(3) left Jacobian.

SO(3) left Jacobian

J_l(\boldsymbol\phi)=I+\frac{1-\cos\theta}{\theta^2}\Phi+\frac{\theta-\sin\theta}{\theta^3}\Phi^2,\qquad \boldsymbol\rho=J_l^{-1}(\boldsymbol\phi)\mathbf t

At small angles, Jₗ≈I+½Φ+⅙Φ²; for pure translation, φ=0 and Jₗ=I, giving ρ=t in that case.

VELOCITY

A twist describes instantaneous rigid-body velocity

For a twist ξ constant over time, T(s)=Exp(sξ^) describes a screw motion. φ specifies angular motion, while ρ couples with rotation to produce finite translation.

OPTIMIZATION

Solve linear increments in R⁶

Gauss–Newton / LM solves for δξ, then Exp maps the update back to SE(3), keeping every iterate a valid rigid transform.

UNCERTAINTY

Covariance belongs to a particular local tangent space

A 6×6 Σ must specify the left/right perturbation, reference pose, frame, component order, and translation units for correct propagation or comparison.

Retraction, Jacobian and Adjoint

Adding a small pose increment requires specifying the multiplication side

The same six-dimensional array is interpreted in different frames on the left or right of T. Either can define a correct optimizer, but residuals, Jacobians, covariances, and update rules must be consistent as a set.

Convention	Update	First-order effect on y=Tp	Point Jacobian	Geometric meaning
Left perturbation / target frame	`T ← Exp(δξₗ^) T`	For y = Tp, δy ≈ δρₗ − [y]×δφₗ	`[I, −[y]×]`	The increment is expressed in the output / world / camera target frame
Right perturbation / source frame	`T ← T Exp(δξᵣ^)`	For y = Tp, δy ≈ Rδρᵣ − R[p]×δφᵣ	`[R, −R[p]×]`	The increment is expressed in the input / object / body local frame
Equivalent conversion via the Adjoint	`δξₗ = AdT δξᵣ`	Exp(δξₗ^)T = T Exp(δξᵣ^)	`Σₗ = AdT Σᵣ AdTᵀ`	Two six-dimensional coordinate descriptions of the same physical perturbation, rather than different motions

Adjoint for ξ = [ρ; φ]

\operatorname{Ad}_T=\begin{bmatrix}R&[\mathbf t]_\times R\\0&R\end{bmatrix},\qquad \begin{bmatrix}\boldsymbol\rho_l\\\boldsymbol\phi_l\end{bmatrix}=\operatorname{Ad}_T\begin{bmatrix}\boldsymbol\rho_r\\\boldsymbol\phi_r\end{bmatrix}

This block structure uses the site's [ρ;φ] order. For a library using [φ;ρ], the blocks of the same Adjoint occupy different positions.

Conjugation identity

T\operatorname{Exp}(\delta\boldsymbol\xi_r)T^{-1}=\operatorname{Exp}(\operatorname{Ad}_T\delta\boldsymbol\xi_r)\ \Longrightarrow\ \operatorname{Exp}(\delta\boldsymbol\xi_l)T=T\operatorname{Exp}(\delta\boldsymbol\xi_r)

The Adjoint represents the same Lie algebra element in another frame / on another perturbation side; it does not add a physical motion.

OPTIMIZER CONTRACT

Fix six conventions before implementation

Pose direction: target-from-source
ξ order: [ρ;φ] or [φ;ρ]
Left- or right-multiplicative retraction
φ units: use rad inside the optimizer
Local coordinates with respect to which the Jacobian is taken
Tangent space and frame in which Σ is expressed

Errors Are Also Geometric Objects

Compare pose errors on the manifold; combine rotation and translation only with a defined scale

Elementwise matrix differences, componentwise Euler-angle differences, and quaternion L2 distances depend on the representation. A more stable approach constructs a relative transform, uses Log for a local error, then weights it according to task scale or covariance.

Right-invariant local pose error

\mathbf e_r=\operatorname{Log}\!\left(T_{\mathrm{est}}^{-1}T_{\mathrm{gt}}\right)^\vee\in\mathbb R^6

The error is expressed on the local / source side of est. Reversing the product order or using a left-invariant error yields six-dimensional coordinates in another frame; either can be valid with a consistent implementation.

Rotation geodesic distance

e_R=\left\|\operatorname{Log}(R_{\mathrm{est}}^\top R_{\mathrm{gt}})^\vee\right\|=\cos^{-1}\!\left(\operatorname{clip}\frac{\operatorname{tr}(R_{\mathrm{est}}^\top R_{\mathrm{gt}})-1}{2}\right)

Clipping prevents floating-point error from pushing acos inputs outside [−1,1]. For symmetric objects, also minimize over the set of equivalent rotations.

TRANSLATION

Meters and millimeters change numerical values

eₜ=‖t_est−t_gt‖ has length units; any combined threshold must state units and object scale.

ROTATION

Use the shortest geodesic for angular error

The principal SO(3) angle lies in [0,π]; Euler component differences can be discontinuous at ±180°.

WHITENING

Scale residuals with covariance

With a valid Σ and consistent frames, use eᵀΣ⁻¹e; this is more meaningful than arbitrarily declaring 1 m = 1 rad.

TASK LOSS

Geometric distance does not determine task equivalence

Grasping, assembly, and projection respond differently to error directions; task Jacobians or success rates are still needed.

NUMERICAL STABILITY

Minimum safeguards for core SO(3) / SE(3) functions

Small angles: Use series or stable special functions for sinθ/θ, (1−cosθ)/θ², and (θ−sinθ)/θ³.
Near π: The Log rotation axis is sensitive to small noise; use stable quaternion / diagonal branches and accept nonuniqueness in axis signs and principal values.
Rotation projection: For a noisy matrix M=UΣVᵀ, use R=U diag(1,1,det(UVᵀ))Vᵀ to prevent reflections.
Orthogonality checks: Continuously log ‖RᵀR−I‖, det(R), finite values, and matrix condition numbers; detect problems before rendering fails.
Finite differences: Check analytical Jacobians with central differences using the same left/right retraction, covering zero, general, and near-π angles.
Matrix composition: Periodically normalize quaternions or project R along long chains if needed, but first locate the drift source; projection must not conceal direction/unit bugs.

A Shared Mathematical Interface

One rigid-geometry framework connects vision, SLAM, robotics, and grasping

Domains mainly differ in state names, observation residuals, and task constraints. Transform composition, local updates, the Adjoint, and error propagation use the same mechanisms.

Domain	Typical state	Typical residuals	Role of this chapter's mathematics
6D object pose	`TCO ∈ SE(3)`	Reprojection, silhouette, depth, surface, or feature residuals	A network may predict any representation, but solving, refinement, and error computation must ultimately respect the rigid-motion manifold
Visual SLAM / BA	`TWC, landmarks, scale, or velocity/bias states`	Cross-frame reprojection, IMU preintegration, loop closures, and relative poses	Solve for δξ in the tangent space at each iteration, then update the pose with a left/right retraction; do not add 4×4 matrices directly
Robot kinematics	`TBE(q), TET, hand–eye extrinsics`	End-effector position/orientation, closed-loop constraints, calibration equations	Twists, the Adjoint, and Jacobians connect joint velocities, tool velocities, and perturbations in different frames
Robotic Grasping	`TBO, TOG, TBG`	Grasp pose error, contact, collision, reachability	Compose object-to-grasp with the visual object pose; transform errors into the frame relevant to the task

02 · NEXT

Coordinate Frames & Transform Conventions

Instantiate target/source with actual object, camera, world, robot, and gripper frames.

03

Rotation & Orientation Representations

Explore Euler angles, matrices, axis–angle, quaternions, continuous representations, and egocentric / allocentric orientations.

07

3D–3D Alignment and Registration

Apply rigid constraints to practical estimation with Kabsch, Umeyama, RANSAC, and ICP.

Failure Modes

Most geometry bugs arise from unspecified semantics

Encode frames, transform directions, component order, update side, and angle/length units in API types, logs, and tests. This lets numerically correct functions form a semantically correct system.

Frequent error	Symptom	Low-cost fix
Assigning w=1 to both points and directions	Translation is incorrectly added to directions, axes, or velocities	Use [p;1] for points and [v;0] for free vectors; test them separately
Transforming normals with A under a general affine map	Normals cease to be perpendicular to the surface after nonuniform scaling or shear	Use A⁻ᵀn, renormalize, and check n′ᵀd′≈0
Applying elementwise SGD updates to a 4×4 pose	R leaves SO(3); determinant and orthogonality drift	Optimize δξ in the tangent space, then update through Exp / retraction; project with SVD if needed
Treating the twist component ρ as the final t	Ignoring Jₗ(φ) at finite angles gives incorrect trajectories and Jacobians	Specify the ξ order and Exp formula; compute t=Jₗρ except for pure translation
Pairing a left Jacobian with right perturbations	Gradient direction and covariance frames disagree, causing slow convergence or divergence	Fix the boxplus definition; compare analytical Jacobians and finite differences using the same perturbation side
Comparing left and right updates with the same six numbers	Treating local-frame and target-frame coordinates as the same physical increment	Convert with δξₗ=AdTδξᵣ before comparing
Using general Exp / Log formulas directly near zero or π	0/0, catastrophic cancellation, unstable axis signs	Use series at small angles and stable branches near π; accept nonuniqueness in principal values/axis signs
Mixing meters and millimeters in the same se(3) norm	Rotation/translation weights lack physical meaning, and thresholds vary with units	Report separately, or combine after task-scale normalization / covariance whitening

GEOMETRY UNIT TESTS

Test more than one plausible-looking pose

inverse(T) · T ≈ I Exp(Log(T)) ≈ T Log(Exp(ξ)) ≈ ξ · principal branch det(R) ≈ +1 RᵀR ≈ I Exp(AdT ξ)T ≈ T Exp(ξ) (Ad A·B) ≈ Ad A · Ad B analytic J ≈ central difference J

Primary & Technical Sources

Representative Primary Sources on Rigid Motion and Lie Theory

Sources can differ in twist order, left/right Jacobians, and perturbation conventions; read their notation contract before using a formula.

01

Solà, Deray & Atchuthan · A micro Lie theory for state estimation in roboticsSystematic formulas for SO(3) / SE(3), Exp / Log, Jacobians, the Adjoint, left/right perturbations, and uncertainty

↗ 02

Murray, Li & Sastry · A Mathematical Introduction to Robotic ManipulationClassic mathematical foundations of rigid motion, exponential coordinates, twists, kinematics, and robotic manipulation

↗ 03

Ethan Eade · Lie Groups for 2D and 3D TransformationsMatrix representations, exponential maps, the Adjoint, and uncertainty derivations for SO(3), SE(3), and Sim(3)

↗ 04

GTSAM · Pose3 documentationPose manifolds versus transform groups; SE(3) composition, inversion, Exp / Log, retraction, and implementation conventions

↗ 05

Modern Robotics · Exponential Coordinates of Rigid-Body MotionFrom se(3) twists to finite SE(3) rigid motions, with robotics interpretations of matrix exponentials/logarithms

↗

← Previous Topic · 00

Introduction to 6D Pose

Next Topic · 02

Coordinate Frames & Transform Conventions

Apply rigid geometry to actual frame chains involving objects, cameras, worlds, and robots.

Continue reading →

From Euclidean Geometry to Optimization on SE(3)

Identify the geometric entity before choosing its transformation rule

The dot product defines length and angle

The cross product has a skew-symmetric matrix form

Weighted sums of points require weights summing to 1

Rigid, similarity, affine, and reflection transforms preserve different properties

Valid rigid transforms are closed under multiplication and inversion

Rotate 90° about z, then translate by [1,2,3]ᵀ

Make invariants, Exp / Log, and perturbation frames computable

Rigid transform

Volume and Orientation

‖A d‖ / ‖d‖

(A d) · (A⁻ᵀ n)

(A d) · (A n)

T = Exp(ξ̂)

t = Jₗ(φ)ρ

Log(Exp(ξ̂))∨

δξₗ

δξᵣ

Tₗ versus Tᵣ

Build a tangent space for linear calculations beside the nonlinear pose manifold

T ∈ SE(3)

ξ̂ ∈ se(3)

ξ = [ρ;φ] ∈ R⁶

A twist describes instantaneous rigid-body velocity

Solve linear increments in R⁶

Covariance belongs to a particular local tangent space

Adding a small pose increment requires specifying the multiplication side

Fix six conventions before implementation

Compare pose errors on the manifold; combine rotation and translation only with a defined scale

Meters and millimeters change numerical values

Use the shortest geodesic for angular error

Scale residuals with covariance

Geometric distance does not determine task equivalence

Minimum safeguards for core SO(3) / SE(3) functions

One rigid-geometry framework connects vision, SLAM, robotics, and grasping

Coordinate Frames & Transform Conventions

Rotation & Orientation Representations

3D–3D Alignment and Registration

Most geometry bugs arise from unspecified semantics

Test more than one plausible-looking pose

Representative Primary Sources on Rigid Motion and Lie Theory

Introduction to 6D Pose

Coordinate Frames & Transform Conventions

Explore the full topic

From Euclidean Geometry
to Optimization on SE(3)