*Due Monday 10/2 at 11:59 pm. You may work individually.*

In this assignment, you are going to implement a simple version of the Iterative Closest Point algorithm in 2D.

Start with the base Matlab code. Run the code by typing the following in the Matlab prompt:

`>> driver(0)`

The target curve is drawn in green, and the source curve is drawn in red. Since the base code does not yet implement ICP, the source curve is hidden behind the blue curve. Once ICP is implemented, the result should look something like this:

The small dots are the intermediate points during the search. You don't need to plot these in your implementation.

The driver creates the source and target curve points, which are both stored as 2xn matrices. So, to access the ith point in `sourcePts`

, you need to specify that you want *all* the rows (specified by `:`

) of the ith column:

`sourcePts(:,i)`

The resulting expression is a 2D column vector. To plot the points (see lines 33-37), say something like

`plot(sourcePts(1,:),sourcePts(2,:),'r-');`

which draws a red line with circles as markers and dotted lines connecting the markers. `sourcePts(1,:)`

gets the 1st row of `sourcePts`

, which corresponds to the X values of the source points. Again, the colon specifies that you want *all* the columns of the matrix. You can specify the line style by modifying the last arguments. Here, `'r-'`

draws a red line through the points.

The `generatePts`

function generates the test data points. In a later task, you will be asked to add a new function in this file to generate more data.

The `perturbPts`

function takes as input the data points and then adds random noise to the points, and then transforms (rotates and translates) the points. The goal of the ICP algorithm is to recover these rotation and translation parameters solely based on the perturbed source points and the target points.

The `transformPts`

function takes the transformation parameters (stores as a 3D vector, see below) and transforms the points. Note that the actual transformation code is vectorized (line 9). The same thing can be accomplished by using a for-loop and iterating through all the points (`source(:,i)`

), but Matlab runs much faster when you vectorize your code. For this assignment, do not worry about vectorizing. It is more important to have clean, readable code.

The pseudocode for ICP is as follows:

*Select*e.g. 1000 random sample points from the source.*Match*each sample to the closest point on the target.*Reject*pairs with distance > k times the median distance.*Optimize*for the best \theta, x, y to be applied to the source so that the pair distances are minimized.

The steps above are repeated until convergence or until the maximum number of iterations is reached.

For this task, ignore steps 1 and 3. Since I have already written the necessary function for step 4, the only things that need to be done are step 2 and the convergence check. Take a look again at `icp2d.m`

. This function takes as input the source and target points, and computes as output the appropriate values of \theta, x, y. The main iteration loop is at the end of the function:

```
% Current theta, x, and y, stored as a column vector:
% thetaXY(1) is theta
% thetaXY(2) is x
% thetaXY(3) is y
thetaXY = [0 0 0]';
for iter = 1 : iterMax
% Do something here
end
```

For this task, the ICP steps 2 and 4 need to go inside this loop.

The very first thing inside the loop should be to compute the new transformed points using the current values of \theta, x, y. To do this, add the following inside the loop:

`sourcePts1 = transformPts(thetaXY,sourcePts);`

Set `iterMax`

in `driver.m`

to `1`

for now. Try hard-coding various values for \theta, x, y by modifying the `thetaXY`

variable and plotting the resulting transformed points. The goal of the ICP algorithm is to find these values automatically.

To complete step 2, you need to first understand what is required for step 4, which is outlined below.

For step 4, we are going to use Matlab's powerful `fminunc`

*optimization* function. I have already written the objective function (`distFun`

), so the only thing you need to do is to call

`thetaXY1 = fminunc(@(thetaXY)distFun(thetaXY,sourcePts,targetPts,closest),thetaXY,opt);`

The output, `thetaXY1`

, is the (locally) optimal \theta, x, y computed by `fminunc`

. The second-to-last argument, `thetaXY`

, is the initial guess for `fminunc`

, for speeding up the search. You should use the \theta, x, y values from the previous iteration as this initial guess.

The `closest`

argument stores which target points are closest to each of the source points. This needs to be computed in the *matching* step.

- The length of the
`closest`

vector should be number of points in the source. - The ith element of
`closest`

stores the index of the closest target point from the ith source point.

For example if `closest = [4 5 4 2 ...]`

, then the first source point's closest point is `targetPts(:,4)`

, the second source point's closest point is `targetPts(:,5)`

, and so on.

To test for convergence, check whether the newly computed values of \theta, x, y are close to the previous values of \theta, x, y. A simple L2 norm will suffice. I.e., check if \sqrt{\Delta \theta^2 + \Delta x^2 + \Delta y^2} is less than `thresh`

.

If the initial guess is too far, then ICP may not converge to a good local minimum. Modify the random perturbation factors (lines 14-17 in `driver.m`

) until you get something that behaves well.

```
theta = randBetween(-30.0,30.0)*pi/180.0;
x = randBetween(-1.0,1.0);
y = randBetween(-1.0,1.0);
noise = 0.01;
```

The argument to the driver function controls the generated curves. `driver(0)`

produces sine curves, and `driver(1)`

produces Lissajous curves. Here is the matched Lissajous curves after ICP is implemented.

Create at least one more curve type in `generatePts`

.

Implement steps 1 and 3 from the pseudocode above.

Step 1 makes ICP scalable, which is especially important for 3D surfaces. Since we're doing 2D curves, the speed might not change much for us here.

Step 3 allows ICP to get partial matches. Use the `medianMult`

argument as the k in the pseudocode above. `medianMult=inf`

implies that no pairs will be rejected, which means that the behavior should default back to the previous version from Task 1. This value depends on the type of the curve. For the sine curve (`caseNumber=0`

), with `medianMult=2`

, you should be able to get something like this:

- For a grade of B, you must finish Tasks 1 and 2, and submit a report (see below).
- For a grade of A, you must also finish Task 3.

Please provide a report with your submission (PDF). The report should include the following:

- Your name
- For each of the figures, provide the parameters (e.g.,
`noise`

,`thresh`

,`iterMax`

, etc.) for reproducing the figure. - Figures from the sine curve, Lissajous curve, and at least one more curve of your own design.
- For each curve above, show one case where ICP worked and one case where it didn't work. Explain why ICP didn't work.
- If you did Task 3, show a partial match as in the figure above. What happens when
`medianMult`

is too big or too small? What happens if the number of samples is too small? - Do you have any comments about this assignment that you would like to share?

Create a zip file named NETID.zip (your email ID; e.g., `sueda.zip`

) so that when you extract it, it creates a folder called NETID/ (e.g., `sueda/`

) that has (ONLY) the following in it:

- Your PDF report
- Your Matlab files

Generated on Thu Sep 14 12:17:28 CDT 2017