added presentation graph

improved berclaz performance
added grid convolution

algo/Berclaz.cpp

@@ -56,7 +56,7 @@ namespace algo
                 for (int x = 0; x < grid.GetWidthCount(); ++x)
                 {
                     // First vertex index
-                    int vi = x + y * grid.GetHeightCount() + z * layer_size;
+                    int vi = x + y * grid.GetWidthCount() + z * layer_size;
 
                     // Get the score, clamp it, prevent division by zero and
                     // logarithm of zero
@@ -78,28 +78,22 @@ namespace algo
                     {
                         // 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.GetWidthCount() + (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, VIRTUAL_EDGE_WEIGHT,
-                                        graph);
+                        boost::add_edge(vertices[vi], sink, VIRTUAL_EDGE_WEIGHT, graph);
                     }
                     else
                     {
@@ -107,14 +101,25 @@ namespace algo
                     }
 
                     // Connect with source and sink
-                    boost::add_edge(source, vertices[vi], VIRTUAL_EDGE_WEIGHT,
-                                    graph);
+                    boost::add_edge(source, vertices[vi], VIRTUAL_EDGE_WEIGHT, graph);
                 }
             }
         }
 
         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("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)
+        ));
     }
 
     void Berclaz::ExtractTracks(DirectedGraph& graph, MultiPredecessorMap& map, Vertex origin,
@@ -163,8 +168,10 @@ namespace algo
             ExtractTracks(graph, ksp_result, sink, tracks);
         }
 
+        // Only connect tracks if the sequence was split
         if (batch_size < sequence.GetFrameCount())
         {
+            //TODO find a better way to connect tracks (n-stage)
             util::Logger::LogDebug("connect tracks");
             ConnectTracks(tracks);
         }

algo/KShortestPaths2.cpp

@@ -56,6 +56,7 @@ namespace algo
 
     void KShortestPaths2::UpdateEdges()
     {
+        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_)
@@ -64,6 +65,7 @@ namespace algo
         }
         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_)
@@ -102,15 +104,12 @@ namespace algo
         }
     }
 
-    void KShortestPaths2::QueueRemoveAll()
+    void KShortestPaths2::ClearAllEdges()
     {
-        boost::graph_traits<DirectedGraph>::edge_iterator ei, ei_end;
-        for (boost::tie(ei, ei_end) = boost::edges(graph_copy_);
-             ei != ei_end; ++ei)
+        boost::graph_traits<DirectedGraph>::vertex_iterator vi, vi_end;
+        for (boost::tie(vi, vi_end) = boost::vertices(graph_copy_); vi != vi_end; ++vi)
         {
-            Vertex s_copy = boost::source(*ei, graph_copy_);
-            Vertex t_copy = boost::target(*ei, graph_copy_);
-            QueueRemoveEdge(s_copy, t_copy);
+            boost::clear_out_edges(*vi, graph_copy_);
         }
     }
 
@@ -143,7 +142,7 @@ namespace algo
             }
 
             ExtendGraph(i);
-            i > 0 ? TransformEdges(i - 1) : TransformEdges(0);
+            TransformEdges(i > 0 ? i - 1 : 0);
             FindAndAugment(i + 1);
             Interlace(i + 1);
         }
@@ -156,9 +155,9 @@ namespace algo
         util::Logger::LogDebug("extend graph iteration: " + std::to_string(iteration));
 
         // Clear all previous working data
-        util::Logger::LogDebug("remove all");
-        QueueRemoveAll();
-        util::Logger::LogDebug("copy all");
+        util::Logger::LogDebug("clear all edges");
+        ClearAllEdges();
+        util::Logger::LogDebug("queue copy edges");
         QueueCopyEdges();
         util::Logger::LogDebug("update edges");
         UpdateEdges();
@@ -201,6 +200,7 @@ namespace algo
             // 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");
@@ -231,6 +231,8 @@ namespace algo
 
         util::Logger::LogDebug("update edges");
         UpdateEdges();
