Example #1
0
def fun_2(this, rankId=0):
    """
    @params: rankInd = 0 for the MAP solution
    Computes the indexes of the particles for the ranked solution, and set the body model accordingly
    Returns the log posterior (or negative energy)
    """

    best = np.zeros(this.nNodes)

    for partId in this.nodeIdx:
        I_sort = np.argsort(-1*this.b[partId]['value'], axis=None).copy()
        this.mapParticleInd[partId] = I_sort[rankId]
        best[partId] = this.b[partId]['value'][I_sort[rankId]]

    if this.verbose > 1:
        print 'MAP particles'
        print this.mapParticleInd

    # Set the object
    for i in this.nodeIdx:
        x = this.b[i]['x'][:, this.mapParticleInd[i]]
        ra, ta, za, zsa = particles.get_part_params(this.particleIdx[i], x)
        this.body.Zp[i] = za
        this.body.Zs[i] = zsa
        this.body.r_abs[i,:] = ra
        this.body.t[i,:] = ta

    i = this.torso
    p = this.mapParticleInd[i]
    for c in range(this.nCameras):
        this.camera[c][0:3] = this.b[i]['x'][this.camIdx[i][c][0:3],p]
        this.camera[c][3:6] = this.b[i]['x'][this.camIdx[i][c][3:6],p]
        this.camera[c][6] = this.b[i]['x'][this.camIdx[i][c][6],p]
        this.camera[c][7:9] = this.b[i]['x'][this.camIdx[i][c][7:9],p]
        this.camera[c][9:14]  = np.zeros(5)

    logB = best[this.torso]
    return logB
Example #2
0
def fun_2(this, rankId=0):
    """
    @params: rankInd = 0 for the MAP solution
    Computes the indexes of the particles for the ranked solution, and set the body model accordingly
    Returns the log posterior (or negative energy)
    """

    best = np.zeros(this.nNodes)

    for partId in this.nodeIdx:
        I_sort = np.argsort(-1 * this.b[partId]["value"], axis=None).copy()
        this.mapParticleInd[partId] = I_sort[rankId]
        best[partId] = this.b[partId]["value"][I_sort[rankId]]

    if this.verbose > 1:
        print "MAP particles"
        print this.mapParticleInd

    # Set the object
    for i in this.nodeIdx:
        x = this.b[i]["x"][:, this.mapParticleInd[i]]
        ra, ta, za, zsa = particles.get_part_params(this.particleIdx[i], x)
        this.body.Zp[i] = za
        this.body.Zs[i] = zsa
        this.body.r_abs[i, :] = ra
        this.body.t[i, :] = ta

    i = this.torso
    p = this.mapParticleInd[i]
    for c in range(this.nCameras):
        this.camera[c][0:3] = this.b[i]["x"][this.camIdx[i][c][0:3], p]
        this.camera[c][3:6] = this.b[i]["x"][this.camIdx[i][c][3:6], p]
        this.camera[c][6] = this.b[i]["x"][this.camIdx[i][c][6], p]
        this.camera[c][7:9] = this.b[i]["x"][this.camIdx[i][c][7:9], p]
        this.camera[c][9:14] = np.zeros(5)

    logB = best[this.torso]
    return logB
