added another ksp implementation

CMakeLists.txt

@@ -23,7 +23,7 @@ set(SOURCE_FILES
         core/ObjectData2D.cpp core/ObjectData2D.h
         algo/Berclaz.cpp algo/Berclaz.h
         util/Grid.cpp util/Grid.h
-        core/ObjectDataBox.cpp core/ObjectDataBox.h algo/KShortestPaths2.cpp algo/KShortestPaths2.h)
+        core/ObjectDataBox.cpp core/ObjectDataBox.h algo/KShortestPaths2.cpp algo/KShortestPaths2.h algo/KShortestPaths3.cpp algo/KShortestPaths3.h algo/KShortestPaths4.cpp algo/KShortestPaths4.h)
 
 find_package(Doxygen)
 

algo/Berclaz.cpp

@@ -8,6 +8,7 @@
 #include "../util/Logger.h"
 #include "../util/FileIO.h"
 #include "KShortestPaths2.h"
+#include "KShortestPaths3.h"
 
 namespace algo
 {
@@ -160,8 +161,7 @@ namespace algo
             Vertex source, sink;
             CreateGraph(graph, source, sink, grid);
 
-            util::Logger::LogDebug("run ksp");
-            KShortestPaths2 ksp;
+            KShortestPaths3 ksp;
             MultiPredecessorMap ksp_result = ksp.Run(graph, source, sink, max_track_count);
 
             util::Logger::LogDebug("extract tracks");

algo/KShortestPaths2.cpp

@@ -142,7 +142,8 @@ namespace algo
             }
 
             ExtendGraph(i);
-            TransformEdges(i > 0 ? i - 1 : 0);
+//            TransformEdges(i > 0 ? i - 1 : 0);
+            TransformEdges(i);
             FindAndAugment(i + 1);
             Interlace(i + 1);
         }
@@ -189,18 +190,14 @@ namespace algo
                  oei != oei_end; ++oei)
             {
                 // Copy the output edges to the output vertices
-                QueueAddEdge(v_copy_out,
-                             boost::target(*oei, graph_copy_),
-                             weights[*oei]);
+                QueueAddEdge(v_copy_out, boost::target(*oei, graph_copy_), weights[*oei]);
 
                 // Remove the old output edge
                 QueueRemoveEdge(v_copy_in, boost::target(*oei, graph_copy_));
             }
 
             // Create auxiliary edge
-            // Is already inverted, because it will not be iterated later
             QueueAddEdge(v_copy_out, v_copy_in, 0.0);
-//            QueueAddEdge(v_copy_in, v_copy_out, 0.0);
         }
 
         util::Logger::LogDebug("update edges");
@@ -232,7 +229,7 @@ namespace algo
         util::Logger::LogDebug("update edges");
         UpdateEdges();
 
-        util::FileIO::WriteCSVMatlab(graph_copy_, "/home/wrede/Dokumente/graph_" + std::to_string(iteration) + "e.csv");
+//        util::FileIO::WriteCSVMatlab(graph_copy_, "/home/wrede/Dokumente/graph_" + std::to_string(iteration) + "e.csv");
     }
 
     void KShortestPaths2::TransformEdges(size_t iteration)
@@ -252,22 +249,7 @@ namespace algo
             // the specified path finding iteration
             if (distances.count(s_copy) > 0 && distances.count(t_copy) > 0)
             {
-                //TODO set all edges to source to zero (experimental)
-//                if (t_copy == orig_to_copy_[source_]) {
-//                    weights[*ei] = 0.0;
-//                    continue;
-//                }
-
-                // check if the vertex was found in previous iterations
-//                if (distances[s_copy] == std::numeric_limits<double>::max() ||
-//                        distances[t_copy] == std::numeric_limits<double>::max())
-//                {
-//                    weights[*ei] = std::numeric_limits<double>::infinity();
-//                }
-//                else
-//                {
-                    weights[*ei] += distances[s_copy] - distances[t_copy];
-//                }
+                weights[*ei] += distances[s_copy] - distances[t_copy];
             }
 
             // Correct rounding errors
@@ -285,7 +267,7 @@ namespace algo
             }
         }
 
-        util::FileIO::WriteCSVMatlab(graph_copy_, "/home/wrede/Dokumente/graph_" + std::to_string(iteration) + "t.csv");
+//        util::FileIO::WriteCSVMatlab(graph_copy_, "/home/wrede/Dokumente/graph_" + std::to_string(iteration) + "t.csv");
     }
 
     void KShortestPaths2::FindAndAugment(size_t iteration)
