O(1) distance lookup structure for quickly accessing route distance information. NOTE: This structure is used internally in the world model and is not intended for use by WorldModel users. More...

#include <IndexedDistanceMap.hpp>

Collaboration diagram for carma_wm::IndexedDistanceMap:

Public Member Functions
void	pushBack (const lanelet::LineString2d &ls)
	Add a linestring to this structure. This function will iterate over the line string to compute distances between each segment. More...

double	elementLength (size_t index) const
	Get the length of the linestring located at the provided index. More...

double	distanceToElement (size_t index) const
	Get the distance to the start of the linestring at the specified index. More...

double	distanceBetween (size_t index, size_t p1_index, size_t p2_index) const
	Get the distance between two points on the same linestring. More...

double	distanceToPointAlongElement (size_t index, size_t point_index) const
	Get the along-line distance to the point on the provided linestring. More...

double	totalLength () const
	Returns the total along-line length of this structure. More...

std::pair< size_t, size_t >	getIndexFromId (const lanelet::Id &id) const
	Returns the indexes of the element identified by the provided Id NOTE: It is up to the user to know if the id is for a linestring or a point. By default index values will be 0. More...

size_t	size () const
	Returns number of linestrings in this structure. More...

size_t	size (size_t index) const
	Returns the size of the linestring at the specified index. More...

std::pair< size_t, size_t >	getElementIndexByDistance (double distance, bool get_point=true) const
	Returns index of the linestring which the provided distance is within. NOTE: Unlike the rest of this class, this method runs in O(log n) where n is this.size() for accessing linestring index If accessing point index (get_point=true) then it runs an addition O(log m) where m is the linestring index.size(); This means the max complexity of this function is O(log n) + O(log m) More...

Private Attributes
std::vector< std::tuple< std::vector< double >, double > >	accum_lengths

std::unordered_map< lanelet::Id, std::pair< size_t, size_t > >	id_index_map

Detailed Description

O(1) distance lookup structure for quickly accessing route distance information. NOTE: This structure is used internally in the world model and is not intended for use by WorldModel users.

This class is meant to be used when a route update occurs to precompute distances along the route to support rapid queries later on Insertion is O(n) where n is the number of points in the linestring This structure does not support element reinsertion Linestrings added to this structure should be fully unique (NO DUPLICATE IDs for linestring or point objects)

NOTE: Pre-computing route distances make queries much faster but could slow down route loading for large routes and cause them to use more memory.

Definition at line 42 of file IndexedDistanceMap.hpp.

Member Function Documentation

◆ distanceBetween()

double carma_wm::IndexedDistanceMap::distanceBetween	(	size_t	index,
		size_t	p1_index,
		size_t	p2_index
	)		const

Get the distance between two points on the same linestring.

NOTE: No bounds checking is performed

Parameters

index	The index of the linestring
p1_index	The index of the first point on the linestring
p2_index	The index of the second point

Returns: The along-line distance between the two provided points on the same linestring

Definition at line 90 of file IndexedDistanceMap.cpp.

{
  return fabs(distanceToPointAlongElement(index, p2_index) - distanceToPointAlongElement(index, p1_index));
}

References distanceToPointAlongElement().

Referenced by carma_wm::CARMAWorldModel::routeTrackPos().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ distanceToElement()

double carma_wm::IndexedDistanceMap::distanceToElement ( size_t index ) const

Get the distance to the start of the linestring at the specified index.

NOTE: No bounds checking is performed

Parameters

index The index of the linestring

Returns: The along-line distance to the start of the linestring at index

Definition at line 85 of file IndexedDistanceMap.cpp.

{
  return std::get<1>(accum_lengths[index]);
}

References accum_lengths.

Referenced by getElementIndexByDistance(), carma_wm::CARMAWorldModel::pointFromRouteTrackPos(), carma_wm::CARMAWorldModel::routeTrackPos(), and totalLength().

Here is the caller graph for this function:

◆ distanceToPointAlongElement()

double carma_wm::IndexedDistanceMap::distanceToPointAlongElement	(	size_t	index,
		size_t	point_index
	)		const

Get the along-line distance to the point on the provided linestring.

NOTE: No bounds checking is performed

Parameters

index	The linestring index
point_index	The point index in the linestring at index

Returns: The along-line distance to the point from the start of the requested linestring

Definition at line 95 of file IndexedDistanceMap.cpp.

{
  return std::get<0>(accum_lengths[index])[point_index];
}

References accum_lengths.

Referenced by distanceBetween(), carma_wm::CARMAWorldModel::pointFromRouteTrackPos(), and carma_wm::CARMAWorldModel::routeTrackPos().

Here is the caller graph for this function:

◆ elementLength()

double carma_wm::IndexedDistanceMap::elementLength ( size_t index ) const

Get the length of the linestring located at the provided index.

NOTE: No bounds checking is performed

Parameters

index The index of the linestring

Returns: The length of the linestring

Definition at line 80 of file IndexedDistanceMap.cpp.

{
  return std::get<0>(accum_lengths[index]).back();
}

References accum_lengths.

Referenced by totalLength().

Here is the caller graph for this function:

◆ getElementIndexByDistance()

std::pair< size_t, size_t > carma_wm::IndexedDistanceMap::getElementIndexByDistance	(	double	distance,
		bool	get_point = `true`
	)		const

Returns index of the linestring which the provided distance is within. NOTE: Unlike the rest of this class, this method runs in O(log n) where n is this.size() for accessing linestring index If accessing point index (get_point=true) then it runs an addition O(log m) where m is the linestring index.size(); This means the max complexity of this function is O(log n) + O(log m)

Exceptions