Example #3
0
def sample_from_nbrs(this, i, x, p_i):
    """
    Sample a new particle by looking at the neighbors. 
    Differently from above, we look at the neighbors and sample the pose and shape deformations.
    We have to consider the model poseDefModelNeighbors, that has variables [z_parent, z_part, r_rel_part, r_rel_child],
    and we want to sample the part pose deformations z_part given the relative pose w.r.t parent and child
    @params this: a dpmp object
    @params i   : the index of the node
    @params x   : the particle array 
    @params p_i : the index of the particle x in the node array b[i]['x']
    """

    pa = this.body.parent[i]
    ch = this.body.child[i]

    # Select the best neighbors particles or at random
    """
    I_pa = np.argmax(this.b[pa]['value'])
    num_x = this.b[pa]['x'].shape[1]
    I_pa = np.random.randint(0,num_x,1)[0]
    x_pa = this.b[pa]['x'][:,I_pa]
    I_ch = np.argmax(this.b[ch]['value'])
    num_x = this.b[ch]['x'].shape[1]
    I_ch = np.random.randint(0,num_x,1)[0]
    x_ch = this.b[ch]['x'][:,I_ch]
    """

    # Select the current best
    I_pa = np.argmax(this.b[pa]['value'])
    I_ch = np.argmax(this.b[ch]['value'])

    proposedParticle = x[:,p_i].copy()
    mu = this.body.poseDefModelNeighbors[i]['mu']
    C = this.body.poseDefModelNeighbors[i]['C']

    # Conditioning varibles
    # Compute the relative Rodrigues vector between the particle and the neightbors. Note that the relative Rodrigues vectors are
    # always defined between the part and its parent, so we need r_rel of i w.r.t pa and of ch w.r.t. i

    x_pa = this.b[pa]['x'][:,I_pa]
    rpa, tpa, zpa, zspa = particles.get_part_params(this.particleIdx[pa], x_pa)

    x_ch = this.b[ch]['x'][:,I_ch]
    rch, tch, zch, zsch = particles.get_part_params(this.particleIdx[ch], x_ch)

    r, t, z, zs = particles.get_part_params(this.particleIdx[i], proposedParticle)

    R0_parent, J = cv2.Rodrigues(rpa)
    R0_part, J = cv2.Rodrigues(r)
    A = np.matrix(R0_part)
    B = np.matrix(R0_parent)

    R = A*B.T*(B*B.T).I

    r_rel_pa, J = cv2.Rodrigues(R)
    R0_child, J = cv2.Rodrigues(rch)
    A = np.matrix(R0_child)
    B = np.matrix(R0_part)

    R = A*B.T*(B*B.T).I

    r_rel_ch, J = cv2.Rodrigues(R)

    X = np.concatenate((r_rel_pa, r_rel_ch))

    # Indexes of the conditioning variables
    na = len(X)
    nmu = np.prod(mu.shape)
    cInd = xrange(nmu-na,nmu)

    # Indexes of the resulting variables
    nb = this.nB[i]
    rInd = xrange(this.body.nPoseBasis[pa], this.body.nPoseBasis[pa]+nb)
     
    mu_ab, C_ab = ba.compute_conditioned_gaussian(this.body, rInd, cInd, mu, C, X)
    proposedParticle[this.particleIdx[i]['zPoseIdx']] = mu_ab

    # For the shape parameters, we propagate the same shape from the parent
    proposedParticle[this.particleIdx[i]['zPoseIdx']] = x_pa[this.particleIdx[pa]['zPoseIdx']]

    return proposedParticle
