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