articles:matrix_vs_quaternion_2 [RoboLab.Suehiro]

この文書は読取専用です。文書のソースを閲覧することは可能ですが、変更はできません。もし変更したい場合は管理者に連絡してください。
====== 回転行列と四元数の姿勢の補間の比較 ======

===== 目的 =====

ベクトル，回転行列，座標変換行列といった三次元幾何演算をpythonのリストをベースに[[./geo.py|geo.py]]というモジュールを自作している．

最近，回転の表現に四元数(QUATERNION)がよく用いられているが， 回転行列(MATRIX)と比較して本当にメリットがあるのかを， 特に四元数が得意とされている回転の補間を演算速度の点で比較してみることにした．

このテストは以下の条件で行った．

  * ProBook 474s
  * メモリ：8 GB
  * CPU：Core™ i5-3230M
  * OS: Ubuntu 20.04
  * jupyter notebook，python3

このipynb自身は，[[./matrix_vs_quaternion_2.ipynb|matrix_vs_quaternion_2.ipynb]]となっている．

===== モジュールの読み込み =====

<code python>
from geo import *
import time
import pandas as pd
</code>
===== データの生成 =====

geo.pyには三次元ベクトル：VECTOR，三次元回転行列：MATRIX，四元数：QUATERNIONのクラスが定義されている． MATRIX,QUATERNIONのa, bはそれぞれx軸，y軸周りの回転を指定する．

<code python>
R1=MATRIX(a=pi/3)
R2=MATRIX(b=pi/6)
q1=QUATERNION(a=pi/3)
q2=QUATERNION(b=pi/6)
</code>
この内容はこうなる．

<code python>
print('R1 =', R1)
print('R2 =', R2)
print('q1 =', q1)
print('q2 =', q2)
</code>
<code>
R1 = m:[[1.0, 0.0, 0.0], [0.0, 0.5000000000000001, -0.8660254037844386], [0.0, 0.8660254037844386, 0.5000000000000001]]
R2 = m:[[0.8660254037844387, 0.0, 0.49999999999999994], [0.0, 1.0, 0.0], [-0.49999999999999994, 0.0, 0.8660254037844387]]
q1 = q:(0.8660254037844387, v:[0.49999999999999994, 0.0, 0.0])
q2 = q:(0.9659258262890683, v:[0.0, 0.25881904510252074, 0.0])
</code>
<code python>
(R1*R2).quaternion()
</code>
<code>
q:(0.8365163037378079, v:[0.4829629131445341, 0.22414386804201336, 0.1294095225512603])
</code>
<code python>
q1*q2
</code>
<code>
q:(0.8365163037378079, v:[0.4829629131445341, 0.2241438680420134, 0.12940952255126034])
</code>
===== 時間計測の関数と結果保存データ =====

<code python>
def test(n,fn):
    i=0
    start=time.time()
    while i< n :
        fn()
        i += 1
    end = time.time()
    rslt=end-start
    return rslt
</code>
<code python>
data = []
</code>
<code python>
def judge(test_name, m_time, q_time) :
    if m_time < q_time :
        judgment = "MATRIXの勝ち"
    elif m_time > q_time :
        judgment = "QUATERNIONの勝ち"
    else :
        judgment = "引き分け"
    return test_name, m_time, q_time, judgment
</code>
===== 補間のための関数 その１ =====

補間した回転角ごとに初期姿勢からの回転変換を生成する．

<code python>
def interpolate_matrix(m_from, m_to, n) :
    m_diff=m_from.trans()*m_to
    th, axis = m_diff.rot_axis()  # rodrigues
    # print(th,axis)
    rslt=[m_from]
    for i in range(1,n-1) :
        th_i = th*i/n
        m_intermed = MATRIX(axis=axis, angle=th_i)
        rslt.append(m_from*m_intermed)
    rslt.append(m_to)
    return rslt
</code>
<code python>
a=interpolate_matrix(R1,R2,100)
</code>
<code python>
for tmp in a :
    print(tmp.quaternion())
