solve_rigid_point_set_rt_pro

Solve the transform vector and rotation matrix of a rigid point set (order can be changed).

Installation

pip install solve_rigid_point_set_rt_pro

Usage

from solve_rigid_point_set_rt_pro import compute_best_rigid_transform_pro
from solve_rigid_point_set_rt     import compute_best_rigid_transform
from solve_rigid_point_set_rt     import apply_transform
import numpy as np

P = np.array([
    [ 20.606943,  -0.005458,  0.040331], # line 0
    [ -4.885081, -26.211561, -0.023027], # line 1
    [ 12.012958,  20.049110, -0.035819], # line 2
    [-27.734814,   6.167912,  0.018507]  # line 3
])

Q = np.array([
    [-115.37230320753432, 126.89401508498568, 790.1637162115628], # line 2
    [ -137.8636773243661,  96.44871350623225, 808.1944560570211], # line 3
    [-132.94031926182967,  137.4289669332182, 782.6069181107938], # line 0
    [-164.12739196778332, 120.93405919807722, 791.4971644346351]  # line 1
])

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

# 3. Apply transformation and calculate error
P_transformed = apply_transform(P, R_est, t_est)
print(P_transformed)