From daf468772df6b3d9c39a28cd58fae33e3126b806 Mon Sep 17 00:00:00 2001 From: Kevin Kreiser Date: Thu, 20 Jul 2017 15:53:04 -0400 Subject: [PATCH] move decision about whether or not to add a label based on limits into routing rather than labelset --- src/meili/map_matching.cc | 5 +- src/meili/routing.cc | 137 +++++++++++++++++++------------------- test/routing.cc | 2 +- valhalla/meili/routing.h | 7 +- 4 files changed, 77 insertions(+), 74 deletions(-) diff --git a/src/meili/map_matching.cc b/src/meili/map_matching.cc index 89da61bea2..ced676ac28 100644 --- a/src/meili/map_matching.cc +++ b/src/meili/map_matching.cc @@ -51,11 +51,12 @@ State::route(const std::vector& states, } // Route - labelset_ = std::make_shared(std::ceil(max_route_distance), std::ceil(max_route_time)); + labelset_ = std::make_shared(max_route_distance); const auto& results = find_shortest_path( graphreader, locations, 0, labelset_, approximator, search_radius, - costing, edgelabel, turn_cost_table); + costing, edgelabel, turn_cost_table, + std::ceil(max_route_distance), std::ceil(max_route_time)); // Cache results label_idx_.clear(); diff --git a/src/meili/routing.cc b/src/meili/routing.cc index d691f02ad9..9e62696d42 100644 --- a/src/meili/routing.cc +++ b/src/meili/routing.cc @@ -15,12 +15,11 @@ namespace valhalla { namespace meili { -LabelSet::LabelSet(const float max_dist, const float max_time, const float bucket_size): - max_dist_(max_dist), max_time_(max_time) { +LabelSet::LabelSet(const float max_dist, const float bucket_size) { const auto edgecost = [this](const uint32_t label) { return labels_[label].sortcost; }; - queue_.reset(new baldr::DoubleBucketQueue(0.0f, max_dist_, bucket_size, edgecost)); + queue_.reset(new baldr::DoubleBucketQueue(0.0f, max_dist, bucket_size, edgecost)); } bool LabelSet::put(const baldr::GraphId& nodeid, const baldr::GraphId& edgeid, @@ -38,31 +37,28 @@ bool LabelSet::put(const baldr::GraphId& nodeid, const baldr::GraphId& edgeid, // Create a new label and push it to the queue if (it == node_status_.end()) { const uint32_t idx = labels_.size(); - // We only add the labels if we are under the limits for distance and for time or time limit is 0 - if (cost.cost < max_dist_ && (max_time_ == 0.f || cost.secs < max_time_)) { - queue_->add(idx, sortcost); - labels_.emplace_back(nodeid, edgeid, - source, target, - cost, turn_cost, sortcost, - predecessor, - edge, travelmode, edgelabel); - node_status_.emplace(nodeid, idx); - return true; - } - } else { - // Decrease cost of the existing label - const auto& status = it->second; - if (!status.permanent && sortcost < labels_[status.label_idx].sortcost) { - // Update queue first since it uses the label cost within the decrease - // method to determine the current bucket. - queue_->decrease(status.label_idx, sortcost); - labels_[status.label_idx] = {nodeid, edgeid, - source, target, - cost, turn_cost, sortcost, - predecessor, - edge, travelmode, edgelabel}; - return true; - } + queue_->add(idx, sortcost); + labels_.emplace_back(nodeid, edgeid, + source, target, + cost, turn_cost, sortcost, + predecessor, + edge, travelmode, edgelabel); + node_status_.emplace(nodeid, idx); + return true; + } + + // Decrease cost of the existing label + const auto& status = it->second; + if (!status.permanent && sortcost < labels_[status.label_idx].sortcost) { + // Update queue first since it uses the label cost within the decrease + // method to determine the current bucket. + queue_->decrease(status.label_idx, sortcost); + labels_[status.label_idx] = {nodeid, edgeid, + source, target, + cost, turn_cost, sortcost, + predecessor, + edge, travelmode, edgelabel}; + return true; } return false; } @@ -85,31 +81,28 @@ bool LabelSet::put(uint16_t dest, // Create a new label and push it to the queue if (it == dest_status_.end()) { const uint32_t idx = labels_.size(); - // We only add the labels if we are under the limits for distance and for time or time limit is 0 - if (cost.cost < max_dist_ && (max_time_ == 0.f || cost.secs < max_time_)) { - queue_->add(idx, sortcost); - labels_.emplace_back(dest, edgeid, - source, target, - cost, turn_cost, sortcost, - predecessor, - edge, travelmode, edgelabel); - dest_status_.emplace(dest, idx); - return true; - } - } else { - // Decrease cost of the existing label - const auto& status = it->second; - if (!status.permanent && sortcost < labels_[status.label_idx].sortcost) { - // Update queue first since it uses the label cost within the decrease - // method to determine the current bucket. - queue_->decrease(status.label_idx, sortcost); - labels_[status.label_idx] = {dest, edgeid, - source, target, - cost, turn_cost, sortcost, - predecessor, - edge, travelmode, edgelabel}; - return true; - } + queue_->add(idx, sortcost); + labels_.emplace_back(dest, edgeid, + source, target, + cost, turn_cost, sortcost, + predecessor, + edge, travelmode, edgelabel); + dest_status_.emplace(dest, idx); + return true; + } + + // Decrease cost of the existing label + const auto& status = it->second; + if (!status.permanent && sortcost < labels_[status.label_idx].sortcost) { + // Update queue first since it uses the label cost within the decrease + // method to determine the current bucket. + queue_->decrease(status.label_idx, sortcost); + labels_[status.label_idx] = {dest, edgeid, + source, target, + cost, turn_cost, sortcost, + predecessor, + edge, travelmode, edgelabel}; + return true; } return false; } @@ -352,7 +345,8 @@ find_shortest_path(baldr::GraphReader& reader, const float search_radius, sif::cost_ptr_t costing, std::shared_ptr edgelabel, - const float turn_cost_table[181]) + const float turn_cost_table[181], + const float max_dist, const float max_time) { sif::Cost label_cost; float label_turn_cost; @@ -372,14 +366,11 @@ find_shortest_path(baldr::GraphReader& reader, return (d2 < search_rad2) ? 0.0f : sqrtf(d2) - search_radius; }; - // Lambda method to expand along edges from this node. This method have to be + // Lambda method to expand along edges from this node. This method has to be // set-up to be called recursively (for transition edges) so we set up a // function reference. std::function expand; - expand = [&reader, &approximator, &pred_edgelabel, &labelset, &edge_dests, - &destinations, &search_radius, &search_rad2, &costing, &travelmode, &label_cost, - &label_turn_cost, &turn_cost_table, &heuristic, &expand] - (const baldr::GraphId& node, const uint32_t label_idx, const bool from_transition) { + expand = [&](const baldr::GraphId& node, const uint32_t label_idx, const bool from_transition) { // Get the node's info. The tile will be guaranteed to be nodeid's tile // in this block. Return if node is not found or is not allowed by costing const baldr::GraphTile* tile = reader.GetGraphTile(node); @@ -439,9 +430,12 @@ find_shortest_path(baldr::GraphReader& reader, // to itself must be 0, so sortcost = cost sif::Cost cost(label_cost.cost + directededge->length() * edge.dist, label_cost.secs + costing->EdgeCost(directededge).secs * edge.dist); - labelset->put(dest, edgeid, 0.f, edge.dist, + // We only add the labels if we are under the limits for distance and for time or time limit is 0 + if (cost.cost < max_dist && (max_time == 0.f || cost.secs < max_time)) { + labelset->put(dest, edgeid, 0.f, edge.dist, cost, turn_cost, cost.cost, label_idx, directededge, travelmode, nullptr); + } } } } @@ -453,13 +447,16 @@ find_shortest_path(baldr::GraphReader& reader, if (endtile != nullptr) { // Get cost - use EdgeCost to get time along the edge. Override // cost portion to be distance. Add heuristic to get sort cost. - const auto end_nodeinfo = endtile->node(directededge->endnode()); sif::Cost cost(label_cost.cost + directededge->length(), label_cost.secs + costing->EdgeCost(directededge).secs); - float sortcost = cost.cost + heuristic(end_nodeinfo->latlng()); - labelset->put(directededge->endnode(), edgeid, 0.f, 1.f, + // We only add the labels if we are under the limits for distance and for time or time limit is 0 + if (cost.cost < max_dist && (max_time == 0.f || cost.secs < max_time)) { + const auto end_nodeinfo = endtile->node(directededge->endnode()); + float sortcost = cost.cost + heuristic(end_nodeinfo->latlng()); + labelset->put(directededge->endnode(), edgeid, 0.f, 1.f, cost, turn_cost, sortcost, label_idx, directededge, travelmode, nullptr); + } } } }; @@ -560,10 +557,13 @@ find_shortest_path(baldr::GraphReader& reader, float f = (other_edge.dist - origin_edge.dist); sif::Cost cost(label_cost.cost + directededge->length() * f, label_cost.secs + costing->EdgeCost(directededge).secs * f); - labelset->put(other_dest, origin_edge.id, + // We only add the labels if we are under the limits for distance and for time or time limit is 0 + if (cost.cost < max_dist && (max_time == 0.f || cost.secs < max_time)) { + labelset->put(other_dest, origin_edge.id, origin_edge.dist, other_edge.dist, cost, turn_cost, cost.cost, label_idx, directededge, travelmode, nullptr); + } } } } @@ -574,7 +574,6 @@ find_shortest_path(baldr::GraphReader& reader, if (endtile == nullptr) { continue; } - const auto nodeinfo = endtile->node(directededge->endnode()); // Get cost - use EdgeCost to get time along the edge. Override // cost portion to be distance. The heuristic cost from a @@ -582,10 +581,14 @@ find_shortest_path(baldr::GraphReader& reader, float f = (1.f - origin_edge.dist); sif::Cost cost(label_cost.cost + directededge->length() * f, label_cost.secs + costing->EdgeCost(directededge).secs * f); - float sortcost = cost.cost + heuristic(nodeinfo->latlng()); - labelset->put(directededge->endnode(), origin_edge.id, origin_edge.dist, 1.f, + // We only add the labels if we are under the limits for distance and for time or time limit is 0 + if (cost.cost < max_dist && (max_time == 0.f || cost.secs < max_time)) { + const auto nodeinfo = endtile->node(directededge->endnode()); + float sortcost = cost.cost + heuristic(nodeinfo->latlng()); + labelset->put(directededge->endnode(), origin_edge.id, origin_edge.dist, 1.f, cost, turn_cost, sortcost, label_idx, directededge, travelmode, nullptr); + } } } } diff --git a/test/routing.cc b/test/routing.cc index 053f0f7569..f923387280 100644 --- a/test/routing.cc +++ b/test/routing.cc @@ -175,7 +175,7 @@ void Benchmark() void TestRoutePathIterator() { - meili::LabelSet labelset(100, 0); + meili::LabelSet labelset(100); // Travel mode is insignificant in the tests sif::TravelMode travelmode = static_cast(0); diff --git a/valhalla/meili/routing.h b/valhalla/meili/routing.h index f6b34f0305..11a75522f4 100644 --- a/valhalla/meili/routing.h +++ b/valhalla/meili/routing.h @@ -146,7 +146,7 @@ struct Status{ class LabelSet { public: - LabelSet(const float max_distance, const float max_time, const float bucket_size = 1.0f); + LabelSet(const float max_distance, const float bucket_size = 1.0f); bool put(const baldr::GraphId& nodeid, sif::TravelMode travelmode, std::shared_ptr edgelabel) { @@ -200,8 +200,6 @@ class LabelSet } private: - float max_dist_; - float max_time_; std::shared_ptr queue_; std::unordered_map node_status_; std::unordered_map dest_status_; @@ -222,7 +220,8 @@ find_shortest_path(baldr::GraphReader& reader, const float search_radius, sif::cost_ptr_t costing, std::shared_ptr edgelabel, - const float turn_cost_table[181]); + const float turn_cost_table[181], + const float max_dist, const float max_time); class RoutePathIterator: public std::iterator