std::invalid_argument if distance does not fit within bounds [0, totalLength()]

Parameters

distance	The downtrack distance in meters to get the element for
get_point	Set to true if you wish to access the index of the point prior to the provided distance. If false then pair->second will always be 0

Returns: A pair of the linestring index (first) and point index (second) which have their start distances before the provided distance

Definition at line 44 of file IndexedDistanceMap.cpp.

                                                                                                           {
  if (distance < 0) {
    throw std::invalid_argument("Distance must non-negative: " + std::to_string(distance));
  }
  if (distance > totalLength()) {
    throw std::invalid_argument("Distance cannot be greater than distance map length");
  }
  if (accum_lengths.size() == 0) {
    throw std::invalid_argument("No data available in distance map");
  }
 
  auto low = std::lower_bound (accum_lengths.begin(), accum_lengths.end(), distance, // Binary search to find the index 
    [](const std::tuple<std::vector<double>, double>& a, const double& b){return std::get<1>(a) < b;});
  
  size_t ls_index = 0;
  if (low == accum_lengths.end()) { // If we reached the end, it means we should pick the final point since we already checked the bounds
    ls_index = accum_lengths.size() - 1;
  } else {
    ls_index = std::max(low - accum_lengths.begin() - 1, (long int)0);
  }
 
  if (!get_point) { // If we are looking for the linestring index then return
    return std::make_pair(ls_index, (size_t) 0);
  }
 
  // If we are looking for the point index then calculate the index in the same manner as before
  double dist_to_ls = distanceToElement(ls_index);
  double relative_dist = distance - dist_to_ls;
 
  auto low_point = std::lower_bound (std::get<0>(accum_lengths[ls_index]).begin(), std::get<0>(accum_lengths[ls_index]).end(), relative_dist, // Binary search to find the point index 
    [](const double& a, const double& b){ return a < b; });
  
  size_t point_idx = std::max(low_point - std::get<0>(accum_lengths[ls_index]).begin() - 1, (long int)0);
  return std::make_pair(ls_index, point_idx);
}

References accum_lengths, distanceToElement(), carma_cooperative_perception::to_string(), and totalLength().

Referenced by carma_wm::CARMAWorldModel::pointFromRouteTrackPos().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ getIndexFromId()

std::pair< size_t, size_t > carma_wm::IndexedDistanceMap::getIndexFromId ( const lanelet::Id & id ) const

Returns the indexes of the element identified by the provided Id NOTE: It is up to the user to know if the id is for a linestring or a point. By default index values will be 0.

Exceptions

std::out_of_range if the container does not have an element with the specified id

Returns: An std::pair of the matched index where the first element is the linestring index and the second is the point index in that linestring

Definition at line 109 of file IndexedDistanceMap.cpp.

{
  return id_index_map.at(id);
}

References id_index_map.

Referenced by carma_wm::CARMAWorldModel::routeTrackPos().

Here is the caller graph for this function:

◆ pushBack()

void carma_wm::IndexedDistanceMap::pushBack ( const lanelet::LineString2d & ls )

Add a linestring to this structure. This function will iterate over the line string to compute distances between each segment.

Exceptions

std::invalid_argument If the provided linstring was already added to this structure

Parameters

ls	The linestring to add

Definition at line 21 of file IndexedDistanceMap.cpp.

{
  if (id_index_map.find(ls.id()) != id_index_map.end())
  {
    throw std::invalid_argument("IndexedDistanceMap already contains this ls");
  }
  std::vector<double> accumulated_dist;
  accumulated_dist.reserve(ls.size());
  accumulated_dist.push_back(0);
  size_t ls_i = accum_lengths.size();
  id_index_map[ls.front().id()] = std::make_pair(ls_i, 0);  // Add first point to id map
  for (size_t i = 0; i < ls.numSegments(); i++)
  {
    auto segment = ls.segment(i);
    double dist = lanelet::geometry::distance2d(segment.first, segment.second);  // length of line string
    accumulated_dist.push_back(dist + accumulated_dist.back());                  // Distance along linestring
    id_index_map[segment.second.id()] = std::make_pair(ls_i, i + 1);             // Add point id and index to map
  }
  auto tuple = std::make_tuple(accumulated_dist, totalLength());
  accum_lengths.push_back(tuple);
  id_index_map[ls.id()] = std::make_pair(ls_i, 0);  // Add linestirng id
}

References accum_lengths, process_bag::i, id_index_map, and totalLength().

Referenced by carma_wm::CARMAWorldModel::computeDowntrackReferenceLine().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ size() [1/2]

size_t carma_wm::IndexedDistanceMap::size ( ) const

Returns number of linestrings in this structure.

Returns: The element count

Definition at line 114 of file IndexedDistanceMap.cpp.

{
  return accum_lengths.size();
}

References accum_lengths.

Referenced by carma_wm::CARMAWorldModel::computeDowntrackReferenceLine().

Here is the caller graph for this function:

◆ size() [2/2]

size_t carma_wm::IndexedDistanceMap::size ( size_t index ) const

Returns the size of the linestring at the specified index.

Returns: The linestring point count

Definition at line 119 of file IndexedDistanceMap.cpp.

{
  return std::get<0>(accum_lengths[index]).size();
}

References accum_lengths.

◆ totalLength()

double carma_wm::IndexedDistanceMap::totalLength ( ) const

Returns the total along-line length of this structure.

Returns: The length

Definition at line 100 of file IndexedDistanceMap.cpp.

{
  if (accum_lengths.size() == 0)
  {
    return 0.0;
  }
  return distanceToElement(accum_lengths.size() - 1) + elementLength(accum_lengths.size() - 1);
}