2016-08-17 03:49:19 -04:00
|
|
|
#include "extractor/guidance/coordinate_extractor.hpp"
|
|
|
|
#include "extractor/guidance/constants.hpp"
|
|
|
|
#include "extractor/guidance/toolkit.hpp"
|
|
|
|
|
|
|
|
#include <algorithm>
|
|
|
|
#include <cstddef>
|
|
|
|
#include <cstdint>
|
|
|
|
#include <iomanip>
|
|
|
|
#include <limits>
|
|
|
|
#include <numeric>
|
|
|
|
#include <tuple>
|
|
|
|
#include <utility>
|
|
|
|
|
|
|
|
#include <boost/range/algorithm/transform.hpp>
|
|
|
|
|
|
|
|
namespace osrm
|
|
|
|
{
|
|
|
|
namespace extractor
|
|
|
|
{
|
|
|
|
namespace guidance
|
|
|
|
{
|
|
|
|
|
|
|
|
namespace
|
|
|
|
{
|
|
|
|
// to use the corrected coordinate, we require it to be at least a bit further down the
|
|
|
|
// road than the offset coordinate. We postulate a minimum Distance of 2 Meters
|
|
|
|
const constexpr double DESIRED_COORDINATE_DIFFERENCE = 2.0;
|
|
|
|
// the default distance we lookahead on a road. This distance prevents small mapping
|
|
|
|
// errors to impact the turn angles.
|
|
|
|
const constexpr double LOOKAHEAD_DISTANCE_WITHOUT_LANES = 10.0;
|
|
|
|
// The standard with of a interstate highway is 3.7 meters. Local roads have
|
|
|
|
// smaller widths, ranging from 2.5 to 3.25 meters. As a compromise, we use
|
|
|
|
// the 3.25 here for our angle calculations
|
|
|
|
const constexpr double ASSUMED_LANE_WIDTH = 3.25;
|
2016-11-18 03:38:26 -05:00
|
|
|
const constexpr double FAR_LOOKAHEAD_DISTANCE = 40.0;
|
2016-08-17 03:49:19 -04:00
|
|
|
|
|
|
|
// The count of lanes assumed when no lanes are present. Since most roads will have lanes for both
|
|
|
|
// directions or a lane count specified, we use 2. Overestimating only makes our calculations safer,
|
|
|
|
// so we are fine for 1-lane ways. larger than 2 lanes should usually be specified in the data.
|
|
|
|
const constexpr std::uint16_t ASSUMED_LANE_COUNT = 2;
|
2016-11-29 04:23:13 -05:00
|
|
|
|
|
|
|
// When looking at lane offsets, motorway exits often are modelled not at a 90 degree angle but at
|
|
|
|
// some slight turn. To correctly detect these offsets, we need to allow for a bit more than just
|
|
|
|
// the lane width as an offset
|
|
|
|
double GetOffsetCorrectionFactor(const RoadClassification &road_classification)
|
|
|
|
{
|
|
|
|
if (road_classification.IsMotorwayClass() || road_classification.IsRampClass())
|
|
|
|
return 2.5;
|
|
|
|
switch (road_classification.GetClass())
|
|
|
|
{
|
|
|
|
case RoadPriorityClass::TRUNK:
|
|
|
|
return 2.0;
|
|
|
|
case RoadPriorityClass::PRIMARY:
|
|
|
|
return 1.5;
|
|
|
|
default:
|
|
|
|
return 1.0;
|
|
|
|
};
|
|
|
|
};
|
2016-08-17 03:49:19 -04:00
|
|
|
}
|
|
|
|
|
|
|
|
CoordinateExtractor::CoordinateExtractor(
|
|
|
|
const util::NodeBasedDynamicGraph &node_based_graph,
|
|
|
|
const extractor::CompressedEdgeContainer &compressed_geometries,
|
|
|
|
const std::vector<extractor::QueryNode> &node_coordinates)
|
|
|
|
: node_based_graph(node_based_graph), compressed_geometries(compressed_geometries),
|
|
|
|
node_coordinates(node_coordinates)
|
|
|
|
{
|
|
|
|
}
|
|
|
|
|
|
|
|
util::Coordinate
|
|
|
|
CoordinateExtractor::GetCoordinateAlongRoad(const NodeID intersection_node,
|
|
|
|
const EdgeID turn_edge,
|
|
|
|
const bool traversed_in_reverse,
|
|
|
|
const NodeID to_node,
|
|
|
|
const std::uint8_t intersection_lanes) const
|
|
|
|
{
|
|
|
|
// we first extract all coordinates from the road
|
|
|
|
auto coordinates =
|
|
|
|
GetCoordinatesAlongRoad(intersection_node, turn_edge, traversed_in_reverse, to_node);
|
|
|
|
|
2016-11-15 05:21:26 -05:00
|
|
|
return ExtractRepresentativeCoordinate(intersection_node,
|
|
|
|
turn_edge,
|
|
|
|
traversed_in_reverse,
|
|
|
|
to_node,
|
|
|
|
intersection_lanes,
|
|
|
|
std::move(coordinates));
|
|
|
|
}
|
|
|
|
|
|
|
|
util::Coordinate CoordinateExtractor::ExtractRepresentativeCoordinate(
|
|
|
|
const NodeID intersection_node,
|
|
|
|
const EdgeID turn_edge,
|
|
|
|
const bool traversed_in_reverse,
|
|
|
|
const NodeID to_node,
|
|
|
|
const std::uint8_t intersection_lanes,
|
|
|
|
std::vector<util::Coordinate> coordinates) const
|
|
|
|
{
|
2016-11-18 03:38:26 -05:00
|
|
|
const auto is_valid_result = [&](const util::Coordinate coordinate) {
|
|
|
|
return util::Coordinate(traversed_in_reverse
|
|
|
|
? node_coordinates[to_node]
|
|
|
|
: node_coordinates[intersection_node]) != coordinate;
|
|
|
|
};
|
|
|
|
// this is only used for debug purposes in assertions. We don't want warnings about it
|
|
|
|
(void)is_valid_result;
|
|
|
|
|
|
|
|
// the lane count might not always be set. We need to assume a positive number, though. Here we
|
|
|
|
// select the number of lanes to operate on
|
2016-11-15 05:21:26 -05:00
|
|
|
const auto considered_lanes =
|
2016-11-29 04:23:13 -05:00
|
|
|
GetOffsetCorrectionFactor(node_based_graph.GetEdgeData(turn_edge).road_classification) *
|
|
|
|
((intersection_lanes == 0) ? ASSUMED_LANE_COUNT : intersection_lanes);
|
2016-11-15 05:21:26 -05:00
|
|
|
|
2016-11-18 03:38:26 -05:00
|
|
|
// Fallback. These roads are small broken self-loops that shouldn't be in the data at all
|
|
|
|
if (intersection_node == to_node)
|
|
|
|
return coordinates[1];
|
|
|
|
|
2016-08-17 03:49:19 -04:00
|
|
|
/* if we are looking at a straight line, we don't care where exactly the coordinate
|
|
|
|
* is. Simply return the final coordinate. Turn angles/turn vectors are the same no matter which
|
|
|
|
* coordinate we look at.
|
|
|
|
*/
|
|
|
|
if (coordinates.size() <= 2)
|
2016-11-18 03:38:26 -05:00
|
|
|
{
|
|
|
|
// Here we can't check for validity, due to possible dead-ends with repeated coordinates
|
|
|
|
// BOOST_ASSERT(is_valid_result(coordinates.back()));
|
2016-08-17 03:49:19 -04:00
|
|
|
return coordinates.back();
|
2016-11-18 03:38:26 -05:00
|
|
|
}
|
2016-08-17 03:49:19 -04:00
|
|
|
|
2016-11-03 11:14:04 -04:00
|
|
|
// due to repeated coordinates / smaller offset errors we skip over the very first parts of the
|
|
|
|
// coordinate set to add a small level of fault tolerance
|
|
|
|
const constexpr double distance_to_skip_over_due_to_coordinate_inaccuracies = 2;
|
|
|
|
|
2016-08-17 03:49:19 -04:00
|
|
|
// fallback, mostly necessary for dead ends
|
|
|
|
if (intersection_node == to_node)
|
2016-11-18 03:38:26 -05:00
|
|
|
{
|
|
|
|
const auto result = ExtractCoordinateAtLength(
|
|
|
|
distance_to_skip_over_due_to_coordinate_inaccuracies, coordinates);
|
|
|
|
BOOST_ASSERT(is_valid_result(coordinates.back()));
|
|
|
|
return result;
|
|
|
|
}
|
2016-08-17 03:49:19 -04:00
|
|
|
|
|
|
|
// If this reduction leaves us with only two coordinates, the turns/angles are represented in a
|
|
|
|
// valid way. Only curved roads and other difficult scenarios will require multiple coordinates.
|
|
|
|
if (coordinates.size() == 2)
|
|
|
|
return coordinates.back();
|
|
|
|
|
|
|
|
const auto &turn_edge_data = node_based_graph.GetEdgeData(turn_edge);
|
2016-11-03 11:14:04 -04:00
|
|
|
|
|
|
|
// roundabouts, check early to avoid other costly checks
|
2016-11-29 04:23:13 -05:00
|
|
|
if (turn_edge_data.roundabout || turn_edge_data.circular)
|
2016-11-18 03:38:26 -05:00
|
|
|
{
|
|
|
|
const auto result = ExtractCoordinateAtLength(
|
|
|
|
distance_to_skip_over_due_to_coordinate_inaccuracies, coordinates);
|
|
|
|
BOOST_ASSERT(is_valid_result(result));
|
|
|
|
return result;
|
|
|
|
}
|
2016-11-03 11:14:04 -04:00
|
|
|
|
2016-08-17 03:49:19 -04:00
|
|
|
const util::Coordinate turn_coordinate =
|
|
|
|
node_coordinates[traversed_in_reverse ? to_node : intersection_node];
|
|
|
|
|
|
|
|
// Low priority roads are usually modelled very strangely. The roads are so small, though, that
|
|
|
|
// our basic heuristic looking at the road should be fine.
|
|
|
|
if (turn_edge_data.road_classification.IsLowPriorityRoadClass())
|
|
|
|
{
|
|
|
|
// Look ahead a tiny bit. Low priority road classes can be modelled fairly distinct in the
|
2016-11-03 11:14:04 -04:00
|
|
|
// very first part of the road. It's less accurate than searching for offsets but the models
|
|
|
|
// contained in OSM are just to strange to capture fully. Using the fallback here we try to
|
|
|
|
// do the best of what we can.
|
|
|
|
coordinates =
|
|
|
|
TrimCoordinatesToLength(std::move(coordinates), LOOKAHEAD_DISTANCE_WITHOUT_LANES);
|
2016-08-17 03:49:19 -04:00
|
|
|
if (coordinates.size() > 2 &&
|
|
|
|
util::coordinate_calculation::haversineDistance(turn_coordinate, coordinates[1]) <
|
|
|
|
ASSUMED_LANE_WIDTH)
|
2016-11-18 03:38:26 -05:00
|
|
|
{
|
|
|
|
const auto result =
|
|
|
|
GetCorrectedCoordinate(turn_coordinate, coordinates[1], coordinates.back());
|
|
|
|
BOOST_ASSERT(is_valid_result(result));
|
|
|
|
return result;
|
|
|
|
}
|
2016-08-17 03:49:19 -04:00
|
|
|
else
|
2016-11-18 03:38:26 -05:00
|
|
|
{
|
|
|
|
BOOST_ASSERT(is_valid_result(coordinates.back()));
|
2016-08-17 03:49:19 -04:00
|
|
|
return coordinates.back();
|
2016-11-18 03:38:26 -05:00
|
|
|
}
|
2016-08-17 03:49:19 -04:00
|
|
|
}
|
|
|
|
|
2016-11-03 11:14:04 -04:00
|
|
|
const auto first_distance =
|
|
|
|
util::coordinate_calculation::haversineDistance(coordinates[0], coordinates[1]);
|
|
|
|
|
|
|
|
/* if the very first coordinate along the road is reasonably far away from the road, we assume
|
|
|
|
* the coordinate to correctly represent the turn. This could probably be improved using
|
|
|
|
* information on the very first turn angle (requires knowledge about previous road) and the
|
|
|
|
* respective lane widths.
|
|
|
|
*/
|
|
|
|
const bool first_coordinate_is_far_away = [&first_distance, considered_lanes]() {
|
|
|
|
const auto required_distance =
|
|
|
|
considered_lanes * 0.5 * ASSUMED_LANE_WIDTH + LOOKAHEAD_DISTANCE_WITHOUT_LANES;
|
|
|
|
return first_distance > required_distance;
|
|
|
|
}();
|
|
|
|
|
|
|
|
if (first_coordinate_is_far_away)
|
|
|
|
{
|
2016-11-18 03:38:26 -05:00
|
|
|
BOOST_ASSERT(is_valid_result(coordinates[1]));
|
2016-11-03 11:14:04 -04:00
|
|
|
return coordinates[1];
|
|
|
|
}
|
|
|
|
|
|
|
|
// now, after the simple checks have succeeded make our further computations simpler
|
|
|
|
const auto lookahead_distance =
|
|
|
|
FAR_LOOKAHEAD_DISTANCE + considered_lanes * ASSUMED_LANE_WIDTH * 0.5;
|
|
|
|
|
2016-08-17 03:49:19 -04:00
|
|
|
/*
|
|
|
|
* The coordinates along the road are in different distances from the source. If only very few
|
|
|
|
* coordinates are close to the intersection, It might just be we simply looked to far down the
|
|
|
|
* road. We can decide to weight coordinates differently based on their distance from the
|
|
|
|
* intersection.
|
|
|
|
* In addition, changes very close to an intersection indicate graphical representation of the
|
|
|
|
* intersection over perceived turn angles.
|
|
|
|
*
|
|
|
|
* a -
|
|
|
|
* \
|
|
|
|
* -------------------- b
|
|
|
|
*
|
|
|
|
* Here the initial angle close to a might simply be due to OSM-Ways being located in the middle
|
|
|
|
* of the actual roads. If a road splits in two, the ways for the separate direction can be
|
|
|
|
* modeled very far apart with a steep angle at the split, even though the roads actually don't
|
|
|
|
* take a turn. The distance between the coordinates can be an indicator for these small changes
|
2016-11-03 11:14:04 -04:00
|
|
|
*
|
|
|
|
* Luckily, these segment distances are a byproduct of trimming
|
2016-08-17 03:49:19 -04:00
|
|
|
*/
|
2016-11-03 11:14:04 -04:00
|
|
|
auto segment_distances = PrepareLengthCache(coordinates, lookahead_distance);
|
|
|
|
coordinates =
|
|
|
|
TrimCoordinatesToLength(std::move(coordinates), lookahead_distance, segment_distances);
|
|
|
|
segment_distances.back() = std::min(segment_distances.back(), lookahead_distance);
|
|
|
|
BOOST_ASSERT(segment_distances.size() == coordinates.size());
|
2016-08-17 03:49:19 -04:00
|
|
|
|
2016-11-18 03:38:26 -05:00
|
|
|
const auto total_distance =
|
|
|
|
std::accumulate(segment_distances.begin(), segment_distances.end(), 0.);
|
|
|
|
|
|
|
|
// if we are now left with two, well than we don't have to worry, or the segment is very small
|
|
|
|
if (coordinates.size() == 2 ||
|
|
|
|
total_distance <= distance_to_skip_over_due_to_coordinate_inaccuracies)
|
|
|
|
{
|
|
|
|
BOOST_ASSERT(is_valid_result(coordinates.back()));
|
2016-11-03 11:14:04 -04:00
|
|
|
return coordinates.back();
|
2016-11-18 03:38:26 -05:00
|
|
|
}
|
2016-08-17 03:49:19 -04:00
|
|
|
|
|
|
|
const double max_deviation_from_straight = GetMaxDeviation(
|
|
|
|
coordinates.begin(), coordinates.end(), coordinates.front(), coordinates.back());
|
|
|
|
|
|
|
|
// if the deviation from a straight line is small, we can savely use the coordinate. We use half
|
|
|
|
// a lane as heuristic to determine if the road is straight enough.
|
|
|
|
if (max_deviation_from_straight < 0.5 * ASSUMED_LANE_WIDTH)
|
2016-11-18 03:38:26 -05:00
|
|
|
{
|
|
|
|
// At loops in traffic circles, we can have small deviations as well (if the circle is tiny)
|
|
|
|
// As a back-up, we have to check for this case
|
|
|
|
if (coordinates.front() == coordinates.back())
|
|
|
|
{
|
|
|
|
const auto result = ExtractCoordinateAtLength(
|
|
|
|
distance_to_skip_over_due_to_coordinate_inaccuracies, coordinates);
|
|
|
|
BOOST_ASSERT(is_valid_result(result));
|
|
|
|
return result;
|
|
|
|
}
|
|
|
|
else
|
|
|
|
{
|
|
|
|
BOOST_ASSERT(is_valid_result(coordinates.back()));
|
|
|
|
return coordinates.back();
|
|
|
|
}
|
|
|
|
}
|
2016-08-17 03:49:19 -04:00
|
|
|
|
|
|
|
/*
|
|
|
|
* if a road turns barely in the beginning, it is similar to the first coordinate being
|
|
|
|
* sufficiently far ahead.
|
|
|
|
* possible negative:
|
|
|
|
* http://www.openstreetmap.org/search?query=52.514503%2013.32252#map=19/52.51450/13.32252
|
|
|
|
*/
|
|
|
|
const auto straight_distance_and_index = [&]() {
|
|
|
|
auto straight_distance = segment_distances[1];
|
|
|
|
|
|
|
|
std::size_t index;
|
|
|
|
for (index = 2; index < coordinates.size(); ++index)
|
|
|
|
{
|
|
|
|
// check the deviation from a straight line
|
|
|
|
if (GetMaxDeviation(coordinates.begin(),
|
|
|
|
coordinates.begin() + index,
|
|
|
|
coordinates.front(),
|
|
|
|
*(coordinates.begin() + index)) < 0.25 * ASSUMED_LANE_WIDTH)
|
|
|
|
straight_distance += segment_distances[index];
|
|
|
|
else
|
|
|
|
break;
|
|
|
|
}
|
|
|
|
return std::make_pair(index - 1, straight_distance);
|
|
|
|
}();
|
|
|
|
const auto straight_distance = straight_distance_and_index.second;
|
|
|
|
const auto straight_index = straight_distance_and_index.first;
|
|
|
|
|
|
|
|
const bool starts_of_without_turn = [&]() {
|
|
|
|
return straight_distance >=
|
|
|
|
considered_lanes * 0.5 * ASSUMED_LANE_WIDTH + LOOKAHEAD_DISTANCE_WITHOUT_LANES;
|
|
|
|
}();
|
2016-11-03 11:14:04 -04:00
|
|
|
|
2016-08-17 03:49:19 -04:00
|
|
|
if (starts_of_without_turn)
|
|
|
|
{
|
|
|
|
// skip over repeated coordinates
|
2016-11-18 03:38:26 -05:00
|
|
|
const auto result = ExtractCoordinateAtLength(5, coordinates, segment_distances);
|
|
|
|
BOOST_ASSERT(is_valid_result(result));
|
|
|
|
return result;
|
2016-08-17 03:49:19 -04:00
|
|
|
}
|
|
|
|
|
|
|
|
// compute the regression vector based on the sum of least squares
|
|
|
|
const auto regression_line = RegressionLine(coordinates);
|
|
|
|
|
|
|
|
/*
|
|
|
|
* If we can find a line that represents the full set of coordinates within a certain range in
|
|
|
|
* relation to ASSUMED_LANE_WIDTH, we use the regression line to express the turn angle.
|
|
|
|
* This yields a transformation similar to:
|
|
|
|
*
|
|
|
|
* c d d
|
|
|
|
* b -> c
|
|
|
|
* b
|
|
|
|
* a a
|
|
|
|
*/
|
|
|
|
const double max_deviation_from_regression = GetMaxDeviation(
|
|
|
|
coordinates.begin(), coordinates.end(), regression_line.first, regression_line.second);
|
|
|
|
|
|
|
|
if (max_deviation_from_regression < 0.35 * ASSUMED_LANE_WIDTH)
|
|
|
|
{
|
|
|
|
// We use the locations on the regression line to offset the regression line onto the
|
|
|
|
// intersection.
|
|
|
|
const auto coord_between_front =
|
|
|
|
util::coordinate_calculation::projectPointOnSegment(
|
|
|
|
regression_line.first, regression_line.second, coordinates.front())
|
|
|
|
.second;
|
|
|
|
const auto coord_between_back =
|
|
|
|
util::coordinate_calculation::projectPointOnSegment(
|
|
|
|
regression_line.first, regression_line.second, coordinates.back())
|
|
|
|
.second;
|
2016-11-18 03:38:26 -05:00
|
|
|
const auto result =
|
|
|
|
GetCorrectedCoordinate(turn_coordinate, coord_between_front, coord_between_back);
|
|
|
|
BOOST_ASSERT(is_valid_result(result));
|
|
|
|
return result;
|
2016-08-17 03:49:19 -04:00
|
|
|
}
|
|
|
|
|
|
|
|
if (IsCurve(coordinates,
|
|
|
|
segment_distances,
|
|
|
|
total_distance,
|
|
|
|
considered_lanes * 0.5 * ASSUMED_LANE_WIDTH,
|
|
|
|
turn_edge_data))
|
|
|
|
{
|
2016-11-18 03:38:26 -05:00
|
|
|
if (total_distance <= distance_to_skip_over_due_to_coordinate_inaccuracies)
|
|
|
|
return coordinates.back();
|
2016-08-17 03:49:19 -04:00
|
|
|
/*
|
|
|
|
* In curves we now have to distinguish between larger curves and tiny curves modelling the
|
|
|
|
* actual turn in the beginnig.
|
|
|
|
*
|
|
|
|
* We distinguish between turns that simply model the initial way of getting onto the
|
|
|
|
* destination lanes and the ones that performa a larger turn.
|
|
|
|
*/
|
2016-11-18 03:38:26 -05:00
|
|
|
const double offset =
|
|
|
|
std::min(0.5 * considered_lanes * ASSUMED_LANE_WIDTH, 0.2 * segment_distances.back());
|
2016-11-03 11:14:04 -04:00
|
|
|
coordinates = TrimCoordinatesToLength(std::move(coordinates), offset, segment_distances);
|
2016-11-22 10:20:54 -05:00
|
|
|
BOOST_ASSERT(coordinates.size() >= 2);
|
2016-11-03 11:14:04 -04:00
|
|
|
segment_distances.resize(coordinates.size());
|
2016-11-22 10:20:54 -05:00
|
|
|
segment_distances.back() = util::coordinate_calculation::haversineDistance(
|
|
|
|
*(coordinates.end() - 2), coordinates.back());
|
2016-08-17 03:49:19 -04:00
|
|
|
const auto vector_head = coordinates.back();
|
2016-11-03 11:14:04 -04:00
|
|
|
coordinates =
|
|
|
|
TrimCoordinatesToLength(std::move(coordinates), 0.5 * offset, segment_distances);
|
2016-08-17 03:49:19 -04:00
|
|
|
BOOST_ASSERT(coordinates.size() >= 2);
|
2016-11-18 03:38:26 -05:00
|
|
|
const auto result =
|
|
|
|
GetCorrectedCoordinate(turn_coordinate, coordinates.back(), vector_head);
|
|
|
|
BOOST_ASSERT(is_valid_result(result));
|
|
|
|
return result;
|
2016-08-17 03:49:19 -04:00
|
|
|
}
|
|
|
|
|
2016-11-03 11:14:04 -04:00
|
|
|
if (IsDirectOffset(coordinates,
|
|
|
|
straight_index,
|
|
|
|
straight_distance,
|
|
|
|
total_distance,
|
|
|
|
segment_distances,
|
|
|
|
considered_lanes))
|
|
|
|
{
|
|
|
|
// could be too agressive? Depend on lanes to check how far we want to go out?
|
|
|
|
// compare
|
|
|
|
// http://www.openstreetmap.org/search?query=52.411243%2013.363575#map=19/52.41124/13.36357
|
|
|
|
const auto offset_index = std::max<decltype(straight_index)>(1, straight_index);
|
2016-11-18 03:38:26 -05:00
|
|
|
const auto result = GetCorrectedCoordinate(
|
2016-11-03 11:14:04 -04:00
|
|
|
turn_coordinate, coordinates[offset_index], coordinates[offset_index + 1]);
|
2016-11-18 03:38:26 -05:00
|
|
|
|
|
|
|
BOOST_ASSERT(is_valid_result(result));
|
|
|
|
return result;
|
2016-11-03 11:14:04 -04:00
|
|
|
}
|
|
|
|
|
2016-08-17 03:49:19 -04:00
|
|
|
{
|
|
|
|
// skip over the first coordinates, in specific the assumed lane count. We add a small
|
|
|
|
// safety factor, to not overshoot on the regression
|
2016-10-25 10:38:45 -04:00
|
|
|
const auto trimming_length = 0.8 * (considered_lanes * ASSUMED_LANE_WIDTH);
|
|
|
|
const auto trimmed_coordinates =
|
|
|
|
TrimCoordinatesByLengthFront(coordinates, 0.8 * trimming_length);
|
|
|
|
if (trimmed_coordinates.size() >= 2 && (total_distance >= trimming_length + 2))
|
2016-08-17 03:49:19 -04:00
|
|
|
{
|
|
|
|
// get the regression line
|
|
|
|
const auto regression_line_trimmed = RegressionLine(trimmed_coordinates);
|
|
|
|
|
|
|
|
// and compute the maximum deviation from it
|
|
|
|
const auto max_deviation_from_trimmed_regression =
|
|
|
|
GetMaxDeviation(trimmed_coordinates.begin(),
|
|
|
|
trimmed_coordinates.end(),
|
|
|
|
regression_line_trimmed.first,
|
|
|
|
regression_line_trimmed.second);
|
|
|
|
|
|
|
|
if (max_deviation_from_trimmed_regression < 0.5 * ASSUMED_LANE_WIDTH)
|
2016-11-18 03:38:26 -05:00
|
|
|
{
|
|
|
|
const auto result = GetCorrectedCoordinate(
|
2016-08-17 03:49:19 -04:00
|
|
|
turn_coordinate, regression_line_trimmed.first, regression_line_trimmed.second);
|
2016-11-18 03:38:26 -05:00
|
|
|
BOOST_ASSERT(is_valid_result(result));
|
|
|
|
return result;
|
|
|
|
}
|
2016-08-17 03:49:19 -04:00
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
// We use the locations on the regression line to offset the regression line onto the
|
|
|
|
// intersection.
|
2016-11-18 03:38:26 -05:00
|
|
|
const auto result =
|
|
|
|
ExtractCoordinateAtLength(LOOKAHEAD_DISTANCE_WITHOUT_LANES, coordinates, segment_distances);
|
|
|
|
BOOST_ASSERT(is_valid_result(result));
|
|
|
|
return result;
|
|
|
|
}
|
|
|
|
|
|
|
|
util::Coordinate
|
|
|
|
CoordinateExtractor::ExtractCoordinateAtLength(const double distance,
|
|
|
|
const std::vector<util::Coordinate> &coordinates,
|
|
|
|
const std::vector<double> &length_cache) const
|
|
|
|
{
|
|
|
|
BOOST_ASSERT(length_cache.size() == coordinates.size());
|
|
|
|
BOOST_ASSERT(!coordinates.empty());
|
|
|
|
|
|
|
|
double accumulated_distance = 0.;
|
|
|
|
auto length_cache_itr = length_cache.begin() + 1;
|
|
|
|
// find the end of the segment containing the coordinate which is at least distance away
|
|
|
|
const auto find_coordinate_at_distance = [distance, &accumulated_distance, &length_cache_itr](
|
|
|
|
const util::Coordinate /*coordinate*/) mutable {
|
|
|
|
const auto result = (accumulated_distance + *length_cache_itr) >= distance;
|
|
|
|
if (!result)
|
|
|
|
{
|
|
|
|
accumulated_distance += *length_cache_itr;
|
|
|
|
++length_cache_itr;
|
|
|
|
}
|
|
|
|
return result;
|
|
|
|
};
|
|
|
|
|
|
|
|
// find the beginning fo the segment (begin here and above for the length cache need to match
|
|
|
|
// up!)
|
|
|
|
const auto coordinate_base =
|
|
|
|
std::find_if(coordinates.begin() + 1, coordinates.end(), find_coordinate_at_distance) - 1;
|
|
|
|
|
|
|
|
if (static_cast<std::size_t>(std::distance(coordinates.begin(), coordinate_base) + 1) >=
|
|
|
|
coordinates.size())
|
|
|
|
return coordinates.back();
|
|
|
|
|
|
|
|
const auto interpolation_factor =
|
|
|
|
ComputeInterpolationFactor(distance - accumulated_distance, 0, *length_cache_itr);
|
|
|
|
|
|
|
|
return util::coordinate_calculation::interpolateLinear(
|
|
|
|
interpolation_factor, *coordinate_base, *(coordinate_base + 1));
|
|
|
|
}
|
|
|
|
|
|
|
|
util::Coordinate CoordinateExtractor::ExtractCoordinateAtLength(
|
|
|
|
const double distance, const std::vector<util::Coordinate> &coordinates) const
|
|
|
|
{
|
|
|
|
BOOST_ASSERT(!coordinates.empty());
|
|
|
|
double accumulated_distance = 0.;
|
|
|
|
// checks (via its state) for an accumulated distance
|
|
|
|
const auto coordinate_at_distance =
|
|
|
|
[ distance, &accumulated_distance, last_coordinate = coordinates.front() ](
|
|
|
|
const util::Coordinate coordinate) mutable
|
|
|
|
{
|
|
|
|
const double segment_distance =
|
|
|
|
util::coordinate_calculation::haversineDistance(last_coordinate, coordinate);
|
|
|
|
const auto result = (accumulated_distance + segment_distance) >= distance;
|
|
|
|
if (!result)
|
|
|
|
accumulated_distance += segment_distance;
|
|
|
|
|
|
|
|
return result;
|
|
|
|
};
|
|
|
|
|
|
|
|
// find the begin of the segment containing the coordinate
|
|
|
|
const auto coordinate_base =
|
|
|
|
std::find_if(coordinates.begin() + 1, coordinates.end(), coordinate_at_distance) - 1;
|
|
|
|
|
|
|
|
if (static_cast<std::size_t>(std::distance(coordinates.begin(), coordinate_base) + 1) >=
|
|
|
|
coordinates.size())
|
|
|
|
return coordinates.back();
|
|
|
|
|
|
|
|
const auto interpolation_factor = ComputeInterpolationFactor(
|
|
|
|
distance - accumulated_distance,
|
|
|
|
0,
|
|
|
|
util::coordinate_calculation::haversineDistance(*coordinate_base, *(coordinate_base + 1)));
|
|
|
|
|
|
|
|
return util::coordinate_calculation::interpolateLinear(
|
|
|
|
interpolation_factor, *coordinate_base, *(coordinate_base + 1));
|
2016-08-17 03:49:19 -04:00
|
|
|
}
|
|
|
|
|
2016-11-10 10:05:24 -05:00
|
|
|
util::Coordinate CoordinateExtractor::GetCoordinateCloseToTurn(const NodeID from_node,
|
|
|
|
const EdgeID turn_edge,
|
|
|
|
const bool traversed_in_reverse,
|
|
|
|
const NodeID to_node) const
|
|
|
|
{
|
|
|
|
const auto end_node = traversed_in_reverse ? from_node : to_node;
|
|
|
|
const auto start_node = traversed_in_reverse ? to_node : from_node;
|
|
|
|
if (!compressed_geometries.HasEntryForID(turn_edge))
|
|
|
|
return node_coordinates[end_node];
|
|
|
|
else
|
|
|
|
{
|
|
|
|
const auto &geometry = compressed_geometries.GetBucketReference(turn_edge);
|
|
|
|
|
|
|
|
// the compressed edges contain node ids, we transfer them to coordinates accessing the
|
|
|
|
// node_coordinates array
|
|
|
|
const auto compressedGeometryToCoordinate =
|
|
|
|
[this](const CompressedEdgeContainer::OnewayCompressedEdge &compressed_edge) {
|
|
|
|
return node_coordinates[compressed_edge.node_id];
|
|
|
|
};
|
|
|
|
|
|
|
|
// return the first coordinate that is reasonably far away from the start node
|
|
|
|
const util::Coordinate start_coordinate = node_coordinates[start_node];
|
|
|
|
|
|
|
|
// OSM data has a tendency to include repeated nodes with identical coordinates. To skip
|
|
|
|
// over these, we search for the first coordinate along the path that is at least a meter
|
|
|
|
// away from the first entry
|
|
|
|
const auto far_enough_away = [start_coordinate, compressedGeometryToCoordinate](
|
|
|
|
const CompressedEdgeContainer::OnewayCompressedEdge &compressed_edge) {
|
|
|
|
return util::coordinate_calculation::haversineDistance(
|
|
|
|
compressedGeometryToCoordinate(compressed_edge), start_coordinate) > 1;
|
|
|
|
};
|
|
|
|
|
|
|
|
// find the first coordinate, that is at least unequal to the begin of the edge
|
|
|
|
if (traversed_in_reverse)
|
|
|
|
{
|
|
|
|
const auto far_enough =
|
|
|
|
std::find_if(geometry.rbegin(), geometry.rend(), far_enough_away);
|
|
|
|
return (far_enough != geometry.rend()) ? compressedGeometryToCoordinate(*far_enough)
|
|
|
|
: node_coordinates[end_node];
|
|
|
|
}
|
|
|
|
else
|
|
|
|
{
|
|
|
|
const auto far_enough = std::find_if(geometry.begin(), geometry.end(), far_enough_away);
|
|
|
|
return (far_enough != geometry.end()) ? compressedGeometryToCoordinate(*far_enough)
|
|
|
|
: node_coordinates[end_node];
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2016-10-20 06:15:36 -04:00
|
|
|
std::vector<util::Coordinate>
|
|
|
|
CoordinateExtractor::GetForwardCoordinatesAlongRoad(const NodeID from, const EdgeID turn_edge) const
|
|
|
|
{
|
|
|
|
return GetCoordinatesAlongRoad(from, turn_edge, false, node_based_graph.GetTarget(turn_edge));
|
|
|
|
}
|
|
|
|
|
2016-08-17 03:49:19 -04:00
|
|
|
std::vector<util::Coordinate>
|
|
|
|
CoordinateExtractor::GetCoordinatesAlongRoad(const NodeID intersection_node,
|
|
|
|
const EdgeID turn_edge,
|
|
|
|
const bool traversed_in_reverse,
|
|
|
|
const NodeID to_node) const
|
|
|
|
{
|
|
|
|
if (!compressed_geometries.HasEntryForID(turn_edge))
|
|
|
|
{
|
|
|
|
if (traversed_in_reverse)
|
|
|
|
return {{node_coordinates[to_node]}, {node_coordinates[intersection_node]}};
|
|
|
|
else
|
|
|
|
return {{node_coordinates[intersection_node]}, {node_coordinates[to_node]}};
|
|
|
|
}
|
|
|
|
else
|
|
|
|
{
|
|
|
|
// extracts the geometry in coordinates from the compressed edge container
|
|
|
|
std::vector<util::Coordinate> result;
|
|
|
|
const auto &geometry = compressed_geometries.GetBucketReference(turn_edge);
|
|
|
|
result.reserve(geometry.size() + 2);
|
|
|
|
|
|
|
|
// the compressed edges contain node ids, we transfer them to coordinates accessing the
|
|
|
|
// node_coordinates array
|
|
|
|
const auto compressedGeometryToCoordinate =
|
|
|
|
[this](const CompressedEdgeContainer::OnewayCompressedEdge &compressed_edge)
|
|
|
|
-> util::Coordinate { return node_coordinates[compressed_edge.node_id]; };
|
|
|
|
|
|
|
|
// add the coordinates to the result in either normal or reversed order, based on
|
|
|
|
// traversed_in_reverse
|
|
|
|
if (traversed_in_reverse)
|
|
|
|
{
|
|
|
|
std::transform(geometry.rbegin(),
|
|
|
|
geometry.rend(),
|
|
|
|
std::back_inserter(result),
|
|
|
|
compressedGeometryToCoordinate);
|
2016-11-15 05:21:26 -05:00
|
|
|
BOOST_ASSERT(intersection_node < node_coordinates.size());
|
2016-08-17 03:49:19 -04:00
|
|
|
result.push_back(node_coordinates[intersection_node]);
|
|
|
|
}
|
|
|
|
else
|
|
|
|
{
|
2016-11-15 05:21:26 -05:00
|
|
|
BOOST_ASSERT(intersection_node < node_coordinates.size());
|
2016-08-17 03:49:19 -04:00
|
|
|
result.push_back(node_coordinates[intersection_node]);
|
|
|
|
std::transform(geometry.begin(),
|
|
|
|
geometry.end(),
|
|
|
|
std::back_inserter(result),
|
|
|
|
compressedGeometryToCoordinate);
|
|
|
|
}
|
|
|
|
return result;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
double
|
|
|
|
CoordinateExtractor::GetMaxDeviation(std::vector<util::Coordinate>::const_iterator range_begin,
|
|
|
|
const std::vector<util::Coordinate>::const_iterator &range_end,
|
|
|
|
const util::Coordinate straight_begin,
|
|
|
|
const util::Coordinate straight_end) const
|
|
|
|
{
|
|
|
|
// compute the deviation of a single coordinate from a straight line
|
|
|
|
auto get_single_deviation = [&](const util::Coordinate coordinate) {
|
|
|
|
// find the projected coordinate
|
|
|
|
auto coord_between = util::coordinate_calculation::projectPointOnSegment(
|
|
|
|
straight_begin, straight_end, coordinate)
|
|
|
|
.second;
|
|
|
|
// and calculate the distance between the intermediate coordinate and the coordinate
|
|
|
|
// on the osrm-way
|
|
|
|
return util::coordinate_calculation::haversineDistance(coord_between, coordinate);
|
|
|
|
};
|
|
|
|
|
|
|
|
// note: we don't accumulate here but rather compute the maximum. The functor passed here is not
|
|
|
|
// summing up anything.
|
|
|
|
return std::accumulate(
|
|
|
|
range_begin, range_end, 0.0, [&](const double current, const util::Coordinate coordinate) {
|
|
|
|
return std::max(current, get_single_deviation(coordinate));
|
|
|
|
});
|
2016-10-27 14:02:21 -04:00
|
|
|
}
|
2016-08-17 03:49:19 -04:00
|
|
|
|
|
|
|
bool CoordinateExtractor::IsCurve(const std::vector<util::Coordinate> &coordinates,
|
|
|
|
const std::vector<double> &segment_distances,
|
|
|
|
const double segment_length,
|
|
|
|
const double considered_lane_width,
|
|
|
|
const util::NodeBasedEdgeData &edge_data) const
|
|
|
|
{
|
|
|
|
BOOST_ASSERT(coordinates.size() > 2);
|
|
|
|
|
|
|
|
// by default, we treat roundabout as curves
|
|
|
|
if (edge_data.roundabout)
|
|
|
|
return true;
|
|
|
|
|
|
|
|
// TODO we might have to fix this to better compensate for errors due to repeated coordinates
|
|
|
|
const bool takes_an_actual_turn = [&coordinates]() {
|
|
|
|
const auto begin_bearing =
|
|
|
|
util::coordinate_calculation::bearing(coordinates[0], coordinates[1]);
|
|
|
|
const auto end_bearing = util::coordinate_calculation::bearing(
|
|
|
|
coordinates[coordinates.size() - 2], coordinates[coordinates.size() - 1]);
|
|
|
|
|
|
|
|
const auto total_angle = angularDeviation(begin_bearing, end_bearing);
|
|
|
|
return total_angle > 0.5 * NARROW_TURN_ANGLE;
|
|
|
|
}();
|
|
|
|
|
|
|
|
if (!takes_an_actual_turn)
|
|
|
|
return false;
|
|
|
|
|
|
|
|
const auto get_deviation = [](const util::Coordinate line_start,
|
|
|
|
const util::Coordinate line_end,
|
|
|
|
const util::Coordinate point) {
|
|
|
|
// find the projected coordinate
|
|
|
|
auto coord_between =
|
|
|
|
util::coordinate_calculation::projectPointOnSegment(line_start, line_end, point).second;
|
|
|
|
// and calculate the distance between the intermediate coordinate and the coordinate
|
|
|
|
return util::coordinate_calculation::haversineDistance(coord_between, point);
|
|
|
|
};
|
|
|
|
|
|
|
|
// a curve needs to be on one side of the coordinate array
|
|
|
|
const bool all_same_side = [&]() {
|
|
|
|
if (coordinates.size() <= 3)
|
|
|
|
return true;
|
|
|
|
|
|
|
|
const bool ccw = util::coordinate_calculation::isCCW(
|
|
|
|
coordinates.front(), coordinates.back(), coordinates[1]);
|
|
|
|
|
|
|
|
return std::all_of(
|
|
|
|
coordinates.begin() + 2, coordinates.end() - 1, [&](const util::Coordinate coordinate) {
|
|
|
|
const bool compare_ccw = util::coordinate_calculation::isCCW(
|
|
|
|
coordinates.front(), coordinates.back(), coordinate);
|
|
|
|
return ccw == compare_ccw;
|
|
|
|
});
|
|
|
|
}();
|
|
|
|
|
|
|
|
if (!all_same_side)
|
|
|
|
return false;
|
|
|
|
|
|
|
|
// check if the deviation is a sequence that increases up to a maximum deviation and decreses
|
|
|
|
// after, following what we would expect from a modelled curve
|
|
|
|
bool has_up_down_deviation = false;
|
|
|
|
std::size_t maximum_deviation_index = 0;
|
|
|
|
double maximum_deviation = 0;
|
|
|
|
|
|
|
|
std::tie(has_up_down_deviation, maximum_deviation_index, maximum_deviation) =
|
|
|
|
[&coordinates, get_deviation]() -> std::tuple<bool, std::size_t, double> {
|
|
|
|
const auto increasing = [&](const util::Coordinate lhs, const util::Coordinate rhs) {
|
|
|
|
return get_deviation(coordinates.front(), coordinates.back(), lhs) <=
|
|
|
|
get_deviation(coordinates.front(), coordinates.back(), rhs);
|
|
|
|
};
|
|
|
|
|
|
|
|
const auto decreasing = [&](const util::Coordinate lhs, const util::Coordinate rhs) {
|
|
|
|
return get_deviation(coordinates.front(), coordinates.back(), lhs) >=
|
|
|
|
get_deviation(coordinates.front(), coordinates.back(), rhs);
|
|
|
|
};
|
|
|
|
|
|
|
|
if (coordinates.size() < 3)
|
|
|
|
return std::make_tuple(true, 0, 0.);
|
|
|
|
|
|
|
|
if (coordinates.size() == 3)
|
|
|
|
return std::make_tuple(
|
|
|
|
true, 1, get_deviation(coordinates.front(), coordinates.back(), coordinates[1]));
|
|
|
|
|
|
|
|
const auto maximum_itr =
|
|
|
|
std::is_sorted_until(coordinates.begin() + 1, coordinates.end(), increasing);
|
|
|
|
|
|
|
|
if (maximum_itr == coordinates.end())
|
|
|
|
return std::make_tuple(true, coordinates.size() - 1, 0.);
|
|
|
|
else if (std::is_sorted(maximum_itr, coordinates.end(), decreasing))
|
|
|
|
return std::make_tuple(
|
|
|
|
true,
|
|
|
|
std::distance(coordinates.begin(), maximum_itr),
|
|
|
|
get_deviation(coordinates.front(), coordinates.back(), *maximum_itr));
|
|
|
|
else
|
|
|
|
return std::make_tuple(false, 0, 0.);
|
|
|
|
}();
|
|
|
|
|
|
|
|
// a curve has increasing deviation from its front/back vertices to a certain point and after it
|
|
|
|
// only decreases
|
|
|
|
if (!has_up_down_deviation)
|
|
|
|
return false;
|
|
|
|
|
|
|
|
// if the maximum deviation is at a quarter of the total curve, we are probably looking at a
|
|
|
|
// normal turn
|
|
|
|
const auto distance_to_max_deviation = std::accumulate(
|
|
|
|
segment_distances.begin(), segment_distances.begin() + maximum_deviation_index, 0.);
|
|
|
|
|
|
|
|
if ((distance_to_max_deviation <= 0.35 * segment_length ||
|
|
|
|
maximum_deviation < std::max(0.3 * considered_lane_width, 0.5 * ASSUMED_LANE_WIDTH)) &&
|
2016-11-03 11:14:04 -04:00
|
|
|
segment_length > LOOKAHEAD_DISTANCE_WITHOUT_LANES)
|
2016-08-17 03:49:19 -04:00
|
|
|
return false;
|
|
|
|
|
|
|
|
BOOST_ASSERT(coordinates.size() >= 3);
|
|
|
|
// Compute all turn angles along the road
|
|
|
|
const auto turn_angles = [coordinates]() {
|
|
|
|
std::vector<double> turn_angles;
|
|
|
|
turn_angles.reserve(coordinates.size() - 2);
|
|
|
|
for (std::size_t index = 0; index + 2 < coordinates.size(); ++index)
|
|
|
|
{
|
|
|
|
turn_angles.push_back(util::coordinate_calculation::computeAngle(
|
|
|
|
coordinates[index], coordinates[index + 1], coordinates[index + 2]));
|
|
|
|
}
|
|
|
|
return turn_angles;
|
|
|
|
}();
|
|
|
|
|
|
|
|
const bool curve_is_valid =
|
|
|
|
[&turn_angles, &segment_distances, &segment_length, &considered_lane_width]() {
|
|
|
|
// internal state for our lamdae
|
|
|
|
bool last_was_straight = false;
|
|
|
|
// a turn angle represents two segments between three coordinates. We initialize the
|
|
|
|
// distance with the very first segment length (in-segment) of the first turn-angle
|
|
|
|
double straight_distance = std::max(0., segment_distances[1] - considered_lane_width);
|
|
|
|
auto distance_itr = segment_distances.begin() + 1;
|
|
|
|
|
|
|
|
// every call to the lamda requires a call to the distances. They need to be aligned
|
|
|
|
BOOST_ASSERT(segment_distances.size() == turn_angles.size() + 2);
|
|
|
|
|
|
|
|
const auto detect_invalid_curve = [&](const double previous_angle,
|
|
|
|
const double current_angle) {
|
|
|
|
const auto both_actually_turn =
|
|
|
|
(angularDeviation(previous_angle, STRAIGHT_ANGLE) > FUZZY_ANGLE_DIFFERENCE) &&
|
|
|
|
(angularDeviation(current_angle, STRAIGHT_ANGLE) > FUZZY_ANGLE_DIFFERENCE);
|
|
|
|
// they cannot be straight, since they differ at least by FUZZY_ANGLE_DIFFERENCE
|
|
|
|
const auto turn_direction_switches =
|
|
|
|
(previous_angle > STRAIGHT_ANGLE) == (current_angle < STRAIGHT_ANGLE);
|
|
|
|
|
|
|
|
// a turn that switches direction mid-curve is not a valid curve
|
|
|
|
if (both_actually_turn && turn_direction_switches)
|
|
|
|
return true;
|
|
|
|
|
|
|
|
const bool is_straight = angularDeviation(current_angle, STRAIGHT_ANGLE) < 5;
|
|
|
|
++distance_itr;
|
|
|
|
if (is_straight)
|
|
|
|
{
|
|
|
|
// since the angle is straight, we augment it by the second part of the segment
|
|
|
|
straight_distance += *distance_itr;
|
|
|
|
if (last_was_straight && straight_distance > 0.3 * segment_length)
|
|
|
|
{
|
|
|
|
return true;
|
|
|
|
}
|
|
|
|
} // if a segment on its own is long enough, thats fair game as well
|
|
|
|
else if (straight_distance > 0.3 * segment_length)
|
|
|
|
return true;
|
|
|
|
else
|
|
|
|
{
|
|
|
|
// we reset the last distance, starting with the next in-segment again
|
|
|
|
straight_distance = *distance_itr;
|
|
|
|
}
|
|
|
|
last_was_straight = is_straight;
|
|
|
|
return false;
|
|
|
|
};
|
|
|
|
|
|
|
|
const auto end_of_straight_segment =
|
|
|
|
std::adjacent_find(turn_angles.begin(), turn_angles.end(), detect_invalid_curve);
|
|
|
|
|
|
|
|
// No curve should have a very long straight segment
|
|
|
|
return end_of_straight_segment == turn_angles.end();
|
|
|
|
}();
|
|
|
|
|
|
|
|
return (segment_length > 2 * considered_lane_width && curve_is_valid);
|
|
|
|
}
|
|
|
|
|
|
|
|
bool CoordinateExtractor::IsDirectOffset(const std::vector<util::Coordinate> &coordinates,
|
|
|
|
const std::size_t straight_index,
|
|
|
|
const double straight_distance,
|
|
|
|
const double segment_length,
|
|
|
|
const std::vector<double> &segment_distances,
|
|
|
|
const std::uint8_t considered_lanes) const
|
|
|
|
{
|
|
|
|
// check if a given length is with half a lane of the assumed lane offset
|
|
|
|
const auto IsCloseToLaneDistance = [considered_lanes](const double width) {
|
|
|
|
// a road usually is connected to the middle of the lanes. So the lane-offset has to
|
|
|
|
// consider half to road
|
|
|
|
const auto lane_offset = 0.5 * considered_lanes * ASSUMED_LANE_WIDTH;
|
2016-11-29 04:23:13 -05:00
|
|
|
return width - lane_offset < ASSUMED_LANE_WIDTH; // less or going over at most a small bit
|
2016-08-17 03:49:19 -04:00
|
|
|
};
|
|
|
|
|
|
|
|
// Check whether the very first coordinate is simply an offset. This is the case if the initial
|
|
|
|
// vertex is close to the turn and the remaining coordinates are nearly straight.
|
|
|
|
const auto offset_index = std::max<decltype(straight_index)>(1, straight_index);
|
|
|
|
|
|
|
|
// we need at least a single coordinate
|
|
|
|
if (offset_index + 1 >= coordinates.size())
|
|
|
|
return false;
|
|
|
|
|
2016-11-29 04:23:13 -05:00
|
|
|
BOOST_ASSERT(segment_distances.size() == coordinates.size());
|
2016-08-17 03:49:19 -04:00
|
|
|
// the straight part has to be around the lane distance
|
2016-11-29 04:23:13 -05:00
|
|
|
if (!IsCloseToLaneDistance(std::accumulate(
|
|
|
|
segment_distances.begin(), segment_distances.begin() + offset_index + 1, 0.)))
|
2016-08-17 03:49:19 -04:00
|
|
|
return false;
|
|
|
|
|
|
|
|
// the segment itself cannot be short
|
2016-11-18 03:38:26 -05:00
|
|
|
if (segment_length < 0.4 * FAR_LOOKAHEAD_DISTANCE)
|
2016-08-17 03:49:19 -04:00
|
|
|
return false;
|
|
|
|
|
|
|
|
// if the remaining segment is short, we don't consider it an offset
|
2016-11-29 04:23:13 -05:00
|
|
|
if ((segment_length - std::max(straight_distance, segment_distances[1])) < 0.1 * segment_length)
|
2016-08-17 03:49:19 -04:00
|
|
|
return false;
|
|
|
|
|
2016-11-29 04:23:13 -05:00
|
|
|
// when we compare too long a distance, we run into problems due to turning roads. Here we
|
|
|
|
// compute which index to consider when checking if the remaining road remains straight
|
|
|
|
const auto segment_offset_past_thirty_meters =
|
|
|
|
std::find_if(segment_distances.begin() + offset_index,
|
|
|
|
segment_distances.end(),
|
|
|
|
[accumulated_distance = 0.](const auto value) mutable {
|
|
|
|
accumulated_distance += value;
|
|
|
|
return value >= 30;
|
|
|
|
});
|
|
|
|
|
|
|
|
// transform the found offset in the segment distances into the appropriate part in the
|
|
|
|
// coordinates array
|
|
|
|
const auto deviation_compare_end =
|
|
|
|
coordinates.begin() +
|
|
|
|
std::min<std::size_t>(
|
|
|
|
coordinates.size(), // don't go over
|
|
|
|
std::distance(segment_distances.begin(), segment_offset_past_thirty_meters) + 1);
|
|
|
|
|
2016-08-17 03:49:19 -04:00
|
|
|
// finally, we cannot be far off from a straight line for the remaining coordinates
|
|
|
|
return 0.5 * ASSUMED_LANE_WIDTH > GetMaxDeviation(coordinates.begin() + offset_index,
|
2016-11-29 04:23:13 -05:00
|
|
|
deviation_compare_end,
|
2016-08-17 03:49:19 -04:00
|
|
|
coordinates[offset_index],
|
2016-11-29 04:23:13 -05:00
|
|
|
*(deviation_compare_end - 1));
|
2016-08-17 03:49:19 -04:00
|
|
|
}
|
|
|
|
|
2016-11-03 11:14:04 -04:00
|
|
|
std::vector<double>
|
|
|
|
CoordinateExtractor::PrepareLengthCache(const std::vector<util::Coordinate> &coordinates,
|
|
|
|
const double limit) const
|
|
|
|
{
|
|
|
|
BOOST_ASSERT(!coordinates.empty());
|
|
|
|
BOOST_ASSERT(limit >= 0);
|
|
|
|
std::vector<double> segment_distances;
|
|
|
|
segment_distances.reserve(coordinates.size());
|
|
|
|
segment_distances.push_back(0);
|
|
|
|
// sentinel
|
2016-11-18 03:38:26 -05:00
|
|
|
std::find_if(std::next(std::begin(coordinates)), std::end(coordinates), [
|
|
|
|
last_coordinate = coordinates.front(),
|
|
|
|
limit,
|
|
|
|
&segment_distances,
|
|
|
|
accumulated_distance = 0.
|
|
|
|
](const util::Coordinate current_coordinate) mutable {
|
|
|
|
const auto distance =
|
|
|
|
util::coordinate_calculation::haversineDistance(last_coordinate, current_coordinate);
|
|
|
|
accumulated_distance += distance;
|
|
|
|
last_coordinate = current_coordinate;
|
|
|
|
segment_distances.push_back(distance);
|
|
|
|
return accumulated_distance >= limit;
|
|
|
|
});
|
2016-11-03 11:14:04 -04:00
|
|
|
return segment_distances;
|
|
|
|
}
|
|
|
|
|
2016-08-17 03:49:19 -04:00
|
|
|
std::vector<util::Coordinate>
|
|
|
|
CoordinateExtractor::TrimCoordinatesToLength(std::vector<util::Coordinate> coordinates,
|
2016-11-03 11:14:04 -04:00
|
|
|
const double desired_length,
|
|
|
|
const std::vector<double> &length_cache) const
|
2016-08-17 03:49:19 -04:00
|
|
|
{
|
|
|
|
BOOST_ASSERT(coordinates.size() >= 2);
|
2016-11-03 11:14:04 -04:00
|
|
|
BOOST_ASSERT(desired_length >= 0);
|
|
|
|
|
2016-08-17 03:49:19 -04:00
|
|
|
double distance_to_current_coordinate = 0;
|
2016-11-03 11:14:04 -04:00
|
|
|
std::size_t coordinate_index = 0;
|
|
|
|
|
|
|
|
const auto compute_length =
|
|
|
|
[&coordinate_index, &distance_to_current_coordinate, &coordinates]() {
|
|
|
|
const auto new_distance =
|
|
|
|
distance_to_current_coordinate +
|
|
|
|
util::coordinate_calculation::haversineDistance(coordinates[coordinate_index - 1],
|
|
|
|
coordinates[coordinate_index]);
|
|
|
|
return new_distance;
|
|
|
|
};
|
|
|
|
|
|
|
|
const auto read_length_from_cache = [&length_cache, &coordinate_index]() {
|
|
|
|
return length_cache[coordinate_index];
|
|
|
|
};
|
|
|
|
|
|
|
|
bool use_cache = !length_cache.empty();
|
2016-08-17 03:49:19 -04:00
|
|
|
|
2016-11-03 11:14:04 -04:00
|
|
|
if (use_cache && length_cache.back() < desired_length && coordinates.size() >= 2)
|
2016-08-17 03:49:19 -04:00
|
|
|
{
|
2016-11-18 03:38:26 -05:00
|
|
|
if (coordinates.size() > length_cache.size())
|
|
|
|
coordinates.erase(coordinates.begin() + length_cache.size(), coordinates.end());
|
|
|
|
|
|
|
|
const auto distance_between_last_coordinates =
|
|
|
|
util::coordinate_calculation::haversineDistance(*(coordinates.end() - 2),
|
|
|
|
*(coordinates.end() - 1));
|
|
|
|
const auto interpolation_factor =
|
|
|
|
ComputeInterpolationFactor(length_cache.back(), 0, distance_between_last_coordinates);
|
|
|
|
|
|
|
|
coordinates.back() = util::coordinate_calculation::interpolateLinear(
|
|
|
|
interpolation_factor, *(coordinates.end() - 2), coordinates.back());
|
|
|
|
return coordinates;
|
2016-08-17 03:49:19 -04:00
|
|
|
}
|
2016-11-03 11:14:04 -04:00
|
|
|
else
|
|
|
|
{
|
|
|
|
BOOST_ASSERT(!use_cache || length_cache.back() >= desired_length);
|
|
|
|
for (coordinate_index = 1; coordinate_index < coordinates.size(); ++coordinate_index)
|
|
|
|
{
|
|
|
|
// get the length to the next candidate, given that we can or cannot have a length cache
|
|
|
|
const auto distance_to_next_coordinate =
|
|
|
|
use_cache ? read_length_from_cache() : compute_length();
|
2016-08-17 03:49:19 -04:00
|
|
|
|
2016-11-03 11:14:04 -04:00
|
|
|
// if we reached the number of coordinates, we can stop here
|
|
|
|
if (distance_to_next_coordinate >= desired_length)
|
|
|
|
{
|
|
|
|
coordinates.resize(coordinate_index + 1);
|
|
|
|
coordinates.back() = util::coordinate_calculation::interpolateLinear(
|
|
|
|
ComputeInterpolationFactor(desired_length,
|
|
|
|
distance_to_current_coordinate,
|
|
|
|
distance_to_next_coordinate),
|
|
|
|
coordinates[coordinate_index - 1],
|
|
|
|
coordinates[coordinate_index]);
|
|
|
|
break;
|
|
|
|
}
|
|
|
|
|
|
|
|
// remember the accumulated distance
|
|
|
|
distance_to_current_coordinate = distance_to_next_coordinate;
|
|
|
|
}
|
|
|
|
BOOST_ASSERT(!coordinates.empty());
|
|
|
|
return coordinates;
|
|
|
|
}
|
2016-08-17 03:49:19 -04:00
|
|
|
}
|
|
|
|
|
|
|
|
util::Coordinate
|
|
|
|
CoordinateExtractor::GetCorrectedCoordinate(const util::Coordinate fixpoint,
|
|
|
|
const util::Coordinate vector_base,
|
|
|
|
const util::Coordinate vector_head) const
|
|
|
|
{
|
|
|
|
// if the coordinates are close together, we were not able to look far ahead, so
|
|
|
|
// we can use the end-coordinate
|
|
|
|
if (util::coordinate_calculation::haversineDistance(vector_base, vector_head) <
|
|
|
|
DESIRED_COORDINATE_DIFFERENCE)
|
|
|
|
return vector_head;
|
|
|
|
else
|
|
|
|
{
|
|
|
|
/* to correct for the initial offset, we move the lookahead coordinate close
|
|
|
|
* to the original road. We do so by subtracting the difference between the
|
|
|
|
* turn coordinate and the offset coordinate from the lookahead coordinge:
|
|
|
|
*
|
|
|
|
* a ------ b ------ c
|
|
|
|
* |
|
|
|
|
* d
|
|
|
|
* \
|
|
|
|
* \
|
|
|
|
* e
|
|
|
|
*
|
|
|
|
* is converted to:
|
|
|
|
*
|
|
|
|
* a ------ b ------ c
|
|
|
|
* \
|
|
|
|
* \
|
|
|
|
* e
|
|
|
|
*
|
|
|
|
* for turn node `b`, vector_base `d` and vector_head `e`
|
|
|
|
*/
|
|
|
|
const auto offset_percentage = 90;
|
|
|
|
const auto corrected_lon =
|
|
|
|
vector_head.lon -
|
|
|
|
util::FixedLongitude{offset_percentage *
|
|
|
|
static_cast<int>(vector_base.lon - fixpoint.lon) / 100};
|
|
|
|
const auto corrected_lat =
|
|
|
|
vector_head.lat -
|
|
|
|
util::FixedLatitude{offset_percentage *
|
|
|
|
static_cast<int>(vector_base.lat - fixpoint.lat) / 100};
|
|
|
|
|
|
|
|
return util::Coordinate(corrected_lon, corrected_lat);
|
|
|
|
}
|
2016-10-27 14:02:21 -04:00
|
|
|
}
|
2016-08-17 03:49:19 -04:00
|
|
|
|
|
|
|
std::vector<util::Coordinate>
|
|
|
|
CoordinateExtractor::SampleCoordinates(const std::vector<util::Coordinate> &coordinates,
|
|
|
|
const double max_sample_length,
|
|
|
|
const double rate) const
|
|
|
|
{
|
|
|
|
BOOST_ASSERT(rate > 0 && coordinates.size() >= 2);
|
|
|
|
|
|
|
|
// the return value
|
|
|
|
std::vector<util::Coordinate> sampled_coordinates;
|
|
|
|
sampled_coordinates.reserve(ceil(max_sample_length / rate) + 2);
|
|
|
|
|
|
|
|
// the very first coordinate is always part of the sample
|
|
|
|
sampled_coordinates.push_back(coordinates.front());
|
|
|
|
|
|
|
|
double carry_length = 0., total_length = 0.;
|
|
|
|
// interpolate coordinates as long as we are not past the desired length
|
|
|
|
const auto add_samples_until_length_limit = [&](const util::Coordinate previous_coordinate,
|
|
|
|
const util::Coordinate current_coordinate) {
|
|
|
|
// pretend to have found an element and stop the sampling
|
|
|
|
if (total_length > max_sample_length)
|
|
|
|
return true;
|
|
|
|
|
|
|
|
const auto distance_between = util::coordinate_calculation::haversineDistance(
|
|
|
|
previous_coordinate, current_coordinate);
|
|
|
|
|
|
|
|
if (carry_length + distance_between >= rate)
|
|
|
|
{
|
|
|
|
// within the current segment, there is at least a single coordinate that we want to
|
|
|
|
// sample. We extract all coordinates that are on our sampling intervals and update our
|
|
|
|
// local sampling item to reflect the travelled distance
|
|
|
|
const auto base_sampling = rate - carry_length;
|
|
|
|
|
|
|
|
// the number of samples in the interval is equal to the length of the interval (+ the
|
|
|
|
// already traversed part from the previous segment) divided by the sampling rate
|
|
|
|
BOOST_ASSERT(max_sample_length > total_length);
|
|
|
|
const std::size_t num_samples = std::floor(
|
|
|
|
(std::min(max_sample_length - total_length, distance_between) + carry_length) /
|
|
|
|
rate);
|
|
|
|
|
|
|
|
for (std::size_t sample_value = 0; sample_value < num_samples; ++sample_value)
|
|
|
|
{
|
|
|
|
const auto interpolation_factor = ComputeInterpolationFactor(
|
|
|
|
base_sampling + sample_value * rate, 0, distance_between);
|
|
|
|
auto sampled_coordinate = util::coordinate_calculation::interpolateLinear(
|
|
|
|
interpolation_factor, previous_coordinate, current_coordinate);
|
|
|
|
sampled_coordinates.emplace_back(sampled_coordinate);
|
|
|
|
}
|
|
|
|
|
|
|
|
// current length needs to reflect how much is missing to the next sample. Here we can
|
|
|
|
// ignore max sample range, because if we reached it, the loop is done anyhow
|
|
|
|
carry_length = (distance_between + carry_length) - (num_samples * rate);
|
|
|
|
}
|
|
|
|
else
|
|
|
|
{
|
|
|
|
// do the necessary bookkeeping and continue
|
|
|
|
carry_length += distance_between;
|
|
|
|
}
|
|
|
|
// the total length travelled is always updated by the full distance
|
|
|
|
total_length += distance_between;
|
|
|
|
|
|
|
|
return false;
|
|
|
|
};
|
|
|
|
|
|
|
|
// misuse of adjacent_find. Loop over coordinates, until a total sample length is reached
|
|
|
|
std::adjacent_find(coordinates.begin(), coordinates.end(), add_samples_until_length_limit);
|
|
|
|
|
|
|
|
return sampled_coordinates;
|
|
|
|
}
|
|
|
|
|
|
|
|
double CoordinateExtractor::ComputeInterpolationFactor(const double desired_distance,
|
|
|
|
const double distance_to_first,
|
|
|
|
const double distance_to_second) const
|
|
|
|
{
|
|
|
|
BOOST_ASSERT(distance_to_first < desired_distance);
|
|
|
|
double segment_length = distance_to_second - distance_to_first;
|
|
|
|
BOOST_ASSERT(segment_length > 0);
|
|
|
|
BOOST_ASSERT(distance_to_second >= desired_distance);
|
|
|
|
double missing_distance = desired_distance - distance_to_first;
|
|
|
|
return std::max(0., std::min(missing_distance / segment_length, 1.0));
|
|
|
|
}
|
|
|
|
|
|
|
|
std::vector<util::Coordinate>
|
|
|
|
CoordinateExtractor::TrimCoordinatesByLengthFront(std::vector<util::Coordinate> coordinates,
|
|
|
|
const double desired_length) const
|
|
|
|
{
|
2016-11-03 11:14:04 -04:00
|
|
|
BOOST_ASSERT(desired_length >= 0);
|
2016-08-17 03:49:19 -04:00
|
|
|
double distance_to_index = 0;
|
|
|
|
std::size_t index = 0;
|
|
|
|
for (std::size_t next_index = 1; next_index < coordinates.size(); ++next_index)
|
|
|
|
{
|
|
|
|
const double next_distance =
|
|
|
|
distance_to_index + util::coordinate_calculation::haversineDistance(
|
|
|
|
coordinates[index], coordinates[next_index]);
|
|
|
|
if (next_distance >= desired_length)
|
|
|
|
{
|
|
|
|
const auto factor =
|
|
|
|
ComputeInterpolationFactor(desired_length, distance_to_index, next_distance);
|
|
|
|
auto interpolated_coordinate = util::coordinate_calculation::interpolateLinear(
|
|
|
|
factor, coordinates[index], coordinates[next_index]);
|
|
|
|
if (index > 0)
|
|
|
|
coordinates.erase(coordinates.begin(), coordinates.begin() + index);
|
|
|
|
coordinates.front() = interpolated_coordinate;
|
|
|
|
return coordinates;
|
|
|
|
}
|
|
|
|
|
|
|
|
distance_to_index = next_distance;
|
|
|
|
index = next_index;
|
|
|
|
}
|
|
|
|
|
|
|
|
// the coordinates in total are too short in length for the desired length
|
|
|
|
// this part is only reached when we don't return from within the above loop
|
|
|
|
coordinates.clear();
|
|
|
|
return coordinates;
|
|
|
|
}
|
|
|
|
|
|
|
|
std::pair<util::Coordinate, util::Coordinate>
|
|
|
|
CoordinateExtractor::RegressionLine(const std::vector<util::Coordinate> &coordinates) const
|
|
|
|
{
|
|
|
|
// create a sample of all coordinates to improve the quality of our regression vector
|
|
|
|
// (less dependent on modelling of the data in OSM)
|
|
|
|
const auto sampled_coordinates = SampleCoordinates(coordinates, FAR_LOOKAHEAD_DISTANCE, 1);
|
|
|
|
|
2016-10-25 10:38:45 -04:00
|
|
|
BOOST_ASSERT(!coordinates.empty());
|
|
|
|
if (sampled_coordinates.size() < 2) // less than 1 meter in length
|
|
|
|
return {coordinates.front(), coordinates.back()};
|
|
|
|
|
2016-08-17 03:49:19 -04:00
|
|
|
// compute the regression vector based on the sum of least squares
|
|
|
|
const auto regression_line = leastSquareRegression(sampled_coordinates);
|
|
|
|
const auto coord_between_front =
|
|
|
|
util::coordinate_calculation::projectPointOnSegment(
|
|
|
|
regression_line.first, regression_line.second, coordinates.front())
|
|
|
|
.second;
|
|
|
|
const auto coord_between_back =
|
|
|
|
util::coordinate_calculation::projectPointOnSegment(
|
|
|
|
regression_line.first, regression_line.second, coordinates.back())
|
|
|
|
.second;
|
|
|
|
|
|
|
|
return {coord_between_front, coord_between_back};
|
|
|
|
}
|
|
|
|
|
|
|
|
} // namespace guidance
|
|
|
|
} // namespace extractor
|
|
|
|
} // namespace osrm
|