Example #4
0
def fun(this, rankInd):
    """
    @params: rankInd = 0 for the MAP solution
    Computes the indexes of the particles for the ranked solution, and set the body model accordingly
    Returns the log posterior (or negative energy)
    Only works for the same number of particles on each node
    """

    logB = None

    # Build a matrix (nParticles x nNodes) with the max-marginal values of all nodes
    B = -np.inf + np.zeros((len(this.b[this.torso]['value']), this.nNodes))
    for i in this.nodeIdx:
        B[:,i] = this.b[i]['value']

    # Sort the matrix to find the global max
    I = np.argsort(-B, axis=None)
    B_sort = B.flatten()[I]

    # Find the unique indexes of the max-marginal (which have the same value on each node)
    _, I_unique = np.unique(B_sort, return_index=True)
    I_unique = I_unique[::-1]

    # The ranked value identifies a root node and a particle. We will traverse the tree from the selected root to find the 
    # particles on each node
    if rankInd < len(I_unique):
        v = np.where(B == B.flatten()[I[I_unique[rankInd]]])
    else:
        v = np.where(B == B.flatten()[I[I_unique[-1]]])
    p = v[0][0]
    rootNode = v[1][0]

    # Depth-first traversal
    schedule = [None]
    schedule[0] = rootNode
    visited = np.zeros((this.nNodes), dtype=np.int)
    parent = np.zeros((this.nNodes), dtype=np.int)

    while len(schedule) > 0:
        # Pop root, add children
        i = schedule[0]
        schedule = schedule[1::]
        if i in this.nodeIdx:
            visited[i] = 1
            nbrs = np.zeros((this.nNodes), dtype=np.int)
            nbrs[this.A[:,i]>=0] = 1
            children = nbrs & ~visited
            N = np.where(children>0)[0]
            schedule.extend(N)
            parent[N] = i
        else:
            continue

        # Do backtracking
        if i == rootNode:
            logB = this.b[i]['value'][p]
            this.mapParticleInd[i] = p
        else:
            # Get the message i->parent that contains the argmax information
            t = this.Q[i, parent[i]]
            ind = np.where(this.m[t]['pDst'] == this.mapParticleInd[parent[i]])[0]
            p = this.m[t]['pSrc'][ind]
            this.mapParticleInd[i] = p
    if this.verbose > 0:
        print 'mapParticleInd:'
        print this.mapParticleInd

    # Set the object 
    for i in this.nodeIdx:
        x = this.b[i]['x'][:, this.mapParticleInd[i]]
        ra, ta, za, zsa = particles.get_part_params(this.particleIdx[i], x)
        this.body.Zp[i] = za
        this.body.Zs[i] = zsa
        this.body.r_abs[i,:] = ra
        this.body.t[i,:] = ta

    return logB
Example #5
0
def fun(this, rankInd):
    """
    @params: rankInd = 0 for the MAP solution
    Computes the indexes of the particles for the ranked solution, and set the body model accordingly
    Returns the log posterior (or negative energy)
    Only works for the same number of particles on each node
    """

    logB = None

    # Build a matrix (nParticles x nNodes) with the max-marginal values of all nodes
    B = -np.inf + np.zeros((len(this.b[this.torso]["value"]), this.nNodes))
    for i in this.nodeIdx:
        B[:, i] = this.b[i]["value"]

    # Sort the matrix to find the global max
    I = np.argsort(-B, axis=None)
    B_sort = B.flatten()[I]

    # Find the unique indexes of the max-marginal (which have the same value on each node)
    _, I_unique = np.unique(B_sort, return_index=True)
    I_unique = I_unique[::-1]

    # The ranked value identifies a root node and a particle. We will traverse the tree from the selected root to find the
    # particles on each node
    if rankInd < len(I_unique):
        v = np.where(B == B.flatten()[I[I_unique[rankInd]]])
    else:
        v = np.where(B == B.flatten()[I[I_unique[-1]]])
    p = v[0][0]
    rootNode = v[1][0]

    # Depth-first traversal
    schedule = [None]
    schedule[0] = rootNode
    visited = np.zeros((this.nNodes), dtype=np.int)
    parent = np.zeros((this.nNodes), dtype=np.int)

    while len(schedule) > 0:
        # Pop root, add children
        i = schedule[0]
        schedule = schedule[1::]
        if i in this.nodeIdx:
            visited[i] = 1
            nbrs = np.zeros((this.nNodes), dtype=np.int)
            nbrs[this.A[:, i] >= 0] = 1
            children = nbrs & ~visited
            N = np.where(children > 0)[0]
            schedule.extend(N)
            parent[N] = i
        else:
            continue

        # Do backtracking
        if i == rootNode:
            logB = this.b[i]["value"][p]
            this.mapParticleInd[i] = p
        else:
            # Get the message i->parent that contains the argmax information
            t = this.Q[i, parent[i]]
            ind = np.where(this.m[t]["pDst"] == this.mapParticleInd[parent[i]])[0]
            p = this.m[t]["pSrc"][ind]
            this.mapParticleInd[i] = p
    if this.verbose > 0:
        print "mapParticleInd:"
        print this.mapParticleInd

    # Set the object
    for i in this.nodeIdx:
        x = this.b[i]["x"][:, this.mapParticleInd[i]]
        ra, ta, za, zsa = particles.get_part_params(this.particleIdx[i], x)
        this.body.Zp[i] = za
        this.body.Zs[i] = zsa
        this.body.r_abs[i, :] = ra
        this.body.t[i, :] = ta

    return logB
