borderaccumulator.h

Go to the documentation of this file.

/********************************************************************************
 * Copyright (C) 2017-2020 German Aerospace Center (DLR). 
 * Eclipse ADORe, Automated Driving Open Research https://eclipse.org/adore
 *
 * This program and the accompanying materials are made available under the 
 * terms of the Eclipse Public License 2.0 which is available at
 * http://www.eclipse.org/legal/epl-2.0.
 *
 * SPDX-License-Identifier: EPL-2.0 
 *
 * Contributors: 
 *   Daniel Heß - initial API and implementation
 *   Robert Markowski - BASFollowNavigation
 ********************************************************************************/
 
#pragma once
 
#include <adore/env/borderbased/borderset.h>
#include <adore/env/borderbased/bordercostmap.h>
#include <adore/env/borderbased/bordertrace.h>
#include <adore/mad/llinearpiecewisefunction.h>
#include <adore/mad/csvlog.h>
#include <unordered_map>
#include <list>
#include <vector>
#include <set>
 
namespace adore
{
    namespace env
    {
        namespace BorderBased
        {
 
            typedef std::vector<Border*> BAContainer;
            //typedef std::unordered_map<adore::env::BorderBased::BorderID, double, adore::env::BorderBased::BorderIDHasher> TBorderID2Cost;
            //typedef std::list<Border*> BAContainer;
 
            class BorderAccumulationStrategy
            {
            protected:
                BorderSet* m_borderSet; 
            public:
                BorderAccumulationStrategy()
                {
                }
                virtual void getNextBorder(Border*& border,bool& inverted)=0;
                bool borderValid(Border* b)
                {
                    return m_borderSet->borderTypeValid(b);
                }
            };
 
 
 
            class BASFollowStraight:public BorderAccumulationStrategy
            {
            private:
                Border* m_current; 
                double m_distance; 
            public:
                BASFollowStraight(Border* border,BorderSet* borderSet,double max_distance)
                {
                    m_current = border;
                    m_borderSet = borderSet;
                    m_distance = max_distance;
                }
                virtual void getNextBorder(Border*& border,bool& inverted) override
                {
                    if(m_current!=0 && m_distance>0)
                    {
                        double continuity = 1.0;
                        Border* successor = 0;
                        border = m_current;
 
                        for(auto it = m_borderSet->getSuccessors(m_current);it.current()!=it.end();it.current()++)
                        {
                            Border* next = it.current()->second;
                            if(!borderValid(next)) {continue;}
                            Border* nextLeft = m_borderSet->getLeftNeighbor(next);
                            if(nextLeft!=0 && m_current->m_left!=0 
                                && m_borderSet->isConnected(*(m_current->m_left),nextLeft->m_id)
                                && !(m_current->m_id==next->m_id))
                            {
                                double r = m_current->getContinuityRating(next);
                                if(successor==0 || r<continuity)
                                {
                                    successor = next;
                                    continuity = r;
                                }
                            }
                        }
                        m_distance -= m_current->getLength();
                        m_current = successor;
                        inverted = false;
                    }
                    else
                    {
                        border = 0;
                    }
                }
            };
            class BASFollowNavigation : public BorderAccumulationStrategy
            {
            private:
                Border* m_current; 
                double m_distance; 
                BorderCostMap* m_borderID2Cost; 
                bool m_continueOnIncreasingCost;
            public:
                BASFollowNavigation(Border* border, BorderSet* borderSet, BorderCostMap* borderID2Cost, double max_distance)
                {
                    //:m_current(border),m_borderSet(borderSet),m_borderID2Cost(borderID2Cost),m_distance(max_distance){}
                    m_current = border;
                    m_borderSet = borderSet;
                    m_borderID2Cost = borderID2Cost;
                    m_distance = max_distance;
                    m_continueOnIncreasingCost = false;
                }
                void setContinueOnIncreasingCost(bool value)
                {
                    m_continueOnIncreasingCost = value;
                }
                virtual void getNextBorder(Border*& border, bool& inverted) override
                {
                    if(m_current!=0 && m_distance>0)
                    {
                        Border* successor = 0;
                        border = m_current;
                        
                        bool previousCostSet = false;
                        double previousCost = 0.0; // initialized to silence -Wmaybe-uninitialized warning
                        auto previousCostResult = m_borderID2Cost->find(m_current->m_id);
                        if( previousCostResult!=m_borderID2Cost->end() )
                        {
                            previousCost = previousCostResult->second.getCombinedCost();
                            previousCostSet = true;
                        }
 
                        double minCost = 0.0; // initialized to silence -Wmaybe-uninitialized warning
 
                        for(auto it = m_borderSet->getSuccessors(m_current);it.current()!=it.end();it.current()++)
                        {
                            // check if corresponding borders are properly connected
                            Border* next = it.current()->second;
                            if(!borderValid(next)) {continue;}
                            Border* nextLeft = m_borderSet->getLeftNeighbor(next);
                            if(nextLeft!=0 && m_current->m_left!=0 
                                && m_borderSet->isConnected(*(m_current->m_left),nextLeft->m_id)
                                && !(m_current->m_id==next->m_id))
                            {
                                auto nextCostResult = m_borderID2Cost->find(next->m_id);
                                if( nextCostResult==m_borderID2Cost->end() )continue;
                                double nextCost = nextCostResult->second.getCombinedCost();
 
                                if( (previousCostSet && nextCost>=previousCost) || (successor!=0 && nextCost>=minCost) )continue;
 
                                minCost = nextCost;
                                successor = next;
                            }
                        }
 
                        if( successor == 0 && m_continueOnIncreasingCost)
                        {
                            BASFollowStraight m_basFallback(m_current,m_borderSet,100000.0);
                            m_basFallback.getNextBorder(successor,inverted);
                            m_basFallback.getNextBorder(successor,inverted);
                        }
 
                        m_distance -= m_current->getLength();
                        m_current = successor;
                        inverted = false;
                    }
                    else
                    {
                        border = 0;
                    }
                }
            };
            class BASNeighbor:public BorderAccumulationStrategy
            {
            private:
                BAContainer::iterator m_current; 
                BAContainer* m_list; 
            public:
                enum Direction
                {
                    LEFT,RIGHT
                };
            private:
                Direction m_direction; 
            public:
                BASNeighbor(BAContainer* list,BorderSet* borderSet,Direction direction)
                {
                    m_list = list;
                    m_current = list->begin();
                    m_borderSet =borderSet;
                    m_direction = direction;
                }
                virtual void getNextBorder(Border*& border,bool& inverted) override
                {
                    if(m_current!=m_list->end())
                    {
                        if(m_direction==LEFT)
                        {
                            border = m_borderSet->getLeftNeighbor(*m_current);
                            inverted = (*m_current)->getNeighborDirection()==Border::OPPOSITE_DIRECTION;
                        }
                        else
                        {
                            border = m_borderSet->getRightNeighbor(*m_current);
                            inverted = (*m_current)->getNeighborDirection()==Border::OPPOSITE_DIRECTION;
                        }
                        if(border==0)
                        {
                            border = *m_current;
                            inverted = false;
                        }
                        m_current ++;
                    }
                    else
                    {
                        border = 0;
                    }
                }
            };
 
