コード例 #1
0
ファイル: jacobians.py プロジェクト: AliAlawieh/kalibr 
def S2(phi):
    p = math.sqrt(dot(phi.T,phi))
    a = phi/p
    sp2 = math.sin(0.5 * p)
    sp = math.sin(p)
    ax = asrl.crossMx(a)
    px = asrl.crossMx(phi)
    return eye(3) - (2/p) * sp2 * sp2 * ax + (1/p)*(p - sp)*np.dot(ax,ax)
コード例 #2
0
def S2(phi):
    p = math.sqrt(dot(phi.T, phi))
    a = phi / p
    sp2 = math.sin(0.5 * p)
    sp = math.sin(p)
    ax = asrl.crossMx(a)
    px = asrl.crossMx(phi)
    return eye(3) - (2 / p) * sp2 * sp2 * ax + (1 / p) * (p - sp) * np.dot(
        ax, ax)
コード例 #3
0
ファイル: jacobians.py プロジェクト: AliAlawieh/kalibr 
def S(phi):
    p = math.sqrt(dot(phi.T,phi))
    a = phi/p
    sp2 = math.sin(0.5 * p)
    ax = asrl.crossMx(a)
    px = asrl.crossMx(phi)
    c1 = (-2 * sp2 * sp2) / (p * p)
    c2 = (p - math.sin(p))/(p * p * p)
    S = eye(3) + c1 * px + c2 * np.dot(px,px)
    return S
コード例 #4
0
def S(phi):
    p = math.sqrt(dot(phi.T, phi))
    a = phi / p
    sp2 = math.sin(0.5 * p)
    ax = asrl.crossMx(a)
    px = asrl.crossMx(phi)
    c1 = (-2 * sp2 * sp2) / (p * p)
    c2 = (p - math.sin(p)) / (p * p * p)
    S = eye(3) + c1 * px + c2 * np.dot(px, px)
    return S
コード例 #5
0
ファイル: jacobians.py プロジェクト: AliAlawieh/kalibr 
def invS2(p):
    return  ( dot(p,p.T) + eye(3) + asrl.crossMx(p) )
コード例 #6
0
ファイル: jacobians.py プロジェクト: AliAlawieh/kalibr 
def invS(phi):
    p = math.sqrt(dot(phi.T,phi))
    a1 = ( 1 / (p*p) ) * ( 1 - 0.5 * p / math.tan(0.5 * p))
    px = asrl.crossMx(phi)
    return eye(3) + 0.5 * px + a1 * dot(px,px)
コード例 #7
0
def invS2(p):
    return (dot(p, p.T) + eye(3) + asrl.crossMx(p))
コード例 #8
0
def invS(phi):
    p = math.sqrt(dot(phi.T, phi))
    a1 = (1 / (p * p)) * (1 - 0.5 * p / math.tan(0.5 * p))
    px = asrl.crossMx(phi)
    return eye(3) + 0.5 * px + a1 * dot(px, px)