solve_rigid_point_set_rt

Solve the transform vector and rotation matrix of a rigid point set.

Installation

pip install solve_rigid_point_set_rt

Usage

from solve_rigid_point_set_rt import compute_best_rigid_transform
from solve_rigid_point_set_rt import apply_transform
from solve_rigid_point_set_rt import calculate_error
import numpy as np

P = np.array([
    [ 20.606943,  -0.005458,  0.040331],
    [ -4.885081, -26.211561, -0.023027],
    [ 12.012958,  20.049110, -0.035819],
    [-27.734814,   6.167912,  0.018507]
])

Q = np.array([
    [-132.94031926182967, 137.4289669332182, 782.6069181107938],
    [-164.12739196778332, 120.93405919807722, 791.4971644346351],
    [-115.37230320753432, 126.89401508498568, 790.1637162115628],
    [-137.8636773243661, 96.44871350623225, 808.1944560570211]
])

# 2. Compute optimal rotation and translation
R_est, t_est = compute_best_rigid_transform(P, Q)

# 3. Apply transformation and calculate error
P_transformed = apply_transform(P, R_est, t_est)
rmse, mean_dist, _ = calculate_error(P_transformed, Q)

# 4. Print results
print("="*50)
print("Optimal Rotation Matrix R:")
print(R_est)
print("\nOptimal Translation Vector t:")
print(t_est)
print("\nRegistration Accuracy:")
print(f"Root Mean Square Error (RMSE): {rmse:.6f}")
print(f"Mean Euclidean Distance: {mean_dist:.6f}")
print("="*50)