@@ -347,23 +329,18 @@ namespace algo
 
         // Add the new distances
         boost::graph_traits<DirectedGraph>::vertex_iterator vi, vi_end;
+        double max_dist = dist_map[orig_to_copy_[sink_]];
         for (boost::tie(vi, vi_end) = boost::vertices(graph_copy_); vi != vi_end; ++vi)
         {
             double dist = dist_map[*vi];
 
-            if (dist == std::numeric_limits<double>::max())
+            if (dist == std::numeric_limits<double>::max() ||
+                    dist == std::numeric_limits<double>::infinity())
             {
-                if (out_to_in_.count(*vi) > 0)
-                {
-                    dist = dist_map[out_to_in_[*vi]];
-                }
+                dist = max_dist;
             }
 
             i_distances_[iteration][*vi] = dist;
-
-//            i_distances_[iteration][*vi] = dist_map[*vi];
-
-//            std::cout << iteration << " " << (*vi + 1) << " = " << i_distances_[iteration][*vi] << std::endl;
         }
 
         // Add the new path (the given path is in the copied graph, so the

algo/KShortestPaths3.cpp

@@ -0,0 +1,459 @@
+//
+// Created by wrede on 24.06.16.
+//
+
+#include <boost/graph/copy.hpp>
+#include <boost/graph/bellman_ford_shortest_paths.hpp>
+#include <boost/graph/dijkstra_shortest_paths.hpp>
+#include "KShortestPaths3.h"
+#include "../util/Logger.h"
+#include "../util/FileIO.h"
+
+namespace algo
+{
+    void KShortestPaths3::CreateCopy()
+    {
+        util::Logger::LogDebug("create copy");
+
+        // Clear all previous data
+        graph_copy_.clear();
+        orig_to_copy_.clear();
+        copy_to_orig_.clear();
+        in_to_out_.clear();
+        out_to_in_.clear();
+
+        // Copy the graph and store the vertex map temporarily
+        std::vector<Vertex> original_to_copy_vector(boost::num_vertices(graph_orig_));
+        VertexVertexMap temp_map(original_to_copy_vector.begin());
+        boost::copy_graph(graph_orig_, graph_copy_, boost::orig_to_copy(temp_map));
+
+        // Copy the vertex map into the two persistent maps, one for each
+        // mapping direction
+        boost::graph_traits<DirectedGraph>::vertex_iterator vi, vi_end;
+        for (boost::tie(vi, vi_end) = boost::vertices(graph_orig_);
+             vi != vi_end; ++vi)
+        {
+            Vertex v_original = (*vi);
+            Vertex v_copy = temp_map[v_original];
+
+            orig_to_copy_[v_original] = v_copy;
+            copy_to_orig_[v_copy] = v_original;
+        }
+
+        // Split every vertex in the copied graph
+        for (boost::tie(vi, vi_end) = boost::vertices(graph_orig_);
+             vi != vi_end; ++vi)
+        {
+            Vertex v_orig = *vi;
+            Vertex v_copy_in = orig_to_copy_[v_orig];
+            Vertex v_copy_out = boost::add_vertex(graph_copy_);
+
+            in_to_out_[v_copy_in] = v_copy_out;
+            out_to_in_[v_copy_out] = v_copy_in;
+            copy_to_orig_[v_copy_out] = v_orig;
+        }
+    }
+
+    void KShortestPaths3::UpdateEdges()
+    {
+        util::Logger::LogDebug("update edges");
+
+        util::Logger::LogDebug("remove edges");
+        // Remove the old edges, needs to be done without any iterator since
+        // the removal invalidates any iterators
+        for (auto edge : edges_to_remove_)
+        {
+            boost::remove_edge(edge.first, edge.second, graph_copy_);
+        }
+        edges_to_remove_.clear();
+
+        util::Logger::LogDebug("add edges");
+        // Add the new edges, needs to be done without any iterator since
+        // the addition invalidates any iterators
+        for (auto edge : edges_to_add_)
+        {
+            boost::add_edge(edge.first.first, edge.first.second,
+                            edge.second, graph_copy_);
+        }
+        edges_to_add_.clear();
+    }
+
+    void KShortestPaths3::QueueAddEdge(Vertex source, Vertex target, double weight)
+    {
+        edges_to_add_.push_back(
+                std::pair<std::pair<Vertex, Vertex>, double>(
+                        std::pair<Vertex, Vertex>(source, target), weight));
+    }
+
+    void KShortestPaths3::QueueRemoveEdge(Vertex source, Vertex target)
+    {
+        edges_to_remove_.push_back(std::pair<Vertex, Vertex>(source, target));
+    }
+
+    void KShortestPaths3::QueueCopyEdges()
+    {
+        EdgeWeightMap weights = boost::get(boost::edge_weight, graph_orig_);
+        boost::graph_traits<DirectedGraph>::edge_iterator ei, ei_end;
+        for (boost::tie(ei, ei_end) = boost::edges(graph_orig_);
+             ei != ei_end; ++ei)
+        {
+            Vertex s_orig = boost::source(*ei, graph_orig_);
+            Vertex t_orig = boost::target(*ei, graph_orig_);
+            Vertex s_copy = orig_to_copy_[s_orig];
+            Vertex t_copy = orig_to_copy_[t_orig];
+            double weight = weights[*ei];
+            QueueAddEdge(s_copy, t_copy, weight);
+        }
+    }
+
+    void KShortestPaths3::ClearAllEdges()
+    {
+        boost::graph_traits<DirectedGraph>::vertex_iterator vi, vi_end;
+        for (boost::tie(vi, vi_end) = boost::vertices(graph_copy_); vi != vi_end; ++vi)
+        {
+            boost::clear_out_edges(*vi, graph_copy_);
+        }
+    }
+
+    MultiPredecessorMap KShortestPaths3::Run(DirectedGraph& graph_orig,
+                                             Vertex source, Vertex sink,
+                                             size_t iterations)
+    {
+        util::Logger::LogDebug("run ksp3");
+
+        // Clear all previous data
+        edges_to_add_.clear();
+        edges_to_remove_.clear();
+        i_distances_.clear();
+        i_paths_.clear();
+
+        graph_orig_ = graph_orig;
+        source_ = source;
+        sink_ = sink;
+
+        CreateCopy();
+        FindAndAugment(0);
+        for (size_t i = 0; i < iterations; ++i)
+        {
+            util::Logger::LogDebug("ksp iteration: " + std::to_string(i));
+
+            if (i > 0)
+            {
+                if (PathCosts(i) >= PathCosts(i - 1))
+                {
+                    return MapPathToOrig(i);
+                }
+            }
+
+            ExtendGraph(i);
+            TransformEdges(i > 0 ? i - 1 : 0);
+            FindAndAugment(i + 1);
+            Interlace(i + 1);
+        }
+
+        return MapPathToOrig(iterations);
+    }
+
+    void KShortestPaths3::ExtendGraph(size_t iteration)
+    {
+        util::Logger::LogDebug("extend graph iteration: " + std::to_string(iteration));
+
+        // Clear all previous working data
+        util::Logger::LogDebug("clear all edges");
+        ClearAllEdges();
+        util::Logger::LogDebug("queue copy edges");
+        QueueCopyEdges();
+        UpdateEdges();
+
+        util::Logger::LogDebug("iterate vertices");
+        MultiPredecessorMap& paths = i_paths_[iteration];
+        boost::graph_traits<DirectedGraph>::out_edge_iterator oei, oei_end;
+        boost::graph_traits<DirectedGraph>::edge_iterator ei, ei_end;
+        boost::graph_traits<DirectedGraph>::vertex_iterator vi, vi_end;
+        EdgeWeightMap weights = boost::get(boost::edge_weight, graph_copy_);
+
+        // Iterate all vertices within the original graph
+        for (boost::tie(vi, vi_end) = boost::vertices(graph_orig_); vi != vi_end; ++vi)
+        {
+            Vertex v_orig = *vi;
+            Vertex v_copy_in = orig_to_copy_[v_orig];
+
+            // Ignore vertices off the paths, the source and the sink
+            if (paths.count(v_copy_in) == 0 || v_orig == source_ || v_orig == sink_)
+            {
+                continue;
+            }
+
+            Vertex v_copy_out = in_to_out_[v_copy_in];
+
+            // Iterate all input edges
+            for (boost::tie(oei, oei_end) = boost::out_edges(v_copy_in, graph_copy_);
+                 oei != oei_end; ++oei)
+            {
+                // Copy the output edges to the output vertices
+                QueueAddEdge(v_copy_out, boost::target(*oei, graph_copy_), weights[*oei]);
+
+                // Remove the old output edge
+                QueueRemoveEdge(v_copy_in, boost::target(*oei, graph_copy_));
+            }
+
+            // Create auxiliary edge
+            QueueAddEdge(v_copy_in, v_copy_out, 0.0);
+        }
+
+        UpdateEdges();
+
+        util::Logger::LogDebug("iterate edges");
+        // Iterate all edges within the copied graph
+        weights = boost::get(boost::edge_weight, graph_copy_);
+        for (boost::tie(ei, ei_end) = boost::edges(graph_copy_); ei != ei_end; ++ei)
+        {
+            Vertex s_copy = boost::source(*ei, graph_copy_);
+            Vertex t_copy = boost::target(*ei, graph_copy_);
+            double weight = weights[*ei];
+
+            // If the edge is part of the paths
+            if (paths.count(t_copy) > 0 && paths[t_copy].count(s_copy) > 0)
+            {
+                // Add the edge with direction and weight inverted
+                QueueAddEdge(t_copy, s_copy, -weight);
+
+                // Remove the old edge
+                QueueRemoveEdge(s_copy, t_copy);
+            }
+        }
+
+        util::Logger::LogDebug("update edges");
+        UpdateEdges();
+
+//        util::FileIO::WriteCSVMatlab(graph_copy_, "/home/wrede/Dokumente/graph_" + std::to_string(iteration) + "e.csv");
+    }
+
+    void KShortestPaths3::TransformEdges(size_t iteration)
+    {
+        util::Logger::LogDebug("transform edges iteration: " + std::to_string(iteration));
+
+        std::unordered_map<Vertex, double>& distances = i_distances_[iteration];
+
+        EdgeWeightMap weights = boost::get(boost::edge_weight, graph_copy_);
+        boost::graph_traits<DirectedGraph>::edge_iterator ei, ei_end;
+        for (boost::tie(ei, ei_end) = boost::edges(graph_copy_); ei != ei_end; ++ei)
+        {
+            Vertex s_copy = boost::source(*ei, graph_copy_);
+            Vertex t_copy = boost::target(*ei, graph_copy_);
+
+            // Transform the edge weights with the distances calculated in
+            // the specified path finding iteration
+            if (distances.count(s_copy) > 0 && distances.count(t_copy) > 0)
+            {
+                weights[*ei] += distances[s_copy] - distances[t_copy];
+            }
+
+            // Correct rounding errors
+            if (weights[*ei] >= -0.0000001 && weights[*ei] <= 0.0000001)
+            {
+                weights[*ei] = 0.0;
+            }
+
+            //TODO debug output (experimental)
+            if (weights[*ei] < 0.0)
+            {
+                util::Logger::LogDebug(std::to_string(s_copy)
+                                       + " -> " + std::to_string(weights[*ei])
+                                       + " -> " + std::to_string(t_copy));
+            }
+        }
+
+//        util::FileIO::WriteCSVMatlab(graph_copy_, "/home/wrede/Dokumente/graph_" + std::to_string(iteration) + "t.csv");
+    }
+
+    void KShortestPaths3::FindAndAugment(size_t iteration)
+    {
+        util::Logger::LogDebug("find and augment iteration: " + std::to_string(iteration));
+
+        // Add new path maps until the needed iteration is reached
+        while (i_paths_.size() < (iteration + 1))
+        {
+            i_paths_.push_back(MultiPredecessorMap());
+        }
+
+        // Add new distance maps until the needed iteration is reached
+        while (i_distances_.size() < (iteration + 1))
+        {
+            i_distances_.push_back(std::unordered_map<Vertex, double>());
+        }
+
+        // Only copy old paths if old paths exist
+        if (iteration > 0)
+        {
+            // Copy the old paths
+            for (auto it = i_paths_[iteration - 1].begin();
+                 it != i_paths_[iteration - 1].end(); ++it)
+            {
+                i_paths_[iteration][it->first] = it->second;
+            }
+        }
+
+        // Prepare variables for path finding
+        size_t graph_size = boost::num_vertices(graph_copy_);
+        Vertex root_vertex = orig_to_copy_[source_];
+        std::vector<Vertex> pred_list(graph_size);
+        std::vector<double> dist_list(graph_size);
+        VertexIndexMap graph_indices = boost::get(boost::vertex_index, graph_copy_);
+        EdgeWeightMap weight_map = boost::get(boost::edge_weight, graph_copy_);
+        PredecessorMap pred_map(&pred_list[0], graph_indices);
+        DistanceMap dist_map(&dist_list[0], graph_indices);
+
+        // Find the shortest path
+        if (iteration == 0)
+        {
+            boost::bellman_ford_shortest_paths(graph_copy_,
+                                               graph_size,
+                                               boost::root_vertex(root_vertex)
+                                                       .weight_map(weight_map)
+                                                       .predecessor_map(pred_map)
+                                                       .distance_map(dist_map));
+        }
+        else
+        {
+            boost::dijkstra_shortest_paths(graph_copy_,
+                                           root_vertex,
+                                           boost::predecessor_map(pred_map)
+                                                   .distance_map(dist_map));
+        }
+
+        // Add the new distances
+        boost::graph_traits<DirectedGraph>::vertex_iterator vi, vi_end;
+        for (boost::tie(vi, vi_end) = boost::vertices(graph_copy_); vi != vi_end; ++vi)
+        {
+            double dist = dist_map[*vi];
+
+            if (dist == std::numeric_limits<double>::max())
+            {
+                if (out_to_in_.count(*vi) > 0)
+                {
+                    dist = dist_map[out_to_in_[*vi]];
+                }
+            }
+
+            i_distances_[iteration][*vi] = dist;
+
+//            i_distances_[iteration][*vi] = dist_map[*vi];
+
+//            std::cout << iteration << " " << (*vi + 1) << " = " << i_distances_[iteration][*vi] << std::endl;
+        }
+
+        // Add the new path
+        Vertex sink_copy = orig_to_copy_[sink_];
+        for (Vertex u_copy = sink_copy, v_copy = pred_map[u_copy];
+             u_copy != v_copy; u_copy = v_copy, v_copy = pred_map[v_copy])
+        {
+            i_paths_[iteration][u_copy].insert(v_copy);
+        }
+    }
+
+    void KShortestPaths3::Interlace(size_t iteration)
+    {
+        util::Logger::LogDebug("interlace iteration: " + std::to_string(iteration));
+
+        MultiPredecessorMap& paths = i_paths_[iteration];
+
+        boost::graph_traits<DirectedGraph>::edge_iterator ei, ei_end;
+        for (boost::tie(ei, ei_end) = boost::edges(graph_copy_);
+             ei != ei_end; ++ei)
+        {
+            Vertex s_copy = boost::source(*ei, graph_copy_);
+            Vertex t_copy = boost::target(*ei, graph_copy_);
+
+            // Ignore source and sink
+            if (s_copy == orig_to_copy_[source_] || t_copy == orig_to_copy_[sink_])
+            {
+                continue;
+            }
+
+            // Is edge within paths?
+            if (paths.count(t_copy) > 0 && paths.count(s_copy) > 0)
+            {
+                // Is edge duplicate?
+                if (paths.count(s_copy) > 0 && paths[s_copy].count(t_copy) > 0)
+                {
+                    paths[t_copy].erase(s_copy);
+                    paths[s_copy].erase(t_copy);
+                }
+            }
+        }
+    }
+
+    double KShortestPaths3::PathCosts(size_t iteration)
+    {
+        util::Logger::LogDebug("path costs iteration: " + std::to_string(iteration));
+
+//        MultiPredecessorMap& paths = i_paths_[iteration];
+        double value = 0.0;
+
+//        EdgeWeightMap weights = boost::get(boost::edge_weight, graph_orig_);
+//        boost::graph_traits<DirectedGraph>::edge_iterator ei, ei_end;
+//        for (boost::tie(ei, ei_end) = boost::edges(graph_orig_);
+//             ei != ei_end; ++ei)
+//        {
+//            Vertex s_orig = boost::source(*ei, graph_orig_);
+//            Vertex t_orig = boost::target(*ei, graph_orig_);
+//
+//            // If the edge is on the path, add the edge weight to the overall
+//            // path costs
+//            if (paths.count(t_orig) > 0 && paths[t_orig].count(s_orig) > 0)
+//            {
+//                value += weights[*ei];
+//            }
+//        }
+
+        for (int i = 0; i < iteration; ++i)
+        {
+            value += i_distances_[i][orig_to_copy_[sink_]];
+        }
+
+        util::Logger::LogDebug("calculated costs: " + std::to_string(value));
+
+        return value;
+    }
+
+    KShortestPaths3::KShortestPaths3()
+    {
+        /* EMPTY */
+    }
+
+    KShortestPaths3::~KShortestPaths3()
+    {
+        /* EMPTY */
+    }
+
+    MultiPredecessorMap KShortestPaths3::MapPathToOrig(size_t iteration)
+    {
+        MultiPredecessorMap& in_paths = i_paths_[iteration];
+        MultiPredecessorMap out_paths;
+
+        for (Vertex first : in_paths[orig_to_copy_[sink_]])
+        {
+            for (Vertex u = first, v = (*in_paths[u].begin()); u != v;
+                 u = v, v = (*in_paths[v].begin()))
+            {
+                Vertex u_orig = copy_to_orig_[u];
+                Vertex v_orig = copy_to_orig_[v];
+
+                if (u_orig == v_orig)
+                {
+                    continue;
+                }
+
+                out_paths[u_orig].insert(v_orig);
+
+                if (v_orig == source_)
+                {
+                    break;
+                }
+            }
+        }
+
+        return out_paths;
+    }
+}

