InterpolatingTimeQueue.cpp

Go to the documentation of this file.

#include "InterpolatingTimeQueue.h"
 
#include "ArmarXCore/core/exceptions/local/ExpressionException.h"
#include "ArmarXCore/core/logging/Logging.h"
#include "ArmarXCore/util/CPPUtility/trace.h"
 
namespace armarx::localization_and_mapping::cartographer_adapter
{
 
    // TODO(fabian.reister): remove and use simox
    Eigen::Isometry3f
    interpolatePose2(const Eigen::Isometry3f& posePre, const Eigen::Isometry3f& poseNext, float t)
    {
        ARMARX_TRACE;
        t = std::clamp(t, 0.F, 1.F);
 
        Eigen::Isometry3f pose = Eigen::Isometry3f::Identity();
 
        pose.translation() = (1. - t) * posePre.translation() + t * poseNext.translation();
 
        const Eigen::Quaternionf rotPrev(posePre.linear().matrix());
        const Eigen::Quaternionf rotNext(poseNext.linear().matrix());
 
        const Eigen::Quaternionf rotNew = rotPrev.slerp(t, rotNext);
 
        pose.linear() = rotNew.toRotationMatrix();
 
        return pose;
    }
 
    PoseStamped
    interpolate(const PoseStamped& posePre, const PoseStamped& poseNext, float t)
    {
        ARMARX_TRACE;
        t = std::clamp(t, 0.F, 1.F);
 
        PoseStamped ps;
        ps.timestamp = posePre.timestamp + (poseNext.timestamp - posePre.timestamp) * t;
 
        ps.pose = interpolatePose2(posePre.pose, poseNext.pose, t);
 
        return ps;
    }
 
 
} // namespace armarx::localization_and_mapping::cartographer_adapter