TopologicalMap.C

Go to the documentation of this file.

/*!@file Beobot/TopologicalMap.C topological map                        */
// //////////////////////////////////////////////////////////////////// //
// The iLab Neuromorphic Vision C++ Toolkit - Copyright (C) 2001 by the //
// University of Southern California (USC) and the iLab at USC.         //
// See http://iLab.usc.edu for information about this project.          //
// //////////////////////////////////////////////////////////////////// //
// Major portions of the iLab Neuromorphic Vision Toolkit are protected //
// under the U.S. patent ``Computation of Intrinsic Perceptual Saliency //
// in Visual Environments, and Applications'' by Christof Koch and      //
// Laurent Itti, California Institute of Technology, 2001 (patent       //
// pending; application number 09/912,225 filed July 23, 2001; see      //
// http://pair.uspto.gov/cgi-bin/final/home.pl for current status).     //
// //////////////////////////////////////////////////////////////////// //
// This file is part of the iLab Neuromorphic Vision C++ Toolkit.       //
//                                                                      //
// The iLab Neuromorphic Vision C++ Toolkit is free software; you can   //
// redistribute it and/or modify it under the terms of the GNU General  //
// Public License as published by the Free Software Foundation; either  //
// version 2 of the License, or (at your option) any later version.     //
//                                                                      //
// The iLab Neuromorphic Vision C++ Toolkit 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 the iLab Neuromorphic Vision C++ Toolkit; if not, write   //
// to the Free Software Foundation, Inc., 59 Temple Place, Suite 330,   //
// Boston, MA 02111-1307 USA.                                           //
// //////////////////////////////////////////////////////////////////// //
//
// Primary maintainer for this file: Christian Siagian <siagian@usc.edu>
// $HeadURL: svn://isvn.usc.edu/software/invt/trunk/saliency/src/Beobot/TopologicalMap.C $
// $Id:TopologicalMap.C 6891 2006-05-25 06:46:56Z siagian $
//

// ######################################################################
/*! Topological map for localization                                    */

#include "Beobot/TopologicalMap.H"
#include "Raster/Raster.H"
#include "Image/DrawOps.H"      // for drawing
#include "Image/MathOps.H"      // for minmax

#include <cstdio>

// ######################################################################
TopologicalMap::TopologicalMap()
  : Map(),
    itsGraph(new Graph())
{
}

// ######################################################################
TopologicalMap::TopologicalMap(std::string fileName)
  : Map(),
    itsGraph(new Graph())
{
  read(fileName);
}

// ######################################################################
TopologicalMap::~TopologicalMap()
{
}

