/** * @brief transform matrix which represents the pose transform as a 4x4 * homogenous matrix * * @result 4x4 homogenous transform matrix */ Eigen::Affine3d toTransform() const { Eigen::Affine3d t; t = orientation; t.pretranslate( position ); return t; }