ice_conversions.cpp

Go to the documentation of this file.

#include "ice_conversions.h"
 
#include <VirtualRobot/Robot.h>
#include <VirtualRobot/RobotConfig.h>
 
#include <SimoxUtility/algorithm/string.h>
#include <SimoxUtility/shapes/AxisAlignedBoundingBox.h>
#include <SimoxUtility/shapes/OrientedBox.h>
 
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
 
#include <RobotAPI/libraries/core/Pose.h>
#include <RobotAPI/libraries/core/FramedPose.h>
 
#include <RobotAPI/libraries/ArmarXObjects/ice_conversions.h>
#include <RobotAPI/libraries/ArmarXObjects/PoseManifoldGaussian.h>
 
 
namespace armarx
{
    std::ostream& data::operator<<(std::ostream& os, const ObjectID& id)
    {
        os << "'" << id.dataset << "/" << id.className;
        if (!id.instanceName.empty())
        {
            os << "/" << id.instanceName;
        }
        return os << "'";
    }
 
}
 
void armarx::fromIce(const data::ObjectID& ice, ObjectID& id)
{
    id = fromIce(ice);
}
 
armarx::ObjectID armarx::fromIce(const data::ObjectID& ice)
{
    return { ice.dataset, ice.className, ice.instanceName };
}
 
void armarx::toIce(data::ObjectID& ice, const ObjectID& id)
{
    ice.dataset = id.dataset();
    ice.className = id.className();
    ice.instanceName = id.instanceName();
}
 
armarx::data::ObjectID armarx::toIce(const ObjectID& id)
{
    data::ObjectID ice;
    toIce(ice, id);
    return ice;
}
 
void armarx::fromIce(const data::ObjectIDSeq& ice, std::vector<ObjectID>& ids)
{
    ids.clear();
    std::transform(ice.begin(), ice.end(), std::back_inserter(ids),
                   static_cast<ObjectID(*)(const data::ObjectID&)>(&fromIce));
}
 
std::vector<armarx::ObjectID> armarx::fromIce(const data::ObjectIDSeq& ice)
{
    std::vector<ObjectID> ids;
    fromIce(ice, ids);
    return ids;
}
 
void armarx::toIce(data::ObjectIDSeq& ice, const std::vector<ObjectID>& ids)
{
    ice.clear();
    std::transform(ids.begin(), ids.end(), std::back_inserter(ice),
                   static_cast<data::ObjectID(*)(const ObjectID&)>(&toIce));
}
 
armarx::data::ObjectIDSeq armarx::toIce(const std::vector<ObjectID>& ids)
{
    data::ObjectIDSeq ice;
    toIce(ice, ids);
    return ice;
}
 
namespace armarx
{
    const simox::meta::EnumNames<objpose::ObjectType> objpose::ObjectTypeNames =
    {
        { objpose::ObjectType::AnyObject, "AnyObject" },
        { objpose::ObjectType::KnownObject, "KnownObject" },
        { objpose::ObjectType::UnknownObject, "UnknownObject" }
    };
 
    objpose::AABB objpose::toIce(const simox::AxisAlignedBoundingBox& aabb)
    {
        objpose::AABB ice;
        ice.center = new Vector3(aabb.center());
        ice.extents = new Vector3(aabb.extents());
        return ice;
    }
 
    void objpose::fromIce(const Box& box, simox::OrientedBoxf& oobb)
    {
        try
        {
            Eigen::Vector3f pos = armarx::fromIce(box.position);
            Eigen::Matrix3f ori = armarx::fromIce(box.orientation).toRotationMatrix();
            Eigen::Vector3f extents = armarx::fromIce(box.extents);
            Eigen::Vector3f corner = pos - ori * extents / 2;
 
            oobb = simox::OrientedBox<float> (corner,
                                              ori.col(0) * extents(0),
                                              ori.col(1) * extents(1),
                                              ori.col(2) * extents(2));
        }
        catch (const armarx::LocalException&)
        {
            // No OOBB information.
            oobb = {};
        }
    }
    void objpose::fromIce(const BoxPtr& box, std::optional<simox::OrientedBox<float>>& oobb)
    {
        if (box)
        {
            oobb = fromIce(*box);
        }
        else
        {
            oobb = std::nullopt;
        }
    }
 
    simox::OrientedBoxf objpose::fromIce(const Box& box)
    {
        simox::OrientedBoxf oobb;
        fromIce(box, oobb);
        return oobb;
    }
 
    void objpose::toIce(Box& box, const simox::OrientedBoxf& oobb)
    {
        box.position = new Vector3(oobb.center());
        box.orientation = new Quaternion(oobb.rotation().eval());
        box.extents = new Vector3(oobb.dimensions());
    }
 
    void objpose::toIce(BoxPtr& box, const std::optional<simox::OrientedBox<float>>& oobb)
    {
        if (oobb)
        {
            box = new Box();
            toIce(*box, *oobb);
        }
    }
 
    objpose::Box objpose::toIce(const simox::OrientedBoxf& oobb)
    {
        objpose::Box box;
        toIce(box, oobb);
        return box;
    }
 
 
    void objpose::fromIce(const data::PoseManifoldGaussian& ice, PoseManifoldGaussian& cov)
    {
        // Only construct error message if necessary.
        if (static_cast<Eigen::Index>(ice.covariance6x6.size()) != cov.covariance.size())
        {
            ARMARX_CHECK_EQUAL(static_cast<Eigen::Index>(ice.covariance6x6.size()), cov.covariance.size())
                    << "Float sequence representing 6x6 covariance matrix must have " << cov.covariance.size()
                    << " values, but has " << ice.covariance6x6.size() << ": \n"
                    << "[" << simox::alg::join(simox::alg::multi_to_string(ice.covariance6x6), ", ") << "]"
                       ;
        }
 
        armarx::fromIce(ice.mean, cov.mean);
        cov.covariance = Eigen::MatrixXf::Map(ice.covariance6x6.data(),
                                              cov.covariance.rows(),
                                              cov.covariance.cols());
    }
 
    void objpose::fromIce(const data::PoseManifoldGaussianPtr& ice, std::optional<PoseManifoldGaussian>& cov)
    {
        if (ice)
        {
            cov = PoseManifoldGaussian();
            fromIce(*ice, cov.value());
        }
        else
        {
            cov = std::nullopt;
        }
    }
 
    std::optional<objpose::PoseManifoldGaussian> objpose::fromIce(const data::PoseManifoldGaussianPtr& ice)
    {
        std::optional<objpose::PoseManifoldGaussian> cov;
        fromIce(ice, cov);
        return cov;
    }
 
 
    void objpose::toIce(data::PoseManifoldGaussian& ice, const PoseManifoldGaussian& cov)
    {
        armarx::toIce(ice.mean, cov.mean);
 
        ice.covariance6x6.resize(cov.covariance.size());
        Eigen::MatrixXf::Map(ice.covariance6x6.data(),
                             cov.covariance.rows(),
                             cov.covariance.cols()) = cov.covariance;
    }
 
    void objpose::toIce(data::PoseManifoldGaussianPtr& ice, const std::optional<PoseManifoldGaussian>& cov)
    {
        if (cov.has_value())
        {
            ice = new data::PoseManifoldGaussian;
            toIce(*ice, cov.value());
        }
        else
        {
            ice = nullptr;
        }
    }
 
    objpose::data::PoseManifoldGaussianPtr objpose::toIce(const std::optional<PoseManifoldGaussian>& cov)
    {
        data::PoseManifoldGaussianPtr ice;
        toIce(ice, cov);
        return ice;
    }
 
}