平面の線分検出

ply_processor_basics/points/convex_hull/__init__.py

@@ -1,2 +1,3 @@
 from .detect_circle import detect_circle as detect_circle
+from .detect_line import detect_line as detect_line
 from .detect_plane_edge import detect_plane_edge as detect_plane_edge

ply_processor_basics/points/convex_hull/detect_circle.py

@@ -19,7 +19,7 @@ def detect_circle(
     :return: 円の中心(3,), 法線ベクトル(3,), 半径
     """
 
-    inliers = detect_plane_edge(points, plane_model)
+    inliers, lines = detect_plane_edge(points, plane_model)
     centers = []
     normals = []
     radiuses = []

ply_processor_basics/points/convex_hull/detect_line.py

@@ -0,0 +1,78 @@
+from typing import List, Tuple
+
+import numpy as np
+from numpy.typing import NDArray
+
+from ply_processor_basics.vector import normalize
+
+from .detect_plane_edge import detect_plane_edge
+
+
+def detect_line(
+    points: NDArray[np.floating], plane_model: NDArray[np.floating], epsilon: float = 1.0
+) -> Tuple[NDArray[np.intp], NDArray[np.intp], List]:
+    """
+    ConvexHullを用いて平面上の直線検出
+
+    :param points: 点群(N, 3)
+    :param plane_model: 平面モデル(4,)
+    :param epsilon: Douglas-Peuckerのepsilon
+    :return: エッジ点のポインタ(N, ), エッジの線分ポインタ(N, 2), 直線の方程式p+tv(p:点, v:方向ベクトル)
+    """
+    inliers, lines = detect_plane_edge(points, plane_model)
+
+    # 輪郭線を可能な限り角の少ない多角形に近似
+    simplified_indices = np.array(ramer_douglas_peucker(points, inliers, epsilon))
+
+    simplified_lines = []
+    for i in range(len(simplified_indices) - 1):
+        simplified_lines.append([simplified_indices[i], simplified_indices[i + 1]])
+
+    # 直線の方程式を算出する
+    line_models = []
+    for line in simplified_lines:
+        p1 = points[line[0]]
+        p2 = points[line[1]]
+        v = p2 - p1
+        v = normalize(v)
+        line_models.append((p1, v))
+    return simplified_indices, np.array(simplified_lines), line_models
+
+
+def ramer_douglas_peucker(points: NDArray[np.floating], inliers: NDArray[np.intp], epsilon: float) -> NDArray[np.intp]:
+    """
+    Ramer-Douglas-Peuckerアルゴリズム
+    """
+    points_raw = points[inliers]
+
+    def recursive(start_index, end_index):
+        dmax = 0
+        index = start_index
+        for i in range(start_index + 1, end_index):
+            d = point_line_distance(points_raw[i], points_raw[start_index], points_raw[end_index])
+            if d > dmax:
+                index = i
+                dmax = d
+
+        if dmax > epsilon:
+            results = recursive(start_index, index) + recursive(index, end_index)[1:]
+        else:
+            results = [inliers[start_index], inliers[end_index]]
+
+        return results
+
+    return np.array(recursive(0, len(inliers) - 1))
+
+
+def point_line_distance(point, start, end):
+    """
+    点と直線の距離
+    """
+    matrix = np.array([start - point, end - point])
+    if np.linalg.matrix_rank(matrix) < 2:
+        return 0
+    u = point - start
+    v = normalize(end - start)
+    vt = np.inner(u, v) * v
+    norm = np.linalg.norm(u - vt)
+    return norm

ply_processor_basics/points/convex_hull/detect_plane_edge.py

@@ -1,11 +1,15 @@
+from typing import Tuple
+
 import numpy as np
 from numpy.typing import NDArray
 from scipy.spatial import ConvexHull
 
 from ply_processor_basics.points import transform_to_plane_coordinates
 
 
-def detect_plane_edge(points: NDArray[np.floating], plane_model: NDArray[np.floating]):
+def detect_plane_edge(
+    points: NDArray[np.floating], plane_model: NDArray[np.floating]
+) -> Tuple[NDArray[np.intp], NDArray[np.intp]]:
     """
     ConvexHullを用いて平面上のエッジ点を抽出する
 
@@ -23,4 +27,5 @@ def detect_plane_edge(points: NDArray[np.floating], plane_model: NDArray[np.floa
     hull = ConvexHull(points_xy)
 
     inliers = hull.vertices
-    return inliers
+    lines = hull.simplices
+    return inliers, lines

tests/points/convex_hull/test_convex_detect_edge.py

@@ -12,7 +12,7 @@ def test_visualize(plypath):
     pcd = o3d.io.read_point_cloud(plypath)
     points = np.asarray(pcd.points)
     inliers, plane_model = detect_plane(points, 1.0)
-    edge_inliers = detect_plane_edge(points[inliers], plane_model)
+    edge_inliers, lines = detect_plane_edge(points[inliers], plane_model)
 
     pcd2 = o3d.geometry.PointCloud()
     pcd2.points = o3d.utility.Vector3dVector(points[inliers][edge_inliers])
@@ -42,7 +42,7 @@ def test_success(plypath):
     pcd = o3d.io.read_point_cloud(plypath)
     points = np.asarray(pcd.points)
     inliers, plane_model = detect_plane(points)
-    edge_inliers = detect_plane_edge(points[inliers], plane_model)
+    edge_inliers, lines = detect_plane_edge(points[inliers], plane_model)
 
     assert len(points[inliers][edge_inliers]) > 0
 
@@ -71,7 +71,7 @@ def test_visualize_realdata(plypath):
     pcd_ = o3d.geometry.PointCloud()
     pcd_.points = o3d.utility.Vector3dVector(points)
     o3d.visualization.draw_geometries([pcd_])
-    edge_inliers = detect_plane_edge(points, plane_model)
+    edge_inliers, lines = detect_plane_edge(points, plane_model)
 
     pcd2 = o3d.geometry.PointCloud()
     pcd2.points = o3d.utility.Vector3dVector(points[edge_inliers])

tests/points/convex_hull/test_convex_detect_line.py

@@ -0,0 +1,84 @@
+import numpy as np
+import open3d as o3d
+import pytest
+
+from ply_processor_basics.points.convex_hull import detect_line
+from ply_processor_basics.points.convex_hull.detect_line import ramer_douglas_peucker
+from ply_processor_basics.points.ransac import detect_plane
+
+
+@pytest.mark.visual
+@pytest.mark.parametrize("plypath", ["data/samples/sample.ply"])
+def test_visualize(plypath):
+    pcd = o3d.io.read_point_cloud(plypath)
+    points = np.asarray(pcd.points)
+    inliers, plane_model = detect_plane(points, 1.0)
+    inliers2, lines, line_models = detect_line(points[inliers], plane_model)
+
+    # 凸包の線分を描画
+    line_set = o3d.geometry.LineSet()
+    line_set.points = o3d.utility.Vector3dVector(points[inliers])
+    line_set.lines = o3d.utility.Vector2iVector(lines)
+    o3d.visualization.draw_geometries([line_set])
+
+
+@pytest.mark.parametrize("plypath", ["data/samples/sample.ply"])
+def test_success(plypath):
+    pcd = o3d.io.read_point_cloud(plypath)
+    points = np.asarray(pcd.points)
+    inliers, plane_model = detect_plane(points, 1.0)
+    inliers2, lines, line_models = detect_line(points[inliers], plane_model)
+
+
+@pytest.mark.parametrize("plypath", ["data/samples/sample.ply"])
+def test_lines_expected(plypath):
+    pcd = o3d.io.read_point_cloud(plypath)
+    points = np.asarray(pcd.points)
+    inliers, plane_model = detect_plane(points, 1.0)
+    inliers2, lines, line_models = detect_line(points[inliers], plane_model)
+
+    print("")
+    for line in line_models:
+        print(line)
+
+    # 直線のどれかが以下である事を期待する
+    # (0, 0, 10)を通り、(1, 0, 0)の方向を持つ
+
+    # (0, 0, 10)を通り、(0, 1, 0)の方向を持つ
+    # (0, 122, 10)を通り、(1, 0, 0)の方向を持つ
+
+
+def is_line_present(lines, line):
+    for l in lines:
+        if np.allclose(l, line):
+            return True
+    return False
+
+
+def test_douglas_peucker():
+    # 直線状の点はすべて排除
+    points = np.array([[0, 0, 0], [1, 0, 0], [2, 0, 0], [3, 0, 0], [4, 0, 0], [5, 0, 0]])
+    inliers = np.arange(len(points))
+    epsilon = 0.5
+    results = ramer_douglas_peucker(points, inliers, epsilon)
+    assert len(results) == 2
+    assert np.allclose(points[results[0]], [0, 0, 0])
+    assert np.allclose(points[results[1]], [5, 0, 0])
+
+    # 全て残る
+    points = np.array([[0, 0, 0], [2, 0, 0], [2, 2, 0], [0, 2, 0]])
+    inliers = np.arange(len(points))
+    epsilon = 0.5
+    results = ramer_douglas_peucker(points, inliers, epsilon)
+    assert len(results) == 4
+    assert np.allclose(points[results[0]], [0, 0, 0])
+    assert np.allclose(points[results[-1]], [0, 2, 0])
+
+    # 一部が排除
+    points = np.array([[0, 0, 0], [1, 0, 0], [2, 1, 0], [3, 1, 0], [4, 0, 0], [5, 0, 0]])
+    inliers = np.arange(len(points))
+    epsilon = 0.5
+    results = ramer_douglas_peucker(points, inliers, epsilon)
+    assert len(results) == 3
+    assert np.allclose(points[results[0]], [0, 0, 0])
+    assert np.allclose(points[results[-1]], [5, 0, 0])