// ######################################################################
bool TopologicalMap::read(std::string fileName)
{
  FILE *fp;  char inLine[200]; char comment[200];

  LINFO("file name: %s", fileName.c_str());
  if((fp = fopen(fileName.c_str(),"rb")) == NULL)
    {
      LINFO("not found");
      itsMapWidth = 0; itsMapHeight = 0;
      LINFO("map size: 0x0");

      itsMapScale = 1.0;
      LINFO("map scale: 1.0");

      itsSegmentList.resize(0);
      itsSegmentEdgeIndexList.resize(0);
      itsSegmentLength.resize(0);
      LINFO("segment size: %d", 0);

      return false;
    }

  // file creation info:
  //  user id, creation date, general map description

  // off-limit areas
  //   # of areas
  //   description of area (how to characterize the dimensions)

  // named places/position
  // need distance relationship wrt to area
  //   # of points, description, x,y,theta
  //   # of edges to connect the places, with expected distances

  // size of area covered
  if (fgets(inLine, 200, fp) == NULL) LFATAL("fgets failed");
  int w, h; sscanf(inLine, "%s %d %d", comment, &w, &h);
  itsMapWidth = w; itsMapHeight = h;
  LINFO("map size: %d %d", itsMapWidth, itsMapHeight);

  // size of area covered
  if (fgets(inLine, 200, fp) == NULL) LFATAL("fgets failed");
  float scale; sscanf(inLine, "%s %f", comment, &scale);
  itsMapScale = scale;
  LINFO("map scale: %f", itsMapScale);

  // segment size
  if (fgets(inLine, 200, fp) == NULL) LFATAL("fgets failed");
  int ssize;  sscanf(inLine, "%s %d", comment, &ssize);
  itsSegmentList.resize(ssize);
  itsSegmentEdgeIndexList.resize(ssize);
  itsSegmentLength.resize(ssize);
  LINFO("segment Num: %d", ssize);

  // skip the nodes section divider
  if (fgets(inLine, 200, fp) == NULL) LFATAL("fgets failed");

  // get the node coordinates
  uint ncount = 0;
  while(fgets(inLine, 200, fp) != NULL && inLine[0] != '=')
  {
    int x, y; sscanf(inLine, "%d %d", &x, &y);
    LINFO("node[%d]: (%d,%d)", itsGraph->getNumNode(), x, y);

    rutz::shared_ptr<Node> node(new Node(ncount, Point2D<int>(x,y)));
    itsGraph->addNode(node);
    ncount++;
  }

  // get the edge information
  while((fgets(inLine, 200, fp) != NULL) && (inLine[0] != '='))
  {
    int s, e; sscanf(inLine, "%d %d", &s, &e);
    Point2D<int> scoor = itsGraph->getNode(s)->getCoordinate();
    Point2D<int> ecoor = itsGraph->getNode(e)->getCoordinate();
    float dist = scoor.distance(ecoor);
    LINFO("edge(%d,%d): %f", s, e, dist);

    rutz::shared_ptr<Edge>
      edge(new Edge(itsGraph->getNode(s),
                    itsGraph->getNode(e), dist));
    itsGraph->addEdge(edge);
  }

  // get the segment information
  uint scount = 0;
  while(fgets(inLine, 200, fp) != NULL)
  {
    std::string line(inLine);
    while(line.length() > 0)
      {
        uint cedge;
        std::string::size_type spos = line.find_first_of(' ');
        if(spos != std::string::npos)
          {
            cedge = uint(atoi(line.substr(0, spos).c_str()));
            line = line.substr(spos+1);
          }
        else
          {
            cedge = uint(atoi(line.c_str()));
            line = std::string("");
          }
        LINFO("seg[%d][%"ZU"]:%d",
              scount, itsSegmentList[scount].size(), cedge);
        itsSegmentList[scount].push_back(itsGraph->getEdge(cedge));
        itsSegmentEdgeIndexList[scount].push_back(cedge);
      }
    scount++;
  }

  fclose(fp);

  // compute all the shortcuts for shortest-distance related operations
  // node-to-node at graph level
  itsGraph->computeAdjecencyList();
  itsGraph->computeDistances();

  // node-to-segment, and segment-to-segment at this level
  computeDistances();

  // segment length at this level
  setSegmentLength();

  return true;
}

// ######################################################################
void TopologicalMap::computeDistances()
{
  // create segment number distance matrix
  uint nnode = itsGraph->getNumNode();
  uint nsegment = getSegmentNum();
  itsNodeSegmentDistanceMatrix.resize(nnode, nsegment);

  // for each node
  for(uint i = 0; i < nnode; i++)
    {
      // for each segment
      for(uint j = 0; j < nsegment; j++)
        {
          // find the closest distance
          float mindist;
          uint nsedge = itsSegmentList[j].size();
          if(nsedge > 0)
            {
              uint snode = itsSegmentList[j][0]->getSourceNode()->getIndex();
              mindist = itsGraph->getUndirectedDistance(i,snode);
            }
          else mindist = -1.0F;

          for(uint k = 0; k < nsedge; k++)
            {
              uint dnode = itsSegmentList[j][k]->getDestNode()->getIndex();
              float cdist = itsGraph->getUndirectedDistance(i, dnode);
              if(mindist > cdist) mindist = cdist;
            }

          LDEBUG("itsSegmentList[%d][%d]: %f",i,j, mindist);
          itsNodeSegmentDistanceMatrix.setVal(i, j, mindist);
        }
    }
}

// ######################################################################
bool TopologicalMap::write(std::string fileName)
{
  LFATAL("NOT YET implemented");
  return true;
}

// ######################################################################
Image<PixRGB<byte> > TopologicalMap::getMapImage(uint w, uint h)
{
  float scale = std::min(w/itsMapWidth, h/itsMapHeight);

  Image<PixRGB<byte> >
    res(int(itsMapWidth * scale), int(itsMapHeight * scale), ZEROS);

  // draw the edges first
  for(uint i = 0; i < itsGraph->getNumEdge(); i++)
    {
      Point2D<int> sp = itsGraph->getEdge(i)->getSourceNode()->getCoordinate();
      Point2D<int> ep = itsGraph->getEdge(i)->getDestNode()->getCoordinate();
      drawArrow(res,
                Point2D<int>(int(sp.i * scale), int(sp.j * scale)),
                Point2D<int>(int(ep.i * scale), int(ep.j * scale)),
                PixRGB<byte>(127,127,127));
    }

  // draw the nodes
  for(uint i = 0; i < itsGraph->getNumNode(); i++)
    {
      Point2D<int> p = itsGraph->getNode(i)->getCoordinate();
      Point2D<int> pd(int(p.i * scale), int(p.j * scale));
      drawDisk(res, pd, 2, PixRGB<byte>(255,255,255));
    }

  drawRect(res,res.getBounds(),PixRGB<byte>(255,255,255));

  return res;
}