+
+        util::FileIO::WriteCSVMatlab(graph_copy_, "/home/wrede/Dokumente/graph_" + std::to_string(iteration) + "e.csv");
     }
 
     void KShortestPaths2::TransformEdges(size_t iteration)
@@ -241,8 +243,7 @@ namespace algo
 
         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)
+        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_);
@@ -252,21 +253,21 @@ namespace algo
             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;
-                }
+//                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
-                {
+//                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];
-                }
+//                }
             }
 
             // Correct rounding errors
@@ -284,7 +285,7 @@ namespace algo
             }
         }
 
-//        util::FileIO::WriteCSVMatlab(graph_copy_, "/home/wrede/Dokumente/graph_t.csv");
+        util::FileIO::WriteCSVMatlab(graph_copy_, "/home/wrede/Dokumente/graph_" + std::to_string(iteration) + "t.csv");
     }
 
     void KShortestPaths2::FindAndAugment(size_t iteration)
@@ -346,10 +347,23 @@ namespace algo
 
         // 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)
+        for (boost::tie(vi, vi_end) = boost::vertices(graph_copy_); vi != vi_end; ++vi)
         {
-            i_distances_[iteration][*vi] = dist_map[*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 (the given path is in the copied graph, so the

algo/KShortestPaths2.h

@@ -33,7 +33,7 @@ namespace algo
         double PathCosts(size_t iteration);
 
         void QueueCopyEdges();
-        void QueueRemoveAll();
+        void ClearAllEdges();
         void QueueRemoveEdge(Vertex source, Vertex target);
         void QueueAddEdge(Vertex source, Vertex target, double weight);
         void UpdateEdges();

core/ObjectData.cpp

@@ -48,12 +48,10 @@ namespace core
         return 0.0;
     }
 
-    ObjectDataPtr ObjectData::Interpolate(ObjectDataPtr obj,
-                                       double fraction) const
+    ObjectDataPtr ObjectData::Interpolate(ObjectDataPtr obj, double fraction) const
     {
         size_t index = static_cast<size_t>(
-                util::MyMath::Lerp(
-                        GetFrameIndex(), obj->GetFrameIndex(), fraction));
+                util::MyMath::Lerp(GetFrameIndex(), obj->GetFrameIndex(), fraction));
 
         return ObjectDataPtr(new ObjectData(index));
     }

core/Tracklet.cpp

@@ -36,6 +36,8 @@ namespace core
 
     void Tracklet::AddPathObject(ObjectDataPtr obj, bool overwrite)
     {
+        // Prevent virtual objects to be added, they should be interpolated later from
+        // the real detections
         if (!obj->IsVirtual())
         {
             bool inserted = false;
@@ -102,19 +104,14 @@ namespace core
     void Tracklet::Visualize(cv::Mat& image, cv::Scalar& color, size_t frame,
                              size_t predecessor_count, size_t successor_count) const
     {
-        if (frame == 0)
-        {
-            predecessor_count = 0;
-        }
+        // Prevent negative values because frame is unsigned
+        predecessor_count = std::min(predecessor_count, frame);
 
         size_t start = (frame - predecessor_count > GetFirstFrameIndex()) ?
                        frame - predecessor_count : GetFirstFrameIndex();
         size_t end = (frame + successor_count < GetLastFrameIndex()) ?
                      frame + successor_count : GetLastFrameIndex();
 
-//        util::Logger::LogDebug("tracklet frame range: " + std::to_string(GetFirstFrameIndex()) + "-" + std::to_string(GetLastFrameIndex()));
-//        util::Logger::LogDebug("tracklet visualize frames: " + std::to_string(start) + "-" + std::to_string(end));
-
         for (auto obj : path_objects_)
         {
             if (obj->GetFrameIndex() >= start && obj->GetFrameIndex() <= end)
@@ -132,8 +129,7 @@ namespace core
             if (gap > 1)
             {
                 path_objects_.insert(path_objects_.begin() + i,
-                                     path_objects_[i - 1]->Interpolate(path_objects_[i],
-                                                                       0.5));
+                                     path_objects_[i - 1]->Interpolate(path_objects_[i], 0.5));
                 --i;
             }
         }
@@ -172,6 +168,19 @@ namespace core
             AddPathObject(obj);
         }
     }
+
+    ObjectDataPtr Tracklet::GetFrameObject(size_t frame_index)
+    {
+        for (auto obj : path_objects_)
+        {
+            if (obj->GetFrameIndex() == frame_index)
+            {
+                return obj;
+            }
+        }
+
+        return nullptr;
+    }
 }
 
 

