Method getMahalanobisDistance

tracking/src/track3d.cpp:328–395

}

double
Track3D::getMahalanobisDistance(double x, double y, double z, 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_map6d_[last_time_predicted_index_]);
      else
        tmp_filter_->getMahalanobisParameters(mahalanobis_map3d_[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, vz;
    if(difference != 0)
    {
      vx = - (x - mahalanobis_map6d_[vIndex].x) / dt;
      vy = - (y - mahalanobis_map6d_[vIndex].y) / dt;
      vz = - (z - mahalanobis_map6d_[vIndex].z) / dt;
    }
    else
    {
      vx = mahalanobis_map6d_[vIndex].x;
      vy = mahalanobis_map6d_[vIndex].y;
      vz = mahalanobis_map6d_[vIndex].z;
    }

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