void CLegGrid::spin()
{
if(!grid_->polar_array.predicted.empty()) grid_->polar_array.predicted.clear();
ROS_INFO_COND(DEBUG,"--- spin ---");
makeStates();
updateKF();
grid_->updateGrid(1);
publishPredictedLegs();
publishProbability();
publishOccupancyGrid();
grid_->projectGrid();
publishProjection();
}
void CHumanGrid::integrateProbabilities()
{
ros::Time now = ros::Time::now();
// float maxw = std::max(std::max(leg_max_, sound_max_), torso_max_);
// lw_ = (leg_max_ > 0.0) ? (maxw / leg_max_) : 1.0;
// sw_ = (sound_max_ > 0.0) ? (maxw / sound_max_) : 1.0;
// tw_ = (torso_max_ > 0.0) ? (maxw / torso_max_) : 1.0;
lw_ = sw_ = tw_ = 1.0; //TODO: REMOVE THESE PARAMS
float num = 0.0;
float denum = (lw_ * leg_weight_) + (sw_ * sound_weight_) + (tw_ * torso_weight_); //
std::vector<float> temp;
temp.resize(prob_.poses.size());
float max = -1000;
for (size_t i = 0; i < grid_->grid_size; i++)
{
num = lw_ * leg_weight_ * leg_prob_.poses.at(i).position.z +
sw_ * sound_weight_ * sound_prob_.poses.at(i).position.z +
tw_ * torso_weight_ * torso_prob_.poses.at(i).position.z;
prob_.poses.at(i).position.z = num / denum;
temp.at(i) = grid_->posterior.at(i) = num / denum;
max = std::max(max, temp.at(i));
}
for (size_t i = 0; i < grid_->grid_size; i++)
{
occupancy_grid_.data.at(i) = (int) 100 * (temp.at(i)) / max;
}
occupancy_grid_.header.stamp = now;
human_grid_pub_.publish(occupancy_grid_);
ROS_INFO_COND(DEBUG, "max probability: %.4f ", max);
// Using Local Maxima
// grid_->updateLocalMaximas();
// hp_.point = grid_->highest_prob_point.point;
//Use highest Point
std::vector<float>::iterator it = std::max_element(temp.begin(), temp.end());
hp_.point = prob_.poses.at(it - temp.begin()).position;
transitState();
last_time_ = now;
hp_.header.stamp = now;
highest_point_pub_.publish(hp_);
printFusedFeatures();
publishLocalMaxima();
grid_->projectGrid();
publishProjection();
}
