finished the ksp implementation

algo/Berclaz.cpp

@@ -10,6 +10,7 @@
 #include "KShortestPaths2.h"
 #include "KShortestPaths3.h"
 #include "KShortestPaths4.h"
+#include "KShortestPaths5.h"
 
 namespace algo
 {
@@ -36,15 +37,12 @@ namespace algo
             }
         }
 
-        util::Logger::LogDebug("vertex count " + std::to_string(boost::num_vertices(graph)));
-        util::Logger::LogDebug("edge count " + std::to_string(boost::num_edges(graph)));
-
         // Add source and sink vertex
         source = boost::add_vertex(core::ObjectDataPtr(new core::ObjectData()), graph);
         sink = boost::add_vertex(core::ObjectDataPtr(new core::ObjectData()), graph);
 
-        util::Logger::LogDebug("source index: " + std::to_string(source));
-        util::Logger::LogDebug("sink index: " + std::to_string(sink));
+//        util::Logger::LogDebug("source index: " + std::to_string(source));
+//        util::Logger::LogDebug("sink index: " + std::to_string(sink));
         util::Logger::LogDebug("add edges");
 
         // Iterate all vertices but source and sink
@@ -109,19 +107,19 @@ namespace algo
         }
 
         util::Logger::LogDebug("vertex count " + std::to_string(boost::num_vertices(graph)));
-        util::Logger::LogDebug("calculated vertex count " + std::to_string(
-           grid.GetWidthCount() * grid.GetHeightCount() * grid.GetDepthCount() + 2
-        ));
+//        util::Logger::LogDebug("calculated vertex count " + std::to_string(
+//           grid.GetWidthCount() * grid.GetHeightCount() * grid.GetDepthCount() + 2
+//        ));
         util::Logger::LogDebug("edge count " + std::to_string(boost::num_edges(graph)));
-        util::Logger::LogDebug("width count " + std::to_string(grid.GetWidthCount()));
-        util::Logger::LogDebug("height count " + std::to_string(grid.GetHeightCount()));
-        util::Logger::LogDebug("depth count " + std::to_string(grid.GetDepthCount()));
-        util::Logger::LogDebug("calculated edge count " + std::to_string(
-           grid.GetWidthCount() * grid.GetHeightCount() * (grid.GetDepthCount() - 1) * 11 +
-           grid.GetWidthCount() * grid.GetHeightCount() * 2 -
-           (grid.GetWidthCount() + grid.GetHeightCount()) * 2 * 3 * (grid.GetDepthCount() - 1) +
-           4 * (grid.GetDepthCount() - 1)
-        ));
+//        util::Logger::LogDebug("width count " + std::to_string(grid.GetWidthCount()));
+//        util::Logger::LogDebug("height count " + std::to_string(grid.GetHeightCount()));
+//        util::Logger::LogDebug("depth count " + std::to_string(grid.GetDepthCount()));
+//        util::Logger::LogDebug("calculated edge count " + std::to_string(
+//           grid.GetWidthCount() * grid.GetHeightCount() * (grid.GetDepthCount() - 1) * 11 +
+//           grid.GetWidthCount() * grid.GetHeightCount() * 2 -
+//           (grid.GetWidthCount() + grid.GetHeightCount()) * 2 * 3 * (grid.GetDepthCount() - 1) +
+//           4 * (grid.GetDepthCount() - 1)
+//        ));
     }
 
     void Berclaz::ExtractTracks(DirectedGraph& graph, MultiPredecessorMap& map, Vertex origin,
@@ -162,26 +160,25 @@ namespace algo
             Vertex source, sink;
             CreateGraph(graph, source, sink, grid);
 
-            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("run ksp");
+            KShortestPaths5 ksp(graph, source, sink);
+            ksp.Run(max_track_count);
+
+            util::Logger::LogDebug("get paths");
+            std::vector<std::vector<Vertex>> paths;
+            ksp.GetPaths(paths);
 
             util::Logger::LogDebug("extract 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";
-//            }
+            VertexValueMap values = boost::get(boost::vertex_name, graph);
+            for (auto path : paths)
+            {
+                core::TrackletPtr tlt(new core::Tracklet());
+                for (auto v : path)
+                {
+                    tlt->AddPathObject(values[v]);
+                }
+                tracks.push_back(tlt);
+            }
         }
 
         // Only connect tracks if the sequence was split

algo/KShortestPaths5.cpp

@@ -6,8 +6,9 @@
 #include <boost/graph/dijkstra_shortest_paths.hpp>
 #include <boost/graph/copy.hpp>
 #include <boost/graph/bellman_ford_shortest_paths.hpp>
+#include <iomanip>
 #include "KShortestPaths5.h"
-#define DEBUG
+#include "../util/Logger.h"
 
 namespace algo
 {
@@ -36,16 +37,21 @@ namespace algo
                 break;
             default:
                 FindPaths(max_path_count);
+                break;
         }
+
+        util::Logger::LogDebug(std::to_string(sink_neighbors_.size()) + " paths have been found");
     }
 
-    void KShortestPaths5::FindPath(DirectedGraph& graph, Vertex& source, Vertex& sink,
-                                   VertexPredecessorMap& predecessors)
+    bool KShortestPaths5::FindPath(DirectedGraph& graph, Vertex& source, Vertex& sink,
+                                   VertexPredecessorMap& predecessors, VertexDistanceMap& distances)
     {
         // The predecessors and the distances
         std::vector<Vertex> p(boost::num_vertices(graph));
         std::vector<Weight> d(boost::num_vertices(graph));
 
+        util::Logger::LogDebug("scan the graph for negative edge weights");
+
         // Scan the graph for negative edge weights to use the proper algorithm
         bool negative_edges = false;
         EdgeIter ei, ei_end;
@@ -58,11 +64,12 @@ namespace algo
             }
         }
 
