Skip to content

Commit

Permalink
Add small number whenever the third column of the transformed points …
Browse files Browse the repository at this point in the history
…is 0
  • Loading branch information
Agustín Castro committed Jan 29, 2024
1 parent 1db42e2 commit 5646d08
Showing 1 changed file with 6 additions and 5 deletions.
11 changes: 6 additions & 5 deletions norfair/camera_motion.py
Original file line number Diff line number Diff line change
Expand Up @@ -150,20 +150,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]
Expand Down

0 comments on commit 5646d08

Please sign in to comment.