//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;
}
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));
}