-#ifdef DEBUG
-        std::cout << "the specified graph has "
-                  << (negative_edges ? "negative" : "only positive")
-                  << " edge weights" << std::endl;
-#endif
+        if (negative_edges)
+            util::Logger::LogDebug("the graph contains negative edges");
+        else
+            util::Logger::LogDebug("the graph contains only positive edges");
+
+        util::Logger::LogDebug("run a single-source shortest paths algorithm");
 
         if (negative_edges)
         {
@@ -94,50 +101,41 @@ namespace algo
                                             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
+        util::Logger::LogDebug("prepare a map of visited vertices to detect negative cycles");
 
-#ifdef DEBUG
-        std::cout << "path: ";
-        std::cout << sink;
-#endif
-        // Record the vertices already visited to detect negative cycles
+        // Record the vertices already visited to detect negative cycles (and store the distances)
         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;
+
+            distances[*vi] = d[*vi];
         }
 
+        util::Logger::LogDebug("insert the path into the output map (and detect negative cycles)");
+
         // 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;
+                util::Logger::LogError("negative cycle at vertex " + std::to_string(u));
+                return false;
             }
 
             predecessors[u] = v;
             visited_vertices[u] = true;
-#ifdef DEBUG
-            std::cout << "<" << v;
-#endif
         }
 
-#ifdef DEBUG
-        std::cout << std::endl;
-#endif
+        return true;
+    }
+
+    bool KShortestPaths5::FindPath(DirectedGraph& graph, Vertex& source, Vertex& sink,
+                                   VertexPredecessorMap& predecessors)
+    {
+        VertexDistanceMap d;
+        return FindPath(graph, source, sink, predecessors, d);
     }
 
     void KShortestPaths5::FindPathPair()
@@ -145,6 +143,8 @@ namespace algo
         VertexIter vi, vi_end;
         EdgeIter ei, ei_end;
 
+        util::Logger::LogDebug("find the first path (in the original graph)");
+
         // Find the first path
         VertexPredecessorMap orig_first_path;
         FindPath(orig_graph_, source_, sink_, orig_first_path);
@@ -154,6 +154,8 @@ namespace algo
         // every edge pointing on the path should end in the newly created vertex in the splitting
         // process
 
+        util::Logger::LogDebug("copy the original graph");
+
         // 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;
@@ -165,6 +167,8 @@ namespace algo
         // Transform the first path by inverting edges (and weights) and splitting nodes along the
         // path
 
+        util::Logger::LogDebug("reverse the first 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])
@@ -172,6 +176,8 @@ namespace algo
             trans_first_path[v] = u;
         }
 
+        util::Logger::LogDebug("invert the edges along the first path");
+
         // Invert edges
         for (auto u = sink_, v = orig_first_path[u]; u != source_; u = v, v = orig_first_path[u])
         {
@@ -186,21 +192,12 @@ namespace algo
             }
             else
             {
-                std::cerr << "error: edge not found " << v << " -> " << u << std::endl;
+                util::Logger::LogError("edge not found " + std::to_string(v) + " -> " +
+                                       std::to_string(u));
             }
         }
 
-#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
+        util::Logger::LogDebug("split the nodes along the first path");
 
         // Split nodes
         VertexPredecessorMap old_to_new;
@@ -221,17 +218,20 @@ namespace algo
             boost::tie(edge, e_found) = boost::edge(v, u, orig_graph_);
             if (e_found)
             {
-                w = boost::get(boost::edge_weight, trans_graph, edge);
+                w = boost::get(boost::edge_weight, orig_graph_, edge);
             }
             else
             {
-                std::cerr << "error: edge not found " << v << " -> " << u << std::endl;
+                util::Logger::LogError("edge not found " + std::to_string(v) + " -> " +
+                                       std::to_string(u));
             }
 
             // Create the edge from the concomitant vertex to the path predecessor
             boost::add_edge(new_u, v, -w, trans_graph);
         }
 
+        util::Logger::LogDebug("extend the copied graph with the remaining edges");
+
         // Add all remaining edges
         for (boost::tie(ei, ei_end) = boost::edges(orig_graph_); ei != ei_end; ++ei)
         {
@@ -262,32 +262,13 @@ namespace algo
             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
+        util::Logger::LogDebug("find the second path (in the copied and transformed graph)");
 
         // 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;
-            }
-        }
+        util::Logger::LogDebug("map the second path into the original graph");
 
         // Map the second path from the transformed graph into the original graph
         VertexPredecessorMap orig_second_path;
@@ -300,20 +281,38 @@ namespace algo
             orig_second_path[orig_u] = orig_v;
         }
 
+        util::Logger::LogDebug("check if the two paths are already vertex disjoint");
+
+        // Check if the two paths have vertices (except source and sink) in common
+        bool vertex_disjoint = true;
+        for (auto u = orig_first_path[sink_]; u != source_; u = orig_first_path[u])
+        {
+            if (orig_second_path.count(u) > 0)
+            {
+                vertex_disjoint = false;
+                break;
+            }
+        }
+
         // If the paths are not vertex disjoint, we need to remove the edges common to both paths
         if (!vertex_disjoint)
         {
+            util::Logger::LogDebug("remove edges used by both paths to guarantee vertex disjointness");
+
             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_first_path.erase(u);
                     orig_second_path.erase(v);
                 }
             }
         }
 
+        util::Logger::LogDebug("add the first and second path to the map of all paths");
+
         // Store the paths
         AddPath(orig_first_path);
         AddPath(orig_second_path);
@@ -321,17 +320,230 @@ namespace algo
 
     void KShortestPaths5::AddPath(VertexPredecessorMap& path)
     {
-        for (auto u = path[sink_], v = path[u]; u != source_; u = v, v = path[u])
+        for (auto edge : path)
         {
-            paths_[u] = v;
+            if (edge.first == edge.second)
+                continue;
+
+            if (edge.first == sink_)
+                sink_neighbors_.push_back(edge.second);
+            else
+                paths_[edge.first] = edge.second;
         }
+    }
 
