add nonlinear refinement

README.md

@@ -35,7 +35,7 @@ Several methods exist to solve for **X** but a vast majority of these methods as
 - Expand documentation in README
 - Add ROS capabilities.
 - Add command line interface.
-- Add post nonlinear refinement.
+- ~~Add post nonlinear refinement.~~
 - Add singular value checking.
 - Add pose selection based off of dual quaternion scalar score.
 - Add eye-on-base functionality.

src/handeye_4dof/calibrator.py

@@ -1,13 +1,8 @@
 import sympy as sy
 import numpy as np
+from scipy.optimize import minimize
 from .dual_quaternions import DualQuaternion
-from .utils import vec_to_skew_symmetric_mat
-
-
-"""
-TODO: (1) add nonlinear refinement
-      (2) add singular value checking
-"""
+from .utils import vec_to_skew_symmetric_mat, rotation_matrix_constraints, obtain_tf_from_rolled_arr
 
 
 class Calibrator4DOF:
@@ -100,3 +95,42 @@ def calibrate(self, antiparallel_screw_axes=False):
             dq_x = dq_rot2 * dq_x
 
         return dq_x
+
+    @staticmethod
+    def nonlinear_refinement(base_to_hand, camera_to_marker, calib_hand_to_camera):
+        # We only vary the first 11 parameters of the transform matrix since we cannot solve for tz.
+        W = np.eye(4)
+        W[-1, -1] = 9
+
+        def base_to_marker_error(xi):
+            base_to_marker = obtain_tf_from_rolled_arr(xi)
+            e = 0
+            for bth, ctm in zip(base_to_hand, camera_to_marker):
+                E = base_to_marker - bth.dot(calib_hand_to_camera).dot(ctm)
+                e += np.trace(E.dot(W).dot(E.T))
+            return e
+
+        # We just use an arbitrary pose as our initial guess.
+        x0 = (base_to_hand[0].dot(calib_hand_to_camera).dot(camera_to_marker[0])).ravel()[:11]
+
+        nl_base_to_marker = obtain_tf_from_rolled_arr(minimize(base_to_marker_error, x0,
+                                                               constraints=rotation_matrix_constraints()).x)
+
+        # Now that we optimized to obtain base to marker transform, we perform the optimization
+        # once more to regain the hand to camera transform (the one we care about).
+        # This could possibly be replaced with some form of transform averaging.
+
+        def hand_to_camera_error(xi):
+            hand_to_camera = obtain_tf_from_rolled_arr(xi)
+            e = 0
+            for bth, ctm in zip(base_to_hand, camera_to_marker):
+                E = hand_to_camera - np.linalg.inv(bth).dot(nl_base_to_marker).dot(np.linalg.inv(ctm))
+                e += np.trace(E.dot(W).dot(E.T))
+            return e
+
+        x0 = (np.linalg.inv(base_to_hand[0]).dot(nl_base_to_marker).dot(np.linalg.inv(camera_to_marker[0]))).ravel()[:11]
+
+        nl_hand_to_camera = obtain_tf_from_rolled_arr(minimize(hand_to_camera_error, x0,
+                                                               constraints=rotation_matrix_constraints()).x)
+
+        return nl_hand_to_camera

src/handeye_4dof/utils.py

@@ -22,3 +22,43 @@ def vec_to_skew_symmetric_mat(vec):
                     [v3, 0.0, -v1],
                     [-v2, v1, 0.0]])
     return mat
