[2% bonus] Kinematic Chain Optimization

[taiya]

Up to 3 submissions will be accepted. Post a snapshot showing several iterations of the algorithm and your code here: https://classroom.github.com/assignment-invitations/6149ec28d392c794b2c98a6c87104963

Extending the material presented in the "IK-ICP" develop an algorithm that performs the alignment of the blue segment structure to the yellow point set (yes, you'll have to create this data on your own). Implement the Point-to-Point ICP as well as the Point-to-Plane variant, and then evaluate the respective convergence speed.

Some helpful information can be found in this paper: https://www.math.ucsd.edu/~sbuss/ResearchWeb/ikmethods/iksurvey.pdf (remember this is a bonus, so you are expected to work independently)

[xuzheng0927]

Are the segments lines or can be curves? Are they simply four points or more points?

[taiya]

Lines is fine. You can think of them as a collection of points sampling densely each segment.

[xuzheng0927]

I don't have very many clues. I simply match the first segment, then apply the first rotation/shift to the rest two, and then match the second segment, apply the second rotation/shift to the third one, then do the matching on the last segment. Before entering the next iteration I shift the three segmentations towards each other so they can get connected. Not sure at all whether this makes sense or not (at least I tried).

From the output it seems both point-to-point and point-to-plane reach convergence. Point-to-plane is slower (around 10 iterations), while point-to-point only takes 2 iterations (is that normal?).

By the way I forgot how to flip the y axis. Sorry about that.

(original) (Point-to-point 2nd iteration) (Point-to-point 50th iteration) (Point-to-plane 2nd iteration) (Point-to-plane 4th iteration) (Point-to-plane 10th iteration)

[drebain]

My implementation solves the problem by minimizing the point to point/plane distance as a function of the three joint rotations. Translation of segments is handled by treating the rotations as a hierarchy.

My point-to-plane implementation seems to converge more slowly than my point-to-point one, so I think I may be expressing the energy in it incorrectly.

Iteration 1:

Iteration 2:

Iteration 4:

Iteration 8:

[taiya]

@xuzheng0927 your solution is not correct, you have to treat it as a single large optimization problem Ax=b. Your "x" contains three rotations: the ones with respect to the first, second and third joint.

[taiya]

@drebain Almost there, but that's not what I am asking for convergence speed :) You have the ground truth alignment, so you can measure how far you are from the optimal solution (norm of theta - theta^*). You have to plot how fast this residual changes as the number of iterations increase. See this example:

(note: repeat the experiment with multiple initializations, compare and then average the results)

[taiya]

@xuzheng0927 @drebain note I've added a repo for the code submission as I am accepting up to three bonus takers, see the first post.

[taiya]

Reading this paper might also help: https://www.math.ucsd.edu/~sbuss/ResearchWeb/ikmethods/iksurvey.pdf

[taiya]

note: submissions to this bonus question will not be accepted beyond the end of this week.