rigid3.h 1014 Bytes
Newer Older
wangdawei's avatar
wangdawei committed
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26
#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>();
}