added another ksp implementation

CMakeLists.txt

@@ -26,7 +26,7 @@ set(SOURCE_FILES
         core/ObjectDataBox.cpp core/ObjectDataBox.h
         algo/KShortestPaths2.cpp algo/KShortestPaths2.h
         algo/KShortestPaths3.cpp algo/KShortestPaths3.h
-        algo/KShortestPaths4.cpp algo/KShortestPaths4.h)
+        algo/KShortestPaths4.cpp algo/KShortestPaths4.h algo/KShortestPaths5.cpp algo/KShortestPaths5.h)
 
 find_package(Doxygen)
 

algo/Berclaz.cpp

@@ -9,6 +9,7 @@
 #include "../util/FileIO.h"
 #include "KShortestPaths2.h"
 #include "KShortestPaths3.h"
+#include "KShortestPaths4.h"
 
 namespace algo
 {
@@ -161,11 +162,26 @@ namespace algo
             Vertex source, sink;
             CreateGraph(graph, source, sink, grid);
 
-            KShortestPaths3 ksp;
-            MultiPredecessorMap ksp_result = ksp.Run(graph, source, sink, max_track_count);
+            util::Logger::LogDebug("run suurballe");
+//            KShortestPaths3 ksp;
+//            MultiPredecessorMap ksp_result = ksp.Run(graph, source, sink, max_track_count);
+            KShortestPaths4 suurballe(graph, source, sink, max_track_count);
+            suurballe.Run();
 
             util::Logger::LogDebug("extract tracks");
-            ExtractTracks(graph, ksp_result, sink, tracks);
+            suurballe.GetTracks(tracks);
+
+//            size_t l = 0;
+//            for (std::vector<Vertex> path : suurballe.GetPaths())
+//            {
+//                l++;
+//                std::cout << "P" << l << "{ ";
+//                for (Vertex v : path)
+//                {
+//                    std::cout << v << " ";
+//                }
+//                std::cout << "}\n";
+//            }
         }
 
         // Only connect tracks if the sequence was split

algo/KShortestPaths4.cpp

@@ -7,7 +7,7 @@
 #include <boost/graph/copy.hpp>
 #include "KShortestPaths4.h"
 #include <iomanip>
-#define DEBUG
+//#define DEBUG
 
 namespace algo
 {
@@ -240,14 +240,16 @@ namespace algo
         }
 
         // For each node on path between (and different from) J and I
-        for (auto v = path_predecessors_[vertex_i]; v != vertex_j; v = path_predecessors_[v])
+        for (auto v = path_predecessors_[vertex_i]; ; v = path_predecessors_[v])
         {
             // Remove the node from candidates
             Remove(vertex_candidates_, v);
+
+            if (v == vertex_j) break;
         }
 
         // For each node on path between (and different from) J and I
-        for (auto v = path_predecessors_[vertex_i]; v != vertex_j; v = path_predecessors_[v])
+        for (auto v = path_predecessors_[vertex_i]; ; v = path_predecessors_[v])
         {
             // Increase the label by the distance to I
             vertex_labels_[v] += vertex_distances_[vertex_i];
@@ -257,6 +259,8 @@ namespace algo
             interlacing_predecessors_positive_[v] = false;
 
             NeighborDistanceTest(v);
+
+            if (v == vertex_j) break;
         }
 
         // Save L_J and compute L_J' to perform a neighbor distance test with L_J' and then restore L_J
@@ -281,12 +285,13 @@ namespace algo
             }
         }
 
