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