Add kinematic optimization using 6-DOF pose observations

[schornakj]

Depends on #72 -- needs to be revised after that one merges.

Add a cost function and optimization that uses the KinematicMeasurement class.

[schornakj]

Here are printouts from the test cases:

[ RUN      ] DHChainMeasurementTest_PerturbedDH.PerfectGuessPerturbedDH
iter      cost      cost_change  |gradient|   |step|    tr_ratio  tr_radius  ls_iter  iter_time  total_time
   0  1.013305e-02    0.00e+00    3.03e+00   0.00e+00   0.00e+00  1.00e+04        0    1.18e-02    1.20e-02
   1  9.232055e-03    9.01e-04    6.24e-04   2.03e-03   1.00e+00  3.00e+04        1    8.90e-03    2.09e-02

Correlation Coeff. > 0.500000:
camera_mount_to_camera_x camera_mount_to_camera_ry 0.706551
camera_mount_to_camera_x camera_mount_to_camera_rz 0.602213
camera_mount_to_camera_x camera_mount_to_camera_rx -0.602179

true chain:
  0.615       0     0.1 -1.5708
      0 -1.5708   0.705       0
      0       0   0.135 -1.5708
  0.755       0       0  1.5708
      0       0       0 -1.5708
  0.085 3.14159       0       0

optimized chain:
       0.615 -8.53398e-07    0.0999999      -1.5708
-3.66592e-07     -1.57079        0.705  4.89676e-07
-3.66592e-07  2.56287e-06     0.134998      -1.5708
       0.755 -1.54474e-06 -2.23184e-07       1.5708
 2.65002e-07  1.25325e-06  2.78112e-08      -1.5708
       0.085      3.14159            0            0

Position Difference Mean: 0.00390005
Position Difference Std. Dev.: 0.00102345
Orientation Difference Mean: 0.00376762
Orientation difference Std. Dev.: 0.00114377
[       OK ] DHChainMeasurementTest_PerturbedDH.PerfectGuessPerturbedDH (53 ms)

[ RUN      ] DHChainMeasurementTest_PerturbedDH_PertubedGuess.PerturbedDHPerturbedGuess
iter      cost      cost_change  |gradient|   |step|    tr_ratio  tr_radius  ls_iter  iter_time  total_time
   0  4.122418e+00    0.00e+00    5.68e+01   0.00e+00   0.00e+00  1.00e+04        0    1.11e-02    1.12e-02
   1  5.450468e-03    4.12e+00    5.07e-01   1.22e-01   1.00e+00  3.00e+04        1    7.91e-03    1.92e-02
   2  5.009846e-03    4.41e-04    4.27e-04   1.28e-03   1.00e+00  9.00e+04        1    6.30e-03    2.55e-02

Correlation Coeff. > 0.500000:
camera_mount_to_camera_x camera_mount_to_camera_ry 0.704936
camera_mount_to_camera_x camera_mount_to_camera_rz 0.602058
camera_mount_to_camera_x camera_mount_to_camera_rx -0.601194
target_mount_to_target_y target_mount_to_target_rz 0.516534
target_mount_to_target_z target_mount_to_target_ry -0.511563

true chain:
  0.615       0     0.1 -1.5708
      0 -1.5708   0.705       0
      0       0   0.135 -1.5708
  0.755       0       0  1.5708
      0       0       0 -1.5708
  0.085 3.14159       0       0

optimized chain:
       0.615 -1.12738e-11    0.0999994      -1.5708
 1.27166e-07      -1.5708        0.705   4.7978e-07
 1.27166e-07  8.06842e-07        0.135      -1.5708
       0.755 -8.60565e-07  6.98747e-09       1.5708
 8.56416e-07 -1.29633e-06 -6.51357e-08      -1.5708
       0.085      3.14159            0            0

Position Difference Mean: 0.00236485
Position Difference Std. Dev.: 0.000899688
Orientation Difference Mean: 0.00288229
Orientation difference Std. Dev.: 0.00093552
[       OK ] DHChainMeasurementTest_PerturbedDH_PertubedGuess.PerturbedDHPerturbedGuess (56 ms)

[marip8]

Closing; this PR is revised in #79