-
         InterlacingConstruction();
     }
 
     void KShortestPaths4::NextPathDefinition()
     {
+        std::cout << "NextPathDefinition\n";
+
         // Create a copy of the paths to work with
         VertexPredecessorMap path_predecessors_old = path_predecessors_;
 
@@ -333,9 +338,9 @@ namespace algo
                 }
                 else
                 {
-                    for (auto &v : sink_neighbors_)
+                    for (auto v : sink_neighbors_)
                     {
-                        Vertex successor = FindPathSuccessor(path_predecessors_, source_, v, vertex_j);
+                        Vertex successor = FindPathSuccessor(path_predecessors_old, source_, v, vertex_j);
                         if (successor != vertex_j)
                         {
                             vertex_t = successor;
@@ -470,8 +475,8 @@ namespace algo
         return element;
     }
 
-    Vertex KShortestPaths4::FindPathSuccessor(VertexPredecessorMap& map, Vertex origin, Vertex destination,
-                                              Vertex element)
+    Vertex KShortestPaths4::FindPathSuccessor(VertexPredecessorMap& map, Vertex origin,
+                                              Vertex destination, Vertex element)
     {
         for (auto u = destination, v = map[u]; u != origin; u = v, v = map[v])
         {
@@ -548,6 +553,23 @@ namespace algo
     double KShortestPaths4::GetTotalPathsLength() {
         return total_paths_distance_;
     }
+
+    void KShortestPaths4::GetTracks(std::vector<core::TrackletPtr>& tracks)
+    {
+        VertexValueMap values = boost::get(boost::vertex_name, graph_);
+
+        for (auto u : sink_neighbors_)
+        {
+            core::TrackletPtr tlt(new core::Tracklet());
+
+            for (auto v = u; v != source_; v = path_predecessors_[v])
+            {
+                tlt->AddPathObject(values[v]);
+            }
+
+            tracks.push_back(tlt);
+        }
+    }
 }
 
 

algo/KShortestPaths4.h

@@ -6,6 +6,7 @@
 #define GBMOT_KSHORTESTPATHS4_H
 
 #include "../graph/Definitions.h"
+#include "../core/Tracklet.h"
 
 namespace algo
 {
@@ -54,6 +55,7 @@ namespace algo
         void Run();
 
         std::vector<std::vector<Vertex>> GetPaths();
+        void GetTracks(std::vector<core::TrackletPtr>& tracks);
         double GetTotalPathsLength();
     };
 }

algo/KShortestPaths5.cpp

@@ -0,0 +1,354 @@
+//
+// Created by wrede on 28.06.16.
+//
+
+#include <boost/graph/named_function_params.hpp>
+#include <boost/graph/dijkstra_shortest_paths.hpp>
+#include <boost/graph/copy.hpp>
+#include <boost/graph/bellman_ford_shortest_paths.hpp>
+#include "KShortestPaths5.h"
+#define DEBUG
+
+namespace algo
+{
+
+    KShortestPaths5::KShortestPaths5(DirectedGraph input_graph, Vertex source, Vertex sink)
+    {
+        orig_graph_ = input_graph;
+        source_ = source;
+        sink_ = sink;
+    }
+
+    void KShortestPaths5::Run(size_t max_path_count)
+    {
+        paths_.clear();
+        sink_neighbors_.clear();
+        switch (max_path_count)
+        {
+            case 0:
+                break;
+            case 1:
+                FindPath(orig_graph_, source_, sink_, paths_);
+                sink_neighbors_.push_back(paths_[sink_]);
+                break;
+            case 2:
+                FindPathPair();
+                break;
+            default:
+                FindPaths(max_path_count);
+        }
+    }
+
+    void KShortestPaths5::FindPath(DirectedGraph& graph, Vertex& source, Vertex& sink,
+                                   VertexPredecessorMap& predecessors)
+    {
+        // The predecessors and the distances
+        std::vector<Vertex> p(boost::num_vertices(graph));
+        std::vector<Weight> d(boost::num_vertices(graph));
+
+        // Scan the graph for negative edge weights to use the proper algorithm
+        bool negative_edges = false;
+        EdgeIter ei, ei_end;
+        for (boost::tie(ei, ei_end) = boost::edges(graph); ei != ei_end; ++ei)
+        {
+            if (boost::get(boost::edge_weight, graph, *ei) < 0)
+            {
+                negative_edges = true;
+                break;
+            }
+        }
+
+#ifdef DEBUG
+        std::cout << "the specified graph has "
+                  << (negative_edges ? "negative" : "only positive")
+                  << " edge weights" << std::endl;
+#endif
+
+        if (negative_edges)
+        {
+            // Run bellman ford to find the single-source shortest paths
+            boost::bellman_ford_shortest_paths(
+                    graph,
+                    boost::num_vertices(graph),
+                    boost::root_vertex(source)
+                            .predecessor_map(
+                                    boost::make_iterator_property_map(
+                                            p.begin(), boost::get(boost::vertex_index, graph)))
+                            .distance_map(
+                                    boost::make_iterator_property_map(
+                                            d.begin(), boost::get(boost::vertex_index, graph))));
+        }
+        else
+        {
+            // Run dijkstra to find the single-source shortest paths
+            boost::dijkstra_shortest_paths(
+                    graph,
+                    source,
+                    boost::predecessor_map(
+                            boost::make_iterator_property_map(
+                                    p.begin(),
+                                    boost::get(boost::vertex_index, graph)))
+                            .distance_map(
+                                    boost::make_iterator_property_map(
+                                            d.begin(),
+                                            boost::get(boost::vertex_index, graph))));
+        }
+
+#ifdef DEBUG
+        {
+            std::cout << "distances and parents:" << std::endl;
+            VertexIter vi, vend;
+            for (boost::tie(vi, vend) = vertices(graph); vi != vend; ++vi)
+            {
+                std::cout << "distance(" << *vi << ") = " << d[*vi] << ", ";
+                std::cout << "parent(" << *vi << ") = " << p[*vi] << std::endl;
+            }
+            std::cout << std::endl;
+        }
+#endif
+
+#ifdef DEBUG
+        std::cout << "path: ";
+        std::cout << sink;
+#endif
+        // Record the vertices already visited to detect negative cycles
+        std::unordered_map<Vertex, bool> visited_vertices;
+        VertexIter vi, vi_end;
+        for (boost::tie(vi, vi_end) = boost::vertices(graph); vi != vi_end; ++vi)
+        {
+            visited_vertices[*vi] = false;
+        }
+
+        // Insert the path from the specified source to target into the specified map
+        for (auto u = sink, v = p[u]; u != source; u = v, v = p[u])
+        {
+            if (visited_vertices[u])
+            {
+                std::cerr << "error: negative cycles are not allowed" << std::endl;
+                break;
+            }
+
+            predecessors[u] = v;
+            visited_vertices[u] = true;
+#ifdef DEBUG
+            std::cout << "<" << v;
+#endif
+        }
+
+#ifdef DEBUG
+        std::cout << std::endl;
+#endif
+    }
+
+    void KShortestPaths5::FindPathPair()
+    {
+        VertexIter vi, vi_end;
+        EdgeIter ei, ei_end;
+
+        // Find the first path
+        VertexPredecessorMap orig_first_path;
+        FindPath(orig_graph_, source_, sink_, orig_first_path);
+
+        // Transform the graph, invert the direction and weight of every edge in the first path
+        // found, then add concomitant vertices for every vertex on the path, eventually check that
+        // every edge pointing on the path should end in the newly created vertex in the splitting
+        // process
+
+        // Create the graph to transform and create the map to map from vertices in the transformed
+        // graph to vertices in the original graph, at first every vertex is mapped to itself
+        DirectedGraph trans_graph;
+        for (boost::tie(vi, vi_end) = boost::vertices(orig_graph_); vi != vi_end; ++vi)
+        {
+            boost::add_vertex(trans_graph);
+        }
+
+        // Transform the first path by inverting edges (and weights) and splitting nodes along the
+        // path
+
+        // Reverse the first path
+        VertexPredecessorMap trans_first_path;
+        for (auto u = sink_, v = orig_first_path[u]; u != source_; u = v, v = orig_first_path[u])
+        {
+            trans_first_path[v] = u;
+        }
+
+        // Invert edges
+        for (auto u = sink_, v = orig_first_path[u]; u != source_; u = v, v = orig_first_path[u])
+        {
+            Edge edge;
+            bool e_found;
+            boost::tie(edge, e_found) = boost::edge(v, u, orig_graph_);
+            if (e_found)
+            {
+                Weight w = boost::get(boost::edge_weight, orig_graph_, edge);
+
+                boost::add_edge(u, v, -w, trans_graph);
+            }
+            else
+            {
+                std::cerr << "error: edge not found " << v << " -> " << u << std::endl;
+            }
+        }
+
+#ifdef DEBUG
+        std::cout << "inverted edges: " << std::endl;
+        for (boost::tie(ei, ei_end) = boost::edges(trans_graph); ei != ei_end; ++ei)
+        {
+            std::cout << boost::source(*ei, trans_graph)
+                      << " > " << boost::target(*ei, trans_graph)
+                      << " | " << boost::get(boost::edge_weight, trans_graph, *ei)
+                      << std::endl;
+        }
+        std::cout << std::endl;
+#endif
+
+        // Split nodes
+        VertexPredecessorMap old_to_new;
+        VertexPredecessorMap new_to_old;
+        for (auto u = orig_first_path[sink_], v = orig_first_path[u];
+             v != source_;
+             u = v, v = orig_first_path[u])
+        {
+            // Create the concomitant vertex
+            Vertex new_u = boost::add_vertex(trans_graph);
+            old_to_new[u] = new_u;
+            new_to_old[new_u] = u;
+
+            // Retrieve the weight from the original path in the original graph
+            Weight w = 0.0;
+            Edge edge;
+            bool e_found;
+            boost::tie(edge, e_found) = boost::edge(v, u, orig_graph_);
+            if (e_found)
+            {
+                w = boost::get(boost::edge_weight, trans_graph, edge);
+            }
+            else
+            {
+                std::cerr << "error: edge not found " << v << " -> " << u << std::endl;
+            }
+
+            // Create the edge from the concomitant vertex to the path predecessor
+            boost::add_edge(new_u, v, -w, trans_graph);
+        }
+
+        // Add all remaining edges
+        for (boost::tie(ei, ei_end) = boost::edges(orig_graph_); ei != ei_end; ++ei)
+        {
+            Vertex source = boost::source(*ei, orig_graph_);
+            Vertex target = boost::target(*ei, orig_graph_);
+            Weight weight = boost::get(boost::edge_weight, orig_graph_, *ei);
+
+            // Ignore vertices on the path (they are already added)
+            if (orig_first_path.count(target) > 0 && orig_first_path[target] == source)
+                continue;
+
+            // If the edge points to source or sink add the edge unchanged
+            if (target == source_ || target == sink_)
+            {
+                boost::add_edge(source, target, weight, trans_graph);
+                continue;
+            }
+
+            // If the edge points to split vertices (vertices on the path except source and sink),
+            // point the edge towards the concomitant vertex
+            if (trans_first_path.count(target) > 0 && old_to_new.count(target) > 0)
+            {
+                boost::add_edge(source, old_to_new[target], weight, trans_graph);
+                continue;
+            }
+
+            // Add every other edge unchanged
+            boost::add_edge(source, target, weight, trans_graph);
+        }
+
+#ifdef DEBUG
+        std::cout << "transformed graph: " << std::endl;
+        for (boost::tie(ei, ei_end) = boost::edges(trans_graph); ei != ei_end; ++ei)
+        {
+            std::cout << boost::source(*ei, trans_graph)
+                      << " > " << boost::target(*ei, trans_graph)
+                      << " | " << boost::get(boost::edge_weight, trans_graph, *ei)
+                      << std::endl;
+        }
+        std::cout << std::endl;
+#endif
+
+        // Find the second path in the transformed graph
+        VertexPredecessorMap trans_second_path;
+        FindPath(trans_graph, source_, sink_, trans_second_path);
+
+        // Check if the two paths have vertices (except source and sink) in common
+        bool vertex_disjoint = true;
+        for (auto u = trans_first_path[sink_]; u != source_; u = trans_first_path[u])
+        {
+            if (trans_second_path.count(u) > 0)
+            {
+                vertex_disjoint = false;
+                break;
+            }
+        }
+
+        // Map the second path from the transformed graph into the original graph
+        VertexPredecessorMap orig_second_path;
+        for (auto u = sink_, v = trans_second_path[u];
+             u != source_;
+             u = v, v = trans_second_path[u])
+        {
+            Vertex orig_u = new_to_old.count(u) > 0 ? new_to_old[u] : u;
+            Vertex orig_v = new_to_old.count(v) > 0 ? new_to_old[v] : v;
+            orig_second_path[orig_u] = orig_v;
+        }
+
+        // If the paths are not vertex disjoint, we need to remove the edges common to both paths
+        if (!vertex_disjoint)
+        {
+            for (auto u = sink_, v = orig_first_path[u];
+                 u != source_;
+                 u = v, v = orig_first_path[u])
+            {
+                if (orig_second_path.count(v) > 0 && orig_second_path[v] == u)
+                {
+                    orig_second_path.erase(v);
+                }
+            }
+        }
+
+        // Store the paths
+        AddPath(orig_first_path);
+        AddPath(orig_second_path);
+    }
+
+    void KShortestPaths5::AddPath(VertexPredecessorMap& path)
+    {
+        for (auto u = path[sink_], v = path[u]; u != source_; u = v, v = path[u])
+        {
+            paths_[u] = v;
+        }
+
+        sink_neighbors_.push_back(path[sink_]);
+    }
+
+    void KShortestPaths5::FindPaths(size_t count)
+    {
+        //TODO
+    }
+
+    void KShortestPaths5::GetPaths(std::vector<std::vector<Vertex>>& paths)
+    {
+        for (auto v : sink_neighbors_)
+        {
+            std::vector<Vertex> path;
+            path.push_back(sink_);
+            for (auto u = v; u != source_; u = paths_[u])
+            {
+                path.insert(path.begin(), u);
+            }
+            path.insert(path.begin(), source_);
+            paths.push_back(path);
+        }
+    }
+}
+
+
+

algo/KShortestPaths5.h

@@ -0,0 +1,35 @@
+//
+// Created by wrede on 28.06.16.
+//
+
+#ifndef GBMOT_KSHORTESTPATHS5_H
+#define GBMOT_KSHORTESTPATHS5_H
+#include "../graph/Definitions.h"
+
+namespace algo
+{
+    class KShortestPaths5
+    {
+    private:
+        DirectedGraph orig_graph_;
+        Vertex source_;
+        Vertex sink_;
+
+        VertexPredecessorMap paths_;
+        std::vector<Vertex> sink_neighbors_;
+
+        void FindPath(DirectedGraph& graph, Vertex& source, Vertex& sink,
+                      VertexPredecessorMap& predecessors);
+        void FindPathPair();
+        void FindPaths(size_t count);
+
+        void AddPath(VertexPredecessorMap& path);
+    public:
+        KShortestPaths5(DirectedGraph input_graph, Vertex source, Vertex sink);
+        void Run(size_t max_path_count);
+        void GetPaths(std::vector<std::vector<Vertex>>& paths);
+    };
+}
+
+
+#endif //GBMOT_KSHORTESTPATHS5_H

core/ObjectData2D.cpp

@@ -2,7 +2,6 @@
 // Created by wrede on 02.06.16.
 //
 
-#include <opencv2/imgproc.hpp>
 #include "ObjectData2D.h"
 #include "../util/MyMath.h"
 

core/ObjectDataAngular.cpp

@@ -2,7 +2,6 @@
 // Created by wrede on 19.05.16.
 //
 
-#include <opencv2/imgproc.hpp>
 #include <math.h>
 #include "ObjectDataAngular.h"
 #include "../util/MyMath.h"

core/ObjectDataBox.cpp

@@ -2,7 +2,6 @@
 // Created by wrede on 09.06.16.
 //
 
-#include <opencv2/imgproc.hpp>
 #include "ObjectDataBox.h"
 #include <math.h>
 #include "../util/MyMath.h"

graph/Definitions.h

@@ -9,28 +9,26 @@
 #include <boost/graph/properties.hpp>
 #include <boost/graph/adjacency_list.hpp>
 
-typedef boost::property<boost::edge_weight_t, double> EdgeProp;
-typedef boost::property <boost::vertex_name_t, core::ObjectDataPtr>
-        VertexProp;
-typedef boost::adjacency_list <
-        boost::listS, boost::vecS, boost::directedS, VertexProp, EdgeProp>
+typedef double Weight;
+typedef boost::property<boost::edge_weight_t, Weight> EdgeProp;
+typedef boost::property<boost::vertex_name_t, core::ObjectDataPtr> VertexProp;
+typedef boost::adjacency_list <boost::listS, boost::vecS, boost::directedS, VertexProp, EdgeProp>
         DirectedGraph;
 typedef boost::graph_traits<DirectedGraph>::edge_descriptor Edge;
 typedef boost::graph_traits<DirectedGraph>::vertex_descriptor Vertex;
-typedef boost::property_map<DirectedGraph, boost::edge_weight_t>::type
-        EdgeWeightMap;
-typedef boost::property_map<DirectedGraph, boost::vertex_index_t>::type
-        VertexIndexMap;
-typedef boost::property_map<DirectedGraph, boost::vertex_name_t>::type
-        VertexValueMap;
-typedef boost::iterator_property_map<Vertex*, VertexIndexMap, Vertex, Vertex&>
-        PredecessorMap;
-typedef std::unordered_map<Vertex, std::set<Vertex>> MultiPredecessorMap;
-typedef std::unordered_map<Vertex, Vertex> VertexPredecessorMap;
-typedef std::unordered_map<Vertex, double> VertexDistanceMap;
-typedef boost::iterator_property_map<double*, VertexIndexMap, double, double&>
-        DistanceMap;
+typedef boost::graph_traits<DirectedGraph>::edge_iterator EdgeIter;
+typedef boost::graph_traits<DirectedGraph>::out_edge_iterator OutEdgeIter;
+typedef boost::graph_traits<DirectedGraph>::vertex_iterator VertexIter;
+typedef boost::property_map<DirectedGraph, boost::edge_weight_t>::type EdgeWeightMap;
+typedef boost::property_map<DirectedGraph, boost::vertex_index_t>::type VertexIndexMap;
+typedef boost::property_map<DirectedGraph, boost::vertex_name_t>::type VertexValueMap;
+typedef boost::iterator_property_map<Vertex*, VertexIndexMap, Vertex, Vertex&> PredecessorMap;
+typedef boost::iterator_property_map<Weight*, VertexIndexMap, Weight, Weight&> DistanceMap;
 typedef boost::iterator_property_map<std::vector<Vertex>::iterator, VertexIndexMap, Vertex, Vertex&>
         VertexVertexMap;
 
+typedef std::unordered_map<Vertex, std::set<Vertex>> MultiPredecessorMap;
+typedef std::unordered_map<Vertex, Vertex> VertexPredecessorMap;
+typedef std::unordered_map<Vertex, Weight> VertexDistanceMap;
+
 #endif //GBMOT_DEFINITIONS_H

main/main.cpp

@@ -13,6 +13,7 @@
 #include "../algo/KShortestPaths2.h"
 #include "../algo/KShortestPaths3.h"
 #include "../algo/KShortestPaths4.h"
+#include "../algo/KShortestPaths5.h"
 #include <boost/program_options.hpp>
 #include <boost/graph/named_function_params.hpp>
 #include <boost/graph/bellman_ford_shortest_paths.hpp>
@@ -772,8 +773,8 @@ void CreatePresentationGraph(DirectedGraph& graph, Vertex& source, Vertex& sink)
     source = vertices[0];
     sink = vertices[9];
 
-    boost::add_edge(vertices[0], vertices[1], 0.0, graph);
-    boost::add_edge(vertices[0], vertices[2], 0.0, graph);
+    boost::add_edge(vertices[0], vertices[1], 1.0, graph);
+    boost::add_edge(vertices[0], vertices[2], 1.0, graph);
     boost::add_edge(vertices[1], vertices[3], 12.0, graph);
     boost::add_edge(vertices[1], vertices[4], 15.0, graph);
     boost::add_edge(vertices[2], vertices[3], 15.0, graph);
@@ -786,8 +787,16 @@ void CreatePresentationGraph(DirectedGraph& graph, Vertex& source, Vertex& sink)
     boost::add_edge(vertices[5], vertices[8], 12.0, graph);
     boost::add_edge(vertices[6], vertices[7], 11.0, graph);
     boost::add_edge(vertices[6], vertices[8], 10.0, graph);
-    boost::add_edge(vertices[7], vertices[9], 0.0, graph);
-    boost::add_edge(vertices[8], vertices[9], 0.0, graph);
+    boost::add_edge(vertices[7], vertices[9], 1.0, graph);
+    boost::add_edge(vertices[8], vertices[9], 1.0, graph);
+
+    // add a negative cycle
+//    boost::add_edge(vertices[4], vertices[6], -1.0, graph);
+//    boost::add_edge(vertices[5], vertices[4], -1.0, graph);
+//    boost::add_edge(vertices[6], vertices[5], -1.0, graph);
+
+    // add a unreachable vertex
+//    boost::add_vertex(core::ObjectDataPtr(new core::ObjectData(10)), graph);
 
 //    boost::add_edge(vertices[0], vertices[1], 0.0, graph);
 //    boost::add_edge(vertices[0], vertices[2], 0.0, graph);
@@ -852,7 +861,7 @@ void TestSuurballe()
     boost::add_edge(vertices[5], vertices[6], 6.0, graph);
     boost::add_edge(vertices[6], vertices[7], 1.0, graph);
 
-    algo::KShortestPaths4 suurballe(graph, source, sink, 4);
+    algo::KShortestPaths4 suurballe(graph, source, sink, 3);
     suurballe.Run();
 
     size_t i = 0;
@@ -870,6 +879,28 @@ void TestSuurballe()
     std::cout << "Total paths length: " << suurballe.GetTotalPathsLength() << std::endl;
 }
 
+void TestYAOKSP()
+{
+    Vertex source, sink;
+    DirectedGraph graph;
+    CreatePresentationGraph(graph, source, sink);
+    algo::KShortestPaths5 ksp(graph, source, sink);
+
+    ksp.Run(2);
+
+    std::vector<std::vector<Vertex>> paths;
+    ksp.GetPaths(paths);
+    for (auto path : paths)
+    {
+        std::cout << "path: ";
+        for (auto v : path)
+        {
+            std::cout << v << " ";
+        }
+        std::cout << std::endl;
+    }
+}
+
 int main(int argc, char** argv)
 {
     //TODO load with frame offset
@@ -882,8 +913,9 @@ int main(int argc, char** argv)
 //    Vertex source, sink;
 //    CreatePresentationGraph(graph, source, sink);
 //
-//    algo::KShortestPaths4 suurballe(graph, source, sink, 4);
+//    algo::KShortestPaths4 suurballe(graph, source, sink, 2);
 //    suurballe.Run();
+//
 //    size_t i = 0;
 //    for (std::vector<Vertex> path : suurballe.GetPaths())
 //    {
@@ -895,6 +927,7 @@ int main(int argc, char** argv)
 //        }
 //        std::cout << "}\n";
 //    }
+//
 //    std::cout << "Total paths length: " << suurballe.GetTotalPathsLength() << std::endl;
 //
 //    util::FileIO::WriteCSVMatlab(graph, "/home/wrede/Dokumente/graph.csv");
@@ -904,7 +937,9 @@ int main(int argc, char** argv)
 
 //    TestGrid();
 
-    TestSuurballe();
+//    TestSuurballe();
+
+    TestYAOKSP();
 
     return 0;
 }