import pycolmap
import numpy as np
tgt_from_src = pycolmap.Sim3d(3, np.random.RandomState(0).randn(3), np.arange(3)+1)
n_points = 50
p_src = np.random.RandomState(1).randn(n_points, 3)
p_tgt = tgt_from_src * p_src
tgt_from_src_est = pycolmap.estimate_sim3d(p_src, p_tgt)
print('least-squares:')
print(tgt_from_src)
print(tgt_from_src_est)
inlier_ratio = 0.5
n_inliers = int(inlier_ratio * n_points)
n_outliers = n_points - n_inliers
for i in np.random.RandomState(2).choice(n_points, n_outliers, replace=False):
    p_src[i] = np.random.RandomState(i).randn() * 10
max_error = 0.1
ret = pycolmap.estimate_sim3d_robust(p_src, p_tgt, {"max_error": max_error})
n_inliers_real = np.count_nonzero(np.linalg.norm(p_tgt - tgt_from_src * p_src, axis=1) < max_error)
print('robust:')
print(ret["tgt_from_src"], ret["num_inliers"], n_inliers_real, n_inliers)