diff --git a/py_bind/optv/orientation.pxd b/py_bind/optv/orientation.pxd index 9f696152..c15efb58 100644 --- a/py_bind/optv/orientation.pxd +++ b/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) diff --git a/py_bind/optv/orientation.pyx b/py_bind/optv/orientation.pyx index 6369fbfa..e39b2bb8 100644 --- a/py_bind/optv/orientation.pyx +++ b/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 = calloc(num_pts, sizeof(vec2d*)) + + for pt in range(num_pts): + targ = targets[pt] + ctargets[pt] = (targ.data) + + return weighted_dumbbell_precision(ctargets, num_pts, num_cams, + cparam._control_par.mm, calib, db_length, db_weight) diff --git a/py_bind/test/test_orientation.py b/py_bind/test/test_orientation.py index 5205c812..c87208d5 100644 --- a/py_bind/test/test_orientation.py +++ b/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 diff --git a/py_bind/test/testing_fodder/calibration/sym_cam4.tif.ori b/py_bind/test/testing_fodder/calibration/sym_cam4.tif.ori index 31c1452c..fbf0baa9 100644 --- a/py_bind/test/testing_fodder/calibration/sym_cam4.tif.ori +++ b/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 diff --git a/py_bind/test/testing_fodder/dumbbell/cam1.tif.ori b/py_bind/test/testing_fodder/dumbbell/cam1.tif.ori new file mode 100644 index 00000000..3e3b7397 --- /dev/null +++ b/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 diff --git a/py_bind/test/testing_fodder/dumbbell/cam2.tif.ori b/py_bind/test/testing_fodder/dumbbell/cam2.tif.ori new file mode 100644 index 00000000..00fddce6 --- /dev/null +++ b/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 diff --git a/py_bind/test/testing_fodder/dumbbell/cam3.tif.ori b/py_bind/test/testing_fodder/dumbbell/cam3.tif.ori new file mode 100644 index 00000000..401ea153 --- /dev/null +++ b/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 diff --git a/py_bind/test/testing_fodder/dumbbell/cam4.tif.ori b/py_bind/test/testing_fodder/dumbbell/cam4.tif.ori new file mode 100644 index 00000000..2577aecd --- /dev/null +++ b/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