# Bunny ICP

## Notebook Setup 
The following cell will install Drake, checkout the manipulation repository, and set up the path (only if necessary).
- On Google's Colaboratory, this **will take approximately two minutes** on the first time it runs (to provision the machine), but should only need to reinstall once every 12 hours.  

More details are available [here](http://manipulation.mit.edu/drake.html).


```python
import importlib
import os, sys
from urllib.request import urlretrieve

if 'google.colab' in sys.modules and importlib.util.find_spec('manipulation') is None:
    urlretrieve(f"http://manipulation.csail.mit.edu/setup/setup_manipulation_colab.py",
                "setup_manipulation_colab.py")
    from setup_manipulation_colab import setup_manipulation
    setup_manipulation(manipulation_sha='c1bdae733682f8a390f848bc6cb0dbbf9ea98602', drake_version='0.25.0', drake_build='releases')

# python libraries
import numpy as np

from manipulation import running_as_notebook

# Start a single meshcat server instance to use for the remainder of this notebook.
from meshcat.servers.zmqserver import start_zmq_server_as_subprocess
server_args = []
if 'google.colab' in sys.modules:
    server_args = ['--ngrok_http_tunnel']
proc, zmq_url, web_url = start_zmq_server_as_subprocess(server_args=server_args)

import meshcat.geometry as g
import meshcat.transformations as tf

from pydrake.all import RigidTransform, RotationMatrix

import open3d as o3d 
import meshcat

from IPython.display import clear_output
clear_output()

from manipulation import FindResource
```

## Problem Description
In the lecture, we learned about the Iterative Closest Point (ICP) algorithm. In this exercise, you will implement the ICP algorithm to solve the standard Stanford Bunny problem!

**These are the main steps of the exercise:**
1. Implement the ```least_squares_transform``` function to optimize transformation given correspondence
2. Implement the ```icp``` algorithm using the functions implemented above.

Let's first visualize the point clouds of Stanford bunny in meshcat!


```python
# Visualize Stanford Bunny 
pcd = o3d.io.read_point_cloud(FindResource("models/bunny/bun_zipper_res2.ply"))
pointcloud_model = np.asarray(pcd.points).transpose()

# First, clean the origin a bit to define nominal pose. 
X = np.array([[1., 0., 0., 0.0],
              [0., np.cos(np.pi/2), -np.sin(np.pi/2), 0.],
              [0., np.sin(np.pi/2), np.cos(np.pi/2), -0.05]])
Xtemp = RigidTransform(X)
X = np.array([[np.cos(np.pi/2), -np.sin(np.pi/2), 0, 0.],
              [np.sin(np.pi/2), np.cos(np.pi/2), 0., 0.],
              [0., 0., 1., 0.]])
X = RigidTransform(X).multiply(Xtemp)

pointcloud_model = X.multiply(pointcloud_model).T

# Next, set up a a distrubance 
X = np.array([[1., 0., 0., -1e-1],
              [0., np.cos(np.pi/6), -np.sin(np.pi/6), 1e-1],
              [0., np.sin(np.pi/6), np.cos(np.pi/6), 1e-1]])
X = RigidTransform(X)
pointcloud_scene = X.multiply(pointcloud_model.T).T

def generate_color_mat(color_vec, shape):
  color_mat = np.tile(np.array(color_vec).astype(np.float32).reshape(3,1), (1, shape[1]))
  return color_mat

vis = meshcat.Visualizer(zmq_url)
vis.delete()
vis["/Background"].set_property('visible', False)
#vis["/Cameras/default/"].set_transform(tf.translation_matrix([0, 0, 1]))
vis["/Cameras/default/rotated/<object>"].set_property("zoom", 10.5)

vis["red_bunny"].set_object(g.PointCloud(pointcloud_model.T, generate_color_mat([1, 0, 0], pointcloud_model.T.shape), size=0.01))
vis["blue_bunny"].set_object(g.PointCloud(pointcloud_scene.T, generate_color_mat([0, 0, 1], pointcloud_scene.T.shape), size=0.01))
```

## Point cloud registration with known correspondences

In this section, you will follow the [derivation](http://manipulation.csail.mit.edu/pose.html#section3) to solve the optimization problem below. 

\begin{align}
    \min_{p\in\mathbb{R}^3,R\in\mathbb{R}^{3\times3}} \quad & \sum_{i=1}^{N_s} \| p + R
    \hspace{.1cm} {^Op^{m_{c_i}}} - p^{s_i}\|^2, \\ s.t. \quad & RR^T =
    I, \quad \det(R)=1\end{align}
    
The goal is to find the transform that registers the point clouds of the model and the scene, assuming the correspondence is known.  You may refer to the implementation from [colab](https://colab.research.google.com/github/RussTedrake/manipulation/blob/master/pose.ipynb#scrollTo=AHfxMwrvb1mz) and the explanation from [textbook](http://manipulation.csail.mit.edu/pose.html#section4).

In the cell below, implement the ```least_squares_transform``` nethod.


```python
def least_squares_transform(scene, model) -> RigidTransform:
    '''
    Calculates the least-squares best-fit transform that maps corresponding
    points scene to model.
    Args:
      scene: Nx3 numpy array of corresponding points
      model: Nx3 numpy array of corresponding points
    Returns:
      X_BA: A RigidTransform object that maps point_cloud_A on to point_cloud_B 
            such that
                        X_BA.multiply(model) ~= scene,
    '''
    X_BA = RigidTransform()
    ##################
    # your code here
    ##################
    return X_BA
```

## Point correspondence from closest point
The ```least_squares_transform``` function assumes that the point correspondence is known. Unfortunately, this is often not the case, so we will have to estimate the point correspondence as well. A common heuristics for estimating point correspondence is the closest point/nearest neighbor. 

We have implemented the closest neighbors using [Open3d's implementation](http://www.open3d.org/docs/release/python_api/open3d.geometry.KDTreeFlann.html), which uses [k-d trees](https://en.wikipedia.org/wiki/K-d_tree).


```python
def nearest_neighbors(scene, model):
    '''
    Find the nearest (Euclidean) neighbor in model for each
    point in scene
    Args:
        scene: Nx3 numpy array of points
        model: Mx3 numpy array of points
    Returns:
        distances: (N, ) numpy array of Euclidean distances from each point in
            scene to its nearest neighbor in model.
        indices: (N, ) numpy array of the indices in model of each
            scene point's nearest neighbor - these are the c_i's
    '''
    distances = np.empty(scene.shape[0], dtype=float)
    indices = np.empty(scene.shape[0], dtype=int)
    
    kdtree = o3d.geometry.KDTreeFlann(model.T)
    for i in range(model.shape[0]):
        nn = kdtree.search_knn_vector_3d(scene[i,], 1)
        indices[i] = nn[1][0]
        distances[i] = np.linalg.norm(scene[i,:] - model[indices[i],:])

    return distances, indices
```

## Iterative Closest Point (ICP)
Now you should be able to register two point clouds iteratively by first finding/updating the estimate of point correspondence with ```nearest_neighbors``` and then computing the transform using ```least_squares_transform```. You may refer to the explanation from [textbook](http://manipulation.csail.mit.edu/pose.html#section4).

**In the cell below, complete the implementation of ICP algorithm using the  ```nearest_neighbors``` and ```least_squares_transform``` methods from above.**


```python
def icp(scene, model, max_iterations=20, tolerance=1e-3, anim=None):
    '''
    Perform ICP to return the correct relative transform between two set of points.
    Args:
        scene: Nx3 numpy array of points
        model: Mx3 numpy array of points
        max_iterations: max amount of iterations the algorithm can perform.
        tolerance: tolerance before the algorithm converges.
    Returns:
      X_BA: A RigidTransform object that maps point_cloud_A on to point_cloud_B 
            such that
                        X_BA.multiply(model) ~= scene,
      mean_error: Mean of all pairwise distances. 
      num_iters: Number of iterations it took the ICP to converge. 
    '''
    X_BA = RigidTransform()

    mean_error = 0
    num_iters = 0
    prev_error = 0
    
    while True:
        num_iters += 1  
          
        # your code here
        ##################

        mean_error = np.inf # Modify to add mean error.
        ##################

        if abs(mean_error - prev_error) < tolerance or num_iters >= max_iterations:
            break

        prev_error = mean_error

        if anim is not None:
          with anim.at_frame(vis, 5 * num_iters) as frame:
            frame["red_bunny"].set_transform(X_BA.GetAsMatrix4())

    return X_BA, mean_error, num_iters
```

Now you should be able to visualize the registration of the Stanford bunny! Have fun!


```python
from meshcat.animation import Animation

anim = Animation()
icp(pointcloud_scene, pointcloud_model, max_iterations=30, tolerance=1e-5, anim=anim)

vis.set_animation(anim)
vis["/Animations/default"].set_property("timeScale", 10.0)
```

## How will this notebook be Graded?##

If you are enrolled in the class, this notebook will be graded using [Gradescope](www.gradescope.com). You should have gotten the enrollement code on our announcement in Piazza. 

For submission of this assignment, you must do two things. 
- Download and submit the notebook `bunny_icp.ipynb` to Gradescope's notebook submission section, along with your notebook for the other problems.
- Copy and Paste your answer to the kinematic singularity problem to Gradescope's written submission section. 

We will evaluate the local functions in the notebook to see if the function behaves as we have expected. For this exercise, the rubric is as follows:
- [3 pts] `least_squares_transform` must be implemented correctly. 
- [3 pts] `icp` must be implemented correctly.


```python
from manipulation.exercises.pose.test_icp import TestICP
from manipulation.exercises.grader import Grader 

Grader.grade_output([TestICP], [locals()], 'results.json')
Grader.print_test_results('results.json')
```


```python

```

ipynb
b99fa99a316d68438659fc0e30c75da855aebe3b
Jupyter Notebook
c823726326068545ef4a430ecf05c10c37c36e2f
BSD-3-Clause
JafarAbdi/manipulation
exercises/pose/bunny_icp.ipynb
c823726326068545ef4a430ecf05c10c37c36e2f
BSD-3-Clause
JafarAbdi/manipulation
exercises/pose/bunny_icp.ipynb
c823726326068545ef4a430ecf05c10c37c36e2f
BSD-3-Clause
JafarAbdi/manipulation
exercises/pose/bunny_icp.ipynb