</code>
<code>
q:(0.8660254037844386, v:[0.49999999999999994, 0.0, 0.0])
q:(0.8685664923780337, v:[0.4955650769003723, 0.0027391376101021715, 0.0])
q:(0.8710783723407913, v:[0.4911134886662798, 0.005478183106975671, 0.0])
q:(0.873560959201833, v:[0.48664538499817545, 0.008217044380501116, 5.268356063861754e-09])
q:(0.8760141694753648, v:[0.48216091615190226, 0.010955629326748035, 5.268356063861754e-09])
q:(0.878437920663485, v:[0.47766023293364057, 0.01369384585107722, 0.0])
q:(0.880832131258958, v:[0.47314348669483647, 0.016431601871239196, 0.0])
q:(0.8831967207479554, v:[0.4686108293271121, 0.019168805320474883, 0.0])
q:(0.8855316096127639, v:[0.46406241325715775, 0.02190536415060079, 7.450580596923828e-09])
q:(0.8878367193344588, v:[0.4594983914416061, 0.02464118633511576, 0.0])
q:(0.8901119723955451, v:[0.45491891736188833, 0.02737617987228907, 0.0])
q:(0.8923572922825634, v:[0.4503241450190728, 0.030110252788255857, 7.450580596923828e-09])
q:(0.8945726034886643, v:[0.4457142289286865, 0.03284331314011253, 0.0])
q:(0.8967578315161462, v:[0.4410893241155184, 0.035575269019003215, 0.0])
q:(0.8989129028789612, v:[0.4364495861084065, 0.03830602855321763, 0.0])
q:(0.9010377451051865, v:[0.43179517093500774, 0.041035499911274524, 5.268356063861754e-09])
q:(0.9031322867394616, v:[0.42712623511655085, 0.0437635913050123, 0.0])
q:(0.9051964573453898, v:[0.42244293566257246, 0.046490210992675966, 0.0])
q:(0.9072301875079093, v:[0.417745430065638, 0.04921526728200174, 0.0])
q:(0.9092334088356255, v:[0.4130338762960443, 0.0519386685333009, 7.450580596923828e-09])
　　　　　　　　　:
               :
q:(0.9716336282114709, v:[0.11084991759724824, 0.2089028202234153, 0.0])
q:(0.9716883266316954, v:[0.10559563563200959, 0.21135599736739819, 0.0])
q:(0.9717103485834275, v:[0.10033780263876534, 0.21380206691586973, 0.0])
q:(0.971699693326101, v:[0.09507659543080955, 0.21624094661106066, 0.0])
q:(0.9716563612180366, v:[0.08981219093490717, 0.21867255443698513, 7.450580596923828e-09])
q:(0.9715803537164303, v:[0.08454476618534205, 0.22109680862220088, 0.0])
q:(0.971471673377304, v:[0.07927449831796592, 0.22351362764255686, 0.0])
q:(0.9713303238554201, v:[0.07400156456423883, 0.22592293022393642, 0.0])
q:(0.9711563099041576, v:[0.06872614224527315, 0.2283246353449894, 0.0])
q:(0.9709496373753531, v:[0.06344840876586723, 0.230718662239858, 0.0])
q:(0.9707103132191043, v:[0.058168541608537835, 0.23310493040089109, 0.0])
q:(0.970438345483535, v:[0.0528867183275577, 0.23548335958135297, 0.0])
q:(0.9701337433145255, v:[0.04760311654297964, 0.23785386979812154, 0.0])
q:(0.9697965169554049, v:[0.04231791393466445, 0.24021638133437792, 0.0])
q:(0.9694266777466063, v:[0.03703128823630815, 0.24257081474228692, 5.268356063861754e-09])
q:(0.9690242381252857, v:[0.03174341722946072, 0.24491709084566954, 5.268356063861754e-09])
q:(0.9685892116249034, v:[0.026454478737552207, 0.247255130742665, 0.0])
q:(0.9681216128747697, v:[0.021164650619910557, 0.24958485580838377, 0.0])
q:(0.9676214575995519, v:[0.01587411076577773, 0.25190618769755274, 0.0])
q:(0.967088762618746, v:[0.010583037088333424, 0.25421904834714837, 0.0])
q:(0.9659258262890683, v:[0.0, 0.2588190451025207, 0.0])
</code>
<code python>
def interpolate_quaternion(q_from,q_to, n) :
    q_diff = q_from.conjugate()*q_to
    si = abs(q_diff.v)
    if si != 0.0 :
        axis = 1.0/si * q_diff.v
    th = atan2(si,q_diff.w)
    # print(th,axis)
    rslt=[q_from]
    for i in range(1,n-1) :
        th_i = th*i/n
        q_intermed=QUATERNION([cos(th_i),sin(th_i)*axis])
        rslt.append(q_from*q_intermed)
    rslt.append(q_to)
    return rslt
