/
Sophus

Sophus



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() );


Related content

Rclcpp
More like this
Evodesk
Evodesk
More like this
Drake Systems
Drake Systems
More like this
Contact Details
Contact Details
More like this
Memory Management
Memory Management
More like this
Bionic
More like this