core/Tracklet.h

@@ -114,6 +114,8 @@ namespace core
         void Flatten();
 
         void Combine(TrackletPtr other);
+
+        ObjectDataPtr GetFrameObject(size_t frame_index);
     };
 }
 

main/main.cpp

@@ -357,6 +357,7 @@ void Run(int argc, char** argv)
         std::cout << opts << std::endl;
         exit(0);
     }
+
     end_time = time(0);
     util::Logger::LogInfo("Time measurement stopped");
     util::Logger::LogInfo("Time passed: "
@@ -364,13 +365,18 @@ void Run(int argc, char** argv)
                           + " minutes");
 
     // Write the output file
-    //util::FileIO::WriteCSV(tracks, output_file, output_delimiter);
+    util::FileIO::WriteTracks(tracks, output_file, output_delimiter);
 
     // Display the tracking data
     if (display)
     {
         util::Visualizer vis;
-        vis.Display(tracks, images_folder);
+
+        if (algorithm == "berclaz")
+            vis.Display(tracks, images_folder, "Visualizer",
+                        0, 24, berclaz_params.h_res, berclaz_params.v_res);
+        else
+            vis.Display(tracks, images_folder);
     }
 }
 
@@ -499,15 +505,17 @@ void CreateTestGraph(DirectedGraph& graph, Vertex& source, Vertex& sink)
 //    boost::add_edge(vertices[8], vertices[7], 0.0, graph);
 }
 
-void TestKSP(DirectedGraph& graph, Vertex& source, Vertex& sink, size_t n_paths)
+void TestKSP(DirectedGraph graph, Vertex source, Vertex sink, size_t n_paths)
 {
+    util::FileIO::WriteCSVMatlab(graph, "/home/wrede/Dokumente/graph_ksp.csv");
+
     algo::KShortestPaths2 ksp;
     MultiPredecessorMap paths = ksp.Run(graph, source, sink, n_paths);
 
     util::FileIO::WriteCSVMatlab(paths, source, sink, "/home/wrede/Dokumente/paths_ksp.csv");
 }
 