-        sink_neighbors_.push_back(path[sink_]);
+    void KShortestPaths5::AddPath(VertexPredecessorMap& in, MultiPredecessorMap& out)
+    {
+        for (auto edge : in)
+        {
+            if (edge.first != edge.second)
+                out[edge.first].insert(edge.second);
+        }
+    }
+
+    void KShortestPaths5::AddPaths(MultiPredecessorMap& paths)
+    {
+        for (auto pair : paths)
+        {
+            Vertex t = pair.first;
+            for (auto s : pair.second)
+            {
+                if (t == sink_)
+                    sink_neighbors_.push_back(s);
+                else
+                    paths_[t] = s;
+            }
+        }
     }
 
     void KShortestPaths5::FindPaths(size_t count)
     {
-        //TODO
+        VertexIter vi, vi_end;
+        EdgeIter ei, ei_end;
+
+        // The edge weights of the original graph
+        EdgeWeightMap orig_weights = boost::get(boost::edge_weight, orig_graph_);
+
+        // All found paths
+        MultiPredecessorMap k_orig_paths;
+
+        // Find the first path (only path found in the original graph)
+        VertexDistanceMap orig_distances;
+        VertexPredecessorMap orig_first_path;
+        if (!FindPath(orig_graph_, source_, sink_, orig_first_path, orig_distances))
+        {
+            util::Logger::LogInfo("Not even a single path could have been found!");
+            return;
+        }
+        AddPath(orig_first_path, k_orig_paths);
+
+        // Transform the original edge weights
+        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_);
+
+            orig_weights[*ei] = orig_distances[source] + orig_weights[*ei] - orig_distances[target];
+        }
+
+        // Find the specified amount of paths iteratively
+        for (size_t i = 1; i < count; ++i)
+        {
+            util::Logger::LogDebug("copy the original graph");
+
+            // Create a graph used for transformations and start by copying the vertices
+            DirectedGraph trans_graph;
+            for (boost::tie(vi, vi_end) = boost::vertices(orig_graph_); vi != vi_end; ++vi)
+            {
+                boost::add_vertex(trans_graph);
+            }
+
+            util::Logger::LogDebug("invert the edges along all previous found paths");
+
+            // Invert edges for all previous paths (also invert their weight)
+            std::vector<EdgeIter> edge_queue;
+            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_);
+
+                if (k_orig_paths.count(target) > 0 && k_orig_paths[target].count(source) > 0)
+                {
+                    Weight weight = orig_weights[*ei];
+
+                    boost::add_edge(target, source, -weight, trans_graph);
+                }
+                else
+                {
+                    edge_queue.push_back(ei);
+                }
+            }
+
+            util::Logger::LogDebug("split the nodes along all previous found path");
+
+            // Split nodes
+            VertexPredecessorMap old_to_new;
+            VertexPredecessorMap new_to_old;
+            for (auto orig_edge : k_orig_paths)
+            {
+                Vertex source = *orig_edge.second.begin();
+                Vertex target = orig_edge.first;
+
+                // Create the concomitant vertex
+                Vertex new_target = boost::add_vertex(trans_graph);
+                old_to_new[target] = new_target;
+                new_to_old[new_target] = target;
+
+                // Retrieve the weight from the original path in the original graph
+                Weight weight = 0.0;
+                Edge edge;
+                bool e_found;
+                boost::tie(edge, e_found) = boost::edge(source, target, orig_graph_);
+                if (e_found)
+                {
+                    weight = orig_weights[edge];
+                }
+                else
+                {
+                    util::Logger::LogError("error: edge not found " + std::to_string(source) +
+                                           " -> " + std::to_string(target));
+                }
+
+                // Create the edge from the concomitant vertex to the path predecessor
+                boost::add_edge(new_target, source, -weight, trans_graph);
+
+                //TODO experimental testing
+                if (weight < 0)
+                    std::cout << "NEGATIVE AUXILIARY EDGE" << std::endl;
+            }
+
+            util::Logger::LogDebug("extend the copied graph with the remaining edges");
+
+            // Add all remaining edges
+            for (auto e : edge_queue)
+            {
+                Vertex source = boost::source(*e, orig_graph_);
+                Vertex target = boost::target(*e, orig_graph_);
+                Weight weight = orig_weights[*e];
+
+                // 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 (k_orig_paths.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);
+            }
+
+            util::Logger::LogDebug("find the second path (in the copied and transformed graph)");
+
+            // Find the next path in the transformed graph
+            VertexPredecessorMap trans_next_path;
+            if (!FindPath(trans_graph, source_, sink_, trans_next_path))
+            {
+                util::Logger::LogInfo("No more paths may be found!");
+                return;
+            }
+
+            util::Logger::LogDebug("map the second path into the original graph");
+
+            // Map the second path from the transformed graph into the original graph
+            VertexPredecessorMap orig_next_path;
+            for (auto u = sink_, v = trans_next_path[u]; u != source_; u = v, v = trans_next_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_next_path[orig_u] = orig_v;
+            }
+
+            AddPath(orig_next_path, k_orig_paths);
+
+            util::Logger::LogDebug("remove edges used by multiple paths to guarantee vertex disjointness");
+
+            // Remove edges used by multiple paths
+            for (auto pair : k_orig_paths)
+            {
+                Vertex target = pair.first;
+                for (auto source : pair.second)
+                {
+                    if (k_orig_paths.count(source) > 0 && k_orig_paths[source].count(target) > 0)
+                    {
+                        k_orig_paths[source].erase(target);
+                        k_orig_paths[target].erase(source);
+                    }
+                }
+            }
+
+            util::Logger::LogDebug("remove vertices with no edges from the path");
+
+            // Remove empty vertices from the paths
+            for (auto iter = k_orig_paths.begin(); iter != k_orig_paths.end(); )
+            {
+                auto pair = *iter;
+                if (pair.second.empty())
+                {
+                    iter = k_orig_paths.erase(iter);
+                }
+                else
+                {
+                    ++iter;
+                }
+            }
+        }
+
+        // Store the paths
+        AddPaths(k_orig_paths);
     }
 
     void KShortestPaths5::GetPaths(std::vector<std::vector<Vertex>>& paths)

algo/KShortestPaths5.h

@@ -18,12 +18,16 @@ namespace algo
         VertexPredecessorMap paths_;
         std::vector<Vertex> sink_neighbors_;
 
