Mercurial Hosting > traffic-intelligence

--- a/trafficintelligence/cvutils.py	Wed Mar 22 22:01:12 2023 -0400
+++ b/trafficintelligence/cvutils.py	Wed Mar 22 23:29:09 2023 -0400
@@ -535,6 +535,12 @@
         [map1, map2] = computeUndistortMaps(width, height, undistortedImageMultiplication, intrinsicCameraMatrix, distortionCoefficients)
         return cv2.remap(img, map1, map2, interpolation=interpolation)

+def cartesian2Homogeneous(m):
+    'Transforms n x m matrix to n x (m+1) by adding last column of 1s'
+    homoM = ones((m.shape[0], m.shape[1]+1))
+    homoM[:,:-1] = m
+    return homoM
+
 def homographyProject(points, homography, output3D = False):
     '''Returns the coordinates of the points (2xN array) projected through homography'''
     if points.shape[0] != 2:
--- a/trafficintelligence/moving.py	Wed Mar 22 22:01:12 2023 -0400
+++ b/trafficintelligence/moving.py	Wed Mar 22 23:29:09 2023 -0400
@@ -357,12 +357,12 @@
         return (p1-p2).norm2()

     @staticmethod
-    def plotAll(points, options = '', closePolygon = False, **kwargs):
+    def plotAll(points, closePolygon = False, options = '', **kwargs):
         xCoords = [p.x for p in points]
         yCoords = [p.y for p in points]
         if closePolygon:
-            xCoords.append[0]
-            yCoords.append[0]
+            xCoords.append(xCoords[0])
+            yCoords.append(yCoords[0])
         plot(xCoords, yCoords, options, **kwargs)

     def similarOrientation(self, refDirection, cosineThreshold):
@@ -1659,7 +1659,7 @@
     def plotOutlineAtInstant(self, t, options = '', **kwargs):
         if self.hasFeatures():
             points = [f.getPositionAtInstant(t) for f in self.getFeatures()]
-            Point.plotAll(points, options, True, kwargs)
+            Point.plotAll(points, True, options, **kwargs)

     def play(self, videoFilename, homography = None, undistort = False, intrinsicCameraMatrix = None, distortionCoefficients = None, undistortedImageMultiplication = 1.):
         cvutils.displayTrajectories(videoFilename, [self], homography = homography, firstFrameNum = self.getFirstInstant(), lastFrameNumArg = self.getLastInstant(), undistort = undistort, intrinsicCameraMatrix = intrinsicCameraMatrix, distortionCoefficients = distortionCoefficients, undistortedImageMultiplication = undistortedImageMultiplication)
--- a/trafficintelligence/storage.py	Wed Mar 22 22:01:12 2023 -0400
+++ b/trafficintelligence/storage.py	Wed Mar 22 23:29:09 2023 -0400
@@ -7,10 +7,10 @@
 from copy import copy
 import sqlite3, logging

-from numpy import log, min as npmin, max as npmax, round as npround, array, sum as npsum, loadtxt, floor as npfloor, ceil as npceil, linalg, int32, int64, reshape, dot, vstack, transpose, ones
+from numpy import log, min as npmin, max as npmax, round as npround, array, sum as npsum, loadtxt, floor as npfloor, ceil as npceil, linalg, int32, int64, reshape, dot, vstack, transpose, ones, zeros_like
 from pandas import read_csv, merge

-from trafficintelligence import utils, moving, events, indicators
+from trafficintelligence import utils, moving, events, indicators, cvutils
 from trafficintelligence.base import VideoFilenameAddable


@@ -1261,8 +1261,7 @@
     """ Inverse a rigid body transform matrix (3x4 as [R|t])
         [R'|-R't; 0|1]
     """
-    import numpy as np
-    inv_Tr = np.zeros_like(Tr)  # 3x4
+    inv_Tr = zeros_like(Tr)  # 3x4
     inv_Tr[0:3, 0:3] = transpose(Tr[0:3, 0:3])
     inv_Tr[0:3, 3] = dot(-transpose(Tr[0:3, 0:3]), Tr[0:3, 3])
     return inv_Tr
@@ -1271,7 +1270,7 @@
     '''Loads KITTI calibration data'''
     calib = {}
     with open(filename, 'r') as f:
-        for l in f.readlines():
+        for l in f:
             l = l.rstrip()
             if len(l) == 0:
                 continue
@@ -1285,10 +1284,8 @@
                 continue
     #calib['Tr_velo_to_cam'] = calib['Tr_velo_cam']
     #calib['Tr_imu_to_velo'] = calib['Tr_imu_velo']
-    temps = reshape(calib['Tr_velo_cam'], (3, 4))
-    calib['Tr_cam_velo'] = inverse_rigid_trans(temps)
-    temps = reshape(calib['Tr_imu_velo'], (3, 4))
-    calib['Tr_velo_imu'] = inverse_rigid_trans(temps)
+    calib['Tr_cam_velo'] = inverse_rigid_trans(reshape(calib['Tr_velo_cam'], (3, 4)))
+    calib['Tr_velo_imu'] = inverse_rigid_trans(reshape(calib['Tr_imu_velo'], (3, 4)))

     P = reshape(calib['P2'], (3, 4))
     calib['c_u'] = P[0, 2]
@@ -1302,23 +1299,6 @@
 # from https://github.com/utiasSTARS/pykitti/blob/master/pykitti/utils.py