-void TestKBellmanFord(DirectedGraph& graph, Vertex& source, Vertex& sink, size_t n_paths)
+void TestKBellmanFord(DirectedGraph graph, Vertex source, Vertex sink, size_t n_paths)
 {
     util::FileIO::WriteCSVMatlab(graph, "/home/wrede/Dokumente/graph_kbf.csv");
 
@@ -751,22 +759,68 @@ 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)
+{
+    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];
+
+    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], 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], 0.0, graph);
+    boost::add_edge(vertices[8], vertices[9], 0.0, 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);
+}
+
 int main(int argc, char** argv)
 {
     //TODO load with frame offset
 
-    Run(argc, argv);
+//    Run(argc, argv);
 
 //    TestTracklet();
 
-//    DirectedGraph graph;
-//    Vertex source, sink;
-//    CreateBerclazGraph(graph, source, sink);
+    DirectedGraph graph;
+    Vertex source, sink;
+    CreatePresentationGraph(graph, source, sink);
 
 //    util::FileIO::WriteCSVMatlab(graph, "/home/wrede/Dokumente/graph.csv");
 
-//    TestKBellmanFord(graph, source, sink, 3);
-//    TestKSP(graph, source, sink, 10);
+    TestKBellmanFord(graph, source, sink, 2);
+    TestKSP(graph, source, sink, 3);
 
 //    TestGrid();
 

util/FileIO.cpp

@@ -6,6 +6,7 @@
 #include "FileIO.h"
 #include "Logger.h"
 #include "dirent.h"
+#include "../core/ObjectData2D.h"
 
 namespace util
 {
@@ -318,8 +319,8 @@ namespace util
         in.close();
     }
 
-    void FileIO::WriteCSV(std::vector<core::TrackletPtr>& tracks, const std::string& file_name,
-                          char delimiter)
+    void FileIO::WriteTracks(std::vector<core::TrackletPtr>& tracks, const std::string& file_name,
+                             char delimiter)
     {
         // Get the frame range
         size_t first_frame = tracks[0]->GetFirstFrameIndex();
@@ -336,13 +337,94 @@ namespace util
 
         if (!out.is_open())
         {
-            util::Logger::LogError("Unable to open the the file: " + file_name);
+            util::Logger::LogError("Unable to open file: " + file_name);
             return;
         }
 
-        //TODO
+        for (size_t frame = first_frame; frame <= last_frame; ++frame)
+        {
+            for (size_t i = 0; i < tracks.size(); ++i)
+            {
+                core::ObjectData2DPtr obj =
+                        std::static_pointer_cast<core::ObjectData2D>(tracks[i]->GetFrameObject(frame));
+
+                if (obj != nullptr)
+                {
+                    out << obj->GetPosition().x << delimiter;
+                    out << obj->GetPosition().y;
+                }
+                else
+                {
+                    out << delimiter;
+                }
+
+                if (i < (tracks.size() - 1))
+                {
+                    out << delimiter;
+                }
+            }
+
+            out << std::endl;
+        }
 
         out.close();
     }
+
+    void FileIO::ReadTracks(std::vector<core::TrackletPtr>& tracks, const std::string& file_name,
+                            char delimiter)
+    {
+        std::ifstream in(file_name, std::ios::in);
+
+        if (!in.is_open())
+        {
+            util::Logger::LogError("Unable to open file: " + file_name);
+            return;
+        }
+
+        std::string line;
+        size_t line_index = 0;
+        while (in.good() && !in.eof())
+        {
+            getline(in, line);
+
+            if (line.size() == 0) continue;
+
+            std::vector<std::string> parts = split(line, delimiter);
+
+            while (tracks.size() < (parts.size() / 2))
+            {
+                tracks.push_back(core::TrackletPtr(new core::Tracklet()));
+            }
+
+            for (size_t i = 1; i < parts.size(); i += 2)
+            {
+                if (!parts[i - 1].empty() && !parts[i].empty())
+                {
+                    cv::Point2d point(std::stod(parts[i - 1]), std::stod(parts[i]));
+
+                    tracks[(i - 1) / 2]->AddPathObject(
+                            core::ObjectData2DPtr(new core::ObjectData2D(line_index, point)));
+                }
+            }
+
+            line_index++;
+        }
+
+        in.close();
+    }
+
+    std::vector<std::string> FileIO::split(const std::string& input, char delimiter)
+    {
+        std::vector<std::string> output;
+        std::stringstream ss(input);
+        std::string part;
+        while (getline(ss, part, delimiter))
+        {
+            output.push_back(part);
+        }
+        return output;
+    }
+
+
 }
 

util/FileIO.h

@@ -105,8 +105,13 @@ namespace util
                             const std::string& file_name, char delimiter);
 
         //TODO comment
-        static void WriteCSV(std::vector<core::TrackletPtr>& tracks, const std::string& file_name,
-                             char delimiter);
+        static void WriteTracks(std::vector<core::TrackletPtr>& tracks, const std::string& file_name,
+                                char delimiter);
+
+        static void ReadTracks(std::vector<core::TrackletPtr>& tracks, const std::string& file_name,
+                               char delimiter);
+
+        static std::vector<std::string> split(const std::string& input, char delimiter);
     };
 }
 

util/Grid.cpp

@@ -109,4 +109,101 @@ namespace util
     {
         return depth_;
     }
