eigen.cpp

Go to the documentation of this file.

#include "eigen.h"
 
#include <algorithm>
 
#include <SimoxUtility/math/convert/mat3f_to_rpy.h>
 
namespace armarx::navigation::conv
{
 
    std::vector<Eigen::Vector3f>
    to3D(const std::vector<Eigen::Vector2f>& v)
 
    {
        std::vector<Eigen::Vector3f> v3;
        v3.reserve(v.size());
 
        std::transform(v.begin(),
                       v.end(),
                       std::back_inserter(v3),
                       static_cast<Eigen::Vector3f (*)(const Eigen::Vector2f&)>(&to3D));
 
        return v3;
    }
 
    std::vector<Eigen::Vector2f>
    to2D(const std::vector<Eigen::Vector3f>& v)
 
    {
        std::vector<Eigen::Vector2f> v2;
        v2.reserve(v.size());
 
        std::transform(v.begin(),
                       v.end(),
                       std::back_inserter(v2),
                       static_cast<Eigen::Vector2f (*)(const Eigen::Vector3f&)>(&to2D));
 
        return v2;
    }
 
    core::Pose2D
    to2D(const core::Pose& p3)
    {
        core::Pose2D p2 = armarx::navigation::core::Pose2D::Identity();
        p2.translation() = p3.translation().head<2>();
 
        const auto rotatedVec = p3.rotation() * Eigen::Vector3f::UnitX();
        const float yaw = std::atan2(rotatedVec.y(), rotatedVec.x());
 
        p2.linear() = Eigen::Rotation2Df(yaw).toRotationMatrix();
 
        return p2;
    }
 
 
} // namespace armarx::navigation::conv