Exemple #1
0
//Update the position of the person when info from the detection is available
void Person::update(tf::Point pos, Time time, bool img_detection)
{
	double dt = time.sec - last_update_time_.sec;
	kf_->predict(dt);
	kf_->update(pos.x(), pos.y(), time);	
	last_update_time_ = time;

	life_time_ = (img_detection) ? 5 : 3;
	return;
}
Exemple #2
0
Person::Person(tf::Point pos, Time time) : id_(++next_id), life_time_(3)
{
	
	kf_ = new PersonKF;

	kf_->init(pos.x(), pos.y(), 0.0, 0.0, time);
	last_update_time_ = time;

	return;
}
void ObjectOfInterestManager::weightedAverage(const OoI_weight_pair& existing, const tf::Point& addition, OoI_weight_pair& result) const
{
    double sum = existing.first + 1.0;
    double ewx = existing.second.getX()*existing.first;
    double ewy = existing.second.getY()*existing.first;
    double ewz = existing.second.getZ()*existing.first;
    result.first++;
    result.second.setX((ewx+addition.getX())/sum);
    result.second.setY((ewy+addition.getY())/sum);
    result.second.setZ((ewz+addition.getZ())/sum);
}
void Person::correct(tf::Point pos, Time time)
{
	updateTime(time,last_correct_time_);
	last_correct_time_ = time;
	last_predict_time_ = time;

	//Prediction is run before correction 
	kf_->predict();
	//This prevents the filter from getting stuck in a state when no corrections are executed
	kf_->statePre.copyTo(kf_->statePost);
	kf_->errorCovPre.copyTo(kf_->errorCovPost);

	setData(kf_->correct((Mat_<float>(2,1) << pos.x(), pos.y())), time);
	return;	
}
/**
 * Randomly chooses vectors, gets the Information Gain for each of 
 *  those vectors, and returns the ray (start and end) with the highest information gain
 */
void randomSelection(PlotRayUtils &plt, RayTracer &rayt, tf::Point &best_start, tf::Point &best_end)
{
  // tf::Point best_start, best_end;

  double bestIG;
  bestIG = 0;
  std::random_device rd;
  std::uniform_real_distribution<double> rand(-1.0,1.0);


  for(int i=0; i<500; i++){
    tf::Point start(0.5, 0.5, rand(rd));
    // start = start.normalize();
    tf::Point end(start.getX(), start.getY(), rand(rd));
    // end.normalized();
    Ray measurement(start, end);
    // auto timer_begin = std::chrono::high_resolution_clock::now();
    double IG = rayt.getIG(measurement, 0.01, 0.002);
    // auto timer_end = std::chrono::high_resolution_clock::now();
    // auto timer_dur = timer_end - timer_begin;
    // cout << "IG: " << IG << endl;
    // cout << "Elapsed time for ray: " << std::chrono::duration_cast<std::chrono::milliseconds>(timer_dur).count() << endl;
    // double IG = plt.getIG(start, end, 0.01, 0.002);
    if (IG > bestIG){
      bestIG = IG;
      best_start = start;
      best_end = end;
    }
  }
  //Ray measurement(best_start, best_end);
  // plt.plotCylinder(best_start, best_end, 0.01, 0.002, true);
  ROS_INFO("Ray is: %f, %f, %f.  %f, %f, %f", 
	   best_start.getX(), best_start.getY(), best_start.getZ(),
	   best_end.getX(), best_end.getY(), best_end.getZ());
  
}
void point32MsgToTF(const geometry_msgs::Point32& from, tf::Point& to)
{
  to.setX(from.x);
  to.setY(from.y);
  to.setZ(from.z);
}
bool ObjectOfInterestManager::sameLocation(const tf::Point& a, const tf::Point& b) const
{
    return a.distance(b) <= this->dist_thresh_;
}
void fixedSelection(PlotRayUtils &plt, RayTracer &rayt, tf::Point &best_start, tf::Point &best_end)
{
  int index;
  double bestIG = 0;
  tf::Point tf_start;
  tf::Point tf_end;
  bestIG = 0;
  std::random_device rd;
  std::uniform_real_distribution<double> rand(0, 1);
  std::uniform_int_distribution<> int_rand(0, 3);
  Eigen::Vector3d start;
  Eigen::Vector3d end;
  double state[6] = {0.3, 0.3, 0.3, 0.5, 0.7, 0.5};
  Eigen::Matrix3d rotationC;
  rotationC << cos(state[5]), -sin(state[5]), 0,
               sin(state[5]), cos(state[5]), 0,
               0, 0, 1;
  Eigen::Matrix3d rotationB;
  rotationB << cos(state[4]), 0 , sin(state[4]),
               0, 1, 0,
               -sin(state[4]), 0, cos(state[4]);
  Eigen::Matrix3d rotationA;
  rotationA << 1, 0, 0 ,
               0, cos(state[3]), -sin(state[3]),
               0, sin(state[3]), cos(state[3]);
  Eigen::Matrix3d rotationM = rotationC * rotationB * rotationA;
  Eigen::Vector3d displaceV(state[0], state[1], state[2]);
  for(int i=0; i<500; i++){
    index = int_rand(rd);
    if (index == 0)
    {
      double y = rand(rd) * 0.31 - 0.35;
      double z = rand(rd) * 0.18 + 0.03;
      start << 2, y, z;
      end << -1, y, z;

    }
    else if (index == 1)
    {
      double x = rand(rd) * 1.1 + 0.1;
      double z = rand(rd) * 0.18 + 0.03;
      start << x, -1, z;
      end << x, 1, z;
    }
    else if (index == 2)
    {
      double x = rand(rd) * 1.1 + 0.1;
      double y = rand(rd) * 0.01 - 0.02;
      start << x, y, 1;
      end << x, y, -1;
    }
    else
    {
      double x = rand(rd) * 0.02 + 0.33;
      double y = rand(rd) * 0.2 - 0.35;
      start << x, y, 1;
      end << x, y, -1;
    }
    
    Eigen::Vector3d tran_start = rotationM * start + displaceV;
    Eigen::Vector3d tran_end = rotationM * end + displaceV;

    tf_start.setValue(tran_start(0, 0), tran_start(1, 0), tran_start(2, 0));
    tf_end.setValue(tran_end(0, 0), tran_end(1, 0), tran_end(2, 0));
    Ray measurement(tf_start, tf_end);
    // auto timer_begin = std::chrono::high_resolution_clock::now();
    double IG = rayt.getIG(measurement, 0.01, 0.002);
    // plt.plotRay(measurement);
    // plt.labelRay(measurement, IG);
    // auto timer_end = std::chrono::high_resolution_clock::now();
    // auto timer_dur = timer_end - timer_begin;
    // cout << "IG: " << IG << endl;
    // cout << "Elapsed time for ray: " << std::chrono::duration_cast<std::chrono::milliseconds>(timer_dur).count() << endl;
    // double IG = plt.getIG(start, end, 0.01, 0.002);
    if (IG > bestIG){
      bestIG = IG;
      best_start = tf_start;
      best_end = tf_end;
    }
  }
  // plt.plotCylinder(best_start, best_end, 0.01, 0.002, true);
  ROS_INFO("Ray is: %f, %f, %f.  %f, %f, %f", 
     best_start.getX(), best_start.getY(), best_start.getZ(),
     best_end.getX(), best_end.getY(), best_end.getZ());
  plt.plotRay(Ray(best_start, best_end));
}