[RoboND_Sensors] brief course notes #robotics

001_perception_sensors.txt

human v.s. robots:

human 3D vision: stereo vision, prior geometry, motion filtering

robot 3D vision: (actively) measure the distance

sensor types:

active sensors: use an enery source to probe the environment

passive sensors: use the enery source within the environment

sensor examples:

active: 
laser range finders (Lidar) 
time of flight camera (ToF)
ultrasonic sensors

passive:
monocular camera (use motion filter to infer structure)
sereo camers 

mixture: RGB-D camera


RGB-D Camera:

RGB-Camera + Depth Sensor

RGB-Camera: 
filter layer + Sensels (sensor array)
CFA (colar filter array)
Bayer Filter (most widely used CFA), diagnal blue and diagnal red + blue
demosaicing (color averaging with neighbors)

Depth Sensor:

use structured light (IR Projector, IR Camera, and On-Board Comparasion)

002_point_cloud.txt

a collection of 3-D points (representing the sensor signal)

associated with meta information:

RGB-values, intensity values, local curvature information

provide: spatial iterator, filter method, statistical analysis operators

003_camera_model.txt

camera calibration:

to measure intrinsic and extrinsic parameters to recover acurate sensor measurement from camera signal

pinhole camera model:

create image of surface embedded in 3D as projections on plane through pinhole
image will be up-side-down
the projection is by camera matrix C

Lens Camera:

use len to reflect multiple light ray to the imaging plane
also introduce distortion

Radial Distortion:
the effect is minimal near the center of the image
x_corrected = x(1 + k1r^2 +k2r^4 +k3r^6)
y_corrected = y(1 + k1r^2 +k2r^4 +k3r^6)


Tangential Distortion:
occurs when the camera len are not perfectly parallel to the image plane
x_corrected = x + [2p1xy + p2(r^2 + 2x^2)]
y_coorected = y + [p1(r^2 + 2y^2) + 2p2xy]

Mathematical Equivalence:

the pinhole model is mathematically equivalent to projection model

004_intrinsic_calibration.txt

use a known geometry pattern (e.g. chess board)

intrinsic parameters:

focal length, optical center, distortion coeffcients

extrinsic parameters:

the camera's body fixed frame w.r.t the world frame

use chess board:

1. create an array of standard target chess board inner points configuration
2. use mutliple calibration images to compute the chess inner points
3. use opencv calibrateCamera to find parameters
4. apply parameters to calibrate new image


return value explained:

the mtx is the intrinsic camera matrix
the dist is the distortion coefficient

005_mark_chessboard_corner.py

import numpy as np
import cv2
import matplotlib.pyplot as plt
import matplotlib.image as mpimg
import glob

# prepare object points
nx = 8
ny = 6

# Make a list of calibration images
images = glob.glob('./Cal*.jpg')
# Select any index to grab an image from the list
idx = -1
# Read in the image
img = mpimg.imread(images[idx])

# Convert to grayscale
gray = cv2.cvtColor(img, cv2.COLOR_RGB2GRAY)

# Find the chessboard corners
ret, corners = cv2.findChessboardCorners(gray, (nx, ny), None)

# If found, draw corners
if ret == True:
    # Draw and display the corners
    cv2.drawChessboardCorners(img, (nx, ny), corners, ret)
    plt.imshow(img)

006_calibrate_camera.py

import numpy as np
import cv2
import glob
import matplotlib.pyplot as plt
import matplotlib.image as mpimg

nx = 8
ny = 6
# prepare object points, like (0,0,0), (1,0,0), (2,0,0) ....,(6,5,0)
objp = np.zeros((ny * nx,3), np.float32)
objp[:,:2] = np.mgrid[0:nx, 0:ny].T.reshape(-1,2)

# Arrays to store object points and image points from all the images.
objpoints = [] # 3d points in real world space
imgpoints = [] # 2d points in image plane.

# Make a list of calibration images
images = glob.glob('./Cal*.jpg')

# Step through the list and search for chessboard corners
for idx, fname in enumerate(images):
    img = mpimg.imread(fname)
    gray = cv2.cvtColor(img, cv2.COLOR_RGB2GRAY)

    # Find the chessboard corners
    ret, corners = cv2.findChessboardCorners(gray, (nx, ny), None)

    # If found, add object points, image points
    if ret == True:
        objpoints.append(objp)
        imgpoints.append(corners)


img = cv2.imread('test_image.jpg')
img_size = (img.shape[1], img.shape[0])

# Do camera calibration given object points and image points
ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(objpoints, imgpoints, img_size, None, None)

# Perform undistortion
undist = cv2.undistort(img, mtx, dist, None, mtx)

# Visualize undistortion
f, (ax1, ax2) = plt.subplots(1, 2, figsize=(24, 9))
f.tight_layout()
ax1.imshow(img)
ax1.set_title('Original Image', fontsize=50)
ax2.imshow(undist)
ax2.set_title('Undistorted Image', fontsize=50)
plt.subplots_adjust(left=0., right=1, top=0.9, bottom=0.)

