About

Explanations and code snippets illustrating how to do things in Sophus.

Construction

# Translation Only
Sophus::SE3f(Eigen::Matrix3f::Identity(), Eigen::Vector3f(-0.2f, 0.0f, +0.5f));
# Rotation Matrix
Eigen::Matrix3f R =(Eigen::AngleAxis<float> (static_cast<float> ( 0.0f/180.0f*3.141592f), Eigen::Vector3f::UnitZ ()) *   
                      Eigen::AngleAxis<float> (static_cast<float> ( 0.0f/180.0f*3.141592f), Eigen::Vector3f::UnitY ()) *
                      Eigen::AngleAxis<float> (static_cast<float> ( 0.0f/180.0f*3.141592f), Eigen::Vector3f::UnitX ()) ).matrix();
Sophus::SE3f T = Sophus::SE3f( R, Eigen::Vector3f(-0.2f, 0.0f, +0.5f) );
 
# Quaternion
Sophus::SE3f( Eigen::Quaternionf(0.970296,-0.000085, -0.241922,  0.000021 ), Eigen::Vector3f(-0.105954, 0.049990, -0.056345) );
 
# Two Vector Rotation + Translation
Eigen::Quaternionf orientation = Eigen::Quaternionf::FromTwoVectors(Eigen::Vector3f::UnitX(), new_x_axis);
Eigen::Vector3f translation = transform_pair.first.inverse().translation();
Sophus::SE3f pose(orientation, translation);

Angles

Angle of Rotation About the Rotational Axis

# pose is of type Sophus::SE3f
Eigen::Matrix<float,3,1> axis_angle = pose.so3().log();
double angle = axis_angle.norm();

Angular Difference

 float angular_difference_from_last = T_last.unit_quaternion().angularDistance( T_current.unit_quaternion() );