wip

CMakeLists.txt

@@ -357,7 +357,7 @@ if(ENABLE_CONAN)
     KEEP_RPATHS
     NO_OUTPUT_DIRS
     OPTIONS boost:filesystem_version=3 # https://stackoverflow.com/questions/73392648/error-with-boost-filesystem-version-in-cmake
-            onetbb:shared=${TBB_SHARED}
+           # onetbb:shared=${TBB_SHARED}
             boost:without_stacktrace=True # Apple Silicon cross-compilation fails without it
     BUILD missing
   )

include/engine/phantom_node.hpp

@@ -214,7 +214,7 @@ struct PhantomNode
     util::Coordinate input_location;
     unsigned short fwd_segment_position;
     // is phantom node valid to be used as source or target
-  private:
+    // private:
     unsigned short is_valid_forward_source : 1;
     unsigned short is_valid_forward_target : 1;
     unsigned short is_valid_reverse_source : 1;

include/engine/routing_algorithms/routing_base_ch.hpp

@@ -471,6 +471,23 @@ double getNetworkDistance(SearchEngineData<Algorithm> &engine_working_data,
                           const PhantomNode &target_phantom,
                           EdgeWeight duration_upper_bound = INVALID_EDGE_WEIGHT);
 
+inline double getNetworkDistanceOld(SearchEngineData<Algorithm> &engine_working_data,
+                                    const DataFacade<ch::Algorithm> &facade,
+                                    SearchEngineData<Algorithm>::QueryHeap &forward_heap,
+                                    SearchEngineData<Algorithm>::QueryHeap &reverse_heap,
+                                    const PhantomNode &source_phantom,
+                                    const PhantomNode &target_phantom,
+                                    EdgeWeight duration_upper_bound = INVALID_EDGE_WEIGHT)
+{
+    return getNetworkDistance(engine_working_data,
+                              facade,
+                              forward_heap,
+                              reverse_heap,
+                              source_phantom,
+                              target_phantom,
+                              duration_upper_bound);
+}
+
 template <typename EdgeMetric>
 std::tuple<EdgeMetric, EdgeDistance> getLoopMetric(const DataFacade<Algorithm> &facade, NodeID node)
 {

include/engine/routing_algorithms/routing_base_mld.hpp

@@ -497,7 +497,7 @@ std::optional<std::pair<NodeID, EdgeWeight>> runSearch(const DataFacade<Algorith
                                                        EdgeWeight weight_upper_bound,
                                                        const Args &...args)
 {
-    if (forward_heap.Empty() || reverse_heap.Empty())
+    if (forward_heap.Empty() && reverse_heap.Empty())
     {
         return {};
     }
@@ -510,9 +510,19 @@ std::optional<std::pair<NodeID, EdgeWeight>> runSearch(const DataFacade<Algorith
     EdgeWeight weight = weight_upper_bound;
     EdgeWeight forward_heap_min = forward_heap.MinKey();
     EdgeWeight reverse_heap_min = reverse_heap.MinKey();
+// if (!reverse_heap.Empty())
+//     reverse_heap_min = reverse_heap.MinKey();
+#if 0
+    while (forward_heap.Size() + reverse_heap.Size() > 0 && (
+           forward_heap_min < weight || reverse_heap_min < weight))
+#else
     while (forward_heap.Size() + reverse_heap.Size() > 0 &&
-           forward_heap_min + reverse_heap_min < weight)
+           (forward_heap_min + reverse_heap_min < weight))
+#endif
     {
+        // std::cerr << "F: " << forward_heap_min << " R: " << reverse_heap_min << " W: " << weight
+        // << std::endl;
+
         if (!forward_heap.Empty())
         {
             routingStep<FORWARD_DIRECTION>(
@@ -529,12 +539,21 @@ std::optional<std::pair<NodeID, EdgeWeight>> runSearch(const DataFacade<Algorith
         }
     };
 
+    // while (!reverse_heap.Empty() && (reverse_heap_min < weight)) {
+    //     routingStep<REVERSE_DIRECTION>(
+    //         facade, reverse_heap, forward_heap, middle, weight, force_step_nodes, args...);
+    //     if (!reverse_heap.Empty())
+    //         reverse_heap_min = reverse_heap.MinKey();
+    // }
+
     // No path found for both target nodes?
     if (weight >= weight_upper_bound || SPECIAL_NODEID == middle)
     {
         return {};
     }
 
+    std::cerr << "MIDDLE = " << middle << std::endl;
+
     return {{middle, weight}};
 }
 
@@ -634,8 +653,8 @@ searchDistance(SearchEngineData<Algorithm> &,
         return INVALID_EDGE_DISTANCE;
     }
 
-    auto [middle, _] = *searchResult;
-
+    auto [middle, weight] = *searchResult;
+    // std::cerr << "W: " << weight << std::endl;
     auto distance = forward_heap.GetData(middle).distance + reverse_heap.GetData(middle).distance;
 
     return distance;
@@ -704,41 +723,148 @@ double getNetworkDistance(SearchEngineData<Algorithm> &engine_working_data,
                           const PhantomNode &target_phantom,
                           EdgeWeight weight_upper_bound = INVALID_EDGE_WEIGHT)
 {
-    forward_heap.Clear();
     reverse_heap.Clear();
+    // forward_heap.Clear();
+
+    // if (forward_heap.Empty())
+    // {
+    //     if (source_phantom.IsValidForwardSource())
+    //     {
+    //         forward_heap.Insert(source_phantom.forward_segment_id.id,
+    //                             EdgeWeight{0} - source_phantom.GetForwardWeightPlusOffset(),
+    //                             {source_phantom.forward_segment_id.id,
+    //                              false,
+    //                              EdgeDistance{0} - source_phantom.GetForwardDistance()});
+    //     }
+
+    //     if (source_phantom.IsValidReverseSource())
+    //     {
+    //         forward_heap.Insert(source_phantom.reverse_segment_id.id,
+    //                             EdgeWeight{0} - source_phantom.GetReverseWeightPlusOffset(),
+    //                             {source_phantom.reverse_segment_id.id,
+    //                              false,
+    //                              EdgeDistance{0} - source_phantom.GetReverseDistance()});
+    //     }
+    // } else {
+    //     const auto node =
+    //     forward_heap.GetHeapNodeIfWasInserted(target_phantom.forward_segment_id.id); if (node) {
+    //         std::cerr << "Found " << target_phantom.forward_segment_id.id << " in forward_heap "
+    //         << node->data.distance <<  std::endl;
+    //     }
+    // }
+
+    //  forward_heap.Clear();
+    if (forward_heap.Empty())
+    {
+        if (source_phantom.IsValidForwardSource())
+        {
+            forward_heap.Insert(source_phantom.forward_segment_id.id,
+                                EdgeWeight{0},
+                                {source_phantom.forward_segment_id.id, false, EdgeDistance{0}});
+        }
+
+        if (source_phantom.IsValidReverseSource())
+        {
+            forward_heap.Insert(source_phantom.reverse_segment_id.id,
+                                EdgeWeight{0},
+                                {source_phantom.reverse_segment_id.id, false, EdgeDistance{0}});
+        }
+    }
+    else
+    {
+        if (target_phantom.IsValidForwardTarget())
+        {
+            if (forward_heap.WasInserted(target_phantom.forward_segment_id.id))
+            {
+                std::cerr << "Found " << target_phantom.forward_segment_id.id << " in forward_heap"
+                          << std::endl;
+            }
+            else
+            {
+                std::cerr << "Not found " << target_phantom.forward_segment_id.id
+                          << " in forward_heap" << std::endl;
+            }
+        }
+        if (target_phantom.IsValidReverseTarget())
+        {
+            if (forward_heap.WasInserted(target_phantom.reverse_segment_id.id))
+            {
+                std::cerr << "Found " << target_phantom.reverse_segment_id.id << " in forward_heap"
+                          << std::endl;
+            }
+            else
+            {
+                std::cerr << "Not found " << target_phantom.reverse_segment_id.id
+                          << " in forward_heap" << std::endl;
+            }
+        }
+    }
+
+    if (target_phantom.IsValidForwardTarget())
+    {
+        reverse_heap.Insert(target_phantom.forward_segment_id.id,
+                            EdgeWeight{0},
+                            {target_phantom.forward_segment_id.id, false, EdgeDistance{0}});
+    }
+
+    if (target_phantom.IsValidReverseTarget())
+    {
+        reverse_heap.Insert(target_phantom.reverse_segment_id.id,
+                            EdgeWeight{0},
+                            {target_phantom.reverse_segment_id.id, false, EdgeDistance{0}});
+    }
+
+    const PhantomEndpoints endpoints{source_phantom, target_phantom};
+
+    auto distance = searchDistance(
+        engine_working_data, facade, forward_heap, reverse_heap, {}, weight_upper_bound, endpoints);
+
+    if (distance == INVALID_EDGE_DISTANCE)
+    {
+        return std::numeric_limits<double>::max();
+    }
+    return from_alias<double>(distance);
+}
+
+template <typename Algorithm>
+double
+getNetworkDistanceOld(SearchEngineData<Algorithm> &engine_working_data,
+                      const DataFacade<Algorithm> &facade,
+                      typename SearchEngineData<Algorithm>::MapMatchingQueryHeap &forward_heap,
+                      typename SearchEngineData<Algorithm>::MapMatchingQueryHeap &reverse_heap,
+                      const PhantomNode &source_phantom,
+                      const PhantomNode &target_phantom,
+                      EdgeWeight weight_upper_bound = INVALID_EDGE_WEIGHT)
+{
+    reverse_heap.Clear();
+    forward_heap.Clear();
 
     if (source_phantom.IsValidForwardSource())
     {
         forward_heap.Insert(source_phantom.forward_segment_id.id,
-                            EdgeWeight{0} - source_phantom.GetForwardWeightPlusOffset(),
-                            {source_phantom.forward_segment_id.id,
-                             false,
-                             EdgeDistance{0} - source_phantom.GetForwardDistance()});
+                            EdgeWeight{0},
+                            {source_phantom.forward_segment_id.id, false, EdgeDistance{0}});
     }
 
     if (source_phantom.IsValidReverseSource())
     {
         forward_heap.Insert(source_phantom.reverse_segment_id.id,
-                            EdgeWeight{0} - source_phantom.GetReverseWeightPlusOffset(),
-                            {source_phantom.reverse_segment_id.id,
-                             false,
-                             EdgeDistance{0} - source_phantom.GetReverseDistance()});
+                            EdgeWeight{0},
+                            {source_phantom.reverse_segment_id.id, false, EdgeDistance{0}});
     }
 
     if (target_phantom.IsValidForwardTarget())
     {
-        reverse_heap.Insert(
-            target_phantom.forward_segment_id.id,
-            target_phantom.GetForwardWeightPlusOffset(),
-            {target_phantom.forward_segment_id.id, false, target_phantom.GetForwardDistance()});
+        reverse_heap.Insert(target_phantom.forward_segment_id.id,
+                            EdgeWeight{0},
+                            {target_phantom.forward_segment_id.id, false, EdgeDistance{0}});
     }
 
     if (target_phantom.IsValidReverseTarget())
     {
-        reverse_heap.Insert(
-            target_phantom.reverse_segment_id.id,
-            target_phantom.GetReverseWeightPlusOffset(),
-            {target_phantom.reverse_segment_id.id, false, target_phantom.GetReverseDistance()});
+        reverse_heap.Insert(target_phantom.reverse_segment_id.id,
+                            EdgeWeight{0},
+                            {target_phantom.reverse_segment_id.id, false, EdgeDistance{0}});
     }
 
     const PhantomEndpoints endpoints{source_phantom, target_phantom};

src/engine/routing_algorithms/map_matching.cpp

@@ -1,4 +1,5 @@
 #include "engine/routing_algorithms/map_matching.hpp"
+#include "engine/phantom_node.hpp"
 #include "engine/routing_algorithms/routing_base_ch.hpp"
 #include "engine/routing_algorithms/routing_base_mld.hpp"
 
@@ -13,6 +14,7 @@
 #include <cstddef>
 #include <deque>
 #include <iomanip>
+#include <limits>
 #include <memory>
 #include <numeric>
 #include <utility>
@@ -217,6 +219,183 @@ SubMatchingList mapMatching(SearchEngineData<Algorithm> &engine_working_data,
             const EdgeWeight weight_upper_bound = to_alias<EdgeWeight>(
                 ((haversine_distance + max_distance_delta) / 4.) * facade.GetWeightMultiplier());
 
+#if 1
+            std::vector<PhantomNode> test_sources;
+            std::vector<PhantomNode> test_targets;
+
+            test_sources.resize(2);
+            test_targets.resize(2);
+
+            test_sources[0].forward_segment_id.id = 273;
+            test_sources[0].forward_segment_id.enabled = 1;
+            test_sources[0].is_valid_forward_source = true;
+
+            test_sources[0].reverse_segment_id.id = 2147483647;
+            test_sources[0].reverse_segment_id.enabled = 0;
+
+            test_targets[0].forward_segment_id.id = 197;
+            test_targets[0].forward_segment_id.enabled = 1;
+            test_targets[0].is_valid_forward_target = true;
+
+            test_targets[0].reverse_segment_id.id = 268;
+            test_targets[0].reverse_segment_id.enabled = 0;
+
+            test_sources[1].forward_segment_id.id = 215;
+            test_sources[1].forward_segment_id.enabled = 1;
+            test_sources[0].is_valid_forward_source = true;
+
+            test_sources[1].reverse_segment_id.id = 2147483647;
+            test_sources[1].reverse_segment_id.enabled = 0;
+
+            test_targets[1].forward_segment_id.id = 197;
+            test_targets[1].forward_segment_id.enabled = 0;
+
+            test_targets[1].reverse_segment_id.id = 268;
+            test_targets[1].reverse_segment_id.enabled = 1;
+            test_targets[1].is_valid_reverse_target = true;
+
+            //    std::cerr << "OLD RUN: \n";
+            // std::vector<double> old_distances;
+
+            // for (const auto& s: test_sources) {
+            //     for (const auto& t: test_targets) {
+            //         forward_heap.Clear();
+            //         double network_distance =
+            //             getNetworkDistanceOld(engine_working_data,
+            //                                facade,
+            //                                forward_heap,
+            //                                reverse_heap,
+            //                                s,
+            //                                t,
+            //                                weight_upper_bound);
+            //         old_distances.push_back(network_distance);
+
+            //     }
+            // }
+
+            //  std::cerr << "NEW RUN: \n";
+            std::vector<double> new_distances;
+
+            for (const auto &s : test_sources)
+            {
+                forward_heap.Clear();
+
+                for (const auto &t : test_targets)
+                {
+
+                    double network_distance = getNetworkDistance(engine_working_data,
+                                                                 facade,
+                                                                 forward_heap,
+                                                                 reverse_heap,
+                                                                 s,
+                                                                 t,
+                                                                 weight_upper_bound);
+                    new_distances.push_back(network_distance);
+                }
+            }
+
+            std::vector<double> expected = {663.997,
+                                            1533.04,
+                                            std::numeric_limits<double>::max(),
+                                            std::numeric_limits<double>::max()};
+            if (new_distances.size() != expected.size())
+            {
+                std::cerr << "New distances size is not equal to expected size" << std::endl;
+                std::exit(1);
+            }
+            // for (size_t index = 0; index < new_distances.size(); index++) {
+            //     std::cerr << new_distances[index] << std::endl;
+            // }
+            for (size_t index = 0; index < new_distances.size(); index++)
+            {
+
+                if (std::abs(new_distances[index] - expected[index]) > 1e-1)
+                {
+                    std::cerr << "New: " << new_distances[index] << " Expected: " << expected[index]
+                              << std::endl;
+                    std::exit(1);
+                }
+            }
+            std::exit(1);
+#elif 1
+            std::vector<double> old_distances;
+            for (const auto s : util::irange<std::size_t>(0UL, prev_viterbi.size()))
+            {
+                if (prev_pruned[s])
+                {
+                    continue;
+                }
+
+                for (const auto s_prime : util::irange<std::size_t>(0UL, current_viterbi.size()))
+                {
+                    const double emission_pr = emission_log_probabilities[t][s_prime];
+                    double new_value = prev_viterbi[s] + emission_pr;
+                    if (current_viterbi[s_prime] > new_value)
+                    {
+                        continue;
+                    }
+
+                    forward_heap.Clear();
+                    double network_distance =
+                        getNetworkDistanceOld(engine_working_data,
+                                              facade,
+                                              forward_heap,
+                                              reverse_heap,
+                                              prev_unbroken_timestamps_list[s].phantom_node,
+                                              current_timestamps_list[s_prime].phantom_node,
+                                              weight_upper_bound);
+                    old_distances.push_back(network_distance);
+                }
+            }
+
+            std::vector<double> new_distances;
+            for (const auto s : util::irange<std::size_t>(0UL, prev_viterbi.size()))
+            {
+                if (prev_pruned[s])
+                {
+                    continue;
+                }
+
+                forward_heap.Clear();
+
+                for (const auto s_prime : util::irange<std::size_t>(0UL, current_viterbi.size()))
+                {
+                    const double emission_pr = emission_log_probabilities[t][s_prime];
+                    double new_value = prev_viterbi[s] + emission_pr;
+                    if (current_viterbi[s_prime] > new_value)
+                    {
+                        continue;
+                    }
+
+                    double network_distance =
+                        getNetworkDistance(engine_working_data,
+                                           facade,
+                                           forward_heap,
+                                           reverse_heap,
+                                           prev_unbroken_timestamps_list[s].phantom_node,
+                                           current_timestamps_list[s_prime].phantom_node,
+                                           weight_upper_bound);
+                    new_distances.push_back(network_distance);
+                }
+            }
+            if (new_distances.size() != old_distances.size())
+            {
+                std::cerr << "New distances size is not equal to expected size" << std::endl;
+                std::exit(1);
+            }
+
+            for (size_t index = 0; index < new_distances.size(); index++)
+            {
+
+                if (std::abs(new_distances[index] - old_distances[index]) > 1e-1)
+                {
+                    std::cerr << "New: " << new_distances[index]
+                              << " Expected: " << old_distances[index] << std::endl;
+                    std::exit(1);
+                }
+            }
+#endif
+
             // compute d_t for this timestamp and the next one
             for (const auto s : util::irange<std::size_t>(0UL, prev_viterbi.size()))
             {
@@ -225,6 +404,8 @@ SubMatchingList mapMatching(SearchEngineData<Algorithm> &engine_working_data,
                     continue;
                 }
 
+                forward_heap.Clear();
+
                 for (const auto s_prime : util::irange<std::size_t>(0UL, current_viterbi.size()))
                 {
                     const double emission_pr = emission_log_probabilities[t][s_prime];
@@ -264,6 +445,8 @@ SubMatchingList mapMatching(SearchEngineData<Algorithm> &engine_working_data,
                         model.breakage[t] = false;
                     }
                 }
+
+                forward_heap.Clear();
             }
 
             if (model.breakage[t])