def CreateSimulatedCamera(x=1,
y=-1,
z=0.01,
roll=-92,
pitch=2,
yaw=50,
image_size=(1280, 1024),
f=0.015):
"""Create a Machine Vision Toolbox central camera model given 6 DoF pose, image size and f.
Args In:
x - position of camera in x-axis world frame (in metres)
y - position of camera in y-axis world frame (in metres)
z - position of camera in z-axis world frame (in metres)
roll - rotation of the camera about the x-axis world frame (in degrees)
pitch - rotation of the camera about the y-axis world frame (in degrees)
yaw - rotation of the camera about the z-axis world frame (in degrees)
image_size - two element tuple specifying the width and height of the image (in pixels)
f - focal length
Returns:
a camera model
"""
# Establish a camera position with respect to the world frame
# position
t_cam = np.r_[x, y, z]
# orientation
R = SO3.Rz(yaw, 'deg') * SO3.Ry(pitch, 'deg') * SO3.Rx(roll, 'deg')
# Create full transformation matrix
T = SE3(t_cam) * SE3.SO3(R)
# print(T)
# Create camera model
cam_model = CentralCamera(imagesize=image_size, f=f, pose=T)
return cam_model
def output(self, t=None):
T = SE3.SO3(self.pose.R, t=self.inputs[0])
return [T]
def convert(R):
return BASE * SE3.SO3(R)
def invcamcal(cls, C):
def rq(S):
# [R,Q] = vgg_rq(S) Just like qr but the other way around.
# If [R,Q] = vgg_rq(X), then R is upper-triangular, Q is orthogonal, and X==R*Q.
# Moreover, if S is a real matrix, then det(Q)>0.
# By awf
S = S.T
Q, U = scipy.linalg.qr(S[::-1, ::-1])
Q = Q.T
Q = Q[::-1, ::-1]
U = U.T
U = U[::-1, ::-1]
if np.linalg.det(Q) < 0:
U[:,0] = -U[:,0]
Q[0,:] = -Q[0,:]
return U, Q
if C.shape != (3,4):
raise ValueError('argument is not a 3x4 matrix')
u, s, v = scipy.linalg.svd(C)
t = v[3,:] # not svd returns v transpose
t = t / t[3]
t = t[0:3]
M = C[0:3,0:3]
# M = K * R
K, R = rq(M)
# deal with K having negative elements on the diagonal
# make a matrix to fix this, K*C has positive diagonal
C = np.diag(np.sign(np.diag(K)));
# now K*R = (K*C) * (inv(C)*R), so we need to check C is a proper rotation
# matrix. If isn't then the situation is unfixable
print(C)
if np.linalg.det(C) != 1:
raise ValueError('cannot correct signs in the intrinsic matrix')
# all good, let's fix it
K = K @ C
R = C.T @ R
# normalize K so that lower left is 1
K = K / K[2,2]
# pull out focal length and scale factors
f = K[0,0]
s = [1, K[1,1] / K[0,0]]
T = SE3(t) * SE3.SO3(R.T)
return cls(f=f, pp=K[0:2,2], rho=s, pose=T)
