// this function is called each frame
void glutDisplay (void)
{
static ros::Duration pub_interval(1.0/pub_rate_temp_);
static ros::Time last_pub(0.0);
static int num_skipped = 0;
num_skipped++;
ros::Time now_time = ros::Time::now();
// Update stuff from OpenNI
XnStatus status = g_Context.WaitAndUpdateAll();
if( status != XN_STATUS_OK ){
ROS_ERROR_STREAM("Updating context failed: " << status);
return;
}
xn::SceneMetaData sceneMD;
xn::DepthMetaData depthMD;
xn::ImageMetaData imageMD;
g_DepthGenerator.GetMetaData(depthMD);
g_UserGenerator.GetUserPixels(0, sceneMD);
//g_ImageGenerator.GetMetaData(imageMD);
cv::Mat depth_image;
getDepthImage(depthMD, depth_image);
double minval, maxval;
cv::minMaxLoc(depth_image, &minval, &maxval);
// Convert user pixels to an OpenCV image
cv::Mat label_image;
getUserLabelImage(sceneMD, label_image);
sensor_msgs::PointCloud cloud;
cloud.header.stamp = now_time;
cloud.header.frame_id = frame_id_;
cloud.channels.resize(1);
cloud.channels[0].name = "intensity";
// Convert users into better format
XnUserID aUsers[15];
XnUInt16 nUsers = 15;
g_UserGenerator.GetUsers(aUsers, nUsers);
cv::Mat this_mask;
XnPoint3D center_mass;
double pixel_area;
cv::Scalar s;
vector<user> users;
if( g_bhasCal && now_time-last_pub > pub_interval )
{
bool has_lock = false;
last_pub = now_time;
ROS_DEBUG_STREAM(num_skipped << " refreshes inbetween publishing");
num_skipped = 0;
cv::imshow( "Tracked user" , user_cal_.getImage() );
cv::imshow( "Original calibration", original_cal_.getImage() );
cv::Mat rgb, rgb_mask;
// getRGB(rgb, rgb_mask);
rgb = getRGB(imageMD);
for (unsigned int i = 0; i < nUsers; i++)
{
user this_user;
this_user.uid = aUsers[i];
// Bitwise mask of pixels belonging to this user
this_mask = (label_image == this_user.uid);
this_user.numpixels = cv::countNonZero(this_mask);
// Compare this user to the target
this_user.pc.init(rgb, this_mask);
double similarity = this_user.pc.compare(user_cal_ );
double sim_to_orig = this_user.pc.compare(original_cal_);
this_user.similarity = similarity;
this_user.similarity_to_orig = sim_to_orig;
/*
ros::Time t1 = ros::Time::now();
double emd = this_user.pc.getEMD (user_cal_ );
double emd_to_orig = this_user.pc.getEMD (original_cal_);
ros::Duration d = (ros::Time::now() - t1);
ROS_INFO_STREAM("EMD took " << (d.sec) );*/
if( now_time > save_timer_ ){
ROS_WARN_STREAM("[bk_skeletal_tracker] Say Cheezbuger");
save_timer_ = now_time + ros::Duration(60*60*24);
g_bSaveFrame = true;
}
if( g_bSaveFrame )
{
time_t t = ros::WallTime::now().sec;
char buf[1024] = "";
struct tm* tms = localtime(&t);
strftime(buf, 1024, "%Y-%m-%d-%H-%M-%S", tms);
std::string prefix = ( boost::format("capture_%s_user%d") % buf % this_user.uid ).str();
cv::Mat rgb_masked;
rgb.copyTo(rgb_masked, this_mask);
saveMat(rgb_masked , prefix + "_rgb" );
saveMat(this_mask , prefix + "_mask" );
saveMat(this_user.pc.getHist() , prefix + "_hist" );
saveMat(this_user.pc.getImage(), prefix + "_himg" );
}
// Mean depth
this_user.meandepth = cv::mean(depth_image, this_mask)[0];
this_user.silhouette_area = 0;
// Find the area of the silhouette in cartesian space
for( int i=0; i<this_mask.rows; i++) {
for( int j=0; j<this_mask.cols; j++ ) {
if( this_mask.at<uchar>(i,j) != 0 )
{
pixel_area = cam_model_.getDeltaX(1, depth_image.at<float>(i,j))
* cam_model_.getDeltaY(1, depth_image.at<float>(i,j));
this_user.silhouette_area += pixel_area;
}
}
}
// Find the center in 3D
g_UserGenerator.GetCoM(this_user.uid, center_mass);
this_user.center3d.point = vecToPt(center_mass);
ROS_DEBUG_STREAM(boost::format("User %d: area %.3fm^2, mean depth %.3fm")
% (unsigned int)this_user.uid % this_user.silhouette_area % this_user.meandepth);
// Screen out unlikely users based on area
if( this_user.meandepth > min_dist_ && this_user.silhouette_area < max_area_ && this_user.silhouette_area > min_area_ )
{
ROS_INFO_STREAM(boost::format("User %d new: %.0f --- orig: %.0f")
% ((int)this_user.uid) % (100*similarity) % (100*sim_to_orig) );
/*ROS_INFO_STREAM(boost::format("EMD new: %.2f --- orig: %.2f")
% (emd) % (emd_to_orig) );*/
if( similarity > PersonCal::getMatchThresh() ) {
user_cal_.update(rgb, this_mask);
}
else{
if( sim_to_orig > PersonCal::getMatchThresh() ) {
ROS_WARN_STREAM("Reset to original calibration");
user_cal_ = original_cal_;
}
}
std::stringstream window_name;
window_name << "user_" << ((int)this_user.uid);
cv::imshow(window_name.str(), this_user.pc.getImage());
ROS_DEBUG("Accepted user");
users.push_back(this_user);
// Visualization
geometry_msgs::Point32 p;
p.x = this_user.center3d.point.x;
p.y = this_user.center3d.point.y;
p.z = this_user.center3d.point.z;
cloud.points.push_back(p);
cloud.channels[0].values.push_back(0.0f);
}
}
// Try to associate the tracker with a user
if( latest_tracker_.first != "" )
{
// Transform the tracker to this time. Note that the pos time is updated but not the restamp.
tf::Point pt;
tf::pointMsgToTF(latest_tracker_.second.pos.pos, pt);
tf::Stamped<tf::Point> loc(pt, latest_tracker_.second.pos.header.stamp, latest_tracker_.second.pos.header.frame_id);
try {
tfl_->transformPoint(frame_id_, now_time-ros::Duration(.1), loc, latest_tracker_.second.pos.header.frame_id, loc);
latest_tracker_.second.pos.header.stamp = now_time;
latest_tracker_.second.pos.header.frame_id = frame_id_;
latest_tracker_.second.pos.pos.x = loc[0];
latest_tracker_.second.pos.pos.y = loc[1];
latest_tracker_.second.pos.pos.z = loc[2];
}
catch (tf::TransformException& ex) {
ROS_ERROR("(finding) Could not transform person to this time");
}
people_msgs::PositionMeasurement pos;
if( users.size() > 0 )
{
std::stringstream users_ss;
users_ss << boost::format("(finding) Tracker \"%s\" = (%.2f,%.2f) Users = ")
% latest_tracker_.first % latest_tracker_.second.pos.pos.x % latest_tracker_.second.pos.pos.y;
// Find the closest user to the tracker
user closest;
closest.distance = BIGDIST_M;
foreach(user u, users)
{
u.distance = pow(latest_tracker_.second.pos.pos.x - u.center3d.point.x, 2.0)
+ pow(latest_tracker_.second.pos.pos.y - u.center3d.point.y, 2.0);
users_ss << boost::format("(%.2f,%.2f), ") % u.center3d.point.x % u.center3d.point.y;
if( u.distance < closest.distance )
{
if( u.similarity > PersonCal::getMatchThresh() ) {
closest = u;
}
else {
ROS_WARN_STREAM("Ignored close user not matching (" << u.uid << ")");
}
}
}//foreach
