Transformer.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_TRANSFORMER_H_
#define _MIRA_TRANSFORMER_H_

#include <string>
#include <list>
#include <vector>

#ifndef Q_MOC_RUN
#include <boost/thread/mutex.hpp>
#endif

#include <error/Exceptions.h>

#include <utils/Foreach.h>

#include <transform/TransformerNode.h>
#include <transform/RigidTransform.h>

#include <math/NearestNeighborInterpolator.h>

namespace mira {


class MIRA_BASE_EXPORT TransformerBase
{
public:

    static const int MAX_TREE_DEPTH = 1000;

public:

    typedef AbstractTransformerNode::AbstractNodePtr AbstractNodePtr;

public:

    class Chain
    {
    public:
        std::list<AbstractNodePtr> inverse;
        std::list<AbstractNodePtr> forward;

        bool empty() const { return inverse.empty() && forward.empty(); }
    };


public:

    TransformerBase();

    ~TransformerBase();

public:

    bool addLink(AbstractNodePtr child, AbstractNodePtr parent);

    void removeLink(AbstractNodePtr child, AbstractNodePtr parent);

public:

    std::list<AbstractNodePtr> getNodes();

    std::list<std::pair<AbstractNodePtr, AbstractNodePtr>> getLinks();

    const std::list<AbstractNodePtr> getRootNodes();

public:

    void getTransformChain(AbstractNodePtr target, AbstractNodePtr source,
                           Chain& oChain);

    bool isTransformAvailable(AbstractNodePtr target, AbstractNodePtr source);

public:

    MIRA_DEFINE_SERIALIZABLE_EXCEPTION(XTransform, XRuntime)

protected:

    void addNode(AbstractNodePtr node);

    AbstractNodePtr getNode(const std::string& nodeID);


private:

    AbstractNodePtr collectNodesUpwardsToRoot(AbstractNodePtr node,
                                              std::list<AbstractNodePtr>& oNodes);

protected:


    typedef std::map<std::string, AbstractNodePtr> IDToNodeMap;

    IDToNodeMap mNodes;

    boost::mutex mMutex;
};


struct TransformDesc
{
    TransformDesc(const TransformerBase::Chain& c1) : chain1(c1) {}

    TransformDesc(const TransformerBase::Chain& c1,
                  const TransformerBase::Chain& c2) : chain1(c1), chain2(c2) {}

    TransformerBase::Chain chain1;
    TransformerBase::Chain chain2;
};


template <typename TNode>
class GenericTransformer : public TransformerBase
{
    typedef TransformerBase Base;

public:

    typedef TNode Node;

    typedef Node*       NodePtr;

    typedef TransformerBase::AbstractNodePtr AbstractNodePtr;

public:

    TransformDesc prepareTransform(NodePtr target, NodePtr source)
    {
        Chain chain;
        getTransformChain(target, source, chain);
        return TransformDesc(chain);
    }

    TransformDesc prepareTransform(NodePtr target, NodePtr source, NodePtr fixed)
    {
        Chain chain1, chain2;
        getTransformChain(target, fixed, chain1);
        getTransformChain(fixed, source, chain2);
        return TransformDesc(chain1, chain2);
    }

    template<typename Transform, typename Filter>
    Transform getTransform(const TransformDesc& desc,
                           const Time& targetTime, const Time& sourceTime,
                           Filter&& filter)
    {
        if(desc.chain2.empty())
            return getTransform<Transform>(desc.chain1, targetTime, filter);
        else
            return getTransform<Transform>(desc.chain2, sourceTime, filter) *
                   getTransform<Transform>(desc.chain1, targetTime, filter);
    }

    template<typename Transform>
    Transform getTransform(const TransformDesc& desc,
                           const Time& targetTime, const Time& sourceTime)
    {
        return getTransform<Transform>(desc, targetTime, sourceTime,
                                       NearestNeighborInterpolator());
    }


    template<typename Transform, typename Filter>
    Transform getTransform(const TransformDesc& desc, const Time& time,
                           Filter&& filter)
    {
        return getTransform<Transform>(desc, time, time, filter);
    }

    template<typename Transform>
    Transform getTransform(const TransformDesc& desc, const Time& time)
    {
        return getTransform<Transform>(desc, time,
                                       NearestNeighborInterpolator());
    }

    template<typename Transform, typename Filter>
    Transform getTransform(NodePtr target, NodePtr source, const Time& time,
                           Filter&& filter)
    {
        TransformDesc desc = prepareTransform(target, source);
        return getTransform<Transform>(desc, time, filter);
    }

    template<typename Transform>
    Transform getTransform(NodePtr target, NodePtr source, const Time& time)
    {
        return getTransform<Transform>(target, source, time,
                                       NearestNeighborInterpolator());
    }

    template<typename Transform, typename Filter>
    Transform getTransform(NodePtr target, const Time& targetTime,
                           NodePtr source, const Time& sourceTime,
                           NodePtr fixed, Filter&& filter)
    {
        TransformDesc desc = prepareTransform(target, source, fixed);
        return getTransform<Transform>(desc, targetTime, sourceTime, filter);
    }