            class BorderAccumulator
            {
            private:
                BAContainer m_list; 
                std::vector<bool> m_list_inverted;
                double m_distance; 
                int m_point_count;
            public:
                typedef adore::mad::LLinearPiecewiseFunctionM<double,3> function_type;
 
                BorderAccumulator():m_distance(0.0),m_point_count(0){}
                
                void clear()
                {
                    m_distance = 0.0;
                    m_point_count = 0;
                    m_list.clear();
                    m_list_inverted.clear();
                }
 
                BorderAccumulator(BorderAccumulationStrategy* bas):m_distance(0.0),m_point_count(0)
                {
                    append(bas);
                }
                 // puts a sequence of borders into the internal list, according to BorderAccumulationStrategy, and returns length of the combined borders
                // * append - accumulate the data from a border trace
                // */
                //void append(BorderTrace* borderTrace, BorderSet* borderSet)
                //{
                //  for(auto it = borderTrace->rbegin();
                //      it!=borderTrace->rend();
                //      it++)
                //  {
                //      Border* current = borderSet->getBorder(it->first);
                //      m_distance += it->second;
                //      m_point_count += current->m_path->getData().nc();
                //      m_list.push_back(current);
                //      m_list_inverted.push_back(false);
                //  }
                //}
 
                void append(Border* current,bool inverted)
                {
                    m_distance += current->getLength();
                    m_point_count += current->m_path->getData().nc();
                    m_list.push_back(current);
                    m_list_inverted.push_back(inverted);
                }
 
                void append(BorderAccumulationStrategy* bas)
                {
                    bool inverted;//field is filled by getNextBorder
                    Border* current;
                    for(    bas->getNextBorder(current,inverted); 
                            current!=0; 
                            bas->getNextBorder(current,inverted)    )
                    {
                        m_distance += current->getLength();
                        m_point_count += current->m_path->getData().nc();
                        m_list.push_back(current);
                        m_list_inverted.push_back(inverted);
                    }
                }
 
                BAContainer* getBorders()
                {
                    return &m_list;
                }
                void defineFunction(function_type& f)
                {
                    adoreMatrix<double,4,0> data;
                    data.set_size(4,m_point_count);
                    int count = 0;
                    auto it2 = m_list_inverted.begin();
                    for( auto it = m_list.begin(); it!= m_list.end(); it++,it2++ )
                    {
                        bool inverted = *it2;
                        int new_count = (*it)->m_path->getData().nc();
                        const adoreMatrix<double>& new_data = (*it)->m_path->getData();
                        for(int i=0;i<new_count;i++)
                        {
                            int idx_new = i;
                            int idx_combined = i + count;
                            if( inverted )
                            {
                                idx_new = new_count - i - 1;
                            }
                            for(int d=1;d<=3;d++)
                            {
                                data(d,idx_combined) = new_data(d,idx_new);
                            }
                        }
                        count += new_count;
                    }
                    data(0,0)=0;
                    for(int i=1;i<m_point_count;i++)
                    {
                        data(0,i) = data(0,i-1) + std::sqrt(  (data(1,i)-data(1,i-1))*(data(1,i)-data(1,i-1))
                                                             +(data(2,i)-data(2,i-1))*(data(2,i)-data(2,i-1))
                                                             +(data(3,i)-data(3,i-1))*(data(3,i)-data(3,i-1))   );
                    }
                    f = function_type(data);
                }
            };
        }
    }
}