007_extrinsic_calibration.txt

coventions:

the camera z-axis is perpendicular to camera plane

the optic center is the z-axis intersection with the plane

the origin of the projection plane is the optic center

the focal length is z_c

z_c * [u, v, 1]^T = K *[x, y, z]^T

the right hand side coordinate is the object coordinate in world frame

the right hand side is the projection coordinate in camera plane

z_c is the optic center coordinate

008_voxel_grid_downsampling.txt

terms:

pixel: picture element
voxel: volumn element

downsampling: make a lower resolution pcl

leaf size is the size the voxel, smaller the leaf size, higher the resolution

009_voxel_grid_downsampling.py

# Create a VoxelGrid filter object for our input point cloud
vox = cloud.make_voxel_grid_filter()

# Choose a voxel (also known as leaf) size
# Note: this (1) is a poor choice of leaf size   
# Experiment and find the appropriate size!
LEAF_SIZE = 0.01

# Set the voxel (or leaf) size  
vox.set_leaf_size(LEAF_SIZE, LEAF_SIZE, LEAF_SIZE)

# Call the filter function to obtain the resultant downsampled point cloud
cloud_filtered = vox.filter()
filename = 'voxel_downsampled.pcd'
pcl.save(cloud_filtered, filename)

010_pass_through_filtering.py

# crop the point cloud along dimensions
# retain only the interesting part of the pcl

# PassThrough filter
# Create a PassThrough filter object.
passthrough = cloud_filtered.make_passthrough_filter()

# Assign axis and range to the passthrough filter object.
filter_axis = 'z'
passthrough.set_filter_field_name(filter_axis)
axis_min = 6
axis_max = 1.1
passthrough.set_filter_limits(axis_min, axis_max)

# Finally use the filter function to obtain the resultant point cloud. 
cloud_filtered = passthrough.filter()
filename = 'pass_through_filtered.pcd'
pcl.save(cloud_filtered, filename)

011_ransac_algorithm.txt

ransac: random sample consensus

algorithm overview:
the data in dateset consists of inliers and outliers
a prior geometric shape is assumed to do segmentation (by a set of equation)

algorithm implementation:

hypothesis: 
a hypothetical shape is generated by randomly selecting a minimal subset of n points
model parameter (the equation parameter) is estimated using the points

verification:
the remaining points are tested against the resulting candiate shape
partition the points into inliers and outliers

reestimate:
inliers are used to reestimate the model parameter

repeat until the inlier set is significant enough

012_ransac.py

# Create the segmentation object
seg = cloud_filtered.make_segmenter()

# Set the model you wish to fit 
seg.set_model_type(pcl.SACMODEL_PLANE)
seg.set_method_type(pcl.SAC_RANSAC)

# Max distance for a point to be considered fitting the model
# Experiment with different values for max_distance 
# for segmenting the table
max_distance = 1
seg.set_distance_threshold(max_distance)

# Call the segment function to obtain set of inlier indices and model coefficients
inliers, coefficients = seg.segment()

extracted_inliers = cloud_filtered.extract(inliers, negative=False)
filename = 'extracted_inliers.pcd'
pcl.save(extracted_inliers, filename)

extracted_outliers = cloud_filtered.extract(inliers, negative=True)
filename = 'extracted_outliers.pcd'
pcl.save(extracted_outliers, filename)

013_outlier_removal.py

#particle noise due to: dust, humidity in air, presence of multiple light source

#assuming a gaussian distribution or apply a gaussian filter to the cloud (the voxel should near its neighbour average)

# Much like the previous filters, we start by creating a filter object: 
outlier_filter = cloud_filtered.make_statistical_outlier_filter()

# Set the number of neighboring points to analyze for any given point
outlier_filter.set_mean_k(50)

# Set threshold scale factor
x = 1.0

# Any point with a mean distance larger than global (mean distance+x*std_dev) will be considered outlier
outlier_filter.set_std_dev_mul_thresh(x)

# Finally call the filter function for magic
cloud_filtered = outlier_filter.filter()

014_DBSCAN_algorithm.txt

DBSCAN: Density-Based Spatial Clustering of Application with Noise

Assummes: known distance boundary but not number of clusters

Also named as: Euclidean Clustering

two hyper parameters: maximum distance within cluster, minimum number of points in a cluster

algorithm:

a point p is a core point if there are minPts within it's epsilon-neighbour

every point within its neighbour is said to be directly reachable from p

q is reachable from p iff there is a path p p1 ... pn q such that: every pi+1 is directly reachable from pi

points not reachable from any other points are outliers

indications:

no points are reachable from non-core point regardless of distance

two points p and q are density-connected if there is a point o such that both p and q are density-reachable from o

the connectness is symmetric

a cluster:

all points within the cluster are mutually density-conncted
a point is density reachable from any point of cluster is part of the cluster

the cluster identity of core point is unique, for non-core-point it is ambiguous