# HG changeset patch
# User Nicolas Saunier <nicolas.saunier@polymtl.ca>
# Date 1323387155 18000
# Node ID 0e60a306d324f42aede5f71cecd6b4222434577b
# Parent  36968a63efe13d0d327305f9b82da9e7af07dd58
added basic code to identify features and save them (crash)

diff -r 36968a63efe1 -r 0e60a306d324 c/Motion.cpp
--- a/c/Motion.cpp	Wed Dec 07 18:51:32 2011 -0500
+++ b/c/Motion.cpp	Thu Dec 08 18:32:35 2011 -0500
@@ -7,9 +7,11 @@
 #include "opencv2/highgui/highgui.hpp"
 
 #include <boost/graph/connected_components.hpp>
+#include <boost/property_map/property_map.hpp>
 
 #include <iostream>
 #include <vector>
+#include <map>
 #include <algorithm>
 #include <utility>
 
@@ -147,7 +149,7 @@
 /******************** FeatureGraph ********************/
 
 void FeatureGraph::addFeature(const FeatureTrajectoryPtr& ft) {
-  UndirectedGraph::vertex_descriptor newVertex = add_vertex(graph);
+  vertex_descriptor newVertex = add_vertex(graph);
   graph[newVertex].feature = ft;
   for (graph_traits<UndirectedGraph>::vertex_iterator vi = vertices(graph).first; 
        vi!=vertices(graph).second; ++vi) { // vi pointer to vertex_descriptor
@@ -165,17 +167,53 @@
   }
 }
 