Example #6
0
def compute_pairwise_stitch_potential(this, a, b, xSrc, xDst):
    """
    Computes the pairwise stitch cost for two connected parts
    @params a: parent part
    @params b: child part
    @params xSrc: set of Na particles of the parent
    @params xDst: set of Nb particle of the child
    Return negative cost(Na x Nb) matrix
    """

    Na = xSrc.shape[1]
    Nb = xDst.shape[1]

    # Computes the vertexes of the part a in global coordinates
    [ra, ta, za, zsa ] = particles.get_part_params(this.particleIdx[a], xSrc)

    Pa = ba.get_part_meshes(this.body, a, za, zsa)
    Ra = np.zeros((3,3,Na))
    for i in xrange(Na):
        Ra[:,:,i], J  = cv2.Rodrigues(ra[:,i])
    Paw = ba.object_to_world(Pa, Ra, ta)

    # Computes the vertexes of the part b in global coordinates
    [rb, tb, zb, zsb ] = particles.get_part_params(this.particleIdx[b], xDst)

    Pb = ba.get_part_meshes(this.body, b, zb, zsb)
    Rb = np.zeros((3,3,Nb))
    for i in xrange(Nb):
        Rb[:,:,i], J  = cv2.Rodrigues(rb[:,i])
    Pbw = ba.object_to_world(Pb, Rb, tb)

    cl = this.body.interfacePointsFromTo[b][a]
    clp = this.body.interfacePointsFromTo[a][b]
    clf = this.body.interfacePointsFlex[a][b]
    if len(Pbw.shape) == 3:
        p2 = Pbw[cl,:,:]
        p1 = Paw[clp,:,:]
        n1 = p1.shape[2]
        n2 = p2.shape[2]
        cost = np.zeros((n1,n2))

        P1 = np.dstack([p1[:,0,:]] * n2)
        P2 = np.dstack([p2[:,0,:]] * n1)
        dx = np.transpose(P1, [0, 2, 1]) - P2

        P1 = np.dstack([p1[:,1,:]] * n2)
        P2 = np.dstack([p2[:,1,:]] * n1)
        dy = np.transpose(P1, [0, 2, 1]) - P2

        P1 = np.dstack([p1[:,2,:]] * n2)
        P2 = np.dstack([p2[:,2,:]] * n1)
        dz = np.transpose(P1, [0, 2, 1]) - P2

        Ta = np.array([ta,]*n2).T
        Tb = np.array([tb,]*n1)
        dT = (Ta - Tb)**2
        St = np.sqrt(np.sum(dT, axis=1))

    else:
        p2 = Pbw[cl,:]
        p1 = Paw[clp,:]
        dx = p1[:,0] - p2[:,0]
        dy = p1[:,1] - p2[:,1]
        dz = p1[:,2] - p2[:,2]
        dT = (ta - tb)**2
        St = np.sqrt(np.sum(dT, axis=0))

    # Setting a penalty for part centers to be close penalizes interpenetration but only for parts that have similar length (arms, legs)
    S = np.array(St < 0.05, dtype=int)
    dsq = (dx**2) + (dy**2) + (dz**2)
    weights = np.ones((dsq.shape))
    N = np.sum(weights)
    weights[clf] = 0.8
    Nw = np.sum(weights)
    weights = weights * N / Nw 
    dsq = dsq * weights
    cost = np.mean(dsq, axis=0)

    # Add a penalty for a total reflection of the parts, that would have a low cost, but
    # we should penalize for inter-penetration
    
    return -this.stitchAlpha[a,b] *( cost.T + S ) 