    template<typename Transform>
    Transform getTransform(NodePtr target, const Time& targetTime,
                           NodePtr source, const Time& sourceTime,
                           NodePtr fixed)
    {
        return getTransform<Transform>(target, targetTime,
                                       source, sourceTime, fixed,
                                       NearestNeighborInterpolator());
    }

public:

    template<typename Transform, typename Filter>
    Transform inferTransform(NodePtr node, NodePtr target, NodePtr source,
                             const Transform& transform,
                             const Time& time, Filter&& filter)
    {
        /*
         CASE 1:
          p ---- x ---- source
          |               .
          | ??            .
          |               .
          v               v
         node --- y --- target
         */

        // path from node to target
        TransformDesc node_target = prepareTransform(target,node);

        // path from node to source
        TransformDesc node_source = prepareTransform(source,node);

        // if the way from node to target goes in reverse direction towards the
        // parent of "node" (this is the case if "node" is the first one in the
        // inverse list), then we have case 2 here
        if(!node_target.chain1.inverse.empty() &&
            node_target.chain1.inverse.front()==node) {
            /*
             CASE 2: (the way to target would be node->p->x->target,
                      via the unknown link (p->node))
              p ---- x ---- target
              |               ^
              | ??            .
              |               .
              v               .
             node --- y --- source
             */

            // make sure, that the way to source does not go via the unknown
            // link (p->node), too. In this case we have a degenerate case and
            // the user has provided invalid nodes
            if(!node_source.chain1.inverse.empty() &&
                node_source.chain1.inverse.front()==node) {
                MIRA_THROW(TransformerBase::XTransform,
                    "Cannot infer the transformation since all paths from '" <<
                    node->getID() << "' to source '" << source->getID() << "' "
                    "or target '" << target->getID() << "' go via "
                    "the link that should be inferred.");
            }

            Transform y = getTransform<Transform>(node_source, time, filter);

            // remove node from node->target path to get p->target path
            assert(!node_target.chain1.inverse.empty());
            assert(node_target.chain1.inverse.front()==node);
            node_target.chain1.inverse.pop_front();

            Transform x = getTransform<Transform>(node_target, time, filter);

            return x*transform.inverse()*y.inverse();

        } else {
            /*
             CASE 1:
             p ---- x ---- source
              |               .
              | ??            .
              |               .
              v               v
             node --- y --- target
             */


            Transform y = getTransform<Transform>(node_target, time, filter);

            // remove node from node->source path to get p->source path
            assert(!node_source.chain1.inverse.empty());
            assert(node_source.chain1.inverse.front()==node);
            node_source.chain1.inverse.pop_front();

            Transform x = getTransform<Transform>(node_source, time, filter);

            return x * transform * y.inverse();
        }
    }

    template<typename Transform>
    Transform inferTransform(NodePtr node, NodePtr target, NodePtr source,
                             const Transform& transform, const Time& time)
    {
        return inferTransform<Transform>(node, target, source, transform, time,
                                         NearestNeighborInterpolator());
    }

public:

    NodePtr getNode(const std::string& nodeID) {
        return castNode(Base::getNode(nodeID));
    }

protected:

    static NodePtr castNode(AbstractNodePtr node) {
        return static_cast<Node*>(node);
    }

    template<typename Transform, typename Filter>
    Transform getTransform(const Chain& chain, const Time& time, Filter&& filter)
    {
        Transform finalTransform; // start with identity transform

        // now concatenate the inverse transforms
        foreach(AbstractNodePtr n, chain.inverse)
            finalTransform*=castNode(n)->template getTransform<Transform>(time, filter).inverse();

        // concatenate the forward transforms (in reverse order because we chained them in reverse order)
        foreach_reverse(AbstractNodePtr n, chain.forward)
            finalTransform*=castNode(n)->template getTransform<Transform>(time, filter);

        return finalTransform;
    }

};


template <typename T, int D, template <typename T_, int D_> class TTransform = RigidTransform>
class Transformer : public GenericTransformer<TransformerNode<TTransform<T, D>>>
{
    typedef GenericTransformer<TransformerNode<TTransform<T, D>>> Base;
public:

    typedef TransformerNode<TTransform<T, D> > Node;

    typedef Node* NodePtr;

public:

    NodePtr newNode(const std::string& nodeID) {
        NodePtr node(new Node(nodeID));
        this->addNode(node);
        return node;
    }
};


typedef Transformer<float, 2> Transformer2f;
typedef Transformer<float, 3> Transformer3f;

typedef Transformer<double, 2> Transformer2d;
typedef Transformer<double, 3> Transformer3d;


typedef Transformer<float, 2, RigidTransformCov> TransformerCov2f;
typedef Transformer<float, 3, RigidTransformCov> TransformerCov3f;

typedef Transformer<double, 2, RigidTransformCov> TransformerCov2d;
typedef Transformer<double, 3, RigidTransformCov> TransformerCov3d;


} // namespace

#endif