def formatParameters(rVec, tVec, linearCoeffs, distCoeffs):
params = Parameters()
if prod(rVec.shape) == 9:
rVec = Rodrigues(rVec)[0]
rVec = rVec.reshape(3)
for i in range(3):
params.add('rvec%d'%i,
value=rVec[i], vary=True)
params.add('tvec%d'%i,
value=tVec[i], vary=True)
params.add('cameraMatrix0',
value=linearCoeffs[0,0], vary=False)
params.add('cameraMatrix1',
value=linearCoeffs[1,1], vary=False)
params.add('cameraMatrix2',
value=linearCoeffs[0,2], vary=False)
params.add('cameraMatrix3',
value=linearCoeffs[1,2], vary=False)
for i in range(4):
params.add('distCoeffs%d'%i,
value=distCoeffs[i,0], vary=False)
return params
def transform_frame(vec, orient, att, frame_id):
vec = numpy.array(vec)
if frame_id == 'aruco_map':
# from 'aruco_map' to 'main_camera'
rmat, j = Rodrigues(orient)
vec.transpose()
vec = rmat.dot(vec)
vec.transpose()
elif frame_id == 'main_camera':
pass
# from 'main_camera' to 'fcu'
a = camera_angle
vec[0], vec[1], vec[2] = vec[1] * math.cos(a) + vec[2] * math.sin(
a), -vec[0], vec[2] * math.cos(a) - vec[1] * math.sin(a)
# from 'fcu' to 'fcu_horiz'
roll, pitch = att[0], att[1]
rmat = euler_matrix(roll, pitch, 0, 'rxyz')
rmat = rmat[0:3, 0:3]
vec.transpose()
vec = rmat.dot(vec)
vec.transpose()
return list(vec)
def formatParameters(rVec, tVec, linearCoeffs, distCoeffs):
params = Parameters()
if prod(rVec.shape) == 9:
rVec = Rodrigues(rVec)[0]
rVec = rVec.reshape(3)
for i in range(3):
params.add('rvec%d'%i,
value=rVec[i], vary=True)
params.add('tvec%d'%i,
value=tVec[i], vary=True)
# image center
params.add('cameraMatrix0',
value=linearCoeffs[0], vary=False)
params.add('cameraMatrix1',
value=linearCoeffs[1], vary=False)
# l and m
params.add('distCoeffs0',
value=distCoeffs[0], vary=False)
params.add('distCoeffs1',
value=distCoeffs[1], vary=False)
return params
def get_params_from_stereo_vision(img_data_camera1, img_data_camera2,\
focals=None, data_size = 200):
"""
get internal and external parameters from stereo vision
"""
op_x = None
if img_data_camera1.shape[-1] > data_size:
img_c1_computeF = img_data_camera1[:, :data_size]
img_c2_computeF = img_data_camera2[:, :data_size]
else:
img_c1_computeF = img_data_camera1
img_c2_computeF = img_data_camera2
F = compute_fundamental_normalized(img_c1_computeF, img_c2_computeF)
if not focals:
#print(img_data_camera1, img_data_camera2)
f0, f1 = compute_focal_len(F)
else:
f0, f1 = focals
focal_min = default_focal - 5
focal_max = default_focal + 5
if not (f0 > focal_min and f0 < focal_max and f1 > focal_min
and f1 < focal_max):
f0 = f1 = default_focal
print("Focals: ", f0, f1)
K1 = np.array([[f0, 0, 0], [0, f0, 0], [0, 0, 1]])
K2 = np.array([[f1, 0, 0], [0, f1, 0], [0, 0, 1]])
# X1 = img_data_camera1[:, 0:600]
# X2 = img_data_camera2[:, 0:600]
# try:
# bestE = ransac_five(X1, X2, K1, K2, 1000, 0.0005)
## print("Points Index: \r\n", bestE[0])
# E_matrix = bestE[1]
## print("Best essential from five point algorithm: \r\n", E_matrix)
# except InvalidDenomError:
# E_matrix = compute_E_from_fundamental(F, K1, K2)
## print("Essential from fundenmental matrix: \r\n", E_matrix)
E_matrix = compute_E_from_fundamental(F, K1, K2)
R_t = compute_unique_R_t_from_essential(K1, K2, E_matrix,\
img_data_camera1, img_data_camera2, 1.0)
R_v = Rodrigues(R_t[0])[0].T
t_v = R_t[1]
init_x = []
if not focals:
init_x.extend([f0, f1])
init_x.extend(R_v.tolist()[0])
init_x.extend(t_v.tolist())
# print("init params: ", init_x)
op_x = optimize_params_two_camera(init_x, img_data_camera1, \
img_data_camera2, len_relation, focals)
# print("optimize params: ", op_x)
return op_x
def plotFrameRefBase(ax, rVec, tVec, s, *args, **kwargs):
'''
graficar la terna de vectores de un marco de referencia
mutation_scale=20, lw=1, arrowstyle="-|>", color="k"
'''
R = Rodrigues(rVec)[0]
# orig coords en world
tWorld = -inv(R).dot(tVec)
x, y, z = tWorld
XYZ = R * s + tWorld
for i in range(3):
# pongo un punto en cada lugar para que el plot me lo tome al escalear
ax.plot([x, XYZ[i, 0]], [y, XYZ[i, 1]], [z, XYZ[i, 2]],
'.',
markersize=0)
eje = Arrow3D([x, XYZ[i, 0]], [y, XYZ[i, 1]], [z, XYZ[i, 2]],
mutation_scale=10,
lw=1,
arrowstyle="-|>",
color="k")
ax.add_artist(eje)
def inverse(imageCorners, rVec, tVec, linearCoeffs, distCoeffs):
if rVec.shape != (3, 3):
rVec, _ = Rodrigues(rVec)
xpp = ((imageCorners[:, 0, 0] - linearCoeffs[0, 2]) / linearCoeffs[0, 0])
ypp = ((imageCorners[:, 0, 1] - linearCoeffs[1, 2]) / linearCoeffs[1, 1])
rpp = sqrt(xpp**2 + ypp**2)
# polynomial coeffs, grade 7
# # (k1,k2,p1,p2[,k3[,k4,k5,k6[,s1,s2,s3,s4[,τx,τy]]]])
poly = [
[
distCoeffs[4, 0], # k3
-r * distCoeffs[7, 0], # k6
distCoeffs[1, 0], # k2
-r * distCoeffs[6, 0], # k5
distCoeffs[0, 0], # k1
-r * distCoeffs[5, 0], # k4
1,
-r
] for r in rpp
]
rootsPoly = [roots(p) for p in poly]
rp_rpp = array([roo[isreal(roo)].real[0] for roo in rootsPoly]) / rpp
xp = xpp * rp_rpp
yp = ypp * rp_rpp
# project to z=0 plane. perhaps calculate faster with homography function?
XYZ = xypToZplane(xp, yp, rVec, tVec)
return XYZ
def lose_func(x, img_pts0, img_pts1, len_relation, focals=None):
"""
lost functions for optimize the params during the stereo calibration
"""
if focals:
f0, f1 = focals
r1x, r1y, r1z, t1x, t1y, t1z = x
else:
f0, f1, r1x, r1y, r1z, t1x, t1y, t1z = x
weight_dist_e = 40 #the weight of distance error
weight_delta_e = 10
weight_img_e = 1 #
K0 = np.diag([f0, scale_hw*f0, 1.0])
K1 = np.diag([f1, scale_hw*f1, 1.0])
proj_M0 = np.column_stack((K0, [0, 0, 0]))
R1 = np.array([r1x, r1y, r1z])
R_M1 = Rodrigues(R1)[0] #vector to matrix
#R_M1 = np.array([[r11, r12, r13], [r21, r22, r23], [r31, r32, r33]])
t_V1 = np.array([ t1x, t1y, t1z])
R_t_M1 = np.column_stack((R_M1, t_V1))
proj_M1 = K1.dot(R_t_M1)
stand_lens = np.array([len_relation["Red_Green"],\
len_relation["Red_Blue"], \
len_relation["Blue_Green"]])
g1, g2 = compute_2points_len_error(img_pts0, img_pts1, proj_M0, proj_M1, stand_lens)
g3 = compute_points_image_error(img_pts0, img_pts1, K0, K1, R_M1, t_V1.tolist())
return np.concatenate((weight_dist_e*g1, weight_delta_e*g2, weight_img_e*g3),axis=0)
def formatParameters(rVec, tVec, linearCoeffs, distCoeffs):
'''
puts all intrinsic and extrinsic parameters in Parameters() format
if there are several extrinsica paramters they are correctly
Inputs
rVec, tVec : arrays of shape (n,3,1) or (3,1)
distCoeffs must have be reshapable to (5)
'''
params = Parameters()
if prod(rVec.shape) == 9:
rVec = Rodrigues(rVec)[0]
rVec = rVec.reshape(3)
for i in range(3):
params.add('rvec%d'%i,
value=rVec[i], vary=False)
params.add('tvec%d'%i,
value=tVec[i], vary=False)
if len(rVec.shape)==3:
for j in range(len(rVec)):
for i in range(3):
params.add('rvec%d%d'%(j,i),
value=rVec[j,i,0], vary=False)
params.add('tvec%d%d'%(j,i),
value=tVec[j,i,0], vary=False)
params.add('fX',
value=linearCoeffs[0,0], vary=False)
params.add('fY',
value=linearCoeffs[1,1], vary=False)
params.add('cX',
value=linearCoeffs[0,2], vary=False)
params.add('cY',
value=linearCoeffs[1,2], vary=False)
# (k1,k2,p1,p2[,k3[,k4,k5,k6[,s1,s2,s3,s4[,τx,τy]]]])
distCoeffs = distCoeffs.reshape(5)
for i in range(5):
params.add('distCoeffs%d'%i,
value=distCoeffs[i], vary=False)
return params
def __init__(self, e_x: float, e_y: float, e_z: float):
"""
Construct a quaternion given the components of a rotation vector.
More information: https://w.wiki/Fci
"""
rotation_matrix, _ = Rodrigues((e_x, e_y, e_z))
self._quaternion = Quaternion(matrix=rotation_matrix)
def RotationMatrix2AxisAngle(rotation_matrix):
"""
Args:
rotation_matrix (Mat33d):
Returns:
axis_angle (Vec3d):
"""
axis_angle = Rodrigues(rotation_matrix)
return axis_angle
def inverseRationalOwn(corners, rotMatrix, tVec, cameraMatrix, distCoeffs):
'''
inverseRational(objPoints, rVec/rotMatrix, tVec, cameraMatrix, distCoeffs)
-> objPoints
takes corners in image and returns corrdinates in scene
function programed to compare with opencv
corners must be of size (n,1,2)
objPoints has size (1,n,3)
'''
if rotMatrix.shape != (3,3):
rotMatrix, _ = Rodrigues(rotMatrix)
xpp = (corners[:,0,0]-cameraMatrix[0,2]) / cameraMatrix[0,0]
ypp = (corners[:,0,1]-cameraMatrix[1,2]) / cameraMatrix[1,1]
rpp = np.sqrt(xpp**2 + ypp**2)
# polynomial coeffs
# # (k1,k2,p1,p2[,k3[,k4,k5,k6[,s1,s2,s3,s4[,τx,τy]]]])
poly = [[distCoeffs[4,0],
-r*distCoeffs[7,0],
distCoeffs[1,0],
-r*distCoeffs[6,0],
distCoeffs[0,0],
-r*distCoeffs[5,0],
1,
-r] for r in rpp]
# roots = np.roots(p)
roots = [np.roots(p) for p in poly]
# # max radious possible
# rppMax = np.sqrt((cameraMatrix[0,2] / cameraMatrix[0,0])**2 +
# (cameraMatrix[1,2] / cameraMatrix[1,1])**2)
# rp_rpp = roots[np.isreal(roots)].real[0] / rpp # asume real positive root, in interval
rp_rpp = np.array([roo[np.isreal(roo)].real[0] for roo in roots]) / rpp
xp = xpp * rp_rpp
yp = ypp * rp_rpp
# auxiliar calculations
a = rotMatrix[0,0] - rotMatrix[2,0] * xp
b = rotMatrix[0,1] - rotMatrix[2,1] * xp
c = tVec[0,0] - tVec[2,0] * xp
d = rotMatrix[1,0] - rotMatrix[2,0] * yp
e = rotMatrix[1,1] - rotMatrix[2,1] * yp
f = tVec[1,0] - tVec[2,0] * yp
q = a*e-d*b
X = -(c*e - f*b)/q # check why wrong sign, why must put '-' in front?
Y = -(f*a - c*d)/q
shape = (1,corners.shape[0],3)
XYZ = np.array([X, Y, np.zeros(shape[1])]).T
XYZ = np.reshape(XYZ, shape)
return XYZ
def face_model_to_origin(self, landmarks, camera):
rotation_vector, translation_vector = self.find_extrinsic(
landmarks, camera)
rotation_matrix = Rodrigues(rotation_vector)[0]
vectors = (rotation_matrix @ self.model_points.T +
translation_vector).T
return camera.vectors_to_origin(vectors)
def rot_matrix_error(R0, R1, method = 'unit_quaternion_product'):
""" R0, R1 are 3x3 or 4x4 homogeneous Rotation matrixes
returns: the value of the error depending on the method """
if ((R0.shape != (4,4)) and (R0.shape != (3,3))):
print ("Error in the R0 input rotation matrix shape, must be 3x3 or 4x4")
print R0
return -1
if ((R1.shape != (4,4)) and (R1.shape != (3,3))):
print ("Error in the R1 input rotation matrix shape, must be 3x3 or 4x4")
print R1
return -1
if R0.shape == (3,3):
R = np.eye(4)
R[:3,:3] = R0
R0 = R
if R1.shape == (3,3):
R = np.eye(4)
R[:3,:3] = R1
R1 = R
if(method == 'unit_quaternion_product' ):
## From the paper "Metrics for 3D Rotations: Comparison and Analysis" D. Huynh
# The 3D rotation error is computed using the inner product of unit quaterions
#We use the ros library TF to convert rotation matrix into unit quaternions
from tf import transformations
q0 = transformations.quaternion_from_matrix(R0)
q1 = transformations.quaternion_from_matrix(R1)
# We convert into unit quaternions
q0 = q0 / np.linalg.norm(q0)
q1 = q1 / np.linalg.norm(q1)
#Find the error as defined in the paper
rot_error = 1 - np.linalg.norm(np.dot(q0,q1))
if(method == 'angle'):
#option 2 find the angle of this rotation. In particular, the above is invalid
#for very large error angles (error > 90 degrees) and is imprecise for large
#error angles (angle > 45 degrees).
E = R1.dot(R0.T)
from cv2 import Rodrigues
rot_vector, J = Rodrigues(E[:3,:3])
angle = np.linalg.norm(rot_vector)
rot_error = np.rad2deg(angle)
return rot_error
def AxisAngle2RotationMatrix(axis_angle):
"""
Args:
axis_angle (Vec3d):
Returns:
rot_matrix (Mat33d):
"""
rot_matrix = Rodrigues(axis_angle)
return rot_matrix
def Euler2AxisAngle(euler):
"""
Args:
euler (Vec3d):
Returns:
axis_angle (float):
"""
rot_matrix = Euler2RotationMatrix(euler)
axis_angle = Rodrigues(rot_matrix)
return rot_matrix
def formatParameters(rVec, tVec, linearCoeffs, distCoeffs):
'''
puts all intrinsic and extrinsic parameters in Parameters() format
if there are several extrinsica paramters they are correctly
Inputs
rVec, tVec : arrays of shape (n,3,1) or (3,1)
distCoeffs must have be reshapable to (5)
'''
params = Parameters()
if prod(rVec.shape) == 9:
rVec = Rodrigues(rVec)[0]
rVec = rVec.reshape(3)
for i in range(3):
params.add('rvec%d' % i, value=rVec[i], vary=False)
params.add('tvec%d' % i, value=tVec[i], vary=False)
if len(rVec.shape) == 3:
for j in range(len(rVec)):
for i in range(3):
params.add('rvec%d%d' % (j, i),
value=rVec[j, i, 0],
vary=False)
params.add('tvec%d%d' % (j, i),
value=tVec[j, i, 0],
vary=False)
params.add('fX', value=linearCoeffs[0, 0], vary=False)
params.add('fY', value=linearCoeffs[1, 1], vary=False)
params.add('cX', value=linearCoeffs[0, 2], vary=False)
params.add('cY', value=linearCoeffs[1, 2], vary=False)
# (k1,k2,p1,p2[,k3[,k4,k5,k6[,s1,s2,s3,s4[,τx,τy]]]])
distCoeffs = distCoeffs.reshape(5)
for i in range(5):
params.add('distCoeffs%d' % i, value=distCoeffs[i], vary=False)
return params
def direct(fiducialPoints, rVec, tVec, linearCoeffs, distCoeffs):
# format as matrix
if rVec.shape != (3, 3):
rVec, _ = Rodrigues(rVec)
xyz = rVec.dot(fiducialPoints[0].T) + tVec
xp = xyz[0] / xyz[2]
yp = xyz[1] / xyz[2]
rp2 = xp**2 + yp**2
aux = (distCoeffs[0] + distCoeffs[1]) / (1 + distCoeffs[0] * sqrt(1 + rp2))
xpp = xp * aux
ypp = yp * aux
u = xpp + linearCoeffs[0]
v = ypp + linearCoeffs[1]
return array([u, v]).reshape((fiducialPoints.shape[1], 1, 2))
def direct(fiducialPoints, rVec, tVec, linearCoeffs, distCoeffs):
# format as matrix
if rVec.shape != (3, 3):
rVec, _ = Rodrigues(rVec)
xyz = rVec.dot(fiducialPoints[0].T) + tVec
xp = xyz[0] / xyz[2]
yp = xyz[1] / xyz[2]
rp = sqrt(xp**2 + yp**2)
thetap = arctan(rp)
rpp = distCoeffs * tan(thetap / 2)
rpp_rp = rpp / rp
xpp = xp * rpp_rp
ypp = yp * rpp_rp
u = xpp + linearCoeffs[0]
v = ypp + linearCoeffs[1]
return array([u, v]).reshape((fiducialPoints.shape[1], 1, 2))
def pose2rot(pose):
'''
use Rodrigues transformation to turn pose vector to rotation matrix
args:
pose: [num_examples, 72], unraveled version of the pose vector in axis angle representation (24*3)
returns:
rot_all: rot matrix, [num_examples, 216], (216=24*3*3)
'''
from cv2 import Rodrigues
num_examples = pose.shape[0]
pose = pose.reshape(num_examples, -1, 3)
rot_all = [
np.array([Rodrigues(pp[i, :])[0] for i in range(pp.shape[0])]).ravel()
for pp in pose
]
rot_all = np.array(rot_all)
return rot_all
def xyhToZplane(xh, yh, rV, tV, Ch=False, Crt=False):
'''
projects a point from homogenous undistorted to 3D asuming z=0
'''
xh, yh, rV, tV = [f64(xh), f64(yh), f64(rV), f64(tV)]
if prod(rV.shape) == 3:
R = Rodrigues(rV)[0]
# auxiliar calculations
a = R[0, 0] - R[2, 0] * xh
b = R[0, 1] - R[2, 1] * xh
c = tV[0] - tV[2] * xh
d = R[1, 0] - R[2, 0] * yh
e = R[1, 1] - R[2, 1] * yh
f = tV[1] - tV[2] * yh
q = a * e - d * b
xm = (f * b - c * e) / q
ym = (c * d - f * a) / q
Chbool = anny(Ch)
Crtbool = anny(Crt)
if Chbool or Crtbool: # no hay incertezas
Cm = zeros((xm.shape[0], 2, 2), dtype=float64)
# calculo jacobianos
JXm_Xp, JXm_rtV = jacobianosHom2Map(xh, yh, rV, tV)
if Crtbool: # contribucion incerteza Crt
JXm_rtVResh = JXm_rtV.reshape((2, 6, 1, 1, -1))
Cm += (JXm_rtVResh * Crt.reshape(
(1, 6, 6, 1, 1)) * JXm_rtVResh.transpose((3, 2, 1, 0, 4))).sum(
(1, 2)).transpose((2, 0, 1))
if Chbool: # incerteza Ch
JXm_XpResh = JXm_Xp.reshape((2, 2, 1, 1, -1))
Cm += (JXm_XpResh * Ch.T.reshape(
(1, 2, 2, 1, -1)) * JXm_XpResh.transpose((3, 2, 1, 0, 4))).sum(
(1, 2)).transpose((2, 0, 1))
else:
Cm = False # return None covariance
return xm, ym, Cm
def rot2pose(rot):
'''
use Rodrigues transformation to turn rotation matrices into pose vector
args:
rot: [num_examples, 216], unraveled version of the 3x3 rot matrix (216=24 joints * 3*3)
returns:
pose_vec: pose vector [num_examples, 72], pose vector in axis angle representation (72=24*3)
'''
from cv2 import Rodrigues
num_examples = rot.shape[0]
rot = rot.reshape(num_examples, -1, 9)
pose_vec = [
np.array(
[Rodrigues(rr[i, :].reshape(3, 3))[0]
for i in range(rr.shape[0])]).ravel() for rr in rot
]
pose_vec = np.array(pose_vec)
return pose_vec
def xypToZplane(xp, yp, rV, tV, Cp=False, Crt=False):
'''
projects a point from homogenous undistorted to 3D asuming z=0
'''
if prod(rV.shape) == 3:
R = Rodrigues(rV)[0]
# auxiliar calculations
a = R[0, 0] - R[2, 0] * xp
b = R[0, 1] - R[2, 1] * xp
c = tV[0] - tV[2] * xp
d = R[1, 0] - R[2, 0] * yp
e = R[1, 1] - R[2, 1] * yp
f = tV[1] - tV[2] * yp
q = a * e - d * b
xm = (f * b - c * e) / q
ym = (c * d - f * a) / q
Cpbool = anny(Cp)
Crtbool = anny(Crt)
if Cpbool or Crtbool: # no hay incertezas
Cm = zeros((xm.shape[0], 2, 2))
# calculo jacobianos
JXm_Xp, JXm_rtV = jacobianosHom2Map(xp, yp, rV, tV)
if Crtbool: # contribucion incerteza Crt
JXm_rtVResh = JXm_rtV.reshape((2, 6, 1, 1, -1))
Cm += (JXm_rtVResh * Crt.reshape(
(1, 6, 6, 1, 1)) * JXm_rtVResh.transpose((3, 2, 1, 0, 4))).sum(
(1, 2)).transpose((2, 0, 1))
if Cpbool: # incerteza Cp
Cp = Cp.transpose((1, 2, 0))
JXm_XpResh = JXm_Xp.reshape((2, 2, 1, 1, -1))
Cm += (JXm_XpResh * Cp.reshape(
(1, 2, 2, 1, -1)) * JXm_XpResh.transpose((3, 2, 1, 0, 4))).sum(
(1, 2)).transpose((2, 0, 1))
else:
Cm = False # return None covariance
return xm, ym, Cm
def get_op_params(camera1,
camera2,
camera_list,
multi_c_p,
params_db,
is_get_f_from_db=False):
"""
get optimize params,
R_T: camera2 coordinate system to camera1 coordinate system
"""
c1_idx, c2_idx = camera_list.index(camera1), camera_list.index(camera2)
common_pts_m = np.row_stack(
(multi_c_p.mask[c1_idx, :], multi_c_p.mask[c2_idx, :]))
common_pts = np.where((common_pts_m[0, :] == 1)
& (common_pts_m[1, :] == 1))[0]
img_data1 = multi_c_p.img_filter_cameras[c1_idx, :, common_pts].T
img_data2 = multi_c_p.img_filter_cameras[c2_idx, :, common_pts].T
if is_get_f_from_db:
f0 = params_db.get_data(camera1 + "_int")
f1 = params_db.get_data(camera2 + "_int")
if f0 and f1:
op_x = get_params_from_stereo_vision(img_data1, img_data2, \
(f0, f1))
else:
op_x = get_params_from_stereo_vision(img_data1, img_data2)
params_db.store_data(camera2 + "_int", op_x[1])
# print("store int params:", camera2, op_x[1])
else:
op_x = get_params_from_stereo_vision(img_data1, img_data2)
params_db.store_data(camera2 + "_int", op_x[1])
# print("store int params:", camera2, op_x[1])
f0 = op_x[0]
f1 = op_x[1]
R_M = Rodrigues(np.array(op_x[2:5]))[0]
t = np.array(np.array(op_x[5:]))
R_T = np.column_stack((R_M, t))
return op_x, f0, f1, R_T
def inverse(imageCorners, rVec, tVec, linearCoeffs, distCoeffs):
# format as matrix
if rVec.shape != (3, 3):
rVec, _ = Rodrigues(rVec)
xpp = imageCorners[:, 0, 0] - linearCoeffs[0]
ypp = imageCorners[:, 0, 1] - linearCoeffs[1]
lm = distCoeffs[0] * (distCoeffs[0] + distCoeffs[1])
aux1 = (lm / xpp - distCoeffs[0])**2
aux2 = (lm / ypp - distCoeffs[0])**2
denom = (aux1 - 1) * (aux2 - 1) - 1
xp = xpp * aux1 / denom
yp = ypp * aux2 / denom
# project to z=0 plane. perhaps calculate faster with homography function?
XYZ = xypToZplane(xp, yp, rVec, tVec)
return XYZ
def inverse(imageCorners, rVec, tVec, linearCoeffs, distCoeffs):
# format as matrix
if rVec.shape != (3, 3):
rVec, _ = Rodrigues(rVec)
xpp = imageCorners[:, 0, 0] - linearCoeffs[0]
ypp = imageCorners[:, 0, 1] - linearCoeffs[1]
rpp = sqrt(xpp**2 + ypp**2)
thetap = 2 * arctan(rpp / distCoeffs)
rp = tan(thetap)
rp_rpp = rp / rpp
xp = xpp * rp_rpp
yp = ypp * rp_rpp
# project to z=0 plane. perhaps calculate faster with homography function?
XYZ = xypToZplane(xp, yp, rVec, tVec)
return XYZ
def inverse(imageCorners, rVec, tVec, linearCoeffs, distCoeffs):
if rVec.shape != (3, 3):
rVec, _ = Rodrigues(rVec)
xpp = ((imageCorners[:, 0, 0] - linearCoeffs[0, 2]) / linearCoeffs[0, 0])
ypp = ((imageCorners[:, 0, 1] - linearCoeffs[1, 2]) / linearCoeffs[1, 1])
rpp = sqrt(xpp**2 + ypp**2)
# polynomial coeffs, grade 9
# # (k1,k2,k3,k4)
poly = [
[
distCoeffs[3, 0], # k4
0,
distCoeffs[2, 0], # k3
0,
distCoeffs[1, 0], # k2
0,
distCoeffs[0, 0], # k1
1,
-r
] for r in rpp
]
rootsPoly = [roots(p) for p in poly]
thetap = array([roo[isreal(roo)].real[0] for roo in rootsPoly]) / rpp
rp = tan(thetap)
rp_rpp = rp / rpp
xp = xpp * rp_rpp
yp = ypp * rp_rpp
# project to z=0 plane. perhaps calculate faster with homography function?
XYZ = xypToZplane(xp, yp, rVec, tVec)
return XYZ
def convert_pose(vect):
M, _ = Rodrigues(np.array(vect).astype(np.float32))
Zv = M[:, 2]
theta = np.arctan2(Zv[0], Zv[2])
phi = np.arcsin(Zv[1])
return np.array([theta, phi])
def fiducialComparison3D(rVec, tVec, fiducail1, fiducial2 = False, label1 = 'Fiducial points', label2 = 'Projected points'):
t = tVec[:,0]
# calcular las puntas de los versores
if rVec.shape == (3,3):
[x,y,z] = rVec
else:
[x,y,z], _ = Rodrigues(rVec)
[x,y,z] = [x,y,z] + t
X1 = fiducail1[0,:,0]
Y1 = fiducail1[0,:,1]
Z1 = fiducail1[0,:,2]
fig = figure()
ax = fig.gca(projection='3d')
ax.plot(X1,Y1,Z1,'xr', markersize=10, label=label1)
if type(fiducial2) is ndarray:
X2 = fiducial2[0,:,0]
Y2 = fiducial2[0,:,1]
Z2 = fiducial2[0,:,2]
ax.plot(X2,Y2,Z2,'+b', markersize=10, label=label2)
xmin = min([0, min(X1), min(X2), min([t[0],x[0],y[0],z[0]])])
ymin = min([0, min(Y1), min(Y2), min([t[1],x[1],y[1],z[1]])])
zmin = min([0, min(Z1), min(Z2), min([t[2],x[2],y[2],z[2]])])
xmax = max([0, max(X1), max(X2), max([t[0],x[0],y[0],z[0]])])
ymax = max([0, max(Y1), max(Y2), max([t[1],x[1],y[1],z[1]])])
zmax = max([0, max(Z1), max(Z2), max([t[2],x[2],y[2],z[2]])])
else:
xmin = min([0, min(X1), min([t[0],x[0],y[0],z[0]])])
ymin = min([0, min(Y1), min([t[1],x[1],y[1],z[1]])])
zmin = min([0, min(Z1), min([t[2],x[2],y[2],z[2]])])
xmax = max([0, max(X1), max([t[0],x[0],y[0],z[0]])])
ymax = max([0, max(Y1), max([t[1],x[1],y[1],z[1]])])
zmax = max([0, max(Z1), max([t[2],x[2],y[2],z[2]])])
ax.set_xbound(xmin, xmax)
ax.set_ybound(ymin, ymax)
ax.set_zbound(zmin, zmax)
ejeX = Arrow3D([0, 1],
[0, 0],
[0, 0],
mutation_scale=20, lw=1, arrowstyle="-|>", color="k")
ejeY = Arrow3D([0, 0],
[0, 1],
[0, 0],
mutation_scale=20, lw=1, arrowstyle="-|>", color="k")
ejeZ = Arrow3D([0, 0],
[0, 0],
[0, 1],
mutation_scale=20, lw=1, arrowstyle="-|>", color="k")
origen = Arrow3D([0, t[0]],
[0, t[1]],
[0, t[2]],
mutation_scale=20, lw=1, arrowstyle="-", color="k", linestyle="dashed")
ejeXc = Arrow3D([t[0], x[0]],
[t[1], x[1]],
[t[2], x[2]],
mutation_scale=20, lw=1, arrowstyle="-|>", color="b")
ejeYc = Arrow3D([t[0], y[0]],
[t[1], y[1]],
[t[2], y[2]],
mutation_scale=20, lw=1, arrowstyle="-|>", color="b")
ejeZc = Arrow3D([t[0], z[0]],
[t[1], z[1]],
[t[2], z[2]],
mutation_scale=20, lw=3, arrowstyle="-|>", color="b")
ax.add_artist(origen)
ax.add_artist(ejeX)
ax.add_artist(ejeY)
ax.add_artist(ejeZ)
ax.add_artist(ejeXc)
ax.add_artist(ejeYc)
ax.add_artist(ejeZc)
ax.legend()
show()
def create_rotation_matrix(rotation):
"""(1, 3) -> (3, 3)"""
return Rodrigues(rotation)[0]
def directRational(objectPoints,
rotMatrix,
tVec,
cameraMatrix,
distCoeffs,
plot=False,
img=None,
corners=None):
'''
directRational(objPoints, rVec/rotMatrix, tVec, cameraMatrix, distCoeffs,
plot=False)
-> corners
takes points in 3D scene coordinates and maps into distorted image
objPoints must have size (1,n,3)
corners is of size (n,1,2)
if plot is enabled, it will plot several intermediate results
'''
if rotMatrix.shape != (3, 3):
rotMatrix, _ = Rodrigues(rotMatrix)
# chequear esta linea a ver si se esta haciendo bien
xyz = [
np.linalg.linalg.dot(rotMatrix, p) + tVec[:, 0]
for p in objectPoints[0]
]
xyz = np.array(xyz).T
# plot rototranslation
if plot:
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
[x, y, z] = rotMatrix
fig1 = plt.figure()
ax1 = fig1.gca(projection='3d')
ax1.plot([0, tVec[0, 0]], [0, tVec[1, 0]], [0, tVec[2, 0]], 'k')
ax1.plot([tVec[0, 0], tVec[0, 0] + x[0]],
[tVec[1, 0], tVec[1, 0] + x[1]],
[tVec[2, 0], tVec[2, 0] + x[2]], 'g')
ax1.plot([tVec[0, 0], tVec[0, 0] + y[0]],
[tVec[1, 0], tVec[1, 0] + y[1]],
[tVec[2, 0], tVec[2, 0] + y[2]], 'r')
ax1.plot([tVec[0, 0], tVec[0, 0] + z[0]],
[tVec[1, 0], tVec[1, 0] + z[1]],
[tVec[2, 0], tVec[2, 0] + z[2]], 'b')
ax1.scatter(objectPoints[0, :, 0], objectPoints[0, :, 1],
objectPoints[0, :, 2])
ax1.plot([0, 1], [0, 0], [0, 0], 'g')
ax1.plot([0, 0], [0, 1], [0, 0], 'r')
ax1.plot([0, 0], [0, 0], [0, 1], 'b')
ax1.scatter(xyz[0], xyz[1], xyz[2])
# to homogenous coords
xpyp = xyz[:2] / xyz[2]
if plot:
ax1.scatter(xpyp[0], xpyp[1], 1)
fig2 = plt.figure()
ax2 = fig2.gca()
ax2.scatter(xpyp[0],
xpyp[1]) # plot in z=1 plane, homogenous coordinates
rp2 = np.sum(xpyp[:2]**2, 0)
rp4 = rp2**2
rp6 = rp2 * rp4
# polynomial coeffs
# # (k1,k2,p1,p2[,k3[,k4,k5,k6[,s1,s2,s3,s4[,τx,τy]]]])
q = (1 + distCoeffs[0,0]*rp2 + distCoeffs[1,0]*rp4 + distCoeffs[4,0]*rp6) / \
(1 + distCoeffs[5,0]*rp2 + distCoeffs[6,0]*rp4 + distCoeffs[7,0]*rp6)
if plot:
# for plotting the distortion
rp = np.sqrt(rp2)
fig3 = plt.figure()
ax3 = fig3.gca()
ax3.scatter(rp, rp * q)
ax3.plot([0, 0.6], [0, 0.6])
# distort
xppypp = xpyp * q
if plot:
ax2.scatter(xppypp[0], xppypp[1])
# linearly project to image
uv = [cameraMatrix[0, 0], cameraMatrix[1, 1]] * xppypp.T + cameraMatrix[:2,
2]
if plot:
# plot distorted positions
fig4 = plt.figure(4)
ax4 = fig4.gca()
ax4.imshow(img)
ax4.scatter(corners[:, 0, 0], corners[:, 0, 1])
ax4.scatter(uv.T[0], uv.T[1])
distortedProyection = uv.reshape([np.shape(uv)[0], 1, 2])
return distortedProyection
img2 = aruco.drawDetectedMarkers(img, corners, ids)
h, w, cha = img2.shape
rvecs, tvecs, objpoints = aruco.estimatePoseSingleMarkers(
corners, sidel, mtx, dist)
try:
if (ids[0] == 23):
index23 = 0
index24 = 1
elif (ids[0] == 24):
index23 = 1
index24 = 0
aruco.drawAxis(img2, mtx, dist, rvecs[index23, 0],
tvecs[index23, 0], sidel / 2)
aruco.drawAxis(img2, mtx, dist, rvecs[index24, 0],
tvecs[index24, 0], sidel / 2)
Rmat23, jacob23 = Rodrigues(rvecs[index23, 0])
Rmat24, jacob24 = Rodrigues(rvecs[index24, 0])
R23inv = np.linalg.inv(Rmat23)
R24inv = np.linalg.inv(Rmat24)
co24to23 = np.matmul(R23inv,
tvecs[index24, 0] - tvecs[index23, 0])
distance = co24to23
if (firstpass == True):
offset = distance
firstpass = False
distance = distance - offset
tempdist = sqrt(distance[0] * distance[0] +
distance[1] * distance[1] +
distance[2] * distance[2])
totdistance.append(distance[1])
finaldistx.append(distance[0])
