Merge pull request #143 from yosefm/dumbbell

Bind the dumbbell target function.

py_bind/optv/orientation.pxd

@@ -26,5 +26,8 @@ cdef extern from "optv/orientation.h":
     double* orient (calibration* cal_in, control_par *cpar, int nfix, 
         vec3d fix[], target pix[], orient_par *flags, double sigmabeta[20])
     orient_par* read_orient_par(char *filename)
+    double weighted_dumbbell_precision(vec2d** targets, int num_targs, 
+        int num_cams, mm_np *multimed_pars, calibration* cals[], 
+        int db_length, double db_weight)
 
 cdef calibration** cal_list2arr(list cals)

py_bind/optv/orientation.pyx

@@ -234,3 +234,35 @@ def full_calibration(Calibration cal,
     free(residuals)
     return ret, used, err_est
 
+def dumbbell_target_func(np.ndarray[ndim=3, dtype=pos_t] targets, 
+    ControlParams cparam, cals, db_length, db_weight):
+    """
+    Wrap the epipolar convergence test.
+    
+    Arguments:
+    np.ndarray[ndim=3, dtype=pos_t] targets - (num_targets, num_cams, 2) array,
+        containing the metric coordinates of each target on the image plane of
+        each camera. Cameras must be in the same order for all targets.
+    ControlParams cparam - needed for the parameters of the tank through which
+        we see the targets.
+    cals - a sequence of Calibration objects for each of the cameras, in the 
+        camera order of ``targets``.
+    db_length - distance between two dumbbell targets.
+    db_weight - weight of relative dumbbell size error in target function.
+    """
+    cdef:
+        np.ndarray[ndim=2, dtype=pos_t] targ
+        vec2d **ctargets
+        calibration **calib = cal_list2arr(cals)
+        int cam, num_cams
+
+    num_cams = targets.shape[1]
+    num_pts = targets.shape[0]
+    ctargets = <vec2d **>calloc(num_pts, sizeof(vec2d*))
+
+    for pt in range(num_pts):
+        targ = targets[pt]
+        ctargets[pt] = <vec2d *>(targ.data)
+
+    return weighted_dumbbell_precision(ctargets, num_pts, num_cams, 
+        cparam._control_par.mm, calib,  db_length, db_weight)

py_bind/test/test_orientation.py

@@ -3,9 +3,9 @@
 import unittest
 
 from optv.calibration import Calibration
-from optv.imgcoord import image_coordinates
+from optv.imgcoord import image_coordinates, flat_image_coordinates
 from optv.orientation import match_detection_to_ref, point_positions, \
-    external_calibration, full_calibration
+    external_calibration, full_calibration, dumbbell_target_func
 from optv.parameters import ControlParams
 from optv.tracking_framebuf import TargetArray
 from optv.transforms import convert_arr_metric_to_pixel
@@ -138,6 +138,54 @@ def test_point_positions(self):
                     targ_num=np.nonzero(skew_dist_jigged[1] > 1e-10)[0][0]))
         if np.any(np.linalg.norm(points - skew_dist_jigged[0], axis=1) > 0.1):
             self.fail('Rays converge on wrong position after jigging.')
+
+    def test_dumbbell(self):
+        # prepare MultimediaParams
+        mult_params = self.control.get_multimedia_params()
+        mult_params.set_n1(1.)
+        mult_params.set_layers(np.array([1.]), np.array([1.]))
+        mult_params.set_n3(1.)
+
+        # 3d point
+        points = np.array([[17.5, 42, 0],
+                           [-17.5, 42, 0]], dtype=float)
+
+        num_cams = 4
+        ori_tmpl = r'testing_fodder/dumbbell/cam{cam_num}.tif.ori'
+        add_file = r'testing_fodder/calibration/cam1.tif.addpar'
+        calibs = []
+        targs_plain = []
+
+        # read calibration for each camera from files
+        for cam in range(num_cams):
+            ori_name = ori_tmpl.format(cam_num=cam + 1)
+            new_cal = Calibration()
+            new_cal.from_file(ori_file=ori_name, add_file=add_file)
+            calibs.append(new_cal)
+
+        for cam_num, cam_cal in enumerate(calibs):
+            new_plain_targ = flat_image_coordinates(
+                points, cam_cal, self.control.get_multimedia_params())
+            targs_plain.append(new_plain_targ)
+
+        targs_plain = np.array(targs_plain).transpose(1,0,2)
+
+        # The cameras are not actually fully calibrated, so the result is not 
+        # an exact 0. The test is that changing the expected distance changes 
+        # the measure.
+        tf = dumbbell_target_func(targs_plain, self.control, calibs, 35., 0.)
+        self.assertAlmostEqual(tf, 7.14860, 5) # just a regression test
+
+        # As we check the db length, the measure increases...
+        tf_len = dumbbell_target_func(
+            targs_plain, self.control, calibs, 35., 1.)
+        self.assertTrue(tf_len > tf)
+
+        # ...but not as much as when giving the wrong length.
+        tf_too_long = dumbbell_target_func(
+            targs_plain, self.control, calibs, 25., 1.)
+        self.assertTrue(tf_too_long > tf_len > tf)
+
 
 class TestGradientDescent(unittest.TestCase):
     # Based on the C tests in liboptv/tests/check_orientation.c

py_bind/test/testing_fodder/calibration/sym_cam4.tif.ori

@@ -1,5 +1,5 @@
    -250.0     1.0   -250.0
-     0.0    -2.434	0.0
+     0.0    -2.356	0.0
 
     -0.9594955 -0.0529875  0.2766960
      0.0171315  0.9693616  0.2450401

py_bind/test/testing_fodder/dumbbell/cam1.tif.ori

@@ -0,0 +1,11 @@
+53.86516000 41.80704000 -250.30874000
+    0.17360000  2.93141000  -0.07124000
+
+    -0.9755121 -0.0696133  0.2086385
+    -0.0341633  0.9850361  0.1689281
+    -0.2172761  0.1576636 -0.9632929
+
+      0.0000   0.0000
+     80.2700
+
+       0.000100000000000    0.000010000000000   -91.500000000000000

py_bind/test/testing_fodder/dumbbell/cam2.tif.ori

@@ -0,0 +1,11 @@
+-40.59589455 40.92752826 -239.13737242
+    0.17687217  3.31216947  0.05902948
+
+    -0.9837706  0.0581390 -0.1697508
+     0.0282589  0.9844464  0.1733978
+     0.1771918  0.1657867 -0.9701123
+
+      0.0000   0.0000
+     78.4800
+
+       0.000100000000000    0.000010000000000   -91.500000000000000

py_bind/test/testing_fodder/dumbbell/cam3.tif.ori

@@ -0,0 +1,11 @@
+-70.67046965 42.87867097 282.50941802
+    -0.18216050  -0.28243198  -0.02534905
+
+     0.9600720  0.0243421 -0.2786921
+     0.0255432  0.9844183  0.1739775
+     0.2785846 -0.1741496  0.9444906
+
+      0.0000   0.0000
+     91.8500
+
+       0.000100000000000    0.000010000000000   109.500000000000000

py_bind/test/testing_fodder/dumbbell/cam4.tif.ori

@@ -0,0 +1,11 @@
+43.09110995 29.48985646 272.33956208
+    -0.12869588  0.17115349  0.01322342
+
+     0.9853028 -0.0130298  0.1703191
+    -0.0087433  0.9919324  0.1264657
+    -0.1705929 -0.1260962  0.9772399
+
+      0.0000   0.0000
+     84.7800
+
+       0.000100000000000    0.000010000000000   109.500000000000000