forked from logzhan/ORB-SLAM3-UESTC
254 lines
8.7 KiB
Python
254 lines
8.7 KiB
Python
|
import sympy
|
||
|
|
import sys
|
||
|
|
import unittest
|
||
|
|
import sophus
|
||
|
|
import functools
|
||
|
|
|
||
|
|
|
||
|
|
class So3:
|
||
|
|
""" 3 dimensional group of orthogonal matrices with determinant 1 """
|
||
|
|
|
||
|
|
def __init__(self, q):
|
||
|
|
""" internally represented by a unit quaternion q """
|
||
|
|
self.q = q
|
||
|
|
|
||
|
|
@staticmethod
|
||
|
|
def exp(v):
|
||
|
|
""" exponential map """
|
||
|
|
theta_sq = sophus.squared_norm(v)
|
||
|
|
theta = sympy.sqrt(theta_sq)
|
||
|
|
return So3(
|
||
|
|
sophus.Quaternion(
|
||
|
|
sympy.cos(0.5 * theta),
|
||
|
|
sympy.sin(0.5 * theta) / theta * v))
|
||
|
|
|
||
|
|
def log(self):
|
||
|
|
""" logarithmic map"""
|
||
|
|
n = sympy.sqrt(sophus.squared_norm(self.q.vec))
|
||
|
|
return 2 * sympy.atan(n / self.q.real) / n * self.q.vec
|
||
|
|
|
||
|
|
def __repr__(self):
|
||
|
|
return "So3:" + repr(self.q)
|
||
|
|
|
||
|
|
def inverse(self):
|
||
|
|
return So3(self.q.conj())
|
||
|
|
|
||
|
|
@staticmethod
|
||
|
|
def hat(o):
|
||
|
|
return sympy.Matrix([[0, -o[2], o[1]],
|
||
|
|
[o[2], 0, -o[0]],
|
||
|
|
[-o[1], o[0], 0]])
|
||
|
|
|
||
|
|
"""vee-operator
|
||
|
|
|
||
|
|
It takes the 3x3-matrix representation ``Omega`` and maps it to the
|
||
|
|
corresponding vector representation of Lie algebra.
|
||
|
|
|
||
|
|
This is the inverse of the hat-operator, see above.
|
||
|
|
|
||
|
|
Precondition: ``Omega`` must have the following structure:
|
||
|
|
|
||
|
|
| 0 -c b |
|
||
|
|
| c 0 -a |
|
||
|
|
| -b a 0 |
|
||
|
|
"""
|
||
|
|
@staticmethod
|
||
|
|
def vee(Omega):
|
||
|
|
v = sophus.Vector3(Omega.row(2).col(1), Omega.row(0).col(2), Omega.row(1).col(0))
|
||
|
|
return v
|
||
|
|
|
||
|
|
def matrix(self):
|
||
|
|
""" returns matrix representation """
|
||
|
|
return sympy.Matrix([[
|
||
|
|
1 - 2 * self.q.vec[1]**2 - 2 * self.q.vec[2]**2,
|
||
|
|
2 * self.q.vec[0] * self.q.vec[1] -
|
||
|
|
2 * self.q.vec[2] * self.q[3],
|
||
|
|
2 * self.q.vec[0] * self.q.vec[2] +
|
||
|
|
2 * self.q.vec[1] * self.q[3]
|
||
|
|
], [
|
||
|
|
2 * self.q.vec[0] * self.q.vec[1] +
|
||
|
|
2 * self.q.vec[2] * self.q[3],
|
||
|
|
1 - 2 * self.q.vec[0]**2 - 2 * self.q.vec[2]**2,
|
||
|
|
2 * self.q.vec[1] * self.q.vec[2] -
|
||
|
|
2 * self.q.vec[0] * self.q[3]
|
||
|
|
], [
|
||
|
|
2 * self.q.vec[0] * self.q.vec[2] -
|
||
|
|
2 * self.q.vec[1] * self.q[3],
|
||
|
|
2 * self.q.vec[1] * self.q.vec[2] +
|
||
|
|
2 * self.q.vec[0] * self.q[3],
|
||
|
|
1 - 2 * self.q.vec[0]**2 - 2 * self.q.vec[1]**2
|
||
|
|
]])
|
||
|
|
|
||
|
|
def __mul__(self, right):
|
||
|
|
""" left-multiplication
|
||
|
|
either rotation concatenation or point-transform """
|
||
|
|
if isinstance(right, sympy.Matrix):
|
||
|
|
assert right.shape == (3, 1), right.shape
|
||
|
|
return (self.q * sophus.Quaternion(0, right) * self.q.conj()).vec
|
||
|
|
elif isinstance(right, So3):
|
||
|
|
return So3(self.q * right.q)
|
||
|
|
assert False, "unsupported type: {0}".format(type(right))
|
||
|
|
|
||
|
|
def __getitem__(self, key):
|
||
|
|
return self.q[key]
|
||
|
|
|
||
|
|
@staticmethod
|
||
|
|
def calc_Dx_exp_x(x):
|
||
|
|
return sympy.Matrix(4, 3, lambda r, c:
|
||
|
|
sympy.diff(So3.exp(x)[r], x[c]))
|
||
|
|
|
||
|
|
@staticmethod
|
||
|
|
def Dx_exp_x_at_0():
|
||
|
|
return sympy.Matrix([[0.5, 0.0, 0.0],
|
||
|
|
[0.0, 0.5, 0.0],
|
||
|
|
[0.0, 0.0, 0.5],
|
||
|
|
[0.0, 0.0, 0.0]])
|
||
|
|
|
||
|
|
@staticmethod
|
||
|
|
def calc_Dx_exp_x_at_0(x):
|
||
|
|
return So3.calc_Dx_exp_x(x).subs(x[0], 0).subs(x[1], 0).limit(x[2], 0)
|
||
|
|
|
||
|
|
def calc_Dx_this_mul_exp_x_at_0(self, x):
|
||
|
|
return sympy.Matrix(4, 3, lambda r, c:
|
||
|
|
sympy.diff((self * So3.exp(x))[r], x[c]))\
|
||
|
|
.subs(x[0], 0).subs(x[1], 0).limit(x[2], 0)
|
||
|
|
|
||
|
|
def calc_Dx_exp_x_mul_this_at_0(self, x):
|
||
|
|
return sympy.Matrix(3, 4, lambda r, c:
|
||
|
|
sympy.diff((self * So3.exp(x))[c], x[r, 0]))\
|
||
|
|
.subs(x[0], 0).subs(x[1], 0).limit(x[2], 0)
|
||
|
|
|
||
|
|
@staticmethod
|
||
|
|
def Dxi_x_matrix(x, i):
|
||
|
|
if i == 0:
|
||
|
|
return sympy.Matrix([[0, 2 * x[1], 2 * x[2]],
|
||
|
|
[2 * x[1], -4 * x[0], -2 * x[3]],
|
||
|
|
[2 * x[2], 2 * x[3], -4 * x[0]]])
|
||
|
|
if i == 1:
|
||
|
|
return sympy.Matrix([[-4 * x[1], 2 * x[0], 2 * x[3]],
|
||
|
|
[2 * x[0], 0, 2 * x[2]],
|
||
|
|
[-2 * x[3], 2 * x[2], -4 * x[1]]])
|
||
|
|
if i == 2:
|
||
|
|
return sympy.Matrix([[-4 * x[2], -2 * x[3], 2 * x[0]],
|
||
|
|
[2 * x[3], -4 * x[2], 2 * x[1]],
|
||
|
|
[2 * x[0], 2 * x[1], 0]])
|
||
|
|
if i == 3:
|
||
|
|
return sympy.Matrix([[0, -2 * x[2], 2 * x[1]],
|
||
|
|
[2 * x[2], 0, -2 * x[0]],
|
||
|
|
[-2 * x[1], 2 * x[0], 0]])
|
||
|
|
|
||
|
|
@staticmethod
|
||
|
|
def calc_Dxi_x_matrix(x, i):
|
||
|
|
return sympy.Matrix(3, 3, lambda r, c:
|
||
|
|
sympy.diff(x.matrix()[r, c], x[i]))
|
||
|
|
|
||
|
|
@staticmethod
|
||
|
|
def Dxi_exp_x_matrix(x, i):
|
||
|
|
R = So3.exp(x)
|
||
|
|
Dx_exp_x = So3.calc_Dx_exp_x(x)
|
||
|
|
l = [Dx_exp_x[j, i] * So3.Dxi_x_matrix(R, j) for j in [0, 1, 2, 3]]
|
||
|
|
return functools.reduce((lambda a, b: a + b), l)
|
||
|
|
|
||
|
|
@staticmethod
|
||
|
|
def calc_Dxi_exp_x_matrix(x, i):
|
||
|
|
return sympy.Matrix(3, 3, lambda r, c:
|
||
|
|
sympy.diff(So3.exp(x).matrix()[r, c], x[i]))
|
||
|
|
|
||
|
|
@staticmethod
|
||
|
|
def Dxi_exp_x_matrix_at_0(i):
|
||
|
|
v = sophus.ZeroVector3()
|
||
|
|
v[i] = 1
|
||
|
|
return So3.hat(v)
|
||
|
|
|
||
|
|
@staticmethod
|
||
|
|
def calc_Dxi_exp_x_matrix_at_0(x, i):
|
||
|
|
return sympy.Matrix(3, 3, lambda r, c:
|
||
|
|
sympy.diff(So3.exp(x).matrix()[r, c], x[i])
|
||
|
|
).subs(x[0], 0).subs(x[1], 0).limit(x[2], 0)
|
||
|
|
|
||
|
|
|
||
|
|
class TestSo3(unittest.TestCase):
|
||
|
|
def setUp(self):
|
||
|
|
omega0, omega1, omega2 = sympy.symbols(
|
||
|
|
'omega[0], omega[1], omega[2]', real=True)
|
||
|
|
x, v0, v1, v2 = sympy.symbols('q.w() q.x() q.y() q.z()', real=True)
|
||
|
|
p0, p1, p2 = sympy.symbols('p0 p1 p2', real=True)
|
||
|
|
v = sophus.Vector3(v0, v1, v2)
|
||
|
|
self.omega = sophus.Vector3(omega0, omega1, omega2)
|
||
|
|
self.a = So3(sophus.Quaternion(x, v))
|
||
|
|
self.p = sophus.Vector3(p0, p1, p2)
|
||
|
|
|
||
|
|
def test_exp_log(self):
|
||
|
|
for o in [sophus.Vector3(0., 1, 0.5),
|
||
|
|
sophus.Vector3(0.1, 0.1, 0.1),
|
||
|
|
sophus.Vector3(0.01, 0.2, 0.03)]:
|
||
|
|
w = So3.exp(o).log()
|
||
|
|
for i in range(0, 3):
|
||
|
|
self.assertAlmostEqual(o[i], w[i])
|
||
|
|
|
||
|
|
def test_matrix(self):
|
||
|
|
R_foo_bar = So3.exp(self.omega)
|
||
|
|
Rmat_foo_bar = R_foo_bar.matrix()
|
||
|
|
point_bar = self.p
|
||
|
|
p1_foo = R_foo_bar * point_bar
|
||
|
|
p2_foo = Rmat_foo_bar * point_bar
|
||
|
|
self.assertEqual(sympy.simplify(p1_foo - p2_foo),
|
||
|
|
sophus.ZeroVector3())
|
||
|
|
|
||
|
|
def test_derivatives(self):
|
||
|
|
self.assertEqual(sympy.simplify(So3.calc_Dx_exp_x_at_0(self.omega) -
|
||
|
|
So3.Dx_exp_x_at_0()),
|
||
|
|
sympy.Matrix.zeros(4, 3))
|
||
|
|
|
||
|
|
for i in [0, 1, 2, 3]:
|
||
|
|
self.assertEqual(sympy.simplify(So3.calc_Dxi_x_matrix(self.a, i) -
|
||
|
|
So3.Dxi_x_matrix(self.a, i)),
|
||
|
|
sympy.Matrix.zeros(3, 3))
|
||
|
|
for i in [0, 1, 2]:
|
||
|
|
self.assertEqual(sympy.simplify(
|
||
|
|
So3.Dxi_exp_x_matrix(self.omega, i) -
|
||
|
|
So3.calc_Dxi_exp_x_matrix(self.omega, i)),
|
||
|
|
sympy.Matrix.zeros(3, 3))
|
||
|
|
self.assertEqual(sympy.simplify(
|
||
|
|
So3.Dxi_exp_x_matrix_at_0(i) -
|
||
|
|
So3.calc_Dxi_exp_x_matrix_at_0(self.omega, i)),
|
||
|
|
sympy.Matrix.zeros(3, 3))
|
||
|
|
|
||
|
|
def test_codegen(self):
|
||
|
|
stream = sophus.cse_codegen(So3.calc_Dx_exp_x(self.omega))
|
||
|
|
filename = "cpp_gencode/So3_Dx_exp_x.cpp"
|
||
|
|
# set to true to generate codegen files
|
||
|
|
if False:
|
||
|
|
file = open(filename, "w")
|
||
|
|
for line in stream:
|
||
|
|
file.write(line)
|
||
|
|
file.close()
|
||
|
|
else:
|
||
|
|
file = open(filename, "r")
|
||
|
|
file_lines = file.readlines()
|
||
|
|
for i, line in enumerate(stream):
|
||
|
|
self.assertEqual(line, file_lines[i])
|
||
|
|
file.close()
|
||
|
|
stream.close
|
||
|
|
|
||
|
|
stream = sophus.cse_codegen(
|
||
|
|
self.a.calc_Dx_this_mul_exp_x_at_0(self.omega))
|
||
|
|
filename = "cpp_gencode/So3_Dx_this_mul_exp_x_at_0.cpp"
|
||
|
|
# set to true to generate codegen files
|
||
|
|
if False:
|
||
|
|
file = open(filename, "w")
|
||
|
|
for line in stream:
|
||
|
|
file.write(line)
|
||
|
|
file.close()
|
||
|
|
else:
|
||
|
|
file = open(filename, "r")
|
||
|
|
file_lines = file.readlines()
|
||
|
|
for i, line in enumerate(stream):
|
||
|
|
self.assertEqual(line, file_lines[i])
|
||
|
|
file.close()
|
||
|
|
stream.close
|
||
|
|
|
||
|
|
|
||
|
|
if __name__ == '__main__':
|
||
|
|
unittest.main()
|