Az Iterative Closest Points algoritmus egy merevtest transzformációt valósít meg. Az OpenCV függvénykönyvtár legújabb verziói már képesek kezelni a pontfelhőket és PLY formátumú modellfájllokat beolvasni.

A következőkben erre látunk példát:

1. Beolvasunk két modell fájlt (egyik a modell, másik a megfigyelés).
2. ICP-vel meghatározzuk a közöttük lévő transzformációt.
3. A kiszámolt transzformációs mátrixszal transzformáljuk a modellt a megfigyeléshez.

import cv2
import numpy as np
import math
from matplotlib import pyplot as plt

MODEL1_FILE = "../csap_ply.ply"
MODEL2_FILE = "../csap2_ply.ply"
model1 = cv2.ppf_match_3d.loadPLYSimple(MODEL1_FILE)
model2 = cv2.ppf_match_3d.loadPLYSimple(MODEL2_FILE)

icp = cv2.ppf_match_3d_ICP(20) ## iterációk száma: 20
print("ICP objektum létrehozva")
retval, residual, pose    = icp.registerModelToScene( model1, model2 )
print("regisztrációs mátrix létrehozva")
aligned_model1 = cv2.ppf_match_3d.transformPCPose(model1,pose)
print("regisztráció kész")
cv2.ppf_match_3d.writePLY(aligned_model1, "aligned.ply")