Programming Assignment 3 - Report

Kurt Lee - 5.12klee@gmail.com

Calvin Zhao - cczhao2@gmail.com
November 20, 2014

Introduction

This programming assignment, PA3, is the first half of a two part programming
assignment of sorts (the second half being PA4). Over the course of both assignments,
we will implement our own version of the iterative closest point (ICP) algorithm for
registration. This time, we will approach only the point matching part of the algorithm.

Mathematical Approach

For this assignment we are tasked with writing code that will find the closest point
pairs for ICP. While the simplest approach would be to conduct a linear search of all
triangles in the list, we will aim a little higher in our implementation. We will instead use a method of bounding boxes: bounding boxes around each triangle are
constructed, with the aim of reducing the number of triangles/points that must be completely checked. The strength here is that we may identify those triangles that do not
have to be thoroughly checked, thus increasing computational efficiency.
The elimination of irrelevant triangles is accomplished by dynamically establishing
a bound defined initially as infinity and then, as the algorithm progresses, the current
lowest found distance. This bound is used to construct the bounding boxes. The result
here is that initially, every point is considered (because the box boundaries stretch out
infinitely). The boxes will then shrink, and those boxes that do not include the point of
interest (that which we are trying to find the closest triangle to) are not considered.
In order for this method to work, we need a way of finding the closest point on a
triangle to our given point. The closest point ~c on a triangle with vertices p~, ~q, ~r to the
given point ~a can be found some linear combination of two sides of the triangle:
~c = p~ + (~q p~) + (~r p~)

(1)

To find the correct linear combination, we must find the correct values for and given
~a. We can find a least squares solution for and from the following system:
~a p~ (~q p~) + (~r p~)
We will express this in b = xA form and multiply both sides by AT :



~q p~
~a p~
~r p~
~ p ~r p~)
~q r

(~a p~)(~q p~)

(2)

(3)


(~q p~)2
(~q p~)(~r p~)
a p)
(~r p~)(~q p~)
(~r p~)2
(4)

1
2




(~q p~)
(~q p~)(~r p~)
(~a p~)(~r p~)

2
(~r p~)(~q p~)
(~r p~) )
(5)

~q p~
~r p~



With and explicitly calculated (the inverse was calculated the standard way of
rearranging the elements and dividing by the determinant, NOT pseudoinverse), we can
return to plug into equation (1) to find the closest point ~c. However, this will only yield
a valid point on the triangle if and satisfy the following constraints:

, 0
+1
For other and , the resultant vector ~c would be a point outside of the triangle.
This is intuitive, as if and cannot combine to be greater than one, or else the resultant
vector will be large and cannot point to a space within the bounds of the triangle. This
situation corresponds to a closest point that should exist on an edge or vertex of the
triangle. To yield the correct closest point, ~c must be projected onto the corresponding
edge/vertex:
=

(~c p~) (~q p~)

(~q p~) (~q p~)

(6)

Then the correct ~c (projected properly onto the triangle) becomes

~c = p + (~q p~)

(7)

Where, if 0 < < 1 then = , if 1 then = 1, and if 0 then = 0.

Description of Algorithms

In this section, we will attempt to describe the algorithmic steps in our program. The
two main parts we will discuss our two main functions for this program: our bounding
box and finding closest point functions.

3.1

Bounding boxes

[ c ] = boundingBoxSearch( d, V, i triang )
While the method of bounding boxes was previously described in the above section,
we will go into detail regarding the algorithm and our specific implementation here. The
implementation of our bounding box search is contained within our MATLAB function
boundingBoxSearch. Our function accepts as input d, our reference points (used to find
the closest points), V, the indices of the vertices of the mesh triangles corresponding
to i triang, and i triang, the locations of the vertices in CT coordinate system. The
function searches through the given mesh for the closest point using bounding boxes.
The boxes limit the amount of FindClosestPoint calculations by defining conditions that
must first be satisfied.
1. We begin by establishing a general bound on distance from the point to the
triangle. In the mathematical discussion, we say that this is initially set to infinity.
However, we here set it to the distance between our point and the very first triangle
we encounter. This is an alternative but equally valid starting point.
2. We define lower and upper bounds L and U (note: not bound) for the current
triangle: these bounds correspond to a box defined by the vertices of the triangle.
3. Then, we check if the reference point is within the greater box defined by L bound and U + bound.