+
+    void Grid::Convolve2D(int vicinity, double* mask, double multiplier)
+    {
+        // [x,y,z]    position in grid
+        // [vx,vy,vz] position in vicinity
+        // [nx,ny,nz] position in grid
+        // [mx,my,mz] position in mask
+        // [mi]       index in mask
+
+        int mask_size = vicinity * 2 + 1;
+        for (int z = 0; z < depth_count_; ++z)
+        {
+            for (int y = 0; y < height_count_; ++y)
+            {
+                for (int x = 0; x < width_count_; ++x)
+                {
+                    double score = 0.0;
+
+                    for (int vy = -vicinity; vy <= vicinity; ++vy)
+                    {
+                        int ny = y + vy;
+
+                        if (ny < 0 || ny >= height_count_) continue;
+
+                        int my = vy + vicinity;
+
+                        for (int vx = -vicinity; vx <= vicinity; ++vx)
+                        {
+                            int nx = x + vx;
+
+                            if (nx < 0 || nx >= width_count_) continue;
+
+                            int mx = vx + vicinity;
+                            int mi = my * mask_size + mx;
+
+                            score += GetValue(nx, ny, z)->GetDetectionScore() * mask[mi];
+                        }
+                    }
+
+                    GetValue(x, y, z)->SetDetectionScore(score * multiplier);
+                }
+            }
+        }
+    }
+
+    void Grid::Convolve3D(int vicinity, double* mask, double multiplier)
+    {
+        // [x,y,z]    position in grid
+        // [vx,vy,vz] position in vicinity
+        // [nx,ny,nz] position in grid
+        // [mx,my,mz] position in mask
+        // [mi]       index in mask
+
+        int mask_size = vicinity * 2 + 1;
+        for (int z = 0; z < depth_count_; ++z)
+        {
+            for (int y = 0; y < height_count_; ++y)
+            {
+                for (int x = 0; x < width_count_; ++x)
+                {
+                    double score = 0.0;
+
+                    for (int vz = -vicinity; vz <= vicinity; ++vz)
+                    {
+                        int nz = z + vz;
+
+                        if (nz < 0 || nz >= depth_count_) continue;
+
+                        int mz = vz + vicinity;
+
+                        for (int vy = -vicinity; vy <= vicinity; ++vy)
+                        {
+                            int ny = y + vy;
+
+                            if (ny < 0 || ny >= height_count_) continue;
+
+                            int my = vy + vicinity;
+
+                            for (int vx = -vicinity; vx <= vicinity; ++vx)
+                            {
+                                int nx = x + vx;
+
+                                if (nx < 0 || nx >= width_count_) continue;
+
+                                int mx = vx + vicinity;
+                                int mi = mz * mask_size * mask_size + my * mask_size + mx;
+
+                                score += GetValue(nx, ny, nz)->GetDetectionScore() * mask[mi];
+                            }
+                        }
+                    }
+
+                    GetValue(x, y, z)->SetDetectionScore(score * multiplier);
+                }
+            }
+        }
+    }
 }

util/Grid.h

@@ -172,6 +172,10 @@ namespace util
          */
         void PositionToIndex(double x, double y, double z,
                              int& xi, int& yi, int& zi) const;
+
+        //TODO comment
+        void Convolve2D(int vicinity, double* mask, double multiplier);
+        void Convolve3D(int vicinity, double* mask, double multiplier);
     };
 }
 

util/Parser.cpp

@@ -211,15 +211,16 @@ namespace util
         double depth = (double) (stop - start);
         Grid grid(res_x, res_y, res_z, width, height, depth);
 
-        // Fill with elements with detection score of zero
+        // Fill with elements with detection score of 0
         for (int z = 0; z < grid.GetDepthCount(); ++z)
         {
             for (int y = 0; y < grid.GetHeightCount(); ++y)
             {
                 for (int x = 0; x < grid.GetWidthCount(); ++x)
                 {
-                    core::ObjectDataPtr value(
-                            new core::ObjectData((size_t)(z + start)));
+                    // Add virtual object, thus the object will not be added to the final track and
+                    // instead be interpolated from real detections
+                    core::ObjectDataPtr value(new core::ObjectData());
 
                     grid.SetValue(value, x, y, z);
                 }
@@ -248,6 +249,27 @@ namespace util
             }
         }
 