algo/KShortestPaths3.h

@@ -0,0 +1,51 @@
+//
+// Created by wrede on 24.06.16.
+//
+
+#ifndef GBMOT_KSHORTESTPATHS3_H
+#define GBMOT_KSHORTESTPATHS3_H
+
+#include "../graph/Definitions.h"
+
+namespace algo
+{
+    class KShortestPaths3
+    {
+    private:
+        DirectedGraph graph_orig_;
+        DirectedGraph graph_copy_;
+        Vertex source_;
+        Vertex sink_;
+        std::unordered_map<Vertex, Vertex> orig_to_copy_;
+        std::unordered_map<Vertex, Vertex> copy_to_orig_;
+        std::unordered_map<Vertex, Vertex> in_to_out_;
+        std::unordered_map<Vertex, Vertex> out_to_in_;
+        std::vector<std::pair<std::pair<Vertex, Vertex>, double>> edges_to_add_;
+        std::vector<std::pair<Vertex, Vertex>> edges_to_remove_;
+        std::vector<std::unordered_map<Vertex, double>> i_distances_;
+        std::vector<MultiPredecessorMap> i_paths_;
+
+        void CreateCopy();
+        void ExtendGraph(size_t iteration);
+        void TransformEdges(size_t iteration);
+        void FindAndAugment(size_t iteration);
+        void Interlace(size_t iteration);
+        MultiPredecessorMap MapPathToOrig(size_t iteration);
+        double PathCosts(size_t iteration);
+
+        void QueueCopyEdges();
+        void ClearAllEdges();
+        void QueueRemoveEdge(Vertex source, Vertex target);
+        void QueueAddEdge(Vertex source, Vertex target, double weight);
+        void UpdateEdges();
+    public:
+        KShortestPaths3();
+        ~KShortestPaths3();
+        MultiPredecessorMap Run(DirectedGraph& graph_orig,
+                                Vertex source, Vertex sink,
+                                size_t iterations);
+    };
+}
+
+
+#endif //GBMOT_KSHORTESTPATHS3_H