(a) If so, we find the distance to the closest triangle with FindClosestPoint and
set it as the new bound if it is smaller than the current bound. The
point returned by FindClosestPoint is set as the current closest point.
(b) If not, move on to the next triangle and continue at step 2.
4. After going through (but not necessarily checking closely) every triangle, bound
will represent the distance to the closest triangle, and the current closest point
will be the actual closest point. We will go back and repeat the entire process for
all reference points to be evaluated.

3.2

Finding the closest point

[ c ] = FindClosestPoint( a,p,q,r)
[ cStar ] = ProjectOnSegment( c,p,q )
This is a fairly straightforward function that accepts the reference point, and the
three vertices of a triangle. The output is the closest point on the triangle, including
vertices and edges, to the reference point. The main algorithm of the bounding box
search is implemented in the function described in the above subsection, so what follows
will simply be a short description of what our code does to find the closest point. The
equations evolved in this function are discussed in the Mathematical Approach section:
specifically (1), (5), (6), and (7).
The function explicitly calculates the values of and with equation (5). If these
values satisfy
0 && 0 && ( + ) 1
then ~c is the closest point on the triangle and is returned.
If any of those constraints are not met however, we must call ProjectOnSegment to
give the correct ~c. ProjectOnSegment takes the improper ~c to be corrected, and the
relevant vertices depending on which constraint is violated. If < 0 it takes r and p,
if < 0 it takes p and q, and otherwise takes q and r. ProjectOnSegment then simply
applies equations (6) and (7), and outputs the correct closest point ~c.

Program Overview

The goal of this assignment is to write code for the matching part of the ICP algorithm. Our MATLAB program centers around our main driver pa3.m. In this central
driver we put to work the funcitons we have developed to implement our bounding box
approach to closest point matching. Along with all of our old functions that are essential to working with point clouds (eg. pointRegistration, pointTransform etc.) we
have developed several new functions. The foremost of these are boundingBoxSearch,
FindClosestPoint, and ProjectOnSegment. A complete listing of functions can be found
listed in section 7 of this report.
Importantly, our main driver pa3.m reads in the input and parses them into a format
friendly to our functions. Most of the functionality of our program has been separated
into our other functions, so the main responsibility of pa3.m is to parse input and produce
~ we will use our
a formatted output file. However, since we are not explicitly given d,
~
point registration and frame transformation methods to find the dk s. The strategy for
this is given in the homework assignment: we (with point registration) determine the

posts FA,k and FB,k of the rigid bodies with respect to the tracker, then computer
~
~
d~k = F1
B,k FA,k Atip . We can now pass d to the other functions, which have been
previously discussed in this report. These return the found closest points, and the main
driver will plot the CT mesh with the original points and the found closest points.

Verification

Verification was done both graphically and through test cases. The file test.m contains all test cases. Test cases are outlined here:
ProjectOnSegment/FindClosestPoint/magnitude:
Test methods using 3 data sets of manually calculated values using the algorithms
outlined above. If the function output is within .001, it is deemed valid.
linearSearch/boundingBoxSearch:
Test methods using the given dataset PA3-C-Debug. Also extracts the expected
output. The test cases extracts the dk from the given output file and uses that
to compute the closest point. It then compares the output of each method with
the expected output. If the maximum error is less than .05, then the function is
deemed valid.
If there is no ouptut from test.m, all cases pass. All errors are in the form of printed
strings, specifiying which function failed and which test case failed.
In addition, all outputs were verified visually using the graphical representation that
the program displays after every run. Most notably, all closest points (colored red) must
lie on the mesh and be close to each appropiate point cloud (magenta boxes). This can
be seen in Figure 1. While this test is not rigorous, it does test for major flaws and
shows that the program more or less does that is expected.

Figure 1: Sample output image after running data set PA3-E.

5

Discussion of Results

One of the major hurdles with performing ICP is optimization. Ensuring that the
each closest point search is efficient is therefore paramount. In this assignment, we implemented a boudning box algorithm to minimize computation. We compared this with
a linear search. Our linear search algorithm searches through every triangle, computes
FindClosestPoint for each triangle, and takes the lowest value as the closest point. It
does this for each vertex given.
This method was used as a baseline for evaluation of the bounding box algorithm.
As seen in Figure 2, BoundingBox had a 169% improvement over LinearSearch. By
reducing the number of FindClosestPoint calls from 47040 to 3500, BoundBox is able
to save a considerable amount of time. While a reduction of 12 seconds may not seem
large, time quickly adds up as more points are computed and more iterations are performed. In this case, only 16 closest points were computed over 1 iteration, which is not
representative of real world applications.