// ######################################################################
float TopologicalMap::getDistance
(uint asnum, float altrav, uint bsnum, float bltrav)
{
  ASSERT(asnum < itsSegmentList.size() && altrav <= 1.0F &&
         bsnum < itsSegmentList.size() && bltrav <= 1.0F    );

  // get the edges that the two points belongs to
  // and the distances to start and end nodes
  float dsa, dea, dsb, deb;
  rutz::shared_ptr<Edge> a = getEdge(asnum, altrav, dsa, dea);
  rutz::shared_ptr<Edge> b = getEdge(bsnum, bltrav, dsb, deb);

  // get the end node indexes
  uint sa = a->getSourceNode()->getIndex();
  uint ea = a->getDestNode()->getIndex();
  uint sb = b->getSourceNode()->getIndex();
  uint eb = b->getDestNode()->getIndex();
  LDEBUG("%d %d  -- %d %d", sa, ea, sb, eb);

  // same edge situation
  // in case we have a directed graph
  float dist = 0.0F;
  if(sa == sb && ea == eb) dist = fabs(dsa - dsb);
  else if(sa == eb && sb == ea) dist = fabs(dsa + dsb - a->getCost());
  else
    {
      // get the shortest distance in
      float dist1 = dsa + itsGraph->getUndirectedDistance(sa, sb) + dsb;
      float dist2 = dsa + itsGraph->getUndirectedDistance(sa, eb) + deb;
      float dist3 = dea + itsGraph->getUndirectedDistance(ea, sb) + dsb;
      float dist4 = dea + itsGraph->getUndirectedDistance(ea, eb) + deb;
      LDEBUG("%f %f %f %f", dist1,dist2,dist3,dist4);
      dist = std::min(std::min(dist1, dist2), std::min(dist3, dist4));
    }
  LDEBUG("-> %f",dist);
  return dist;
}