algo/KShortestPaths4.cpp

@@ -0,0 +1,270 @@
+//
+// Created by wrede on 24.06.16.
+//
+
+#include <boost/graph/named_function_params.hpp>
+#include <boost/graph/bellman_ford_shortest_paths.hpp>
+#include "KShortestPaths4.h"
+
+namespace algo
+{
+    KShortestPaths4::KShortestPaths4(DirectedGraph graph, Vertex source, Vertex sink,
+                                     size_t max_paths_count)
+    {
+        graph_ = graph;
+        source_ = source;
+        sink_ = sink;
+
+        vertex_labels_ = VertexDistanceMap();
+        vertex_distances_ = VertexDistanceMap();
+        vertex_predecessors_ = VertexVertexMap();
+        vertex_candidates_ = std::vector<Vertex>();
+
+        i_shortest_paths_ = std::vector<VertexVertexMap>();
+
+        max_paths_count_ = max_paths_count;
+
+        total_paths_count_ = 0;
+        total_paths_distance_ = 0.0;
+    }
+
+    void KShortestPaths4::Run()
+    {
+        Initialization();
+    }
+
+    void KShortestPaths4::Initialization()
+    {
+        // Find the first path with predecessors and distances
+        size_t graph_size = boost::num_vertices(graph_);
+        std::vector<Vertex> pred_list(graph_size);
+        std::vector<double> dist_list(graph_size);
+        VertexIndexMap graph_indices = boost::get(boost::vertex_index, graph_);
+        EdgeWeightMap weight_map = boost::get(boost::edge_weight, graph_);
+        PredecessorMap path_pred_map(&pred_list[0], graph_indices);
+        DistanceMap path_dist_map(&dist_list[0], graph_indices);
+        boost::bellman_ford_shortest_paths(graph_, graph_size,
+                                           boost::root_vertex(source_)
+                                                   .weight_map(weight_map)
+                                                   .predecessor_map(path_pred_map)
+                                                   .distance_map(path_dist_map));
+        double path_total_distance = path_dist_map[sink_];
+
+        //TODO add the path to the i_shortest_paths_;
+
+        // If only one path is needed the algorithm can terminate successful
+        if (max_paths_count_ == 1) return; //TODO return with the found path
+
+        // Store the vertices of the path that are the predecessor of the source vertex
+        // and the successor of the sink vertex
+        Vertex source_predecessor = source_;
+        Vertex sink_successor = sink_;
+        for (Vertex u = path_pred_map[sink_], v = path_pred_map[u]; u != v; u = v, v = path_pred_map[v])
+        {
+            if (v == sink_)
+            {
+                source_predecessor = u;
+                break;
+            }
+        }
+        sink_successor = path_pred_map[sink_];
+
+        // Set the distance label for all vertices
+        boost::graph_traits<DirectedGraph>::vertex_iterator vi, vi_end;
+        for (boost::tie(vi, vi_end) = boost::vertices(graph_); vi != vi_end; ++vi)
+        {
+            double distance = path_dist_map[*vi];
+
+            // If the vertex is not part of the spanning tree, set the distance to the total
+            // distance of the path from source to sink
+            if (path_pred_map[*vi] == *vi && vi != source_)
+            {
+                distance = path_total_distance;
+            }
+
+            vertex_labels_[*vi] = distance;
+        }
+
+        // Define the candidate list
+        for (boost::tie(vi, vi_end) = boost::vertices(graph_); vi != vi_end; ++vi)
+        {
+            if (*vi == source_ || *vi == source_predecessor) continue;
+
+            vertex_candidates_.push_back(*vi);
+        }
+
+        // For each candidate define the distance and predecessors
+        EdgeWeightMap edge_weights = boost::get(boost::edge_weight, graph_);
+        boost::graph_traits<DirectedGraph>::edge_descriptor ed;
+        bool e_found;
+        for (Vertex v : vertex_candidates_)
+        {
+            if (path_pred_map[v] == source_)
+            {
+                boost::tie(ed, e_found) = boost::edge(source_, v, graph_);
+                vertex_distances_[v] = boost::get(edge_weights, ed) - vertex_labels_[v];
+                vertex_predecessors_[v] = source_;
+            }
+            else
+            {
+                vertex_distances_[v] = std::numeric_limits<double>::max();
+                vertex_predecessors_[v] = std::numeric_limits<size_t>::max();
+            }
+        }
+
+        total_paths_count_ = 1;
+        total_paths_distance_ = path_total_distance;
+
+        InterlacingConstruction();
+    }
+
+    void KShortestPaths4::InterlacingConstruction()
+    {
+        // Find the vertex in the candidate list with the minimum distance
+        Vertex min_distance_vertex = 0;
+        size_t min_distance_index = 0;
+        double min_distance = std::numeric_limits<double>::max();
+        for (size_t i = 0; i < vertex_candidates_.size(); ++i)
+        {
+            Vertex v = vertex_candidates_[i];
+            double distance = vertex_distances_[v];
+            if (distance < min_distance)
+            {
+                min_distance_vertex = v;
+                min_distance_index = i;
+                min_distance = distance;
+            }
+        }
+
+        if (min_distance == std::numeric_limits<double>::max())
+        {
+            NonFeasibleTermination();
+            return;
+        }
+
+        // Add the distance to the vertex label and remove the vertex from the candidates
+        vertex_labels_[min_distance_vertex] += min_distance;
+        vertex_candidates_.erase(vertex_candidates_.begin() + min_distance_index);
+
+        if (min_distance_vertex == sink_)
+        {
+            //TODO goto P_M+1 Definition
+        }
+        else
+        {
+            for (Vertex v : i_shortest_paths_[total_paths_count_ - 1])
+            {
+                if (min_distance_vertex == v)
+                {
+                    NegativeInterlacing(min_distance_vertex);
+                    return;
+                }
+            }
+
+            NeighborDistanceTest(min_distance_vertex);
+            InterlacingConstruction();
+        }
+    }
+
+    void KShortestPaths4::NeighborDistanceTest(Vertex r)
+    {
+        // Compute the distance to all neighbors and find the best fitting neighbor,
+        // than save that neighbor and the new calculated distance
+        EdgeWeightMap edge_weights = boost::get(boost::edge_weight, graph_);
+        boost::graph_traits<DirectedGraph>::edge_iterator oei, oei_end;
+        for (boost::tie(oei, oei_end) = boost::out_edges(r, graph_); oei != oei_end; ++oei)
+        {
+            Vertex j = boost::target(*oei, graph_);
+
+            // Check if the vertex is a candidate
+            if (Contains(vertex_candidates_, j))
+            {
+                // Calculate possible new edge weight for the candidate
+                double delta = vertex_labels_[r] + edge_weights[*oei] - vertex_labels_[j];
+
+                if (vertex_distances_[j] > delta)
+                {
+                    vertex_distances_[j] = delta;
+                    vertex_predecessors_[j] = r;
+                }
+            }
+        }
+    }
+
+    void KShortestPaths4::NegativeInterlacing(Vertex input)
+    {
+        // Find the path containing the specified vertex
+        std::list<Vertex> path;
+        for (auto p : i_shortest_paths_)
+        {
+            if (Contains(p, input) > 0)
+            {
+                path = p;
+                break;
+            }
+        }
+        // Iterate the path in reverse vertex order //TODO check for correct order
+        Vertex j;
+        bool found_input = false;
+        bool is_last = false;
+        for (auto vertex = path.rbegin(); vertex != path.rend(); ++vertex)
+        {
+            // Find the first vertex which is not a candidate
+            if (!found_input)
+            {
+                if (*vertex == input)
+                {
+                    found_input = true;
+                }
+            }
+            else
+            {
+                j = *vertex;
+                break;
+            }
+        }
+
+        //TODO implement
+    }
+
+    void KShortestPaths4::FeasibleTermination()
+    {
+        //TODO implement
+    }
+
+    void KShortestPaths4::NonFeasibleTermination()
+    {
+        //TODO implement
+    }
+
+    bool KShortestPaths4::Contains(std::vector<Vertex>& vector, Vertex& element)
+    {
+        for (Vertex v : vector)
+        {
+            if (v == element)
+            {
+                return true;
+            }
+        }
+
+        return false;
+    }
+
+    bool KShortestPaths4::Contains(std::list<Vertex>& list, Vertex& element)
+    {
+        for (Vertex v : list)
+        {
+            if (v == element)
+            {
+                return true;
+            }
+        }
+
+        return false;
+    }
+}
+
+
+
+
+

