IndexedDistanceMap.hpp

Go to the documentation of this file.

#pragma once
 
/*
 * Copyright (C) 2022 LEIDOS.
 *
 * Licensed under the Apache License, Version 2.0 (the "License"); you may not
 * use this file except in compliance with the License. You may obtain a copy of
 * the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
 * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
 * License for the specific language governing permissions and limitations under
 * the License.
 */
 
#include <vector>
#include <tuple>
#include <utility>
#include <lanelet2_core/primitives/LineString.h>
#include <lanelet2_core/geometry/Point.h>
#include <unordered_map>
#include <math.h>
 
namespace carma_wm
{
class IndexedDistanceMap
{
private:
  // Distance storage structure
  // A vector of tuples where the first vector is indexed by line segment and the second vector is index by point index
  // on that line segment. The double value in the tuple stores the total along-line distance to the start of that line
  // segment from the first point on the first line segment The double value inside the interior vector store the
  // along-line distance of the point from the start of its line segment
  std::vector<std::tuple<std::vector<double>, double>> accum_lengths;
 
  // Id mapping structure
  // Sores the linestring and point index's as values with their lanelet Ids as the key
  std::unordered_map<lanelet::Id, std::pair<size_t, size_t>> id_index_map;
 
public:
  void pushBack(const lanelet::LineString2d& ls);
 
  double elementLength(size_t index) const;
 
  double distanceToElement(size_t index) const;
 
  double distanceBetween(size_t index, size_t p1_index, size_t p2_index) const;
 
  double distanceToPointAlongElement(size_t index, size_t point_index) const;
 
  double totalLength() const;
 
  std::pair<size_t, size_t> getIndexFromId(const lanelet::Id& id) const;
 
  size_t size() const;
 
  size_t size(size_t index) const;
 
  std::pair<size_t, size_t> getElementIndexByDistance(double distance, bool get_point=true) const;
};
} //namespace carma_wm