Rigid3D typically stores these as a unit quaternion (for rotation) and a 3-vector (for translation). For this tutorial, we'll use the Sophus::SE3d (C++) or scipy.spatial.transform.Rotation (Python). If you're working with ROS, tf2::Transform is analogous. C++ (Eigen + Sophus) #include <sophus/se3.hpp> #include <Eigen/Core> #include <iostream> using Sophus::SE3d; using Eigen::Vector3d; using Eigen::Quaterniond; Python (NumPy + SciPy or transforms3d) import numpy as np from scipy.spatial.transform import Rotation as R # Or use `from transforms3d.quaternions import quat2mat` 3. Creating a Rigid3D Object Let’s create a transformation that represents: rotate 90° about Z-axis, then translate by (1, 0, 0).
T_ba = np.linalg.inv(T_ab) # For rigid transforms, this is more efficient: R_inv = T_ab[:3,:3].T t_inv = -R_inv @ T_ab[:3,3] C++: rigid3d tutorial
p_a = np.array([0, 1, 0]) p_b = T[:3,:3] @ p_a + T[:3,3] print(p_b) # [0., 0., 0.] If you have ( T_bc ) and ( T_ab ), the transform from ( a ) to ( c ) is: Rigid3D typically stores these as a unit quaternion