Figure 2: Profiler of Linear Search (top) and Bounding Box (bottom)

We see that there are signifigant advantages to using boundingBoxSearch. However
further optimization can be done by building a data structure to organize the BoudingBoxes. This can be done through a KD tree or map. Creating more optimal data
structures would reduce the number of triangles that must be checked. With the current

implementation, every triangle and its bounding box is computed for a point, even when
the given point failed to lie in the bounding box in previous iterations. Reducing the
number of triangles considered would further reduce time.
This is most important
when the full ICP is implemented as the time saved has a compounding effect. We will
consider these more advanced optimization strageties in the future.

Figure 3: A table showing the differences in our calculated output the given output.

Figure 4: A histogram showing the error given in Figure 3.

The low error as shown in figures 3 and 4 suggests that our method for finding the
closest points is fairly precise. We believe that the observed error (it is very small) is
attributed to the error in d~k , as our calculations for ~ck should be exact. We solve the
least squares for and exactly, so the finding of the closest points should involve no
error. It follows that the error we observe in ~c is propagated from the error in d~k .
The error in d~k can be traced to the approximate nature of the quaternion method for
point registration. Furthermore, in some of the data sets, B, which should be rigidly
screwed in to the bone, is seen to shift from frame to frame. This is also responsible for
some of the observed error and suggests distortion in the given data.
In addition, error between computed and expected can be magnified in certain situations from the locations of the given d~k . For instance, if d~k lies in a valley with multiple
close mesh points that are spacially far apart, slight deviations when computing d~k may
7

cause the findClosestPoint algorithm to find a point distant to the expected. While the
distance from ~ck to d~k may still be very similar to the expected, the actual closest point
may be far due to small deviations in d~k . Similarly, error may be minimized in certain
situations. If d~k lies at the top of a peak, even relatively large deviations in d~k will give
you similar closest points.

Summary of Functions and Files

A comprehensive listing of source files can be found in the README within the
PROGRAM directory of our submission. A listing of functions implemented for our
program is compiled below (private parsing fxns not included here but listed in submitted README).

7.1

New functions

c = FindClosestPoint( a,p,q,r)
Takes a point, and the three vertices of a triangle as input, and outputs c, the
closest point on the triangle to the given point.
cStar = ProjectOnSegment( c,p,q )
A supporting function called by FindClosestPoint - if the closest point would be on
a vertex or edge of the triangle, this function projects c onto the segment properly.
mag = magnitude(v)
Very simply finds the magnitude of a given vector v.
c = boundingBoxSearch( d, V, i triang )
Searches through the given mesh for the closest point using bounding boxes. The
boxes limit the amount of FindClosestPoint calculations by defining conditions that
must first be satisfied.
c = linearSearch( d, V, i triang )
A non-integral function that simply allows us to compare our bounding box search
with a linear search to gauge efficiency.
pa3
The main driver for our program, calls the other functions to accomplish our task.

7.2

Old functions (from PA1,2

output args = pointTransform(R, p, v)

Transforms given matrix of points v with transformation given by rotation R
and position p.
R, p = pointRegistration(input, output)
Accepts two point sets input and output as function input and returns the associated
transformation as a rotation matrix R and translation vector p. Before the main
body of the program, we calculate the average/center of each point set as a
and b.
R, p = frameInverse(R 1, p 1)
Inverts given frame R1 , p1 to R11 and p = R1 p1 .
R, p = frameTransform(R 1, p 1, R 2, p 2) Transforms frames through F1 = [R1 , p1 ] and F2 =
[R2 , p2 ] to one frame transformation, R and p
8

Assignment Roles
While both of us were involved in all aspects of the assignment, at times we individually focused on different areas:
Calvin: Function writing/programming, result verification and discussion, program
documentation
Kurt: Report, project documentation, mathematical background/context, closest
point implementation

Works Consulted
A. Segal, D. Haehnel, S. Thrun. Stanford University. Generalized-ICP. Accessed
11/18/2014 http://www.robots.ox.ac.uk/ avsegal/resources/papers/Generalized ICP.pdf
D. Eberly, Distance Between Point and Triangle in 3D. 9/28/1999.
And all of Professor Taylors posted class notes on the course webpage.

10