コード例 #1
0
ファイル: cumulative.py プロジェクト: mfkiwl/kalibr
def cumQuat2(bs, t, qc):
    cumulativeBi = bs.getLocalCumulativeBi(t)
    ci = bs.localCoefficientVectorIndices(t)
    qci = qc[:, ci]
    rval = qci[:, 0]
    for i in range(0, len(ci) - 1):
        dqi = qdot(qinv(qci[:, i]), qci[:, i + 1])
        ldqi = qlog(dqi)
        rval = qdot(rval, qexp(ldqi * cumulativeBi[i + 1]))

    return rval
コード例 #2
0
ファイル: cumulative.py プロジェクト: AliAlawieh/kalibr
def cumQuat2(bs,t,qc):
    cumulativeBi = bs.getLocalCumulativeBi(t);
    ci = bs.localCoefficientVectorIndices(t)
    qci = qc[:,ci]
    rval = qci[:,0]
    for i in range(0,len(ci) - 1):
        dqi = qdot(qinv(qci[:,i]),qci[:,i+1])
        ldqi = qlog(dqi)
        rval= qdot(rval, qexp(ldqi * cumulativeBi[i+1]))
        
    return rval
コード例 #3
0
ファイル: cumulative.py プロジェクト: mfkiwl/kalibr
def cumQuat(bs, t, qc):
    si = bs.segmentIndex(t)
    Mi = bs.Mi(si)
    u = bs.u(t, 0)
    bi = np.dot(u.T, Mi)

    cumulativeBi = bs.getCumulativeBiFunction(t)

    ci = bs.localCoefficientVectorIndices(t)

    qci = qc[:, ci]
    rval = qci[:, 0]
    for i in range(0, len(ci) - 1):
        dqi = qdot(qinv(qci[:, i]), qci[:, i + 1])
        ldqi = qlog(dqi)
        tildeB = np.sum(bi[i + 1:])
        diff = tildeB - cumulativeBi(int(ci[i] + 1))
        if abs(diff) > 1e-9:
            print(diff)
        rval = qdot(rval, qexp(ldqi * tildeB))

    return rval
コード例 #4
0
ファイル: cumulative.py プロジェクト: AliAlawieh/kalibr
def cumQuat(bs,t,qc):
    si = bs.segmentIndex(t)
    Mi = bs.Mi(si)
    u = bs.u(t,0)
    bi = np.dot(u.T,Mi)
    
    cumulativeBi = bs.getCumulativeBiFunction(t);
    
    ci = bs.localCoefficientVectorIndices(t)
    
    qci = qc[:,ci]
    rval = qci[:,0]
    for i in range(0,len(ci) - 1):
        dqi = qdot(qinv(qci[:,i]),qci[:,i+1])
        ldqi = qlog(dqi)
        tildeB = np.sum(bi[i+1:])
        diff = tildeB - cumulativeBi(int(ci[i]+1))
        if abs(diff) > 1e-9 :
            print diff
        rval= qdot(rval, qexp(ldqi * tildeB))
        

    return rval