Add dlt calibration tools and triangulation

navipy/arenatools/cam_dlt.py

+import pandas as pd
+import numpy as np
+
+
+def dlt_reconstruct(coeff, campts, z=None):
+    """
+    This function reconstructs the 3D position of a coordinate based on a set
+    of DLT coefficients and [u,v] pixel coordinates from 2 or more cameras
+
+   :param coeff: - 11 DLT coefficients for all n cameras, [11,n] array
+   :param campts: - [u,v] pixel coordinates from all n cameras over f frames
+   :param z: the z coordinate of all points for reconstruction \
+from a single camera
+   :returns: xyz - the xyz location in each frame, an [f,3] array\
+rmse - the root mean square error for each xyz point, and [f,1] array,\
+units are [u,v] i.e. camera coordinates or pixels
+    """
+    # number of cameras
+    ncams = campts.columns.levels[0].shape[0]
+    if (ncams == 1) and (z is None):
+        raise NameError('reconstruction from a single camera require z')
+
+    # setup output variables
+    xyz = pd.DataFrame(index=campts.index, columns=['x', 'y', 'z'])
+    rmse = pd.Series(index=campts.index)
+    # process each frame
+    for ii, frame_i in enumerate(campts.index):
+        # get a list of cameras with non-NaN [u,v]
+        row = campts.loc[frame_i, :].unstack()
+        validcam = row.dropna(how='any')
+        cdx = validcam.index
+        if validcam.shape[0] >= 2:
+            # Two or more cameras
+            u = campts.loc[frame_i, cdx].u
+            v = campts.loc[frame_i, cdx].v
+
+            # initialize least-square solution matrices
+            m1 = np.zeros((cdx.shape[0]*2, 3))
+            m2 = np.zeros((cdx.shape[0]*2, 1))
+
+            m1[0: cdx.size*2: 2, 0] = u*coeff.loc[8, cdx]-coeff.loc[0, cdx]
+            m1[0: cdx.size*2: 2, 1] = u*coeff.loc[9, cdx]-coeff.loc[1, cdx]
+            m1[0: cdx.size*2: 2, 2] = u*coeff.loc[10, cdx]-coeff.loc[2, cdx]
+            m1[1: cdx.size*2: 2, 0] = v*coeff.loc[8, cdx]-coeff.loc[4, cdx]
+            m1[1: cdx.size*2: 2, 1] = v*coeff.loc[9, cdx]-coeff.loc[5, cdx]
+            m1[1: cdx.size*2: 2, 2] = v*coeff.loc[10, cdx]-coeff.loc[7, cdx]
+            m2[0: cdx.size*2: 2, 0] = coeff.loc[3, cdx]-u
+            m2[1: cdx.size*2: 2, 0] = coeff.loc[7, cdx]-v
+
+            # get the least squares solution to the reconstruction
+            xyz.loc[frame_i, ['x', 'y', 'z']] = \
+                np.linalg.lstsq(m1, m2, rcond=None)[0]
+
+            # compute ideal [u,v] for each camera
+            uv = m1.dot(xyz.loc[frame_i, ['x', 'y', 'z']].transpose())
+
+            # compute the number of degrees of freedom in the reconstruction
+            dof = m2.size-3
+
+            # estimate the root mean square reconstruction error
+            rmse.loc[frame_i] = (np.sum((m2-uv) ** 2)/dof) ^ 0.5
+
+        elif (validcam.shape[0] == 1) and (z is not None):
+            # http://www.kwon3d.com/theory/dlt/dlt.html
+            # equation 19 with z = constant
+            # the term with z can be move to right side
+            # then equation 21 can be solved as follow:
+            u = campts.loc[frame_i, cdx].unstack().u
+            v = campts.loc[frame_i, cdx].unstack().v
+
+            # initialize least-square solution matrices
+            m1 = np.zeros((cdx.shape[0]*2, 2))
+            m2 = np.zeros((cdx.shape[0]*2, 1))
+
+            m1[0: cdx.size*2: 2, 0] = u*coeff.loc[8, cdx]-coeff.loc[0, cdx]
+            m1[0: cdx.size*2: 2, 1] = u*coeff.loc[9, cdx]-coeff.loc[1, cdx]
+            m1[1: cdx.size*2: 2, 0] = v*coeff.loc[8, cdx]-coeff.loc[4, cdx]
+            m1[1: cdx.size*2: 2, 1] = v*coeff.loc[9, cdx]-coeff.loc[5, cdx]
+            m2[0: cdx.size*2: 2, 0] = coeff.loc[3, cdx]-u
+            m2[1: cdx.size*2: 2, 0] = coeff.loc[7, cdx]-v
+
+            m2[0: cdx.size*2: 2, 0] -= \
+                (u*coeff.loc[10, cdx] - coeff.loc[2, cdx])*z.loc[frame_i]
+            m2[1: cdx.size*2: 2, 0] -= \
+                (v*coeff.loc[10, cdx] - coeff.loc[6, cdx])*z.loc[frame_i]
+
+            # get the least squares solution to the reconstruction
+            xyz.loc[frame_i, ['x', 'y']] = \
+                np.squeeze(np.linalg.lstsq(m1, m2, rcond=None)[0])
+            xyz.loc[frame_i, 'z'] = z.loc[frame_i]
+
+            # compute ideal [u,v] for each camera
+            uv = m1.dot(xyz.loc[frame_i, ['x', 'y']].transpose())
+
+            # compute the number of degrees of freedom in the reconstruction
+            dof = m2.size-3
+
+            # estimate the root mean square reconstruction error
+            rmse.loc[frame_i] = (np.sum((m2-uv) ** 2)/dof) ** 0.5
+    return xyz, rmse
+
+
+def dlt_inverse(coeff, frames):
+    """
+    This function reconstructs the pixel coordinates of a 3D coordinate as
+    seen by the camera specificed by DLT coefficients c
+
+   :param coeff: - 11 DLT coefficients for the camera, [11,1] array
+   :param frames: - [x,y,z] coordinates over f frames,[f,3] array
+   :returns: uv - pixel coordinates in each frame, [f,2] array
+    """
+    # write the matrix solution out longhand for vector operation over
+    # all pointsat once
+    uv = np.zeros((frames.shape[0], 2))
+    frames = frames.loc[:, ['x', 'y', 'z']].values
+
+    normalisation = frames[:, 0]*coeff[8] + \
+        frames[:, 1]*coeff[9]+frames[:, 2]*coeff[10] + 1
+    uv[:, 0] = frames[:, 0]*coeff[0]+frames[:, 1] * \
+        coeff[1]+frames[:, 2]*coeff[2]+coeff[3]
+    uv[:, 1] = frames[:, 0]*coeff[4]+frames[:, 1] * \
+        coeff[5]+frames[:, 2]*coeff[6]+coeff[7]
+    uv[:, 0] /= normalisation
+    uv[:, 1] /= normalisation
+    return uv
+
+
+def dlt_compute_coeffs(frames, campts):
+    """
+    A basic implementation of 11 parameters DLT
+
+    : param frames: an array of x, y, z calibration point coordinates
+    : param campts: an array of u, v pixel coordinates from the camera
+    : returns: dlt coefficients and root mean square error
+
+    Notes: frame and camera points must have the same number of rows and at \
+least contains six rows. Also the frame points must not all lie within a \
+single plane.
+    """
+
+    # remove NaNs
+    valid_idx = frames.dropna(how='any').index
+    valid_idx = campts.loc[valid_idx, :].dropna(how='any').index
+
+    # valid df
+    vframes = frames.loc[valid_idx, :]
+    vcampts = campts.loc[valid_idx, :]
+
+    # re arange the frame matrix to facilitate the linear least
+    # sqaures solution
+    matrix = np.zeros((vframes.shape[0]*2, 11))  # 11 for the dlt
+    for num_i, index_i in enumerate(vframes.index):
+        matrix[2*num_i, 0:3] = vframes.loc[index_i, ['x', 'y', 'z']]
+        matrix[2*num_i+1, 4:7] = vframes.loc[index_i, ['x', 'y', 'z']]
+        matrix[2*num_i, 3] = 1
+        matrix[2*num_i+1, 7] = 1
+        matrix[2*num_i, 8:11] = \
+            vframes.loc[index_i, ['x', 'y', 'z']]*(-vcampts.loc[index_i, 'u'])
+        matrix[2*num_i+1, 8:11] = \
+            vframes.loc[index_i, ['x', 'y', 'z']]*(-vcampts.loc[index_i, 'v'])
+
+    # re argen the campts array for the linear solution
+    vcampts_f = np.reshape(np.flipud(np.rot90(vcampts)), vcampts.size, 1)
+    print(vcampts_f.shape)
+    print(matrix.shape)
+    # get the linear solution the 11 parameters
+    coeff = np.linalg.lstsq(matrix, vcampts_f, rcond=None)[0]
+    # compute the position of the frame in u,v coordinates given the linear
+    # solution from the previous line
+
+    matrix_uv = dlt_inverse(coeff, vframes)
+
+    # compute the rmse between the ideal frame u,v and the
+    # recorded frame u,v
+    rmse = np.sqrt(np.mean(np.sum((matrix_uv-vcampts)**2)))
+    return coeff, rmse

