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

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

欧拉约定是 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]。在你的情况下，这是不幸的，因为会有一个较小的表示，但是需要第一个角度的负值。

如果你真的需要欧拉角，你可以试试 Eigen 的 unsupported Euler-Angles module，它允许更多的输出配置。

但是如果你只需要一个好的 SO3->R^3 映射，我强烈建议使用与李群相关的对数映射（Sophus::SO3::log 应该计算）。另外，我会无耻地插入一篇关于该主题的论文："Integrating Generic Sensor Fusion Algorithms with Sound State Representations through Encapsulation of Manifolds"（在网上应该不难找到）。

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

Two similar pose has a big relative euler angle

eigen

eigen3

euler-angles