Question

欧拉角有两个相似的姿势：

s_euler_angle_o1:
0.000549608
    -3.1334
    1.23193
s_euler_angle_o2:
0.0222646
 -3.10948
  1.31032

但是Eigen计算出的相对欧拉角是

o1_euler_angle_o2:
 3.11182
-3.12907
-3.06258

Euler约定为ZYX。我计算o1_euler_angle_o2的代码如下：

#include <iostream>
#include <vector>
#include "sophus/se3.h"
#include "sophus/so3.h"
#include <Eigen/Core>
Sophus::SE3 EulerTranslatetoSE3(Eigen::Vector3d euler_angle, Eigen::Vector3d translate){
        Eigen::Matrix3d o1_R_on = (Eigen::AngleAxisd(euler_angle[0],Eigen::Vector3d::UnitZ())*Eigen::AngleAxisd(euler_angle[1],Eigen::Vector3d::UnitY())*Eigen::AngleAxisd(euler_angle[2],Eigen::Vector3d::UnitX())).matrix();
        Sophus::SE3 o1_T_on_sop(o1_R_on,translate);
        return o1_T_on_sop;

    }
int main() {
    Eigen::Vector3d s_euler_angle_o1 = Eigen::Vector3d(0.000549608,-3.1334,1.23193);
    Eigen::Vector3d s_euler_angle_o2 = Eigen::Vector3d(0.0222646,-3.10948,1.31032);
    Eigen::Vector3d s_t_o1 = Eigen::Vector3d(0.0148442,0.0453362,0.342611);
    Eigen::Vector3d s_t_o2 = Eigen::Vector3d(0.0213847,0.0488918,0.341841);

    Sophus::SE3 s_T_o1 = EulerTranslatetoSE3(s_euler_angle_o1,s_t_o1);
    Sophus::SE3 s_T_o2 = EulerTranslatetoSE3(s_euler_angle_o2,s_t_o2);
    Sophus::SE3 o1_T_o2 = s_T_o1.inverse() * s_T_o2;
    Eigen::Matrix3d o1_R_o2 = o1_T_o2.rotation_matrix();
    Eigen::Vector3d o1_euler_angle_o2 = o1_R_o2.eulerAngles(2,1,0);
    std::cout<<"o1_euler_angle_o2:\n"<<o1_euler_angle_o2<<std::endl;
    return 0;
}

CMakelist.txt如下：

cmake_minimum_required(VERSION 3.14)
project(test)

set(CMAKE_CXX_STANDARD 14)
#sophus
find_package(Sophus REQUIRED)
include_directories(${Sophus_INCLUDE_DIRS})
add_executable(test main.cpp)
# Eigen
include_directories("/usr/include/eigen3" )
target_link_libraries(test PRIVATE
        ${DEPENDENCIES}
        ${Sophus_LIBRARIES}
        )

我使用欧拉角，因为欧拉角是三维的。遗传算法更容易实现。

这个相对姿势是唯一的结果吗？如果不是，使用欧拉角是否可以反映较小的相对姿态？

Answer 1

结果完全有效。但是通过documentation of .eulerAngles()，返回角度的范围为[0:pi]x[-pi:pi]x[-pi:pi]。在您的情况下，这很不幸，因为会有较小的表示形式，但是对于第一个角度它需要一个负值。

如果您确实需要Euler-Angles，可以尝试使用Eigen的unsupported Euler-Angles module，它可以对输出进行更多配置。

但是，如果您只需要一个良好的SO3-> R ^ 3映射，我强烈建议您使用与Lie-group相关联的对数映射（这就是Sophus::SO3::log应该计算的）。我还将毫不客气地插入有关该主题的论文：“通过歧管封装将通用传感器融合算法与声音状态表示相集成”（在网上很难找到）。

两个相似的姿势具有相对的欧拉角

1 个答案: