Stochastic Methods in Robotics (Probability, Fourier ... · • Pose Distributions for Nonholonomic...
Transcript of Stochastic Methods in Robotics (Probability, Fourier ... · • Pose Distributions for Nonholonomic...
![Page 1: Stochastic Methods in Robotics (Probability, Fourier ... · • Pose Distributions for Nonholonomic Systems (Mobile Robots and Steerable Needles) • Lie-Theoretic Estimation and](https://reader033.fdocuments.us/reader033/viewer/2022043017/5f3976ef5dc58635dd56c0a5/html5/thumbnails/1.jpg)
Stochastic Methods in Robotics (Probability, Fourier Analysis, Lie Groups and Applications)
Gregory S. Chirikjian
Department of Mechanical Engineering Johns Hopkins University
Georgia Tech, June 12-22
![Page 2: Stochastic Methods in Robotics (Probability, Fourier ... · • Pose Distributions for Nonholonomic Systems (Mobile Robots and Steerable Needles) • Lie-Theoretic Estimation and](https://reader033.fdocuments.us/reader033/viewer/2022043017/5f3976ef5dc58635dd56c0a5/html5/thumbnails/2.jpg)
Contents
• Overview of Problem Domains• Introduction to Classical Probability, Convolution and Fourier Analysis• Stochastic Differential Equations• Lie Groups – What are they and how to use them (Invariant Integration, Differential
Operators, Convolution, Fourier Analysis, Gaussian Distributions)
• Workspace Generation and Inverse Kinematics of Highly Articulated Manipulators
• Calculus of Variations• Conformational Statistics of Stiff Macromolecules (DNA)• Pose Distributions for Nonholonomic Systems (Mobile Robots
and Steerable Needles)• Lie-Theoretic Estimation and Filtering Problems• New Insights into AX=XB Sensor Calibration • Coset-Spaces of Lie Groups by Discrete Subgroups • Action Recognition
![Page 3: Stochastic Methods in Robotics (Probability, Fourier ... · • Pose Distributions for Nonholonomic Systems (Mobile Robots and Steerable Needles) • Lie-Theoretic Estimation and](https://reader033.fdocuments.us/reader033/viewer/2022043017/5f3976ef5dc58635dd56c0a5/html5/thumbnails/3.jpg)
Binary Manipulators in Our Lab
215 3.3104
configurations
236 6.91010
configurations2.1106
configurations
![Page 4: Stochastic Methods in Robotics (Probability, Fourier ... · • Pose Distributions for Nonholonomic Systems (Mobile Robots and Steerable Needles) • Lie-Theoretic Estimation and](https://reader033.fdocuments.us/reader033/viewer/2022043017/5f3976ef5dc58635dd56c0a5/html5/thumbnails/4.jpg)
Workspace Density
• It describes the density of the reachable frames in the work space.
• It is a probabilistic measure-ment of accuracy over the workspace.
Ebert-Uphoff, I., Chirikjian, G.S., ``Inverse Kinematics of Discretely Actuated
Hyper-Redundant Manipulators Using Workspace Densities,'' ICRA’96, pp. 139-145
Long, A.W., Wolfe, K.C., Mashner, M.J., Chirikjian, G.S., ``The Banana Distribution
is Gaussian: A Localization Study with Exponential Coordinates,'‘ RSS, Sydney,
NSW, Australia, July 09 - July 13, 2012.
![Page 5: Stochastic Methods in Robotics (Probability, Fourier ... · • Pose Distributions for Nonholonomic Systems (Mobile Robots and Steerable Needles) • Lie-Theoretic Estimation and](https://reader033.fdocuments.us/reader033/viewer/2022043017/5f3976ef5dc58635dd56c0a5/html5/thumbnails/5.jpg)
SDE for the Kinematic Cart
(Zhou and Chirikjian, ICRA 2003)
![Page 6: Stochastic Methods in Robotics (Probability, Fourier ... · • Pose Distributions for Nonholonomic Systems (Mobile Robots and Steerable Needles) • Lie-Theoretic Estimation and](https://reader033.fdocuments.us/reader033/viewer/2022043017/5f3976ef5dc58635dd56c0a5/html5/thumbnails/6.jpg)
A. Long, K. Wolfe, M. Mashner, G.
Chirikjian, ``The Banana Distribution
is Gaussian’’ RSS 2012
![Page 7: Stochastic Methods in Robotics (Probability, Fourier ... · • Pose Distributions for Nonholonomic Systems (Mobile Robots and Steerable Needles) • Lie-Theoretic Estimation and](https://reader033.fdocuments.us/reader033/viewer/2022043017/5f3976ef5dc58635dd56c0a5/html5/thumbnails/7.jpg)
![Page 8: Stochastic Methods in Robotics (Probability, Fourier ... · • Pose Distributions for Nonholonomic Systems (Mobile Robots and Steerable Needles) • Lie-Theoretic Estimation and](https://reader033.fdocuments.us/reader033/viewer/2022043017/5f3976ef5dc58635dd56c0a5/html5/thumbnails/8.jpg)
Examples of Lie Groups
![Page 9: Stochastic Methods in Robotics (Probability, Fourier ... · • Pose Distributions for Nonholonomic Systems (Mobile Robots and Steerable Needles) • Lie-Theoretic Estimation and](https://reader033.fdocuments.us/reader033/viewer/2022043017/5f3976ef5dc58635dd56c0a5/html5/thumbnails/9.jpg)
PART 1: INTRODUCTORY MATHEMATICS
9
![Page 10: Stochastic Methods in Robotics (Probability, Fourier ... · • Pose Distributions for Nonholonomic Systems (Mobile Robots and Steerable Needles) • Lie-Theoretic Estimation and](https://reader033.fdocuments.us/reader033/viewer/2022043017/5f3976ef5dc58635dd56c0a5/html5/thumbnails/10.jpg)
PART 1(a): Probability and Statistics
10
![Page 11: Stochastic Methods in Robotics (Probability, Fourier ... · • Pose Distributions for Nonholonomic Systems (Mobile Robots and Steerable Needles) • Lie-Theoretic Estimation and](https://reader033.fdocuments.us/reader033/viewer/2022043017/5f3976ef5dc58635dd56c0a5/html5/thumbnails/11.jpg)
Gaussian Distribution on the Real Line
![Page 12: Stochastic Methods in Robotics (Probability, Fourier ... · • Pose Distributions for Nonholonomic Systems (Mobile Robots and Steerable Needles) • Lie-Theoretic Estimation and](https://reader033.fdocuments.us/reader033/viewer/2022043017/5f3976ef5dc58635dd56c0a5/html5/thumbnails/12.jpg)
Convolution of Gaussians
![Page 13: Stochastic Methods in Robotics (Probability, Fourier ... · • Pose Distributions for Nonholonomic Systems (Mobile Robots and Steerable Needles) • Lie-Theoretic Estimation and](https://reader033.fdocuments.us/reader033/viewer/2022043017/5f3976ef5dc58635dd56c0a5/html5/thumbnails/13.jpg)
Classical Fourier Analysis
![Page 14: Stochastic Methods in Robotics (Probability, Fourier ... · • Pose Distributions for Nonholonomic Systems (Mobile Robots and Steerable Needles) • Lie-Theoretic Estimation and](https://reader033.fdocuments.us/reader033/viewer/2022043017/5f3976ef5dc58635dd56c0a5/html5/thumbnails/14.jpg)
Gaussians Wrapped Around the Circle
This is exactly equivalent to
![Page 15: Stochastic Methods in Robotics (Probability, Fourier ... · • Pose Distributions for Nonholonomic Systems (Mobile Robots and Steerable Needles) • Lie-Theoretic Estimation and](https://reader033.fdocuments.us/reader033/viewer/2022043017/5f3976ef5dc58635dd56c0a5/html5/thumbnails/15.jpg)
Gassian as Solution to Heat Equation (circle case)
![Page 16: Stochastic Methods in Robotics (Probability, Fourier ... · • Pose Distributions for Nonholonomic Systems (Mobile Robots and Steerable Needles) • Lie-Theoretic Estimation and](https://reader033.fdocuments.us/reader033/viewer/2022043017/5f3976ef5dc58635dd56c0a5/html5/thumbnails/16.jpg)
Gassian as Solution to Heat Equation (R^n case)
![Page 17: Stochastic Methods in Robotics (Probability, Fourier ... · • Pose Distributions for Nonholonomic Systems (Mobile Robots and Steerable Needles) • Lie-Theoretic Estimation and](https://reader033.fdocuments.us/reader033/viewer/2022043017/5f3976ef5dc58635dd56c0a5/html5/thumbnails/17.jpg)
Multivariate Gaussian
![Page 18: Stochastic Methods in Robotics (Probability, Fourier ... · • Pose Distributions for Nonholonomic Systems (Mobile Robots and Steerable Needles) • Lie-Theoretic Estimation and](https://reader033.fdocuments.us/reader033/viewer/2022043017/5f3976ef5dc58635dd56c0a5/html5/thumbnails/18.jpg)
Convolution on R^n
![Page 19: Stochastic Methods in Robotics (Probability, Fourier ... · • Pose Distributions for Nonholonomic Systems (Mobile Robots and Steerable Needles) • Lie-Theoretic Estimation and](https://reader033.fdocuments.us/reader033/viewer/2022043017/5f3976ef5dc58635dd56c0a5/html5/thumbnails/19.jpg)
A Little Bit of Information Theory
Entropy
Kullback-Leibler Divergence
![Page 20: Stochastic Methods in Robotics (Probability, Fourier ... · • Pose Distributions for Nonholonomic Systems (Mobile Robots and Steerable Needles) • Lie-Theoretic Estimation and](https://reader033.fdocuments.us/reader033/viewer/2022043017/5f3976ef5dc58635dd56c0a5/html5/thumbnails/20.jpg)
Entropy Power Inequality
![Page 21: Stochastic Methods in Robotics (Probability, Fourier ... · • Pose Distributions for Nonholonomic Systems (Mobile Robots and Steerable Needles) • Lie-Theoretic Estimation and](https://reader033.fdocuments.us/reader033/viewer/2022043017/5f3976ef5dc58635dd56c0a5/html5/thumbnails/21.jpg)
PART 1(b): Lie Groups and Lie Algebras
21
![Page 22: Stochastic Methods in Robotics (Probability, Fourier ... · • Pose Distributions for Nonholonomic Systems (Mobile Robots and Steerable Needles) • Lie-Theoretic Estimation and](https://reader033.fdocuments.us/reader033/viewer/2022043017/5f3976ef5dc58635dd56c0a5/html5/thumbnails/22.jpg)
Rigid-Body Motion Group
• Special Euclidean motion group SE(N)
– An element of G=SE(N):
– Group operation: matrix multiplication
• For example, an element of SE(2) in polar coordinates:
10T
aAg
100
sincossin
cossincos
),,(
r
r
rg
![Page 23: Stochastic Methods in Robotics (Probability, Fourier ... · • Pose Distributions for Nonholonomic Systems (Mobile Robots and Steerable Needles) • Lie-Theoretic Estimation and](https://reader033.fdocuments.us/reader033/viewer/2022043017/5f3976ef5dc58635dd56c0a5/html5/thumbnails/23.jpg)
• Lie algebra of SE(2)
• Lie algebra of SE(3)
• Infinitesimal motions
000
001
010~
1X
000
000
100~
2X
000
100
000~
3X
0000
0010
0100
0000
~1X
0000
0000
0001
0010
~3X
0000
0001
0000
0100
~2X
0000
0000
0000
1000
~4X
0000
1000
0000
0000
~6X
0000
0000
1000
0000
~5X
iii XIXH~
)~
exp()(
![Page 24: Stochastic Methods in Robotics (Probability, Fourier ... · • Pose Distributions for Nonholonomic Systems (Mobile Robots and Steerable Needles) • Lie-Theoretic Estimation and](https://reader033.fdocuments.us/reader033/viewer/2022043017/5f3976ef5dc58635dd56c0a5/html5/thumbnails/24.jpg)
A Little Bit of Lie Group Theory
![Page 25: Stochastic Methods in Robotics (Probability, Fourier ... · • Pose Distributions for Nonholonomic Systems (Mobile Robots and Steerable Needles) • Lie-Theoretic Estimation and](https://reader033.fdocuments.us/reader033/viewer/2022043017/5f3976ef5dc58635dd56c0a5/html5/thumbnails/25.jpg)
exp and log for SO(3)
![Page 26: Stochastic Methods in Robotics (Probability, Fourier ... · • Pose Distributions for Nonholonomic Systems (Mobile Robots and Steerable Needles) • Lie-Theoretic Estimation and](https://reader033.fdocuments.us/reader033/viewer/2022043017/5f3976ef5dc58635dd56c0a5/html5/thumbnails/26.jpg)
26
Lie-group-theoretic Notation
• Coordinate free no singularities
(3) ofelement basis:
0
(3)))(),(()(For
1
6
1
1
seX
A
AAgg
AAAXgg
SEtAttg
i
T
T
T
TT
i
ii
v
ω
aξ
0
a
a
(3) ofelement basis :
,)3()(For
3
1
soX
AA
XAA
SOtA
i
T
i
ii
T
ω
)(tg
Space-fixed frame
Body-fixed frame
ω
v
![Page 27: Stochastic Methods in Robotics (Probability, Fourier ... · • Pose Distributions for Nonholonomic Systems (Mobile Robots and Steerable Needles) • Lie-Theoretic Estimation and](https://reader033.fdocuments.us/reader033/viewer/2022043017/5f3976ef5dc58635dd56c0a5/html5/thumbnails/27.jpg)
For more details see
![Page 28: Stochastic Methods in Robotics (Probability, Fourier ... · • Pose Distributions for Nonholonomic Systems (Mobile Robots and Steerable Needles) • Lie-Theoretic Estimation and](https://reader033.fdocuments.us/reader033/viewer/2022043017/5f3976ef5dc58635dd56c0a5/html5/thumbnails/28.jpg)
Differential Operators Acting on Functions on SE(N)
00 ))~
(())(())((~
titi
R
i XtIHfdt
dtHHf
dt
dqHfX
00 ))~
(())(())((~
titi
L
i HXtIfdt
dHtHf
dt
dqHfX
![Page 29: Stochastic Methods in Robotics (Probability, Fourier ... · • Pose Distributions for Nonholonomic Systems (Mobile Robots and Steerable Needles) • Lie-Theoretic Estimation and](https://reader033.fdocuments.us/reader033/viewer/2022043017/5f3976ef5dc58635dd56c0a5/html5/thumbnails/29.jpg)
Differential operators defined for SE(2)
rrX
rrX
X
R
R
R
)cos()sin(
~
)sin()cos(
~
~
3
2
1
rrX
rrX
X
L
L
L
cossin
~
sincos
~
~
3
2
1
![Page 30: Stochastic Methods in Robotics (Probability, Fourier ... · • Pose Distributions for Nonholonomic Systems (Mobile Robots and Steerable Needles) • Lie-Theoretic Estimation and](https://reader033.fdocuments.us/reader033/viewer/2022043017/5f3976ef5dc58635dd56c0a5/html5/thumbnails/30.jpg)
Differential operators defined for SE(3)
6,5,4for
3,2,1for~
3iA
iXX
ia
T
R
iR
i
6,5,4for
3,2,1for)(~
3
3
1
ia
ia
eeaX
X
i
k k
ki
L
iL
i
![Page 31: Stochastic Methods in Robotics (Probability, Fourier ... · • Pose Distributions for Nonholonomic Systems (Mobile Robots and Steerable Needles) • Lie-Theoretic Estimation and](https://reader033.fdocuments.us/reader033/viewer/2022043017/5f3976ef5dc58635dd56c0a5/html5/thumbnails/31.jpg)
31
Variational Calculus on Lie groups• Given the functional and constraints
one can get the Euler-Poincaré equation as:
where
2
1
2
1
)(,);;( 1
t
t
kk
t
t
dtghCdttgggfJ
,)(11,
m
l
ll
R
i
n
kj
j
k
ij
ki
hfXCff
dt
d
6
1
0
,
))exp(()(
k
k
k
ijji
t
i
R
i
XCXX
tXgfdt
dgfX
![Page 32: Stochastic Methods in Robotics (Probability, Fourier ... · • Pose Distributions for Nonholonomic Systems (Mobile Robots and Steerable Needles) • Lie-Theoretic Estimation and](https://reader033.fdocuments.us/reader033/viewer/2022043017/5f3976ef5dc58635dd56c0a5/html5/thumbnails/32.jpg)
Probability on Lie Groups
![Page 33: Stochastic Methods in Robotics (Probability, Fourier ... · • Pose Distributions for Nonholonomic Systems (Mobile Robots and Steerable Needles) • Lie-Theoretic Estimation and](https://reader033.fdocuments.us/reader033/viewer/2022043017/5f3976ef5dc58635dd56c0a5/html5/thumbnails/33.jpg)
• Integration of functions on SO(3)
• Integration of functions on SE(2) and SE(3)
)3(
321|))((det|))((1
)(SO Qq
dqdqdqqAJqAfV
dAAf
)3(
2
0 0
2
0
2sin),,(
8
1)(
SOdddfdAAf
)(
1|))((det|))(()(NSE Qq
NdqdqqHJqHfdHHf
21212
1),,( dxdxdxxdH
3212321 sin8
1),,(),,,,,( dadadaddddaddAaaadH
e.g.
e.g.
SE(2)
e.g.
SE(3)
![Page 34: Stochastic Methods in Robotics (Probability, Fourier ... · • Pose Distributions for Nonholonomic Systems (Mobile Robots and Steerable Needles) • Lie-Theoretic Estimation and](https://reader033.fdocuments.us/reader033/viewer/2022043017/5f3976ef5dc58635dd56c0a5/html5/thumbnails/34.jpg)
![Page 35: Stochastic Methods in Robotics (Probability, Fourier ... · • Pose Distributions for Nonholonomic Systems (Mobile Robots and Steerable Needles) • Lie-Theoretic Estimation and](https://reader033.fdocuments.us/reader033/viewer/2022043017/5f3976ef5dc58635dd56c0a5/html5/thumbnails/35.jpg)
![Page 36: Stochastic Methods in Robotics (Probability, Fourier ... · • Pose Distributions for Nonholonomic Systems (Mobile Robots and Steerable Needles) • Lie-Theoretic Estimation and](https://reader033.fdocuments.us/reader033/viewer/2022043017/5f3976ef5dc58635dd56c0a5/html5/thumbnails/36.jpg)
Fourier Analysis of Motion
• Fourier transform of a function of motion, f(g)
• Inverse Fourier transform of a function of motion
G
dgpgUgfpffF ),()()(ˆ)( 1
dpppgUpftracegffF N 11 )),()(ˆ()()ˆ(
where g SE(N) , p is a frequency parameter,
U(g,p) is a matrix representation of SE(N), and
dg is a volume element at g.
![Page 37: Stochastic Methods in Robotics (Probability, Fourier ... · • Pose Distributions for Nonholonomic Systems (Mobile Robots and Steerable Needles) • Lie-Theoretic Estimation and](https://reader033.fdocuments.us/reader033/viewer/2022043017/5f3976ef5dc58635dd56c0a5/html5/thumbnails/37.jpg)
Convolution and the SE(3) Fourier Transform
)()()*( 1221 fFfFffF
Gdhghfhfgff )()())(*( 1
2121
G.S. Chirikjian, Stochastic Models, Information Theory,and Lie Groups, Vol. 1, 2, Birkhauser, 2009, 2011.
G.S. Chirikjian, A.B. Kyatkin, Engineering Applications ofNoncommutative Harmonic Analysis, CRC Press, 2001.
![Page 38: Stochastic Methods in Robotics (Probability, Fourier ... · • Pose Distributions for Nonholonomic Systems (Mobile Robots and Steerable Needles) • Lie-Theoretic Estimation and](https://reader033.fdocuments.us/reader033/viewer/2022043017/5f3976ef5dc58635dd56c0a5/html5/thumbnails/38.jpg)
PART 2: APPLICATIONS TO
CONFORMATIONAL ENSEMBLES
38
![Page 39: Stochastic Methods in Robotics (Probability, Fourier ... · • Pose Distributions for Nonholonomic Systems (Mobile Robots and Steerable Needles) • Lie-Theoretic Estimation and](https://reader033.fdocuments.us/reader033/viewer/2022043017/5f3976ef5dc58635dd56c0a5/html5/thumbnails/39.jpg)
Workspace Density
• It describes the density of the reachable frames in the work space.
• It is a probabilistic measure-ment of accuracy over the workspace.
Ebert-Uphoff, I., Chirikjian, G.S., ``Inverse Kinematics of Discretely Actuated
Hyper-Redundant Manipulators Using Workspace Densities,'' ICRA’96, pp. 139-145
Long, A.W., Wolfe, K.C., Mashner, M.J., Chirikjian, G.S., ``The Banana Distribution
is Gaussian: A Localization Study with Exponential Coordinates,'‘ RSS, Sydney,
NSW, Australia, July 09 - July 13, 2012.
![Page 40: Stochastic Methods in Robotics (Probability, Fourier ... · • Pose Distributions for Nonholonomic Systems (Mobile Robots and Steerable Needles) • Lie-Theoretic Estimation and](https://reader033.fdocuments.us/reader033/viewer/2022043017/5f3976ef5dc58635dd56c0a5/html5/thumbnails/40.jpg)
A diffusion equation describing the PDF of relative
pose between the frame of reference at arc length s and
that at the proximal end of the chain
zp
yp
Proximal End
zp’
yp’
zd yd
Distal End s L
),,()~~~~
2
1(
),,(6
3
1
3
1,
sfXXdXXDs
sf R
l
R
ll
lk
R
k
R
llk RaRa
Initial condition: f(a,R,0)= (a) (R)
1][ BD lkD bBd1][ ldDefining
A General Semiflexible Polymer Model
![Page 41: Stochastic Methods in Robotics (Probability, Fourier ... · • Pose Distributions for Nonholonomic Systems (Mobile Robots and Steerable Needles) • Lie-Theoretic Estimation and](https://reader033.fdocuments.us/reader033/viewer/2022043017/5f3976ef5dc58635dd56c0a5/html5/thumbnails/41.jpg)
41
Operational Properties of SE(n) Fourier Transform
)(ˆ),~
(
)(),()(),~
exp(
)(,)~
exp()(
)(),())~
exp((~
1
0
),(),(),(
0
1
)~
exp(
1
0
2121
pfpX
hdphUhfpXtUdt
d
hdphXtUdt
dhf
gdpgUXtgfdt
dfXF
i
G
ti
pgUpgUpggU
G
ti
Xtgh
G
ti
R
i
i
![Page 42: Stochastic Methods in Robotics (Probability, Fourier ... · • Pose Distributions for Nonholonomic Systems (Mobile Robots and Steerable Needles) • Lie-Theoretic Estimation and](https://reader033.fdocuments.us/reader033/viewer/2022043017/5f3976ef5dc58635dd56c0a5/html5/thumbnails/42.jpg)
Explicit Expression of for SE(2)
0)),~
(exp(),~
(
tii pXtU
dt
dpX
)()),,,(( ])([ paJeipagu mn
nmnimn
mn
)(2
),~
(
)(2
),~
(
),~
(
1,1,3
1,1,2
,1
nmnmmn
nmnmmn
nmmn
ppX
jppX
jmpX
),~
( pX i
![Page 43: Stochastic Methods in Robotics (Probability, Fourier ... · • Pose Distributions for Nonholonomic Systems (Mobile Robots and Steerable Needles) • Lie-Theoretic Estimation and](https://reader033.fdocuments.us/reader033/viewer/2022043017/5f3976ef5dc58635dd56c0a5/html5/thumbnails/43.jpg)
Explicit Expression of for SE(3)
0)),~
(exp(),~
(
tii pXtU
dt
dpX
mmllmlml
mmll
l
mmmll
l
mmlml
mmll
l
mmmll
l
mmlml
jmpX
cj
cj
pX
ccpX
,,3,;,
,1,,1,2,;,
,1,,1,1,;,
''''
''''''
''''''
),~
(
22),
~(
2
1
2
1),
~(
l
lk
l
km
s
mlmlAUamlspmlpgu )()](,|,|,[),( ''
,;, ''
),~
( pX i
![Page 44: Stochastic Methods in Robotics (Probability, Fourier ... · • Pose Distributions for Nonholonomic Systems (Mobile Robots and Steerable Needles) • Lie-Theoretic Estimation and](https://reader033.fdocuments.us/reader033/viewer/2022043017/5f3976ef5dc58635dd56c0a5/html5/thumbnails/44.jpg)
Explicit Expression of (Xi , p) for SE(3)
llmm
s
mlllmm
s
mlllmm
s
ml
llmm
s
mlllmm
s
mlllmm
s
mlmlml
jpjpjp
jpjpjppX
i
i
,11,,,1,,,11,,
,11,,,1,,,11,,4,;,
'''''''
'''''''''
222
222),
~(
llmm
s
mlllmm
s
mlllmm
s
ml
llmm
s
mlllmm
s
mlllmm
s
mlmlml
ppp
ppppX
i
i
,11,,,1,,,11,,
,11,,,1,,,11,,5,;,
'''''''
'''''''''
222
222),
~(
llmm
s
mlllmmllmm
s
mlmlmljp
ll
smjpjppX i ,1,,,,,1,,6,;, '''''''''
)1(),
~(
![Page 45: Stochastic Methods in Robotics (Probability, Fourier ... · • Pose Distributions for Nonholonomic Systems (Mobile Robots and Steerable Needles) • Lie-Theoretic Estimation and](https://reader033.fdocuments.us/reader033/viewer/2022043017/5f3976ef5dc58635dd56c0a5/html5/thumbnails/45.jpg)
45
Solving for the evolving PDF Using the SE(3) FT
rrr
ds
dfB
f ˆˆ
where B is a constant matrix.
),,()~~~~
2
1(
),,(6
3
1
3
1,
sfXXdXXDs
sf R
l
R
ll
lk
R
k
R
llk RaRa
rsr esp Bf ),(ˆ
dpppUpfsfr rl rl
l
lm
l
lm
r
mlml
r
mlml
2
||' ||
'
''0
,;','',';,2);,()(ˆ
2
1),,(
RaRa
Applying SE(3) Fourier transform
Solving ODE
Applying inverse transform
![Page 46: Stochastic Methods in Robotics (Probability, Fourier ... · • Pose Distributions for Nonholonomic Systems (Mobile Robots and Steerable Needles) • Lie-Theoretic Estimation and](https://reader033.fdocuments.us/reader033/viewer/2022043017/5f3976ef5dc58635dd56c0a5/html5/thumbnails/46.jpg)
Numerical Examples
2
1
0.50.1
![Page 47: Stochastic Methods in Robotics (Probability, Fourier ... · • Pose Distributions for Nonholonomic Systems (Mobile Robots and Steerable Needles) • Lie-Theoretic Estimation and](https://reader033.fdocuments.us/reader033/viewer/2022043017/5f3976ef5dc58635dd56c0a5/html5/thumbnails/47.jpg)
47
The Structure of a Bent Macromolecular Chain
xd1,xp2
b
zd2
yd2
xd2
zp1 yp1
xp1
zp2 yp2
yd1
zd1
Subchain 1 Subchain 2
1) A bent macromolecular chain consists of two
intrinsically straight segments.
2) A bend or twist is a rotation at the separating point
between the two segments with no translation.
A General Algorithm for Bent or
Twisted Macromolecular Chains
![Page 48: Stochastic Methods in Robotics (Probability, Fourier ... · • Pose Distributions for Nonholonomic Systems (Mobile Robots and Steerable Needles) • Lie-Theoretic Estimation and](https://reader033.fdocuments.us/reader033/viewer/2022043017/5f3976ef5dc58635dd56c0a5/html5/thumbnails/48.jpg)
48
The PDF of the End-to-End Pose for a Bent Chain
),)(**(),( 321 RaRa ffff
•f1(a,R) and f3(a,R) are obtained by solving the
differential equation for nonbent polymer.
•f2(a,R)= (a)(Rb-1R), where Rb is the rotation
made at the bend.
2) The convolution on SE(3)
)3(
1 )()()())(*(SE
jiji dffff hghhg
A General Algorithm for Bent or
Twisted Macromolecular Chains
1) A convolution of 3 PDFs
![Page 49: Stochastic Methods in Robotics (Probability, Fourier ... · • Pose Distributions for Nonholonomic Systems (Mobile Robots and Steerable Needles) • Lie-Theoretic Estimation and](https://reader033.fdocuments.us/reader033/viewer/2022043017/5f3976ef5dc58635dd56c0a5/html5/thumbnails/49.jpg)
49
1. Variation of f(a) with respect to Bending Angle
and Bending Location
0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1 -0.5
0
0.5
1
1.5
2
2.5
3
3.5
4
Kratky-Porod model with 0=0.5, L1=L2=0.5
a
f(a)
b=
b=3/4
b=/2
b=0
b=/4
0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1 -0.5
0
0.5
1
1.5
2
2.5
3
3.5
4
a
f(a)
Kratky-Porod model with 0=0.5, b=/2
L1=0.1 L1=0.2
L1=0.3 L1=0.4 L1=0.5
Examples
![Page 50: Stochastic Methods in Robotics (Probability, Fourier ... · • Pose Distributions for Nonholonomic Systems (Mobile Robots and Steerable Needles) • Lie-Theoretic Estimation and](https://reader033.fdocuments.us/reader033/viewer/2022043017/5f3976ef5dc58635dd56c0a5/html5/thumbnails/50.jpg)
1) G. S. Chirikjian, ``Modeling Loop Entropy,’’ Methods in
Enzymology, 487, 2011
1) Y. Zhou, G. S. Chirikjian, ``Conformational Statistics of SemiflexibleMacromolecular Chains with Internal Joints,’’
Macromolecules. 39:1950-1960. 2006
1) Zhou, Y., Chirikjian, G.S., “Conformational Statistics of Bent Semi-flexible Polymers”, Journal of Chemical Physics, vol.119, no.9, pp.4962-4970, 2003.
2) G. S. Chirikjian, Y. Wang, ``Conformational Statistics of StiffMacromolecules as Solutions to PDEs on the Rotation and MotionGroups,’’ Physical Review E. 62(1):880-892. 2000
DNA References
![Page 51: Stochastic Methods in Robotics (Probability, Fourier ... · • Pose Distributions for Nonholonomic Systems (Mobile Robots and Steerable Needles) • Lie-Theoretic Estimation and](https://reader033.fdocuments.us/reader033/viewer/2022043017/5f3976ef5dc58635dd56c0a5/html5/thumbnails/51.jpg)
PART 4: STOCHASTIC KINEMATICS AND INFORMATION-DRIVEN MOTION
![Page 52: Stochastic Methods in Robotics (Probability, Fourier ... · • Pose Distributions for Nonholonomic Systems (Mobile Robots and Steerable Needles) • Lie-Theoretic Estimation and](https://reader033.fdocuments.us/reader033/viewer/2022043017/5f3976ef5dc58635dd56c0a5/html5/thumbnails/52.jpg)
![Page 53: Stochastic Methods in Robotics (Probability, Fourier ... · • Pose Distributions for Nonholonomic Systems (Mobile Robots and Steerable Needles) • Lie-Theoretic Estimation and](https://reader033.fdocuments.us/reader033/viewer/2022043017/5f3976ef5dc58635dd56c0a5/html5/thumbnails/53.jpg)
![Page 54: Stochastic Methods in Robotics (Probability, Fourier ... · • Pose Distributions for Nonholonomic Systems (Mobile Robots and Steerable Needles) • Lie-Theoretic Estimation and](https://reader033.fdocuments.us/reader033/viewer/2022043017/5f3976ef5dc58635dd56c0a5/html5/thumbnails/54.jpg)
![Page 55: Stochastic Methods in Robotics (Probability, Fourier ... · • Pose Distributions for Nonholonomic Systems (Mobile Robots and Steerable Needles) • Lie-Theoretic Estimation and](https://reader033.fdocuments.us/reader033/viewer/2022043017/5f3976ef5dc58635dd56c0a5/html5/thumbnails/55.jpg)
![Page 56: Stochastic Methods in Robotics (Probability, Fourier ... · • Pose Distributions for Nonholonomic Systems (Mobile Robots and Steerable Needles) • Lie-Theoretic Estimation and](https://reader033.fdocuments.us/reader033/viewer/2022043017/5f3976ef5dc58635dd56c0a5/html5/thumbnails/56.jpg)
![Page 57: Stochastic Methods in Robotics (Probability, Fourier ... · • Pose Distributions for Nonholonomic Systems (Mobile Robots and Steerable Needles) • Lie-Theoretic Estimation and](https://reader033.fdocuments.us/reader033/viewer/2022043017/5f3976ef5dc58635dd56c0a5/html5/thumbnails/57.jpg)
![Page 58: Stochastic Methods in Robotics (Probability, Fourier ... · • Pose Distributions for Nonholonomic Systems (Mobile Robots and Steerable Needles) • Lie-Theoretic Estimation and](https://reader033.fdocuments.us/reader033/viewer/2022043017/5f3976ef5dc58635dd56c0a5/html5/thumbnails/58.jpg)
![Page 59: Stochastic Methods in Robotics (Probability, Fourier ... · • Pose Distributions for Nonholonomic Systems (Mobile Robots and Steerable Needles) • Lie-Theoretic Estimation and](https://reader033.fdocuments.us/reader033/viewer/2022043017/5f3976ef5dc58635dd56c0a5/html5/thumbnails/59.jpg)
![Page 60: Stochastic Methods in Robotics (Probability, Fourier ... · • Pose Distributions for Nonholonomic Systems (Mobile Robots and Steerable Needles) • Lie-Theoretic Estimation and](https://reader033.fdocuments.us/reader033/viewer/2022043017/5f3976ef5dc58635dd56c0a5/html5/thumbnails/60.jpg)
Flexible Needles with Bevel tip
http://research.vuse.vanderbilt.edu/MEDLab/research_files/needlesteer.htm
Needle with a bevel tip
![Page 61: Stochastic Methods in Robotics (Probability, Fourier ... · • Pose Distributions for Nonholonomic Systems (Mobile Robots and Steerable Needles) • Lie-Theoretic Estimation and](https://reader033.fdocuments.us/reader033/viewer/2022043017/5f3976ef5dc58635dd56c0a5/html5/thumbnails/61.jpg)
Needle Model
v
v
v
v
v
z
y
x
z
y
x
0
0
0
Deterministic nonholonomic model
: insertion speed : twisting angular velocityv
Experiments by Dr. Kyle Reed
![Page 62: Stochastic Methods in Robotics (Probability, Fourier ... · • Pose Distributions for Nonholonomic Systems (Mobile Robots and Steerable Needles) • Lie-Theoretic Estimation and](https://reader033.fdocuments.us/reader033/viewer/2022043017/5f3976ef5dc58635dd56c0a5/html5/thumbnails/62.jpg)
Stochastic needle model
)(twi
i
: Unit Gaussian white noise
: Strength of noise)(1)( 22 twtv
)()( 11 twt
2
1
22
11
0000
0000010000)(
W
WT
d
ddtdtgg
T
: Wiener process
g : SE(3) frame for needle tip pose
)(tiW
Park, W., Kim, J.S., Zhou, Y., Cowan, N.J., Okamura, A.M., Chirikjian,G.S., ``Diffusion-based motion planning for a nonholonomicflexible needle model,'‘ ICRA’05, Barcelona, Spain
Park, W., Reed, K. B., Okamura, A. M., Chirikjian, G. S., ``Estimation of modelparameters for steerable needles,'' ICRA, Anchorage, Alaska, 2010.
![Page 63: Stochastic Methods in Robotics (Probability, Fourier ... · • Pose Distributions for Nonholonomic Systems (Mobile Robots and Steerable Needles) • Lie-Theoretic Estimation and](https://reader033.fdocuments.us/reader033/viewer/2022043017/5f3976ef5dc58635dd56c0a5/html5/thumbnails/63.jpg)
63
1) Chirikjian, G.S. "A binary paradigm for robotic manipulators.“ICRA 1994. pp. 3063-3069.
2) Ebert-Uphoff, I., Chirikjian, G.S., ``Inverse Kinematics ofDiscretely Actuated Hyper-Redundant Manipulators UsingWorkspace Densities,'‘ ICRA 1996, 139-145.
3) Chirikjian, G.S., Burdick, J.W., ``An Obstacle AvoidanceAlgorithm for Hyper-Redundant Manipulators,''ICRA 1990, 625-631.
4) Park, W., Wang, Y., Chirikjian, G.S., ``The path-of-probabilityalgorithm for steering and feedback control of flexible needles,'‘International Journal of Robotics Research, Vol. 29, No. 7,813-830 (2010)
References
![Page 64: Stochastic Methods in Robotics (Probability, Fourier ... · • Pose Distributions for Nonholonomic Systems (Mobile Robots and Steerable Needles) • Lie-Theoretic Estimation and](https://reader033.fdocuments.us/reader033/viewer/2022043017/5f3976ef5dc58635dd56c0a5/html5/thumbnails/64.jpg)
Alterovitz, R., Lim, A., Goldberg, K., Chirikjian, G.S., Okamura, A.M.,``Steering flexible needles under Markov motion uncertainty,'' IROS,pp. 120-125, August 2005.
R. J. Webster III, J. S. Kim, N. J. Cowan, G. S. Chirikjian and A. M. Okamura,"Nonholonomic Modeling of Needle Steering," International Journal ofRobotics Research, Vol. 25, No. 5-6, pp. 509-525, May-June 2006.
Park, W., Wang, Y., Chirikjian, G.S., ``The path-of-probability
algorithm for steering and feedback control of flexible needles,'‘
IJRR, Vol. 29, No. 7, 813-830 (2010)
Other References
![Page 65: Stochastic Methods in Robotics (Probability, Fourier ... · • Pose Distributions for Nonholonomic Systems (Mobile Robots and Steerable Needles) • Lie-Theoretic Estimation and](https://reader033.fdocuments.us/reader033/viewer/2022043017/5f3976ef5dc58635dd56c0a5/html5/thumbnails/65.jpg)
![Page 66: Stochastic Methods in Robotics (Probability, Fourier ... · • Pose Distributions for Nonholonomic Systems (Mobile Robots and Steerable Needles) • Lie-Theoretic Estimation and](https://reader033.fdocuments.us/reader033/viewer/2022043017/5f3976ef5dc58635dd56c0a5/html5/thumbnails/66.jpg)
![Page 67: Stochastic Methods in Robotics (Probability, Fourier ... · • Pose Distributions for Nonholonomic Systems (Mobile Robots and Steerable Needles) • Lie-Theoretic Estimation and](https://reader033.fdocuments.us/reader033/viewer/2022043017/5f3976ef5dc58635dd56c0a5/html5/thumbnails/67.jpg)
GC’s presentation is supported by the NSF IRD Program
![Page 68: Stochastic Methods in Robotics (Probability, Fourier ... · • Pose Distributions for Nonholonomic Systems (Mobile Robots and Steerable Needles) • Lie-Theoretic Estimation and](https://reader033.fdocuments.us/reader033/viewer/2022043017/5f3976ef5dc58635dd56c0a5/html5/thumbnails/68.jpg)
![Page 69: Stochastic Methods in Robotics (Probability, Fourier ... · • Pose Distributions for Nonholonomic Systems (Mobile Robots and Steerable Needles) • Lie-Theoretic Estimation and](https://reader033.fdocuments.us/reader033/viewer/2022043017/5f3976ef5dc58635dd56c0a5/html5/thumbnails/69.jpg)
SDE for the Kinematic Cart
(Zhou and Chirikjian, ICRA 2003)
![Page 70: Stochastic Methods in Robotics (Probability, Fourier ... · • Pose Distributions for Nonholonomic Systems (Mobile Robots and Steerable Needles) • Lie-Theoretic Estimation and](https://reader033.fdocuments.us/reader033/viewer/2022043017/5f3976ef5dc58635dd56c0a5/html5/thumbnails/70.jpg)
Long et al, ``The Banana Distribution is Gaussian’’ RSS 2012
![Page 71: Stochastic Methods in Robotics (Probability, Fourier ... · • Pose Distributions for Nonholonomic Systems (Mobile Robots and Steerable Needles) • Lie-Theoretic Estimation and](https://reader033.fdocuments.us/reader033/viewer/2022043017/5f3976ef5dc58635dd56c0a5/html5/thumbnails/71.jpg)
Exponential Coordinates for SE(2)
![Page 72: Stochastic Methods in Robotics (Probability, Fourier ... · • Pose Distributions for Nonholonomic Systems (Mobile Robots and Steerable Needles) • Lie-Theoretic Estimation and](https://reader033.fdocuments.us/reader033/viewer/2022043017/5f3976ef5dc58635dd56c0a5/html5/thumbnails/72.jpg)
![Page 73: Stochastic Methods in Robotics (Probability, Fourier ... · • Pose Distributions for Nonholonomic Systems (Mobile Robots and Steerable Needles) • Lie-Theoretic Estimation and](https://reader033.fdocuments.us/reader033/viewer/2022043017/5f3976ef5dc58635dd56c0a5/html5/thumbnails/73.jpg)
![Page 74: Stochastic Methods in Robotics (Probability, Fourier ... · • Pose Distributions for Nonholonomic Systems (Mobile Robots and Steerable Needles) • Lie-Theoretic Estimation and](https://reader033.fdocuments.us/reader033/viewer/2022043017/5f3976ef5dc58635dd56c0a5/html5/thumbnails/74.jpg)
![Page 75: Stochastic Methods in Robotics (Probability, Fourier ... · • Pose Distributions for Nonholonomic Systems (Mobile Robots and Steerable Needles) • Lie-Theoretic Estimation and](https://reader033.fdocuments.us/reader033/viewer/2022043017/5f3976ef5dc58635dd56c0a5/html5/thumbnails/75.jpg)
![Page 76: Stochastic Methods in Robotics (Probability, Fourier ... · • Pose Distributions for Nonholonomic Systems (Mobile Robots and Steerable Needles) • Lie-Theoretic Estimation and](https://reader033.fdocuments.us/reader033/viewer/2022043017/5f3976ef5dc58635dd56c0a5/html5/thumbnails/76.jpg)
![Page 77: Stochastic Methods in Robotics (Probability, Fourier ... · • Pose Distributions for Nonholonomic Systems (Mobile Robots and Steerable Needles) • Lie-Theoretic Estimation and](https://reader033.fdocuments.us/reader033/viewer/2022043017/5f3976ef5dc58635dd56c0a5/html5/thumbnails/77.jpg)
![Page 78: Stochastic Methods in Robotics (Probability, Fourier ... · • Pose Distributions for Nonholonomic Systems (Mobile Robots and Steerable Needles) • Lie-Theoretic Estimation and](https://reader033.fdocuments.us/reader033/viewer/2022043017/5f3976ef5dc58635dd56c0a5/html5/thumbnails/78.jpg)
![Page 79: Stochastic Methods in Robotics (Probability, Fourier ... · • Pose Distributions for Nonholonomic Systems (Mobile Robots and Steerable Needles) • Lie-Theoretic Estimation and](https://reader033.fdocuments.us/reader033/viewer/2022043017/5f3976ef5dc58635dd56c0a5/html5/thumbnails/79.jpg)
![Page 80: Stochastic Methods in Robotics (Probability, Fourier ... · • Pose Distributions for Nonholonomic Systems (Mobile Robots and Steerable Needles) • Lie-Theoretic Estimation and](https://reader033.fdocuments.us/reader033/viewer/2022043017/5f3976ef5dc58635dd56c0a5/html5/thumbnails/80.jpg)
![Page 81: Stochastic Methods in Robotics (Probability, Fourier ... · • Pose Distributions for Nonholonomic Systems (Mobile Robots and Steerable Needles) • Lie-Theoretic Estimation and](https://reader033.fdocuments.us/reader033/viewer/2022043017/5f3976ef5dc58635dd56c0a5/html5/thumbnails/81.jpg)
![Page 82: Stochastic Methods in Robotics (Probability, Fourier ... · • Pose Distributions for Nonholonomic Systems (Mobile Robots and Steerable Needles) • Lie-Theoretic Estimation and](https://reader033.fdocuments.us/reader033/viewer/2022043017/5f3976ef5dc58635dd56c0a5/html5/thumbnails/82.jpg)
Matching 2nd Order Taylor Series and 2nd Order BCH Expansions
Both are of the form
We match a, b, K for each.
![Page 83: Stochastic Methods in Robotics (Probability, Fourier ... · • Pose Distributions for Nonholonomic Systems (Mobile Robots and Steerable Needles) • Lie-Theoretic Estimation and](https://reader033.fdocuments.us/reader033/viewer/2022043017/5f3976ef5dc58635dd56c0a5/html5/thumbnails/83.jpg)
![Page 84: Stochastic Methods in Robotics (Probability, Fourier ... · • Pose Distributions for Nonholonomic Systems (Mobile Robots and Steerable Needles) • Lie-Theoretic Estimation and](https://reader033.fdocuments.us/reader033/viewer/2022043017/5f3976ef5dc58635dd56c0a5/html5/thumbnails/84.jpg)
![Page 85: Stochastic Methods in Robotics (Probability, Fourier ... · • Pose Distributions for Nonholonomic Systems (Mobile Robots and Steerable Needles) • Lie-Theoretic Estimation and](https://reader033.fdocuments.us/reader033/viewer/2022043017/5f3976ef5dc58635dd56c0a5/html5/thumbnails/85.jpg)
![Page 86: Stochastic Methods in Robotics (Probability, Fourier ... · • Pose Distributions for Nonholonomic Systems (Mobile Robots and Steerable Needles) • Lie-Theoretic Estimation and](https://reader033.fdocuments.us/reader033/viewer/2022043017/5f3976ef5dc58635dd56c0a5/html5/thumbnails/86.jpg)
![Page 87: Stochastic Methods in Robotics (Probability, Fourier ... · • Pose Distributions for Nonholonomic Systems (Mobile Robots and Steerable Needles) • Lie-Theoretic Estimation and](https://reader033.fdocuments.us/reader033/viewer/2022043017/5f3976ef5dc58635dd56c0a5/html5/thumbnails/87.jpg)
![Page 88: Stochastic Methods in Robotics (Probability, Fourier ... · • Pose Distributions for Nonholonomic Systems (Mobile Robots and Steerable Needles) • Lie-Theoretic Estimation and](https://reader033.fdocuments.us/reader033/viewer/2022043017/5f3976ef5dc58635dd56c0a5/html5/thumbnails/88.jpg)
![Page 89: Stochastic Methods in Robotics (Probability, Fourier ... · • Pose Distributions for Nonholonomic Systems (Mobile Robots and Steerable Needles) • Lie-Theoretic Estimation and](https://reader033.fdocuments.us/reader033/viewer/2022043017/5f3976ef5dc58635dd56c0a5/html5/thumbnails/89.jpg)
If you like this …
Have a look at my book:
``Stochastic Models, Information Theory, and Lie Groups’’
![Page 90: Stochastic Methods in Robotics (Probability, Fourier ... · • Pose Distributions for Nonholonomic Systems (Mobile Robots and Steerable Needles) • Lie-Theoretic Estimation and](https://reader033.fdocuments.us/reader033/viewer/2022043017/5f3976ef5dc58635dd56c0a5/html5/thumbnails/90.jpg)
An Information-Theoretic Approach to theCorrespondence-Free AX=XB Sensor Calibration
ProblemM. Kendal Ackerman1, Alexis Cheng2
and Gregory Chirikjian1
1Dept. Mech. Engineering, 2Dept. Computer Science
Johns Hopkins University, Baltimore, MD, USA
International Conference on Robotics and Automation
June 2014
![Page 91: Stochastic Methods in Robotics (Probability, Fourier ... · • Pose Distributions for Nonholonomic Systems (Mobile Robots and Steerable Needles) • Lie-Theoretic Estimation and](https://reader033.fdocuments.us/reader033/viewer/2022043017/5f3976ef5dc58635dd56c0a5/html5/thumbnails/91.jpg)
Outline
• The AX=XB Problem
• Notation and Probability Theory Review
• Solution Constraints
• An information Theoretic Approach
• Results and Conclusions
![Page 92: Stochastic Methods in Robotics (Probability, Fourier ... · • Pose Distributions for Nonholonomic Systems (Mobile Robots and Steerable Needles) • Lie-Theoretic Estimation and](https://reader033.fdocuments.us/reader033/viewer/2022043017/5f3976ef5dc58635dd56c0a5/html5/thumbnails/92.jpg)
AX = XB is one of the most common mathematical formulations used in robot-sensor calibration problems.
It can be found in a variety of applications including:
Camera calibration [1] Robot eye-to-hand
calibration [1]
Aerial vehicle sensor
calibration [2]
Image guided therapy
(IGT) sensor
calibration [3][1] Tsai, R., Lenz, R. (1989)[2] Mair, E., Fleps, M., Suppa, M., Burschka, D. (2011)
[3] Boctor, E.M. (2006)
Courtes
y:
Darius
Burschk
a
Courtes
y:
CAMP
at TUM
![Page 93: Stochastic Methods in Robotics (Probability, Fourier ... · • Pose Distributions for Nonholonomic Systems (Mobile Robots and Steerable Needles) • Lie-Theoretic Estimation and](https://reader033.fdocuments.us/reader033/viewer/2022043017/5f3976ef5dc58635dd56c0a5/html5/thumbnails/93.jpg)
𝐴, 𝐵, 𝑋 ϵ 𝑆𝐸 3 whereSE 3 = ℝ3⋉ 𝑆𝑂 3 and
SO 3 ≔ {𝑅 𝜖 ℝ3x3|𝑅𝑅𝑇 = 𝕀, det 𝑅 = +1}SE(3) is the Lie Group describing rigid body motions in 3-dimensional space, i.e.:
𝐻 ϵ 𝑆𝐸(3), where and
𝑅 ϵ 𝑆𝑂(3) (a proper rotation matrix), 𝐭 ϵ ℝ3 (a translation vector)
AX=XB
![Page 94: Stochastic Methods in Robotics (Probability, Fourier ... · • Pose Distributions for Nonholonomic Systems (Mobile Robots and Steerable Needles) • Lie-Theoretic Estimation and](https://reader033.fdocuments.us/reader033/viewer/2022043017/5f3976ef5dc58635dd56c0a5/html5/thumbnails/94.jpg)
AX=XB Solution Space
It is well known that it is not possible to solve for a unique X from a single pair of exact (A, B),but if there are two instances of independent exact measurements, (A1,B1) and (A2, B2), then the
problem can be solved.
One Parameter solution set
Rank 2
+
Two unspecified degrees of freedom
![Page 95: Stochastic Methods in Robotics (Probability, Fourier ... · • Pose Distributions for Nonholonomic Systems (Mobile Robots and Steerable Needles) • Lie-Theoretic Estimation and](https://reader033.fdocuments.us/reader033/viewer/2022043017/5f3976ef5dc58635dd56c0a5/html5/thumbnails/95.jpg)
The goal becomes one of finding an X with least-squared error given corresponding pairs (Ai, Bi) for i=1,2,...,n.
A unique solution is possible given two pairs
with certain constraints[4,5]
• SE(3) geometric invariants satisfied
• Angle of the rotation axes is “sufficiently
large”
• (Ai, Bi) pairs have Correspondence
AX=XB Solution Space
[4] Chen (1991)
[5] Ackerman, M.K., Cheng, A., Shiffman, B., Boctor, E., Chirikjian, G. (2014)
![Page 96: Stochastic Methods in Robotics (Probability, Fourier ... · • Pose Distributions for Nonholonomic Systems (Mobile Robots and Steerable Needles) • Lie-Theoretic Estimation and](https://reader033.fdocuments.us/reader033/viewer/2022043017/5f3976ef5dc58635dd56c0a5/html5/thumbnails/96.jpg)
In experimental applications, it is often the case that the data streams containing the A’s and B’s:
• will present at different sample rates,• will be asynchronous, • and each stream may contain gaps in information.
AX=XB Correspondence
Data Stream
Data Stream A’s
B’s
A1
B1
A2
B2
An
Bn
…
What we mean by correspondence:
![Page 97: Stochastic Methods in Robotics (Probability, Fourier ... · • Pose Distributions for Nonholonomic Systems (Mobile Robots and Steerable Needles) • Lie-Theoretic Estimation and](https://reader033.fdocuments.us/reader033/viewer/2022043017/5f3976ef5dc58635dd56c0a5/html5/thumbnails/97.jpg)
While our method removes the need to know the correspondence of the data, there have been other attempts in the literature to regenerate the correspondence by
1. time stamping the data[5]
2. dedicated software modules for syncing the data[6]
3. analyzing components of the sensor data stream to determine a correlation[7,8]
We present a method for calculating the calibration transformation, X, that works for data without any a priori knowledge of the correspondence between the A’s and B’s.
AX=XB Correspondence
[5] Mills, D. L.[6] Kang, H. J., Cheng, A., Boctor, E. M.[7] Mair, E., Fleps, M., Suppa, M., Burschka, D [8] Ackerman, M.,K., Cheng, A., Shiffman, B., Boctor, E., Chirikjian, G.
![Page 98: Stochastic Methods in Robotics (Probability, Fourier ... · • Pose Distributions for Nonholonomic Systems (Mobile Robots and Steerable Needles) • Lie-Theoretic Estimation and](https://reader033.fdocuments.us/reader033/viewer/2022043017/5f3976ef5dc58635dd56c0a5/html5/thumbnails/98.jpg)
Rigid Body Motion
is a Lie group and therefore the concept of integration exists:
, where:
The group of rigid body
motions, SE(3),
which is “natural”, because for any
𝐻0 ϵ 𝑆𝐸 3 ,
,
![Page 99: Stochastic Methods in Robotics (Probability, Fourier ... · • Pose Distributions for Nonholonomic Systems (Mobile Robots and Steerable Needles) • Lie-Theoretic Estimation and](https://reader033.fdocuments.us/reader033/viewer/2022043017/5f3976ef5dc58635dd56c0a5/html5/thumbnails/99.jpg)
Convolution on SE(3)
Define a delta function as:
Define the convolution of two functions as:
And a shifted delta function as:, where A 𝜖 𝑆𝐸 3
The “v” operator is defined as:
(x1, x2, … , xn)T
![Page 100: Stochastic Methods in Robotics (Probability, Fourier ... · • Pose Distributions for Nonholonomic Systems (Mobile Robots and Steerable Needles) • Lie-Theoretic Estimation and](https://reader033.fdocuments.us/reader033/viewer/2022043017/5f3976ef5dc58635dd56c0a5/html5/thumbnails/100.jpg)
PDFs on SE(3)
The mean and covariance of a probability density function, f(H), can be defined as
Where is usually something akin to
Traditional Riemannian-geometric approach:
Avoid the arbitrary bias of a weighting matrix and avoid the need for a bi-invariant distance metric, which does not
exist for SE(3)
![Page 101: Stochastic Methods in Robotics (Probability, Fourier ... · • Pose Distributions for Nonholonomic Systems (Mobile Robots and Steerable Needles) • Lie-Theoretic Estimation and](https://reader033.fdocuments.us/reader033/viewer/2022043017/5f3976ef5dc58635dd56c0a5/html5/thumbnails/101.jpg)
Mean and Covariance
and
We use:
Where is usually something
akin to
Traditional Riemannian-geometric approach:
Avoid the arbitrary bias of a weighting matrix and avoid the need for a bi-invariant distance metric, which does not
exist for SE(3)
![Page 102: Stochastic Methods in Robotics (Probability, Fourier ... · • Pose Distributions for Nonholonomic Systems (Mobile Robots and Steerable Needles) • Lie-Theoretic Estimation and](https://reader033.fdocuments.us/reader033/viewer/2022043017/5f3976ef5dc58635dd56c0a5/html5/thumbnails/102.jpg)
The “Batch” Equations
because real-valued functions can be added and convolution is a linear operation on functions, all n instances can be written into a single equation of the form
Given
We can normalize the functions to be
probability density functions (pdfs):
where
![Page 103: Stochastic Methods in Robotics (Probability, Fourier ... · • Pose Distributions for Nonholonomic Systems (Mobile Robots and Steerable Needles) • Lie-Theoretic Estimation and](https://reader033.fdocuments.us/reader033/viewer/2022043017/5f3976ef5dc58635dd56c0a5/html5/thumbnails/103.jpg)
The “Batch” Equations
Given that , we can write the evolution of the mean and covariance as:
where
and the “hat” operator is defined such
that given
and
≈
![Page 104: Stochastic Methods in Robotics (Probability, Fourier ... · • Pose Distributions for Nonholonomic Systems (Mobile Robots and Steerable Needles) • Lie-Theoretic Estimation and](https://reader033.fdocuments.us/reader033/viewer/2022043017/5f3976ef5dc58635dd56c0a5/html5/thumbnails/104.jpg)
The “Batch” Equations
Since the mean of is and its covariance is the zero matrix we can write the “Batch” formulation
Batch Method “AX=XB” Equations:
(1
)
(2
)
![Page 105: Stochastic Methods in Robotics (Probability, Fourier ... · • Pose Distributions for Nonholonomic Systems (Mobile Robots and Steerable Needles) • Lie-Theoretic Estimation and](https://reader033.fdocuments.us/reader033/viewer/2022043017/5f3976ef5dc58635dd56c0a5/html5/thumbnails/105.jpg)
Solution Space
The search for an appropriate X can begin with re-writing (1) asBy defining
The equation can be separated into
rotational and translational components,
From which it can be seen that the
possible solution space, for (1), is two
dimensional.
Batch Method “AX=XB” Equations:
(1
)
(2
)
and
![Page 106: Stochastic Methods in Robotics (Probability, Fourier ... · • Pose Distributions for Nonholonomic Systems (Mobile Robots and Steerable Needles) • Lie-Theoretic Estimation and](https://reader033.fdocuments.us/reader033/viewer/2022043017/5f3976ef5dc58635dd56c0a5/html5/thumbnails/106.jpg)
Discretization
With the discrete nature of our application, we can likewise define the mean and covariance in a discrete sense
An iterative procedure can be used for
computing MA which uses an initial
estimate of the formThen a gradient descent procedure is used
to update so as to minimize the cost
The covariance can then be computed:
![Page 107: Stochastic Methods in Robotics (Probability, Fourier ... · • Pose Distributions for Nonholonomic Systems (Mobile Robots and Steerable Needles) • Lie-Theoretic Estimation and](https://reader033.fdocuments.us/reader033/viewer/2022043017/5f3976ef5dc58635dd56c0a5/html5/thumbnails/107.jpg)
Where is the free parameter and
where is the free
parameter
Solution Space
For the rotational part, we can write 𝑅𝑋 as
We then re-write the translation part as
𝒏𝑨 is rank 2 so there is a degree of freedom in 𝒕𝑿along 𝒏𝑨. Therefore we write 𝒕𝑿 as
![Page 108: Stochastic Methods in Robotics (Probability, Fourier ... · • Pose Distributions for Nonholonomic Systems (Mobile Robots and Steerable Needles) • Lie-Theoretic Estimation and](https://reader033.fdocuments.us/reader033/viewer/2022043017/5f3976ef5dc58635dd56c0a5/html5/thumbnails/108.jpg)
Solution Space
A feasible solution to the batch equation can be parameterized as
where
Given that (1) constrains the possible
solutions to a two-dimensional “cylinder”,
the problem of solving for X reduces to
that of solving (2) on this cylinder by
determining the values (𝝓, 𝒔). There is
therefore no need to search elsewhere in
the 6D group SE(3).
![Page 109: Stochastic Methods in Robotics (Probability, Fourier ... · • Pose Distributions for Nonholonomic Systems (Mobile Robots and Steerable Needles) • Lie-Theoretic Estimation and](https://reader033.fdocuments.us/reader033/viewer/2022043017/5f3976ef5dc58635dd56c0a5/html5/thumbnails/109.jpg)
∙ 𝑭𝟐 Minimization
Minimize the cost function
𝐶1is quadratic in 𝑠 and can be written
as
The minimization of 𝑠 is solved for in
closed-form,
and then is substituted into the
original expression, leaving a one
dimensional search over
![Page 110: Stochastic Methods in Robotics (Probability, Fourier ... · • Pose Distributions for Nonholonomic Systems (Mobile Robots and Steerable Needles) • Lie-Theoretic Estimation and](https://reader033.fdocuments.us/reader033/viewer/2022043017/5f3976ef5dc58635dd56c0a5/html5/thumbnails/110.jpg)
KL Divergence
A Gaussian on SE(3) can be defined when the norm Σ is small as
where Σ denotes the determinant of Σ and
We write the Kullback-Leibler divergence
of the two distributions as
![Page 111: Stochastic Methods in Robotics (Probability, Fourier ... · • Pose Distributions for Nonholonomic Systems (Mobile Robots and Steerable Needles) • Lie-Theoretic Estimation and](https://reader033.fdocuments.us/reader033/viewer/2022043017/5f3976ef5dc58635dd56c0a5/html5/thumbnails/111.jpg)
Minimal KL Divergence
Minimize the cost function
where
If we take into account that our
search is limited to the cylinder as
defined in (1), then automatically
an
d
![Page 112: Stochastic Methods in Robotics (Probability, Fourier ... · • Pose Distributions for Nonholonomic Systems (Mobile Robots and Steerable Needles) • Lie-Theoretic Estimation and](https://reader033.fdocuments.us/reader033/viewer/2022043017/5f3976ef5dc58635dd56c0a5/html5/thumbnails/112.jpg)
Minimal KL Divergence
We can now write a new variable,, and minimize the cost function
where
Since SE(3) is unimodular, and additive
and positive multiplicative constants can
be ignored, we can simply consider the
first term in the KL-divergence, scaled by a
factor of two:
an
d
![Page 113: Stochastic Methods in Robotics (Probability, Fourier ... · • Pose Distributions for Nonholonomic Systems (Mobile Robots and Steerable Needles) • Lie-Theoretic Estimation and](https://reader033.fdocuments.us/reader033/viewer/2022043017/5f3976ef5dc58635dd56c0a5/html5/thumbnails/113.jpg)
Minimal KL Divergence
Since SE(3) is unimodular, and
additive and positive multiplicative
constants can be ignored, we can
simply consider the first term in the
KL-divergence, scaled by a factor of
twominimized over
Minimization over s can be done in
closed form as in the previous
approach, since 𝐶2is also quadratic
in s. After substitution, this again
results in a one dimensional search
![Page 114: Stochastic Methods in Robotics (Probability, Fourier ... · • Pose Distributions for Nonholonomic Systems (Mobile Robots and Steerable Needles) • Lie-Theoretic Estimation and](https://reader033.fdocuments.us/reader033/viewer/2022043017/5f3976ef5dc58635dd56c0a5/html5/thumbnails/114.jpg)
To experimentally test our methods for AX=XB calibration we use an Ultrasound (US) sensor calibration process. It should be noted that these methods can be extended to other application areas, both in US and more
generally.
Calibration of Sensor Data
on the Euclidean Group
Ultrasound Calibration
The AX=XB Problem
![Page 115: Stochastic Methods in Robotics (Probability, Fourier ... · • Pose Distributions for Nonholonomic Systems (Mobile Robots and Steerable Needles) • Lie-Theoretic Estimation and](https://reader033.fdocuments.us/reader033/viewer/2022043017/5f3976ef5dc58635dd56c0a5/html5/thumbnails/115.jpg)
3D image volumes Augmented reality environments
US Elastography
Through calibration we recover parameters that are required to perform more advanced forms of image based guidance using Ultrasound (US)
![Page 116: Stochastic Methods in Robotics (Probability, Fourier ... · • Pose Distributions for Nonholonomic Systems (Mobile Robots and Steerable Needles) • Lie-Theoretic Estimation and](https://reader033.fdocuments.us/reader033/viewer/2022043017/5f3976ef5dc58635dd56c0a5/html5/thumbnails/116.jpg)
![Page 117: Stochastic Methods in Robotics (Probability, Fourier ... · • Pose Distributions for Nonholonomic Systems (Mobile Robots and Steerable Needles) • Lie-Theoretic Estimation and](https://reader033.fdocuments.us/reader033/viewer/2022043017/5f3976ef5dc58635dd56c0a5/html5/thumbnails/117.jpg)
Simulation Results
The two algorithms were unaffected by knowledge
of correspondence and, in each case performed
with a high level of accuracy. The results are the
average of ten trials.
![Page 118: Stochastic Methods in Robotics (Probability, Fourier ... · • Pose Distributions for Nonholonomic Systems (Mobile Robots and Steerable Needles) • Lie-Theoretic Estimation and](https://reader033.fdocuments.us/reader033/viewer/2022043017/5f3976ef5dc58635dd56c0a5/html5/thumbnails/118.jpg)
US Experimental Results
For each reconstruction point, we found its closest
point match on the model and computed the sum
squared difference between them. Our results
show the mean and the standard deviation of this
sum of squared differences and indicates that the
error is reasonable.
![Page 119: Stochastic Methods in Robotics (Probability, Fourier ... · • Pose Distributions for Nonholonomic Systems (Mobile Robots and Steerable Needles) • Lie-Theoretic Estimation and](https://reader033.fdocuments.us/reader033/viewer/2022043017/5f3976ef5dc58635dd56c0a5/html5/thumbnails/119.jpg)
US Experimental Results
To examine the accuracy of the computed X, we
performed a reconstruction of the phantom model.
![Page 120: Stochastic Methods in Robotics (Probability, Fourier ... · • Pose Distributions for Nonholonomic Systems (Mobile Robots and Steerable Needles) • Lie-Theoretic Estimation and](https://reader033.fdocuments.us/reader033/viewer/2022043017/5f3976ef5dc58635dd56c0a5/html5/thumbnails/120.jpg)
• We established that the AX = XB sensor calibration problem can be formulated with a “Batch”, probabilistic formulation that does not require a priori knowledge of the A and B correspondence.
• We presented an information-theoretic algorithm (KL Batch) that solves for X by minimizing the Kullback-Leibler divergence of the A and B sensor stream distributions with respect to the unknown X.
• In both simulation and experimentation, we demonstrated that this method reliably recovers an unknown X without the need for correspondence.
Conclusions
![Page 121: Stochastic Methods in Robotics (Probability, Fourier ... · • Pose Distributions for Nonholonomic Systems (Mobile Robots and Steerable Needles) • Lie-Theoretic Estimation and](https://reader033.fdocuments.us/reader033/viewer/2022043017/5f3976ef5dc58635dd56c0a5/html5/thumbnails/121.jpg)
• We will further examine the proposed methods experimentally, for ultrasound calibration, as well as other contexts.
• We will work to improve our probability theoretic formulation by specifically accounting for sensor measurement noise, representing X by a mean and covariance, and not just a Dirac delta distribution.
Future Work
![Page 122: Stochastic Methods in Robotics (Probability, Fourier ... · • Pose Distributions for Nonholonomic Systems (Mobile Robots and Steerable Needles) • Lie-Theoretic Estimation and](https://reader033.fdocuments.us/reader033/viewer/2022043017/5f3976ef5dc58635dd56c0a5/html5/thumbnails/122.jpg)
This work was supported by NSF Grant RI-
Medium: 1162095
Acknowledgements
![Page 123: Stochastic Methods in Robotics (Probability, Fourier ... · • Pose Distributions for Nonholonomic Systems (Mobile Robots and Steerable Needles) • Lie-Theoretic Estimation and](https://reader033.fdocuments.us/reader033/viewer/2022043017/5f3976ef5dc58635dd56c0a5/html5/thumbnails/123.jpg)
Voronoi Cells in Lie Groups and Coset Decompositions: Implications for
Optimization, Integration, and Fourier Analysis
Yan Yan and Gregory S. Chirikjian
Department of Mechanical Engineering
Johns Hopkins University, Baltimore, MD 21218
![Page 124: Stochastic Methods in Robotics (Probability, Fourier ... · • Pose Distributions for Nonholonomic Systems (Mobile Robots and Steerable Needles) • Lie-Theoretic Estimation and](https://reader033.fdocuments.us/reader033/viewer/2022043017/5f3976ef5dc58635dd56c0a5/html5/thumbnails/124.jpg)
• Review of basic concepts in group theory and the Lie groups SE(2) and SO(3).
• Generating almost-uniform sample points in SE(2)and SO(3) based on coset decomposition.
• More efficient computations of convolutions on groups developed by coset decomposition.
Outlines
![Page 125: Stochastic Methods in Robotics (Probability, Fourier ... · • Pose Distributions for Nonholonomic Systems (Mobile Robots and Steerable Needles) • Lie-Theoretic Estimation and](https://reader033.fdocuments.us/reader033/viewer/2022043017/5f3976ef5dc58635dd56c0a5/html5/thumbnails/125.jpg)
• A group 𝐺,∘ is a set, 𝐺, together with a binary operation,∘,
that satisfies (1) closure; (2) associativity; (3) existence of identity element; (4) existence of inverse element.
In this paper, we mainly focus on the group of rotations in space, SO(3), and the group of rigid-body motions of the plane, SE(2).
• A subgroup is a subset of a group 𝐻 ⊆ 𝐺 which is itself a group that is closed under the group operation of 𝐺.
• SO(3) and SE(2) contain discrete subgroups.
Basic concepts in group theory
Groups of rotational symmetry operations of the Platonic solids Five chiral wallpaper groups
p1 p2 p4
p3 p6
![Page 126: Stochastic Methods in Robotics (Probability, Fourier ... · • Pose Distributions for Nonholonomic Systems (Mobile Robots and Steerable Needles) • Lie-Theoretic Estimation and](https://reader033.fdocuments.us/reader033/viewer/2022043017/5f3976ef5dc58635dd56c0a5/html5/thumbnails/126.jpg)
Let Γ, Γ′ < G denote discrete subgroups, then left- and right-coset-spaces are defined as
A double coset space is define as
Basic concepts in group theory
and .
Associated with any (double-) coset, it is possible to define a fundamental domain in G, which is a set of distinguished (double-) coset representatives, exactly one per (double-) coset. It has the same dimension as G, but lesser volume.
It can be constructed as Voronoi cells in G.
and when
![Page 127: Stochastic Methods in Robotics (Probability, Fourier ... · • Pose Distributions for Nonholonomic Systems (Mobile Robots and Steerable Needles) • Lie-Theoretic Estimation and](https://reader033.fdocuments.us/reader033/viewer/2022043017/5f3976ef5dc58635dd56c0a5/html5/thumbnails/127.jpg)
SO(3) represented in
exponential coordinates
Voronoi cells
Γ: the Icosahedral group
The center Voronoi cell
the fundamental domain
Fundamental domains for SO(3)
Constructed as Voronoi cells with 0
Fundamental domain for other discrete subgroups in SO(3):
The Tetrahedral group The Icosahedral groupThe Octahedral group
Smallest volume!
(Yan and Chirikjian, ICRA’12)
![Page 128: Stochastic Methods in Robotics (Probability, Fourier ... · • Pose Distributions for Nonholonomic Systems (Mobile Robots and Steerable Needles) • Lie-Theoretic Estimation and](https://reader033.fdocuments.us/reader033/viewer/2022043017/5f3976ef5dc58635dd56c0a5/html5/thumbnails/128.jpg)
Fundamental domains for SE(2)
For the first time, we establish the fundamental domains for SE(2) when Γ is one of the five chiral wallpaper groups, p1, p2, p3, p4 and p5.
Distance function: 0
SE(2) represented in ℝ3 Voronoi cells with 𝜞 = p1
The center Voronoi cell the fundamental domain
For p1: it looks like a hexagonal box with the height
from –π to π.
We note that if the lattice is square instead of
parallelogrammatic, the center Voronoi cells
becomes a square box.
![Page 129: Stochastic Methods in Robotics (Probability, Fourier ... · • Pose Distributions for Nonholonomic Systems (Mobile Robots and Steerable Needles) • Lie-Theoretic Estimation and](https://reader033.fdocuments.us/reader033/viewer/2022043017/5f3976ef5dc58635dd56c0a5/html5/thumbnails/129.jpg)
Fundamental domains for SE(2)
Fundamental domains of SE(2) based on the five chiral wallpaper groups:
p1 p2 p4
p3 p6
![Page 130: Stochastic Methods in Robotics (Probability, Fourier ... · • Pose Distributions for Nonholonomic Systems (Mobile Robots and Steerable Needles) • Lie-Theoretic Estimation and](https://reader033.fdocuments.us/reader033/viewer/2022043017/5f3976ef5dc58635dd56c0a5/html5/thumbnails/130.jpg)
Why do we study this?
![Page 131: Stochastic Methods in Robotics (Probability, Fourier ... · • Pose Distributions for Nonholonomic Systems (Mobile Robots and Steerable Needles) • Lie-Theoretic Estimation and](https://reader033.fdocuments.us/reader033/viewer/2022043017/5f3976ef5dc58635dd56c0a5/html5/thumbnails/131.jpg)
Application 1: generating almost-uniform samples
![Page 132: Stochastic Methods in Robotics (Probability, Fourier ... · • Pose Distributions for Nonholonomic Systems (Mobile Robots and Steerable Needles) • Lie-Theoretic Estimation and](https://reader033.fdocuments.us/reader033/viewer/2022043017/5f3976ef5dc58635dd56c0a5/html5/thumbnails/132.jpg)
• The discretization of the groups of rotations or rigid-body motions, arises in many applications such as robot motion planning;
computational structural biology;
Computer graphics
• Uniform sampling will prevent search algorithms from oversampling or undersampling large portions of the C-space.
• This affects both the performance and reliability of planning algorithms.
Importance of uniform sampling
![Page 133: Stochastic Methods in Robotics (Probability, Fourier ... · • Pose Distributions for Nonholonomic Systems (Mobile Robots and Steerable Needles) • Lie-Theoretic Estimation and](https://reader033.fdocuments.us/reader033/viewer/2022043017/5f3976ef5dc58635dd56c0a5/html5/thumbnails/133.jpg)
SO(3) Example:
…
Sampling based on single coset decomposition
(Yan and Chirikjian, ICRA’12)
when is small,
![Page 134: Stochastic Methods in Robotics (Probability, Fourier ... · • Pose Distributions for Nonholonomic Systems (Mobile Robots and Steerable Needles) • Lie-Theoretic Estimation and](https://reader033.fdocuments.us/reader033/viewer/2022043017/5f3976ef5dc58635dd56c0a5/html5/thumbnails/134.jpg)
Distortion measure: ,
our sampling method ZXZ Euler angles
The grids generated on SO(3) are almost uniform!
![Page 135: Stochastic Methods in Robotics (Probability, Fourier ... · • Pose Distributions for Nonholonomic Systems (Mobile Robots and Steerable Needles) • Lie-Theoretic Estimation and](https://reader033.fdocuments.us/reader033/viewer/2022043017/5f3976ef5dc58635dd56c0a5/html5/thumbnails/135.jpg)
Can we do better than this?
![Page 136: Stochastic Methods in Robotics (Probability, Fourier ... · • Pose Distributions for Nonholonomic Systems (Mobile Robots and Steerable Needles) • Lie-Theoretic Estimation and](https://reader033.fdocuments.us/reader033/viewer/2022043017/5f3976ef5dc58635dd56c0a5/html5/thumbnails/136.jpg)
Sampling based on double coset decomposition
Given two finite subgroups, 𝐻,𝐾 < 𝐺, where 𝐺 = 𝑆𝑂 3 , 𝐻 ∩ 𝐾 = 1,the resulting non-overlapping tiles satisfy
Some examples of double-coset spaces:
Yellow-shaded region: single coset-space 𝐹𝑆𝑂(3)/𝐾 with 𝐾= the icosahedral group
Red-shaded region: double coset-space 𝐹𝐻\𝑆𝑂(3)/𝐾 with 𝐾= the icosahedral group,
𝐻= the conjugated (a) tetrahedral , (b) octahedral, and (c) icosahedral groups.The conjugated group 𝐻: 𝐻 = 𝑔𝐻0𝑔
−1 for 𝑔 ∈ 𝐺.
![Page 137: Stochastic Methods in Robotics (Probability, Fourier ... · • Pose Distributions for Nonholonomic Systems (Mobile Robots and Steerable Needles) • Lie-Theoretic Estimation and](https://reader033.fdocuments.us/reader033/viewer/2022043017/5f3976ef5dc58635dd56c0a5/html5/thumbnails/137.jpg)
Sampling based on double coset decomposition
As 𝑯| ∙ |𝑲 increases, the size of the center Voronoi cells shrinks, which leads to smaller distortion.
![Page 138: Stochastic Methods in Robotics (Probability, Fourier ... · • Pose Distributions for Nonholonomic Systems (Mobile Robots and Steerable Needles) • Lie-Theoretic Estimation and](https://reader033.fdocuments.us/reader033/viewer/2022043017/5f3976ef5dc58635dd56c0a5/html5/thumbnails/138.jpg)
Advantages of this sampling approach?
has low metric distortionis deterministichas grid structure with respect to the metric on SO(3)can easily achieve any level of resolution
![Page 139: Stochastic Methods in Robotics (Probability, Fourier ... · • Pose Distributions for Nonholonomic Systems (Mobile Robots and Steerable Needles) • Lie-Theoretic Estimation and](https://reader033.fdocuments.us/reader033/viewer/2022043017/5f3976ef5dc58635dd56c0a5/html5/thumbnails/139.jpg)
Application 2: Efficient computation of convolution on rotation and motion groups
![Page 140: Stochastic Methods in Robotics (Probability, Fourier ... · • Pose Distributions for Nonholonomic Systems (Mobile Robots and Steerable Needles) • Lie-Theoretic Estimation and](https://reader033.fdocuments.us/reader033/viewer/2022043017/5f3976ef5dc58635dd56c0a5/html5/thumbnails/140.jpg)
Fast Convolutions by Direct Evaluation
Convolution on groups:
Here, 𝑑ℎ is the natural integration measure for 𝐺.
An integration over 𝐺 :
where 𝑑𝑔′ is the same volume element as for 𝐺, but restricted to 𝐹𝐻\G/𝐾 < 𝐺.
Efficient algorithms for computing convolutions on roation and motion groups have been developed previously using “group FFTs” --- Chirikjian and Kyatkin, 01; Kostelec and Rockmore, 08; Maslen and Rockmore, 97.
Usually Euler angle decompositions are used for SO(3). We introduce two potential alternatives to this approach based on double
coset decompositions described earlier.
Computed by direct evaluation:
![Page 141: Stochastic Methods in Robotics (Probability, Fourier ... · • Pose Distributions for Nonholonomic Systems (Mobile Robots and Steerable Needles) • Lie-Theoretic Estimation and](https://reader033.fdocuments.us/reader033/viewer/2022043017/5f3976ef5dc58635dd56c0a5/html5/thumbnails/141.jpg)
Fast Approximate FFTs on SO(3) and SE(2)
Instead of using Euler angles to parameterize SO(3), we can develop different FFT algorithms based on different parameterizations and coset decompositions.
Specific property of IURs (irreducible unitary representations)
We use this property together with the double coset decomposition:
• The fact that on the fundamental domain centered on the identity 𝑈(𝑒𝑥𝑝𝑋, 𝑙) can be expressed as a truncated Taylor series in 𝑋 is then very useful because 𝑊 𝑋, 𝑙 will have polynomial entries, each of which can be computed by evaluation on their boundary.
• Therefore, the computation of the integral over 𝐹𝐻\𝑆𝑂(3)/𝐾 is efficient.
![Page 142: Stochastic Methods in Robotics (Probability, Fourier ... · • Pose Distributions for Nonholonomic Systems (Mobile Robots and Steerable Needles) • Lie-Theoretic Estimation and](https://reader033.fdocuments.us/reader033/viewer/2022043017/5f3976ef5dc58635dd56c0a5/html5/thumbnails/142.jpg)
• We make a connection between Voronoi cells in the groups SO(3) and SE(2) centered on elements of discrete subgroups, and coset- and double-coset-spaces.
• We show that sampling within these Voronoi cells can be made almost uniform by exponentiating a Cartesian grid in a region of the corresponding Lie algebra, which is the pre-image of these cells under the exponential map.
• We show how the resulting cells, and the samples therein, can be used for searches, optimization, and Fourier analysis on certain Lie groups of interest in robotics and control.
Conclusions (for this part)
![Page 143: Stochastic Methods in Robotics (Probability, Fourier ... · • Pose Distributions for Nonholonomic Systems (Mobile Robots and Steerable Needles) • Lie-Theoretic Estimation and](https://reader033.fdocuments.us/reader033/viewer/2022043017/5f3976ef5dc58635dd56c0a5/html5/thumbnails/143.jpg)
![Page 144: Stochastic Methods in Robotics (Probability, Fourier ... · • Pose Distributions for Nonholonomic Systems (Mobile Robots and Steerable Needles) • Lie-Theoretic Estimation and](https://reader033.fdocuments.us/reader033/viewer/2022043017/5f3976ef5dc58635dd56c0a5/html5/thumbnails/144.jpg)
First Some Pretty Pictures from Old Work: Elastic Network Interpolation for the GroEL-GroES complex
![Page 145: Stochastic Methods in Robotics (Probability, Fourier ... · • Pose Distributions for Nonholonomic Systems (Mobile Robots and Steerable Needles) • Lie-Theoretic Estimation and](https://reader033.fdocuments.us/reader033/viewer/2022043017/5f3976ef5dc58635dd56c0a5/html5/thumbnails/145.jpg)
What is the Structure of the Space of Motions of
Bodies that Move Collectively with Symmetry ?
Figure generated using ``Escher Mobile iphoneApp’’
developed in the group of G. Chapuis at EPFL
![Page 146: Stochastic Methods in Robotics (Probability, Fourier ... · • Pose Distributions for Nonholonomic Systems (Mobile Robots and Steerable Needles) • Lie-Theoretic Estimation and](https://reader033.fdocuments.us/reader033/viewer/2022043017/5f3976ef5dc58635dd56c0a5/html5/thumbnails/146.jpg)
How to Characterize the Free Space of Motions of Bodies that Move Collectively with Symmetry ?
![Page 147: Stochastic Methods in Robotics (Probability, Fourier ... · • Pose Distributions for Nonholonomic Systems (Mobile Robots and Steerable Needles) • Lie-Theoretic Estimation and](https://reader033.fdocuments.us/reader033/viewer/2022043017/5f3976ef5dc58635dd56c0a5/html5/thumbnails/147.jpg)
:electron density of a single protein; : Fourier transform;
:crystal symmetry operation (known); :rigid-body motion (unknown).
Protein X-Ray Crystallography
FilmDiffracted X-rays
Direct X-ray beam
Reflection
Diffraction pattern from X-ray crystallography experiment:
Electron density of a single-protein:
: the Cartesian coordinates of the i-th atoms; :the electron density map of i-th atom in a reference frame centered on it.
![Page 148: Stochastic Methods in Robotics (Probability, Fourier ... · • Pose Distributions for Nonholonomic Systems (Mobile Robots and Steerable Needles) • Lie-Theoretic Estimation and](https://reader033.fdocuments.us/reader033/viewer/2022043017/5f3976ef5dc58635dd56c0a5/html5/thumbnails/148.jpg)
![Page 149: Stochastic Methods in Robotics (Probability, Fourier ... · • Pose Distributions for Nonholonomic Systems (Mobile Robots and Steerable Needles) • Lie-Theoretic Estimation and](https://reader033.fdocuments.us/reader033/viewer/2022043017/5f3976ef5dc58635dd56c0a5/html5/thumbnails/149.jpg)
Symmetries in the Density Functionof a Protein Crystal
![Page 150: Stochastic Methods in Robotics (Probability, Fourier ... · • Pose Distributions for Nonholonomic Systems (Mobile Robots and Steerable Needles) • Lie-Theoretic Estimation and](https://reader033.fdocuments.us/reader033/viewer/2022043017/5f3976ef5dc58635dd56c0a5/html5/thumbnails/150.jpg)
Rigid-Body Motions in Euclidean Space
![Page 151: Stochastic Methods in Robotics (Probability, Fourier ... · • Pose Distributions for Nonholonomic Systems (Mobile Robots and Steerable Needles) • Lie-Theoretic Estimation and](https://reader033.fdocuments.us/reader033/viewer/2022043017/5f3976ef5dc58635dd56c0a5/html5/thumbnails/151.jpg)
Decomposing Continuous Motions
![Page 152: Stochastic Methods in Robotics (Probability, Fourier ... · • Pose Distributions for Nonholonomic Systems (Mobile Robots and Steerable Needles) • Lie-Theoretic Estimation and](https://reader033.fdocuments.us/reader033/viewer/2022043017/5f3976ef5dc58635dd56c0a5/html5/thumbnails/152.jpg)
Discrete (Crystallographic) Motion Groups
![Page 153: Stochastic Methods in Robotics (Probability, Fourier ... · • Pose Distributions for Nonholonomic Systems (Mobile Robots and Steerable Needles) • Lie-Theoretic Estimation and](https://reader033.fdocuments.us/reader033/viewer/2022043017/5f3976ef5dc58635dd56c0a5/html5/thumbnails/153.jpg)
Examples of Crystallographic Space Groups
![Page 154: Stochastic Methods in Robotics (Probability, Fourier ... · • Pose Distributions for Nonholonomic Systems (Mobile Robots and Steerable Needles) • Lie-Theoretic Estimation and](https://reader033.fdocuments.us/reader033/viewer/2022043017/5f3976ef5dc58635dd56c0a5/html5/thumbnails/154.jpg)
Cosets, Quotients, and Fundamental Domains
![Page 155: Stochastic Methods in Robotics (Probability, Fourier ... · • Pose Distributions for Nonholonomic Systems (Mobile Robots and Steerable Needles) • Lie-Theoretic Estimation and](https://reader033.fdocuments.us/reader033/viewer/2022043017/5f3976ef5dc58635dd56c0a5/html5/thumbnails/155.jpg)
Concrete Planar Examples
p4 =
![Page 156: Stochastic Methods in Robotics (Probability, Fourier ... · • Pose Distributions for Nonholonomic Systems (Mobile Robots and Steerable Needles) • Lie-Theoretic Estimation and](https://reader033.fdocuments.us/reader033/viewer/2022043017/5f3976ef5dc58635dd56c0a5/html5/thumbnails/156.jpg)
Embeddings and Immersions of Motion Spaces in R^n
p1\SE(2) = T^3
![Page 157: Stochastic Methods in Robotics (Probability, Fourier ... · • Pose Distributions for Nonholonomic Systems (Mobile Robots and Steerable Needles) • Lie-Theoretic Estimation and](https://reader033.fdocuments.us/reader033/viewer/2022043017/5f3976ef5dc58635dd56c0a5/html5/thumbnails/157.jpg)
Immersions of p4\SE(2) in R^6
![Page 158: Stochastic Methods in Robotics (Probability, Fourier ... · • Pose Distributions for Nonholonomic Systems (Mobile Robots and Steerable Needles) • Lie-Theoretic Estimation and](https://reader033.fdocuments.us/reader033/viewer/2022043017/5f3976ef5dc58635dd56c0a5/html5/thumbnails/158.jpg)
Conclusions
• In protein crystals bodies are arranged with symmetry, but there is a hidden rigid-body motion that is important to find.
• This motion lives in a coset space (quotient of SE(3) by a discrete subgroup of crystallo-graphic symmetry operations).
• This paper characterizes this space (which is a manifold) and corresponding fundamental domains
![Page 159: Stochastic Methods in Robotics (Probability, Fourier ... · • Pose Distributions for Nonholonomic Systems (Mobile Robots and Steerable Needles) • Lie-Theoretic Estimation and](https://reader033.fdocuments.us/reader033/viewer/2022043017/5f3976ef5dc58635dd56c0a5/html5/thumbnails/159.jpg)
References
G.S. Chirikjian, ``Mathematical Aspects of Molecular Replacement: I. Algebraic Properties of Motion Spaces,'‘ Acta. Cryst. A (2011). A67, 435–446
G.S. Chirikjian, Yan, Y., ``Mathematical Aspects of Molecular Replacement: II. Geometry of Motion Spaces,'‘ Acta. Cryst. A (2012). A68, 208-221.
Chirikjian, G.S., Stochastic Models, Information Theory, and Lie Groups: Volume 2 - Analytic Methods and Modern Applications,
Birkhauser, Dec. 2011.
Chirikjian, G.S., Kyatkin, A.B., Engineering Applications of Noncommutative Harmonic Analysis, CRC Press, 2001.
![Page 160: Stochastic Methods in Robotics (Probability, Fourier ... · • Pose Distributions for Nonholonomic Systems (Mobile Robots and Steerable Needles) • Lie-Theoretic Estimation and](https://reader033.fdocuments.us/reader033/viewer/2022043017/5f3976ef5dc58635dd56c0a5/html5/thumbnails/160.jpg)