apply_transform.py

import numpy as np
import pandas as pd
import argparse

def load_points_from_tsv(path: str) -> tuple[np.ndarray, np.ndarray]:
    df = pd.read_csv(path, sep='\t')
    if not all(col in df.columns for col in ['ID', 'position.x', 'position.y', 'position.z']):
        raise ValueError("必要な列 'ID', 'position.x', 'position.y', 'position.z' が見つかりません")

    ids = df['ID'].values
    points = df[['position.x', 'position.y', 'position.z']].values
    return ids, points

def apply_transform(points: np.ndarray, transform: np.ndarray) -> np.ndarray:
    is_single = points.ndim == 1
    if is_single:
        points = points[np.newaxis, :]

    ones = np.ones((points.shape[0], 1))
    points_h = np.hstack([points, ones])
    transformed_h = (transform @ points_h.T).T
    transformed = transformed_h[:, :3]

    return transformed[0] if is_single else transformed

def main():
    parser = argparse.ArgumentParser(description="TSVから3D座標を読み込み、変換行列を適用して出力する")
    parser.add_argument("input_path", help="入力TSVファイルのパス（ID, position.x, position.y, position.z列が必要）")
    args = parser.parse_args()

    # 変換行列（必要に応じて変更してください）
    transform = np.array([
        [ 0.989162 -0.018384  0.145673 -0.038322]
        [ 0.00832   0.997555  0.069393 -0.028196]
        [-0.146593 -0.067429  0.986896 -0.043362]
        [ 0.        0.        0.        1.      ]
    ])

    ids, points = load_points_from_tsv(args.input_path)
    transformed = apply_transform(points, transform)

    print("ID\tx\ty\tz")
    for i, p in zip(ids, transformed):
        print(f"{i}\t{p[0]:.6f}\t{p[1]:.6f}\t{p[2]:.6f}")

if __name__ == "__main__":
    main()

rigid_transform_from_tsv.py

import numpy as np
import pandas as pd

def load_points_from_tsv(path: str) -> np.ndarray:
    df = pd.read_csv(path, sep='\t')
    if not all(col in df.columns for col in ['position.x', 'position.y', 'position.z']):
        raise ValueError("TSVファイルに 'position.x', 'position.y', 'position.z' の列が必要です")
    return df[['position.x', 'position.y', 'position.z']].values

def compute_rigid_transform(src_points: np.ndarray, target_points: np.ndarray) -> np.ndarray:
    assert src_points.shape == target_points.shape

    centroid_src = np.mean(src_points, axis=0)
    centroid_tgt = np.mean(target_points, axis=0)

    src_centered = src_points - centroid_src
    tgt_centered = target_points - centroid_tgt

    H = src_centered.T @ tgt_centered
    U, S, Vt = np.linalg.svd(H)
    R = Vt.T @ U.T

    if np.linalg.det(R) < 0:
        Vt[2, :] *= -1
        R = Vt.T @ U.T

    t = centroid_tgt - R @ centroid_src

    transform = np.eye(4)
    transform[:3, :3] = R
    transform[:3, 3] = t

    return transform

def apply_transform(points: np.ndarray, transform: np.ndarray) -> np.ndarray:
    ones = np.ones((points.shape[0], 1))
    points_h = np.hstack([points, ones])  # (N, 4)
    transformed_h = (transform @ points_h.T).T  # (N, 4)
    return transformed_h[:, :3]  # (N, 3)

def compute_error(transformed_src: np.ndarray, target: np.ndarray) -> tuple[float, float]:
    diffs = transformed_src - target
    dists = np.linalg.norm(diffs, axis=1)
    rms_error = np.sqrt(np.mean(dists ** 2))
    max_error = np.max(dists)
    return rms_error, max_error

def main():
    src_path = "src_points.tsv"
    tgt_path = "target_points.tsv"

    src_points = load_points_from_tsv(src_path)
    tgt_points = load_points_from_tsv(tgt_path)

    transform = compute_rigid_transform(src_points, tgt_points)

    np.set_printoptions(precision=6, suppress=True)
    print("変換行列（4x4）:")
    print(transform)

    transformed_src = apply_transform(src_points, transform)

    rms, max_err = compute_error(transformed_src, tgt_points)
    print(f"\n▶ RMS誤差: {rms:.6f} m")
    print(f"▶ 最大誤差: {max_err:.6f} m")

if __name__ == "__main__":
    main()

src_points.tsv

target_points.tsv