[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)