// 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