</code>
<code python>
b = interpolate_quaternion(q1, q2, 100)
</code>
<code python>
b
</code>
<code>
[q:(0.8660254037844387, v:[0.49999999999999994, 0.0, 0.0]),
 q:(0.8685664923780337, v:[0.49556507690037227, 0.002739137610098667, 2.168404344971009e-19]),
 q:(0.8710783723407913, v:[0.4911134886662798, 0.00547818310697403, 0.0]),
 q:(0.873560959201833, v:[0.48664538499817545, 0.00821704438050042, 4.336808689942018e-19]),
 q:(0.8760141694753648, v:[0.4821609161519022, 0.010955629326747329, 0.0]),
 q:(0.8784379206634849, v:[0.47766023293364057, 0.01369384585107674, 8.673617379884035e-19]),
 q:(0.8808321312589579, v:[0.4731434866948364, 0.01643160187124013, 0.0]),
 q:(0.8831967207479554, v:[0.46861082932711207, 0.019168805320475046, -1.734723475976807e-18]),
 q:(0.8855316096127639, v:[0.46406241325715775, 0.02190536415060122, 0.0]),
 q:(0.8878367193344587, v:[0.4594983914416061, 0.024641186335115972, 0.0]),
                  :
                  :
 q:(0.9716883266316954, v:[0.10559563563200969, 0.21135599736739816, 0.0]),
 q:(0.9717103485834275, v:[0.10033780263876535, 0.2138020669158698, 0.0]),
 q:(0.971699693326101, v:[0.09507659543080976, 0.2162409466110606, 0.0]),
 q:(0.9716563612180366, v:[0.08981219093490717, 0.21867255443698527, 0.0]),
 q:(0.9715803537164303, v:[0.08454476618534201, 0.22109680862220094, 0.0]),
 q:(0.9714716733773041, v:[0.07927449831796574, 0.22351362764255686, 0.0]),
 q:(0.97133032385542, v:[0.07400156456423912, 0.22592293022393642, 0.0]),
 q:(0.9711563099041576, v:[0.06872614224527368, 0.22832463534498953, 0.0]),
 q:(0.9709496373753532, v:[0.06344840876586683, 0.230718662239858, 0.0]),
 q:(0.9707103132191042, v:[0.05816854160853774, 0.23310493040089106, 1.3877787807814457e-17]),
 q:(0.970438345483535, v:[0.05288671832755765, 0.235483359581353, 1.3877787807814457e-17]),
 q:(0.9701337433145255, v:[0.047603116542979496, 0.2378538697981216, 0.0]),
 q:(0.969796516955405, v:[0.04231791393466472, 0.24021638133437795, 1.3877787807814457e-17]),
 q:(0.9694266777466064, v:[0.03703128823630819, 0.242570814742287, 0.0]),
 q:(0.9690242381252857, v:[0.03174341722946106, 0.24491709084566965, 0.0]),
 q:(0.9685892116249035, v:[0.0264544787375528, 0.247255130742665, 0.0]),
 q:(0.9681216128747696, v:[0.02116465061991013, 0.2495848558083839, 1.3877787807814457e-17]),
 q:(0.9676214575995518, v:[0.015874110765777494, 0.25190618769755274, 0.0]),
 q:(0.967088762618746, v:[0.010583037088333302, 0.25421904834714837, -1.3877787807814457e-17]),
 q:(0.9659258262890683, v:[0.0, 0.25881904510252074, 0.0])]