Example #7
0
def sample_from_nbrs(this, i, x, p_i):
    """
    Sample a new particle by looking at the neighbors. 
    Differently from above, we look at the neighbors and sample the pose and shape deformations.
    We have to consider the model poseDefModelNeighbors, that has variables [z_parent, z_part, r_rel_part, r_rel_child],
    and we want to sample the part pose deformations z_part given the relative pose w.r.t parent and child
    @params this: a dpmp object
    @params i   : the index of the node
    @params x   : the particle array 
    @params p_i : the index of the particle x in the node array b[i]['x']
    """

    pa = this.body.parent[i]
    ch = this.body.child[i]

    # Select the best neighbors particles or at random
    """
    I_pa = np.argmax(this.b[pa]['value'])
    num_x = this.b[pa]['x'].shape[1]
    I_pa = np.random.randint(0,num_x,1)[0]
    x_pa = this.b[pa]['x'][:,I_pa]
    I_ch = np.argmax(this.b[ch]['value'])
    num_x = this.b[ch]['x'].shape[1]
    I_ch = np.random.randint(0,num_x,1)[0]
    x_ch = this.b[ch]['x'][:,I_ch]
    """

    # Select the current best
    I_pa = np.argmax(this.b[pa]['value'])
    I_ch = np.argmax(this.b[ch]['value'])

    proposedParticle = x[:, p_i].copy()
    mu = this.body.poseDefModelNeighbors[i]['mu']
    C = this.body.poseDefModelNeighbors[i]['C']

    # Conditioning varibles
    # Compute the relative Rodrigues vector between the particle and the neightbors. Note that the relative Rodrigues vectors are
    # always defined between the part and its parent, so we need r_rel of i w.r.t pa and of ch w.r.t. i

    x_pa = this.b[pa]['x'][:, I_pa]
    rpa, tpa, zpa, zspa = particles.get_part_params(this.particleIdx[pa], x_pa)

    x_ch = this.b[ch]['x'][:, I_ch]
    rch, tch, zch, zsch = particles.get_part_params(this.particleIdx[ch], x_ch)

    r, t, z, zs = particles.get_part_params(this.particleIdx[i],
                                            proposedParticle)

    R0_parent, J = cv2.Rodrigues(rpa)
    R0_part, J = cv2.Rodrigues(r)
    A = np.matrix(R0_part)
    B = np.matrix(R0_parent)

    R = A * B.T * (B * B.T).I

    r_rel_pa, J = cv2.Rodrigues(R)
    R0_child, J = cv2.Rodrigues(rch)
    A = np.matrix(R0_child)
    B = np.matrix(R0_part)

    R = A * B.T * (B * B.T).I

    r_rel_ch, J = cv2.Rodrigues(R)

    X = np.concatenate((r_rel_pa, r_rel_ch))

    # Indexes of the conditioning variables
    na = len(X)
    nmu = np.prod(mu.shape)
    cInd = xrange(nmu - na, nmu)

    # Indexes of the resulting variables
    nb = this.nB[i]
    rInd = xrange(this.body.nPoseBasis[pa], this.body.nPoseBasis[pa] + nb)

    mu_ab, C_ab = ba.compute_conditioned_gaussian(this.body, rInd, cInd, mu, C,
                                                  X)
    proposedParticle[this.particleIdx[i]['zPoseIdx']] = mu_ab

    # For the shape parameters, we propagate the same shape from the parent
    proposedParticle[this.particleIdx[i]['zPoseIdx']] = x_pa[
        this.particleIdx[pa]['zPoseIdx']]

    return proposedParticle