Merge pull request #158 from alexlib/single_cam

Single camera case

Author: alexlib

.gitignore

@@ -14,3 +14,5 @@ liboptv/build
 py_bind/build
 *~
 
+py_bind/optv/.vscode/settings.json
+py_bind/optv/.vscode/c_cpp_properties.json

liboptv/src/correspondences.c

@@ -4,14 +4,18 @@ Routine:	       	correspondences.c
 
 Author/Copyright:      	Hans-Gerd Maas
 
-Address:	      	Institute of correcteddesy and Photogrammetry
+Address:	      	Institute of Geodesy and Photogrammetry
 	      		ETH - Hoenggerberg
 	      		CH - 8093 Zurich
 
 Creation Date:	       	1988/89
 
 Description:	       	establishment of correspondences for 2/3/4 cameras
 
+Modified for OpenPTV: Yosef Meller and Alex Liberzon
+
+Date:                   2013-2018
+
 ****************************************************************************/
 
 #include <stdio.h>

liboptv/src/segmentation.c

@@ -113,6 +113,7 @@ int targ_rec (unsigned char *img, target_par *targ_par, int xmin,
 
                 for (n=0; n<4; n++) {
                     xn = x4[n];  yn = y4[n];
+                    if (!(xn < xmax) || !(yn < ymax)) continue;
                     gv = *(img0 + imx*yn + xn);
 
                     /* conditions for threshold, discontinuity, image borders */

liboptv/tests/check_track.c

@@ -65,6 +65,7 @@ int copy_res_dir(char *src, char *dest) {
             return 1;
         }
     }
+    return 0;
 }
 
 int empty_res_dir() {
@@ -87,6 +88,7 @@ int empty_res_dir() {
             return 1;
         }
     }
+    return 0;
 }
 
 START_TEST(test_predict)

py_bind/optv/.vscode/settings.json

@@ -0,0 +1,3 @@
+{
+    "python.pythonPath": "/Users/alex/miniconda3/envs/pyptv/bin/python"
+}

py_bind/optv/correspondences.pyx

@@ -1,10 +1,9 @@
-# -*- coding: utf-8 -*-
 """
 Implementation of bindings for correspondences and related data structures.
 
 Created on Fri Oct 28 13:46:39 2016
 
-@author: yosef
+@author: Yosef Meller, Alex Liberzon, TAU
 """
 
 from libc.stdlib cimport malloc, calloc, free
@@ -17,7 +16,7 @@ from optv.calibration cimport Calibration, calibration
 from optv.orientation cimport COORD_UNUSED
 from optv.tracking_framebuf cimport TargetArray, Target, target, frame, \
     PT_UNUSED, CORRES_NONE
-
+    
 cdef class MatchedCoords:
     """
     Keeps a block of 2D flat coordinates, each with a "point number", the same
@@ -145,9 +144,15 @@ def correspondences(list img_pts, list flat_coords, list cals,
     num_targs - total number of targets (must be greater than the sum of 
         previous 3).
     """
+    cdef int num_cams = len(cals)
+
+    # Special case of a single camera, follow the single_cam_correspondence docstring    
+    if num_cams == 1:
+        sorted_pos, sorted_corresp, num_targs = single_cam_correspondence(img_pts, flat_coords, cals)
+        return sorted_pos, sorted_corresp, num_targs
+
     cdef:
         int pt, cam
-        int num_cams = len(cals)
 
         calibration **calib = <calibration **> malloc(
             num_cams * sizeof(calibration *))
