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

最新推荐文章于 2024-01-27 09:46:06 发布

原创

最新推荐文章于 2024-01-27 09:46:06 发布 · 809 阅读

0 ·

CC 4.0 BY-SA版权

文章标签：

#计算机视觉 #c++

这个代码实现了3维旋转群So3，通过单位四元数表示，并提供了exp和log映射、矩阵表示、乘法及导数计算等方法。测试用例验证了exp和log的逆运算性质，矩阵表示的正确性，以及导数计算的准确性。代码还包含了代码生成的辅助功能。

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] +