// ######################################################################
float TopologicalMap::getPath
(uint asnum, float altrav, uint bsnum, float bltrav,
 std::vector<int> &moves)
{
  // NOTE: assuming graph is undirected
  //       if want directed get path
  //       has to find eindexa and b for the opposite direction
  //       i.e. edge(ea,sa) and edge(eb,sb)

  ASSERT(asnum < itsSegmentList.size() && altrav <= 1.0F &&
         bsnum < itsSegmentList.size() && bltrav <= 1.0F    );
  moves.clear();

  // get the edges that the two points belongs to
  // and the distances to start and end nodes
  float dsa, dea, dsb, deb; uint eindexa, eindexb;
  rutz::shared_ptr<Edge> a = getEdge(asnum, altrav, dsa, dea, eindexa);
  rutz::shared_ptr<Edge> b = getEdge(bsnum, bltrav, dsb, deb, eindexb);

  // get the end node indexes
  uint sa = a->getSourceNode()->getIndex();
  uint ea = a->getDestNode()->getIndex();
  uint sb = b->getSourceNode()->getIndex();
  uint eb = b->getDestNode()->getIndex();
  LDEBUG("%d %d  -- %d %d", sa, ea, sb, eb);

  // same edge situation with same start and end
  float dist = 0.0F;
  if(sa == sb && ea == eb)
    {
      dist = fabs(dsa - dsb);
      if(dsb > dsa) moves.push_back(TOPOMAP_MOVE_FORWARD);
      else
        {
          moves.push_back(TOPOMAP_TURN_AROUND);
          moves.push_back(TOPOMAP_MOVE_FORWARD);
        }
    }
  // same edge situation with flipped start and end
  else if(sa == eb && sb == ea)
    {
      dist = fabs(dsa + dsb - a->getCost());
      if(deb < dsa) moves.push_back(TOPOMAP_MOVE_FORWARD);
      else
        {
          moves.push_back(TOPOMAP_TURN_AROUND);
          moves.push_back(TOPOMAP_MOVE_FORWARD);
        }
    }
  // goes through a few other edges
  else
    {
      // get the shortest distance in
      std::vector<uint> edges1;
      float dist1 = dsa +
        itsGraph->getUndirectedDistance(sa, sb, edges1) + dsb;
      std::vector<uint> edges2;
      float dist2 = dsa +
        itsGraph->getUndirectedDistance(sa, eb, edges2) + deb;
      std::vector<uint> edges3;
      float dist3 = dea +
        itsGraph->getUndirectedDistance(ea, sb, edges3) + dsb;
      std::vector<uint> edges4;
      float dist4 = dea +
        itsGraph->getUndirectedDistance(ea, eb, edges4) + deb;
      dist = std::min(std::min(dist1, dist2), std::min(dist3, dist4));
      LDEBUG("%f %f %f %f: min: %f", dist1, dist2, dist3, dist4, dist);

      // get the edges
      if(dist1 == dist)
        {
          LDEBUG("E1: %"ZU, edges1.size());
          moves.push_back(TOPOMAP_TURN_AROUND);
          moves.push_back(TOPOMAP_MOVE_FORWARD);
          for(uint i = 0; i < edges1.size(); i++)
            {
              moves.push_back(edges1[i]);
              LDEBUG("E1: %d", edges1[i]);
            }
          moves.push_back(eindexb);
        }
      else if(dist2 == dist)
        {
          LDEBUG("E2: %"ZU, edges2.size());
          moves.push_back(TOPOMAP_TURN_AROUND);
          moves.push_back(TOPOMAP_MOVE_FORWARD);
          for(uint i = 0; i < edges2.size(); i++)
            {
              moves.push_back(edges2[i]);
              LDEBUG("E2: %d", edges2[i]);
            }
          moves.push_back(eindexb);

          // needs opposite edge eindexb for directed case
        }
      else if(dist3 == dist)
        {
          LDEBUG("E3: %"ZU,edges3.size());
          moves.push_back(TOPOMAP_MOVE_FORWARD);
          for(uint i = 0; i < edges3.size(); i++)
            {
              moves.push_back(edges3[i]);
              LDEBUG("E3: %d", edges3[i]);
            }
          moves.push_back(eindexb);
        }
      else if(dist4 == dist)
        {
          LDEBUG("E4: %"ZU, edges4.size());
          moves.push_back(TOPOMAP_MOVE_FORWARD);
          for(uint i = 0; i < edges4.size(); i++)
            {
              moves.push_back(edges4[i]);
              LDEBUG("E4: %d", edges4[i]);
            }
          moves.push_back(eindexb);

          // needs opposite edge eindexb for directed case
        }

      for(uint i = 0; i < moves.size(); i++)
        LDEBUG("[%d]: %d", i, moves[i]);
    }
  return dist;
}


// ######################################################################
rutz::shared_ptr<Edge> TopologicalMap::getEdge
(uint cseg, float ltrav, float &sdist, float &edist)
{
  uint i;
  return getEdge(cseg, ltrav, sdist, edist, i);
}

// ######################################################################
rutz::shared_ptr<Edge> TopologicalMap::getEdge
(uint cseg, float ltrav, float &sdist, float &edist, uint &eindex)
{
  ASSERT((cseg < itsSegmentList.size()) && (ltrav <= 1.0F));

  // using the segment we can get its list of edges
  float slen = getSegmentLength(cseg);
  float altrav = ltrav * slen;
  LDEBUG("%d: %f * %f = altrav: %f", cseg, ltrav, slen, altrav);

  // iterate through the list of edges
  float cltrav = 0.0F;
  uint nsedge = itsSegmentList[cseg].size();
  if(nsedge == 0) return rutz::shared_ptr<Edge>();

  uint j = 0;
  while((j < nsedge) && (altrav >= cltrav))
    { cltrav += itsSegmentList[cseg][j]->getCost(); j++; }
  LDEBUG("nsedge: %d -> j: %d", nsedge, j);

  // get the distance to the next node
  edist = cltrav - altrav;
  sdist = itsSegmentList[cseg][j-1]->getCost() - edist;
  LDEBUG("sdist: %f, edist: %f", sdist, edist);
  eindex = itsSegmentEdgeIndexList[cseg][j-1];
  return itsSegmentList[cseg][j-1];
}

// ######################################################################
rutz::shared_ptr<Edge> TopologicalMap::getEdge
(Point2D<int> loc, float &sdist, float &edist)
{
  LFATAL("NOT YET implemented");

  // can be used to implement conversion from Point2D<int> to (snum,ltrav)
  return rutz::shared_ptr<Edge>();
}

