File size: 3,819 Bytes
3d1f2c9 |
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 |
import sys
import cv2
import numpy as np
from typing import List, Tuple, Optional
from shapely.geometry import LineString, Polygon
def plane_from_P(P, cam_pos, principal_point):
def is_invertible(a):
# return a.shape[0] == a.shape[1] and np.linalg.matrix_rank(a) == a.shape[0]
return np.linalg.cond(a) < 1 / np.finfo(a.dtype).eps
if not any(np.isnan(P.flatten())):
H = np.delete(P, 2, axis=1)
pp = np.array([principal_point[0], principal_point[1], 1.])
if is_invertible(H):
pp_proj = np.linalg.inv(H) @ pp
else:
pp_proj = np.linalg.pinv(H) @ pp
pp_proj /= pp_proj[-1]
plane_vector = pp_proj - cam_pos
return plane_vector, cam_pos
else:
return None, None
def plane_from_H(H, cam_pos, principal_point):
def is_invertible(a):
# return a.shape[0] == a.shape[1] and np.linalg.matrix_rank(a) == a.shape[0]
return np.linalg.cond(a) < 1 / np.finfo(a.dtype).eps
if not any(np.isnan(H.flatten())):
pp = np.array([principal_point[0], principal_point[1], 1.])
if is_invertible(H):
pp_proj = np.linalg.inv(H) @ pp
else:
pp_proj = np.linalg.pinv(H) @ pp
pp_proj /= pp_proj[-1]
plane_vector = pp_proj - cam_pos
return plane_vector, cam_pos
else:
return None, None
def is_in_front_of_plane(point, plane_normal, plane_point):
return np.dot(point - plane_point, plane_normal) > 0
def line_plane_intersection(p1, p2, plane_normal, plane_point, epsilon=0.5):
points_clipped = []
p1 = np.array(p1)
p2 = np.array(p2)
p1_f = is_in_front_of_plane(p1, plane_normal, plane_point)
p2_f = is_in_front_of_plane(p2, plane_normal, plane_point)
p_f = [p1_f, p2_f]
if not p1_f and not p2_f:
return points_clipped
if (p1_f and p2_f):
return [p1, p2]
for count, p in enumerate([p1, p2]):
if p_f[count]:
points_clipped.append(p)
else:
# Line direction vector
line_dir = p2 - p1
# Check if the line and plane are parallel
denom = np.dot(plane_normal, line_dir)
if np.isclose(denom, 0):
# Line and plane are parallel (no intersection or line is within the plane)
continue
# Calculate the value of t
t = np.dot(plane_normal, (plane_point - p1)) / denom
# Find the intersection point
intersection_point = p1 + t * line_dir
intersection_point += epsilon * plane_normal / np.linalg.norm(plane_normal)
points_clipped.append(intersection_point)
return points_clipped
def get_opt_vector(pos, rot):
position_meters = pos
rotation = rot
rot_vector, _ = cv2.Rodrigues(rotation)
return np.concatenate((position_meters, rot_vector.ravel()))
def vector_to_mtx(vector, mtx):
x_focal_length = mtx[0, 0]
y_focal_length = mtx[1, 1]
principal_point = (mtx[0, 2], mtx[1, 2])
position_meters = vector[:3]
rot_vector = np.array(vector[3:])
rotation, _ = cv2.Rodrigues(rot_vector)
It = np.eye(4)[:-1]
It[:, -1] = -position_meters
Q = np.array([[x_focal_length, 0, principal_point[0]],
[0, y_focal_length, principal_point[1]],
[0, 0, 1]])
P = Q @ (rotation @ It)
return P
def point_to_line_distance(l1, l2, p):
A = (l2[1] - l1[1])
B = (l2[0] - l1[0])
C = l2[0] * l1[1] - l2[1] * l1[0]
num = (A * p[0] - B * p[1] + C)
den = np.sqrt(A ** 2 + B ** 2)
if den > 0:
return num / den
else:
return 0
|