Module calib3d.tf1.tf1_calib
Expand source code
import cv2
import tensorflow as tf
from calib3d import Calib
# pylint: disable=unexpected-keyword-arg, no-value-for-parameter, too-many-function-args
class TensorflowCalib():
def __init__(self, *, width, height, T=None, K, kc, r=None, R=None, Kinv=None, Pinv=None, P=None, dtype=tf.float32):
self.width = tf.cast(width, dtype=dtype)
self.height = tf.cast(height, dtype=dtype)
self.K = tf.cast(K, dtype=dtype)
self.T = tf.cast(T, dtype=dtype)
self.r = tf.cast(r, dtype=dtype)
self.R = tf.cast(R, dtype=dtype) if R is not None else rodrigues_batch(r, dtype=dtype)
self.P = tf.cast(P, dtype=dtype) if P is not None else tf.matmul(self.K, tf.concat((self.R, self.T), axis=-1))
self.Pinv = tf.cast(Pinv, dtype=dtype) if Pinv is not None else pinv(self.P, dtype=dtype)
self.Kinv = tf.cast(Kinv, dtype=dtype) if Kinv is not None else pinv(self.K, dtype=dtype)
self.kc = tf.cast(kc, dtype=dtype) if kc is not None else None
self.C = -tf.matmul(self.R, self.T, transpose_a=True) # pylint: disable=invalid-unary-operand-type
self.batch_size = K.shape[0]
self.dtype = dtype
@classmethod
def from_numpy(cls, calib: Calib, dtype=tf.float64):
return cls(
K=tf.constant(calib.K, dtype=dtype)[tf.newaxis],
r=tf.constant(cv2.Rodrigues(calib.R)[0], dtype=dtype)[tf.newaxis],
T=tf.constant(calib.T, dtype=dtype)[tf.newaxis],
width=tf.constant(calib.width, dtype=dtype)[tf.newaxis],
height=tf.constant(calib.height, dtype=dtype)[tf.newaxis],
kc=tf.constant(calib.kc, dtype=dtype)[tf.newaxis],
dtype=dtype
)
def project_3D_to_2D(self, point3D):
point2D = from_homogenous(tf.matmul(self.P, to_homogenous(point3D, dtype=self.dtype)))
# TODO: avoid distort points too much outside the image as the distortion model is not perfect
return self.distort(point2D)
def project_2D_to_3D(self, point2D, Z):
point2D = self.rectify(point2D)
X = from_homogenous(tf.matmul(self.Pinv, to_homogenous(point2D, dtype=self.dtype)))
d = (X - self.C)
P = batch_expand(tf.constant([[0],[0],[Z]], dtype=self.dtype), d)
n = batch_expand(tf.constant([[0],[0],[1]], dtype=self.dtype), d)
return find_intersection(self.C, d, P, n)
def distort(self, point2D):
if self.kc is None:
return point2D
point2D = from_homogenous(tf.matmul(self.Kinv, to_homogenous(point2D, dtype=self.dtype)))
rad1, rad2, tan1, rad3, tan2 = self.kc[:,0], self.kc[:,1], self.kc[:,2], self.kc[:,3], self.kc[:,4]
r2 = point2D[:,0]*point2D[:,0] + point2D[:,1]*point2D[:,1]
delta = 1 + rad1[:,tf.newaxis]*r2 + rad2[:,tf.newaxis]*r2*r2 + rad3[:,tf.newaxis]*r2*r2*r2
dx = tf.stack((
2*tan1[:,tf.newaxis]*point2D[:,0]*point2D[:,1] + tan2[:,tf.newaxis]*(r2 + 2*point2D[:,0]*point2D[:,0]),
2*tan2[:,tf.newaxis]*point2D[:,0]*point2D[:,1] + tan1[:,tf.newaxis]*(r2 + 2*point2D[:,1]*point2D[:,1])
), axis=1)
point2D = point2D*delta + dx
return from_homogenous(tf.matmul(self.K, to_homogenous(point2D, dtype=self.dtype)))
def rectify(self, point2D):
if self.kc is None:
return point2D
point2D = from_homogenous(tf.matmul(self.Kinv, to_homogenous(point2D, dtype=self.dtype)))
rad1, rad2, tan1, rad3, tan2 = self.kc[:,0], self.kc[:,1], self.kc[:,2], self.kc[:,3], self.kc[:,4]
r2 = point2D[:,0]*point2D[:,0] + point2D[:,1]*point2D[:,1]
delta = 1 + rad1[:,tf.newaxis]*r2 + rad2[:,tf.newaxis]*r2*r2 + rad3[:,tf.newaxis]*r2*r2*r2
dx = tf.stack((
2*tan1[:,tf.newaxis]*point2D[:,0]*point2D[:,1] + tan2[:,tf.newaxis]*(r2 + 2*point2D[:,0]*point2D[:,0]),
2*tan2[:,tf.newaxis]*point2D[:,0]*point2D[:,1] + tan1[:,tf.newaxis]*(r2 + 2*point2D[:,1]*point2D[:,1])
), axis=1)
point2D = (point2D - dx)/delta[:,tf.newaxis]
return from_homogenous(tf.matmul(self.K, to_homogenous(point2D, dtype=self.dtype)))
def batch_expand(input_tensor, batch_tensor):
# https://stackoverflow.com/questions/57716363/explicit-broadcasting-of-variable-batch-size-tensor
length = len(batch_tensor.shape)-1
input_tensor = input_tensor[tf.newaxis]
broadcast_shape = tf.where([True, *[False]*length], tf.shape(batch_tensor), tf.shape(input_tensor))
return tf.broadcast_to(input_tensor, broadcast_shape)
def to_homogenous(points, dtype=tf.float64):
_,_,N = points.shape
ones = tf.ones((1,N), dtype=dtype)
ones = batch_expand(ones, points)
return tf.concat((points, ones), axis=-2)
def from_homogenous(points):
return points[:,:-1,:]/points[:,-1:,:]
def pinv(a, rcond=1e-15, dtype=tf.float64):
s, u, v = tf.svd(a)
# Ignore singular values close to zero to prevent numerical overflow
limit = rcond * tf.reduce_max(s)
non_zero = tf.greater(s, limit)
reciprocal = tf.where(non_zero, tf.cast(tf.reciprocal(s), dtype=dtype), tf.zeros_like(s))
lhs = tf.matmul(v, tf.matrix_diag(reciprocal))
return tf.matmul(lhs, u, transpose_b=True)
def find_intersection(C, d, P, n):
d = d/tf.norm(d, axis=-2, keepdims=True)
dist = tf.tensordot(P-C, n, axes=[[1],[1]])[:,:,0] / tf.tensordot(d, n, axes=[[1],[1]])[:,tf.newaxis,:,0,0] # Distance between plane z=Z and camera
return C + dist*d
# https://github.com/blzq/tf_rodrigues/blob/master/rodrigues.py
def rodrigues_batch(rvecs, dtype=tf.float64):
""" Convert a batch of axis-angle rotations in rotation vector form shaped
(batch, 3) to a batch of rotation matrices shaped (batch, 3, 3).
See
https://en.wikipedia.org/wiki/Rodrigues%27_rotation_formula#Matrix_notation
https://en.wikipedia.org/wiki/Rotation_matrix#Rotation_matrix_from_axis_and_angle
"""
rvecs = tf.cast(rvecs, dtype=dtype)
batch_size = tf.shape(rvecs)[0]
assert rvecs.shape[1] == 3
thetas = tf.norm(rvecs, axis=1, keepdims=True)
is_zero = tf.equal(tf.squeeze(thetas), 0.0)
u = rvecs / thetas
# Each K is the cross product matrix of unit axis vectors
# pyformat: disable
zero = tf.zeros([batch_size], dtype=dtype) # for broadcasting
Ks_1 = tf.stack([ zero , -u[:, 2], u[:, 1] ], axis=1) # row 1
Ks_2 = tf.stack([ u[:, 2], zero , -u[:, 0] ], axis=1) # row 2
Ks_3 = tf.stack([ -u[:, 1], u[:, 0], zero ], axis=1) # row 3
# pyformat: enable
Ks = tf.stack([Ks_1, Ks_2, Ks_3], axis=1) # stack rows
Rs = tf.eye(3, batch_shape=[batch_size], dtype=dtype) + \
tf.sin(thetas)[..., tf.newaxis] * Ks + \
(1 - tf.cos(thetas)[..., tf.newaxis]) * tf.matmul(Ks, Ks)
# Avoid returning NaNs where division by zero happened
return tf.where(is_zero, tf.eye(3, batch_shape=[batch_size], dtype=dtype), Rs)
Functions
def batch_expand(input_tensor, batch_tensor)
-
Expand source code
def batch_expand(input_tensor, batch_tensor): # https://stackoverflow.com/questions/57716363/explicit-broadcasting-of-variable-batch-size-tensor length = len(batch_tensor.shape)-1 input_tensor = input_tensor[tf.newaxis] broadcast_shape = tf.where([True, *[False]*length], tf.shape(batch_tensor), tf.shape(input_tensor)) return tf.broadcast_to(input_tensor, broadcast_shape)
def find_intersection(C, d, P, n)
-
Expand source code
def find_intersection(C, d, P, n): d = d/tf.norm(d, axis=-2, keepdims=True) dist = tf.tensordot(P-C, n, axes=[[1],[1]])[:,:,0] / tf.tensordot(d, n, axes=[[1],[1]])[:,tf.newaxis,:,0,0] # Distance between plane z=Z and camera return C + dist*d
def from_homogenous(points)
-
Expand source code
def from_homogenous(points): return points[:,:-1,:]/points[:,-1:,:]
def pinv(a, rcond=1e-15, dtype=tf.float64)
-
Expand source code
def pinv(a, rcond=1e-15, dtype=tf.float64): s, u, v = tf.svd(a) # Ignore singular values close to zero to prevent numerical overflow limit = rcond * tf.reduce_max(s) non_zero = tf.greater(s, limit) reciprocal = tf.where(non_zero, tf.cast(tf.reciprocal(s), dtype=dtype), tf.zeros_like(s)) lhs = tf.matmul(v, tf.matrix_diag(reciprocal)) return tf.matmul(lhs, u, transpose_b=True)
def rodrigues_batch(rvecs, dtype=tf.float64)
-
Convert a batch of axis-angle rotations in rotation vector form shaped (batch, 3) to a batch of rotation matrices shaped (batch, 3, 3). See https://en.wikipedia.org/wiki/Rodrigues%27_rotation_formula#Matrix_notation https://en.wikipedia.org/wiki/Rotation_matrix#Rotation_matrix_from_axis_and_angle
Expand source code
def rodrigues_batch(rvecs, dtype=tf.float64): """ Convert a batch of axis-angle rotations in rotation vector form shaped (batch, 3) to a batch of rotation matrices shaped (batch, 3, 3). See https://en.wikipedia.org/wiki/Rodrigues%27_rotation_formula#Matrix_notation https://en.wikipedia.org/wiki/Rotation_matrix#Rotation_matrix_from_axis_and_angle """ rvecs = tf.cast(rvecs, dtype=dtype) batch_size = tf.shape(rvecs)[0] assert rvecs.shape[1] == 3 thetas = tf.norm(rvecs, axis=1, keepdims=True) is_zero = tf.equal(tf.squeeze(thetas), 0.0) u = rvecs / thetas # Each K is the cross product matrix of unit axis vectors # pyformat: disable zero = tf.zeros([batch_size], dtype=dtype) # for broadcasting Ks_1 = tf.stack([ zero , -u[:, 2], u[:, 1] ], axis=1) # row 1 Ks_2 = tf.stack([ u[:, 2], zero , -u[:, 0] ], axis=1) # row 2 Ks_3 = tf.stack([ -u[:, 1], u[:, 0], zero ], axis=1) # row 3 # pyformat: enable Ks = tf.stack([Ks_1, Ks_2, Ks_3], axis=1) # stack rows Rs = tf.eye(3, batch_shape=[batch_size], dtype=dtype) + \ tf.sin(thetas)[..., tf.newaxis] * Ks + \ (1 - tf.cos(thetas)[..., tf.newaxis]) * tf.matmul(Ks, Ks) # Avoid returning NaNs where division by zero happened return tf.where(is_zero, tf.eye(3, batch_shape=[batch_size], dtype=dtype), Rs)
def to_homogenous(points, dtype=tf.float64)
-
Expand source code
def to_homogenous(points, dtype=tf.float64): _,_,N = points.shape ones = tf.ones((1,N), dtype=dtype) ones = batch_expand(ones, points) return tf.concat((points, ones), axis=-2)
Classes
class TensorflowCalib (*, width, height, T=None, K, kc, r=None, R=None, Kinv=None, Pinv=None, P=None, dtype=tf.float32)
-
Expand source code
class TensorflowCalib(): def __init__(self, *, width, height, T=None, K, kc, r=None, R=None, Kinv=None, Pinv=None, P=None, dtype=tf.float32): self.width = tf.cast(width, dtype=dtype) self.height = tf.cast(height, dtype=dtype) self.K = tf.cast(K, dtype=dtype) self.T = tf.cast(T, dtype=dtype) self.r = tf.cast(r, dtype=dtype) self.R = tf.cast(R, dtype=dtype) if R is not None else rodrigues_batch(r, dtype=dtype) self.P = tf.cast(P, dtype=dtype) if P is not None else tf.matmul(self.K, tf.concat((self.R, self.T), axis=-1)) self.Pinv = tf.cast(Pinv, dtype=dtype) if Pinv is not None else pinv(self.P, dtype=dtype) self.Kinv = tf.cast(Kinv, dtype=dtype) if Kinv is not None else pinv(self.K, dtype=dtype) self.kc = tf.cast(kc, dtype=dtype) if kc is not None else None self.C = -tf.matmul(self.R, self.T, transpose_a=True) # pylint: disable=invalid-unary-operand-type self.batch_size = K.shape[0] self.dtype = dtype @classmethod def from_numpy(cls, calib: Calib, dtype=tf.float64): return cls( K=tf.constant(calib.K, dtype=dtype)[tf.newaxis], r=tf.constant(cv2.Rodrigues(calib.R)[0], dtype=dtype)[tf.newaxis], T=tf.constant(calib.T, dtype=dtype)[tf.newaxis], width=tf.constant(calib.width, dtype=dtype)[tf.newaxis], height=tf.constant(calib.height, dtype=dtype)[tf.newaxis], kc=tf.constant(calib.kc, dtype=dtype)[tf.newaxis], dtype=dtype ) def project_3D_to_2D(self, point3D): point2D = from_homogenous(tf.matmul(self.P, to_homogenous(point3D, dtype=self.dtype))) # TODO: avoid distort points too much outside the image as the distortion model is not perfect return self.distort(point2D) def project_2D_to_3D(self, point2D, Z): point2D = self.rectify(point2D) X = from_homogenous(tf.matmul(self.Pinv, to_homogenous(point2D, dtype=self.dtype))) d = (X - self.C) P = batch_expand(tf.constant([[0],[0],[Z]], dtype=self.dtype), d) n = batch_expand(tf.constant([[0],[0],[1]], dtype=self.dtype), d) return find_intersection(self.C, d, P, n) def distort(self, point2D): if self.kc is None: return point2D point2D = from_homogenous(tf.matmul(self.Kinv, to_homogenous(point2D, dtype=self.dtype))) rad1, rad2, tan1, rad3, tan2 = self.kc[:,0], self.kc[:,1], self.kc[:,2], self.kc[:,3], self.kc[:,4] r2 = point2D[:,0]*point2D[:,0] + point2D[:,1]*point2D[:,1] delta = 1 + rad1[:,tf.newaxis]*r2 + rad2[:,tf.newaxis]*r2*r2 + rad3[:,tf.newaxis]*r2*r2*r2 dx = tf.stack(( 2*tan1[:,tf.newaxis]*point2D[:,0]*point2D[:,1] + tan2[:,tf.newaxis]*(r2 + 2*point2D[:,0]*point2D[:,0]), 2*tan2[:,tf.newaxis]*point2D[:,0]*point2D[:,1] + tan1[:,tf.newaxis]*(r2 + 2*point2D[:,1]*point2D[:,1]) ), axis=1) point2D = point2D*delta + dx return from_homogenous(tf.matmul(self.K, to_homogenous(point2D, dtype=self.dtype))) def rectify(self, point2D): if self.kc is None: return point2D point2D = from_homogenous(tf.matmul(self.Kinv, to_homogenous(point2D, dtype=self.dtype))) rad1, rad2, tan1, rad3, tan2 = self.kc[:,0], self.kc[:,1], self.kc[:,2], self.kc[:,3], self.kc[:,4] r2 = point2D[:,0]*point2D[:,0] + point2D[:,1]*point2D[:,1] delta = 1 + rad1[:,tf.newaxis]*r2 + rad2[:,tf.newaxis]*r2*r2 + rad3[:,tf.newaxis]*r2*r2*r2 dx = tf.stack(( 2*tan1[:,tf.newaxis]*point2D[:,0]*point2D[:,1] + tan2[:,tf.newaxis]*(r2 + 2*point2D[:,0]*point2D[:,0]), 2*tan2[:,tf.newaxis]*point2D[:,0]*point2D[:,1] + tan1[:,tf.newaxis]*(r2 + 2*point2D[:,1]*point2D[:,1]) ), axis=1) point2D = (point2D - dx)/delta[:,tf.newaxis] return from_homogenous(tf.matmul(self.K, to_homogenous(point2D, dtype=self.dtype)))
Static methods
def from_numpy(calib: Calib, dtype=tf.float64)
-
Expand source code
@classmethod def from_numpy(cls, calib: Calib, dtype=tf.float64): return cls( K=tf.constant(calib.K, dtype=dtype)[tf.newaxis], r=tf.constant(cv2.Rodrigues(calib.R)[0], dtype=dtype)[tf.newaxis], T=tf.constant(calib.T, dtype=dtype)[tf.newaxis], width=tf.constant(calib.width, dtype=dtype)[tf.newaxis], height=tf.constant(calib.height, dtype=dtype)[tf.newaxis], kc=tf.constant(calib.kc, dtype=dtype)[tf.newaxis], dtype=dtype )
Methods
def distort(self, point2D)
-
Expand source code
def distort(self, point2D): if self.kc is None: return point2D point2D = from_homogenous(tf.matmul(self.Kinv, to_homogenous(point2D, dtype=self.dtype))) rad1, rad2, tan1, rad3, tan2 = self.kc[:,0], self.kc[:,1], self.kc[:,2], self.kc[:,3], self.kc[:,4] r2 = point2D[:,0]*point2D[:,0] + point2D[:,1]*point2D[:,1] delta = 1 + rad1[:,tf.newaxis]*r2 + rad2[:,tf.newaxis]*r2*r2 + rad3[:,tf.newaxis]*r2*r2*r2 dx = tf.stack(( 2*tan1[:,tf.newaxis]*point2D[:,0]*point2D[:,1] + tan2[:,tf.newaxis]*(r2 + 2*point2D[:,0]*point2D[:,0]), 2*tan2[:,tf.newaxis]*point2D[:,0]*point2D[:,1] + tan1[:,tf.newaxis]*(r2 + 2*point2D[:,1]*point2D[:,1]) ), axis=1) point2D = point2D*delta + dx return from_homogenous(tf.matmul(self.K, to_homogenous(point2D, dtype=self.dtype)))
def project_2D_to_3D(self, point2D, Z)
-
Expand source code
def project_2D_to_3D(self, point2D, Z): point2D = self.rectify(point2D) X = from_homogenous(tf.matmul(self.Pinv, to_homogenous(point2D, dtype=self.dtype))) d = (X - self.C) P = batch_expand(tf.constant([[0],[0],[Z]], dtype=self.dtype), d) n = batch_expand(tf.constant([[0],[0],[1]], dtype=self.dtype), d) return find_intersection(self.C, d, P, n)
def project_3D_to_2D(self, point3D)
-
Expand source code
def project_3D_to_2D(self, point3D): point2D = from_homogenous(tf.matmul(self.P, to_homogenous(point3D, dtype=self.dtype))) # TODO: avoid distort points too much outside the image as the distortion model is not perfect return self.distort(point2D)
def rectify(self, point2D)
-
Expand source code
def rectify(self, point2D): if self.kc is None: return point2D point2D = from_homogenous(tf.matmul(self.Kinv, to_homogenous(point2D, dtype=self.dtype))) rad1, rad2, tan1, rad3, tan2 = self.kc[:,0], self.kc[:,1], self.kc[:,2], self.kc[:,3], self.kc[:,4] r2 = point2D[:,0]*point2D[:,0] + point2D[:,1]*point2D[:,1] delta = 1 + rad1[:,tf.newaxis]*r2 + rad2[:,tf.newaxis]*r2*r2 + rad3[:,tf.newaxis]*r2*r2*r2 dx = tf.stack(( 2*tan1[:,tf.newaxis]*point2D[:,0]*point2D[:,1] + tan2[:,tf.newaxis]*(r2 + 2*point2D[:,0]*point2D[:,0]), 2*tan2[:,tf.newaxis]*point2D[:,0]*point2D[:,1] + tan1[:,tf.newaxis]*(r2 + 2*point2D[:,1]*point2D[:,1]) ), axis=1) point2D = (point2D - dx)/delta[:,tf.newaxis] return from_homogenous(tf.matmul(self.K, to_homogenous(point2D, dtype=self.dtype)))