[Feature]: Optimize EPnP by eliminating unnecessary dynamic allocations and intermediate matrix construction

🚀 Feature Description

Optimize the EPnP implementation by removing unnecessary dynamic matrix allocations (DMatrix/DVector) where dimensions are known at compile time, and avoid constructing the intermediate design matrix M when only MᵀM is required.

These changes should reduce heap allocations, eliminate redundant memory copies, and improve cache locality while preserving the existing algorithm.

📂 Feature Category

Rust Core Library

💡 Motivation

The current EPnP implementation performs several dynamic allocations despite operating almost entirely on matrices with fixed dimensions (e.g. 12×12, 12×4, 6×10, 6×4).

Using nalgebra::DMatrix and DVector in these cases introduces unnecessary runtime overhead:

Heap allocations Runtime dimension checks Dynamic indexing Reduced opportunities for compiler optimizations such as loop unrolling and vectorization

Additionally, the current implementation constructs the full design matrix M, flattens it into a Vec, and then copies it again into a DMatrix, even though the algorithm ultimately only requires the matrix product MᵀM.

This results in unnecessary work and memory traffic.

💭 Proposed Solution

1. Replace fixed-size DMatrix/DVector with SMatrix/SVector

Many matrices used throughout the EPnP pipeline have compile-time dimensions and can be represented as statically sized matrices instead of dynamically allocated ones.

Examples include:

12×12 12×4 6×10 6×4 6×3 6×5

Using SMatrix/SVector allows the compiler to generate more efficient code while removing heap allocations.

2. Compute MᵀM directly

Currently the implementation performs the following sequence:

Build M ↓ Flatten into Vec ↓ Construct DMatrix ↓ Compute MᵀM

Since only MᵀM is required for the subsequent eigendecomposition, the intermediate matrix M does not need to be materialized.

Instead, each pair of rows generated for a correspondence can directly accumulate into the fixed-size 12×12 normal matrix:

for each correspondence: generate row_x generate row_y

MtM += row_xᵀ * row_x
MtM += row_yᵀ * row_y

This removes:

construction of the large intermediate matrix transpose operation matrix multiplication associated allocations 3. Remove unnecessary Vec copies

The current implementation performs:

rows ↓ Vec (copy 1) ↓ DMatrix::from_row_slice(...) (copy 2)

Every element is copied into a temporary Vec, then copied again into the dynamically allocated matrix.

By either using fixed-size matrices directly or accumulating MᵀM on the fly, both copies can be eliminated entirely.

📚 Library Reference

nalgebra SMatrix / SVector Current EPnP implementation EPnP paper (Lepetit et al., IJCV 2009)

🎯 Use Cases

Faster EPnP execution Reduced heap allocations Better cache locality Improved compiler optimization opportunities More efficient execution on embedded and resource-constrained platforms

🤝 Contribution Intent

• I plan to submit a PR to implement this feature
• I'm requesting this feature but not planning to implement it