</code>
===== 補間のための関数 その２ =====

補間のための増分に対応する回転変換を逐次演算する．高速化されるが誤差の累積があり得る．

<code python>
def interpolate_matrix2(m_from, m_to, n) :
    m_diff=m_from.trans()*m_to
    th, axis = m_diff.rot_axis()  # rodrigues
    # print(th,axis)
    m_delta=MATRIX(axis=axis,angle=th/n)
    m_intermed=MATRIX(mat=m_from)
    rslt=[m_from]
    for i in range(1,n-1) :
        m_intermed=m_intermed*m_delta
        rslt.append(m_intermed)
    rslt.append(m_to)
    return rslt
</code>
<code python>
c=interpolate_matrix2(R1,R2,100)
</code>
<code python>
for tmp in c :
    print(tmp.quaternion())
</code>
<code>
q:(0.8660254037844386, v:[0.49999999999999994, 0.0, 0.0])
q:(0.8685664923780337, v:[0.4955650769003723, 0.0027391376101021715, 0.0])
q:(0.8710783723407913, v:[0.4911134886662798, 0.005478183106975671, 0.0])
q:(0.873560959201833, v:[0.4866453849981754, 0.008217044380506184, 0.0])
q:(0.8760141694753648, v:[0.48216091615190226, 0.010955629326750567, 0.0])
q:(0.8784379206634849, v:[0.4776602329336406, 0.01369384585108026, 0.0])
q:(0.8808321312589579, v:[0.4731434866948365, 0.01643160187124342, 0.0])
q:(0.8831967207479554, v:[0.4686108293271122, 0.01916880532047778, 0.0])
q:(0.8855316096127639, v:[0.46406241325715786, 0.021905364150603956, 0.0])
q:(0.8878367193344587, v:[0.4594983914416062, 0.024641186335118574, 0.0])
q:(0.890111972395545, v:[0.45491891736188844, 0.027376179872292114, 0.0])
q:(0.8923572922825632, v:[0.450324145019073, 0.030110252788259084, 0.0])
q:(0.8945726034886642, v:[0.4457142289286866, 0.03284331314011549, 0.0])
q:(0.896757831516146, v:[0.4410893241155185, 0.035575269019006726, 0.0])
q:(0.898912902878961, v:[0.43644958610840656, 0.03830602855322089, 7.450580596923828e-09])
q:(0.9010377451051864, v:[0.43179517093500785, 0.04103549991127757, 5.268356063861754e-09])
q:(0.9031322867394612, v:[0.4271262351165509, 0.04376359130501547, 0.0])
q:(0.9051964573453896, v:[0.4224429356625726, 0.04649021099267925, 7.450580596923828e-09])
q:(0.907230187507909, v:[0.4177454300656382, 0.04921526728200541, 7.450580596923828e-09])
q:(0.9092334088356253, v:[0.41303387629604454, 0.05193866853330437, 0.0])
q:(0.9112060539631117, v:[0.4083084327965086, 0.05466032316254457, 0.0])
                     :
                     :
