# Normal vector estimation — PythonRobotics documentation

# Normal vector estimation

## Normal vector calculation of a 3D triangle

A 3D point is as a vector:

\[p = [x, y, z]\]

When there are 3 points in 3D space, \(p_1, p_2, p_3\),

we can calculate a normal vector n of a 3D triangle which is consisted of the points.

\[n = \frac{v1 \times v2}{|v1 \times v2|}\]

where

\[v1 = p2 - p1\]
\[v2 = p3 - p1\]

This is an example of normal vector calculation:

### API

Mapping.normal_vector_estimation.normal_vector_estimation.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 – normal vector (3,)

  Return type

    np.array

## 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

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) 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:

1. Select 3 points randomly from the point cloud.

2. Calculate a normal vector of a plane which is consists of the sampled 3 points.

3. Calculate the distance between the calculated plane and the all point cloud.

4. If the distance is less than a threshold, the point is considered to be an inlier.

5. Repeat the above steps until the inlier ratio is greater than a threshold.

This is an example of RANSAC based normal vector estimation:

### API

Mapping.normal_vector_estimation.normal_vector_estimation.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,)