Pset #3 Part 2 of 2 Robust Perception

For the bonus question in problem set 2, many of you extracted the corners of the model and scene point clouds, and achieved 10/10 matching by running ICP on only the corner points. Although seemingly hacky, this strategy is actually an example of using features, whose correspondences between two point clouds can be easily determined.

Unfortunately, real point cloud data are usually noisy, and features such as corners are not always straightforward to extract. In this notebook, we will walk you through an alternative approach to reliably estimate the pose of the box by using normals of the point clouds.

0. Setup

In [ ]:
%reload_ext autoreload
%autoreload 2
import matplotlib.pyplot as plt
from matplotlib import cm
import numpy as np
import meshcat
import meshcat.geometry as g

from point_cloud_processing import (VisualizeTransformedScenePointCloud,
                                    make_meshcat_color_array,
                                    GetBoxScenePointCloud, X_WBox)
from sklearn.neighbors import NearestNeighbors
from pydrake.math import RigidTransform, RotationMatrix, RollPitchYaw
In [ ]:
# First start meshcat for visualization - this only has to be run once.

# If you interrupt the kernel of this notebook, you'll need to run this cell again to 
# restart the meshcat server, and then refresh the visualization window. 

# This will open a mesh-cat server in the background, click on the url to display visualization in a separate window. 
vis = meshcat.Visualizer()
In [ ]:
# Clear visualizer.
vis.delete()
In [ ]:
# Load box scene point cloud.
box_point_cloud_scene = GetBoxScenePointCloud()

# Model box point cloud
box_point_cloud_model = np.load("./resources/box_point_cloud_model.npy")
# Remove the bottom of the sampled model point cloud
box_point_cloud_model = box_point_cloud_model[box_point_cloud_model[:, 2] > 0.002]
In [ ]:
# show point clouds in meshcat
# scene point cloud shown in blue
vis["scene_point_cloud"].set_object(
    g.PointCloud(position=box_point_cloud_scene.T,
                 color=make_meshcat_color_array(box_point_cloud_scene.shape[0], 0, 0, 1),
                 size=0.002))

# model point cloud shown in red
vis['model_point_cloud'].set_object(
    g.PointCloud(box_point_cloud_model.T, make_meshcat_color_array(len(box_point_cloud_model), 1.0, 0, 0),
                 size=0.002))

1. Planar fits

