Search code examples
eigeneigen3euler-angles

Two similar pose has a big relative euler angle

There are two similar poses represented by Euler angle:

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

But the relative Euler angle computed by Eigen is 

o1_euler_angle_o2:
 3.11182
-3.12907
-3.06258

The Euler convention is ZYX. The code I compute the o1_euler_angle_o2 is below:

#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 is below:

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

I use Euler angle since the Euler angle is three-dimension. It is easier to implement in genetic algorithm.

Is this relative pose the only result? If not, is there a result can reflect the small relative pose using Euler angle?

Solution

  • The result is entirely valid. But by the documentation of .eulerAngles() the range of returned angles is [0:pi]x[-pi:pi]x[-pi:pi]. In your case this is unfortunate, since there would be a smaller representation which however requires a negative value for the first angle.

    If you really need Euler-Angles, you can try out the unsupported Euler-Angles module of Eigen, which allows much more configuration of the output.

    But if you just need a good SO3->R^3 mapping I strongly suggest using the logarithmic map associated with the Lie-group (that is what Sophus::SO3::log should be calculating). Also I'll shamelessly plug a paper on that topic: "Integrating Generic Sensor Fusion Algorithms with Sound State Representations through Encapsulation of Manifolds" (shouldn't be hard to find online).