-        void FindPath(DirectedGraph& graph, Vertex& source, Vertex& sink,
+        bool FindPath(DirectedGraph& graph, Vertex& source, Vertex& sink,
+                      VertexPredecessorMap& predecessors, VertexDistanceMap& distances);
+        bool FindPath(DirectedGraph& graph, Vertex& source, Vertex& sink,
                       VertexPredecessorMap& predecessors);
         void FindPathPair();
         void FindPaths(size_t count);
 
         void AddPath(VertexPredecessorMap& path);
+        void AddPath(VertexPredecessorMap& in, MultiPredecessorMap& out);
+        void AddPaths(MultiPredecessorMap& paths);
     public:
         KShortestPaths5(DirectedGraph input_graph, Vertex source, Vertex sink);
         void Run(size_t max_path_count);

algo/NStage.cpp

@@ -8,18 +8,17 @@
 
 namespace algo
 {
-    NStage::NStage(size_t max_frame_skip,
-                       std::vector<double> penalty_value,
-                       std::vector<size_t> max_tracklet_count)
+    NStage::NStage(std::vector<size_t> max_frame_skip,
+                   std::vector<double> penalty_value,
+                   std::vector<size_t> max_tracklet_count)
     {
-        max_frame_skip_ = max_frame_skip;
+        max_frame_skips_ = max_frame_skip;
         penalty_values_ = penalty_value;
         max_tracklet_counts_ = max_tracklet_count;
         iterations_ = std::min(max_tracklet_count.size(), penalty_value.size());
     }
 
-    void NStage::CreateObjectGraph(DirectedGraph& graph,
-                                     const core::DetectionSequence& detections)
+    void NStage::CreateObjectGraph(DirectedGraph& graph, const core::DetectionSequence& detections)
     {
         util::Logger::LogInfo("Creating object graph");
 
@@ -37,8 +36,7 @@ namespace algo
 
             for (size_t j = 0; j < detections.GetObjectCount(i); ++j)
             {
-                Vertex v = boost::add_vertex(detections.GetObject(i, j),
-                                             graph);
+                Vertex v = boost::add_vertex(detections.GetObject(i, j), graph);
 
                 layer.push_back(v);
             }
@@ -61,9 +59,7 @@ namespace algo
                 Vertex u = layers[i][j];
 
                 // For each next frame/layer until maxFrameSkip or end
-                for (size_t k = 1;
-                     k != (max_frame_skip_ + 1) && i + k < layers.size();
-                     ++k)
+                for (size_t k = 1; k <= max_frame_skips_[0] && i + k < layers.size(); ++k)
                 {
                     // To every edge in the next frame/layer
                     for (size_t l = 0; l < layers[i + k].size(); ++l)
@@ -91,10 +87,8 @@ namespace algo
         util::Logger::LogDebug("edge count " + std::to_string(boost::num_edges(graph)));
     }
 
-    void NStage::CreateTrackletGraph(DirectedGraph& obj_graph,
-                                       DirectedGraph& tlt_graph,
-                                       size_t frame_count,
-                                       size_t iteration)
+    void NStage::CreateTrackletGraph(DirectedGraph& obj_graph, DirectedGraph& tlt_graph,
+                                     size_t frame_count, size_t iteration)
     {
         util::Logger::LogInfo("Creating tracklet graph");
 
@@ -115,6 +109,9 @@ namespace algo
         Vertex obj_src = obj_indices[0];
         Vertex obj_snk = obj_indices[obj_graph_size - 1];
 
+        //TODO experimental
+        EdgeWeightMap weight_map = boost::get(boost::edge_weight, obj_graph);
+
         // Iteratively run dijkstra to extract tracklets
         for (size_t i = 0; i != max_tracklet_counts_[iteration]; ++i)
         {
@@ -132,26 +129,34 @@ namespace algo
 
             // Create the tracklet
             core::TrackletPtr tracklet(new core::Tracklet);
-            for (Vertex u = obj_pred_map[obj_snk], v = obj_snk;
-                 u != v;
-                 v = u, u = obj_pred_map[v])
+            for (Vertex u = obj_pred_map[obj_snk], v = obj_snk; u != v; v = u, u = obj_pred_map[v])
             {
                 tracklet->AddPathObject(obj_values[u]);
 
                 // Leave source and sink untouched
                 if (!obj_values[u]->IsVirtual())
                 {
+                    //TODO original
                     // Remove the path by setting all used edges to a weight of
                     // infinity
-                    std::pair<DirectedGraph::out_edge_iterator,
-                              DirectedGraph::out_edge_iterator> edge_iter = boost::out_edges(u, obj_graph);
-
-                    for (DirectedGraph::out_edge_iterator iter = edge_iter.first;
-                         iter != edge_iter.second;
-                         ++iter)
+//                    std::pair<DirectedGraph::out_edge_iterator,
+//                              DirectedGraph::out_edge_iterator> edge_iter = boost::out_edges(u, obj_graph);
+//
+//                    for (DirectedGraph::out_edge_iterator iter = edge_iter.first;
+//                         iter != edge_iter.second;
+//                         ++iter)
+//                    {
+//                        boost::get(boost::edge_weight, obj_graph, *iter)
+//                                = std::numeric_limits<double>::infinity();
+//                    }
+
+                    //TODO experimental
+                    OutEdgeIter oei, oei_end;
+                    for (boost::tie(oei, oei_end) = boost::out_edges(u, obj_graph);
+                         oei != oei_end;
+                         ++oei)
                     {
-                        boost::get(boost::edge_weight, obj_graph, *iter)
-                                = std::numeric_limits<double>::infinity();
+                        weight_map[*oei] = std::numeric_limits<double>::infinity();
                     }
                 }
             }
@@ -162,8 +167,7 @@ namespace algo
         }
 
         // Add sink to tracklet graph
-        Vertex tlt_snk =
-                boost::add_vertex(core::ObjectDataPtr(new core::ObjectData()),
+        Vertex tlt_snk = boost::add_vertex(core::ObjectDataPtr(new core::ObjectData()),
                                   tlt_graph);
 
         util::Logger::LogDebug("adding edges");
@@ -177,8 +181,7 @@ namespace algo
         for (size_t i = 1; i < tlt_graph_size - 1; ++i)
         {
             Vertex u = tlt_indices[i];
-            core::TrackletPtr u_ptr =
-                    std::static_pointer_cast<core::Tracklet>(tlt_values[u]);
+            core::TrackletPtr u_ptr = std::static_pointer_cast<core::Tracklet>(tlt_values[u]);
             size_t u_first_frame = u_ptr->GetFirstFrameIndex();
             size_t u_last_frame = u_ptr->GetLastFrameIndex();
 
@@ -193,7 +196,8 @@ namespace algo
                     size_t v_first_frame = v_ptr->GetFirstFrameIndex();
 
                     // Link only tracklets that are in temporal order
-                    if (u_last_frame < v_first_frame)
+                    if (u_last_frame < v_first_frame &&
+                            (v_first_frame - u_last_frame < max_frame_skips_[iteration]))
                     {
                         boost::add_edge(u, v,
                                         tlt_values[u]->CompareTo(tlt_values[v]),
@@ -218,7 +222,7 @@ namespace algo
     }
 
     void NStage::ExtractTracks(DirectedGraph& tlt_graph, size_t depth,
-                                 std::vector<core::TrackletPtr>& tracks)
+                               std::vector<core::TrackletPtr>& tracks)
     {
         util::Logger::LogInfo("Extracting tracks");
 
@@ -243,7 +247,7 @@ namespace algo
     }
 
     void NStage::Run(const core::DetectionSequence& sequence,
-                       std::vector<core::TrackletPtr>& tracks)
+                     std::vector<core::TrackletPtr>& tracks)
     {
         // Running the two stage graph algorithm
         DirectedGraph obj_graph;

algo/NStage.h

@@ -21,7 +21,7 @@ namespace algo
         /**
          * Maximum edge length to link object
          */
-        size_t max_frame_skip_;
+        std::vector<size_t> max_frame_skips_;
 
         /**
          * Edge value to link to source and sink
@@ -43,8 +43,7 @@ namespace algo
          * @param graph The graph to write into
          * @param detections The objects to use for the graph
          */
-        void CreateObjectGraph(DirectedGraph& graph,
-                               const core::DetectionSequence& detections);
+        void CreateObjectGraph(DirectedGraph& graph, const core::DetectionSequence& detections);
 
         /**
          * Reduces the object graph into linked tracklets.
@@ -53,10 +52,8 @@ namespace algo
          * @param frame_count The frame count of the object graph
          * @param iteration The current iteration
          */
-        void CreateTrackletGraph(DirectedGraph& obj_graph,
-                                 DirectedGraph& tlt_graph,
-                                 size_t frame_count,
-                                 size_t iteration);
+        void CreateTrackletGraph(DirectedGraph& obj_graph, DirectedGraph& tlt_graph,
+                                 size_t frame_count, size_t iteration);
 
         /**
          * Extracts the finished tracks from the given tracklet graph.
@@ -64,8 +61,7 @@ namespace algo
          * @param depth The depth to flatten the tracklets to
          * @param tracks The vector to write the extracted tracks in
          */
-        void ExtractTracks(DirectedGraph& tlt_graph,
-                           size_t depth,
+        void ExtractTracks(DirectedGraph& tlt_graph, size_t depth,
                            std::vector<core::TrackletPtr>& tracks);
     public:
         /**
@@ -76,8 +72,9 @@ namespace algo
          * @param penalty_value The edge value to link to source and sink
          * @param max_tracklet_count The maximum number of tracklets to create
          */
-        NStage(size_t max_frame_skip, std::vector<double> penalty_value,
-                 std::vector<size_t> max_tracklet_count);
+        NStage(std::vector<size_t> max_frame_skip,
+               std::vector<double> penalty_value,
+               std::vector<size_t> max_tracklet_count);
 
         void Run(const core::DetectionSequence& sequence,
                  std::vector<core::TrackletPtr>& tracks);

main/main.cpp

@@ -17,10 +17,11 @@
 #include <boost/program_options.hpp>
 #include <boost/graph/named_function_params.hpp>
 #include <boost/graph/bellman_ford_shortest_paths.hpp>
+#include <iomanip>
 
 struct
 {
-    size_t max_frame_skip;
+    std::string max_frame_skip;
     std::string max_tracklet_count;
     std::string penalty_value;
 } n_stage_params;
@@ -30,13 +31,14 @@ void RunNStage(core::DetectionSequence& sequence,
 {
     util::Logger::LogInfo("Running n-stage");
 
+    std::vector<size_t> max_frame_skips;
     std::vector<double> penalty_values;
     std::vector<size_t> max_tracklet_counts;
 
     // Parse strings to vectors
     size_t d_index;
     std::string str, part;
-    str = n_stage_params.max_tracklet_count;
+    str = n_stage_params.max_frame_skip;
     do
     {
         d_index = str.find(",");
@@ -45,7 +47,7 @@ void RunNStage(core::DetectionSequence& sequence,
 
         if (part.size() > 0)
         {
-            max_tracklet_counts.push_back((unsigned long&&) atoi(part.c_str()));
+            max_frame_skips.push_back((unsigned long&&) atoi(part.c_str()));
         }
 
         str = str.substr(d_index + 1);
@@ -66,10 +68,24 @@ void RunNStage(core::DetectionSequence& sequence,
         str = str.substr(d_index + 1);
     }
     while (d_index != std::string::npos);
+    str = n_stage_params.max_tracklet_count;
+    do
+    {
+        d_index = str.find(",");
+
+        part = str.substr(0, d_index);
+
+        if (part.size() > 0)
+        {
+            max_tracklet_counts.push_back((unsigned long&&) atoi(part.c_str()));
+        }
+
+        str = str.substr(d_index + 1);
+    }
+    while (d_index != std::string::npos);
 
     // Init n stage
-    algo::NStage n_stage(n_stage_params.max_frame_skip,
-                         penalty_values, max_tracklet_counts);
+    algo::NStage n_stage(max_frame_skips, penalty_values, max_tracklet_counts);
 
     n_stage.Run(sequence, tracks);
 
@@ -118,8 +134,8 @@ void RunBerclaz(core::DetectionSequence& sequence,
 void Run(int argc, char** argv)
 {
     // Algorithm independent values
-    std::string input_file, output_file, images_folder, algorithm, config_path, header, input_format;
-    bool info, debug, display;
+    std::string input_file, output_path, images_folder, algorithm, config_path, header, input_format;
+    bool info, debug, display, output;
     char input_delimiter, output_delimiter;
     double temporal_weight, spatial_weight, angular_weight, image_width, image_height;
 
@@ -139,14 +155,18 @@ void Run(int argc, char** argv)
              boost::program_options::value<bool>(&display)
                      ->default_value(false),
              "if a window with the images and the detected tracks should be opened")
+            ("output",
+             boost::program_options::value<bool>(&output)
+                     ->default_value(false),
+             "if the results should be written into the specified output folder")
             ("config",
              boost::program_options::value<std::string>(&config_path),
              "the path to the config file, if no path is given the command line arguments are read")
             ("input-file",
              boost::program_options::value<std::string>(&input_file),
              "set detections file path")
-            ("output-file",
-             boost::program_options::value<std::string>(&output_file),
+            ("output-path",
+             boost::program_options::value<std::string>(&output_path),
              "set the output file path")
             ("output-delimiter",
              boost::program_options::value<char>(&output_delimiter)
@@ -180,8 +200,8 @@ void Run(int argc, char** argv)
              boost::program_options::value<std::string>(&algorithm),
              "set the algorithm to use, current viable options: n-stage berclaz")
             ("max-frame-skip",
-             boost::program_options::value<size_t>(&n_stage_params.max_frame_skip)
-                     ->default_value(1),
+             boost::program_options::value<std::string>(&n_stage_params.max_frame_skip)
+                     ->default_value("1,1"),
              "(n stage) set the maximum number of frames a track can skip between two detections,"
                      " if set to less or equal than zero all frames are linked")
             ("max-tracklet-count",
@@ -263,7 +283,7 @@ void Run(int argc, char** argv)
     }
     else if (opt_var_map.count("input-file") == 0 ||
              opt_var_map.count("input-format") == 0 ||
-             opt_var_map.count("output-file") == 0)
+                (opt_var_map.count("output-path") == 0 && output))
     {
         std::cout << opts << std::endl;
         exit(0);
@@ -368,7 +388,10 @@ void Run(int argc, char** argv)
                           + " minutes");
 
     // Write the output file
-    util::FileIO::WriteTracks(tracks, output_file, output_delimiter);
+    if (output)
+    {
+        util::FileIO::WriteTracks(tracks, output_path + "/tracks.csv", output_delimiter);
+    }
 
     // Display the tracking data
     if (display)
@@ -376,10 +399,10 @@ void Run(int argc, char** argv)
         util::Visualizer vis;
 
         if (algorithm == "berclaz")
-            vis.Display(tracks, images_folder, "Visualizer",
+            vis.Display(tracks, images_folder, output, output_path, "Visualizer",
                         0, 24, berclaz_params.h_res, berclaz_params.v_res);
         else
-            vis.Display(tracks, images_folder);
+            vis.Display(tracks, images_folder, output, output_path);
     }
 }
 
@@ -726,34 +749,33 @@ void CreateBerclazGraph(DirectedGraph& graph, Vertex& source, Vertex& sink)
                 {
                     // Iterate all nearby cells in the next frame
                     for (int ny = std::max(0, y - vicinity_size);
-                         ny <
-                         std::min(grid.GetHeightCount(), y + vicinity_size + 1);
+                         ny < std::min(grid.GetHeightCount(), y + vicinity_size + 1);
                          ++ny)
                     {
                         for (int nx = std::max(0, x - vicinity_size);
-                             nx < std::min(grid.GetWidthCount(),
-                                           x + vicinity_size + 1);
+                             nx < std::min(grid.GetWidthCount(), x + vicinity_size + 1);
                              ++nx)
                         {
                             // Second vertex index
-                            int vj = nx + ny * grid.GetHeightCount() +
-                                     (z + 1) * layer_size;
+                            int vj = nx + ny * grid.GetHeightCount() + (z + 1) * layer_size;
 
                             // Connect to nearby cells
-                            boost::add_edge(vertices[vi], vertices[vj],
-                                            weight, graph);
+                            boost::add_edge(vertices[vi], vertices[vj], weight, graph);
                         }
                     }
 
-                    boost::add_edge(vertices[vi], sink, 0.0, graph);
+//                    boost::add_edge(vertices[vi], sink, 0.0, graph);
                 }
                 else
                 {
                     boost::add_edge(vertices[vi], sink, weight, graph);
                 }
 
-                // Connect with source and sink
-                boost::add_edge(source, vertices[vi], 0.0, graph);
+                if (z < 1)
+                {
+                    // Connect with source
+                    boost::add_edge(source, vertices[vi], 0.0, graph);
+                }
             }
         }
     }
@@ -762,58 +784,75 @@ void CreateBerclazGraph(DirectedGraph& graph, Vertex& source, Vertex& sink)
     util::Logger::LogDebug("edge count " + std::to_string(boost::num_edges(graph)));
 }
 
-void CreatePresentationGraph(DirectedGraph& graph, Vertex& source, Vertex& sink)
+void CreatePresentationGraph(DirectedGraph& graph, Vertex& source, Vertex& sink, bool two_paths)
 {
     std::vector<Vertex> vertices;
-    for (size_t i = 0; i < 10; ++i)
-    {
-        vertices.push_back(boost::add_vertex(core::ObjectDataPtr(new core::ObjectData(i)), graph));
-    }
 
-    source = vertices[0];
-    sink = vertices[9];
+    if (two_paths)
+    {
+        for (size_t i = 0; i < 10; ++i)
+        {
+            vertices.push_back(
+                    boost::add_vertex(core::ObjectDataPtr(new core::ObjectData(i)), 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);
-    boost::add_edge(vertices[2], vertices[4], 10.0, graph);
-    boost::add_edge(vertices[3], vertices[5], 15.0, graph);
-    boost::add_edge(vertices[3], vertices[6], 12.0, graph);
-    boost::add_edge(vertices[4], vertices[5], 12.0, graph);
-    boost::add_edge(vertices[4], vertices[6], 11.0, graph);
-    boost::add_edge(vertices[5], vertices[7], 12.0, graph);
-    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], 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);
+        source = vertices[0];
+        sink = vertices[9];
+
+        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);
+        boost::add_edge(vertices[2], vertices[4], 10.0, graph);
+        boost::add_edge(vertices[3], vertices[5], 15.0, graph);
+        boost::add_edge(vertices[3], vertices[6], 12.0, graph);
+        boost::add_edge(vertices[4], vertices[5], 12.0, graph);
+        boost::add_edge(vertices[4], vertices[6], 11.0, graph);
+        boost::add_edge(vertices[5], vertices[7], 12.0, graph);
+        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], 1.0, graph);
+        boost::add_edge(vertices[8], vertices[9], 1.0, graph);
+    }
+    else
+    {
+        for (size_t i = 0; i < 14; ++i)
+        {
+            vertices.push_back(
+                    boost::add_vertex(core::ObjectDataPtr(new core::ObjectData(i)), graph));
+        }
 
-//    boost::add_edge(vertices[0], vertices[1], 0.0, graph);
-//    boost::add_edge(vertices[0], vertices[2], 0.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);
-//    boost::add_edge(vertices[2], vertices[4], 10.0, graph);
-//    boost::add_edge(vertices[3], vertices[5], 12.0, graph);
-//    boost::add_edge(vertices[3], vertices[6], 12.0, graph);
-//    boost::add_edge(vertices[4], vertices[5], 11.0, graph);
-//    boost::add_edge(vertices[4], vertices[6], 10.0, graph);
-//    boost::add_edge(vertices[5], vertices[7], 15.0, graph);
-//    boost::add_edge(vertices[5], vertices[8], 12.0, graph);
-//    boost::add_edge(vertices[6], vertices[7], 12.0, graph);
-//    boost::add_edge(vertices[6], vertices[8], 11.0, graph);
-//    boost::add_edge(vertices[7], vertices[9], 0.0, graph);
-//    boost::add_edge(vertices[8], vertices[9], 0.0, graph);
+        source = vertices[0];
+        sink = vertices[9];
+
+        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);
+        boost::add_edge(vertices[2], vertices[4], 10.0, graph);
+        boost::add_edge(vertices[3], vertices[5], 15.0, graph);
+        boost::add_edge(vertices[3], vertices[6], 12.0, graph);
+        boost::add_edge(vertices[4], vertices[5], 12.0, graph);
+        boost::add_edge(vertices[4], vertices[6], 11.0, graph);
+        boost::add_edge(vertices[5], vertices[7], 12.0, graph);
+        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], 1.0, graph);
+        boost::add_edge(vertices[8], vertices[9], 1.0, graph);
+
+        boost::add_edge(vertices[0], vertices[10], 20.0, graph);
+        boost::add_edge(vertices[10], vertices[11], 20.0, graph);
+        boost::add_edge(vertices[10], vertices[3], 20.0, graph);
+        boost::add_edge(vertices[10], vertices[4], 20.0, graph);
+        boost::add_edge(vertices[11], vertices[12], 20.0, graph);
+        boost::add_edge(vertices[11], vertices[5], 20.0, graph);
+        boost::add_edge(vertices[12], vertices[6], 20.0, graph);
+        boost::add_edge(vertices[13], vertices[9], 20.0, graph);
+    }
 }
 
 void TestSuurballe()
@@ -879,14 +918,66 @@ void TestSuurballe()
     std::cout << "Total paths length: " << suurballe.GetTotalPathsLength() << std::endl;
 }
 