q:(0.9716883266316939, v:[0.10559563563201564, 0.21135599736740054, 2.1721988524665314e-08])
q:(0.971710348583426, v:[0.10033780263877183, 0.21380206691587214, 2.2351741790771484e-08])
q:(0.9716996933260995, v:[0.09507659543081672, 0.2162409466110629, 2.1073424255447017e-08])
q:(0.9716563612180351, v:[0.08981219093491474, 0.21867255443698747, 2.1721988524665314e-08])
q:(0.9715803537164288, v:[0.0845447661853501, 0.2210968086222032, 2.1721988524665314e-08])
q:(0.9714716733773026, v:[0.07927449831797449, 0.22351362764255917, 2.1721988524665314e-08])
q:(0.9713303238554185, v:[0.07400156456424876, 0.2259229302239387, 2.1721988524665314e-08])
q:(0.9711563099041559, v:[0.06872614224528406, 0.22832463534499184, 2.1721988524665314e-08])
q:(0.9709496373753517, v:[0.06344840876587816, 0.23071866223986032, 2.2351741790771484e-08])
q:(0.9707103132191027, v:[0.05816854160855024, 0.23310493040089333, 2.29642316809631e-08])
q:(0.9704383454835334, v:[0.052886718327571615, 0.23548335958135527, 2.356080457693621e-08])
q:(0.970133743314524, v:[0.047603116542995094, 0.23785386979812392, 2.29642316809631e-08])
q:(0.9697965169554033, v:[0.04231791393468248, 0.24021638133438028, 2.356080457693621e-08])
q:(0.9694266777466046, v:[0.037031288236328765, 0.24257081474228928, 2.526614808710392e-08])
q:(0.969024238125284, v:[0.031743417229485635, 0.24491709084567187, 2.47107803102985e-08])
q:(0.9685892116249017, v:[0.026454478737582634, 0.24725513074266725, 2.414264045062609e-08])
q:(0.968121612874768, v:[0.021164650619947274, 0.2495848558083861, 2.414264045062609e-08])
q:(0.9676214575995501, v:[0.015874110765828436, 0.2519061876975549, 2.414264045062609e-08])
q:(0.9670887626187442, v:[0.010583037088410793, 0.2542190483471506, 2.47107803102985e-08])
q:(0.9659258262890683, v:[0.0, 0.2588190451025207, 0.0])
</code>
<code python>
def interpolate_quaternion2(q_from,q_to, n) :
    q_diff = q_from.conjugate()*q_to
    si = abs(q_diff.v)
    if si != 0.0 :
        axis = 1.0/si * q_diff.v
    th = atan2(si,q_diff.w)
    # print(th,axis)
    q_delta=QUATERNION([cos(th/n),sin(th/n)*axis])
    q_intermed=QUATERNION([q_from.w,q_from.v])
    rslt=[q_from]
    for i in range(1,n-1) :
        th_i = th*i/n
        q_intermed=q_intermed*q_delta
        rslt.append(q_intermed)
    rslt.append(q_to)
    return rslt
