Method getMahalanobisDistance

tracking/src/track.cpp:324–387

    }

    double
    Track::getMahalanobisDistance(double x, double y, const ros::Time& when)
    {
      int difference = int(round((when - last_time_predicted_).toSec() / period_));
//      std::cout << "time difference from last detection: " << difference << std::endl;
      int index;
      if(difference <= 0)
      {
        index = (MAX_SIZE + last_time_predicted_index_ + difference) % MAX_SIZE;
      }
      else
      {
        for(int i = 0; i < difference; i++)
        {
          tmp_filter_->predict();
          last_time_predicted_index_ = (last_time_predicted_index_ + 1) % MAX_SIZE;
          if (velocity_in_motion_term_)
            tmp_filter_->getMahalanobisParameters(mahalanobis_map4d_[last_time_predicted_index_]);
          else
            tmp_filter_->getMahalanobisParameters(mahalanobis_map2d_[last_time_predicted_index_]);
          tmp_filter_->update();
        }
        last_time_predicted_ = when;
        index = last_time_predicted_index_;
      }

      if (velocity_in_motion_term_)
      {
        ros::Duration d(1.0);
        ros::Duration d2(2.0);

        double t = std::max(first_time_detected_.toSec(), (when - d).toSec());
        t = std::min(t, last_time_detected_.toSec());
        t = std::max(t, (last_time_predicted_ - d2).toSec());
        double dt = t - last_time_predicted_.toSec();

        difference = int(round(dt / period_));
        int vIndex = (MAX_SIZE + last_time_predicted_index_ + difference) % MAX_SIZE;

//        std::cout << "dt: " << dt << std::endl;
//        std::cout << "vIndex: " << vIndex << std::endl;

        double vx, vy;
        if(difference != 0)
        {
          vx = - (x - mahalanobis_map4d_[vIndex].x) / dt;
          vy = - (y - mahalanobis_map4d_[vIndex].y) / dt;
        }
        else
        {
          vx = mahalanobis_map4d_[vIndex].x;
          vy = mahalanobis_map4d_[vIndex].y;
        }

//        std::cout << "vx: " << vx << ", vy: " << vy<< std::endl;

        return open_ptrack::tracking::KalmanFilter::performMahalanobisDistance(x, y, vx, vy, mahalanobis_map4d_[index]);
      }