// ######################################################################
float TopologicalMap::getSegmentDistance
(uint sseg, float ltrav, uint dseg, uint &intIndex)
{
  ASSERT(sseg < itsSegmentList.size() &&
         dseg < itsSegmentList.size() && ltrav <= 1.0F);
  //LINFO(" [%d to %d]", sseg, dseg);

  // if the point is in the same segment
  // the closest node is an invalid value
  if(sseg == dseg) { intIndex = itsGraph->getNumNode(); return 0.0F; }

  // get the edge of the starting point
  float slen = getSegmentLength(sseg);
  float altrav = ltrav * slen;
  float clen = altrav; uint i = 0;
  //LINFO("slen: %f, altrav: %f",slen, altrav);

  while(clen > 0.0F)
    {
      float len = itsSegmentList[sseg][i]->getCost();
      if(len > clen)
        {
          uint sind = itsSegmentList[sseg][i]->getSourceNode()->getIndex();
          uint eind = itsSegmentList[sseg][i]->getDestNode()->getIndex();

          float sdist =
            itsNodeSegmentDistanceMatrix[Point2D<int>(sind,dseg)] + clen;
          float edist =
            itsNodeSegmentDistanceMatrix[Point2D<int>(eind,dseg)] + len - clen;

          //LINFO("2 sind: (%d  %f), eind: (%d %f)", sind, sdist, eind, edist);

          if(sdist > edist)
            { intIndex = eind; return edist; }
          else
            { intIndex = sind; return sdist; }
        }
      else if(len == clen)
        {
          uint eind = itsSegmentList[sseg][i]->getDestNode()->getIndex();
          float edist = itsNodeSegmentDistanceMatrix[Point2D<int>(eind,dseg)];

          //LINFO("1 eind: (%d %f)", eind, edist);
          intIndex = eind;
          return edist;
        }
      else{ clen -= len; i++; }
      //LINFO("len: %f, clen: %f", len, clen);
    }

  // the point is on the source node of the first edge on the list
  uint sind = itsSegmentList[sseg][0]->getSourceNode()->getIndex();
  float sdist = itsNodeSegmentDistanceMatrix[Point2D<int>(sind,dseg)];
  intIndex = sind;
  //LINFO("0 sind: (%d  %f)", sind, sdist);

  return sdist;
}

// ######################################################################
void TopologicalMap::setSegmentLength()
{
  // for each segment
  for(uint i = 0; i < itsSegmentList.size(); i++)
    {
      // add up the segment length (cost)
      itsSegmentLength[i] = 0.0F;
      uint nsedge = itsSegmentList[i].size();
      for(uint j = 0; j < nsedge; j++)
        itsSegmentLength[i] += itsSegmentList[i][j]->getCost();
    }
}

// ######################################################################
float TopologicalMap::getSegmentLength(uint index)
{
  ASSERT(index < itsSegmentLength.size());
  return itsSegmentLength[index];
}

// ######################################################################
float TopologicalMap::getNodeSegmentMaxDistance()
{
  float min, max;
  getMinMax(itsNodeSegmentDistanceMatrix, min, max);
  if(min == -1.0F) return -1.0F;
  else return max;
}

// ######################################################################
void TopologicalMap::getLocationFloat(uint cseg, float ltrav,
                                      float &x, float &y)
{
  ASSERT((cseg < itsSegmentList.size()) &&
         (ltrav <= 1.0F) && (ltrav >= 0.0F));

  // using the segment we can get its list of edges
  float slen = getSegmentLength(cseg);
  float altrav = ltrav * slen;
  LDEBUG("%d: %f * %f = altrav: %f", cseg, ltrav, slen, altrav);

  // iterate through the list of edges
  float cltrav = 0.0F;
  uint nsedge = itsSegmentList[cseg].size();
  if(nsedge == 0) { x = -1.0F; y = -1.0F; return; }
  uint j = 0;
  while((j < nsedge) && (altrav >= cltrav))
    { cltrav += itsSegmentList[cseg][j]->getCost(); j++; }
  LDEBUG("nsedge: %d -> j: %d", nsedge, j);

  // get the distance to the next node
  float rdist = cltrav - altrav;
  float dist = itsSegmentList[cseg][j-1]->getCost();
  Point2D<int> sloc =
    itsSegmentList[cseg][j-1]->getSourceNode()->getCoordinate();
  Point2D<int> eloc =
    itsSegmentList[cseg][j-1]->getDestNode()->getCoordinate();
  LDEBUG("%f/%f (%d,%d) -> (%d,%d)",
        rdist, dist, sloc.i,sloc.j, eloc.i, eloc.j);

  x = eloc.i - rdist/dist*(eloc.i - sloc.i);
  y = eloc.j - rdist/dist*(eloc.j - sloc.j);
}