-# def get_cam_in_ex(calib):
-#     """
-#     Qingwu
-#     :param calib: calib from kitti_tracking
-#     :return:  cam_parameters: Camera intrinsics and extrinsics
-#     """
-#     cam_parameters = {}
-#     P = np.reshape(calib["P2"], [3, 4])
-#     cam_parameters["c_u"] = P[0, 2]
-#     cam_parameters["c_v"] = P[1, 2]
-#     cam_parameters["f_u"] = P[0, 0]
-#     cam_parameters["f_v"] = P[1, 1]
-#     cam_parameters["b_x"] = P[0, 3] / (-cam_parameters["f_u"])  # relative
-#     cam_parameters["b_y"] = P[1, 3] / (-cam_parameters["f_v"])
-#     return cam_parameters
-
-
 def loadTrajectoriesFromKITTI(filename, kittiCalibration, oxts):
     '''Reads data from KITTI ground truth or output from an object detection and tracking method

@@ -1329,6 +1309,7 @@
     from pykitti.utils import roty

     invR0 = linalg.inv(reshape(kittiCalibration['R_rect'], (3, 3)))
+    transCam2Velo = transpose(kittiCalibration['Tr_cam_velo'])

     header = ['frame', # 0, 1, ..., n
               'trackingid', # -1, 0 , 1, ..., k
@@ -1357,7 +1338,7 @@
         t = moving.Trajectory()#([[float(numbers[6])],[float(numbers[7])]])
         featureTrajectories = [moving.Trajectory() for i in range(4)]
         for i, r in tmp.iterrows():
-            _, Tr_imu2w = oxts[r.frame]
+            _, Tr_imu_to_world = oxts[r.frame]
             # transfer_matrix = {'P2': reshape(kittiCalibration['P2'], (3, 4)),
             #                    'R_rect': kittiCalibration['R_rect'],
             #                    'Tr_cam_to_velo': kittiCalibration['Tr_cam_velo'],
@@ -1380,17 +1361,18 @@
             corners3d[2, :] = corners3d[2, :] + r.z#obj.t[2]
             # corners3d = transpose(corners3d)
             # box3d_pts_3d_velo = calib.project_rect_to_velo(box3d_pts_3d)
-            veloCorners = transpose(dot(invR0, corners3d)) # 8x3 avoid double transpose np.transpose(pts_3d_rect)))
-            #             boxes3d_world_single, flag_imu, flag_world = calib_function.project_velo_to_world(bboxes_3d_velo=boxes3d_velo_single, Tr_matrix=transfer_matrix)
+            refCorners = transpose(dot(invR0, corners3d)) # 8x3 avoid double transpose np.transpose(pts_3d_rect))) in pts_3d_ref = self.project_rect_to_ref(pts_3d_rect)
+            homRefCorners = cvutils.cartesian2Homogeneous(refCorners)
+            veloCorners = dot(homRefCorners, transCam2Velo)
+# self.project_ref_to_velo(pts_3d_ref)
+            # boxes3d_world_single, flag_imu, flag_world = calib_function.project_velo_to_world(bboxes_3d_velo=boxes3d_velo_single, Tr_matrix=transfer_matrix)
             #Tr_velo_to_imu = transfer_matrix['Tr_velo_to_imu']
             #Tr_imu_to_world = transfer_matrix['Tr_imu_to_world']
-            homVeloCorners = ones((veloCorners.shape[0], veloCorners.shape[1]+1))
-            homVeloCorners[:,:-1] = veloCorners # 8x4 with ones in last column
+            homVeloCorners = cvutils.cartesian2Homogeneous(veloCorners)
             imuCorners = dot(kittiCalibration['Tr_velo_imu'], homVeloCorners.T).T # 8x3

-            homImuCorners = ones((imuCorners.shape[0], imuCorners.shape[1]+1))
-            homImuCorners[:,:-1] = imuCorners # 8x4 with ones in last column
-            worldCorners = dot(Tr_imu2w, homImuCorners.T).T # 8x3
+            homImuCorners = cvutils.cartesian2Homogeneous(imuCorners)
+            worldCorners = dot(Tr_imu_to_world, homImuCorners.T).T # 8x3

             # take first 4 lines of corners, x,y,_ # x0, y0, _ = boxes3d[0]
             xCoords = worldCorners[:4,0]
@@ -1399,8 +1381,7 @@
             for i in range(4):
                 featureTrajectories[i].addPositionXY(xCoords[i], yCoords[i])
             # check https://docs.opencv.org/3.4/d9/d0c/group__calib3d.html#ga1019495a2c8d1743ed5cc23fa0daff8c
-        newObj = moving.MovingObject(num = objNum, timeInterval = moving.TimeInterval(tmp.frame.min(), tmp.frame.max()), positions = t, userType = tmp.iloc[0].type, features = [moving.MovingObject(num = featureNum+i, timeInterval = moving.TimeInterval(tmp.frame.min(), tmp.frame.max()), positions = featureTrajectories[i]) for i in range(4)])
-        #newObj.featureNumbers = [f.getNum() for f in newObj.getFeatures()]
+        newObj = moving.MovingObject(num = objNum, timeInterval = moving.TimeInterval(int(tmp.frame.min()), int(tmp.frame.max())), positions = t, userType = tmp.iloc[0].type, features = [moving.MovingObject(num = featureNum+i, timeInterval = moving.TimeInterval(int(tmp.frame.min()), int(tmp.frame.max())), positions = featureTrajectories[i]) for i in range(4)])
         objects.append(newObj)
         featureNum += 4
     return objects