|
|
@@ -0,0 +1,735 @@
|
|
|
+/*
|
|
|
+ * markerAssociationDP.cpp
|
|
|
+ *
|
|
|
+ * Created on: Feb 6, 2013
|
|
|
+ * Author: koerner
|
|
|
+ */
|
|
|
+
|
|
|
+#include <boost/config.hpp>
|
|
|
+
|
|
|
+#include <boost/graph/adjacency_list.hpp>
|
|
|
+#include <boost/graph/dijkstra_shortest_paths.hpp>
|
|
|
+#include <boost/graph/graph_traits.hpp>
|
|
|
+#include <boost/graph/graphviz.hpp>
|
|
|
+#include <boost/graph/iteration_macros.hpp>
|
|
|
+#include <boost/graph/properties.hpp>
|
|
|
+
|
|
|
+#include <boost/property_map/property_map.hpp>
|
|
|
+
|
|
|
+#include <boost/program_options.hpp>
|
|
|
+#include <boost/format.hpp>
|
|
|
+
|
|
|
+#include <opencv2/opencv.hpp>
|
|
|
+
|
|
|
+#include <limits>
|
|
|
+#include <iostream>
|
|
|
+#include <fstream>
|
|
|
+#include <utility>
|
|
|
+#include <vector>
|
|
|
+
|
|
|
+/**
|
|
|
+ * command line parameters
|
|
|
+ */
|
|
|
+
|
|
|
+// command line parameters
|
|
|
+struct Params {
|
|
|
+ // filename of 3d point filen
|
|
|
+ std::string strTracklets;
|
|
|
+
|
|
|
+ // output filename
|
|
|
+ std::string outputFilename;
|
|
|
+
|
|
|
+ // number of tracklets to be extracted
|
|
|
+ size_t trackCount;
|
|
|
+
|
|
|
+ // maximum temporal distance (in frames) for two tracklets to get linked
|
|
|
+ size_t maxTemporalDist;
|
|
|
+
|
|
|
+ // maximum spatial distance (in mm) for two tracklets to get linked
|
|
|
+ double maxSpatialDist;
|
|
|
+
|
|
|
+ // minimum length of tracks to be selected
|
|
|
+ double minTrackLength;
|
|
|
+};
|
|
|
+
|
|
|
+// get command line parameters
|
|
|
+bool evalCmdLine(int argc, char **argv, Params &p) {
|
|
|
+ // define parameters
|
|
|
+ boost::program_options::options_description desc("Allowed options");
|
|
|
+ desc.add_options()
|
|
|
+ ("help", "produce help message")
|
|
|
+ ("trackletFile,t", boost::program_options::value< std::string >(), "tracklet input file (as generated by \"graphBasedTracklets\")")
|
|
|
+ ("outputFile,o", boost::program_options::value< std::string >(), "output file")
|
|
|
+ ("trackCount,c", boost::program_options::value< size_t >()->default_value(1), "number of tracks to be extracted")
|
|
|
+ ("maxSpatialDist,d",
|
|
|
+ boost::program_options::value< double >()->default_value(1.5),
|
|
|
+ "maximum spatial distance (in mm) for two tracklets to get linked")
|
|
|
+ ("maxTemporalDist,s",
|
|
|
+ boost::program_options::value< size_t >()->default_value(150),
|
|
|
+ "maximum temporal distance (in frames) for two tracklets to get linked")
|
|
|
+ ("minTrackLength,l", boost::program_options::value< double >()->default_value(0.85), "minimum length of tracks to be selected (if value is <= 1, it is interpreted as fraction of the sequence length)")
|
|
|
+ ;
|
|
|
+
|
|
|
+ // parse parameters
|
|
|
+ boost::program_options::variables_map vm;
|
|
|
+ boost::program_options::store(boost::program_options::parse_command_line(argc, argv, desc), vm);
|
|
|
+ boost::program_options::notify(vm);
|
|
|
+
|
|
|
+ if (vm.count("help") || vm.empty()) {
|
|
|
+ std::cout << desc << std::endl;
|
|
|
+ exit(EXIT_SUCCESS);
|
|
|
+ }
|
|
|
+
|
|
|
+ // default parameters
|
|
|
+ if (vm.count("trackletFile")) {
|
|
|
+ p.strTracklets = vm["trackletFile"].as< std::string >();
|
|
|
+ } else {
|
|
|
+ std::cerr << "No tracklet input file passed!\n";
|
|
|
+ std::cout << desc << std::endl;
|
|
|
+ exit(EXIT_SUCCESS);
|
|
|
+ }
|
|
|
+ if (vm.count("outputFile")) {
|
|
|
+ p.outputFilename = vm["outputFile"].as< std::string >();
|
|
|
+ } else {
|
|
|
+ std::cerr << "No output file passed!" << std::endl;
|
|
|
+ std::cout << desc << std::endl;
|
|
|
+ exit(EXIT_SUCCESS);
|
|
|
+ }
|
|
|
+
|
|
|
+ p.trackCount = vm["trackCount"].as< size_t >();
|
|
|
+ p.maxTemporalDist = vm["maxTemporalDist"].as< size_t >();
|
|
|
+ p.maxSpatialDist = vm["maxSpatialDist"].as< double >();
|
|
|
+ p.minTrackLength = vm["minTrackLength"].as< double >();
|
|
|
+
|
|
|
+ return true;
|
|
|
+}
|
|
|
+
|
|
|
+/**
|
|
|
+ * helper functions
|
|
|
+ */
|
|
|
+
|
|
|
+double mean(const std::vector< double > &values) {
|
|
|
+ if (values.size() == 0) {
|
|
|
+ return 0.0;
|
|
|
+ }
|
|
|
+
|
|
|
+ double sum = 0.0;
|
|
|
+ for (size_t k = 0; k < values.size(); k++) {
|
|
|
+ sum += values[k];
|
|
|
+ }
|
|
|
+
|
|
|
+ return sum / double(values.size());
|
|
|
+}
|
|
|
+
|
|
|
+/**
|
|
|
+ * tracklet handling
|
|
|
+ */
|
|
|
+
|
|
|
+// tracklet class
|
|
|
+class Tracklet {
|
|
|
+public:
|
|
|
+ // constructors
|
|
|
+ Tracklet() {
|
|
|
+ this->rawPoints.clear();
|
|
|
+ this->ids.clear();
|
|
|
+ }
|
|
|
+
|
|
|
+ Tracklet(const Tracklet &tracklet) {
|
|
|
+ this->rawPoints = tracklet.rawPoints;
|
|
|
+ this->ids = tracklet.ids;
|
|
|
+ }
|
|
|
+
|
|
|
+ Tracklet& operator=(const Tracklet &tracklet) {
|
|
|
+ this->rawPoints = tracklet.rawPoints;
|
|
|
+ this->ids = tracklet.ids;
|
|
|
+ return *this;
|
|
|
+ }
|
|
|
+
|
|
|
+ // getters/setters
|
|
|
+ bool isVirtual() const {
|
|
|
+ return this->rawPoints.empty();
|
|
|
+ }
|
|
|
+
|
|
|
+ std::string getIdString() const {
|
|
|
+ std::stringstream ss;
|
|
|
+ for (size_t k = 0; k < this->ids.size(); k++) {
|
|
|
+ if (k > 0) {
|
|
|
+ ss << ",";
|
|
|
+ }
|
|
|
+ ss << ids[k];
|
|
|
+ }
|
|
|
+ return ss.str();
|
|
|
+ }
|
|
|
+
|
|
|
+ std::vector< size_t > &getIds() {
|
|
|
+ return this->ids;
|
|
|
+ }
|
|
|
+
|
|
|
+ const std::vector< size_t > &getIds() const {
|
|
|
+ return this->ids;
|
|
|
+ }
|
|
|
+
|
|
|
+ void appendId(size_t _id) {
|
|
|
+ this->ids.push_back(_id);
|
|
|
+ }
|
|
|
+
|
|
|
+ void appendIds(std::vector< size_t > _ids) {
|
|
|
+ this->ids.insert(this->ids.end(), _ids.begin(), _ids.end());
|
|
|
+ }
|
|
|
+
|
|
|
+ size_t getFirstFrame() const {
|
|
|
+ //return this->firstFrame;
|
|
|
+ return this->rawPoints.begin()->first;
|
|
|
+ }
|
|
|
+
|
|
|
+ size_t getLastFrame() const {
|
|
|
+ return this->rawPoints.rbegin()->first;
|
|
|
+ }
|
|
|
+
|
|
|
+ size_t getRange() const {
|
|
|
+ return this->getLastFrame() - this->getFirstFrame() + 1;
|
|
|
+ }
|
|
|
+
|
|
|
+ const cv::Point3d &getFirstPoint() const {
|
|
|
+ return this->rawPoints.begin()->second;
|
|
|
+ }
|
|
|
+
|
|
|
+ const cv::Point3d &getLastPoint() const {
|
|
|
+ return this->rawPoints.rbegin()->second;
|
|
|
+ }
|
|
|
+
|
|
|
+ void printDebugInfo() const {
|
|
|
+ std::cout << "=== DEBUG ===" << std::endl;
|
|
|
+ std::cout << "IDs: " << this->getIdString() << std::endl;
|
|
|
+ std::cout << "frames: (" << this->getFirstFrame() << "," << this->getLastFrame() << ")" << std::endl;
|
|
|
+ std::cout << "raw point count: " << this->pRawPoints()->size() << std::endl;
|
|
|
+ std::cout << "first point: " << this->rawPoints.begin()->second << std::endl;
|
|
|
+ }
|
|
|
+
|
|
|
+ void interpolatePoints(void) {
|
|
|
+ for (std::map< size_t, cv::Point3d >::iterator it = this->rawPoints.begin();
|
|
|
+ it != this->rawPoints.end(); ++it) {
|
|
|
+ cv::Point3d p = it->second;
|
|
|
+
|
|
|
+// std::cout << it->first << p;
|
|
|
+
|
|
|
+ std::map< size_t, cv::Point3d >::iterator itNext = it;
|
|
|
+ std::advance(itNext, 1);
|
|
|
+
|
|
|
+ if (itNext != this->rawPoints.end()) {
|
|
|
+ size_t dT = itNext->first - it->first;
|
|
|
+
|
|
|
+ cv::Point3d q = itNext->second;
|
|
|
+ cv::Point3d v = (q - p) * (1.0 / (double)(dT));
|
|
|
+
|
|
|
+ if (dT > 0) {
|
|
|
+ for (size_t i = 1; i < dT; ++i) {
|
|
|
+// std::cout << " -> " << it->first + i << (p + v * (double)i);
|
|
|
+ this->rawPoints.insert(std::make_pair(it->first + i, p + v * (double)i));
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+ //std::cout << std::endl;
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ const double getMatchingScore(const Tracklet &tracklet) {
|
|
|
+ size_t tStart = std::max(this->getFirstFrame(), tracklet.getFirstFrame());
|
|
|
+ size_t tEnd = std::min(this->getLastFrame(), tracklet.getLastFrame());
|
|
|
+
|
|
|
+ int tDiff = tEnd - tStart;
|
|
|
+ double tRatio = 2.0 * (double)tDiff / (double)(this->pRawPoints()->size() + tracklet.pRawPoints()->size() - 2);
|
|
|
+
|
|
|
+ if (tRatio > 0.0) {
|
|
|
+ const std::map< size_t, cv::Point3d >* p = this->pRawPoints();
|
|
|
+ const std::map< size_t, cv::Point3d >* q = tracklet.pRawPoints();
|
|
|
+ double dist = 0.0f;
|
|
|
+ for (size_t i = tStart; i <= tEnd; ++i) {
|
|
|
+ dist += cv::norm(p->at(i) - q->at(i));
|
|
|
+ }
|
|
|
+ dist /= (double)tDiff;
|
|
|
+
|
|
|
+ return dist;
|
|
|
+ } else {
|
|
|
+ return std::numeric_limits< double >::max();
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ const std::map< size_t, cv::Point3d >* pRawPoints() const {
|
|
|
+ return &this->rawPoints;
|
|
|
+ }
|
|
|
+
|
|
|
+ void addPoint(size_t _frame, cv::Point3d &_point) {
|
|
|
+ this->rawPoints[_frame] = _point;
|
|
|
+ }
|
|
|
+
|
|
|
+ void append(Tracklet &tracklet) {
|
|
|
+ this->rawPoints.insert(tracklet.pRawPoints()->begin(), tracklet.pRawPoints()->end());
|
|
|
+ this->appendIds(tracklet.getIds());
|
|
|
+ }
|
|
|
+
|
|
|
+ void getVelocities(std::vector< double > &velocities) {
|
|
|
+ // clear object to be returned
|
|
|
+ velocities = std::vector< double >(0);
|
|
|
+
|
|
|
+ // need at least two points
|
|
|
+ if (this->rawPoints.size() <= 1) {
|
|
|
+ return;
|
|
|
+ }
|
|
|
+
|
|
|
+ // for consecutive points (in time), calculate magnitude of difference (a.k.a. velocity)
|
|
|
+ for (std::map< size_t, cv::Point3d >::iterator it = this->rawPoints.begin(); it != this->rawPoints.end(); it++) {
|
|
|
+ std::map< size_t, cv::Point3d >::iterator it2 = it;
|
|
|
+ it2++;
|
|
|
+ if (it2 != this->rawPoints.end()) {
|
|
|
+ double dx = it->second.x - it2->second.x;
|
|
|
+ double dy = it->second.y - it2->second.y;
|
|
|
+ double v = sqrt(dx * dx + dy * dy);
|
|
|
+ velocities.push_back(v);
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ // linking cost to another tracklet (assuming "this" comes before "target" in time)
|
|
|
+ double linkingCost(Tracklet &target, const Params ¶ms) {
|
|
|
+ // temporal distance (if "target" does not start after "this" ends, linking is not possible)
|
|
|
+ int dTemporal = int(target.getFirstFrame()) - this->getLastFrame();
|
|
|
+ if ((dTemporal <= 0) || (dTemporal > int(params.maxTemporalDist))) {
|
|
|
+ return std::numeric_limits< double >::infinity();
|
|
|
+ }
|
|
|
+
|
|
|
+ // distance between nearest end-points
|
|
|
+ double dx = target.getFirstPoint().x - this->getLastPoint().x;
|
|
|
+ double dy = target.getFirstPoint().y - this->getLastPoint().y;
|
|
|
+ double dSpatial = sqrt(dx * dx + dy * dy);
|
|
|
+ if (dSpatial > params.maxSpatialDist) {
|
|
|
+ return std::numeric_limits< double >::infinity();
|
|
|
+ }
|
|
|
+
|
|
|
+ // angular distance between nearest end-points
|
|
|
+ double dz = target.getFirstPoint().z - this->getLastPoint().z;
|
|
|
+ double dAngular = std::max(0.0, abs(dz) - 5.0);
|
|
|
+ if (dAngular > 25) {
|
|
|
+ return std::numeric_limits< double >::infinity();
|
|
|
+ }
|
|
|
+
|
|
|
+ // difference between mean velocities
|
|
|
+ std::vector< double > velocitiesThis, velocitiesTarget;
|
|
|
+ this->getVelocities(velocitiesThis);
|
|
|
+ target.getVelocities(velocitiesTarget);
|
|
|
+ double dMeanVelocities = std::abs(mean(velocitiesThis) - mean(velocitiesTarget));
|
|
|
+
|
|
|
+ //
|
|
|
+
|
|
|
+ // final cost
|
|
|
+ return 0.1 * double(dTemporal) * (dAngular + dSpatial + dMeanVelocities);
|
|
|
+ }
|
|
|
+
|
|
|
+protected:
|
|
|
+ std::map< size_t, cv::Point3d > rawPoints;
|
|
|
+ std::vector< size_t > ids;
|
|
|
+};
|
|
|
+
|
|
|
+void mergeDuplicates(std::vector< Tracklet > &tracklets, const double maxScore = 2.0) {
|
|
|
+ // iterate over all tracklets
|
|
|
+ for (std::vector< Tracklet >::iterator it1 = tracklets.begin(); it1 != tracklets.end(); ++it1) {
|
|
|
+
|
|
|
+ // get maximum time span
|
|
|
+ size_t tMin = it1->getFirstFrame();
|
|
|
+ size_t tMax = it1->getLastFrame();
|
|
|
+
|
|
|
+ // collect tracklets to be merged
|
|
|
+ std::vector< std::vector< Tracklet >::iterator > toBeMerged;
|
|
|
+
|
|
|
+ // first one by default
|
|
|
+ toBeMerged.push_back(it1);
|
|
|
+
|
|
|
+ // iterate over all succeeding tracklets
|
|
|
+ for (std::vector< Tracklet >::iterator it2 = it1 + 1; it2 != tracklets.end(); ++it2) {
|
|
|
+ double match = it1->getMatchingScore(*it2);
|
|
|
+ if (match < maxScore) {
|
|
|
+ /*
|
|
|
+ std::cout
|
|
|
+ << std::distance(tracklets.begin(), it1) << " " << it1->getIdString() << " -> "
|
|
|
+ << std::distance(tracklets.begin(), it2) << " " << it2->getIdString() << ": "
|
|
|
+ << match << std::endl;
|
|
|
+ */
|
|
|
+
|
|
|
+ // update time span
|
|
|
+ tMin = std::min(tMin, it2->getFirstFrame());
|
|
|
+ tMax = std::max(tMax, it2->getLastFrame());
|
|
|
+
|
|
|
+ // add to merge candidates
|
|
|
+ toBeMerged.push_back(it2);
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ if (toBeMerged.size() > 1) {
|
|
|
+ std::cout << "Merging " << toBeMerged.size() << " tracks!" << std::endl;
|
|
|
+ //std::cout << "tMin: " << tMin << ", tMax: " << tMax << std::endl;
|
|
|
+
|
|
|
+ // our new tracklet
|
|
|
+ Tracklet mergedTracklet;
|
|
|
+
|
|
|
+ // iterate over complete time span
|
|
|
+ for (size_t t = tMin; t <= tMax; ++t) {
|
|
|
+ cv::Point3d p;
|
|
|
+ size_t nP = 0;
|
|
|
+
|
|
|
+ // get point for current time step of each merging candidate
|
|
|
+ for (size_t i = 0; i < toBeMerged.size(); ++i) {
|
|
|
+ std::map< size_t, cv::Point3d >::const_iterator itT = toBeMerged[i]->pRawPoints()->find(t);
|
|
|
+ if (itT != toBeMerged[i]->pRawPoints()->end()) {
|
|
|
+ p = p + itT->second;
|
|
|
+ ++nP;
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ // result is the average of all candidate points
|
|
|
+ p *= 1.0 / (double)nP;
|
|
|
+
|
|
|
+ // add to new tracklet
|
|
|
+ mergedTracklet.addPoint(t, p);
|
|
|
+ }
|
|
|
+
|
|
|
+ // delete merged tracks
|
|
|
+ for (int i = toBeMerged.size() - 1; i >= 0; --i) {
|
|
|
+ mergedTracklet.appendIds(toBeMerged[i]->getIds());
|
|
|
+ if (i > 0) {
|
|
|
+ tracklets.erase(toBeMerged[i]);
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ // replace first tracklet by the merged one
|
|
|
+ *it1 = mergedTracklet;
|
|
|
+ }
|
|
|
+ }
|
|
|
+}
|
|
|
+
|
|
|
+void readTracklets(const std::string &filename,
|
|
|
+ std::vector< Tracklet > &tracklets,
|
|
|
+ size_t &nFrames) {
|
|
|
+ // open file
|
|
|
+ std::cout << "Reading tracklets from file " << filename << std::endl;
|
|
|
+ std::ifstream pointFile(filename.c_str(), std::ios_base::in);
|
|
|
+ if (!pointFile.is_open()) {
|
|
|
+ std::cerr << "Could not open file " << std::endl;
|
|
|
+ return;
|
|
|
+ }
|
|
|
+
|
|
|
+ // result object
|
|
|
+ tracklets.clear();
|
|
|
+
|
|
|
+ // read in all 3d points
|
|
|
+ std::string line;
|
|
|
+
|
|
|
+ // for each line...
|
|
|
+ nFrames = 0;
|
|
|
+ size_t id = 0;
|
|
|
+ while (std::getline(pointFile, line)) {
|
|
|
+ // print current frame number
|
|
|
+ //std::cerr << "\rReading points... " << std::flush;
|
|
|
+
|
|
|
+ // points of the current frame
|
|
|
+ std::vector< cv::Point3d > tracklet;
|
|
|
+
|
|
|
+ // read 3d points for each camera
|
|
|
+ Tracklet tmpTracklet;
|
|
|
+ tmpTracklet.appendId(id++);
|
|
|
+
|
|
|
+ std::stringstream split(line);
|
|
|
+ cv::Point3d tmpPoint;
|
|
|
+ size_t frame = 0;
|
|
|
+
|
|
|
+ // for each point...
|
|
|
+ while (split.good()) {
|
|
|
+ // read all points of this line
|
|
|
+ if ((split >> tmpPoint.x >> tmpPoint.y >> tmpPoint.z >> frame).good()) {
|
|
|
+ tmpTracklet.addPoint(frame, tmpPoint);
|
|
|
+
|
|
|
+ // update global frame count
|
|
|
+ nFrames = std::max(nFrames, tmpTracklet.getLastFrame() + 1);
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ // add tracklet to list
|
|
|
+ tmpTracklet.interpolatePoints();
|
|
|
+ tracklets.push_back(tmpTracklet);
|
|
|
+ }
|
|
|
+ uint nTracklets = tracklets.size();
|
|
|
+ std::cerr << std::endl << "Read " << nTracklets << " tracklets" << std::endl;
|
|
|
+
|
|
|
+ // merge duplicate tracklets
|
|
|
+ mergeDuplicates(tracklets);
|
|
|
+ nTracklets = tracklets.size();
|
|
|
+ std::cerr << std::endl << "Merged to " << nTracklets << " tracklets" << std::endl;
|
|
|
+}
|
|
|
+
|
|
|
+/**
|
|
|
+ * graph-based typedefs
|
|
|
+ */
|
|
|
+
|
|
|
+typedef double Weight;
|
|
|
+typedef boost::property< boost::edge_weight_t, Weight > WeightProperty;
|
|
|
+typedef boost::property< boost::vertex_name_t, Tracklet > NameProperty;
|
|
|
+
|
|
|
+typedef boost::adjacency_list<
|
|
|
+ boost::listS,
|
|
|
+ boost::vecS,
|
|
|
+ boost::directedS,
|
|
|
+ NameProperty,
|
|
|
+ WeightProperty > Graph;
|
|
|
+
|
|
|
+typedef boost::graph_traits< Graph >::vertex_descriptor Vertex;
|
|
|
+
|
|
|
+typedef boost::property_map< Graph, boost::vertex_index_t >::type IndexMap;
|
|
|
+typedef boost::property_map< Graph, boost::vertex_name_t >::type NameMap;
|
|
|
+
|
|
|
+typedef boost::iterator_property_map< Vertex*, IndexMap, Vertex, Vertex& > PredecessorMap;
|
|
|
+typedef boost::iterator_property_map< Weight*, IndexMap, Weight, Weight& > DistanceMap;
|
|
|
+
|
|
|
+//
|
|
|
+template< class G >
|
|
|
+class VertexWriter {
|
|
|
+public:
|
|
|
+ VertexWriter(G _g) :
|
|
|
+ g(_g) {
|
|
|
+ this->nameMap = boost::get(boost::vertex_name, g);
|
|
|
+ }
|
|
|
+
|
|
|
+ template< class VertexOrEdge >
|
|
|
+ void operator()(std::ostream& out, const VertexOrEdge& v) const {
|
|
|
+ if (nameMap[v].isVirtual()) {
|
|
|
+ out << "[label=\"" << nameMap[v].getIdString() << "\",shape=\"circle\"]";
|
|
|
+ } else {
|
|
|
+ out << "[label=\"" << nameMap[v].getIdString() << ":\\n(" << nameMap[v].getFirstFrame() << ","
|
|
|
+ << nameMap[v].getLastFrame() << ")\",shape=\"box\"]";
|
|
|
+ }
|
|
|
+ //
|
|
|
+ }
|
|
|
+private:
|
|
|
+ G g;
|
|
|
+ NameMap nameMap;
|
|
|
+};
|
|
|
+
|
|
|
+//
|
|
|
+template< class G >
|
|
|
+class EdgeWriter {
|
|
|
+public:
|
|
|
+ EdgeWriter(G _g) :
|
|
|
+ g(_g) {
|
|
|
+ }
|
|
|
+ template< class VertexOrEdge >
|
|
|
+ void operator()(std::ostream& out, const VertexOrEdge& v) const {
|
|
|
+ out << "[label=\"" << boost::get(boost::edge_weight, g, v) << "\"]";
|
|
|
+ }
|
|
|
+private:
|
|
|
+ G g;
|
|
|
+};
|
|
|
+
|
|
|
+// tracklet graph to dot file
|
|
|
+void trackletsToDot(std::string filename, const Graph &g) {
|
|
|
+ std::cout << "Writing graph to file \"" << filename << "\"" << std::endl;
|
|
|
+ std::ofstream graphFile(filename.c_str());
|
|
|
+ if (!graphFile.is_open()) {
|
|
|
+ std::cerr << "Could not open file \"" << filename << "\"" << std::endl;
|
|
|
+ return;
|
|
|
+ }
|
|
|
+
|
|
|
+ VertexWriter< Graph > vertexWriter(g);
|
|
|
+ EdgeWriter< Graph > edgeWriter(g);
|
|
|
+
|
|
|
+ boost::write_graphviz(graphFile, g, vertexWriter, edgeWriter);
|
|
|
+
|
|
|
+ graphFile.close();
|
|
|
+}
|
|
|
+
|
|
|
+/**
|
|
|
+ * main tracking functions
|
|
|
+ */
|
|
|
+
|
|
|
+// nFrames: global frame count (needed to calculated weights to vSink)
|
|
|
+// return tracks
|
|
|
+void createTracksFromTracklets(const std::vector< Tracklet > &tracklets,
|
|
|
+ const Params ¶ms,
|
|
|
+ const size_t nFrames,
|
|
|
+ std::vector< Tracklet > &tracks) {
|
|
|
+ //
|
|
|
+ // build graph
|
|
|
+ //
|
|
|
+
|
|
|
+ Graph g;
|
|
|
+ std::vector< Vertex > vertices;
|
|
|
+
|
|
|
+ size_t trackletStart = 0;
|
|
|
+ size_t trackletMax = tracklets.size();
|
|
|
+
|
|
|
+ // adding vertices
|
|
|
+ for (size_t iTracklet = trackletStart; iTracklet < trackletMax; ++iTracklet) {
|
|
|
+ std::cout << "\rAdding node for tracklet " << iTracklet + 1 << std::flush;
|
|
|
+ Vertex v = boost::add_vertex(tracklets[iTracklet], g);
|
|
|
+ vertices.push_back(v);
|
|
|
+ }
|
|
|
+
|
|
|
+ // virtual source and sink
|
|
|
+ Vertex vSource = boost::add_vertex(Tracklet(), g);
|
|
|
+ Vertex vSink = boost::add_vertex(Tracklet(), g);
|
|
|
+
|
|
|
+ // debug
|
|
|
+ ///boost::add_edge(vSource, vSink, 0, g);
|
|
|
+
|
|
|
+ NameMap nameMap = boost::get(boost::vertex_name, g);
|
|
|
+
|
|
|
+ std::cout << std::endl;
|
|
|
+
|
|
|
+ // adding edges (layer eq tracklets)
|
|
|
+ for (size_t iTracklet = 0; iTracklet < vertices.size(); ++iTracklet) {
|
|
|
+ // all points of frame "iLayers"
|
|
|
+ Vertex tracklet1 = vertices[iTracklet];
|
|
|
+
|
|
|
+ // create edges to source and sink
|
|
|
+ std::cout << "\rAdding edges vSource -> " << iTracklet + 1 << " -> vSink" << std::flush;
|
|
|
+ boost::add_edge(vSource, tracklet1, (nameMap[tracklet1].getFirstFrame() + 1) * 1, g);
|
|
|
+ boost::add_edge(tracklet1, vSink, (nFrames - nameMap[tracklet1].getLastFrame()) * 1, g);
|
|
|
+
|
|
|
+ for (size_t jTracklet = 0; jTracklet < vertices.size(); ++jTracklet) {
|
|
|
+ // get second tracklet for linking
|
|
|
+ Vertex tracklet2 = vertices[jTracklet];
|
|
|
+
|
|
|
+ // this is where the magic happens... the cost function!
|
|
|
+ double ijCost = nameMap[tracklet1].linkingCost(nameMap[tracklet2], params);
|
|
|
+
|
|
|
+ // create edge between both nodes
|
|
|
+ if (ijCost < std::numeric_limits< double >::infinity()) {
|
|
|
+ boost::add_edge(tracklet1, tracklet2, ijCost, g);
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+ std::cout << std::endl;
|
|
|
+
|
|
|
+ //
|
|
|
+ // find tracks (shortest paths in tracklet graph)
|
|
|
+ //
|
|
|
+
|
|
|
+ // Create things for Dijkstra
|
|
|
+ std::vector< Vertex > predecessors(boost::num_vertices(g)); // To store parents
|
|
|
+ std::vector< Weight > distances(boost::num_vertices(g)); // To store distances
|
|
|
+
|
|
|
+ IndexMap indexMap = boost::get(boost::vertex_index, g);
|
|
|
+ PredecessorMap predecessorMap(&predecessors[0], indexMap);
|
|
|
+ DistanceMap distanceMap(&distances[0], indexMap);
|
|
|
+
|
|
|
+ // export graph to graphviz dot file
|
|
|
+ //trackletsToDot("tmp2.dot", g);
|
|
|
+
|
|
|
+ // each track consists of merged tracklets (which in turn is again a tracklet)
|
|
|
+ tracks.clear();
|
|
|
+
|
|
|
+ // Compute shortest paths from input layer vertices to the sink
|
|
|
+ for (size_t tracklet = 0; tracklet < params.trackCount; ++tracklet) {
|
|
|
+ // preparations for Dijkstra
|
|
|
+ boost::dijkstra_shortest_paths(g, // the graph
|
|
|
+ vSource, // the source vertex
|
|
|
+ boost::distance_map(distanceMap).predecessor_map(predecessorMap));
|
|
|
+
|
|
|
+ // stop if no more tracks can be found
|
|
|
+ if (distanceMap[vSink] == std::numeric_limits<double>::max()) {
|
|
|
+ std::cout << "No more tracks can be found" << std::endl;
|
|
|
+ break;
|
|
|
+ }
|
|
|
+
|
|
|
+ // Extract the shortest path
|
|
|
+ typedef std::vector< Graph::edge_descriptor > PathType;
|
|
|
+
|
|
|
+ PathType path;
|
|
|
+
|
|
|
+ Vertex v = vSink; // We want to start at the sink and work our way back to the source
|
|
|
+ for (Vertex u = predecessorMap[v]; u != v; v = u, u = predecessorMap[v]) {
|
|
|
+ std::pair< Graph::edge_descriptor, bool > edgePair = boost::edge(u, v, g);
|
|
|
+ Graph::edge_descriptor edge = edgePair.first;
|
|
|
+ path.push_back(edge);
|
|
|
+ }
|
|
|
+
|
|
|
+ // traverse shortest path (actually: traverse reverse path in a reverse direction)
|
|
|
+ Tracklet currentTrack;
|
|
|
+ bool vFirst = true;
|
|
|
+ for (PathType::reverse_iterator pathIterator = path.rbegin(); pathIterator != path.rend(); ++pathIterator) {
|
|
|
+ // append non-virtual tracklets to this path
|
|
|
+ if (!nameMap[boost::source(*pathIterator, g)].isVirtual()) {
|
|
|
+ currentTrack.append(nameMap[boost::source(*pathIterator, g)]);
|
|
|
+ }
|
|
|
+
|
|
|
+ // for each node of the path (but not the first, i.e. vSource), set the weights of all outgoing edges to Inf (for Dijkstra equivalent to deletion of the node)
|
|
|
+ if (!vFirst) {
|
|
|
+ std::pair< Graph::out_edge_iterator, Graph::out_edge_iterator > edgeIts = boost::out_edges(boost::source(*pathIterator,
|
|
|
+ g),
|
|
|
+ g);
|
|
|
+ for (Graph::out_edge_iterator edgeIt = edgeIts.first; edgeIt != edgeIts.second; edgeIt++) {
|
|
|
+ boost::get(boost::edge_weight, g, *edgeIt) = std::numeric_limits< Weight >::infinity();
|
|
|
+ }
|
|
|
+ }
|
|
|
+ vFirst = false;
|
|
|
+ }
|
|
|
+
|
|
|
+ // interpolate
|
|
|
+ currentTrack.interpolatePoints();
|
|
|
+
|
|
|
+ tracks.push_back(currentTrack);
|
|
|
+
|
|
|
+ std::cout << "Found track #" << tracklet + 1 << "/" << params.trackCount << ": " << path.size() - 1 << " tracklet(s), cost: "
|
|
|
+ << distanceMap[vSink] << std::endl;
|
|
|
+ }
|
|
|
+
|
|
|
+ // merge duplicate tracks
|
|
|
+ mergeDuplicates(tracks);
|
|
|
+ std::cerr << std::endl << "Merged to " << tracks.size() << " tracks" << std::endl;
|
|
|
+}
|
|
|
+
|
|
|
+void writeTracks(std::string filename, std::vector< Tracklet > &tracks, size_t nFrames, double minTrackLength) {
|
|
|
+ // open file
|
|
|
+ std::ofstream f(filename.c_str());
|
|
|
+
|
|
|
+ // get minimum track length
|
|
|
+ size_t minLength;
|
|
|
+ if (minTrackLength <= 1.0) {
|
|
|
+ minLength = std::min(size_t(minTrackLength * double(nFrames)), nFrames);
|
|
|
+ } else {
|
|
|
+ minLength = std::min(size_t(minTrackLength), nFrames);
|
|
|
+ }
|
|
|
+
|
|
|
+ // print each track
|
|
|
+ size_t finalCount = 0;
|
|
|
+ for (size_t track = 0; track < tracks.size(); ++track) {
|
|
|
+ if (tracks[track].getRange() >= minLength) {
|
|
|
+ const std::map< size_t, cv::Point3d > *p = tracks[track].pRawPoints();
|
|
|
+ for (std::map< size_t, cv::Point3d >::const_iterator it = p->begin(); it != p->end(); it++) {
|
|
|
+ f << it->second.x << " " << it->second.y << " " << it->second.z << " " << it->first << " ";
|
|
|
+ }
|
|
|
+ //f << tracks[track].getIdString()
|
|
|
+ f << std::endl;
|
|
|
+ finalCount++;
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ f.close();
|
|
|
+
|
|
|
+ std::cout << finalCount << " tracks with length >= " << minLength << " were found" << std::endl;
|
|
|
+}
|
|
|
+
|
|
|
+int main(int argc, char** argv) {
|
|
|
+ // get command line parameters
|
|
|
+ Params p;
|
|
|
+ if (!evalCmdLine(argc, argv, p)) {
|
|
|
+ std::cerr << "Error while parsing arguments!" << std::endl;
|
|
|
+ return 1;
|
|
|
+ }
|
|
|
+
|
|
|
+ size_t nFrames;
|
|
|
+ std::vector< Tracklet > tracklets, tracks;
|
|
|
+ readTracklets(p.strTracklets, tracklets, nFrames);
|
|
|
+ createTracksFromTracklets(tracklets, p, nFrames, tracks);
|
|
|
+ writeTracks(p.outputFilename, tracks, nFrames, p.minTrackLength);
|
|
|
+
|
|
|
+ return EXIT_SUCCESS;
|
|
|
+}
|
|
|
+
|
Helge Wrede
%!s(int64=9) %!d(string=hai) anos