Add Normal vector estimation (AtsushiSakai#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

Author: AtsushiSakai

ArmNavigation/rrt_star_seven_joint_arm_control/rrt_star_seven_joint_arm_control.py

@@ -5,7 +5,6 @@
 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 @@ def __init__(self, start, goal, robot, obstacle_list, rand_area,
         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 @@ def search_best_goal_node(self):
 
     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 @@ def find_near_nodes(self, new_node):
                     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 @@ def generate_visibility_nodes(self, start_x, start_y, goal_x, goal_y,
             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_calc.png

@@ -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
+

docs/modules/mapping/normal_vector_estimation/normal_vector_estimation_main.rst

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

docs/modules/mapping/normal_vector_estimation/ransac_normal_vector_estimation.png

@@ -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__)