Kinematic Optimization with 6D Pose Measurements

[marip8]

This PR supercedes #74. Notable changes include:

• Changed residual calculation to include x/y/z difference and a scaled quaternion angular difference
• Removed unit test residual error checks because of the mixed linear and rotational units in the residuals
• Added an example with data collected on a robotic part positioner

[marip8]

Results from the example

Calibration validation against measurements:

Position Difference Mean: 0.00174474
Position Difference Std. Dev.: 0.000923751
Orientation Difference Mean: 0.00323072
Orientation difference Std. Dev.: 0.00175761

Calibration validation against measurements (static DH parameters):

Position Difference Mean: 0.00382386
Position Difference Std. Dev.: 0.00144508
Orientation Difference Mean: 0.00534404
Orientation difference Std. Dev.: 0.00219113

Percent improvement: calibration vs. nominal kinematic model

Position: 54.3723%
Position Std. Dev.: 36.0761%
Orientation: 39.5453%
Orientation Std. Dev.: 19.7856%

[schornakj]

My only suggestion is to remove a TODO comment I had added. Otherwise I think it's good to go.