使用SciPy解决se(3)优化问题?

w8f9ii69  于 2023-06-06  发布在  其他
关注(0)|答案(1)|浏览(188)

我想使用scipy.optimize包来解决一个刚体优化问题(寻找对象的最佳平移和旋转)。参数化这个问题的一种方法是通过李群理论和李代数se(3)(通常称为扭曲坐标)。这意味着状态向量由6个参数组成(即pose = [rho_x, rho_y, rho_z, theta_0, theta_1, theta_3]),通过所谓的“wedge”或“hat”算子和矩阵幂运算,可以从SE(3)变换矩阵(平移向量和旋转矩阵的组合)获得SE(3)变换矩阵。(我发现this cheat-sheet是一个很好的概述)
我的问题是scipy能否用于优化由扭曲坐标参数化的3D姿势?我在某个地方读到过这样的问题需要“流形优化”方法。那么我可以使用scipy的least_square()minimize(method="BFGS")吗?还是说这些只适用于平坦的欧氏空间
另外,如果有人知道scipy用于解决3D姿势优化问题的任何例子,这也会对我有很大帮助。过去几天我一直在寻找例子,但没有太大的成功。
这个问题是我在robotics stackexchange here上发布的一个更一般的问题的具体实现部分。

7dl7o3gd

7dl7o3gd1#

我假设你对李群和李代数有很好的理解。如果使用se(3)作为变量,通过计算损失将其转化为SE(3),则问题是无约束的,易于通过Scipy优化。那么,你的主要困难是什么?
要将se(3)(李代数)转换为SE(3)(李群),您也可以使用以下代码:

from scipy.spatial.transform import Rotation
import numpy as np

def skew(x:np.ndarray):
    return np.array([[0,-x[2],x[1]],
                     [x[2],0,-x[0]],
                     [-x[1],x[0],0]])
def computeV(rvec:np.ndarray):
    theta = np.linalg.norm(rvec)
    skew_rvec = skew(rvec)
    skew_rvec2 = skew_rvec * skew_rvec
    V = np.eye(3) + (1 - np.cos(theta))/theta**2 * skew(rvec) + (theta - np.sin(theta))/theta**3 * skew_rvec2
    return V
    
def toVec(SE3:np.ndarray):
    """SE3 Matrix to rvec and tvec

    Args:
        SE3 (np.ndarray): 4x4 `np.ndarray`

    Returns:
        rvec, tvec: `np.ndarray`
    """
    R = Rotation.from_matrix(SE3[:3,:3])
    rvec = R.as_rotvec()
    V = computeV(rvec)
    tvec = np.linalg.inv(V) @ SE3[:3,3]
    return rvec, tvec

def toMat(rvec:np.ndarray, tvec:np.ndarray):
    """rvec and tvec to SE3 Matrix

    Args:
        rvec (`np.ndarray`): 1x3 rotation vector\n
        tvec (`np.ndarray`): 1x3 translation vector
        
    Returns:
        SE3: 4x4 `np.ndarray`
    """
    R = Rotation.from_rotvec(rvec)
    V = computeV(rvec)
    mat = np.eye(4)
    mat[:3,:3] = R.as_matrix()
    mat[:3,3] = V @ tvec
    return mat

验证:

a = np.random.rand(6)
    print("se3: {}".format(a))
    SE3 = toMat(a[:3],a[3:])
    print("SE3:{}".format(SE3))
    print("rvec:{}, tvec:{}".format(*toVec(SE3)))

相关问题