%23%20%2F%2F%2F%20script%0A%23%20requires-python%20%3D%20%22%3E%3D3.11%22%0A%23%20dependencies%20%3D%20%5B%22marimo%3E%3D0.23.9%22%2C%20%22numpy%3E%3D2.0%22%2C%20%22urllib3%3E%3D2.2%22%5D%0A%23%20%2F%2F%2F%0A%0Aimport%20marimo%0A%0A__generated_with%20%3D%20%220.25.2%22%0Aapp%20%3D%20marimo.App()%0A%0A%0A%40app.cell(hide_code%3DTrue)%0Adef%20_(mo)%3A%0A%20%20%20%20mo.md(r%22%22%22%0A%20%20%20%20%23%20Determining%20rigid%20body%20transformation%20using%20the%20SVD%20algorithm%0A%0A%20%20%20%20%3E%20Marcos%20Duarte%2C%20Renato%20Naville%20Watanabe%3Cbr%3E%0A%20%20%20%20%3E%20%5BLaboratory%20of%20Biomechanics%20and%20Motor%20Control%5D(https%3A%2F%2Fbmclab.pesquisa.ufabc.edu.br%2F)%3Cbr%3E%0A%20%20%20%20%3E%20Federal%20University%20of%20ABC%2C%20Brazil%0A%20%20%20%20%22%22%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell(hide_code%3DTrue)%0Adef%20_(mo)%3A%0A%20%20%20%20mo.md(r%22%22%22%0A%20%20%20%20%23%23%20How%20to%20use%20this%20guide%0A%0A%20%20%20%20A%20marker%20cluster%20has%20four%20measured%20points%2C%20yet%20noise%20prevents%20all%20four%20from%20matching%20a%20perfectly%20rigid%20motion.%20Which%20rotation%20and%20translation%20should%20we%20report%3F%20The%20SVD%20fit%20uses%20all%20the%20markers%20and%20minimizes%20their%20squared%20residuals.%0A%0A%20%20%20%20Start%20with%20the%20exact%20quarter-turn%20example%2C%20inspect%20the%20array%20shapes%20in%20the%20multiple-frame%20example%2C%20and%20then%20use%20the%20Exercises%20to%20test%20noise%2C%20translation%20and%20reflection.%20Try%20the%20independent%20Problems%20after%20the%20checkpoint.%20All%20solutions%20start%20collapsed.%20Prerequisites%20are%20vectors%2C%20matrices%20and%20the%20distinction%20between%20a%20rotation%20and%20a%20reflection.%0A%0A%20%20%20%20**Opening%20challenge.**%20If%20every%20marker%20in%20the%20second%20frame%20is%20shifted%20by%20the%20same%202%20cm%2C%20should%20the%20estimated%20rotation%20change%3F%20What%20if%20just%20one%20marker%20moves%3F%0A%0A%20%20%20%20%23%23%20Python%20setup%0A%0A%20%20%20%20NumPy%20supplies%20SVD%20and%20array%20operations.%20The%20attributed%20%60svdt%60%20helper%20is%20retrieved%20from%20the%20BMC%20repository%20when%20it%20is%20not%20present%20locally.%20This%20requires%20network%20access%20for%20a%20standalone%20copy%3B%20experimental%20inputs%20are%20not%20replaced%20with%20simulated%20observations.%0A%20%20%20%20%22%22%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell(hide_code%3DTrue)%0Adef%20_(mo)%3A%0A%20%20%20%20mo.md(r%22%22%22%0A%20%20%20%20Ideally%2C%20three%20non-collinear%20markers%20placed%20on%20a%20moving%20rigid%20body%20is%20everything%20we%20need%20to%20describe%20its%20movement%20(translation%20and%20rotation)%20in%20relation%20to%20a%20fixed%20coordinate%20system.%20However%2C%20in%20practical%20situations%20of%20human%20motion%20analysis%2C%20markers%20are%20placed%20on%20the%20soft%20tissue%20of%20a%20deformable%20body%20and%20this%20generates%20artifacts%20caused%20by%20muscle%20contraction%2C%20skin%20deformation%2C%20marker%20wobbling%2C%20etc.%20In%20this%20situation%2C%20the%20use%20of%20only%20three%20markers%20can%20produce%20unreliable%20results.%20It%20has%20been%20shown%20that%20four%20or%20more%20markers%20on%20the%20segment%20followed%20by%20a%20mathematical%20procedure%20to%20calculate%20the%20'best'%20rigid-body%20transformation%20taking%20into%20account%20all%20these%20markers%20produces%20more%20robust%20results%20(S%C3%B6derkvist%20%26%20Wedin%201993%3B%20Challis%201995%3B%20Cappozzo%20et%20al.%201997).%0A%0A%20%20%20%20One%20mathematical%20procedure%20to%20calculate%20the%20transformation%20with%20three%20or%20more%20marker%20positions%20envolves%20the%20use%20of%20the%20%5Bsingular%20value%20decomposition%5D(https%3A%2F%2Fen.wikipedia.org%2Fwiki%2FSingular_value_decomposition)%20(SVD)%20algorithm%20from%20linear%20algebra.%20The%20SVD%20algorithm%20decomposes%20a%20matrix%20%24%5Cmathbf%7BM%7D%24%20(which%20represents%20a%20general%20transformation%20between%20two%20coordinate%20systems)%20into%20three%20simple%20transformations%3A%20a%20rotation%20%24%5Cmathbf%7BV%5ET%7D%24%2C%20a%20scaling%20factor%20%24%5Cmathbf%7BS%7D%24%20along%20the%20rotated%20axes%20and%20a%20second%20rotation%20%24%5Cmathbf%7BU%7D%24%3A%0A%0A%20%20%20%20%24%24%0A%20%20%20%20%5Cmathbf%7BM%7D%3D%20%5Cmathbf%7BU%5C%3BS%5C%3BV%5ET%7D%0A%20%20%20%20%24%24%0A%0A%20%20%20%20The%20orthogonal%20polar%20factor%20is%20%24UV%5ET%24%2C%20but%20it%20can%20be%20a%20reflection.%20To%20obtain%20a%20proper%20rotation%2C%20ordered%20singular%20values%20and%20a%20determinant%20correction%20give%3A%0A%0A%20%20%20%20%24%24%0A%20%20%20%20%5Cmathbf%7BR%7D%3D%5Cmathbf%7BU%7D%5C%2C%5Coperatorname%7Bdiag%7D(1%2C1%2C%5Cdet(%5Cmathbf%7BUV%7D%5ET))%5C%2C%5Cmathbf%7BV%7D%5ET%0A%20%20%20%20%24%24%0A%0A%20%20%20%20The%20matrices%20%24%5Cmathbf%7BU%7D%24%20and%20%24%5Cmathbf%7BV%7D%24%20are%20orthogonal%20(determinant%20%24%5Cpm1%24)%3B%20a%20physical%20rotation%20requires%20determinant%20%24%2B1%24.%20The%20order%20of%20the%20factors%20depends%20on%20whether%20the%20cross-covariance%20is%20formed%20as%20%24A%5ETB%24%20or%20%24B%5ETA%24.%20The%20supplied%20%60svdt%60%20uses%20the%20corresponding%20transpose%20convention.%20See%20%5Brotation%20matrices%20and%20handedness%5D(https%3A%2F%2Fmodernrobotics.northwestern.edu%2Fnu-gm-book-resource%2F3-2-1-rotation-matrices-part-1-of-2%2F).%0A%0A%20%20%20%20For%20example%2C%20if%20we%20have%20registered%20the%20position%20of%20four%20markers%20placed%20on%20a%20moving%20segment%20in%20100%20different%20instants%20and%20the%20position%20of%20these%20same%20markers%20during%2C%20what%20is%20known%20in%20Biomechanics%2C%20a%20static%20calibration%20trial%2C%20we%20would%20use%20the%20SVD%20algorithm%20to%20calculate%20the%20100%20rotation%20matrices%20(between%20the%20static%20trials%20and%20the%20100%20instants)%20in%20order%20to%20find%20the%20Cardan%20angles%20for%20each%20instant.%0A%0A%20%20%20%20The%20function%20%60svdt.py%60%20(its%20code%20is%20shown%20at%20the%20end%20of%20this%20text)%20determines%20the%20rotation%20matrix%20(%24R%24)%20and%20the%20translation%20vector%20(%24L%24)%20for%20a%20rigid%20body%20after%20the%20following%20transformation%3A%20%24B%20%3D%20R*A%20%2B%20L%20%2B%20err%24.%20Where%20%24A%24%20and%20%24B%24%20represent%20the%20rigid%20body%20in%20different%20instants%20and%20err%20is%20an%20aleatory%20noise.%20%24A%24%20and%20%24B%24%20are%20matrices%20with%20the%20marker%20coordinates%20at%20different%20instants%20(at%20least%20three%20non-collinear%20markers%20are%20necessary%20to%20determine%20the%203D%20transformation).%3Cbr%3E%0A%20%20%20%20The%20matrix%20%24A%24%20can%20be%20thought%20to%20represent%20a%20local%20coordinate%20system%20(but%20%24A%24%20it's%20not%20a%20basis)%20and%20matrix%20%24B%24%20the%20global%20coordinate%20system.%20The%20operation%20%24P_g%20%3D%20R*P_l%20%2B%20L%24%20calculates%20the%20coordinates%20of%20the%20point%20%24P_l%24%20(expressed%20in%20the%20local%20coordinate%20system)%20in%20the%20global%20coordinate%20system%20(%24P_g%24).%0A%0A%20%20%20%20Let's%20test%20the%20%60svdt%60%20function%3A%0A%20%20%20%20%22%22%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell(hide_code%3DTrue)%0Adef%20_()%3A%0A%20%20%20%20try%3A%0A%20%20%20%20%20%20%20%20from%20notebook_resources%20import%20repo_file%0A%20%20%20%20except%20ModuleNotFoundError%20as%20_error%3A%0A%20%20%20%20%20%20%20%20if%20_error.name%20!%3D%20%22notebook_resources%22%3A%0A%20%20%20%20%20%20%20%20%20%20%20%20raise%0A%20%20%20%20%20%20%20%20raise%20ModuleNotFoundError(%0A%20%20%20%20%20%20%20%20%20%20%20%20%22This%20notebook%20needs%20notebook_resources.py.%20Download%20it%20from%3A%5Cn%22%0A%20%20%20%20%20%20%20%20%20%20%20%20%22https%3A%2F%2Fraw.githubusercontent.com%2FBMClab%2FBMC%2Fmaster%2Ffunctions%2Fnotebook_resources.py%5Cn%22%0A%20%20%20%20%20%20%20%20%20%20%20%20%22Save%20it%20in%20the%20same%20folder%20as%20this%20notebook%2C%20then%20rerun%20this%20cell.%22%2C%0A%20%20%20%20%20%20%20%20%20%20%20%20name%3D%22notebook_resources%22%2C%0A%20%20%20%20%20%20%20%20)%20from%20None%0A%20%20%20%20return%20(repo_file%2C)%0A%0A%0A%40app.cell%0Adef%20_(repo_file)%3A%0A%20%20%20%20%23%20Import%20the%20necessary%20libraries%0A%20%20%20%20import%20numpy%20as%20np%0A%20%20%20%20import%20sys%0A%20%20%20%20sys.path.insert(0%2C%20str(repo_file('functions%2Fsvdt.py').parent))%20%20%23%20add%20to%20pythonpath%0A%20%20%20%20return%20(np%2C)%0A%0A%0A%40app.cell%0Adef%20_(np)%3A%0A%20%20%20%20from%20svdt%20import%20svdt%0A%20%20%20%20_A%20%3D%20np.array(%5B0%2C%200%2C%200%2C%201%2C%200%2C%200%2C%200%2C%201%2C%200%2C%201%2C%201%2C%200%5D)%0A%20%20%20%20_B%20%3D%20np.array(%5B0%2C%200%2C%200%2C%200%2C%201%2C%200%2C%20-1%2C%200%2C%200%2C%20-1%2C%201%2C%200%5D)%0A%20%20%20%20(_R%2C%20_L%2C%20_RMSE)%20%3D%20svdt(_A%2C%20_B)%0A%20%20%20%20print('Rotation%20matrix%3A%5Cn'%2C%20np.around(_R%2C%204))%0A%20%20%20%20print('Translation%20vector%3A%5Cn'%2C%20np.around(_L%2C%204))%0A%20%20%20%20print('RMSE%3A%5Cn'%2C%20np.around(_RMSE%2C%204))%0A%20%20%20%20_A%20%3D%20np.array(%5B%5B0%2C%200%2C%200%5D%2C%20%5B1%2C%200%2C%200%5D%2C%20%5B0%2C%201%2C%200%5D%2C%20%5B1%2C%201%2C%200%5D%5D)%0A%20%20%20%20_B%20%3D%20np.array(%5B%5B0%2C%200%2C%200%5D%2C%20%5B0%2C%201%2C%200%5D%2C%20%5B-1%2C%200%2C%200%5D%2C%20%5B-1%2C%201%2C%200%5D%5D)%0A%20%20%20%20(_R%2C%20_L%2C%20_RMSE)%20%3D%20svdt(_A%2C%20_B%2C%20order%3D'row')%0A%20%20%20%20print('Rotation%20matrix%3A%5Cn'%2C%20np.around(_R%2C%204))%0A%20%20%20%20print('Translation%20vector%3A%5Cn'%2C%20np.around(_L%2C%204))%0A%20%20%20%20print('RMSE%3A%5Cn'%2C%20np.around(_RMSE%2C%204))%0A%20%20%20%20return%20(svdt%2C)%0A%0A%0A%40app.cell(hide_code%3DTrue)%0Adef%20_(mo)%3A%0A%20%20%20%20mo.md(r%22%22%22%0A%20%20%20%20**Exercise%201.**%20Apply%20the%20quarter-turn%20matrix%20to%20%24(1%2C0%2C0)%24%20and%20%24(0%2C1%2C0)%24.%20Does%20it%20preserve%20distance%20and%20handedness%3F%0A%0A%20%20%20%20%2F%2F%2F%20details%20%7C%20Worked%20solution%20%E2%80%94%20Exercise%201%0A%0A%20%20%20%20With%20%24R%3D%5B%5B0%2C-1%2C0%5D%2C%5B1%2C0%2C0%5D%2C%5B0%2C0%2C1%5D%5D%24%2C%20the%20images%20are%20%24(0%2C1%2C0)%24%20and%20%24(-1%2C0%2C0)%24.%20%24R%5ETR%3DI%24%20preserves%20lengths%3B%20%24%5Cdet%20R%3D1%24%20preserves%20handedness.%0A%0A%20%20%20%20%2F%2F%2F%0A%20%20%20%20%22%22%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell(hide_code%3DTrue)%0Adef%20_(mo)%3A%0A%20%20%20%20mo.md(r%22%22%22%0A%20%20%20%20For%20the%20matrix%20of%20a%20pure%20rotation%20around%20the%20z%20axis%2C%20the%20element%20in%20the%20first%20row%20and%20second%20column%20is%20%24-sin%5Cgamma%24%2C%20which%20means%20the%20rotation%20was%20%2490%5Eo%24%2C%20as%20expected.%0A%0A%20%20%20%20A%20typical%20use%20of%20the%20%60svdt%60%20function%20is%20to%20calculate%20the%20transformation%20between%20%24A%24%20and%20%24B%24%20(%24B%20%3D%20R*A%20%2B%20L%24)%2C%20where%20%24A%24%20is%20the%20matrix%20with%20the%20markers%20data%20in%20one%20instant%20(the%20calibration%20or%20static%20trial)%20and%20%24B%24%20is%20the%20matrix%20with%20the%20markers%20data%20of%20more%20than%20one%20instant%20(the%20dynamic%20trial).%3Cbr%3E%0A%20%20%20%20Input%20%24A%24%20as%20a%201D%20array%20%60%5Bx1%2C%20y1%2C%20z1%2C%20...%2C%20xn%2C%20yn%2C%20zn%5D%60%20where%20%60n%60%20is%20the%20number%20of%20markers%20and%20%24B%24%20a%202D%20array%20with%20the%20different%20instants%20as%20rows%20(like%20in%20%24A%24).%3Cbr%3E%0A%20%20%20%20The%20output%20%24R%24%20has%20the%20shape%20%60(3%2C%203%2C%20tn)%60%2C%20where%20%60tn%60%20is%20the%20number%20of%20instants%2C%20%24L%24%20the%20shape%20%60(tn%2C%203)%60%2C%20and%20%24RMSE%24%20the%20shape%20%60(tn)%60.%20If%20%60tn%60%20is%20equal%20to%20one%2C%20the%20outputs%20have%20the%20same%20shape%20as%20in%20%60svdt%60%20(the%20last%20dimension%20of%20the%20outputs%20above%20is%20dropped).%0A%0A%20%20%20%20Let's%20show%20this%20case%3A%0A%20%20%20%20%22%22%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell%0Adef%20_(np%2C%20svdt)%3A%0A%20%20%20%20_A%20%3D%20np.array(%5B1%2C%200%2C%200%2C%200%2C%201%2C%200%2C%200%2C%200%2C%201%5D)%0A%20%20%20%20_B%20%3D%20np.array(%5B0%2C%201%2C%200%2C%20-1%2C%200%2C%200%2C%200%2C%200%2C%201%5D)%0A%20%20%20%20_B%20%3D%20np.vstack((_B%2C%20_B))%0A%20%20%20%20(_R%2C%20_L%2C%20_RMSE)%20%3D%20svdt(_A%2C%20_B)%0A%20%20%20%20print('Rotation%20matrix%3A%5Cn'%2C%20np.around(_R%2C%204))%0A%20%20%20%20print('Translation%20vector%3A%5Cn'%2C%20np.around(_L%2C%204))%0A%20%20%20%20print('RMSE%3A%5Cn'%2C%20np.around(_RMSE%2C%204))%0A%20%20%20%20return%0A%0A%0A%40app.cell(hide_code%3DTrue)%0Adef%20_(mo)%3A%0A%20%20%20%20mo.md(r%22%22%22%0A%20%20%20%20**Exercise%202.**%20All%20target%20points%20are%20additionally%20shifted%20by%20%24(0.02%2C0%2C0)%24%20m.%20What%20changes%20in%20the%20fitted%20transformation%20and%20residual%3F%0A%0A%20%20%20%20%2F%2F%2F%20details%20%7C%20Worked%20solution%20%E2%80%94%20Exercise%202%0A%0A%20%20%20%20The%20centroid%20shifts%20by%20the%20same%20amount%2C%20while%20centered%20target%20coordinates%20are%20unchanged.%20The%20rotation%20therefore%20stays%20the%20same%3B%20translation%20increases%20by%20%24(0.02%2C0%2C0)%24%20m%20and%20residuals%20do%20not%20change.%0A%0A%20%20%20%20%2F%2F%2F%0A%20%20%20%20%22%22%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell(hide_code%3DTrue)%0Adef%20_(mo)%3A%0A%20%20%20%20mo.md(r%22%22%22%0A%20%20%20%20**Exercise%203.**%20Why%20do%20three%20collinear%20markers%20fail%20to%20determine%20a%20unique%203D%20rotation%2C%20although%20an%20SVD%20can%20still%20return%20numbers%3F%0A%0A%20%20%20%20%2F%2F%2F%20details%20%7C%20Worked%20solution%20%E2%80%94%20Exercise%203%0A%0A%20%20%20%20Rotating%20about%20the%20marker%20line%20leaves%20every%20marker%20unchanged.%20That%20rotational%20degree%20of%20freedom%20is%20unobserved%2C%20so%20a%20numerical%20result%20is%20not%20a%20uniquely%20identified%20physical%20orientation.%0A%0A%20%20%20%20%2F%2F%2F%0A%20%20%20%20%22%22%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell(hide_code%3DTrue)%0Adef%20_(mo)%3A%0A%20%20%20%20mo.md(r%22%22%22%0A%20%20%20%20%23%23%20Checkpoint%20questions%0A%0A%20%20%20%201.%20What%20does%20determinant%20%E2%88%921%20mean%3F%0A%20%20%20%202.%20Does%20a%20small%20residual%20guarantee%20correct%20anatomical%20landmarks%3F%0A%20%20%20%203.%20Which%20translation%20accompanies%20a%20fitted%20rotation%3F%0A%0A%20%20%20%20%2F%2F%2F%20details%20%7C%20Checkpoint%20answers%0A%0A%20%20%20%201.%20A%20reflection%2C%20not%20a%20proper%20rotation.%202.%20No%3B%20systematic%20anatomical%20placement%20error%20can%20coexist%20with%20a%20rigid%20cluster%20and%20small%20residual.%203.%20The%20target%20centroid%20minus%20the%20rotated%20source%20centroid.%0A%0A%20%20%20%20%2F%2F%2F%0A%20%20%20%20%22%22%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell(hide_code%3DTrue)%0Adef%20_(mo)%3A%0A%20%20%20%20mo.md(r%22%22%22%0A%20%20%20%20%23%23%20Problems%0A%0A%20%20%20%201.%20A%20rigid%20set%20is%20rotated%2090%C2%B0%20about%20positive%20%24z%24%20and%20translated%20by%20%24(1%2C2%2C3)%24%20m.%20Give%20the%20rotation%2C%20translation%20and%20image%20of%20%24(2%2C0%2C0)%24%20m.%0A%20%20%20%202.%20Fit%20a%20reflected%2C%20noncoplanar%20marker%20set%20with%20an%20unconstrained%20orthogonal%20map%20and%20with%20a%20proper%20rotation.%20Which%20can%20have%20zero%20residual%3F%0A%20%20%20%203.%20Is%20a%20cluster%20that%20slides%20rigidly%20on%20the%20skin%20detectable%20from%20its%20internal%20distances%20alone%3F%0A%20%20%20%20%22%22%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell(hide_code%3DTrue)%0Adef%20_(mo)%3A%0A%20%20%20%20mo.md(r%22%22%22%0A%20%20%20%20%2F%2F%2F%20details%20%7C%20Final%20answers%20to%20the%20problems%0A%0A%20%20%20%201.%20%24R%3D%5B%5B0%2C-1%2C0%5D%2C%5B1%2C0%2C0%5D%2C%5B0%2C0%2C1%5D%5D%24%2C%20%24L%3D(1%2C2%2C3)%24%20m%3B%20image%20%24(1%2C4%2C3)%24%20m.%0A%20%20%20%202.%20The%20orthogonal%20reflection%20can%20have%20zero%20residual%3B%20the%20proper%20rotation%20generally%20cannot.%0A%20%20%20%203.%20No%3B%20an%20independent%20anatomical%20reference%20or%20bone%20measurement%20is%20needed.%0A%0A%20%20%20%20%2F%2F%2F%0A%20%20%20%20%22%22%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell(hide_code%3DTrue)%0Adef%20_(mo)%3A%0A%20%20%20%20mo.md(r%22%22%22%0A%20%20%20%20%23%23%23%20Function%20%60svdt.py%60%0A%20%20%20%20%22%22%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell%0Adef%20_(np)%3A%0A%20%20%20%20%22%22%22Calculates%20the%20transformation%20between%20two%20coordinate%20systems%20using%20SVD.%22%22%22%0A%20%20%20%20__author__%20%3D%20'Marcos%20Duarte%2C%20https%3A%2F%2Fgithub.com%2Fdemotu%2FBMC'%0A%20%20%20%20__version__%20%3D%20'1.0.1'%0A%20%20%20%20__license__%20%3D%20'MIT'%0A%0A%20%20%20%20def%20svdt_1(A%2C%20B%2C%20order%3D'col')%3A%0A%20%20%20%20%20%20%20%20%22%22%22Calculates%20the%20transformation%20between%20two%20coordinate%20systems%20using%20SVD.%0A%0A%20%20%20%20%20%20%20%20This%20function%20determines%20the%20rotation%20matrix%20(R)%20and%20the%20translation%20vector%0A%20%20%20%20%20%20%20%20(L)%20for%20a%20rigid%20body%20after%20the%20following%20transformation%20%5B1%5D_%2C%20%5B2%5D_%3A%0A%20%20%20%20%20%20%20%20B%20%3D%20R*A%20%2B%20L%20%2B%20err.%0A%20%20%20%20%20%20%20%20Where%20A%20and%20B%20represents%20the%20rigid%20body%20in%20different%20instants%20and%20err%20is%20an%0A%20%20%20%20%20%20%20%20aleatory%20noise%20(which%20should%20be%20zero%20for%20a%20perfect%20rigid%20body).%20A%20and%20B%20are%0A%20%20%20%20%20%20%20%20matrices%20with%20the%20marker%20coordinates%20at%20different%20instants%20(at%20least%20three%0A%20%20%20%20%20%20%20%20non-collinear%20markers%20are%20necessary%20to%20determine%20the%203D%20transformation).%0A%0A%20%20%20%20%20%20%20%20The%20matrix%20A%20can%20be%20thought%20to%20represent%20a%20local%20coordinate%20system%20(but%20A%0A%20%20%20%20%20%20%20%20it's%20not%20a%20basis)%20and%20matrix%20B%20the%20global%20coordinate%20system.%20The%20operation%0A%20%20%20%20%20%20%20%20Pg%20%3D%20R*Pl%20%2B%20L%20calculates%20the%20coordinates%20of%20the%20point%20Pl%20(expressed%20in%20the%0A%20%20%20%20%20%20%20%20local%20coordinate%20system)%20in%20the%20global%20coordinate%20system%20(Pg).%0A%0A%20%20%20%20%20%20%20%20A%20typical%20use%20of%20the%20svdt%20function%20is%20to%20calculate%20the%20transformation%0A%20%20%20%20%20%20%20%20between%20A%20and%20B%20(B%20%3D%20R*A%20%2B%20L)%2C%20where%20A%20is%20the%20matrix%20with%20the%20markers%20data%0A%20%20%20%20%20%20%20%20in%20one%20instant%20(the%20calibration%20or%20static%20trial)%20and%20B%20is%20the%20matrix%20with%0A%20%20%20%20%20%20%20%20the%20markers%20data%20for%20one%20or%20more%20instants%20(the%20dynamic%20trial).%0A%0A%20%20%20%20%20%20%20%20If%20the%20parameter%20order%3D'row'%2C%20the%20A%20and%20B%20parameters%20should%20have%20the%20shape%0A%20%20%20%20%20%20%20%20(n%2C%203)%2C%20i.e.%2C%20n%20rows%20and%203%20columns%2C%20where%20n%20is%20the%20number%20of%20markers.%0A%20%20%20%20%20%20%20%20If%20order%3D'col'%2C%20A%20can%20be%20a%201D%20array%20with%20the%20shape%20(n*3%2C%20like%0A%20%20%20%20%20%20%20%20%5Bx1%2C%20y1%2C%20z1%2C%20...%2C%20xn%2C%20yn%2C%20zn%5D%20and%20B%20a%201D%20array%20with%20the%20same%20structure%20of%20A%0A%20%20%20%20%20%20%20%20or%20a%202D%20array%20with%20the%20shape%20(ni%2C%20n*3)%20where%20ni%20is%20the%20number%20of%20instants.%0A%20%20%20%20%20%20%20%20The%20output%20R%20has%20the%20shape%20(ni%2C%203%2C%203)%2C%20L%20has%20the%20shape%20(ni%2C%203)%2C%20and%20RMSE%0A%20%20%20%20%20%20%20%20has%20the%20shape%20(ni%2C).%20If%20ni%20is%20equal%20to%20one%2C%20the%20outputs%20will%20have%20the%0A%20%20%20%20%20%20%20%20singleton%20dimension%20dropped.%0A%0A%20%20%20%20%20%20%20%20Part%20of%20this%20code%20is%20based%20on%20the%20programs%20written%20by%20Alberto%20Leardini%2C%0A%20%20%20%20%20%20%20%20Christoph%20Reinschmidt%2C%20and%20Ton%20van%20den%20Bogert.%0A%0A%20%20%20%20%20%20%20%20Parameters%0A%20%20%20%20%20%20%20%20----------%0A%20%20%20%20%20%20%20%20A%20%20%20%3A%20Numpy%20array%0A%20%20%20%20%20%20%20%20%20%20%20%20Coordinates%20%5Bx%2Cy%2Cz%5D%20of%20at%20least%20three%20markers%20with%20two%20possible%20shapes%3A%0A%20%20%20%20%20%20%20%20%20%20%20%20order%3D'row'%3A%202D%20array%20(n%2C%203)%2C%20where%20n%20is%20the%20number%20of%20markers.%0A%20%20%20%20%20%20%20%20%20%20%20%20order%3D'col'%3A%201D%20array%20(3*nmarkers%2C)%20like%20%5Bx1%2C%20y1%2C%20z1%2C%20...%2C%20xn%2C%20yn%2C%20zn%5D.%0A%0A%20%20%20%20%20%20%20%20B%20%20%20%3A%202D%20Numpy%20array%0A%20%20%20%20%20%20%20%20%20%20%20%20Coordinates%20%5Bx%2Cy%2Cz%5D%20of%20at%20least%20three%20markers%20with%20two%20possible%20shapes%3A%0A%20%20%20%20%20%20%20%20%20%20%20%20order%3D'row'%3A%202D%20array%20(n%2C%203)%2C%20where%20n%20is%20the%20number%20of%20markers.%0A%20%20%20%20%20%20%20%20%20%20%20%20order%3D'col'%3A%202D%20array%20(ni%2C%20n*3)%2C%20where%20ni%20is%20the%20number%20of%20instants.%0A%20%20%20%20%20%20%20%20%20%20%20%20If%20ni%3D1%2C%20B%20is%20a%201D%20array%20like%20A.%0A%0A%20%20%20%20%20%20%20%20order%20%3A%20string%0A%20%20%20%20%20%20%20%20%20%20%20%20'col'%3A%20specifies%20that%20A%20and%20B%20are%20column%20oriented%20(default).%0A%20%20%20%20%20%20%20%20%20%20%20%20'row'%3A%20specifies%20that%20A%20and%20B%20are%20row%20oriented.%0A%0A%20%20%20%20%20%20%20%20Returns%0A%20%20%20%20%20%20%20%20-------%0A%20%20%20%20%20%20%20%20R%20%20%20%3A%20Numpy%20array%0A%20%20%20%20%20%20%20%20%20%20%20%20Rotation%20matrix%20between%20A%20and%20B%20with%20two%20possible%20shapes%3A%0A%20%20%20%20%20%20%20%20%20%20%20%20order%3D'row'%3A%20(3%2C%203).%0A%20%20%20%20%20%20%20%20%20%20%20%20order%3D'col'%3A%20(ni%2C%203%2C%203)%2C%20where%20ni%20is%20the%20number%20of%20instants.%0A%20%20%20%20%20%20%20%20%20%20%20%20If%20ni%3D1%2C%20R%20will%20have%20the%20singleton%20dimension%20dropped.%0A%0A%20%20%20%20%20%20%20%20L%20%20%20%3A%20Numpy%20array%0A%20%20%20%20%20%20%20%20%20%20%20%20Translation%20vector%20between%20A%20and%20B%20with%20two%20possible%20shapes%3A%0A%20%20%20%20%20%20%20%20%20%20%20%20order%3D'row'%3A%20(3%2C)%20if%20order%20%3D%20'row'.%0A%20%20%20%20%20%20%20%20%20%20%20%20order%3D'col'%3A%20(ni%2C%203)%2C%20where%20ni%20is%20the%20number%20of%20instants.%0A%20%20%20%20%20%20%20%20%20%20%20%20If%20ni%3D1%2C%20L%20will%20have%20the%20singleton%20dimension%20dropped.%0A%0A%20%20%20%20%20%20%20%20RMSE%20%3A%20array%0A%20%20%20%20%20%20%20%20%20%20%20%20Root-mean-squared%20error%20for%20the%20rigid%20body%20model%3A%20B%20%3D%20R*A%20%2B%20L%20%2B%20err%0A%20%20%20%20%20%20%20%20%20%20%20%20with%20two%20possible%20shapes%3A%0A%20%20%20%20%20%20%20%20%20%20%20%20order%3D'row'%3A%20(1%2C).%0A%20%20%20%20%20%20%20%20%20%20%20%20order%3D'col'%3A%20(ni%2C)%2C%20where%20ni%20is%20the%20number%20of%20instants.%0A%0A%20%20%20%20%20%20%20%20See%20Also%0A%20%20%20%20%20%20%20%20--------%0A%20%20%20%20%20%20%20%20numpy.linalg.svd%0A%0A%20%20%20%20%20%20%20%20Notes%0A%20%20%20%20%20%20%20%20-----%0A%20%20%20%20%20%20%20%20The%20singular%20value%20decomposition%20(SVD)%20algorithm%20decomposes%20a%20matrix%20M%0A%20%20%20%20%20%20%20%20(which%20represents%20a%20general%20transformation%20between%20two%20coordinate%20systems)%0A%20%20%20%20%20%20%20%20into%20three%20simple%20transformations%20%5B3%5D_%3A%20a%20rotation%20Vt%2C%20a%20scaling%20factor%20S%0A%20%20%20%20%20%20%20%20along%20the%20%20rotated%20axes%20and%20a%20second%20rotation%20U%3A%20M%20%3D%20U*S*Vt.%0A%20%20%20%20%20%20%20%20The%20rotation%20matrix%20is%20given%20by%3A%20R%20%3D%20U*Vt.%0A%0A%20%20%20%20%20%20%20%20References%0A%20%20%20%20%20%20%20%20----------%0A%20%20%20%20%20%20%20%20..%20%5B1%5D%20Soderkvist%2C%20Kedin%20(1993)%20Journal%20of%20Biomechanics%2C%2026%2C%201473-1477.%0A%20%20%20%20%20%20%20%20..%20%5B2%5D%20http%3A%2F%2Fwww.kwon3d.com%2Ftheory%2Fjkinem%2Frotmat.html.%0A%20%20%20%20%20%20%20%20..%20%5B3%5D%20http%3A%2F%2Fen.wikipedia.org%2Fwiki%2FSingular_value_decomposition.%0A%0A%0A%20%20%20%20%20%20%20%20Examples%0A%20%20%20%20%20%20%20%20--------%0A%20%20%20%20%20%20%20%20%3E%3E%3E%20import%20numpy%20as%20np%0A%20%20%20%20%20%20%20%20%3E%3E%3E%20from%20svdt%20import%20svdt%0A%20%20%20%20%20%20%20%20%3E%3E%3E%20A%20%3D%20np.array(%5B0%2C0%2C0%2C%201%2C0%2C0%2C%20%200%2C1%2C0%2C%20%201%2C1%2C0%5D)%20%20%23%20four%20markers%0A%20%20%20%20%20%20%20%20%3E%3E%3E%20B%20%3D%20np.array(%5B0%2C0%2C0%2C%200%2C1%2C0%2C%20-1%2C0%2C0%2C%20-1%2C1%2C0%5D)%20%20%23%20four%20markers%0A%20%20%20%20%20%20%20%20%3E%3E%3E%20R%2C%20L%2C%20RMSE%20%3D%20svdt(A%2C%20B)%0A%20%20%20%20%20%20%20%20%3E%3E%3E%20B%20%3D%20np.vstack((B%2C%20B))%20%20%23%20simulate%20two%20instants%20(two%20rows)%0A%20%20%20%20%20%20%20%20%3E%3E%3E%20R%2C%20L%2C%20RMSE%20%3D%20svdt(A%2C%20B)%0A%20%20%20%20%20%20%20%20%3E%3E%3E%20A%20%3D%20np.array(%5B%5B0%2C0%2C0%5D%2C%20%5B1%2C0%2C0%5D%2C%20%5B%200%2C1%2C0%5D%2C%20%5B%201%2C1%2C0%5D%5D)%20%20%23%20four%20markers%0A%20%20%20%20%20%20%20%20%3E%3E%3E%20B%20%3D%20np.array(%5B%5B0%2C0%2C0%5D%2C%20%5B0%2C1%2C0%5D%2C%20%5B-1%2C0%2C0%5D%2C%20%5B-1%2C1%2C0%5D%5D)%20%20%23%20four%20markers%0A%20%20%20%20%20%20%20%20%3E%3E%3E%20R%2C%20L%2C%20RMSE%20%3D%20svdt(A%2C%20B%2C%20order%3D'row')%0A%20%20%20%20%20%20%20%20%22%22%22%0A%20%20%20%20%20%20%20%20(_A%2C%20_B)%20%3D%20(np.asarray(_A)%2C%20np.asarray(_B))%0A%20%20%20%20%20%20%20%20if%20order%20%3D%3D%20'row'%20or%20_B.ndim%20%3D%3D%201%3A%0A%20%20%20%20%20%20%20%20%20%20%20%20if%20_B.ndim%20%3D%3D%201%3A%0A%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20_A%20%3D%20_A.reshape(int(_A.size%20%2F%203)%2C%203)%0A%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20_B%20%3D%20_B.reshape(int(_B.size%20%2F%203)%2C%203)%0A%20%20%20%20%20%20%20%20%20%20%20%20(_R%2C%20_L%2C%20_RMSE)%20%3D%20svd(_A%2C%20_B)%0A%20%20%20%20%20%20%20%20else%3A%0A%20%20%20%20%20%20%20%20%20%20%20%20_A%20%3D%20_A.reshape(int(_A.size%20%2F%203)%2C%203)%0A%20%20%20%20%20%20%20%20%20%20%20%20ni%20%3D%20_B.shape%5B0%5D%0A%20%20%20%20%20%20%20%20%20%20%20%20_R%20%3D%20np.empty((ni%2C%203%2C%203))%0A%20%20%20%20%20%20%20%20%20%20%20%20_L%20%3D%20np.empty((ni%2C%203))%0A%20%20%20%20%20%20%20%20%20%20%20%20_RMSE%20%3D%20np.empty(ni)%0A%20%20%20%20%20%20%20%20%20%20%20%20for%20i%20in%20range(ni)%3A%0A%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20(_R%5Bi%2C%20%3A%2C%20%3A%5D%2C%20_L%5Bi%2C%20%3A%5D%2C%20_RMSE%5Bi%5D)%20%3D%20svd(_A%2C%20_B%5Bi%2C%20%3A%5D.reshape(_A.shape))%0A%20%20%20%20%20%20%20%20return%20(_R%2C%20_L%2C%20_RMSE)%0A%0A%20%20%20%20def%20svd(A%2C%20B)%3A%0A%20%20%20%20%20%20%20%20%22%22%22Calculates%20the%20transformation%20between%20two%20coordinate%20systems%20using%20SVD.%0A%0A%20%20%20%20%20%20%20%20See%20the%20help%20of%20the%20svdt%20function.%0A%0A%20%20%20%20%20%20%20%20Parameters%0A%20%20%20%20%20%20%20%20----------%0A%20%20%20%20%20%20%20%20A%20%20%20%3A%202D%20Numpy%20array%20(n%2C%203)%2C%20where%20n%20is%20the%20number%20of%20markers.%0A%20%20%20%20%20%20%20%20%20%20%20%20Coordinates%20%5Bx%2Cy%2Cz%5D%20of%20at%20least%20three%20markers%0A%20%20%20%20%20%20%20%20B%20%20%20%3A%202D%20Numpy%20array%20(n%2C%203)%2C%20where%20n%20is%20the%20number%20of%20markers.%0A%20%20%20%20%20%20%20%20%20%20%20%20Coordinates%20%5Bx%2Cy%2Cz%5D%20of%20at%20least%20three%20markers%0A%0A%20%20%20%20%20%20%20%20Returns%0A%20%20%20%20%20%20%20%20-------%0A%20%20%20%20%20%20%20%20R%20%20%20%20%3A%202D%20Numpy%20array%20(3%2C%203)%0A%20%20%20%20%20%20%20%20%20%20%20%20%20Rotation%20matrix%20between%20A%20and%20B%0A%20%20%20%20%20%20%20%20L%20%20%20%20%3A%201D%20Numpy%20array%20(3%2C)%0A%20%20%20%20%20%20%20%20%20%20%20%20%20Translation%20vector%20between%20A%20and%20B%0A%20%20%20%20%20%20%20%20RMSE%20%3A%20float%0A%20%20%20%20%20%20%20%20%20%20%20%20%20Root-mean-squared%20error%20for%20the%20rigid%20body%20model%3A%20B%20%3D%20R*A%20%2B%20L%20%2B%20err.%0A%0A%20%20%20%20%20%20%20%20See%20Also%0A%20%20%20%20%20%20%20%20--------%0A%20%20%20%20%20%20%20%20numpy.linalg.svd%0A%20%20%20%20%20%20%20%20%22%22%22%0A%20%20%20%20%20%20%20%20Am%20%3D%20np.mean(_A%2C%20axis%3D0)%0A%20%20%20%20%20%20%20%20Bm%20%3D%20np.mean(_B%2C%20axis%3D0)%0A%20%20%20%20%20%20%20%20M%20%3D%20np.dot((_B%20-%20Bm).T%2C%20_A%20-%20Am)%0A%20%20%20%20%20%20%20%20(U%2C%20S%2C%20Vt)%20%3D%20np.linalg.svd(M)%0A%20%20%20%20%20%20%20%20_R%20%3D%20np.dot(U%2C%20np.dot(np.diag(%5B1%2C%201%2C%20np.linalg.det(np.dot(U%2C%20Vt))%5D)%2C%20Vt))%0A%20%20%20%20%20%20%20%20_L%20%3D%20_B.mean(0)%20-%20np.dot(_R%2C%20_A.mean(0))%0A%20%20%20%20%20%20%20%20err%20%3D%200%0A%20%20%20%20%20%20%20%20for%20i%20in%20range(_A.shape%5B0%5D)%3A%0A%20%20%20%20%20%20%20%20%20%20%20%20Bp%20%3D%20np.dot(_R%2C%20_A%5Bi%2C%20%3A%5D)%20%2B%20_L%0A%20%20%20%20%20%20%20%20%20%20%20%20err%20%3D%20err%20%2B%20np.sum((Bp%20-%20_B%5Bi%2C%20%3A%5D)%20**%202)%0A%20%20%20%20%20%20%20%20_RMSE%20%3D%20np.sqrt(err%20%2F%20_A.shape%5B0%5D%20%2F%203)%0A%20%20%20%20%20%20%20%20return%20(_R%2C%20_L%2C%20_RMSE)%0A%0A%20%20%20%20return%0A%0A%0A%40app.cell%0Adef%20_()%3A%0A%20%20%20%20import%20marimo%20as%20mo%0A%0A%20%20%20%20return%20(mo%2C)%0A%0A%0A%40app.cell(hide_code%3DTrue)%0Adef%20_(mo)%3A%0A%20%20%20%20mo.md(r%22%22%22%0A%20%20%20%20%23%23%20Go%20deeper%0A%0A%20%20%20%20Read%20%5BCoordinateSystem%5D(https%3A%2F%2Fmolab.marimo.io%2Fgithub%2FBMClab%2FBMC%2Fblob%2Fmaster%2Fnotebooks%2Fcoordinate_system.py)%20for%20the%20complementary%20three-marker%20construction.%0A%0A%20%20%20%20%23%23%23%20Video%20lectures%20on%20the%20Internet%0A%0A%20%20%20%20%5BLynch%20and%20Park%3A%20rotation%20matrices%5D(https%3A%2F%2Fmodernrobotics.northwestern.edu%2Fnu-gm-book-resource%2F3-2-1-rotation-matrices-part-1-of-2%2F)%20explains%20rigid%20geometry%20and%20coordinate%20changes%3B%20compare%20those%20distance-preserving%20transformations%20with%20the%20fitted%20mappings%20in%20this%20guide.%0A%20%20%20%20%22%22%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell(hide_code%3DTrue)%0Adef%20_(mo)%3A%0A%20%20%20%20mo.md(r%22%22%22%0A%20%20%20%20%23%23%20References%0A%0A%20%20%20%20-%20Cappozzo%20A%2C%20Cappello%20A%2C%20Della%20Croce%20U%2C%20Pensalfini%20F%20(1997)%20%5BSurface-marker%20cluster%20design%20criteria%20for%203-D%20bone%20movement%20reconstruction%5D(https%3A%2F%2Fpubmed.ncbi.nlm.nih.gov%2F9401217).%20IEEE%20Trans%20Biomed%20Eng.%2C%2044%3A1165-1174.%0A%0A%20%20%20%20-%20Challis%20JH%20(1995).%20%5BA%20procedure%20for%20determining%20rigid%20body%20transformation%20parameters%5D(https%3A%2F%2Fpubmed.ncbi.nlm.nih.gov%2F7601872).%20Journal%20of%20Biomechanics%2C%2028%2C%20733-737.%0A%0A%20%20%20%20-%20S%C3%B6derkvist%20I%2C%20Wedin%20PA%20(1993)%20%5BDetermining%20the%20movements%20of%20the%20skeleton%20using%20well-configured%20markers%5D(https%3A%2F%2Fpubmed.ncbi.nlm.nih.gov%2F8308052).%20Journal%20of%20Biomechanics%2C%2026%2C%201473-1477.%0A%20%20%20%20%22%22%22)%0A%20%20%20%20return%0A%0A%0Aif%20__name__%20%3D%3D%20%22__main__%22%3A%0A%20%20%20%20app.run()%0A
63861e574b210b0f18543a2110d1a6b8