Recall that a plane equation is of the form $$a x + b y + c z + d = 0$$ where $[a,b,c]^T$ is a vector normal to the plane and (if it's a unit normal) $d$ is the negative of the distance from the origin to the plane in the direction of the normal. We'll representa a plane by the vector $[a,b,c,d]$.

We'll start by fitting a plane to a small neighborhood (a ball of radius $r$) of points. As we discussed in class this can be (again) done by SVD. You can find a discussion of this in these slides. Look at the slide entitled "Best Fitting Plane Recipe -- PCA". Note however that this assumes that the data points in $Y$ are stored as columns, whereas we've been storing our points as rows. So, we have the transpose of $Y$ as data... how do we get the normal?

In the file ransac.py complete the function fit_plane. It takes a data matrix and returns a plane equation (as a (4,) array).

The fitted planes are shown as translucent disks of radius $r$ centered at the points. The gray lines represent the planes' normals.

In [ ]:
from ransac import fit_plane, test_on_points
pts_model = np.array([300, 573, 2000])
pts_scene = np.array([5421, 7000, 44375])
r=0.02
test_on_points(vis, 'Model', box_point_cloud_model, 
               lambda cloud, indices: fit_plane(cloud[indices]), pts=pts_model, radius=r)
test_on_points(vis, 'Scene', box_point_cloud_scene, 
               lambda cloud, indices: fit_plane(cloud[indices]), pts=pts_scene, radius=r)

2. Ransac

If we run planar fits on point sets with "outliers", the plane fits can be quite wrong. For instance, if you try to fit a plane at a point near an edge of the box, the fitted plane is unlikely to align with one of the box's faces.

In such situations, we can use Ransac to get more reliable estimates. The idea is to fit a plane using many random choice of a minimal set of points (3), fit a plane for each one, count how many points are within a distance tolerance to that plane (the inliers) and return the estimate with the most inliers.

In the file ransac.py, complete the function fit_plane_ransac. It takes a data matrix, a tolerance and a number of iterations. It returns a plane equation and a count of inliers.

In [ ]:
from ransac import fit_plane_ransac, test_on_points
# seed random number generator for deterministic outcome.
np.random.seed(6881)

pts_model = np.array([300, 573, 2000])
pts_scene = np.array([5421, 7000, 44375])
test_on_points(vis, 'Model', box_point_cloud_model,
               lambda cloud, indices: fit_plane_ransac(cloud[indices], 1e-5, 10),
               pts=pts_model)
test_on_points(vis, 'Scene', box_point_cloud_scene,
               lambda cloud, indices: fit_plane_ransac(cloud[indices], 1e-3, 10),
               pts=pts_scene)

3. Voxel sub-sampling

As the scene point cloud is generated from depth cameras, the distribution of points on different faces of the box is highly non-uniform. The top face is the most dense because it can be seen by every camera. Some faces are very sparse because they are only visible to a single camera at a high incidence angle.

This non-uniformity can change the geometric center of the point cloud, and over-represent some faces than others. A common technique to restore uniformity is called voxel sub-sampling or voxel-grid filtering.

A voxel is a cube inside a large 3D grid. The idea behind voxel sub-sampling is to put your point cloud in a 3D grid, and replace all points inside a voxel by a single point. Specifically, the algorithm takes the following steps to down-sample your point cloud:

  1. Choose a reasonable voxel_size.
  2. For every point (x, y, z) in the point cloud, compute the integer indices of the voxel which (x, y, z) occupies by 
    (x_int, y_int, z_int) = int([x/voxel_size, y/voxel_size, z/voxel_size])
  3. Add (x, y, z) to a dictionary using (x_int, y_int, z_int) as its key.
  4. After looping through all points in the point cloud, the dictionary's values are the down-sampled point cloud.

Implement this algorithm in VoxelSubsample in file ransac.py.

In [ ]:
from ransac import VoxelSubsample

# Compute a reasonable voxel size.
box_max = np.max(box_point_cloud_scene, axis=0)
box_min = np.min(box_point_cloud_scene, axis=0)
voxel_size = np.linalg.norm(box_max - box_min) / 20

# voxel sub-sampling
box_point_cloud_scene_sampled = VoxelSubsample(box_point_cloud_scene, voxel_size)
box_point_cloud_model_sampled = VoxelSubsample(box_point_cloud_model, voxel_size)
In [ ]:
# Display sub-sampled scene point cloud in green.
vis["scene_point_cloud_sampled"].set_object(
    g.PointCloud(position=box_point_cloud_scene_sampled.T,
                 color=make_meshcat_color_array(
                     box_point_cloud_scene_sampled.shape[0], 0, 1, 0),
                     size=0.004))

# Display sub-sampled model point cloud in cyan.
vis["scene_point_model_sampled"].set_object(
    g.PointCloud(position=box_point_cloud_model_sampled.T,
                 color=make_meshcat_color_array(
                     box_point_cloud_model_sampled.shape[0], 0, 1, 1),
                     size=0.004))

4. Compute normals using Ransac

The next step is to compute the normals of the down-sampled point clouds using fit_plane_ransac.

A face has an "outward" and an "inward" normal, which differs from each other by a minus sign. Since the normals will later be used to compute the relative rotations between two point clouds, it is important that all normals of one face are pointing in the same direction. Note that Ransac doesn't tell you which normal it returns.

With that in mind, complete ComputeNormals in ransac.py. You need to make all normals point out of the box in order to pass the test case.

In [ ]:
from ransac import DrawNormals, ComputeNormals 
# compute normals
np.random.seed(6881)
radius = 0.03
normals_scene, points_scene = ComputeNormals(box_point_cloud_scene_sampled, radius=radius)
normals_model, points_model = ComputeNormals(box_point_cloud_model_sampled, radius=radius)

# print some of the estimated normals.
print(normals_model[:10])
In [ ]:
# Pick 50 normals to visualize
np.random.seed(6881)
def DrawSomeNormals(vis, name, num_normals_to_draw, normals, centers, radius):
    N = len(normals)
    pts_idx = np.random.randint(N, size=num_normals_to_draw)
    DrawNormals(
        vis=vis, name=name, normals=normals[pts_idx], 
        centers=centers[pts_idx], radius=radius)

DrawSomeNormals(vis=vis, name="scene", num_normals_to_draw=50, 
               normals=normals_scene, centers=points_scene, radius=radius)
DrawSomeNormals(vis=vis, name="model", num_normals_to_draw=50,
               normals=normals_model, centers=points_model, radius=radius)

5. Planar alignment using normals

The rotation from the scene point cloud to the model point cloud can be estimated by solving \begin{gather} \min_{R, c} \ \ \ \sum_{i} \| R n_{s_i} - n_{m_{c_i}}\|_2^2 \\ s.t. \ \ \ R^T = R^{-1}, \mathrm{det}(R) = 1 \\ \end{gather} where $n_{s_i}$ are the normals of the scene point cloud, $n_{m_j}$ are the normals of the model point cloud, and $c_i$ is the integer index of the point in the $m$ that corresponds most closely with the $i$th point in the scene.

Similar to the vanilla ICP, this optimization can be solved iteratively using SVD, with correspondences identified by \begin{align*} c_i = \underset{j}{\arg\min} \ \ \| R n_{s_i} - n_{m_j}\|_2^2 \\ \end{align*}

After obtaining the optimal solution $R^*$, $t^*$ is simply \begin{align*} t^* = \mu_m - R^* \mu_s \end{align*} where $\mu_m = \frac{1}{N_m} \sum_i m_{i}$ and $\mu_s = \frac{1}{N_s} \sum_i s_i$.

In this algorithm, the iterative steps are only used to solve for rotation. In comparsion, the vanilla ICP jointly updates rotation and translation in every step.

Implement the above algorithm by completing least_squares_transform and align_planes_with_normals in file planar_alignment.py.

Although the algorithm described here is expected to outperform vanilla ICP on boxes, it is not necessarily "better". Since this algorithm is matching normals with nearest normals while ignoring the position information, we only expect this to work well on convex objects, or non-convex objects whose convex hull shares some faces with the real object.

In [ ]:
from planar_alignment import align_planes_with_normals
from point_cloud_processing import VisualizeTransformedScenePointCloud

X_MS_estimated, mean_error, num_iters = align_planes_with_normals(
    box_point_cloud_scene_sampled, box_point_cloud_model_sampled, 
    normals_scene, normals_model)

VisualizeTransformedScenePointCloud(X_MS_estimated, box_point_cloud_scene_sampled, vis)
print("Final transform:")
print(X_MS_estimated.matrix())
print("mean error:", mean_error)
print("number of iterations:", num_iters)

from point_cloud_processing import X_WBox
X_error = X_MS_estimated.multiply(X_WBox)
print("translation_error: ", X_error.translation())
print("orientation_error (as a quaternion): ", X_error.rotation().ToQuaternion().wxyz())

6. Run your point cloud registration pipeline on the bonus question from Pset 2

Finally, you are ready to run the entire pipeline on the box-registration question from pset 2. If you've passed all tests, running the cells below should give 10 successes out of 10 tries. Enjoy!

In [ ]:
# Clean up visualization
vis.delete()
vis["scene_point_cloud"].set_object(
    g.PointCloud(position=box_point_cloud_scene.T,
                 color=make_meshcat_color_array(box_point_cloud_scene.shape[0], 0, 0, 1),
                 size=0.002))
In [ ]:
# get ground truth box pose.
from point_cloud_processing import X_WBox
from box_point_cloud_sampling import GetBoxModelPointCloud
np.random.seed(6881)

success_count = 0
for i in range(10):
    i += 1
    # Generate random model point cloud
    box_point_cloud_model_test = GetBoxModelPointCloud(num_points=3000)
    box_point_cloud_model_test = box_point_cloud_model_test[box_point_cloud_model_test[:, 2] > 0.002]
    
    # Sub-sampling.
    box_point_cloud_scene_sampled = VoxelSubsample(
        box_point_cloud_scene, voxel_size)
    box_point_cloud_model_sampled = VoxelSubsample(
        box_point_cloud_model_test, voxel_size)

    # compute normals
    normals_scene, centers_scene = ComputeNormals(
        box_point_cloud_scene_sampled, radius=radius)
    normals_model, centers_model = ComputeNormals(
        box_point_cloud_model_sampled, radius=radius)
    
    # Call ICP with normals.
    X_MS_estimated, mean_error, num_iters = align_planes_with_normals(
        box_point_cloud_scene_sampled, box_point_cloud_model_sampled, 
        normals_scene, normals_model)
    
    # Check results
    Xe = X_MS_estimated.multiply(X_WBox)
    is_translation_correct = np.allclose(Xe.translation(), np.zeros(3), atol=1e-2)

    Q_e = Xe.rotation().ToQuaternion()
    def rotation_about_xyz_by_pi(q):
        assert q.w() >= 0
        is_z = np.allclose(q.wxyz(), np.array([0, 0, 0, 1]), atol=0.01)
        is_y = np.allclose(q.wxyz(), np.array([0, 0, 1, 0]), atol=0.01)
        is_x = np.allclose(q.wxyz(), np.array([0, 1, 0, 0]), atol=0.01)
        return is_x or is_y or is_z

    is_rotation_correct = Q_e.w() > 0.99 or rotation_about_z_by_pi(Q_e)
    is_successful = is_translation_correct and is_rotation_correct
    if is_successful:
        success_count += 1
    print("trial ", i, 'success' if is_successful else 'fail')
        
    # Create a yellow meshcat point cloud for visualization.
    VisualizeTransformedScenePointCloud(X_MS_estimated, box_point_cloud_scene, vis)
    # model point cloud shown in red
    vis['model_point_cloud'].set_object(
        g.PointCloud(box_point_cloud_model_test.T, 
                     make_meshcat_color_array(len(box_point_cloud_model_test), 1.0, 0, 0),
                     size=0.002))

print("success_count:", success_count)

7. Testing your pipeline on real point clouds collected from the lab.

In lab 1, we will ask you to collect real point cloud data of one of the objects in the image below. You can generate a model boxy point cloud of any dimensions by using the function in the next cell.

Boxy objects

Write a short report on how well (or not well) the point cloud registration pipeline in this notebook works on the data you collected, show your results with some pictures, and give a short analysis on your findings.

In [ ]:
from box_point_cloud_sampling import GetBoxModelPointCloud
my_box_model_point_cloud = GetBoxModelPointCloud(num_points=2000, l=0.2, w=0.15, h=0.15)

Run Test

In [ ]:
import subprocess
# Run the tests
subprocess.run("python3 ./test_pset_3.py test_results.json", shell=True)

import test_pset_3
# Print the results json for review
print(test_pset_3.pretty_format_json_results("test_results.json"))
In [ ]: