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