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))
Example #2
0
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))
Example #3
0
 def distgauss_local(x1, x2):
     return gaussPDF(vec.dist(x1, x2), mean, var)
Example #4
0
 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)))
Example #5
0
 def call(dists):
     pairs = zip(dists, dists[1:])
     diffs = map(lambda (x, y): vec.dist(x, y), pairs)
     return gaussPDF(sum(diffs), mean, var)
Example #6
0
def gaussPDFRN(pos, meanvec, var):
    return exp(-0.5 * ((vec.dist(pos, meanvec))**2) / var)
Example #7
0
def distgauss(x1, x2):
    return gaussPDF(vec.dist(x1, x2), DIST_MEAN, DIST_VAR)
Example #8
0
    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