<commit_before>/*
 * TransformTree.cpp
 *
 *  Created on: Sep 21, 2015
 *      Author: aboeckmann
 */
#include "TransformGraph.hpp"
#include "TransformGraphExceptions.hpp"
#include <envire_core/events/TransformAddedEvent.hpp>
#include <envire_core/events/TransformRemovedEvent.hpp>
#include <envire_core/events/TransformModifiedEvent.hpp>
#include <envire_core/events/FrameAddedEvent.hpp>
#include <envire_core/events/FrameRemovedEvent.hpp>
#include <type_traits> //For is_same()

using namespace envire::core;
using namespace std;



TransformGraph::TransformGraph(envire::core::Environment const &environment) :
    LabeledTransformGraph (environment)
{
    static_assert(is_same<Base::graph_type::graph_type::graph_type::vertex_list_selector, boost::listS>::value,
                  "vertex list type should be listS to ensure that vertex_descriptors remain valid");
    static_assert(is_same<Base::graph_type::graph_type::graph_type::edge_list_selector, boost::listS>::value,
                  "edge list type should be listS to ensure that vertex_descriptors remain valid");
}

void TransformGraph::addFrame(const FrameId& frame)
{
    vertex_descriptor desc = vertex(frame);
    if(desc == null_vertex())
    {
        desc = add_vertex(frame);
    }
    else
    {
      throw FrameAlreadyExistsException(frame);
    }
}


void TransformGraph::addTransform(const FrameId& origin, const FrameId& target,
                                 const Transform& tf)
{
    //try to find vertices
    vertex_descriptor originDesc = vertex(origin);
    vertex_descriptor targetDesc = vertex(target);
    //if they don't exist create them
    if(originDesc == null_vertex())
    {
        originDesc = add_vertex(origin);
    }
    if(targetDesc == null_vertex())
    {
        targetDesc = add_vertex(target);
    }

    //check if there is an edge between the vertices.
    //If a->b exists, b->a also exist. Therefore we need to check only one direction
    EdgePair e = boost::edge(originDesc, targetDesc, *this);
    if(e.second)// edge already exists
    {
        throw TransformAlreadyExistsException(origin, target);
    }
    //we always add two edges, one for the transform and one for the inverse transform
    edge_descriptor originToTarget = add_edge(originDesc, targetDesc, tf, origin, target);

    Transform invTf = tf;
    invTf.setTransform(invTf.transform.inverse());
    edge_descriptor targetToOrigin = add_edge(targetDesc, originDesc, invTf, target, origin);
}


const Transform TransformGraph::getTransform(const vertex_descriptor originVertex,
                                             const vertex_descriptor targetVertex) const
{
    if(num_edges() == 0)
    {
        throw UnknownTransformException(getFrameId(originVertex), getFrameId(targetVertex));
    }
    //direct edges
    EdgePair pair;
    pair = boost::edge(originVertex, targetVertex, *this);
    if(!pair.second)
    {
        /** It is not a direct edge transformation **/
        Transform tf;
        envire::core::TransformGraphBFSVisitor <vertex_descriptor>visit(targetVertex, this->graph());

        // check whether the vertices exist in the graph
        // search algorithm result on segfault in case some of the vertices do not exist
        if (originVertex == null_vertex() || targetVertex == null_vertex())
        {
            throw UnknownTransformException(getFrameId(originVertex), getFrameId(targetVertex));
        }

        try
        {
            boost::breadth_first_search(this->graph(), originVertex, visitor(visit));

        }catch(const FoundFrameException &e)
        {
            //std::cout<< e.what() << std::endl;
            base::TransformWithCovariance &trans(tf.transform);

            /** Compute the transformation **/
            trans.setTransform (base::Affine3d::Identity());
            std::deque<vertex_descriptor>::iterator it = visit.tree->begin();
            for (; (it+1) != visit.tree->end(); ++it)
            {
                pair = boost::edge(*it, *(it+1), graph());
                trans = trans * (*this)[pair.first].transform.transform;
            }
        }

        if(tf.transform.hasValidTransform())
        {
            return tf;
        }
        else
        {
            throw UnknownTransformException(getFrameId(originVertex), getFrameId(targetVertex));
        }
    }

    return (*this)[pair.first].transform;
}

const Transform TransformGraph::getTransform(const FrameId& origin, const FrameId& target) const
{
    if(num_edges() == 0)
    {
        throw UnknownTransformException(origin, target);
    }
    const vertex_descriptor originVertex = getVertex(origin);//will throw
    const vertex_descriptor targetVertex = getVertex(target); //will throw   
    return getTransform(originVertex, targetVertex);
}

