From 2049fe2ba46d1c2262c80cb7ff7ad08d59a68ba6 Mon Sep 17 00:00:00 2001 From: gbot Date: Fri, 15 Sep 2023 17:53:20 +0200 Subject: [PATCH 01/11] prevent homography opencv error --- norfair/camera_motion.py | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/norfair/camera_motion.py b/norfair/camera_motion.py index 6e89bd6f..8d083105 100644 --- a/norfair/camera_motion.py +++ b/norfair/camera_motion.py @@ -212,7 +212,16 @@ def __init__( def __call__( self, curr_pts: np.ndarray, prev_pts: np.ndarray - ) -> Tuple[bool, HomographyTransformation]: + ) -> Tuple[bool, Optional[HomographyTransformation]]: + + if prev_pts.shape[0] < 4: + # Preventing the following error: cv2.error: OpenCV(4.8.0) + # /io/opencv/modules/calib3d/src/fundam.cpp:385: + # error: (-28:Unknown error code -28) The input arrays should have + # at least 4 corresponding point sets to calculate Homography + # in function 'findHomography' + return True, None + homography_matrix, points_used = cv2.findHomography( prev_pts, curr_pts, From c847befc3e2addb12650b36d851db10679ae2666 Mon Sep 17 00:00:00 2001 From: gbot Date: Fri, 15 Sep 2023 18:41:29 +0200 Subject: [PATCH 02/11] prevent error on _get_sparse_flow --- norfair/camera_motion.py | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/norfair/camera_motion.py b/norfair/camera_motion.py index 8d083105..c11b048b 100644 --- a/norfair/camera_motion.py +++ b/norfair/camera_motion.py @@ -270,6 +270,12 @@ def _get_sparse_flow( mask=mask, ) + if prev_pts is None: + # Preventing the following error: cv2.error: OpenCV(4.8.0) + # /io/opencv/modules/video/src/lkpyramid.cpp:1260: error: (-215:Assertion failed) + # (npoints = prevPtsMat.checkVector(2, CV_32F, true)) >= 0 in function 'calc' + return None, None + # compute optical flow curr_pts, status, err = cv2.calcOpticalFlowPyrLK( gray_prvs, gray_next, prev_pts, None From e9c0e46ba10b4191ddb1d8a9174175d56367c119 Mon Sep 17 00:00:00 2001 From: gbot Date: Fri, 15 Sep 2023 18:41:58 +0200 Subject: [PATCH 03/11] handle None points and update operations --- norfair/camera_motion.py | 33 +++++++++++++++++++-------------- 1 file changed, 19 insertions(+), 14 deletions(-) diff --git a/norfair/camera_motion.py b/norfair/camera_motion.py index c11b048b..cfc40fd6 100644 --- a/norfair/camera_motion.py +++ b/norfair/camera_motion.py @@ -211,10 +211,10 @@ def __init__( self.proportion_points_used_threshold = proportion_points_used_threshold def __call__( - self, curr_pts: np.ndarray, prev_pts: np.ndarray + self, curr_pts: Optional[np.ndarray], prev_pts: Optional[np.ndarray] ) -> Tuple[bool, Optional[HomographyTransformation]]: - if prev_pts.shape[0] < 4: + if prev_pts is None or curr_pts is None or prev_pts.shape[0] < 4: # Preventing the following error: cv2.error: OpenCV(4.8.0) # /io/opencv/modules/calib3d/src/fundam.cpp:385: # error: (-28:Unknown error code -28) The input arrays should have @@ -391,7 +391,7 @@ def update( self.gray_prvs = self.gray_next self.prev_mask = mask - curr_pts, self.prev_pts = _get_sparse_flow( + curr_pts, prev_pts = _get_sparse_flow( self.gray_next, self.gray_prvs, self.prev_pts, @@ -401,17 +401,22 @@ def update( self.prev_mask, quality_level=self.quality_level, ) - if self.draw_flow: - for (curr, prev) in zip(curr_pts, self.prev_pts): - c = tuple(curr.astype(int).ravel()) - p = tuple(prev.astype(int).ravel()) - cv2.line(frame, c, p, self.flow_color, 2) - cv2.circle(frame, c, 3, self.flow_color, -1) - - update_prvs, coord_transformations = self.transformations_getter( - curr_pts, - self.prev_pts, - ) + + update_prvs, coord_transformations = True, None + if curr_pts is not None and prev_pts is not None: + self.prev_pts = prev_pts + + if self.draw_flow: + for (curr, prev) in zip(curr_pts, self.prev_pts): + c = tuple(curr.astype(int).ravel()) + p = tuple(prev.astype(int).ravel()) + cv2.line(frame, c, p, self.flow_color, 2) + cv2.circle(frame, c, 3, self.flow_color, -1) + + update_prvs, coord_transformations = self.transformations_getter( + curr_pts, + self.prev_pts, + ) if update_prvs: self.gray_prvs = self.gray_next From 8c92fa0ffaf19df8f17e25c70047b1228cfc4dc4 Mon Sep 17 00:00:00 2001 From: gbot Date: Sun, 15 Oct 2023 11:47:10 +0200 Subject: [PATCH 04/11] best working version --- norfair/camera_motion.py | 74 +++++++++++++++++++++------------------- 1 file changed, 39 insertions(+), 35 deletions(-) diff --git a/norfair/camera_motion.py b/norfair/camera_motion.py index cfc40fd6..6aad9ef3 100644 --- a/norfair/camera_motion.py +++ b/norfair/camera_motion.py @@ -1,5 +1,7 @@ "Camera motion stimation module." +import copy from abc import ABC, abstractmethod +from logging import warning from typing import Optional, Tuple import numpy as np @@ -151,7 +153,11 @@ def abs_to_rel(self, points: np.ndarray): points_transformed = points_transformed / points_transformed[:, -1].reshape( -1, 1 ) - return points_transformed[:, :2] + new_points_transformed = points_transformed[:, :2] + if np.isnan(new_points_transformed).any() or np.isinf(new_points_transformed).any(): + new_points_transformed = np.array([[0, 0], [0, 0]]) + + return new_points_transformed def rel_to_abs(self, points: np.ndarray): ones = np.ones((len(points), 1)) @@ -211,15 +217,12 @@ def __init__( self.proportion_points_used_threshold = proportion_points_used_threshold def __call__( - self, curr_pts: Optional[np.ndarray], prev_pts: Optional[np.ndarray] + self, curr_pts: np.ndarray, prev_pts: np.ndarray ) -> Tuple[bool, Optional[HomographyTransformation]]: - if prev_pts is None or curr_pts is None or prev_pts.shape[0] < 4: - # Preventing the following error: cv2.error: OpenCV(4.8.0) - # /io/opencv/modules/calib3d/src/fundam.cpp:385: - # error: (-28:Unknown error code -28) The input arrays should have - # at least 4 corresponding point sets to calculate Homography - # in function 'findHomography' + if not (isinstance(prev_pts, np.ndarray) and prev_pts.shape[0] >= 4 + and isinstance(curr_pts, np.ndarray) and curr_pts.shape[0] >= 4): + warning('Can\'t calculate homography, less than 4 four points to match') return True, None homography_matrix, points_used = cv2.findHomography( @@ -270,12 +273,6 @@ def _get_sparse_flow( mask=mask, ) - if prev_pts is None: - # Preventing the following error: cv2.error: OpenCV(4.8.0) - # /io/opencv/modules/video/src/lkpyramid.cpp:1260: error: (-215:Assertion failed) - # (npoints = prevPtsMat.checkVector(2, CV_32F, true)) >= 0 in function 'calc' - return None, None - # compute optical flow curr_pts, status, err = cv2.calcOpticalFlowPyrLK( gray_prvs, gray_next, prev_pts, None @@ -355,13 +352,15 @@ def __init__( transformations_getter = HomographyTransformationGetter() self.transformations_getter = transformations_getter + self.transformations_getter_copy = copy.deepcopy(transformations_getter) + self.prev_mask = None self.gray_next = None self.quality_level = quality_level def update( self, frame: np.ndarray, mask: np.ndarray = None - ) -> CoordinatesTransformation: + ) -> Optional[CoordinatesTransformation]: """ Estimate camera motion for each frame @@ -386,41 +385,46 @@ def update( The CoordinatesTransformation that can transform coordinates on this frame to absolute coordinates or vice versa. """ + self.gray_next = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) if self.gray_prvs is None: self.gray_prvs = self.gray_next self.prev_mask = mask - curr_pts, prev_pts = _get_sparse_flow( - self.gray_next, - self.gray_prvs, - self.prev_pts, - self.max_points, - self.min_distance, - self.block_size, - self.prev_mask, - quality_level=self.quality_level, - ) - - update_prvs, coord_transformations = True, None - if curr_pts is not None and prev_pts is not None: - self.prev_pts = prev_pts - + curr_pts, prev_pts = None, None + try: + curr_pts, prev_pts = _get_sparse_flow( + self.gray_next, + self.gray_prvs, + self.prev_pts, + self.max_points, + self.min_distance, + self.block_size, + self.prev_mask, + quality_level=self.quality_level, + ) if self.draw_flow: - for (curr, prev) in zip(curr_pts, self.prev_pts): + for (curr, prev) in zip(curr_pts, prev_pts): c = tuple(curr.astype(int).ravel()) p = tuple(prev.astype(int).ravel()) cv2.line(frame, c, p, self.flow_color, 2) cv2.circle(frame, c, 3, self.flow_color, -1) + except Exception as e: + warning(e) - update_prvs, coord_transformations = self.transformations_getter( - curr_pts, - self.prev_pts, - ) + update_prvs, coord_transformations = True, None + try: + update_prvs, coord_transformations = self.transformations_getter(curr_pts, prev_pts) + except Exception as e: + warning(e) + del self.transformations_getter + self.transformations_getter = copy.deepcopy(self.transformations_getter_copy) if update_prvs: self.gray_prvs = self.gray_next self.prev_pts = None self.prev_mask = mask + else: + self.prev_pts = prev_pts return coord_transformations From a1a8547e53abcc7e2f1539a4e71210737a249093 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Agust=C3=ADn=20Castro?= Date: Thu, 12 Oct 2023 13:59:48 -0300 Subject: [PATCH 05/11] Added warning message when homography couldn't be computed In that case, we keep the transformation from last frame --- norfair/camera_motion.py | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/norfair/camera_motion.py b/norfair/camera_motion.py index 6aad9ef3..19826c48 100644 --- a/norfair/camera_motion.py +++ b/norfair/camera_motion.py @@ -3,6 +3,7 @@ from abc import ABC, abstractmethod from logging import warning from typing import Optional, Tuple +from logging import warning import numpy as np @@ -222,8 +223,12 @@ def __call__( if not (isinstance(prev_pts, np.ndarray) and prev_pts.shape[0] >= 4 and isinstance(curr_pts, np.ndarray) and curr_pts.shape[0] >= 4): - warning('Can\'t calculate homography, less than 4 four points to match') - return True, None + warning("The homography couldn't be computed in this frame " + "due to low amount of points") + if isinstance(self.data, np.ndarray): + return True, HomographyTransformation(self.data) + else: + return True, None homography_matrix, points_used = cv2.findHomography( prev_pts, From 8e5c2fdcdaa1dd044682ff1a2d9661a05f873301 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Agust=C3=ADn=20Castro?= Date: Thu, 12 Oct 2023 14:38:29 -0300 Subject: [PATCH 06/11] If not enough points and we have no previous transformation, return None --- norfair/camera_motion.py | 8 ++------ 1 file changed, 2 insertions(+), 6 deletions(-) diff --git a/norfair/camera_motion.py b/norfair/camera_motion.py index 19826c48..a3f88f80 100644 --- a/norfair/camera_motion.py +++ b/norfair/camera_motion.py @@ -223,12 +223,8 @@ def __call__( if not (isinstance(prev_pts, np.ndarray) and prev_pts.shape[0] >= 4 and isinstance(curr_pts, np.ndarray) and curr_pts.shape[0] >= 4): - warning("The homography couldn't be computed in this frame " - "due to low amount of points") - if isinstance(self.data, np.ndarray): - return True, HomographyTransformation(self.data) - else: - return True, None + warning('Can\'t calculate homography, less than 4 four points to match') + return True, None homography_matrix, points_used = cv2.findHomography( prev_pts, From 6cde4b2aefb1a1dd56ecb86def43e699ad567d10 Mon Sep 17 00:00:00 2001 From: gbot Date: Sun, 15 Oct 2023 11:57:10 +0200 Subject: [PATCH 07/11] homography calculation warning --- norfair/camera_motion.py | 12 +++++++++--- 1 file changed, 9 insertions(+), 3 deletions(-) diff --git a/norfair/camera_motion.py b/norfair/camera_motion.py index a3f88f80..d82277c5 100644 --- a/norfair/camera_motion.py +++ b/norfair/camera_motion.py @@ -223,8 +223,12 @@ def __call__( if not (isinstance(prev_pts, np.ndarray) and prev_pts.shape[0] >= 4 and isinstance(curr_pts, np.ndarray) and curr_pts.shape[0] >= 4): - warning('Can\'t calculate homography, less than 4 four points to match') - return True, None + warning("The homography couldn't be computed in this frame " + "due to low amount of points") + if isinstance(self.data, np.ndarray): + return True, HomographyTransformation(self.data) + else: + return True, None homography_matrix, points_used = cv2.findHomography( prev_pts, @@ -415,7 +419,9 @@ def update( update_prvs, coord_transformations = True, None try: - update_prvs, coord_transformations = self.transformations_getter(curr_pts, prev_pts) + update_prvs, coord_transformations = self.transformations_getter( + curr_pts, prev_pts + ) except Exception as e: warning(e) del self.transformations_getter From 631164073b7e62693608536863b083c2457260a4 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Agust=C3=ADn=20Castro?= Date: Mon, 29 Jan 2024 15:01:58 -0300 Subject: [PATCH 08/11] Add small number whenever the third column of the transformed points is 0 --- norfair/camera_motion.py | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/norfair/camera_motion.py b/norfair/camera_motion.py index d82277c5..76e52c3e 100644 --- a/norfair/camera_motion.py +++ b/norfair/camera_motion.py @@ -151,20 +151,21 @@ def abs_to_rel(self, points: np.ndarray): ones = np.ones((len(points), 1)) points_with_ones = np.hstack((points, ones)) points_transformed = points_with_ones @ self.homography_matrix.T - points_transformed = points_transformed / points_transformed[:, -1].reshape( + last_column = points_transformed[:, -1] + last_column[last_column == 0] = 0.0000001 + points_transformed = points_transformed / last_column.reshape( -1, 1 ) new_points_transformed = points_transformed[:, :2] - if np.isnan(new_points_transformed).any() or np.isinf(new_points_transformed).any(): - new_points_transformed = np.array([[0, 0], [0, 0]]) - return new_points_transformed def rel_to_abs(self, points: np.ndarray): ones = np.ones((len(points), 1)) points_with_ones = np.hstack((points, ones)) points_transformed = points_with_ones @ self.inverse_homography_matrix.T - points_transformed = points_transformed / points_transformed[:, -1].reshape( + last_column = points_transformed[:, -1] + last_column[last_column == 0] = 0.0000001 + points_transformed = points_transformed / last_column.reshape( -1, 1 ) return points_transformed[:, :2] From 18920ae2f17ceb673c2b58d27e8e6c4634b07015 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Agust=C3=ADn=20Castro?= Date: Mon, 29 Jan 2024 15:03:08 -0300 Subject: [PATCH 09/11] Use new drawing functions in camera_modetion demo --- demos/camera_motion/src/demo.py | 29 +++++++++++++++++++++-------- 1 file changed, 21 insertions(+), 8 deletions(-) diff --git a/demos/camera_motion/src/demo.py b/demos/camera_motion/src/demo.py index c13f264b..42673dd0 100644 --- a/demos/camera_motion/src/demo.py +++ b/demos/camera_motion/src/demo.py @@ -11,14 +11,15 @@ Tracker, Video, draw_absolute_grid, - draw_tracked_boxes, ) + from norfair.camera_motion import ( HomographyTransformationGetter, MotionEstimator, TranslationTransformationGetter, ) -from norfair.drawing import draw_tracked_objects + +from norfair.drawing import draw_points, draw_boxes def yolo_detections_to_norfair_detections(yolo_detections, track_boxes): @@ -66,7 +67,7 @@ def run(): parser.add_argument( "--distance-threshold", type=float, - default=0.8, + default=None, help="Max distance to consider when matching detections and tracked objects", ) parser.add_argument( @@ -223,10 +224,22 @@ def run(): else partial(video.show, downsample_ratio=args.downsample_ratio) ) + distance_threshold = args.distance_threshold + if args.track_boxes: + drawing_function = draw_boxes + distance_function = "iou" + if args.distance_threshold is None: + distance_threshold = 0.5 + else: + drawing_function = draw_points + distance_function = "euclidean" + if args.distance_threshold is None: + distance_threshold = video.input_height / 10 + tracker = Tracker( - distance_function="euclidean", + distance_function=distance_function, detection_threshold=args.confidence_threshold, - distance_threshold=args.distance_threshold, + distance_threshold=distance_threshold, initialization_delay=args.initialization_delay, hit_counter_max=args.hit_counter_max, ) @@ -259,11 +272,11 @@ def run(): ) if args.draw_objects: - draw_tracked_objects( + drawing_function( frame, tracked_objects, - id_size=args.id_size, - id_thickness=None + text_size=args.id_size, + text_thickness=None if args.id_size is None else int(args.id_size * 2), ) From c6d1f435e8b294eba3d9c4fc73feb25a38648194 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Agust=C3=ADn=20Castro?= Date: Tue, 30 Jan 2024 11:24:14 -0300 Subject: [PATCH 10/11] Remove repeated import --- norfair/camera_motion.py | 1 - 1 file changed, 1 deletion(-) diff --git a/norfair/camera_motion.py b/norfair/camera_motion.py index 76e52c3e..c7b32047 100644 --- a/norfair/camera_motion.py +++ b/norfair/camera_motion.py @@ -3,7 +3,6 @@ from abc import ABC, abstractmethod from logging import warning from typing import Optional, Tuple -from logging import warning import numpy as np From b38fb36e4fd30fa04393be60e9bf0c6a42fced29 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Agust=C3=ADn=20Castro?= Date: Tue, 30 Jan 2024 11:26:19 -0300 Subject: [PATCH 11/11] Run black on camera_motion.py --- norfair/camera_motion.py | 26 +++++++++++++++----------- 1 file changed, 15 insertions(+), 11 deletions(-) diff --git a/norfair/camera_motion.py b/norfair/camera_motion.py index c7b32047..cdf6a8ba 100644 --- a/norfair/camera_motion.py +++ b/norfair/camera_motion.py @@ -152,9 +152,7 @@ def abs_to_rel(self, points: np.ndarray): points_transformed = points_with_ones @ self.homography_matrix.T last_column = points_transformed[:, -1] last_column[last_column == 0] = 0.0000001 - points_transformed = points_transformed / last_column.reshape( - -1, 1 - ) + points_transformed = points_transformed / last_column.reshape(-1, 1) new_points_transformed = points_transformed[:, :2] return new_points_transformed @@ -164,9 +162,7 @@ def rel_to_abs(self, points: np.ndarray): points_transformed = points_with_ones @ self.inverse_homography_matrix.T last_column = points_transformed[:, -1] last_column[last_column == 0] = 0.0000001 - points_transformed = points_transformed / last_column.reshape( - -1, 1 - ) + points_transformed = points_transformed / last_column.reshape(-1, 1) return points_transformed[:, :2] @@ -221,10 +217,16 @@ def __call__( self, curr_pts: np.ndarray, prev_pts: np.ndarray ) -> Tuple[bool, Optional[HomographyTransformation]]: - if not (isinstance(prev_pts, np.ndarray) and prev_pts.shape[0] >= 4 - and isinstance(curr_pts, np.ndarray) and curr_pts.shape[0] >= 4): - warning("The homography couldn't be computed in this frame " - "due to low amount of points") + if not ( + isinstance(prev_pts, np.ndarray) + and prev_pts.shape[0] >= 4 + and isinstance(curr_pts, np.ndarray) + and curr_pts.shape[0] >= 4 + ): + warning( + "The homography couldn't be computed in this frame " + "due to low amount of points" + ) if isinstance(self.data, np.ndarray): return True, HomographyTransformation(self.data) else: @@ -425,7 +427,9 @@ def update( except Exception as e: warning(e) del self.transformations_getter - self.transformations_getter = copy.deepcopy(self.transformations_getter_copy) + self.transformations_getter = copy.deepcopy( + self.transformations_getter_copy + ) if update_prvs: self.gray_prvs = self.gray_next