// ######################################################################
Point2D<int> TopologicalMap::getLocation(uint cseg, float ltrav)
{
  float x, y;
  getLocationFloat(cseg, ltrav, x, y);
  uint iloc = uint(x + 0.5F);
  uint jloc = uint(y + 0.5F);
  LDEBUG("(%d, %f) -> %d, %d", cseg, ltrav, iloc, jloc);
  return Point2D<int>(iloc, jloc);
}

// ######################################################################
void TopologicalMap::getLocation(Point2D<int>, uint &cseg, float &ltrav)
{
  // use get edge (also not yet implemented)
  // to implement conversion from Point2D<int> to (snum,ltrav)

  // check if the point is within the map
  LFATAL("NOT YET implemented");
}

// ######################################################################
uint TopologicalMap::getSegmentLocation(Point2D<int> loc)
{
  // use get edge (also not yet implemented)
  // to implement conversion from Point2D<int> to (snum,ltrav)

  LFATAL("NOT YET implemented");

  return 0;
}

// ######################################################################
float TopologicalMap::getSegmentLengthTraveled(Point2D<int> loc)
{
  // use get edge (also not yet implemented)
  // to implement conversion from Point2D<int> to (snum,ltrav)

  LFATAL("NOT YET implemented");
  return 0.0;
}

// ######################################################################
std::vector<rutz::shared_ptr<Node> >
TopologicalMap::getNodesInInterval(uint index, float fltrav, float lltrav)
{
  ASSERT(index < itsSegmentLength.size());
  ASSERT(fltrav >= 0.0 && fltrav <= 1.0);
  ASSERT(lltrav >= 0.0 && lltrav <= 1.0);

  float slen = getSegmentLength(index);
  std::vector<rutz::shared_ptr<Node> > res;
  float caltrav = 0.0;

  // go through the edges in the segment
  uint ssize = itsSegmentList[index].size();
  for(uint i = 0; i < ssize; i++)
    {
      // check if the source node is within the interval
      float ltrav = caltrav/slen;
      LDEBUG("%d: %f * %f = altrav: %f", index, ltrav, slen, caltrav);

      if(ltrav >= fltrav && ltrav <= lltrav)
        res.push_back(itsSegmentList[index][i]->getSourceNode());
      else if(ltrav > lltrav) return res;

      caltrav += itsSegmentList[index][i]->getCost();
    }

  if (lltrav == 1.0)
    res.push_back(itsSegmentList[index][ssize-1]->getDestNode());

  return res;
}

// ######################################################################
std::vector<std::pair<uint,float> >
TopologicalMap::getNodeLocationsInInterval
(uint index, float fltrav, float lltrav)
{
  ASSERT(index < itsSegmentLength.size());
  ASSERT(fltrav >= 0.0 && fltrav <= 1.0);
  ASSERT(lltrav >= 0.0 && lltrav <= 1.0);

  float slen = getSegmentLength(index);
  std::vector<std::pair<uint,float> > res;
  float caltrav = 0.0;

  // go through the edges in the segment
  uint ssize = itsSegmentList[index].size();
  for(uint i = 0; i < ssize; i++)
    {
      // check if the source node is within the interval
      float ltrav = caltrav/slen;
      LDEBUG("%d: %f * %f = caltrav: %f", index, ltrav, slen, caltrav);

      if(ltrav >= fltrav && ltrav <= lltrav)
        res.push_back(std::pair<uint,float>(index, ltrav));
      else if(ltrav > lltrav) return res;

      caltrav += itsSegmentList[index][i]->getCost();
    }

  if (lltrav == 1.0F)
    res.push_back(std::pair<uint,float>(index, 1.0F));

  return res;
}

// ######################################################################
float TopologicalMap::getAngle
(rutz::shared_ptr<Edge> e1, rutz::shared_ptr<Edge> e2)
{
  return itsGraph->getAngle(e1, e2);
}

// ######################################################################
/* So things look consistent in everyone's emacs... */
/* Local Variables: */
/* indent-tabs-mode: nil */
/* End: */