const Transform TransformGraph::getTransform(const vertex_descriptor originVertex,
                                             const vertex_descriptor targetVertex,
                                             const TreeView &view) const
{
    /* An identity transformation **/
    Transform tf(Eigen::Vector3d::Zero(), Eigen::Quaterniond::Identity());
    if (originVertex == targetVertex)
    {
        return tf;
    }

    base::TransformWithCovariance origin_tf(base::Affine3d::Identity()); // An identity transformation

    /** Get transformation from origin to the root **/
    vertex_descriptor od = originVertex;
    while(!view.isRoot(od))
    {
        EdgePair pair;
        pair = boost::edge(od, view.tree.at(od).parent, *this);
        if (pair.second)
        {
            origin_tf = origin_tf * (*this)[pair.first].transform.transform;
        }
        od = view.tree.at(od).parent;
    }

    base::TransformWithCovariance target_tf(base::Affine3d::Identity()); // An identity transformation

    /** Get transformation from target to the root **/
    vertex_descriptor td = targetVertex;
    while(!view.isRoot(td))
    {
        EdgePair pair;
        pair = boost::edge(td, view.tree.at(td).parent, *this);
        if (pair.second)
        {
            target_tf = target_tf * (*this)[pair.first].transform.transform;
        }
        td = view.tree.at(td).parent;
    }

    tf.transform = origin_tf * target_tf.inverse();

    return tf;
}

const Transform TransformGraph::getTransform(const FrameId& origin, const FrameId& target, const TreeView &view) const
{
    const vertex_descriptor originVertex = getVertex(origin);//will throw
    const vertex_descriptor targetVertex = getVertex(target); //will throw
    return getTransform(originVertex, targetVertex, view);
}