algo/KShortestPaths4.h

@@ -0,0 +1,45 @@
+//
+// Created by wrede on 24.06.16.
+//
+
+#ifndef GBMOT_KSHORTESTPATHS4_H
+#define GBMOT_KSHORTESTPATHS4_H
+
+#include "../graph/Definitions.h"
+
+namespace algo
+{
+    class KShortestPaths4
+    {
+    private:
+        DirectedGraph graph_;
+        Vertex source_;
+        Vertex sink_;
+        VertexDistanceMap vertex_labels_;
+        VertexDistanceMap vertex_distances_;
+        VertexVertexMap vertex_predecessors_;
+        std::vector<Vertex> vertex_candidates_;
+
+        std::vector<std::list<Vertex>> i_shortest_paths_;
+
+        size_t max_paths_count_;
+
+        size_t total_paths_count_;
+        double total_paths_distance_;
+
+        void Initialization();
+        void InterlacingConstruction();
+        void NeighborDistanceTest(Vertex r);
+        void NegativeInterlacing(Vertex input);
+        void FeasibleTermination();
+        void NonFeasibleTermination();
+
+        bool Contains(std::vector<Vertex>& vector, Vertex& element);
+        bool Contains(std::list<Vertex>& list, Vertex& element);
+    public:
+        KShortestPaths4(DirectedGraph graph, Vertex source, Vertex sink, size_t max_paths_count);
+        void Run();
+    };
+}
+
+#endif //GBMOT_KSHORTESTPATHS4_H

