def make_distgauss_grad(mean, var):
def distgauss_grad((x1, y1), (x2, y2)):
dFdx1 = (((x1-x2)**2 + (y1-y2)**2)**(-0.5))*(x1-x2) if x1 != x2 or y1 != y2 else 0
dFdy1 = (((x1-x2)**2 + (y1-y2)**2)**(-0.5))*(y1-y2) if x1 != x2 or y1 != y2 else 0
dFdx2 = -(((x1-x2)**2 + (y1-y2)**2)**(-0.5))*(x1-x2) if x1 != x2 or y1 != y2 else 0
dFdy2 = -(((x1-x2)**2 + (y1-y2)**2)**(-0.5))*(x1-x2) if x1 != x2 or y1 != y2 else 0
dist = vec.dist((x1, y1), (x2, y2))
FM = (dist - mean)
dUdx1 = distgauss((x1, y1), (x2, y2)) * (FM/var) * dFdx1
dUdy1 = distgauss((x1, y1), (x2, y2)) * (FM/var) * dFdy1
dUdx2 = distgauss((x1, y1), (x2, y2)) * (FM/var) * dFdx2
dUdy2 = distgauss((x1, y1), (x2, y2)) * (FM/var) * dFdy2
return ((dUdx1, dUdy1), (dUdx2, dUdy2))
def make_distgauss_grad(mean, var):
def distgauss_grad((x1, y1), (x2, y2)):
dFdx1 = (
((x1 - x2)**2 +
(y1 - y2)**2)**(-0.5)) * (x1 - x2) if x1 != x2 or y1 != y2 else 0
dFdy1 = (
((x1 - x2)**2 +
(y1 - y2)**2)**(-0.5)) * (y1 - y2) if x1 != x2 or y1 != y2 else 0
dFdx2 = -(
((x1 - x2)**2 +
(y1 - y2)**2)**(-0.5)) * (x1 - x2) if x1 != x2 or y1 != y2 else 0
dFdy2 = -(
((x1 - x2)**2 +
(y1 - y2)**2)**(-0.5)) * (x1 - x2) if x1 != x2 or y1 != y2 else 0
dist = vec.dist((x1, y1), (x2, y2))
FM = (dist - mean)
dUdx1 = distgauss((x1, y1), (x2, y2)) * (FM / var) * dFdx1
dUdy1 = distgauss((x1, y1), (x2, y2)) * (FM / var) * dFdy1
dUdx2 = distgauss((x1, y1), (x2, y2)) * (FM / var) * dFdx2
dUdy2 = distgauss((x1, y1), (x2, y2)) * (FM / var) * dFdy2
return ((dUdx1, dUdy1), (dUdx2, dUdy2))
def distgauss_local(x1, x2):
return gaussPDF(vec.dist(x1, x2), mean, var)
def call(dists):
pairs = zip(dists, dists[1:])
diffs = map(lambda (x, y): vec.dist(x, y), pairs)
return gaussPDF(sum(diffs), 0, var) * soft_geq(mean)(
sum(dists) / float(len(dists)))
def call(dists):
pairs = zip(dists, dists[1:])
diffs = map(lambda (x, y): vec.dist(x, y), pairs)
return gaussPDF(sum(diffs), mean, var)
def gaussPDFRN(pos, meanvec, var):
return exp(-0.5 * ((vec.dist(pos, meanvec))**2) / var)
def distgauss(x1, x2):
return gaussPDF(vec.dist(x1, x2), DIST_MEAN, DIST_VAR)
else:
return 0
def distgauss_grad((x1, y1), (x2, y2)):
dFdx1 = (((x1 - x2)**2 +
(y1 - y2)**2)**(-0.5)) * (x1 - x2) if x1 != x2 or y1 != y2 else 0
dFdy1 = (((x1 - x2)**2 +
(y1 - y2)**2)**(-0.5)) * (y1 - y2) if x1 != x2 or y1 != y2 else 0
dFdx2 = -(
((x1 - x2)**2 +
(y1 - y2)**2)**(-0.5)) * (x1 - x2) if x1 != x2 or y1 != y2 else 0
dFdy2 = -(
((x1 - x2)**2 +
(y1 - y2)**2)**(-0.5)) * (x1 - x2) if x1 != x2 or y1 != y2 else 0
dist = vec.dist((x1, y1), (x2, y2))
FM = (dist - DIST_MEAN)
dUdx1 = distgauss((x1, y1), (x2, y2)) * (FM / DIST_VAR) * dFdx1
dUdy1 = distgauss((x1, y1), (x2, y2)) * (FM / DIST_VAR) * dFdy1
dUdx2 = distgauss((x1, y1), (x2, y2)) * (FM / DIST_VAR) * dFdx2
dUdy2 = distgauss((x1, y1), (x2, y2)) * (FM / DIST_VAR) * dFdy2
return ((dUdx1, dUdy1), (dUdx2, dUdy2))
def approx_signed_straigtness_grad((x1, y1), (x2, y2), (x3, y3)):
offset = 0.2
straightness_old = signed_straightness((x1, y1), (x2, y2), (x3, y3))
dSdx1 = (signed_straightness((x1 + offset, y1), (x2, y2),
(x3, y3)) - straightness_old) / offset
dSdy1 = (signed_straightness((x1, y1 + offset), (x2, y2),
(x3, y3)) - straightness_old) / offset
def distgauss(x1, x2):
return gaussPDF(vec.dist(x1, x2), DIST_MEAN, DIST_VAR)
def gaussPDFR2(pos, meanvec, var):
return exp(-0.5* ((vec.dist(pos, meanvec))**2) / var )
def distgauss_local(x1, x2):
return gaussPDF(vec.dist(x1, x2), mean, var)
def call(dists):
pairs = zip(dists, dists[1:])
diffs = map(lambda (x, y): vec.dist(x, y), pairs)
return gaussPDF(sum(diffs), 0, var) * soft_geq(mean)(sum(dists) / float(len(dists)))
def call(dists):
pairs = zip(dists, dists[1:])
diffs = map(lambda (x, y): vec.dist(x, y), pairs)
return gaussPDF(sum(diffs), mean, var)
vec32 = (xy2[0] - xy3[0], xy2[1] - xy3[1], 0)
if vec.length(vec12) > 0 and vec.length(vec32) > 0:
vec12 = vec.norm(vec12)
vec32 = vec.norm(vec32)
sign = 1 if vec.cross(vec12, vec32)[2] > 0 else -1
return sign * vec.length(vec.add(vec12, vec32))
else:
return 0
def distgauss_grad((x1, y1), (x2, y2)):
dFdx1 = (((x1-x2)**2 + (y1-y2)**2)**(-0.5))*(x1-x2) if x1 != x2 or y1 != y2 else 0
dFdy1 = (((x1-x2)**2 + (y1-y2)**2)**(-0.5))*(y1-y2) if x1 != x2 or y1 != y2 else 0
dFdx2 = -(((x1-x2)**2 + (y1-y2)**2)**(-0.5))*(x1-x2) if x1 != x2 or y1 != y2 else 0
dFdy2 = -(((x1-x2)**2 + (y1-y2)**2)**(-0.5))*(x1-x2) if x1 != x2 or y1 != y2 else 0
dist = vec.dist((x1, y1), (x2, y2))
FM = (dist - DIST_MEAN)
dUdx1 = distgauss((x1, y1), (x2, y2)) * (FM/DIST_VAR) * dFdx1
dUdy1 = distgauss((x1, y1), (x2, y2)) * (FM/DIST_VAR) * dFdy1
dUdx2 = distgauss((x1, y1), (x2, y2)) * (FM/DIST_VAR) * dFdx2
dUdy2 = distgauss((x1, y1), (x2, y2)) * (FM/DIST_VAR) * dFdy2
return ((dUdx1, dUdy1), (dUdx2, dUdy2))
def approx_signed_straigtness_grad((x1, y1), (x2, y2), (x3, y3)):
offset = 0.2
straightness_old = signed_straightness((x1, y1), (x2, y2), (x3, y3))
dSdx1 = (signed_straightness((x1+offset, y1), (x2, y2), (x3, y3))-straightness_old)/offset
dSdy1 = (signed_straightness((x1, y1+offset), (x2, y2), (x3, y3))-straightness_old)/offset
dSdx2 = (signed_straightness((x1, y1), (x2+offset, y2), (x3, y3))-straightness_old)/offset
dSdy2 = (signed_straightness((x1, y1), (x2, y2+offset), (x3, y3))-straightness_old)/offset
dSdx3 = (signed_straightness((x1, y1), (x2, y2), (x3+offset, y3))-straightness_old)/offset
