Exemple #1
0
 void
 Tracker::fillUnassociatedDetections()
 {
   // Fill a list with detections not associated to any track:
   if(cost_matrix_.cols == 0 && detections_.size() > 0)
   {
     for(size_t measure = 0; measure < detections_.size(); measure++)
       unassociated_detections_.push_back(detections_[measure]);
   }
   else
   {
     for(int measure = 0; measure < cost_matrix_.cols; measure++)
     {
       bool associated = false;
       for(int track = 0; track < cost_matrix_.rows; track++)
       {
         if(cost_matrix_(track, measure) == 0.0)
         {
           if(distance_matrix_(track, measure) > gate_distance_)
             break;
           associated = true;
         }
       }
       if(!associated/* && detections_[measure].getConfidence() > min_confidence_*/)
       {
         unassociated_detections_.push_back(detections_[measure]);
       }
     }
   }
 }
Exemple #2
0
    void
    Tracker::createCostMatrix()
    {
      cost_matrix_ = distance_matrix_.clone();
      for(int i = 0; i < distance_matrix_.rows; i++)
      {
        for(int j = 0; j < distance_matrix_.cols; j++)
        {
          if(distance_matrix_(i, j) > gate_distance_)
            cost_matrix_(i, j) = 1000000.0;
        }
      }


//      	std::cout << "Munkres input matrix:" << std::endl;
//      	for(int row = 0; row < cost_matrix_.rows; row++)
//      	{
//      		for(int col = 0; col < cost_matrix_.cols; col++)
//      		{
//      			std::cout.width(8);
//      			std::cout << cost_matrix_(row,col) << ",";
//      		}
//      		std::cout << std::endl;
//      	}
//      	std::cout << std::endl;
    }
Exemple #3
0
    void
    Tracker::updateDetectedTracks()
    {
//      	std::cout << "Munkres output matrix:" << std::endl;
//      	for(int row = 0; row < cost_matrix_.rows; row++) {
//      		for(int col = 0; col < cost_matrix_.cols; col++) {
//      			std::cout.width(1);
//      			std::cout << cost_matrix_(row,col) << ",";
//      		}
//      		std::cout << std::endl;
//      	}
//      	std::cout << std::endl;

      // Iterate over every track:
      int track = 0;
      for(std::list<open_ptrack::tracking::Track*>::iterator it = tracks_.begin(); it != tracks_.end(); it++)
      {
        bool updated = false;
        open_ptrack::tracking::Track* t = *it;

        for(int measure = 0; measure < cost_matrix_.cols; measure++)
        {
          // If a detection<->track association has been found:
          if(cost_matrix_(track, measure) == 0.0 && distance_matrix_(track, measure) <= gate_distance_)
          {
            open_ptrack::detection::Detection& d = detections_[measure];

            // If the detection has enough confidence in the current frame or in a recent past:
//            if ((t->getLowConfidenceConsecutiveFrames() < 10) || ((d.getConfidence() - 0.5) > min_confidence_detections_))
            if ((t->getLowConfidenceConsecutiveFrames() < 10) || (d.getConfidence() > ((min_confidence_ + min_confidence_detections_)/2)))
            {
              // Update track with the associated detection:
              bool first_update = false;
              t->update(d.getWorldCentroid()(0), d.getWorldCentroid()(1), d.getWorldCentroid()(2),d.getHeight(),
                  d.getDistance(), //distance_matrix_(track, measure),
                  d.getConfidence(), min_confidence_, min_confidence_detections_,
                  d.getSource(), first_update);

              t->setVisibility(d.isOccluded() ? Track::OCCLUDED : Track::VISIBLE);
              updated = true;
              break;
            }
            else
            {
              std::cout << "Id: " << t->getId() << ", lowConfConsFrames: " << t->getLowConfidenceConsecutiveFrames() << ", newConf: " << d.getConfidence()<< std::endl;
            }
          }
        }
        if(!updated)
        {
          if(t->getVisibility() != Track::NOT_VISIBLE)
          {
            t->setVisibility(Track::NOT_VISIBLE);
            //t->update();
          }
        }
        track++;
      }
      //	std::cout << std::endl;
    }
Exemple #4
0
    void
    Tracker::createDistanceMatrix()
    {
      distance_matrix_ = cv::Mat_<double>(tracks_.size(), detections_.size());
      int track = 0;
      for(std::list<Track*>::iterator it = tracks_.begin(); it != tracks_.end(); it++)
      {
        //double x, y, height, vx, vz;
        Track* t = *it;
        //t->predict(x, y, height, vx, vz);
        int measure = 0;
        for(std::vector<open_ptrack::detection::Detection>::iterator dit = detections_.begin(); dit != detections_.end(); dit++)
        {
          double detector_likelihood;

          // Compute detector likelihood:
          if (detector_likelihood_)
          {
            detector_likelihood = dit->getConfidence();
            //				detector_likelihood = log((dit->getConfidence() + 3) / 6);
          }
          else
          {
            detector_likelihood = 0;
          }

          // Compute motion likelihood:
          double motion_likelihood = t->getMahalanobisDistance(
              dit->getWorldCentroid()(0),
              dit->getWorldCentroid()(1),
              dit->getSource()->getTime());

          // Compute joint likelihood and put it in the distance matrix:

          distance_matrix_(track, measure++) = likelihood_weights_[0] * detector_likelihood + likelihood_weights_[1] * motion_likelihood;

          //std::cout << (*it)->getId() << ": " << "Motion likelihood: " << motionWeight * motionLikelihood << std::endl;
          //if (detector_likelihood_)
          //	std::cout << (*it)->getId() << ": " << "Detector likelihood: " << detectorWeight * dit->getConfidence() << std::endl;
          //std::cout << (*it)->getId() << ": " << "JOINT LIKELIHOOD: " << distance_matrix_(track, measure-1) << std::endl;

          /*ROS_INFO("%d(%f, %f) = %f", t->getId(),
					dit->getWorldCentroid()(0),
					dit->getWorldCentroid()(1),
					distance_matrix_(track, measure - 1));*/
        }
        track++;
      }

//      	std::cout << "Distance matrix:" << std::endl;
//      	for(int row = 0; row < distance_matrix_.rows; row++)
//      	{
//      		for(int col = 0; col < distance_matrix_.cols; col++)
//      		{
//      			std::cout.width(8);
//      			std::cout << distance_matrix_(row,col) << ",";
//      		}
//      		std::cout << std::endl;
//      	}
//      	std::cout << std::endl;
    }