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

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)