</code>
<code python>
d=interpolate_quaternion2(q1,q2,100)
</code>
<code python>
d
</code>
<code>
[q:(0.8660254037844387, v:[0.49999999999999994, 0.0, 0.0]),
 q:(0.8685664923780337, v:[0.49556507690037227, 0.002739137610098667, 2.168404344971009e-19]),
 q:(0.8710783723407913, v:[0.4911134886662797, 0.00547818310697403, 4.980553729855286e-19]),
 q:(0.8735609592018329, v:[0.4866453849981753, 0.008217044380500419, 4.3029273720518457e-19]),
 q:(0.8760141694753646, v:[0.48216091615190204, 0.010955629326747327, 4.336808689942018e-19]),
 q:(0.8784379206634847, v:[0.47766023293364035, 0.013693845851076735, 3.9302328752599536e-19]),
 q:(0.8808321312589575, v:[0.47314348669483625, 0.01643160187124012, 6.2341624917916505e-19]),
 q:(0.883196720747955, v:[0.46861082932711184, 0.019168805320475036, 8.944667923005412e-19]),
 q:(0.8855316096127634, v:[0.4640624132571575, 0.021905364150601212, 1.0977546996415732e-18]),
 q:(0.8878367193344583, v:[0.45949839144160587, 0.024641186335115965, 1.4230153513872246e-18]),
 q:(0.8901119723955445, v:[0.4549189173618881, 0.027376179872288946, 1.8973538018496328e-18]),
 q:(0.8923572922825629, v:[0.45032414501907264, 0.03011025278825601, 2.3310346708438345e-18]),
 q:(0.8945726034886636, v:[0.4457142289286863, 0.032843313140112164, 2.791820594150174e-18]),
 q:(0.8967578315161454, v:[0.44108932411551816, 0.035575269019003486, 3.2526065174565133e-18]),
 q:(0.8989129028789605, v:[0.43644958610840623, 0.03830602855321787, 3.848917712323541e-18]),
 q:(0.901037745105186, v:[0.43179517093500747, 0.041035499911274545, 4.336808689942018e-18]),
 q:(0.9031322867394608, v:[0.4271262351165505, 0.04376359130501225, 4.526544070126981e-18]),
 q:(0.905196457345389, v:[0.42244293566257224, 0.0464902109926759, 4.797594613248357e-18]),
 q:(0.9072301875079085, v:[0.41774543006563775, 0.049215267282001755, 5.3939058081153846e-18]),
 q:(0.9092334088356246, v:[0.41303387629604416, 0.0519386685333009, 5.9631119486702744e-18]),
 q:(0.911206053963111, v:[0.40830843279650814, 0.054660323162540936, 6.667843360785852e-18]),
 q:(0.9131480565531743, v:[0.40356925847683794, 0.05738013964442585, 7.37257477290143e-18]),
                      :
                      :
 q:(0.9716336282114678, v:[0.1108499175972483, 0.20890282022341475, 4.336808689942018e-17]),
 q:(0.9716883266316922, v:[0.10559563563200967, 0.21135599736739757, 4.466912950640278e-17]),
 q:(0.9717103485834244, v:[0.10033780263876536, 0.21380206691586923, 4.597017211338539e-17]),
 q:(0.9716996933260977, v:[0.09507659543080976, 0.21624094661106003, 4.7271214720367993e-17]),
 q:(0.9716563612180333, v:[0.08981219093490717, 0.2186725544369847, 4.87890977618477e-17]),
 q:(0.971580353716427, v:[0.08454476618534208, 0.22109680862220032, 5.0306980803327406e-17]),
 q:(0.9714716733773008, v:[0.07927449831796572, 0.22351362764255628, 5.204170427930421e-17]),
 q:(0.9713303238554167, v:[0.07400156456423924, 0.22592293022393575, 5.355958732078392e-17]),
 q:(0.9711563099041541, v:[0.0687261422452737, 0.2283246353449889, 5.4860629927766524e-17]),
 q:(0.9709496373753498, v:[0.06344840876586694, 0.23071866223985735, 5.681219383824043e-17]),
 q:(0.9707103132191008, v:[0.05816854160853785, 0.23310493040089042, 5.854691731421724e-17]),
 q:(0.9704383454835315, v:[0.052886718327557765, 0.23548335958135236, 6.028164079019405e-17]),
 q:(0.970133743314522, v:[0.04760311654297963, 0.23785386979812093, 6.223320470066795e-17]),
 q:(0.9697965169554014, v:[0.042317913934664886, 0.2402163813343772, 6.418476861114186e-17]),
 q:(0.9694266777466027, v:[0.03703128823630836, 0.24257081474228626, 6.591949208711867e-17]),
 q:(0.969024238125282, v:[0.031743417229461314, 0.2449170908456689, 6.765421556309548e-17]),
 q:(0.9685892116248997, v:[0.02645447873755296, 0.24725513074266423, 6.938893903907228e-17]),
 q:(0.968121612874766, v:[0.021164650619910446, 0.24958485580838305, 7.090682208055199e-17]),
 q:(0.967621457599548, v:[0.015874110765777768, 0.25190618769755185, 7.22078646875346e-17]),
 q:(0.9670887626187421, v:[0.010583037088333575, 0.25421904834714754, 7.37257477290143e-17]),
 q:(0.9659258262890683, v:[0.0, 0.25881904510252074, 0.0])]