+void CreateSuurballeGraph(DirectedGraph& graph, Vertex& source, Vertex& sink, bool first)
+{
+    std::vector<Vertex> vertices;
+
+    if (first)
+    {
+        // First example graph
+        for (int i = 0; i < 7; ++i)
+        {
+            vertices.push_back(boost::add_vertex(graph));
+        }
+
+        source = vertices[0];
+        sink = vertices[6];
+
+        boost::add_edge(vertices[0], vertices[1], 5.0, graph);
+        boost::add_edge(vertices[0], vertices[4], 2.0, graph);
+        boost::add_edge(vertices[1], vertices[2], 1.0, graph);
+        boost::add_edge(vertices[1], vertices[4], 1.0, graph);
+        boost::add_edge(vertices[2], vertices[6], 1.0, graph);
+        boost::add_edge(vertices[3], vertices[2], 1.0, graph);
+        boost::add_edge(vertices[4], vertices[3], 2.0, graph);
+        boost::add_edge(vertices[4], vertices[5], 1.0, graph);
+        boost::add_edge(vertices[5], vertices[2], 1.0, graph);
+        boost::add_edge(vertices[5], vertices[6], 1.0, graph);
+    }
+    else
+    {
+        // Second example graph
+        for (int i = 0; i < 8; ++i)
+        {
+            vertices.push_back(boost::add_vertex(graph));
+        }
+        source = vertices[0];
+        sink = vertices[7];
+        boost::add_edge(vertices[0], vertices[1], 1.0, graph);
+        boost::add_edge(vertices[0], vertices[4], 8.0, graph);
+        boost::add_edge(vertices[0], vertices[5], 1.0, graph);
+        boost::add_edge(vertices[1], vertices[2], 1.0, graph);
+        boost::add_edge(vertices[1], vertices[7], 8.0, graph);
+        boost::add_edge(vertices[2], vertices[3], 1.0, graph);
+        boost::add_edge(vertices[3], vertices[4], 1.0, graph);
+        boost::add_edge(vertices[3], vertices[6], 2.0, graph);
+        boost::add_edge(vertices[4], vertices[7], 1.0, graph);
+        boost::add_edge(vertices[5], vertices[2], 2.0, graph);
+        boost::add_edge(vertices[5], vertices[6], 6.0, graph);
+        boost::add_edge(vertices[6], vertices[7], 1.0, graph);
+    }
+}
+
 void TestYAOKSP()
 {
     Vertex source, sink;
     DirectedGraph graph;
-    CreatePresentationGraph(graph, source, sink);
+//    CreatePresentationGraph(graph, source, sink, true);
+    CreateSuurballeGraph(graph, source, sink, false);
+//    CreateBerclazGraph(graph, source, sink);
     algo::KShortestPaths5 ksp(graph, source, sink);
 
-    ksp.Run(2);
+    ksp.Run(3);
 
     std::vector<std::vector<Vertex>> paths;
     ksp.GetPaths(paths);
@@ -895,7 +986,7 @@ void TestYAOKSP()
         std::cout << "path: ";
         for (auto v : path)
         {
-            std::cout << v << " ";
+            std::cout << std::setw(2) << v << " ";
         }
         std::cout << std::endl;
     }