-void FeatureGraph::connectedComponents(const int& lastInstant) {
-  vector<UndirectedGraph::vertex_descriptor> components(num_vertices(graph));
-  //vector_property_map<UndirectedGraph::vertex_descriptor> components(num_vertices(graph));
-  //vector_property_map< graph_traits<UndirectedGraph>::vertex_descriptor, property_map<UndirectedGraph, vertex_index_t>::const_type > components(num_vertices(graph));
+vector<vector<FeatureGraph::vertex_descriptor> > FeatureGraph::connectedComponents(const int& lastInstant) {
+  int nVertices = num_vertices(graph);
+  vector<vertex_descriptor> components(nVertices);
+  // map<UndirectedGraph::vertex_descriptor, graph_traits<UndirectedGraph>::vertices_size_type> vertex2component;
+  // associative_property_map< map<UndirectedGraph::vertex_descriptor, graph_traits<UndirectedGraph>::vertices_size_type> > components(vertex2component);
 
   int num = connected_components(graph, &components[0]);
   cout << "Total number of components: " << num << endl;
 
-  for (unsigned int i = 0; i < num_vertices(graph); ++i)
-    cout << "Vertex " << i <<" is in component " << components[i] << endl;
-  cout << endl;
+  vector<unsigned int> lastInstants(num, 0);
+  vector<vector<vertex_descriptor> > tmpobjects(num), objects;
+
+  for (int i = 0; i < nVertices; ++i) {
+    cout << "Vertex " << i <<" is in component " << components[i] << endl;// "(last " << graph[i].feature->getLastInstant() << " " << lastInstants[components[i]] << " " << (lastInstants[components[i]] < graph[i].feature->getLastInstant()) << ")" << endl;
+    if (lastInstants[components[i]] < graph[i].feature->getLastInstant())
+      lastInstants[components[i]] = graph[i].feature->getLastInstant();
+    tmpobjects[components[i]].push_back(i);
+  }
+
+  for (int i = 0; i < num; ++i) {
+    cout << i << " " << lastInstants[i] << endl;
+    if (static_cast<int>(lastInstants[i]) < lastInstant)
+      objects.push_back(tmpobjects[i]);
+  }
+
+  return objects;
+}
+
+vector<vector<unsigned int> > FeatureGraph::getFeatureGroups(const vector<vector<FeatureGraph::vertex_descriptor> >& objectHypotheses) {
+  vector<vector<unsigned int> > featureGroups;
+
+  for (unsigned int i=0; i<objectHypotheses.size(); ++i) {
+    // check that there is on average at least minNFeaturesPerGroup features at each frame in the group
+    float totalFeatureTime=0;
+    for (unsigned int j=0; j<objectHypotheses.size(); ++j)
+      totalFeatureTime += static_cast<float>(graph[objectHypotheses[i][j]].feature->length());
+    
+    if (totalFeatureTime/static_cast<float>(objectHypotheses.size()) > minNFeaturesPerGroup) {
+      cout << "save group" << endl;
+      featureGroups.push_back(vector<unsigned int>());
+      for (unsigned int j=0; j<objectHypotheses.size(); ++j)
+	featureGroups.back().push_back(graph[objectHypotheses[i][j]].feature->getId());
+    }
+    // remove vertices: todo using listS
+  }
+
+  return featureGroups;
 }
 
 string FeatureGraph::informationString(void) {
diff -r 36968a63efe1 -r 0e60a306d324 c/Parameters.cpp
--- a/c/Parameters.cpp	Wed Dec 07 18:51:32 2011 -0500
+++ b/c/Parameters.cpp	Thu Dec 08 18:32:35 2011 -0500
@@ -55,7 +55,7 @@
     ("mm-segmentation-distance", po::value<float>(&mmSegmentationDistance), "segmentation distance in feature grouping")
     ("max-distance", po::value<float>(&maxDistance), "maximum distance between features for grouping")
     ("min-velocity-cosine", po::value<float>(&minVelocityCosine), "minimum cosine of the angle between the velocity vectors for grouping")
-    ("min-nfeatures-group", po::value<int>(&minNFeaturesPerGroup), "minimum average number of features per frame to create a vehicle hypothesis")
+    ("min-nfeatures-group", po::value<float>(&minNFeaturesPerGroup), "minimum average number of features per frame to create a vehicle hypothesis")
     ;
     // ("max-uturn-cosine", po::value<float>(&maxUTurnCosine), "maximum cosine value to detect U-turn")
     // ("nframes-avoid-uturn", po::value<int>(&nFramesAvoidUTurn), "number of frames over which a feature should not make a U-turn")
diff -r 36968a63efe1 -r 0e60a306d324 c/feature-based-tracking.cpp
--- a/c/feature-based-tracking.cpp	Wed Dec 07 18:51:32 2011 -0500
+++ b/c/feature-based-tracking.cpp	Thu Dec 08 18:32:35 2011 -0500
@@ -265,8 +265,9 @@
   int maxTrajectoryLength;
   trajectoryDB->maxTrajectoryLength(maxTrajectoryLength);
   cout << "max trajectory length " << maxTrajectoryLength << endl;
+  maxTrajectoryLength = 20; // for tests
 
-  FeatureGraph featureGraph(params.mmConnectionDistance, params.mmSegmentationDistance, params.minFeatureTime);
+  FeatureGraph featureGraph(params.mmConnectionDistance, params.mmSegmentationDistance, params.minFeatureTime, params.minNFeaturesPerGroup);
 
   // main loop
   // TODO version that can be interrupted?
@@ -286,8 +287,14 @@
     }
 
     // check for connected components that are old enough (no chance to match with trajectories to be added later
+    // we could check only when some features are getting old enough?
     if (frameNum%10 == 0) {
-      featureGraph.connectedComponents(frameNum-maxTrajectoryLength+params.minFeatureTime);
+      vector<vector<FeatureGraph::vertex_descriptor> > objects = featureGraph.connectedComponents(frameNum-maxTrajectoryLength+params.minFeatureTime);
+      cout << objects.size() << " objects" << endl;
+
+      if (!objects.empty()) {
+	vector<vector<unsigned int> > featureGroups = featureGraph.getFeatureGroups(objects);
+      }
     }
 
     cout << featureGraph.informationString() << endl;
diff -r 36968a63efe1 -r 0e60a306d324 include/Motion.hpp
--- a/include/Motion.hpp	Wed Dec 07 18:51:32 2011 -0500
+++ b/include/Motion.hpp	Thu Dec 08 18:32:35 2011 -0500
@@ -26,6 +26,7 @@
 
   unsigned int length(void) const { return positions->size();} // cautious if not continuous: max-min+1
 
+  unsigned int getId(void) const { return positions->getId();}
   void setId(const unsigned int& id) { positions->setId(id);velocities->setId(id);}
 
   void setLost(void) { lost = true;}
@@ -92,20 +93,6 @@
 /** Class to group features: Beymer et al. 99/Saunier and Sayed 06
     \todo create various graph types with different parameters, that accept different feature distances or ways to connect and segment features */
 class FeatureGraph {
-public:
-  //FeatureGraph(float _minDistance, float _maxDistance) : minDistance (_minDistance), maxDistance(_maxDistance) {}
-  FeatureGraph(float _connectionDistance, float _segmentationDistance, unsigned int _minFeatureTime) : connectionDistance (_connectionDistance), segmentationDistance(_segmentationDistance), minFeatureTime(_minFeatureTime) {}
-
-  void addFeature(const FeatureTrajectoryPtr& ft);
-
-  // add vertex, includes adding links to current vertices
-  // find connected components, check if old enough, if so, remove
-
-  /// Computes the connected components: features have to be older than lastInstant
-  void connectedComponents(const int& lastInstant);
-
-  std::string informationString(void);
-
 protected:
   struct FeatureConnection {
     float minDistance;
@@ -118,9 +105,32 @@
 
   typedef boost::adjacency_list<boost::listS, boost::vecS, boost::undirectedS, VertexInformation, FeatureConnection> UndirectedGraph;
 
+public:
+  typedef UndirectedGraph::vertex_descriptor vertex_descriptor;
+
+  //FeatureGraph(float _minDistance, float _maxDistance) : minDistance (_minDistance), maxDistance(_maxDistance) {}
+  FeatureGraph(float _connectionDistance, float _segmentationDistance, unsigned int _minFeatureTime, float _minNFeaturesPerGroup) : connectionDistance (_connectionDistance), segmentationDistance(_segmentationDistance), minFeatureTime(_minFeatureTime), minNFeaturesPerGroup(_minNFeaturesPerGroup) {}
+
+  void addFeature(const FeatureTrajectoryPtr& ft);
+
+  // add vertex, includes adding links to current vertices
+  // find connected components, check if old enough, if so, remove
+
+  /// Computes the connected components: features have to be older than lastInstant
+  std::vector<std::vector<vertex_descriptor> > connectedComponents(const int& lastInstant);
+
+  /** Performs some checks on groups of features and return their lists of ids if correct
+      Removes the vertices from the graph
+   */
+  std::vector<std::vector<unsigned int> > getFeatureGroups(const std::vector<std::vector<vertex_descriptor> >& objectHypotheses);
+
+  std::string informationString(void);
+
+protected:
   float connectionDistance;
   float segmentationDistance;
   unsigned int minFeatureTime;
+  float minNFeaturesPerGroup;
   // float minDistance;
   // float maxDistance;
 
diff -r 36968a63efe1 -r 0e60a306d324 include/Parameters.hpp
--- a/include/Parameters.hpp	Wed Dec 07 18:51:32 2011 -0500
+++ b/include/Parameters.hpp	Thu Dec 08 18:32:35 2011 -0500
@@ -47,7 +47,7 @@
   float mmSegmentationDistance;
   float maxDistance;
   float minVelocityCosine;
-  int minNFeaturesPerGroup;
+  float minNFeaturesPerGroup;
 
   std::string parameterDescription;