programming-assignment-3 (5).pdf

8/10/2019 programming-assignment-3 (5).pdf

1/10

Programming Assignment 3 - Report

Kurt Lee - [email protected]

Calvin Zhao - [email protected]

November 20, 2014

1


2/10

1 Introduction

This programming assignment, PA3, is the first half of a two part programmingassignment 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 forregistration. This time, we will approach only the point matching part of the algorithm.

2 Mathematical Approach

For this assignment we are tasked with writing code that will find the closest pointpairs for ICP. While the simplest approach would be to conduct a linear search of alltriangles in the list, we will aim a little higher in our implementation. We will in-stead use a method of bounding boxes: bounding boxes around each triangle areconstructed, with the aim of reducing the number of triangles/points that must be com-

pletely checked. The strength here is that we may identify those triangles that do nothave to be thoroughly checked, thus increasing computational efficiency.

The elimination of irrelevant triangles is accomplished by dynamically establishinga bound defined initially as infinity and then, as the algorithm progresses, the currentlowest found distance. This bound is used to construct the bounding boxes. The resulthere is that initially, every point is considered (because the box boundaries stretch outinfinitely). The boxes will then shrink, and those boxes that do not include the point ofinterest (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 atriangle to our given point. The closest point con a triangle with vertices p, q,r to thegiven pointa 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 givena. We can find a least squares solution for and from the following system:

a p (q p) +(r p) (2)

We will express this in b= xA form and multiply both sides by AT:

a p

q p

r p

(3)

q r p r p)

a p)

q pr p

(q p)

2

(q p)(r p)(r p)(q p) (r p)2

(4) (a p)(q p) (a p)(r p)

(q p)2 (q p)(r p)(r p)(q p) (r p)2)

1

(5)

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

2


3/10

, 0

+ 1

For other and , the resultant vector c would be a point outside of the triangle.This is intuitive, as ifandcannot combine to be greater than one, or else the resultantvector will be large and cannot point to a space within the bounds of the triangle. Thissituation corresponds to a closest point that shouldexist on an edge or vertex of thetriangle. To yield the correct closest point,cmust be projected onto the correspondingedge/vertex:

= (c p) (q p)

(q p) (q p) (6)

Then the correctc (projected properly onto the triangle) becomes

c= p +(q p) (7)

Where, if 0<


4/10

(a) If so, we find the distance to the closest triangle with FindClosestPoint andset 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, boundwill represent the distance to the closest triangle, and the current closest pointwill be the actual closest point. We will go back and repeat the entire process forall 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 thethree vertices of a triangle. The output is the closest point on the triangle, includingvertices and edges, to the reference point. The main algorithm of the bounding boxsearch is implemented in the function described in the above subsection, so what followswill simply be a short description of what our code does to find the closest point. Theequations 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 thesevalues satisfy

0 && 0 && (+) 1

thencis the closest point on the triangle and is returned.

If any of those constraints are not met however, we must call ProjectOnSegment togive the correct c. ProjectOnSegment takes the improperc to be corrected, and therelevant vertices depending on which constraint is violated. If < 0 it takes r and p,if


5/10

posts FA,k and FB,k of the rigid bodies with respect to the tracker, then computerdk = F

1

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 maindriver will plot the CT mesh with the original points and the found closest points.

5 Verification

Verification was done both graphically and through test cases. The file test.m con-tains all test cases. Test cases are outlined here:

ProjectOnSegment/FindClosestPoint/magnitude:Test methods using 3 data sets of manually calculated values using the algorithmsoutlined 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 expectedoutput. The test cases extracts the dk from the given output file and uses thatto compute the closest point. It then compares the output of each method withthe expected output. If the maximum error is less than .05, then the function isdeemed valid.

If there is no ouptut from test.m, all cases pass. All errors are in the form of printedstrings, specifiying which function failed and which test case failed.

In addition, all outputs were verified visually using the graphical representation thatthe program displays after every run. Most notably, all closest points (colored red) mustlie on the mesh and be close to each appropiate point cloud (magenta boxes). This canbe seen in Figure 1. While this test is not rigorous, it does test for major flaws andshows that the program more or less does that is expected.

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

5


6/10

6 Discussion of Results

One of the ma jor hurdles with performing ICP is optimization. Ensuring that theeach closest point search is efficient is therefore paramount. In this assignment, we im-plemented a boudning box algorithm to minimize computation. We compared this witha linear search. Our linear search algorithm searches through every triangle, computesFindClosestPoint for each triangle, and takes the lowest value as the closest point. Itdoes 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. Byreducing the number of FindClosestPoint calls from 47040 to 3500, BoundBox is ableto save a considerable amount of time. While a reduction of 12 seconds may not seemlarge, time quickly adds up as more points are computed and more iterations are per-formed. In this case, only 16 closest points were computed over 1 iteration, which is notrepresentative 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. Howeverfurther optimization can be done by building a data structure to organize the Boud-ingBoxes. This can be done through a KD tree or map. Creating more optimal datastructures would reduce the number of triangles that must be checked. With the current

6


7/10

implementation, every triangle and its bounding box is computed for a point, even whenthe 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 importantwhen the full ICP is implemented as the time saved has a compounding effect. We willconsider 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 theclosest points is fairly precise. We believe that the observed error (it is very small) is

attributed to the error in dk, as our calculations for ck should be exact. We solve theleast squares for and exactly, so the finding of the closest points should involve noerror. It follows that the error we observe in cis propagated from the error in dk.The error in dk can be traced to the approximate nature of the quaternion method forpoint registration. Furthermore, in some of the data sets, B, which should be rigidlyscrewed in to the bone, is seen to shift from frame to frame. This is also responsible forsome of the observed error and suggests distortion in the given data.

In addition, error between computed and expected can be magnified in certain situa-tions from the locations of the given dk. For instance, if dk lies in a valley with multiple

close mesh points that are spacially far apart, slight deviations when computing dk may

7


8/10

cause the findClosestPoint algorithm to find a point distant to the expected. While thedistance fromck to dk may still be very similar to the expected, the actual closest point

may be far due to small deviations in dk. Similarly, error may be minimized in certainsituations. If dk lies at the top of a peak, even relatively large deviations in dk will give

you similar closest points.

7 Summary of Functions and Files

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

7.1 New functionsc = FindClosestPoint( a,p,q,r)

Takes a point, and the three vertices of a triangle as input, and outputs c, theclosest 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 ona 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. Theboxes limit the amount of FindClosestPoint calculations by defining conditions thatmust first be satisfied.

c = linearSearch( d, V, i triang )A non-integral function that simply allows us to compare our bounding box searchwith a linear search to gauge efficiency.

pa3The 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 Rand position p.

R, p = pointRegistration(input, output)Accepts two point sets input and output as function input and returns the associatedtransformation as a rotation matrix R and translation vector p. Before the mainbody of the program, we calculate the average/center of each point set asa and b.

R, p = frameInverse(R 1, p 1)Inverts given frameR1, p1 to R

1

1 andp= R1p1.

R, p = frameTransform(R 1, p 1, R 2, p 2) Transforms frames throughF1 = [R1, p1] andF2 =[R2, p2] to one frame transformation, R and p

8


9/10

Assignment Roles

While both of us were involved in all aspects of the assignment, at times we individ-ually focused on different areas:

Calvin: Function writing/programming, result verification and discussion, programdocumentation

Kurt: Report, project documentation, mathematical background/context, closestpoint implementation

9


10/10

Works Consulted

A. Segal, D. Haehnel, S. Thrun. Stanford University. Generalized-ICP. Accessed11/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

programming-assignment-3 (5).pdf

Documents