@@ -905,7 +996,7 @@ int main(int argc, char** argv)
 {
     //TODO load with frame offset
 
-//    Run(argc, argv);
+    Run(argc, argv);
 
 //    TestTracklet();
 
@@ -939,7 +1030,7 @@ int main(int argc, char** argv)
 
 //    TestSuurballe();
 
-    TestYAOKSP();
+//    TestYAOKSP();
 
     return 0;
 }

util/Logger.cpp

@@ -17,17 +17,22 @@ namespace util
         std::cout << message;
     }
 
+    void Logger::LogErrorMessage(const std::string& message)
+    {
+        std::cerr << message;
+    }
+
     void Logger::LogInfo(const std::string& message)
     {
         if (Instance().info_)
         {
-            Instance().LogMessage("[Info] " + message + "\n");
+            Instance().LogMessage("[Info ] " + message + "\n");
         }
     }
 
     void Logger::LogError(const std::string& message)
     {
-        Instance().LogMessage("[Error] " + message + "\n");
+        Instance().LogErrorMessage("[Error] " + message + "\n");
     }
 
     void Logger::LogDebug(const std::string& message)

util/Logger.h

@@ -36,6 +36,12 @@ namespace util
          * @param message The message to log
          */
         void LogMessage(const std::string& message);
+
+        /**
+         * Logs the given error message.
+         * @param message The error message to log
+         */
+        void LogErrorMessage(const std::string& message);
     public:
         /**
          * -> Singleton

util/Visualizer.cpp

@@ -10,7 +10,7 @@
 namespace util
 {
     void Visualizer::Display(core::DetectionSequence& sequence,
-                             std::string image_folder,
+                             std::string image_folder, bool output, std::string output_path,
                              std::string title, size_t first_frame,
                              int play_fps)
     {
@@ -116,8 +116,9 @@ namespace util
     }
 
     void Visualizer::Display(std::vector<core::TrackletPtr>& tracks,
-                             std::string image_folder, std::string title,
-                             size_t first_frame, int play_fps, int grid_width, int grid_height)
+                             std::string image_folder, bool output, std::string output_path,
+                             std::string title, size_t first_frame, int play_fps, int grid_width,
+                             int grid_height)
     {
         util::Logger::LogInfo("Displaying data");
 
@@ -127,9 +128,6 @@ namespace util
         std::vector<std::string> image_files;
         util::FileIO::ListFiles(image_folder, image_files);
 
-        // Create window
-        cv::namedWindow(title, CV_WINDOW_AUTOSIZE);
-
         // Generate a random color for each individual track
         std::vector<cv::Scalar> colors;
         std::random_device rd;
@@ -147,6 +145,26 @@ namespace util
             colors.push_back(color);
         }
 
+        //TODO move to extra class
+        //TODO create necessary directories
+        if (output)
+        {
+            util::Logger::LogInfo("Start writing output images");
+            for (size_t i = 0; i < image_files.size(); ++i)
+            {
+                cv::Mat image = cv::imread(image_folder + "/" + image_files[i], 1);
+                for (size_t j = 0; j < tracks.size(); ++j)
+                {
+                    tracks[j]->Visualize(image, colors[j], i, 0, 0);
+                }
+                cv::imwrite(output_path + "/images/" + image_files[i], image);
+            }
+            util::Logger::LogInfo("Finished writing output images");
+        }
+
+        // Create window
+        cv::namedWindow(title, CV_WINDOW_AUTOSIZE);
+
         // Display frames and data
         int target_delay = 1000 / play_fps;
         int last_frame_time = GetTime();
@@ -155,11 +173,9 @@ namespace util
         while (true)
         {
             // Display image
-            cv::Mat image = cv::imread(image_folder + "/"
-                                       + image_files[current_frame],
-                                       1);
+            cv::Mat image = cv::imread(image_folder + "/" + image_files[current_frame], 1);
 
-            //TODO draw grid (experimental)
+            // Draw grid
             if (grid_width > 0 && grid_height > 0)
             {
                 cv::Scalar gridColor(255, 255, 255);
@@ -177,6 +193,7 @@ namespace util
                 }
             }
 
+            //Visualize the tracklets
             for (size_t i = 0; i < tracks.size(); ++i)
             {
                 tracks[i]->Visualize(image, colors[i], current_frame, 0, 0);

util/Visualizer.h

@@ -38,7 +38,7 @@ namespace util
          * @param play_fps The FPS to use when auto play is activated.
          */
         void Display(core::DetectionSequence& sequence,
-                     std::string image_folder,
+                     std::string image_folder, bool output, std::string output_path,
                      std::string title = "Visualizer", size_t first_frame = 0,
                      int play_fps = 24);
 
@@ -57,7 +57,7 @@ namespace util
          * @param grid_height The number of cells in a column
          */
         void Display(std::vector<core::TrackletPtr>& tracks,
-                     std::string image_folder,
+                     std::string image_folder, bool output, std::string output_path,
                      std::string title = "Visualizer", size_t first_frame = 0,
                      int play_fps = 24, int grid_width = 0, int grid_height = 0);
     };