void TransformGraph::updateTransform(const FrameId& origin, const FrameId& target,
                                    const Transform& tf)
{
    const vertex_descriptor originVertex = getVertex(origin);//will throw
    const vertex_descriptor targetVertex = getVertex(target); //will throw
    return updateTransform(originVertex, targetVertex, tf);<commit_msg>TransformGraph: tiny improvement<commit_after>/*
 * TransformTree.cpp
 *
 *  Created on: Sep 21, 2015
 *      Author: aboeckmann
 */
#include "TransformGraph.hpp"
#include "TransformGraphExceptions.hpp"
#include <envire_core/events/TransformAddedEvent.hpp>
#include <envire_core/events/TransformRemovedEvent.hpp>
#include <envire_core/events/TransformModifiedEvent.hpp>
#include <envire_core/events/FrameAddedEvent.hpp>
#include <envire_core/events/FrameRemovedEvent.hpp>
#include <type_traits> //For is_same()

using namespace envire::core;
using namespace std;



TransformGraph::TransformGraph(envire::core::Environment const &environment) :
    LabeledTransformGraph (environment)
{
    static_assert(is_same<Base::graph_type::graph_type::graph_type::vertex_list_selector, boost::listS>::value,
                  "vertex list type should be listS to ensure that vertex_descriptors remain valid");
    static_assert(is_same<Base::graph_type::graph_type::graph_type::edge_list_selector, boost::listS>::value,
                  "edge list type should be listS to ensure that vertex_descriptors remain valid");
}

void TransformGraph::addFrame(const FrameId& frame)
{
    vertex_descriptor desc = vertex(frame);
    if(desc == null_vertex())
    {
        desc = add_vertex(frame);
    }
    else
    {
      throw FrameAlreadyExistsException(frame);
    }
}


void TransformGraph::addTransform(const FrameId& origin, const FrameId& target,
                                 const Transform& tf)
{
    //try to find vertices
    vertex_descriptor originDesc = vertex(origin);
    vertex_descriptor targetDesc = vertex(target);
    //if they don't exist create them
    if(originDesc == null_vertex())
    {
        originDesc = add_vertex(origin);
    }
    if(targetDesc == null_vertex())
    {
        targetDesc = add_vertex(target);
    }

    //check if there is an edge between the vertices.
    //If a->b exists, b->a also exist. Therefore we need to check only one direction
    EdgePair e = boost::edge(originDesc, targetDesc, *this);
    if(e.second)// edge already exists
    {
        throw TransformAlreadyExistsException(origin, target);
    }
    //we always add two edges, one for the transform and one for the inverse transform
    edge_descriptor originToTarget = add_edge(originDesc, targetDesc, tf, origin, target);

    Transform invTf = tf;
    invTf.setTransform(invTf.transform.inverse());
    edge_descriptor targetToOrigin = add_edge(targetDesc, originDesc, invTf, target, origin);
}


const Transform TransformGraph::getTransform(const vertex_descriptor originVertex,
                                             const vertex_descriptor targetVertex) const
{
    if(num_edges() == 0)
    {
        throw UnknownTransformException(getFrameId(originVertex), getFrameId(targetVertex));
    }
    //direct edges
    EdgePair pair;
    pair = boost::edge(originVertex, targetVertex, *this);
    if(!pair.second)
    {
        /** It is not a direct edge transformation **/
        Transform tf;
        envire::core::TransformGraphBFSVisitor <vertex_descriptor>visit(targetVertex, this->graph());

        // check whether the vertices exist in the graph
        // search algorithm result on segfault in case some of the vertices do not exist
        if (originVertex == null_vertex() || targetVertex == null_vertex())
        {
            throw UnknownTransformException(getFrameId(originVertex), getFrameId(targetVertex));
        }

        try
        {
            boost::breadth_first_search(this->graph(), originVertex, visitor(visit));

        }catch(const FoundFrameException &e)
        {
            //std::cout<< e.what() << std::endl;
            base::TransformWithCovariance &trans(tf.transform);

            /** Compute the transformation **/
            trans.setTransform (base::Affine3d::Identity());
            std::deque<vertex_descriptor>::iterator it = visit.tree->begin();
            for (; (it+1) != visit.tree->end(); ++it)
            {
                pair = boost::edge(*it, *(it+1), graph());
                trans = trans * (*this)[pair.first].transform.transform;
            }
        }

        if(tf.transform.hasValidTransform())
        {
            return tf;
        }
        else
        {
            throw UnknownTransformException(getFrameId(originVertex), getFrameId(targetVertex));
        }
    }

    return (*this)[pair.first].transform;
}

const Transform TransformGraph::getTransform(const FrameId& origin, const FrameId& target) const
{
    if(num_edges() == 0)
    {
        throw UnknownTransformException(origin, target);
    }
    const vertex_descriptor originVertex = getVertex(origin);//will throw
    const vertex_descriptor targetVertex = getVertex(target); //will throw   
    return getTransform(originVertex, targetVertex);
}

const Transform TransformGraph::getTransform(const vertex_descriptor originVertex,
                                             const vertex_descriptor targetVertex,
                                             const TreeView &view) const
{
    if (originVertex == targetVertex)
    {
        /* An identity transformation **/
        return Transform(Eigen::Vector3d::Zero(), Eigen::Quaterniond::Identity());
    }

    base::TransformWithCovariance origin_tf(base::Affine3d::Identity()); // An identity transformation

    /** Get transformation from origin to the root **/
    vertex_descriptor od = originVertex;
    while(!view.isRoot(od))
    {
        EdgePair pair(boost::edge(od, view.tree.at(od).parent, *this));
        if (pair.second)
        {
            origin_tf = origin_tf * (*this)[pair.first].transform.transform;
        }
        od = view.tree.at(od).parent;
    }

    base::TransformWithCovariance target_tf(base::Affine3d::Identity()); // An identity transformation

    /** Get transformation from target to the root **/
    vertex_descriptor td = targetVertex;
    while(!view.isRoot(td))
    {
        EdgePair pair;
        pair = boost::edge(td, view.tree.at(td).parent, *this);
        if (pair.second)
        {
            target_tf = target_tf * (*this)[pair.first].transform.transform;
        }
        td = view.tree.at(td).parent;
    }

    return origin_tf * target_tf.inverse();
}

const Transform TransformGraph::getTransform(const FrameId& origin, const FrameId& target, const TreeView &view) const
{
    const vertex_descriptor originVertex = getVertex(origin);//will throw
    const vertex_descriptor targetVertex = getVertex(target); //will throw
    return getTransform(originVertex, targetVertex, view);
}

void TransformGraph::updateTransform(const FrameId& origin, const FrameId& target,
                                    const Transform& tf)
{
    const vertex_descriptor originVertex = getVertex(origin);//will throw
    const vertex_descriptor targetVertex = getVertex(target); //will throw
    return updateTransform(originVertex, targetVertex, tf);
{'fraction_non_alphanumeric': 0.10872541249369551, 'fraction_numerical': 0.002017436414727286, 'mean_word_length': 2.9532896610652237, 'pattern_counts': {'":': 0, '<': 31, '<?xml version=': 0, '>': 35, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 16, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '4796290', 'n_tokens_mistral': 3795, 'n_tokens_neox': 3667, 'n_words': 901}