navipy/scripts/dlt_calibrator.py

+import argparse
+import pandas as pd
+import numpy as np
+import cv2
+import os
+from navipy.arenatools.cam_dlt import dlt_inverse
+from navipy.arenatools.cam_dlt import dlt_compute_coeffs
+
+
+keybinding = dict()
+keybinding['Quit without saving'] = 'q'
+keybinding['Save and quite'] = 'e'
+keybinding['Forward'] = 'f'
+keybinding['Backward'] = 'b'
+keybinding['Skip'] = 's'
+keybinding['Calculate coeff'] = 'c'
+
+
+def parser_dlt_calibrator():
+    # Create command line options
+
+    description = 'DLT calibrator provide a elementary user '
+    description += 'interface to determine the dlt coeffs of '
+    description += 'a camera from an image and calibration'
+    description += '\n\n'
+    description += 'Key bindings:\n'
+    description += '-------------\n'
+    for key, val in keybinding.items():
+        description += '{} : {}\n'.format(val, key)
+    parser = argparse.ArgumentParser(
+        description=description,
+        formatter_class=argparse.RawDescriptionHelpFormatter)
+    arghelp = 'Path to the calibration image'
+    parser.add_argument('-i', '--image',
+                        required=True,
+                        help=arghelp)
+    arghelp = 'Path to the csv files containing calibration points'
+    parser.add_argument('-p', '--points',
+                        required=True,
+                        help=arghelp)
+    arghelp = 'Scaling of the image'
+    parser.add_argument('-s', '--scale',
+                        default=0.5,
+                        help=arghelp)
+    return parser
+
+
+def click(event, x, y, flags, param):
+    # grab references to the global variables
+    global campts, index_i, scale
+    # if the left mouse button was clicked, record the starting
+    # (x, y) coordinates and indicate that cropping is being
+    # performed
+    if event == cv2.EVENT_LBUTTONDOWN:
+        campts.loc[index_i, ['u', 'v']] = [x/scale, y/scale]
+
+
+def main():
+    # Fetch arguments
+    args = vars(parser_dlt_calibrator().parse_args())
+    # Load the u,v points if any, otherwise
+    # set all u,v for each frame to nan, because
+    # we do not know the position of x,y,z points on cam
+    frames = pd.read_csv(args['points'])
+    if ('u' in frames.columns) and ('v' in frames.columns):
+        campts = frames.loc[:, ['u', 'v']]
+        frames = frames.drop('u', axis=1)
+        frames = frames.drop('v', axis=1)
+    else:
+        campts = pd.DataFrame(index=frames.index, columns=['u', 'v'])
+    # The image may need to be scale because screen may have
+    # less pixel than the image
+    scale = args['scale']
+    imageref = cv2.imread(args['image'])
+    # define some constants
+    showframe_ref = 50
+    showframe = showframe_ref
+    font = cv2.FONT_HERSHEY_SIMPLEX
+    coeff = None
+
+    # create an image window
+    cv2.namedWindow("image")
+    cv2.setMouseCallback("image", click)
+    # Loop until users quit
+    idx = 0  # User will control it during the loop
+    while True:
+        # Make sure the idx do not go outof bound
+        idx = np.clip(idx, 0, frames.shape[0]-1)
+        # load image
+        index_i = frames.index[idx]
+        mimage = imageref.copy()
+        # Display stuff at given time
+        if showframe > 0:
+            cv2.putText(mimage, str(index_i), (50, 50),
+                        font, 2, (0, 0, 255), 3, cv2.LINE_AA)
+            showframe -= 1
+        for nbi, row in campts.dropna().iterrows():
+            cv2.circle(mimage, (row.u.astype(int), row.v.astype(int)),
+                       5, (0, 0, 255), 1)
+            cv2.putText(mimage, str(nbi),
+                        (row.u.astype(int), row.v.astype(int)),
+                        font, 1, (0, 0, 255), 2, cv2.LINE_AA)
+        if coeff is not None:
+            matrix_uv = dlt_inverse(coeff, frames)
+            # print(matrix_uv)
+            matrix_uv = pd.DataFrame(data=matrix_uv,
+                                     index=frames.index,
+                                     columns=['u', 'v'])
+            matrix_uv[matrix_uv < 0] = np.nan
+            matrix_uv[matrix_uv.u > mimage.shape[0]] = np.nan
+            matrix_uv[matrix_uv.v > mimage.shape[1]] = np.nan
+            for nbi, row in matrix_uv.dropna().iterrows():
+                cv2.circle(mimage, (row.u.astype(int), row.v.astype(int)),
+                           5, (0, 255, 0), 1)
+                cv2.putText(mimage, str(nbi),
+                            (row.u.astype(int), row.v.astype(int)),
+                            font, 1, (0, 255, 0), 2, cv2.LINE_AA)
+        mimage = cv2.resize(mimage, (0, 0), fx=scale, fy=scale)
+        cv2.imshow("image", mimage)
+        # Wait for keys
+        key = cv2.waitKey(20) & 0xFF  # 20ms
+        # Key binding
+        if key == ord("q"):
+            print('quit without saving')
+            break
+        if key == ord("f"):
+            # forward
+            showframe = showframe_ref
+            idx += 1
+        if key == ord("s"):
+            # skip
+            showframe = showframe_ref
+            campts.loc[index_i, :] = np.nan
+            idx += 1
+        if key == ord("b"):
+            # backward
+            showframe = showframe_ref
+            idx -= 1
+        if key == ord("e"):
+            print('save and quit')
+            frames['u'] = campts.u
+            frames['v'] = campts.v
+            frames.to_csv(args['points'])
+            break
+        if key == ord("c"):
+            print('calibrate')
+            coeff, rmse = dlt_compute_coeffs(frames, campts)
+            print(rmse)
+            print(coeff)
+            coeff = pd.Series(data=coeff)
+            coeff.to_csv(os.path.splitext(args['points'])[0]+'_coeff.csv')
+
+    # close all open windows
+    cv2.destroyAllWindows()
+    print('End')
+
+
+if __name__ == "__main__":
+    # execute only if run as a script
+    main()

setup.py

@@ -76,7 +76,8 @@ setup_dict = {'name': 'navipy',
                       'blendunittest=navipy.scripts.blendunittest:main',
                       'blendongrid=navipy.scripts.blend_ongrid:main',
                       'blendoverlaytraj=navipy.scripts.blend_overlaytraj:main',
-                      'blendalongtraj=navipy.scripts.blend_alongtraj:main'
+                      'blendalongtraj=navipy.scripts.blend_alongtraj:main',
+                      'dltcalibrator=navipy.scripts.dlt_calibrator:main'
                   ]},
               }