@@ -171,49 +176,115 @@ def correspondences(list img_pts, list flat_coords, list cals,
         frm.targets[cam] = (<TargetArray>img_pts[cam])._tarr
         frm.num_targets[cam] = len(img_pts[cam])
         corrected[cam] = (<MatchedCoords>flat_coords[cam]).buf
-        
+    
     # The biz:
     corresp_buf = corresp(&frm, corrected, 
         vparam._volume_par, cparam._control_par, calib, match_counts)
-    
+
     # Distribute data to return structures:
     sorted_pos = [None]*(num_cams - 1)
     sorted_corresp = [None]*(num_cams - 1)
     last_count = 0
-    
+
     for clique_type in xrange(num_cams - 1):
         num_points = match_counts[clique_type]
         clique_targs = np.full((num_cams, num_points, 2), PT_UNUSED, 
             dtype=np.float64)
         clique_ids = np.full((num_cams, num_points), CORRES_NONE, 
             dtype=np.int_)
-        
+    
         # Trace back the pixel target properties through the flat metric
         # intermediary that's x-sorted.
         for cam in range(num_cams):            
             for pt in range(num_points):
                 geo_id = corresp_buf[pt + last_count].p[cam]
                 if geo_id < 0:
                     continue
-                
+            
                 p1 = corrected[cam][geo_id].pnr
                 clique_ids[cam, pt] = p1
 
                 if p1 > -1:
                     targ = img_pts[cam][p1]
                     clique_targs[cam, pt, 0] = (<Target> targ)._targ.x
                     clique_targs[cam, pt, 1] = (<Target> targ)._targ.y
-        
+    
         last_count += num_points
         sorted_pos[clique_type] = clique_targs
         sorted_corresp[clique_type] = clique_ids
-    
+
     # Clean up.
     num_targs = match_counts[num_cams - 1]
+        
+        
+        
     free(frm.targets)
     free(frm.num_targets)
     free(calib)
     free(match_counts)
     free(corresp_buf) # Note this for future returning of correspondences.
 
     return sorted_pos, sorted_corresp, num_targs
+    
+    
+def single_cam_correspondence(list img_pts, list flat_coords, list cals):
+    """ 
+    Single camera correspondence is not a real correspondence, it will be only a projection 
+    of a 2D target from the image space into the 3D position, x,y,z using epi_mm_2d 
+    function. Here we only update the pointers of the targets and return it in a proper format. 
+    
+     Arguments:
+    img_pts - a list of c := len(cals), containing TargetArray objects, each 
+        with the target coordinates of n detections in the respective image.
+        The target arrays are clobbered: returned arrays have the tnr property 
+        set. the pnr property should be set to the target index in its array.
+    flat_coords - a list of MatchedCoordinates objects, one per camera, holding
+        the x-sorted flat-coordinates conversion of the respective image 
+        targets.
+    cals - a list of Calibration objects, each for the camera taking one image.
+
+    Returns:
+    sorted_pos - a tuple of (c,?,2) arrays, each with the positions in each of 
+        c image planes of points belonging to quadruplets, triplets, pairs 
+        found.
+    sorted_corresp - a tuple of (c,?) arrays, each with the point identifiers
+        of targets belonging to a quad/trip/etc per camera.
+    num_targs - total number of targets (must be greater than the sum of 
+        previous 3). 
+    """
+    cdef: 
+        int pt, num_points
+        coord_2d *corrected = <coord_2d *> malloc(sizeof(coord_2d *))
+    
+    corrected = (<MatchedCoords>flat_coords[0]).buf
+
+    sorted_pos = [None]
+    sorted_corresp = [None]
+
+    num_points = len(img_pts[0])
+
+    clique_targs = np.full((1, num_points, 2), PT_UNUSED, 
+        dtype=np.float64)
+    clique_ids = np.full((1, num_points), CORRES_NONE, 
+        dtype=np.int_)
+
+    # Trace back the pixel target properties through the flat metric
+    # intermediary that's x-sorted.
+    for pt in range(num_points):
+
+        # From Beat code (issue #118) pix[0][geo[0][i].pnr].tnr=i;
+
+        p1 = corrected[pt].pnr
+        clique_ids[0, pt] = p1
+
+        if p1 > -1:
+            targ = img_pts[0][p1]
+            clique_targs[0, pt, 0] = (<Target> targ)._targ.x
+            clique_targs[0, pt, 1] = (<Target> targ)._targ.x
+            # we also update the tnr, see docstring of correspondences
+            (<Target> targ)._targ.tnr = pt
+
+    sorted_pos[0] = clique_targs
+    sorted_corresp[0] = clique_ids
+
+    return sorted_pos, sorted_corresp, num_points

py_bind/optv/epipolar.pxd

@@ -0,0 +1,14 @@
+# -*- coding: utf-8 -*-
+"""
+Created on Thu Apr 18 11:12:36 2018
+
+@author: alexlib
+"""
+
+from optv.calibration cimport calibration
+from optv.parameters cimport mm_np, volume_par
+from optv.vec_utils cimport vec3d
+
+cdef extern from "optv/epi.h":
+    void  epi_mm_2D (double xl, double yl, calibration *cal, mm_np *mmp, 
+        volume_par *vpar, vec3d out);

py_bind/optv/orientation.pxd

@@ -1,7 +1,8 @@
 from optv.calibration cimport calibration
-from optv.parameters cimport control_par, mm_np
+from optv.parameters cimport control_par, mm_np, volume_par
 from optv.tracking_framebuf cimport target
 from optv.vec_utils cimport vec3d
+from optv.epipolar cimport epi_mm_2D
 
 cdef extern from "optv/sortgrid.h":
     target *sortgrid(calibration *cal, control_par *cpar, int nfix, vec3d fix[], int num, int eps, target pix[])
@@ -21,6 +22,10 @@ cdef extern from "optv/orientation.h":
 
     double point_position(vec2d targets[], int num_cams, mm_np *multimed_pars,
         calibration* cals[], vec3d res);
+    double single_cam_point_position(vec2d targets[], int num_cams, mm_np *multimed_pars,
+        calibration* cals[], vec3d res);
+    double multi_cam_point_position(vec2d targets[], int num_cams, mm_np *multimed_pars,
+        calibration* cals[], vec3d res);
     int raw_orient(calibration* cal, control_par *cpar, int nfix, vec3d fix[], 
         target pix[]);
     double* orient (calibration* cal_in, control_par *cpar, int nfix, 

py_bind/optv/orientation.pyx

@@ -6,7 +6,9 @@ from libc.stdlib cimport calloc, free
 
 from optv.tracking_framebuf cimport TargetArray
 from optv.calibration cimport Calibration
-from optv.parameters cimport ControlParams
+from optv.parameters cimport ControlParams, VolumeParams
+from optv.epipolar cimport epi_mm_2D
+
 
 def match_detection_to_ref(Calibration cal,
                            np.ndarray[ndim=2, dtype=pos_t] ref_pts,
@@ -68,6 +70,75 @@ cdef calibration** cal_list2arr(list cals):
     return calib
 
 def point_positions(np.ndarray[ndim=3, dtype=pos_t] targets,
+                    ControlParams cparam, cals, VolumeParams vparam):
+    """
+    Calculate the 3D positions of the points given by their 2D projections
+    using one of the options: 
+    - for a single camera, uses single_cam_point_positions()
+    - for multiple cameras, uses multi_cam_point_positions()
+
+    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``.
+    VolumeParams vparam - an object holding observed volume size parameters, needed
+        for the single camera case only.
+
+    Returns:
+    res - (n,3) array for n points represented by their targets.
+    rcm - n-length array, the Ray Convergence Measure for eachpoint for multi camera
+        option, or zeros for a single camera option
+    """
+    cdef:
+        np.ndarray[ndim=2, dtype=pos_t] res
+        np.ndarray[ndim=1, dtype=pos_t] rcm
+
+    if len(cals) == 1:
+        res, rcm = single_cam_point_positions(targets, cparam, cals, vparam)
+    elif len(cals) > 1:
+        res, rcm = multi_cam_point_positions(targets,cparam,cals)
+    else:
+        raise ValueError("wrong number of cameras in point_positions")
+    
+    return res, rcm
+
+
+def single_cam_point_positions(np.ndarray[ndim=3, dtype=pos_t] targets,
+                    ControlParams cparam, cals, VolumeParams vparam):
+    """
+    Calculates the 3D positions of the points from a single camera using
+    the 2D target positions given in metric coordinates
+
+    """
+    cdef:
+        np.ndarray[ndim=2, dtype=pos_t] res
+        np.ndarray[ndim=1, dtype=pos_t] rcm
+        np.ndarray[ndim=2, dtype=pos_t] targ
+        calibration ** calib = cal_list2arr(cals)
+        int cam, num_cams
+
+    # So we can address targets.data directly instead of get_ptr stuff:
+    targets = np.ascontiguousarray(targets)
+    
+    num_targets = targets.shape[0]
+    num_cams = targets.shape[1]
+    res = np.empty((num_targets, 3))
+    rcm = np.zeros(num_targets)
+
+    for pt in range(num_targets):
+        targ = targets[pt]
+        epi_mm_2D (targ[0][0], targ[0][1],
+                            calib[0], cparam._control_par.mm, vparam._volume_par, 
+                            <vec3d> np.PyArray_GETPTR2(res, pt, 0));
+
+    return res, rcm
+          
+
+def multi_cam_point_positions(np.ndarray[ndim=3, dtype=pos_t] targets,
                     ControlParams cparam, cals):
     """
     Calculate the 3D positions of the points given by their 2D projections.

py_bind/optv/segmentation.pyx

@@ -1,4 +1,3 @@
-# -*- coding: utf-8 -*-
 """
 Bindings for image segmentation / target recognition routins in liboptv.