def test_quat():
print "Testing Quat..."
q1 = Quat()
print " q1: {}".format(q1)
q2 = Quat(.5, .5, .5, .5)
v3 = (q1.x() + q2.x(), q1.y() + q2.y(), q1.z() + q2.z(), q1.w() + q2.w())
q3 = Quat(v3)
q4 = Quat(10)
v1 = Vec4(1, 2, 4, 5)
q5 = Quat(v1)
m1 = Quat.from_mat3(Mat3(0,-1,0, 1,0,0, 0,0,1))
q6 = Quat(m1)
q1 += q2
q2 -= q3
q3 *= 2
q4 /= 2
def getQuat(self):
returnValue = libpanda._inPkJyo8Dmu(self.this)
import Quat
returnObject = Quat.Quat(None)
returnObject.this = returnValue
if returnObject.this == 0:
return None
return returnObject
return
def __overloaded___mul___ptrConstLRotationf_ptrConstLQuaternionf(self, other):
returnValue = libpanda._inPUZN3dT59(self.this, other.this)
import Quat
returnObject = Quat.Quat(None)
returnObject.this = returnValue
if returnObject.this == 0:
return None
returnObject.userManagesMemory = 1
return returnObject
return
def solve_at_delay(
delay, Rws, tws, times_cam, a_imu, times_imu,
state,
cutoff=50, solve_bias=False,
its=50, eps=1e-6, plot=False):
'''
Delay camera times by delay amount and solve.
Modifies the entries of the state argument.
state: [g, b, s]
cutoff: Cuttoff frequency in Hz
its: Maximum iterations
eps: Minimum delta for convergence
'''
grav, bias, scale = state
# print("Delay:", delay)
# print("g, b, s:", grav, bias, scale)
# print("|g| =", norm(grav))
times_delay = times_cam - delay
s, e = 0, len(times_imu) - 1
while times_imu[s] < times_delay[0]:
s += 1
while times_imu[e] > times_delay[-1]:
e -= 1
times = times_imu[s:e]
a_imu = a_imu[s:e]
Rws_int = Quat.slerp1d(times, times_delay, Rws)
Rs_int = Rws_int.transpose(0,2,1)
N = len(times_delay)
P_cam = (times_delay[-1] - times_delay[0]) / (N - 1)
P_imu = (times[-1] - times[0]) / (e - s - 1)
# tws = Util.low_pass(tws, P_cam, cutoff, axis=0)
a_imu = Util.low_pass(a_imu, P_imu, cutoff, axis=0)
diff2 = 1/P_cam**2 * r_[1., -2., 1.]
aws = filters.convolve1d(tws, diff2, axis=0)
aws_int = Util.interp(times, times_delay, aws)
aw_cams = Util.low_pass(aws_int, P_imu, cutoff, axis=0)
J, r, w = init_jacobian(grav, bias, scale, Rs_int, aw_cams, a_imu, solve_bias=solve_bias, w_grav=1)
for it in range(its):
H, x = jacobian(J, r, w, grav, bias, scale, Rs_int, aw_cams, a_imu, solve_bias=solve_bias, w_grav=1)
u = -np.linalg.solve(H, x)
if np.isnan(u).any():
raise ValueError("NaNs in solution!")
grav += u[0:3]
scale += u[-1]
if solve_bias:
bias += u[3:6]
normu = norm(u)
if normu < eps:
break
# Visualize in nutmeg
F = len(aw_cams)
if plot:
fig = pynutmeg.figure('cam vs imu', 'figs/fig_triple.qml')
a_cams = empty_like(aw_cams)
for f in range(F):
R = Rs_int[f]
a_cams[f] = dot(R, grav) + bias + scale*dot(R, aw_cams[f])
titles = ['X', 'Y', 'Z']
for i in range(3):
ax = 'ax{}'.format(i)
camhandle = ax + '.P1'
imuhandle = ax + '.P2'
fig.set(ax + '.yAxis', label=titles[i] + ' Accel (m/s^2)')
fig.set(camhandle, x=times_cam, y=a_cams[:,i])
fig.set(imuhandle, x=times_cam, y=a_imu[:,i])
fig.set('ax0', minX=0, maxX=times_cam[-1])
fig.set('ax2.xAxis', label='Time (s)')
state[0] = grav
state[1] = bias
state[2] = scale
res = norm((r*w)[:3*F])
# print("Res:", res)
return res, state
