eigen_conversions.cpp

Go to the documentation of this file.

 
#include "eigen_conversions.h"
 
#include <Eigen/Geometry>
 
namespace armarx::localization_and_mapping::cartographer_adapter
{
 
    Eigen::Isometry3f
    toEigen(const ::cartographer::transform::Rigid3d& pose)
    {
        Eigen::Isometry3f eigenPose = Eigen::Isometry3f::Identity();
 
        eigenPose.translation() = pose.translation().cast<float>();
        eigenPose.linear() = pose.rotation().cast<float>().toRotationMatrix();
 
        return eigenPose;
    }
 
    ::cartographer::transform::Rigid3d
    fromEigen(const Eigen::Isometry3f& pose)
    {
        return ::cartographer::transform::Rigid3d(pose.translation().cast<double>(),
                                                  Eigen::AngleAxisf(pose.linear()).cast<double>());
    }
 
 
} // namespace armarx::localization_and_mapping::cartographer_adapter