print("\t dP(landmark):")
print(dP(S(0,0), landmark))
print("\t P(observed_landmark): ", P(S(0,0), observed_landmark))
print("\t dP*dS:")
print(dP(S(0,0), landmark)*dS(0,0))
print("\t dP*dS (fvp):")
fvp = FeatureVisualPoint('fvp'+str(i))
fvp.xy.value = (P(S(0,0), landmark)[0], P(S(0,0), landmark)[1])
fvp.Z.value = np.inner(inverseHomogeneousMatrix(S(0,0)), landmark)[2]
plug(w_M_c.jacobian, fvp.Jq)
fvp.jacobian.recompute(T)
print(np.matrix(fvp.jacobian.value)[0:2,0:6])
l.signal(obsName + '_JfeatureReferencePosition').value = \
matrixToTuple(dP(S(0, 0), landmark))
l.signal(obsName + '_JsensorPosition').value = matrixToTuple(dS(0, 0))
l.signal(obsName + '_weight').value = (1., 1.)
l.signal(obsName + '_featureObservedPosition').value = \
tuple(P(S(0,0), observed_landmark).tolist())
l.signal(obsName + '_featureReferencePosition').value = \
tuple(P(S(0,0), landmark).tolist())
# Select (x, y, yaw) only!
l.signal(obsName + '_correctedDofs').value = correctedDofs
print("\n")
l.configurationOffset.recompute(T)
return (.5 * (xr - x) * (xr - x) + .5 * (yr - y) * (yr - y),)
def dS((x, y, theta), sensor):
return ((1, 0, r * cos(sensorPos[sensor] + theta)),
(0, 1, r * sin(sensorPos[sensor] + theta)))
def dP((x, y), l):
(xr, yr) = features[l]
return ((x - xr, y - yr,), )
for sensor in xrange(len(sensorPos)):
for i in xrange(len(features)):
prefix = 'obs_{0}_{1}'.format(sensor, i)
l.add_landmark_observation(prefix)
l.signal(prefix + '_JfeatureReferencePosition').value = dP(S(expectedRobot, sensor), i)
l.signal(prefix + '_JsensorPosition').value = dS(expectedRobot, sensor)
l.signal(prefix + '_weight').value = (1.,)
l.signal(prefix + '_featureObservedPosition').value = P(S(realRobot, sensor), i)
l.signal(prefix + '_featureReferencePosition').value = \
P(S(expectedRobot, sensor), i)
l.signal(prefix + '_correctedDofs').value = \
(1., 1., 1.)
l.configurationOffset.recompute(0)
for sensor in xrange(len(sensorPos)):