Merge pull request #278 from gfugante/fix-homography-error

[FIX][MOTION ESTIMATION] Fix error on homography calculation

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),
                 )

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
@@ -148,18 +150,19 @@ 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(
-            -1, 1
-        )
-        return points_transformed[:, :2]
+        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]
+        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(
-            -1, 1
-        )
+        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]
 
 
@@ -212,7 +215,23 @@ def __init__(
 
     def __call__(
         self, curr_pts: np.ndarray, prev_pts: np.ndarray
-    ) -> Tuple[bool, HomographyTransformation]:
+    ) -> 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 isinstance(self.data, np.ndarray):
+                return True, HomographyTransformation(self.data)
+            else:
+                return True, None
+
         homography_matrix, points_used = cv2.findHomography(
             prev_pts,
             curr_pts,
@@ -340,13 +359,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
 
@@ -371,36 +392,50 @@ 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, self.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):
-                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,
-        )
+        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, 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 = 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