+
+
+def obtain_tf_from_rolled_arr(xi):
+    # xi := ux, vx, wx, tx, uy, vy, wy, ty, uz, vz, wz
+    # For calibration, we do not care about tz as we cannot solve for it anyways
+    tf = np.zeros(16)
+    tf[:11] = xi
+    tf[-1] = 1
+    return tf.reshape((4, 4))
+
+
+def rotation_matrix_constraints():
+    # source: https://graphics.stanford.edu/courses/cs348a-17-winter/ReaderNotes/handout17.pdf
+    def constraint1(xi):
+        ux, vx, wx, tx, uy, vy, wy, ty, uz, vz, wz = xi
+        return ux**2 + uy**2 + uz**2 - 1
+
+    def constraint2(xi):
+        ux, vx, wx, tx, uy, vy, wy, ty, uz, vz, wz = xi
+        return vx**2 + vy**2 + vz**2 - 1
+
+    def constraint3(xi):
+        ux, vx, wx, tx, uy, vy, wy, ty, uz, vz, wz = xi
+        return wx**2 + wy**2 + wz**2 - 1
+
+    def constraint4(xi):
+        ux, vx, wx, tx, uy, vy, wy, ty, uz, vz, wz = xi
+        return ux*vx + uy*vy + uz*vz
+
+    def constraint5(xi):
+        ux, vx, wx, tx, uy, vy, wy, ty, uz, vz, wz = xi
+        return ux*wx + uy*wy + uz*wz
+
+    def constraint6(xi):
+        ux, vx, wx, tx, uy, vy, wy, ty, uz, vz, wz = xi
+        return wx*vx + wy*vy + wz*vz
+
+    constraints = [constraint1, constraint2, constraint3, constraint4, constraint5, constraint6]
+
+    return [{'type': 'eq', 'fun': c} for c in constraints]

src/main.py

@@ -9,30 +9,47 @@
 
 
 def main():
+    with open("../example_data/pose_samples.pkl", "rb") as f:
+        try:
+            base_to_hand, camera_to_marker = pickle.load(f)
+        except UnicodeDecodeError:
+            # python 2 to python 3 pickle
+            base_to_hand, camera_to_marker = pickle.load(f, encoding='latin1')
+
     with open("../example_data/paired_poses.pkl", "rb") as f:
         motions = pickle.load(f)
 
-    motions = random.sample(motions, k=24)
+    # motions = random.sample(motions, k=24)
 
     cb = Calibrator4DOF(motions)
 
     # Our camera and end effector z-axes are antiparallel so we must apply a 180deg x-axis rotation.
     dq_x = cb.calibrate(antiparallel_screw_axes=True)
 
     # Calibration Obtained Hand to Camera
-    H = np.linalg.inv(dq_x.as_transform())
-    translation = H[:3, -1]
-    rot = np.rad2deg(R.from_matrix(H[:3, :3]).as_euler('xyz'))
+    ca_hand_to_camera = np.linalg.inv(dq_x.as_transform())
+
+    # Calibration Obtained with post nonlinear refinement
+    nl_hand_to_camera = cb.nonlinear_refinement(base_to_hand, camera_to_marker, ca_hand_to_camera)
+
+    ca_rotation = np.rad2deg(R.from_matrix(ca_hand_to_camera[:3, :3]).as_euler('xyz'))
+    nl_rotation = np.rad2deg(R.from_matrix(nl_hand_to_camera[:3, :3]).as_euler('xyz'))
 
     # Ground Truth Hand to Camera
     gt_translation = [-0.456, -0.037, -0.112]
     gt_rotation = [180, 0, 0]
 
-    # Note that z-translation is meant to be inaccurate.
-    print("Hand to Camera Transform Comparison")
-    print("Obtained Translation: {} | Ground Truth Translation {}".format(translation, gt_translation))
-    print("Obtained Rotation: {} | Ground Truth Rotation {}".format(rot, gt_rotation))
+    # NOTE: (1) Ground Truth itself may be inaccurate (manually measured).
+    #       (2) z-translation is an invalid number.
+    np.set_printoptions(precision=5)
+    print("Hand to Camera Transform Comparisons")
+    print("Translations: Calibration  {}".format(ca_hand_to_camera[:3, -1]))
+    print("              Nonlinear    {}".format(nl_hand_to_camera[:3, -1]))
+    print("              Ground Truth {}".format(gt_translation))
+    print("Rotations:    Calibration  {}".format(ca_rotation))
+    print("              Nonlinear    {}".format(nl_rotation))
+    print("              Ground Truth {}".format(gt_rotation))
 
 
 if __name__ == '__main__':
-    main()
+    main()