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
