TransformerNode.h

Go to the documentation of this file.

/*
 * Copyright (C) 2012 by
 *   MetraLabs GmbH (MLAB), GERMANY
 * and
 *   Neuroinformatics and Cognitive Robotics Labs (NICR) at TU Ilmenau, GERMANY
 * All rights reserved.
 *
 * Contact: info@mira-project.org
 *
 * Commercial Usage:
 *   Licensees holding valid commercial licenses may use this file in
 *   accordance with the commercial license agreement provided with the
 *   software or, alternatively, in accordance with the terms contained in
 *   a written agreement between you and MLAB or NICR.
 *
 * GNU General Public License Usage:
 *   Alternatively, this file may be used under the terms of the GNU
 *   General Public License version 3.0 as published by the Free Software
 *   Foundation and appearing in the file LICENSE.GPL3 included in the
 *   packaging of this file. Please review the following information to
 *   ensure the GNU General Public License version 3.0 requirements will be
 *   met: http://www.gnu.org/copyleft/gpl.html.
 *   Alternatively you may (at your option) use any later version of the GNU
 *   General Public License if such license has been publicly approved by
 *   MLAB and NICR (or its successors, if any).
 *
 * IN NO EVENT SHALL "MLAB" OR "NICR" BE LIABLE TO ANY PARTY FOR DIRECT,
 * INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES ARISING OUT OF
 * THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN IF "MLAB" OR
 * "NICR" HAS BEEN ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 * "MLAB" AND "NICR" SPECIFICALLY DISCLAIM ANY WARRANTIES, INCLUDING,
 * BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND
 * FITNESS FOR A PARTICULAR PURPOSE. THE SOFTWARE PROVIDED HEREUNDER IS
 * ON AN "AS IS" BASIS, AND "MLAB" AND "NICR" HAVE NO OBLIGATION TO
 * PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS OR MODIFICATIONS.
 */

#ifndef _MIRA_TRANSFORMERNODE_H_
#define _MIRA_TRANSFORMERNODE_H_

#include <string>

#ifndef Q_MOC_RUN
#include <boost/iterator/transform_iterator.hpp>
#endif

#include <utils/Stamped.h>
#include <utils/Time.h>

#include <utils/StampedDataQueue.h>
#include <utils/IteratorRangeContainer.h>

#include <transform/RigidTransform.h>

namespace mira {


// forward decls
class TransformerBase;
class NearestNeighborInterpolator;

class AbstractTransformerNode
{
public:

    typedef AbstractTransformerNode* AbstractNodePtr;

protected:
    AbstractTransformerNode(const std::string& id) :
        mID(id), mParent(NULL) {}

    virtual ~AbstractTransformerNode() {}

public:

    const std::string& getID() const { return mID; }

    const std::list<AbstractNodePtr>& getChildren() const { return mChildren; }

    const AbstractNodePtr& getParent() const { return mParent; }

    uint32 getDescendantCount() const;

    bool isAncestorOf(const AbstractNodePtr other) const;

public:

    virtual bool isEmpty() const = 0;

    template<typename Transform, typename Filter>
    Transform getTransform(const Time& time, Filter&& filter) {
        // #####################################################################
        // If you get a compiler error here, the class that was passed as
        // template parameter TNode does not implement the necessary
        // getTransform() method. Please implement that method in your transform
        // node class!
        // #####################################################################
        static_assert(sizeof(Transform)==0,
            "getTransform() must be implemented in your derived Node class");
        return Transform(); // to prevent compiler warning
    }

protected:
    friend class TransformerBase;

    std::string                mID;

    AbstractNodePtr            mParent;

    std::list<AbstractNodePtr> mChildren;
};



template <typename T, int D, template <typename T_, int D_> class TTransform>
class Transformer;

template <typename StorageTransform>
class TransformerNode : public AbstractTransformerNode
{
    typedef AbstractTransformerNode Base;

    typedef StampedDataQueue<StorageTransform> Storage;

protected:
    template <typename T, int D, template <typename T_, int D_> class TTransform_>
    friend class Transformer;

    TransformerNode(const std::string& id) : Base(id) {}

private:

    // the following two types are used in getTransform(...,Filter&&)
    // they extract the time(as uint64) and the transform out of a
    // Stamped<Transform>
    template <typename Transform>
    struct GetTime
    {
        GetTime(const Time& iT0) : t0(iT0) {}

        typedef int64 result_type;
        result_type operator()(const Stamped<Transform>& p) const {
            return (result_type)(p.timestamp-t0).totalMilliseconds();
        }

        Time t0;
    };

    template <typename Transform>
    struct GetTransform
    {
        typedef const Transform& result_type;
        result_type operator()(const Stamped<Transform>& p) const {
            return p;
        }
    };


public:

    Storage& storage() { return mTransformStorage; }

    void setTransform(const StorageTransform& transform, const Time& time) {
        mTransformStorage.insert(typename Storage::value_type(transform, time));
    }
    virtual bool isEmpty() const { return mTransformStorage.empty(); }

    template<typename Transform, typename Filter>
    Transform getTransform(const Time& time, Filter&& filter)
    {
        // get interval that is required by the filter
        auto its = mTransformStorage.getInterval(time,filter.samples(),
                                                 filter.samplesBefore(),
                                                 filter.samplesAfter());

        const Time t0 = its.first->timestamp;

        // using adapters, iterators and the IteratorRangeContainer, the
        // following code mimics two different containers: one that contains
        // the time as float and one that contains the transforms. In fact
        // there is one container only (mTransformStorage) that contains both
        // within a single container.
        typedef typename Storage::const_iterator const_iterator;
        typedef boost::transform_iterator<GetTime<Transform>, const_iterator> GetTimeIterator;
        GetTimeIterator begin1 = GetTimeIterator(its.first ,GetTime<Transform>(t0));
        GetTimeIterator end1   = GetTimeIterator(its.second,GetTime<Transform>(t0));

        typedef boost::transform_iterator<GetTransform<Transform>, const_iterator> GetTransformIterator;
        GetTransformIterator begin2 = GetTransformIterator(its.first ,GetTransform<Transform>());
        GetTransformIterator end2   = GetTransformIterator(its.second,GetTransform<Transform>());

        typedef mira::IteratorRangeContainer<GetTimeIterator> TimeContainer;
        typedef mira::IteratorRangeContainer<GetTransformIterator> TransformContainer;
        TimeContainer      timeContainer(begin1, end1);
        TransformContainer transformContainer(begin2, end2);

        // call the filter to obtain the filtered transform at the desired time
        return filter.template apply<float, Transform>(timeContainer,
                  transformContainer, (float)(time-t0).totalMilliseconds());
    }

    // specialize for NearestNeighborInterpolator and make a direct call
    // to getData for better performance
    template<typename Transform>
    Transform getTransform(const Time& time, NearestNeighborInterpolator&) {
        return mTransformStorage.getData(time);
    }

protected:

    Storage mTransformStorage;
};



} // namespace

#endif /* _MIRA_TRANSFORMERNODE_H_ */