/
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
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
Bionic
More like this