def J_comp():
# kk,r,c = cv2.decomposeProjectionMatrix(p)
var_k = sp.Matrix([[f,0,px],[0,f,py],[0,0,1]])
var_R = sp.Matrix([[r11,r12,r13],[r21,r22,r23],[r31,r32,r33]])
var_c = sp.Matrix([cx,cy,cz]).T
var_x = sp.Matrix([x1,y1,z1]).T
var_q = sp.Matrix([qw,qx,qy,qz])
fn_R = sp.Quaternion(qw,qx,qy,qz)
fn_R = fn_R.to_rotation_matrix()
# pprint.pprint(fn_R)
proj = var_k @ var_R
# print(proj)
var_xc = var_x - var_c
# print(var_xc)
w = np.dot(proj[2,:], var_xc.T)[0,0]
w = 1/w
f_rcx = sp.Matrix([(proj[0,:]*var_xc.T), (proj[1,:]*var_xc.T)]) * w
f_rcx = sp.Matrix(np.array(f_rcx[:]).reshape(2,1))
# print(f_rcx)
var_R = var_R.reshape(1,9)
var_c = var_c.reshape(1,3)
var_x = var_x.reshape(1,3)
# print(var_R)
dfdr = f_rcx.jacobian(var_R)
dfdc = f_rcx.jacobian(var_c)
dfdx = f_rcx.jacobian(var_x)
# print(dfdr.shape)
# print(dfdc.shape)
# print(dfdx.shape)
var_q = var_q.reshape(1,4)
fn_R = fn_R.reshape(9,1)
drdq = fn_R.jacobian(var_q)
dfdq = dfdr @ drdq
# print(dfdq.shape)
# print(drdq.shape)
# vars = sp.Matrix([[var_R.reshape(1,9),var_c.reshape(1,3),var_x.reshape(1,3)]])
# vars = sp.Matrix(np.array(vars[:]).reshape(1,15)[0])
# # print(vars)
# Jac = f_rcx.jacobian(vars)
# print(Jac.shape)
return dfdq,dfdc,dfdx,f_rcx
import sympy as sp
import numpy as np
quat_vars = sp.symbols('q:4')
theta_vars = sp.symbols('theta:3')
g = sp.Quaternion(*quat_vars) * sp.Quaternion(1, *theta_vars)
g_mat = sp.Matrix([getattr(g, att) for att in 'abcd'])
G = g_mat.jacobian(theta_vars)
for i in range(1):
quat_this = sp.Quaternion(*np.random.uniform(-1, 1, 4)).normalize()
subdict = dict(zip(quat_vars, [getattr(quat_this, att) for att in 'abcd']))
G_this = G.subs(subdict)
P_this = sp.randMatrix(3, 3)
P_this = P_this * P_this.T
Pq = G_this * sp.randMatrix(3, 3) * G_this.T
tx0, ty0, tz0 = sp.symbols("tx0, ty0, tz0", real=True) # translation at t=0
tx1, ty1, tz1 = sp.symbols("tx1, ty1, tz1", real=True) # translation at t=1
qx0, qy0, qz0, qw0 = sp.symbols("qx0, qy0, qz0, qw0",
real=True) # quaternion at t=0
qx1, qy1, qz1, qw1 = sp.symbols("qx1, qy1, qz1, qw1",
real=True) # quaternion at t=1
# coefficients for upper triangular matrices
s000, s001, s002, s003, s011, s012, s013, s022, s023 = sp.symbols(
"s000, s001, s002, s003, s011, s012, s013, s022, s023", real=True)
s100, s101, s102, s103, s111, s112, s113, s122, s123 = sp.symbols(
"s100, s101, s102, s103, s111, s112, s113, s122, s123", real=True)
q0 = sp.Quaternion(qw0, qx0, qy0, qz0)
q1 = sp.Quaternion(qw1, qx1, qy1, qz1)
# assuming that q1 is qperp = normalize(q1-q0*cosTheta), where cosTheta=dot(q0, q1) and theta = acos(cosTheta).
# this simplifies the terms of the symbolic expressions later
qt = q0 * sp.cos(t * theta) + q1 * sp.sin(t * theta)
S0 = sp.Matrix([[s000, s001, s002, s003], [0, s011, s012, s013],
[0, 0, s022, s023], [0, 0, 0, 1]])
S1 = sp.Matrix([[s100, s101, s102, s103], [0, s111, s112, s113],
[0, 0, s122, s123], [0, 0, 0, 1]])
D0 = sp.Matrix([[1, 0, 0, tx0], [0, 1, 0, ty0], [0, 0, 1, tz0], [0, 0, 0, 1]])
D1 = sp.Matrix([[1, 0, 0, tx1], [0, 1, 0, ty1], [0, 0, 1, tz1], [0, 0, 0, 1]])
p0 = sp.Matrix([px0, py0, pz0, 1])
p1 = sp.Matrix([px1, py1, pz1, 1])
c_phi2, c_theta2, c_psi2 = [sp.cos(i) for i in half_angles]
s_phi2, s_theta2, s_psi2 = [sp.sin(i) for i in half_angles]
quaternion = sp.Array(
[
c_phi2 * c_theta2 * c_psi2 + s_phi2 * s_theta2 * s_psi2,
s_phi2 * c_theta2 * c_psi2 - c_phi2 * s_theta2 * s_psi2,
c_phi2 * s_theta2 * c_psi2 + s_phi2 * c_theta2 * s_psi2,
c_phi2 * c_theta2 * s_psi2 - s_phi2 * s_theta2 * c_psi2,
]
)
return quaternion
def quaternion_derivative(quaterion, omega):
omega_quat = Quaternion(0, *omega)
q_dot = (1/2) * quaterion * omega_quat
return [getattr(q_dot, field) for field in 'abcd']
pi = sp.symbols('pi')
q = Quaternion(0, 1, 0, 0).normalize()
omega = sp.Matrix([0, pi, 0])
omega_quat = Quaternion(0, *omega)
q_dot = (1/2) * q * omega_quat
euler = quaternion_to_euler(q)
q2 = euler_to_quaternion(euler)
derivative = quaternion_derivative(
sp.Quaternion(0, 1, 0, 0), [0, sp.symbols('pi'), 0])