</code>
===== ループ回数の決定 =====

<code python>
test(100, lambda : interpolate_matrix(R1,R2,100))
</code>
<code>
0.18225622177124023
</code>
<code python>
test(1000, lambda : interpolate_matrix(R1,R2,100))
</code>
<code>
1.597233772277832
</code>
<code python>
test(10000, lambda : interpolate_matrix(R1,R2,100))
</code>
<code>
18.435190200805664
</code>
<code python>
test(100000, lambda : interpolate_matrix(R1,R2,100))
</code>
<code>
177.21397614479065
</code>
内部で100分割の補間をしているので，計測の繰り返しは10000とする．

<code python>
N=10000
</code>
===== 補間その１ =====

<code python>
m_time = test(N, lambda : interpolate_matrix(R1,R2,100))
print(m_time)
</code>
<code>
17.621865272521973
</code>
<code python>
q_time = test(N, lambda : interpolate_quaternion(q1,q2,100))
print(q_time)
</code>
<code>
11.456907510757446
</code>
<code python>
data.append(judge('補間その１', m_time, q_time))
</code>
===== 補間その２ =====

<code python>
m_time = test(N, lambda : interpolate_matrix2(R1,R2,100))
print(m_time)
</code>
<code>
8.184473276138306
</code>
<code python>
q_time = test(N, lambda : interpolate_quaternion2(q1,q2,100))
print(q_time)
</code>
<code>
7.437374830245972
</code>
<code python>
data.append(judge('補間その２', m_time, q_time))
</code>
===== 結論 =====

まとめの表

<code python>
df = pd.DataFrame(data, columns=["項目", "MATRIX", "QUATERNION", "結果"])
df
</code>
<HTML>
<div>
<style scoped>
    .dataframe tbody tr th:only-of-type {
        vertical-align: middle;
    }

    .dataframe tbody tr th {
        vertical-align: top;
    }

    .dataframe thead th {
        text-align: right;
    }
</style>
<table border="1" class="dataframe">
<thead>
<tr style="text-align: right;">
<th>
</th>
<th>
</HTML>
項目
<HTML>
</th>
<th>
</HTML>
MATRIX
<HTML>
</th>
<th>
</HTML>
QUATERNION
<HTML>
</th>
<th>
</HTML>
結果
<HTML>
</th>
</tr>
</thead>
<tbody>
<tr>
<th>
</HTML>
0
<HTML>
</th>
<td>
</HTML>
補間その１
<HTML>
</td>
<td>
</HTML>
17.621865
<HTML>
</td>
<td>
</HTML>
11.456908
<HTML>
</td>
<td>
</HTML>
QUATERNIONの勝ち
<HTML>
</td>
</tr>
<tr>
<th>
</HTML>
1
<HTML>
</th>
<td>
</HTML>
補間その２
<HTML>
</td>
<td>
</HTML>
8.184473
<HTML>
</td>
<td>
</HTML>
7.437375
<HTML>
</td>
<td>
</HTML>
QUATERNIONの勝ち
<HTML>
</td>
</tr>
</tbody>
</table>
</div>
</HTML>
予想通り回転の補間に関しては，QUATERNIONの方が速かった．

回転の合成（乗算）に関してQUATERNIONの方が速いのは元々わかっていたことではある．

直交行列の方はロドリゲス（回転軸と回転角度）からの生成に時間がかかるためさらに遅くなっている．

回転変換の生成をその都度行わなければ，速度の差はほぼ回転の合成の差にとどまっている．

しかし，ロボットの軌道生成を考えた場合は，姿勢の補間だけではなく位置も補完する必要がある． さらにそこから逆運動学を解く必要もある． それらを行うとすれば直交行列で計算したほうが速いはずである．

上記のようなロボットの運動学や原点位置まで含めた座標変換を考えた場合に 四元数にどの程度の利点が残るかは私にはまだよくわかっていない．

<code python>

</code>