sophas中exp()方法的源码分析引自so3.py

最新推荐文章于 2023-05-22 20:21:54 发布

火星种萝卜

最新推荐文章于 2023-05-22 20:21:54 发布

阅读量686

点赞数

分类专栏： SLAM 文章标签：计算机视觉 c++

本文链接：https://blog.csdn.net/studyvcmfc/article/details/124050281

版权

SLAM 专栏收录该内容

46 篇文章 3 订阅

订阅专栏

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)

@staticmethod
def hat(o):
return sympy.Matrix([[0, -o[2], o[1]],
[o[2], 0, -o[0]],
[-o[1], o[0], 0]])

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()

sophas中exp()方法的源码分析引自so3.py

exp()方法的执行过程