<jupyter_start><jupyter_text>Normal Estimation from Depth Image 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).<jupyter_code>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/scripts/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')

from manipulation import running_as_notebook

# Setup rendering (with xvfb), if necessary:
import os
if 'google.colab' in sys.modules and os.getenv("DISPLAY") is None:
    from pyvirtualdisplay import Display
    display = Display(visible=0, size=(1400, 900))
    display.start()

# setup ngrok server
server_args = []
if 'google.colab' in sys.modules:
  server_args = ['--ngrok_http_tunnel']

from meshcat.servers.zmqserver import start_zmq_server_as_subprocess
proc, zmq_url, web_url = start_zmq_server_as_subprocess(server_args=server_args)

import numpy as np

from pydrake.all import ( 
    AddMultibodyPlantSceneGraph, ConnectMeshcatVisualizer, 
    DiagramBuilder, RigidTransform, RotationMatrix, Parser, Simulator,
    FindResourceOrThrow,Quaternion, AddTriad
)

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

from manipulation.meshcat_utils import draw_open3d_point_cloud, draw_points
from manipulation.mustard_depth_camera_example import MustardExampleSystem
from manipulation.utils import FindResource
from manipulation.open3d_utils import create_open3d_point_cloud

import matplotlib.pyplot as plt
from matplotlib.patches import Rectangle
from copy import deepcopy
import pydot
from ipywidgets import Dropdown, Layout
from IPython.display import display, HTML, SVG<jupyter_output><empty_output><jupyter_text>Problem DescriptionIn the lecture, we learned about estimating the point cloud normal vectors and surface curvations. For this exercise, you will investigate a slightly different approach. In particular, you will exploit the structure already presented in a depth image to avoid computing nearest neighbors. **These are the main steps of the exercise:**1. Implement the `estimate_normal_by_nearest_pixels` method.2. Come up with an example that breaks the `estimate_normal_by_nearest_pixels` method.Run the cell below to set up the simulation environment.<jupyter_code>class NormalEstimation:
    def __init__(self):
        diagram = MustardExampleSystem()
        context = diagram.CreateDefaultContext()
        
        # setup
        self.meshcat_vis = meshcat.Visualizer(zmq_url=zmq_url)
        self.meshcat_vis["/Background"].set_property("visible", False)
        
        # getting data
        self.point_cloud = diagram.GetOutputPort("camera0_point_cloud").Eval(context)
        self.rgb_im = diagram.GetOutputPort('camera0_rgb_image').Eval(context).data
        self.depth_im_read = diagram.GetOutputPort('camera0_depth_image').Eval(context).data.squeeze()
        self.depth_im = deepcopy(self.depth_im_read)
        self.depth_im[self.depth_im == np.inf] = 10.0
        label_im = diagram.GetOutputPort('camera0_label_image').Eval(context).data.squeeze()
        pcd = create_open3d_point_cloud(self.point_cloud)
        draw_open3d_point_cloud(self.meshcat_vis["self.point_cloud"], pcd)
        self.mask = label_im == 1
        self.kdtree = o3d.geometry.KDTreeFlann(pcd)
        self.pts = np.asarray(pcd.points)
        
        # camera specs
        cam0 = diagram.GetSubsystemByName('camera0')
        cam0_context = cam0.GetMyMutableContextFromRoot(context)
        cam0_pose = cam0.GetOutputPort('X_WB').Eval(cam0_context)
        self.cam_info = cam0.depth_camera_info()
        self.X_WC = RigidTransform(quaternion=Quaternion(cam0_pose[3:]), p=cam0_pose[0:3])
        
    def project_depth_to_pC(self, depth_pixel, uv=None):
        """
        project depth pixels to points in camera frame
        using pinhole camera model
        Input:
            depth_pixels: numpy array of (nx3) or (3,)
        Output:
            pC: 3D point in camera frame, numpy array of (nx3)
        """
        # switch u,v due to python convention
        v = depth_pixel[:,0]
        u = depth_pixel[:,1]
        Z = depth_pixel[:,2]
        # read camera intrinsics
        cx = self.cam_info.center_x()
        cy = self.cam_info.center_y()
        fx = self.cam_info.focal_x()
        fy = self.cam_info.focal_y()
        X = (u-cx) * Z/fx
        Y = (v-cy) * Z/fy
        pC = np.c_[X,Y,Z]
        return pC

    def plot_scanning_window(self, u_range, v_range):
        """
        visualize the scanning window
        u_range: (u_start, u_end)
        v_range: (v_start, v_end)
        u, v are the 1st and 2nd axis of the image array
        """
        # switch u, v range to get x, y
        x0, x1 = v_range
        y0, y1 = u_range
        fig,  ax = plt.subplots()
        ax.imshow(self.depth_im)
        ax.add_patch( Rectangle((x0, y0), 
                                x1-x0, y1-y0,
                                alpha=0.5,
                                fc ='r') )
    def vis_normals(self, normals):
        """
        """
        for i in range(len(normals)):
            name = 'normal_vec_{}'.format(i)
            AddTriad(vis=self.meshcat_vis, name=name, prefix='', length=0.01, radius=0.001)
            self.meshcat_vis[''][name].set_transform(normals[i].GetAsMatrix4())
    
def bbox(img):
    a = np.where(img != 0)
    bbox = ([np.min(a[0]), np.max(a[0])], [np.min(a[1]), np.max(a[1])])
    return bbox
        
