Add Normal vector estimation (#781)

* add normal vector calculation routine.

* add normal vector calculation routine.

* add normal vector estimation

* fix unittests in not matplotlib frontend

* fix lint ci

* add ransac based normal distribution estimation

* add normal_vector_estimation_main.rst

* normal_vector_estimation_main.rst を更新

* update normal_vector_estimation_main.rst

* update normal_vector_estimation_main.rst

* normal_vector_estimation_main.rst

* normal_vector_estimation_main.rst を更新

* add normal_vector_estimation_main.rst

* add normal_vector_estimation_main.rst

* add normal_vector_estimation_main.rst

* add normal_vector_estimation_main.rst

* add normal_vector_estimation_main.rst

* add normal_vector_estimation_main.rst

* add normal_vector_estimation_main.rst

* add normal_vector_estimation_main.rst

* add normal_vector_estimation_main.rst

ArmNavigation/rrt_star_seven_joint_arm_control/rrt_star_seven_joint_arm_control.py

@@ -5,7 +5,6 @@ Author: Mahyar Abdeetedal (mahyaret)
 import math
 import random
 import numpy as np
-from mpl_toolkits import mplot3d
 import matplotlib.pyplot as plt
 import sys
 import pathlib
@@ -79,8 +78,9 @@ class RRTStar:
         self.obstacle_list = obstacle_list
         self.connect_circle_dist = connect_circle_dist
         self.goal_node = self.Node(goal)
-        self.ax = plt.axes(projection='3d')
         self.node_list = []
+        if show_animation:
+            self.ax = plt.axes(projection='3d')
 
     def planning(self, animation=False, search_until_max_iter=False):
         """
@@ -176,7 +176,7 @@ class RRTStar:
 
     def find_near_nodes(self, new_node):
         nnode = len(self.node_list) + 1
-        r = self.connect_circle_dist * math.sqrt((math.log(nnode) / nnode))
+        r = self.connect_circle_dist * math.sqrt(math.log(nnode) / nnode)
         # if expand_dist exists, search vertices in
         # a range no more than expand_dist
         if hasattr(self, 'expand_dis'):

Mapping/normal_vector_estimation/normal_vector_estimation.py

@@ -0,0 +1,177 @@
+import numpy as np
+from matplotlib import pyplot as plt
+
+from utils.plot import plot_3d_vector_arrow, plot_triangle, set_equal_3d_axis
+
+show_animation = True
+
+
+def calc_normal_vector(p1, p2, p3):
+    """Calculate normal vector of triangle
+
+    Parameters
+    ----------
+    p1 : np.array
+        3D point
+    p2 : np.array
+        3D point
+    p3 : np.array
+        3D point
+
+    Returns
+    -------
+    normal_vector : np.array
+        normal vector (3,)
+
+    """
+    # calculate two vectors of triangle
+    v1 = p2 - p1
+    v2 = p3 - p1
+
+    # calculate normal vector
+    normal_vector = np.cross(v1, v2)
+
+    # normalize vector
+    normal_vector = normal_vector / np.linalg.norm(normal_vector)
+
+    return normal_vector
+
+
+def sample_3d_points_from_a_plane(num_samples, normal):
+    points_2d = np.random.normal(size=(num_samples, 2))  # 2D points on a plane
+    d = 0
+    for i in range(len(points_2d)):
+        point_3d = np.append(points_2d[i], 0)
+        d += normal @ point_3d
+    d /= len(points_2d)
+
+    points_3d = np.zeros((len(points_2d), 3))
+    for i in range(len(points_2d)):
+        point_2d = np.append(points_2d[i], 0)
+        projection_length = (d - normal @ point_2d) / np.linalg.norm(normal)
+        points_3d[i] = point_2d + projection_length * normal
+
+    return points_3d
+
+
+def distance_to_plane(point, normal, origin):
+    dot_product = np.dot(normal, point) - np.dot(normal, origin)
+    if np.isclose(dot_product, 0):
+        return 0.0
+    else:
+        distance = abs(dot_product) / np.linalg.norm(normal)
+        return distance
+
+
+def ransac_normal_vector_estimation(points_3d, inlier_radio_th=0.7,
+                                    inlier_dist=0.1, p=0.99):
+    """
+    RANSAC based normal vector estimation
+
+    Parameters
+    ----------
+    points_3d : np.array
+        3D points (N, 3)
+    inlier_radio_th : float
+        Inlier ratio threshold. If inlier ratio is larger than this value,
+        the iteration is stopped. Default is 0.7.
+    inlier_dist : float
+        Inlier distance threshold. If distance between points and estimated
+        plane is smaller than this value, the point is inlier. Default is 0.1.
+    p : float
+         Probability that at least one of the sets of random samples does not
+         include an outlier. If this probability is near 1, the iteration
+         number is large. Default is 0.99.
+
+    Returns
+    -------
+    center_vector : np.array
+        Center of estimated plane. (3,)
+    normal_vector : np.array
+        Normal vector of estimated plane. (3,)
+
+    """
+    center = np.mean(points_3d, axis=0)
+
+    max_iter = int(np.floor(np.log(1.0-p)/np.log(1.0-(1.0-inlier_radio_th)**3)))
+
+    for ite in range(max_iter):
+        # Random sampling
+        sampled_ids = np.random.choice(points_3d.shape[0], size=3,
+                                       replace=False)
+        sampled_points = points_3d[sampled_ids, :]
+        p1 = sampled_points[0, :]
+        p2 = sampled_points[1, :]
+        p3 = sampled_points[2, :]
+        normal_vector = calc_normal_vector(p1, p2, p3)
+
+        # calc inlier ratio
+        n_inliner = 0
+        for i in range(points_3d.shape[0]):
+            p = points_3d[i, :]
+            if distance_to_plane(p, normal_vector, center) <= inlier_dist:
+                n_inliner += 1
+        inlier_ratio = n_inliner / points_3d.shape[0]
+        print(f"Iter:{ite}, {inlier_ratio=}")
+        if inlier_ratio > inlier_radio_th:
+            return center, normal_vector
+
+    return center, None
+
+
+def main1():
+    p1 = np.array([0.0, 0.0, 1.0])
+    p2 = np.array([1.0, 1.0, 0.0])
+    p3 = np.array([0.0, 1.0, 0.0])
+
+    center = np.mean([p1, p2, p3], axis=0)
+    normal_vector = calc_normal_vector(p1, p2, p3)
+    print(f"{center=}")
+    print(f"{normal_vector=}")
+
+    if show_animation:
+        fig = plt.figure()
+        ax = fig.add_subplot(projection='3d')
+        set_equal_3d_axis(ax, [0.0, 2.5], [0.0, 2.5], [0.0, 3.0])
+        plot_triangle(p1, p2, p3, ax)
+        ax.plot(center[0], center[1], center[2], "ro")
+        plot_3d_vector_arrow(ax, center, center + normal_vector)
+        plt.show()
+
+
+def main2(rng=None):
+    true_normal = np.array([0, 1, 1])
+    true_normal = true_normal / np.linalg.norm(true_normal)
+    num_samples = 100
+    noise_scale = 0.1
+
+    points_3d = sample_3d_points_from_a_plane(num_samples, true_normal)
+    # add random noise
+    points_3d += np.random.normal(size=points_3d.shape, scale=noise_scale)
+
+    print(f"{points_3d.shape=}")
+
+    center, estimated_normal = ransac_normal_vector_estimation(
+        points_3d, inlier_dist=noise_scale)
+
+    if estimated_normal is None:
+        print("Failed to estimate normal vector")
+        return
+
+    print(f"{true_normal=}")
+    print(f"{estimated_normal=}")
+
+    if show_animation:
+        fig = plt.figure()
+        ax = fig.add_subplot(projection='3d')
+        ax.plot(points_3d[:, 0], points_3d[:, 1], points_3d[:, 2], ".r")
+        plot_3d_vector_arrow(ax, center, center + true_normal)
+        plot_3d_vector_arrow(ax, center, center + estimated_normal)
+        set_equal_3d_axis(ax, [-3.0, 3.0], [-3.0, 3.0], [-3.0, 3.0])
+        plt.title("RANSAC based Normal vector estimation")
+        plt.show()
+
+
+if __name__ == '__main__':
+    # main1()
+    main2()

PathPlanning/DynamicWindowApproach/dynamic_window_approach.py

@@ -300,8 +300,7 @@ def main(gx=10.0, gy=10.0, robot_type=RobotType.circle):
     if show_animation:
         plt.plot(trajectory[:, 0], trajectory[:, 1], "-r")
         plt.pause(0.0001)
-
-    plt.show()
+        plt.show()
 
 
 if __name__ == '__main__':

PathPlanning/RRTStar/rrt_star.py

@@ -186,7 +186,7 @@ class RRTStar(RRT):
                     radius r
         """
         nnode = len(self.node_list) + 1
-        r = self.connect_circle_dist * math.sqrt((math.log(nnode) / nnode))
+        r = self.connect_circle_dist * math.sqrt(math.log(nnode) / nnode)
         # if expand_dist exists, search vertices in a range no more than
         # expand_dist
         if hasattr(self, 'expand_dis'):
@@ -280,7 +280,7 @@ def main():
             rrt_star.draw_graph()
             plt.plot([x for (x, y) in path], [y for (x, y) in path], 'r--')
             plt.grid(True)
-    plt.show()
+            plt.show()
 
 
 if __name__ == '__main__':

PathPlanning/VisibilityRoadMap/visibility_road_map.py

@@ -62,8 +62,9 @@ class VisibilityRoadMap:
             for (vx, vy) in zip(cvx_list, cvy_list):
                 nodes.append(DijkstraSearch.Node(vx, vy))
 
-        for node in nodes:
-            plt.plot(node.x, node.y, "xr")
+        if self.do_plot:
+            for node in nodes:
+                plt.plot(node.x, node.y, "xr")
 
         return nodes
 

docs/modules/mapping/mapping_main.rst

@@ -13,4 +13,4 @@ Mapping
    k_means_object_clustering/k_means_object_clustering
    circle_fitting/circle_fitting
    rectangle_fitting/rectangle_fitting
-
+   normal_vector_estimation/normal_vector_estimation

docs/modules/mapping/normal_vector_estimation/normal_vector_estimation_main.rst

@@ -0,0 +1,74 @@
+Normal vector estimation
+-------------------------
+
+
+Normal vector calculation of a 3D triangle
+~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+
+A 3D point is as a vector:
+
+.. math:: p = [x, y, z]
+
+When there are 3 points in 3D space, :math:`p_1, p_2, p_3`,
+
+we can calculate a normal vector n of a 3D triangle which is consisted of the points.
+
+.. math:: n = \frac{v1 \times v2}{|v1 \times v2|}
+
+where
+
+.. math:: v1 = p2 - p1
+
+.. math:: v2 = p3 - p1
+
+This is an example of normal vector calculation:
+
+.. figure:: normal_vector_calc.png
+
+API
+=====
+
+.. autofunction:: Mapping.normal_vector_estimation.normal_vector_estimation.calc_normal_vector
+
+Normal vector estimation with RANdam SAmpling Consensus(RANSAC)
+~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+
+Consider the problem of estimating the normal vector of a plane based on a
+set of N 3D points where a plane can be observed.
+
+There is a way that uses all point cloud data to estimate a plane and
+a normal vector using the `least-squares method <https://stackoverflow.com/a/44315221/8387766>`_
+
+However, this method is vulnerable to noise of the point cloud.
+
+In this document, we will use a method that uses
+`RANdam SAmpling Consensus(RANSAC) <https://en.wikipedia.org/wiki/Random_sample_consensus>`_
+to estimate a plane and a normal vector.
+
+RANSAC is a robust estimation methods for data set with outliers.
+
+
+
+This RANSAC based normal vector estimation method is as follows:
+
+#. Select 3 points randomly from the point cloud.
+
+#. Calculate a normal vector of a plane which is consists of the sampled 3 points.
+
+#. Calculate the distance between the calculated plane and the all point cloud.
+
+#. If the distance is less than a threshold, the point is considered to be an inlier.
+
+#. Repeat the above steps until the inlier ratio is greater than a threshold.
+
+
+
+This is an example of RANSAC based normal vector estimation:
+
+.. figure:: ransac_normal_vector_estimation.png
+
+API
+=====
+
+.. autofunction:: Mapping.normal_vector_estimation.normal_vector_estimation.ransac_normal_vector_estimation
+

ruff.toml

@@ -1,7 +1,7 @@
 line-length = 88
 
 select = ["F", "E", "W", "UP"]
-ignore = ["E501", "E741"]
+ignore = ["E501", "E741", "E402"]
 exclude = [
 ]
 

tests/test_normal_vector_estimation.py

@@ -0,0 +1,19 @@
+import conftest  # Add root path to sys.path
+from Mapping.normal_vector_estimation import normal_vector_estimation as m
+import random
+
+random.seed(12345)
+
+
+def test_1():
+    m.show_animation = False
+    m.main1()
+
+
+def test_2():
+    m.show_animation = False
+    m.main2()
+
+
+if __name__ == '__main__':
+    conftest.run_this_test(__file__)

utils/plot.py

@@ -4,6 +4,10 @@ Matplotlib based plotting utilities
 import math
 import matplotlib.pyplot as plt
 import numpy as np
+from mpl_toolkits.mplot3d import art3d
+from matplotlib.patches import FancyArrowPatch
+from mpl_toolkits.mplot3d.proj3d import proj_transform
+from mpl_toolkits.mplot3d import Axes3D
 
 
 def plot_arrow(x, y, yaw, arrow_length=1.0,
@@ -84,3 +88,78 @@ def plot_curvature(x_list, y_list, heading_list, curvature,
     plt.plot(cx, cy, c, label=label)
     for ix, iy, icx, icy in zip(x_list, y_list, cx, cy):
         plt.plot([ix, icx], [iy, icy], c)
+
+
+class Arrow3D(FancyArrowPatch):
+
+    def __init__(self, x, y, z, dx, dy, dz, *args, **kwargs):
+        super().__init__((0, 0), (0, 0), *args, **kwargs)
+        self._xyz = (x, y, z)
+        self._dxdydz = (dx, dy, dz)
+
+    def draw(self, renderer):
+        x1, y1, z1 = self._xyz
+        dx, dy, dz = self._dxdydz
+        x2, y2, z2 = (x1 + dx, y1 + dy, z1 + dz)
+
+        xs, ys, zs = proj_transform((x1, x2), (y1, y2), (z1, z2), self.axes.M)
+        self.set_positions((xs[0], ys[0]), (xs[1], ys[1]))
+        super().draw(renderer)
+
+    def do_3d_projection(self, renderer=None):
+        x1, y1, z1 = self._xyz
+        dx, dy, dz = self._dxdydz
+        x2, y2, z2 = (x1 + dx, y1 + dy, z1 + dz)
+
+        xs, ys, zs = proj_transform((x1, x2), (y1, y2), (z1, z2), self.axes.M)
+        self.set_positions((xs[0], ys[0]), (xs[1], ys[1]))
+
+        return np.min(zs)
+
+
+def _arrow3D(ax, x, y, z, dx, dy, dz, *args, **kwargs):
+    '''Add an 3d arrow to an `Axes3D` instance.'''
+    arrow = Arrow3D(x, y, z, dx, dy, dz, *args, **kwargs)
+    ax.add_artist(arrow)
+
+
+def plot_3d_vector_arrow(ax, p1, p2):
+    setattr(Axes3D, 'arrow3D', _arrow3D)
+    ax.arrow3D(p1[0], p1[1], p1[2],
+               p2[0]-p1[0], p2[1]-p1[1], p2[2]-p1[2],
+               mutation_scale=20,
+               arrowstyle="-|>",
+               )
+
+
+
+def plot_triangle(p1, p2, p3, ax):
+    ax.add_collection3d(art3d.Poly3DCollection([[p1, p2, p3]], color='b'))
+
+
+def set_equal_3d_axis(ax, x_lims, y_lims, z_lims):
+    """Helper function to set equal axis
+
+    Args:
+        ax (Axes3DSubplot): matplotlib 3D axis, created by
+        `ax = fig.add_subplot(projection='3d')`
+        x_lims (np.array): array containing min and max value of x
+        y_lims (np.array): array containing min and max value of y
+        z_lims (np.array): array containing min and max value of z
+    """
+    x_lims = np.asarray(x_lims)
+    y_lims = np.asarray(y_lims)
+    z_lims = np.asarray(z_lims)
+    # compute max required range
+    max_range = np.array([x_lims.max() - x_lims.min(),
+                          y_lims.max() - y_lims.min(),
+                          z_lims.max() - z_lims.min()]).max() / 2.0
+    # compute mid-point along each axis
+    mid_x = (x_lims.max() + x_lims.min()) * 0.5
+    mid_y = (y_lims.max() + y_lims.min()) * 0.5
+    mid_z = (z_lims.max() + z_lims.min()) * 0.5
+
+    # set limits to axis
+    ax.set_xlim(mid_x - max_range, mid_x + max_range)
+    ax.set_ylim(mid_y - max_range, mid_y + max_range)
+    ax.set_zlim(mid_z - max_range, mid_z + max_range)