Merge pull request #23 from HiraiKyo/fix/ransac_plane-precise-normal

Precise normal from plane, fixed ZeroDivisionError

README.md

@@ -8,18 +8,6 @@ Basic libraries for manipulating point cloud.
 pip install git+https://github.com/HiraiKyo/ply-processor-basics
 ```
 
-## Development
-
-### Running test
-
-`visual`タグはopen3d.geometry等で表示を確認する用なので、テスト実行時は外す
-
-```sh
-poetry run pytest -s {filepath} -m "not visual"
-```
-
-TDD開発時にopen3dで表示を確認しつつ進める場合には、そのテストに`@pytest.mark.visual`タグを付けて自動テストに影響しないようにする。
-
 ## Methods
 
 ### STL
@@ -30,6 +18,14 @@ TDD開発時にopen3dで表示を確認しつつ進める場合には、その
 
 #### `vector.normalize`
 
+#### `vector.estimate_vector`
+
+ベクトルの集合から四分位範囲で平均ベクトルを算出する
+
+#### `vector.ensure_consistent_direction`
+
+基準ベクトル方向にベクトル群を反転する（法線ベクトルが2種類出る対応に用いる）
+
 ### Matrix
 
 #### `matrix.get_rotation_from_vectors`
@@ -62,3 +58,15 @@ TDD開発時にopen3dで表示を確認しつつ進める場合には、その
 - 円柱半径: 17.5
 - 円柱高さ: 40.0
 - エッジ面: 122.0
+
+## Development
+
+### Running test
+
+`visual`タグはopen3d.geometry等で表示を確認する用なので、テスト実行時は外す
+
+```sh
+poetry run pytest -s {filepath} -m "not visual"
+```
+
+TDD開発時にopen3dで表示を確認しつつ進める場合には、そのテストに`@pytest.mark.visual`タグを付けて自動テストに影響しないようにする。

ply_processor_basics/matrix/get_rotation_from_vectors.py

@@ -22,7 +22,9 @@ def get_rotation_from_vectors(vec1: NDArray[np.float32], vec2: NDArray[np.float3
     if np.allclose(a, b):
         return np.eye(3)
     if np.allclose(a, -b):
-        perpendicular = normalize(np.array([1, 0, 0]) if np.allclose(a, [0, 0, 1]) else np.cross([0, 0, 1], a))
+        perpendicular = normalize(
+            np.array([1, 0, 0]) if np.allclose(a, [0, 0, 1]) or np.allclose(a, [0, 0, -1]) else np.cross([0, 0, 1], a)
+        )
         return Rotation.from_rotvec(np.pi * perpendicular).as_matrix()
     axis = normalize(np.cross(a, b))
     angle = np.arccos(np.clip(np.dot(a, b), -1.0, 1.0))

ply_processor_basics/pcd/snapshot.py

@@ -30,18 +30,24 @@ def snapshot(
 
     for pcd in pcds:
         vis.add_geometry(pcd)
-
+    print(vis)
     opt = vis.get_render_option()
-    opt.show_coordinate_frame = True
-    opt.mesh_show_back_face = True
-    opt.mesh_show_wireframe = True
-    opt.background_color = np.asarray([1, 1, 1])
+    if opt is None:
+        print("[ply_processor_basics] Open3d Warning: No render option")
+    else:
+        opt.show_coordinate_frame = True
+        opt.mesh_show_back_face = True
+        opt.mesh_show_wireframe = True
+        opt.background_color = np.asarray([1, 1, 1])
 
     ctr = vis.get_view_control()
-    ctr.set_zoom(cam_zoom)
-    ctr.set_front(cam_front)
-    ctr.set_lookat(cam_lookat)
-    ctr.set_up(cam_up)
+    if ctr is None:
+        print("[ply_processor_basics] Open3d Warning: No view control")
+    else:
+        ctr.set_zoom(cam_zoom)
+        ctr.set_front(cam_front)
+        ctr.set_lookat(cam_lookat)
+        ctr.set_up(cam_up)
 
     for pcd in pcds:
         vis.update_geometry(pcd)

ply_processor_basics/points/ransac/detect_plane.py

@@ -38,7 +38,7 @@ def __init__(self):
         self.inliers = []
         self.equation = []
 
-    def fit(self, pts, thresh=0.1, minPoints=100, maxIteration=1000):
+    def fit(self, pts, thresh=0.1, minPoints=100, maxIteration=1000, normal_samples=10):
         n_points = pts.shape[0]
         best_eq = []
         best_inliers = []
@@ -78,4 +78,28 @@ def fit(self, pts, thresh=0.1, minPoints=100, maxIteration=1000):
         if len(self.inliers) < minPoints:
             return [], []
 
+        # # 抽出点群から平面方程式を再推定
+        # pt_samples = pts[self.inliers]
+        # idx = np.random.choice(len(pt_samples), (normal_samples, 3), replace=True)
+        # vecA = pt_samples[idx[:, 1]] - pt_samples[idx[:, 0]]
+        # vecB = pt_samples[idx[:, 2]] - pt_samples[idx[:, 0]]
+        # normals = np.cross(vecA, vecB)
+        # norms = np.linalg.norm(normals, axis=1)
+
+        # # 有効な法線ベクトルのみを選択（ノルムが閾値以上）
+        # mask = norms > 1e-6
+        # valid_normals = normals[mask]
+        # valid_norms = norms[mask, np.newaxis]
+
+        # normal_samples = valid_normals / valid_norms
+        # normal_samples = ensure_consistent_direction(normal_samples)
+        # normal = estimate_vector(normal_samples)
+
+        # assert np.linalg.norm(normal) > 1e-6
+
+        # # 平面方程式の最終推定
+        # centroid = np.mean(pt_samples, axis=0)
+        # d = -np.dot(normal, centroid)
+        # self.equation = np.hstack([normal, d])
+
         return self.equation, self.inliers

ply_processor_basics/vector/__init__.py

@@ -1 +1,3 @@
+from .estimate import ensure_consistent_direction as ensure_consistent_direction
+from .estimate import estimate_vector as estimate_vector
 from .normalize import normalize as normalize

ply_processor_basics/vector/estimate.py

@@ -0,0 +1,59 @@
+import numpy as np
+from numpy.typing import NDArray
+
+
+def estimate_vector(vector_samples: NDArray[np.floating]) -> NDArray[np.floating]:
+    """
+    複数のベクトルから外れ値を考慮した平均ベクトルを推定する
+
+    外れ値除去の閾値 1.5 * IQR
+
+    :param vector_samples: ベクトルの集合(N, 3)
+    :return: 推定された平均ベクトル(1, 3)
+    """
+
+    # 1. 単位ベクトル化
+    vector_samples = np.array(vector_samples)
+    vector_samples = vector_samples / np.linalg.norm(vector_samples, axis=1)[:, np.newaxis]
+
+    # 2. 平均ベクトルの計算
+    mean: NDArray[np.floating] = np.mean(vector_samples, axis=0)
+    mean = mean / np.linalg.norm(mean)
+
+    # 3. 外れ値の除去
+    cos_similarities = np.dot(vector_samples, mean)
+    q1, q3 = np.percentile(cos_similarities, [25, 75])
+    iqr = q3 - q1
+    lower_bound = q1 - 1.5 * iqr
+    upper_bound = q3 + 1.5 * iqr
+    filtered_samples = vector_samples[(cos_similarities >= lower_bound) & (cos_similarities <= upper_bound)]
+
+    # 4. 最終的なベクトルの計算
+    final: NDArray[np.floating] = np.mean(filtered_samples, axis=0)
+    final = final / np.linalg.norm(final)
+
+    return final
+
+
+def ensure_consistent_direction(samples: NDArray[np.floating], reference_vector=None) -> NDArray[np.floating]:
+    """
+    ベクトルの方向の一貫性を確保する関数
+
+    :param samples: ベクトルの集合(N, 3)
+    :param reference_vector: 参照ベクトル(1, 3)
+    :return: 一貫性が確保されたベクトルの集合(N, 3)
+    """
+    samples = np.array(samples)
+
+    if reference_vector is None:
+        reference_vector = samples[0]
+    else:
+        reference_vector = np.array(reference_vector)
+
+    # 各ベクトルと参照ベクトルの内積を計算
+    dots = np.dot(samples, reference_vector)
+
+    # 内積が負のベクトルを反転
+    samples[dots < 0] *= -1
+
+    return samples