InterpolatingTimeQueue.h

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * ArmarX is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation.
 *
 * ArmarX is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program. If not, see <http://www.gnu.org/licenses/>.
 *
 * @author     Fabian Reister ( fabian dot reister at kit dot edu )
 * @date       2021
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#pragma once
 
#include <algorithm>
#include <deque>
#include <iterator>
#include <shared_mutex>
#include <vector>
 
#include <Eigen/Core>
#include <Eigen/Geometry>
 
#include <SimoxUtility/math/pose/interpolate.h>
 
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
 
#include "TimeQueue.h"
 
namespace armarx::localization_and_mapping::cartographer_adapter
{
 
    struct PoseStamped
    {
        Eigen::Isometry3f pose;
        int64_t timestamp;
    };
 
    PoseStamped interpolate(const PoseStamped& posePre, const PoseStamped& poseNext, float t);
 
    template <typename MessageType>
    class InterpolatingTimeQueue : public TimeQueue<MessageType>
    {
    public:
        InterpolatingTimeQueue(const std::size_t maxQueueSize = 100) :
            TimeQueue<MessageType>(maxQueueSize)
        {
        }
 
        MessageType
        lookupInterpolate(int64_t timestamp) const
        {
            ARMARX_TRACE;
            std::shared_lock g{TimeQueue<MessageType>::mtx};
            // ARMARX_CHECK(this->_has(timestamp));
 
            const auto poseNextIt = TimeQueue<MessageType>::findFirstElementAtOrAfter(timestamp);
            ARMARX_CHECK(poseNextIt != TimeQueue<MessageType>::queue.cend())
                << "Cannot perform lookup of timestamp " << timestamp;
 
            // if element is the first in the queue -> cannot interpolate
            if (poseNextIt == TimeQueue<MessageType>::queue.cbegin())
            {
                return *poseNextIt;
            }
 
            const auto posePreIt = poseNextIt - 1;
 
            const auto& posePre = *posePreIt;
            const auto& poseNext = *poseNextIt;
 
            // the time fraction [0..1] of the lookup wrt to posePre and poseNext
            const float t = static_cast<float>(timestamp - posePre.timestamp) /
                            (poseNext.timestamp - posePre.timestamp);
 
            return interpolate(posePre, poseNext, t);
        }
 
        std::size_t
        size() const noexcept
        {
            std::shared_lock g{TimeQueue<MessageType>::mtx};
            return TimeQueue<MessageType>::queue.size();
        }
    };
 
} // namespace armarx::localization_and_mapping::cartographer_adapter