env = NormalEstimation()
mask = env.mask
depth_im = env.depth_im<jupyter_output><empty_output><jupyter_text>The object of interest is the mustard bottle. Our goal in this exercise is to compute the estimate of point cloud normals of the mustard bottle from a depth image. The depth image is visualized below.<jupyter_code>if (running_as_notebook):
    plt.imshow(depth_im)<jupyter_output><empty_output><jupyter_text>The core idea of the approach is to exploit the fact that a depth image already includes spatial information among pixels. For example, for a selected pixel, the pixels that surround it are likely to be its nearest neighbors. Therefore, instead of computing nearest neighbors, we can instead use the nearest pixels in place of nearest neighbors. The cell below provides a sequence of screenshots of the method, where a square/rectangular window moves across the depth image. All pixels in the sliding window is used to compute the normal vector of the center point of the window. In your implementation below, you will use a smaller window and a smaller step size to get better accuracy.<jupyter_code>uv_step = 40
u_bound, v_bound = bbox(mask)
for u in range(u_bound[0], u_bound[1], uv_step):
    for v in range(v_bound[0], v_bound[1], uv_step):
        center = [u,v]
        u_length = 30
        v_length = 30
        if (running_as_notebook):
            env.plot_scanning_window([center[0] - u_length, center[0] + u_length+1],
                                    [center[1] - v_length, center[1] + v_length+1])<jupyter_output><empty_output><jupyter_text>Mapping Depth Image to Point CloudNote that pixel indices of a depth image is not a valid position measurement in the 3D world. Fortunately, there is a simple mapping from pixel locations to poses in the 3D world, and it is called the [pinhole camera model](https://docs.opencv.org/3.4/d9/d0c/group__calib3d.html). We have helped you map all pixels of the depth image to points in the camera frame in the cell below. In case you need to gain direct access to this mapping, please refer to the `project_depth_to_pC` method in the `NormalEstimation` class.The diagram below is found from [OpenCV documentation](https://docs.opencv.org/3.4/d9/d0c/group__calib3d.html). Note that the $u$ and $v$ directions are reversed in Python due to the difference in convention. ![](https://docs.opencv.org/3.4/pinhole_camera_model.png)<jupyter_code>u_range = np.arange(depth_im.shape[0])
v_range = np.arange(depth_im.shape[1])
depth_v, depth_u = np.meshgrid(v_range, u_range)
depth_pnts = np.dstack([depth_u, depth_v, depth_im])
depth_pnts = depth_pnts.reshape([depth_pnts.shape[0]*depth_pnts.shape[1], 3])
# point poses in camera frame
pC =env.project_depth_to_pC(depth_pnts)<jupyter_output><empty_output><jupyter_text>Computing Surface Normals by Nearest PixelsNow you should be able to complete the implementation in the cell below. Note that locations of sliding windows are provided to you for the ease of grading. the pose of the depth camera is `X_WC`, **it is a different depth camera from the one shown in the meshcat visualizer**. **Complete the implementation of the `estimate_normal_by_nearest_pixels` below, make sure the +z axis of the normal frame points outward, toward the depth camera (different from the one shown in the meshcat visualizer)**<jupyter_code>X_WC = env.X_WC
def estimate_normal_by_nearest_pixels(X_WC, pC, uv_step=10):
    """
    compute the surface normals from the nearest pixels (by a sliding window)
    Input:
        X_WC: RigidTransform of the camera in world frame
        pC: 3D points computed from the depth image in the camera frame
        uv_step: recommended step size for the sliding window (see codes below)
    Output:
        normals: a list of RigidTransforms of the normal frames in world frame.
                 The +z axis of the normal frame is the normal vector, it should
                 points outward (towards the camera)
    """
    normals = []
    u_bound, v_bound = bbox(mask)
    for u in range(u_bound[0], u_bound[1], uv_step):
        for v in range(v_bound[0], v_bound[1], uv_step):
            # center of the window at depth_im[u,v]
            center = [u,v]
            u_length = 3
            v_length = 3
            # side of the window
            u_range = np.arange(center[0] - u_length, center[0] + u_length+1)
            v_range = np.arange(center[1] - v_length, center[1] + v_length+1)
            ###################
            # fill your code here
            ###################
    return normals
normals = estimate_normal_by_nearest_pixels(X_WC, pC)
env.vis_normals(normals)<jupyter_output><empty_output><jupyter_text>Normal Vector Estimation with Noisy DepthThe depth image tested in the first part of this exercise is a perfect depth image with no noise and missing values. Now imagine what will happen when noises and outliers are presented in the same depth image. **Illustrate a counter-example illustrating a case where the scanning window method cannot produce a good normal estimate.**  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 `normal_estimation_depth.ipynb` to Gradescope's notebook submission section, along with your notebook for the other problems.- Answer the counter-example in the written section of Gradescope as a part of your `pdf` submission. 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:- [4 pts] `estimate_normal_by_nearest_pixels` must be implemented correctly. - [2 pts] Provide a reasonable scenario that breaks the `estimate_normal_by_nearest_pixels` method.<jupyter_code>from manipulation.exercises.clutter.test_normal import TestNormal
from manipulation.exercises.grader import Grader 

Grader.grade_output([TestNormal], [locals()], 'results.json')
Grader.print_test_results('results.json')<jupyter_output><empty_output>
{'fraction_non_alphanumeric': 0.08468497840084767, 'fraction_numerical': 0.012470453989730214, 'mean_word_length': 3.683206106870229, 'pattern_counts': {'":': 0, '<': 29, '<?xml version=': 0, '>': 29, 'https://': 3, 'lorem ipsum': 0, 'www.': 1, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 3, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'chain_length': '7', 'dir': 'jupyter-structured-clean-dedup', 'id': '49113', 'license': 'BSD-3-Clause', 'n_tokens_mistral': 3781, 'n_tokens_neox': 3667, 'n_words': 1207, 'path': 'exercises/clutter/normal_estimation_depth.ipynb', 'repo_name': 'avalenzu/manipulation', 'size': '12269'}