+        // Convolve with linear filter
+//        int vicinity = 1;
+//        double multiplier = 0.25;
+//        double* linear_filter = new double[9] {
+//                0.25, 0.50, 0.25,
+//                0.50, 1.00, 0.50,
+//                0.25, 0.50, 0.25
+//        };
+//        grid.Convolve2D(vicinity, linear_filter, multiplier);
+//        delete[] linear_filter;
+
+        // Convolve with gaussian filter
+        int vicinity = 1;
+        double* gaussian_filter = new double[9] {
+                0.002284, 0.043222, 0.002284,
+                0.043222, 0.817976, 0.043222,
+                0.002284, 0.043222, 0.002284
+        };
+        grid.Convolve2D(vicinity, gaussian_filter, 1.0);
+        delete[] gaussian_filter;
+
         return grid;
     }
 }

util/Visualizer.cpp

@@ -117,7 +117,7 @@ namespace util
 
     void Visualizer::Display(std::vector<core::TrackletPtr>& tracks,
                              std::string image_folder, std::string title,
-                             size_t first_frame, int play_fps)
+                             size_t first_frame, int play_fps, int grid_width, int grid_height)
     {
         util::Logger::LogInfo("Displaying data");
 
@@ -134,10 +134,10 @@ namespace util
         std::vector<cv::Scalar> colors;
         std::random_device rd;
         std::mt19937 gen(rd());
-        colors.push_back(cv::Scalar(0, 0, 255));
         colors.push_back(cv::Scalar(0, 255, 0));
-        colors.push_back(cv::Scalar(255, 0, 0));
+        colors.push_back(cv::Scalar(0, 0, 255));
         colors.push_back(cv::Scalar(0, 255, 255));
+        colors.push_back(cv::Scalar(255, 0, 0));
         for (size_t i = 4; i < tracks.size(); ++i)
         {
             // BGR
@@ -159,12 +159,26 @@ namespace util
                                        + image_files[current_frame],
                                        1);
 
-//            util::Logger::LogDebug("display frame " + std::to_string(current_frame));
-            for (size_t i = 0; i < tracks.size(); ++i)
+            //TODO draw grid (experimental)
+            if (grid_width > 0 && grid_height > 0)
             {
-//                util::Logger::LogDebug("display track #" + std::to_string(i));
+                cv::Scalar gridColor(255, 255, 255);
+                double cellWidth = image.cols / grid_width;
+                double cellHeight = image.rows / grid_height;
+                for (int x = 0; x < grid_width; ++x)
+                {
+                    for (int y = 0; y < grid_height; ++y)
+                    {
+                        cv::rectangle(image,
+                                      cv::Point2d(x * cellWidth, y * cellHeight),
+                                      cv::Point2d((x + 1) * cellWidth, (y + 1) * cellHeight),
+                                      gridColor);
+                    }
+                }
+            }
 
-//                tracks[i]->Visualize(image, colors[i]);
+            for (size_t i = 0; i < tracks.size(); ++i)
+            {
                 tracks[i]->Visualize(image, colors[i], current_frame, 0, 0);
             }
 

util/Visualizer.h

@@ -46,16 +46,20 @@ namespace util
          * Displays the given tracks in an window.
          * Use D for next frame, A for previous frame, F to toggle auto play and
          * ESC to exit.
+         * If a grid size greater zero is specified a grid will be overlayed.
+         *
          * @param tracks The tracks to display
          * @param image_folder The images to use
          * @param title The window title
          * @param first_frame The frame to start at
-         * @param play_fps The FPS to use when auto play is activated.
+         * @param play_fps The FPS to use when auto play is activated
+         * @param grid_width The number of cells in a row
+         * @param grid_height The number of cells in a column
          */
         void Display(std::vector<core::TrackletPtr>& tracks,
                      std::string image_folder,
                      std::string title = "Visualizer", size_t first_frame = 0,
-                     int play_fps = 24);
+                     int play_fps = 24, int grid_width = 0, int grid_height = 0);
     };
 }