graph/Definitions.h

@@ -26,6 +26,8 @@ typedef boost::property_map<DirectedGraph, boost::vertex_name_t>::type
 typedef boost::iterator_property_map<Vertex*, VertexIndexMap, Vertex, Vertex&>
         PredecessorMap;
 typedef std::unordered_map<Vertex, std::set<Vertex>> MultiPredecessorMap;
+typedef std::unordered_map<Vertex, std::set<Vertex>> VertexAdjacencyMap;
+typedef std::unordered_map<Vertex, double> VertexDistanceMap;
 typedef boost::iterator_property_map<double*, VertexIndexMap, double, double&>
         DistanceMap;
 typedef boost::iterator_property_map<

main/main.cpp

@@ -11,6 +11,7 @@
 #include "../core/ObjectDataAngular.h"
 #include "../algo/Berclaz.h"
 #include "../algo/KShortestPaths2.h"
+#include "../algo/KShortestPaths3.h"
 #include <boost/program_options.hpp>
 #include <boost/graph/named_function_params.hpp>
 #include <boost/graph/bellman_ford_shortest_paths.hpp>
@@ -817,7 +818,7 @@ int main(int argc, char** argv)
     Vertex source, sink;
     CreatePresentationGraph(graph, source, sink);
 
-//    util::FileIO::WriteCSVMatlab(graph, "/home/wrede/Dokumente/graph.csv");
+    util::FileIO::WriteCSVMatlab(graph, "/home/wrede/Dokumente/graph.csv");
 
     TestKBellmanFord(graph, source, sink, 2);
     TestKSP(graph, source, sink, 3);