Sb = zeros([n,2,2]) Sc = zeros([n,2,2]) qa = zeros([n,4]) qb = zeros([n,4]) qc = zeros([n,4]) Pc = zeros([n,2,2]) p = zeros([n,2]) # collapsed point Pt = zeros([n,2,2]) # true variance sensor Rt = zeros([n,2,2]) # true rotation sensor St = zeros([n,2,2]) # true scale sensor it = zeros(n) for i in range(n): V1 = sm.cov2d(m(v1,i),sensor) V2 = sm.cov2d(m(v2,i),sensor) V3 = sm.cov2d(m(v3,i),sensor) V4 = sm.cov2d(m(v4,i),sensor) Q1 = computeQ(m(v1,i),m(v2,i)) Q2 = computeQ(m(v2,i),m(v3,i)) Q3 = computeQ(m(v3,i),m(v4,i)) Qinv = linalg.inv(Q1 + 2.*Q2 + Q3) p[i] = (Qinv[:-1,-1]/Qinv[-1,-1]).T Pt[i] = sm.cov2d(m(p,i),sensor) Rt[i],St[i] = decomposeCov(Pt[i]) R1,S1 = decomposeCov(V1) R2,S2 = decomposeCov(V2)
return linalg.inv(C1 + C2 + N)[:, -1] def weight(S1, S2): return 2000.0 * (S1.trace() + S2.trace()) sensor = sm.SensorModel() v1 = mat([-0.4, 2.3]).T v2 = mat([0.4, 2.3]).T v3 = mat([0.9, 1.8]).T v4 = mat([0.9, 1.4]).T points = array(hstack([v1, v2, v3, v4])).T V1 = mat(sm.cov2d(v1, sensor)) V2 = mat(sm.cov2d(v2, sensor)) V3 = mat(sm.cov2d(v3, sensor)) V4 = mat(sm.cov2d(v4, sensor)) R1, S1 = decomposeCov(V1) R2, S2 = decomposeCov(V2) R3, S3 = decomposeCov(V3) R4, S4 = decomposeCov(V4) mu = zeros([10, 2]) for i in range(10): b1 = computeBeta(aff(v1), aff(v2), 10.0 ** -i) b2 = computeBeta(aff(v2), aff(v3), 10.0 ** -i) b3 = computeBeta(aff(v3), aff(v4), 10.0 ** -i) Qinv = linalg.inv(b1 * b1.T + 2.0 * b2 * b2.T + b3 * b3.T) mu[i] = (Qinv[:-1, -1] / Qinv[-1, -1]).T.A