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