#pragma once #include "../carto/transform/transform.h" using Rigid3f = cartographer::transform::Rigid3f; using Rigid3d = cartographer::transform::Rigid3d; inline void rigid3f_to_isometry3f(const Rigid3f &rigid_pos, Eigen::Isometry3f &iso_pos) { iso_pos.setIdentity(); iso_pos.matrix().block<3, 3>(0, 0) = rigid_pos.rotation().toRotationMatrix(); iso_pos.matrix().topRightCorner(3, 1) = rigid_pos.translation(); } inline void rigid3d_to_isometry3f(const Rigid3d& rigid_pos, Eigen::Isometry3f& iso_pos) { iso_pos.setIdentity(); iso_pos.matrix().block<3, 3>(0, 0) = rigid_pos.rotation().toRotationMatrix().cast<float>(); iso_pos.matrix().topRightCorner(3, 1) = rigid_pos.translation().cast<float>(); } inline void eigen3d_to_isometry3f(const Eigen::Quaterniond &q, const Eigen::Vector3d &t, Eigen::Isometry3f &iso_pos) { iso_pos.setIdentity(); iso_pos.matrix().block<3, 3>(0, 0) = q.toRotationMatrix().cast<float>(); iso_pos.matrix().topRightCorner(3, 1) = t.cast<float>(); }