how can I use tf message filter with cob_perception_msgs::ColorDepthImageArray?
I have a project for activities analysis and face recognition . I just link my project of activity recognition which is subscribed to tf topic and the project of "cob_people_perception" but the problem is there no synchronization to link every skeleton with it's appropriate face. I will be so grateful if you help me how to synchronize tf topic with cob_perception_msgs::ColorDepthImageArray topic with tf:MessageFilter . Bellow is the code where I want to make synchronization .Thank you for advance . Plateform : Elementary Os Packages : cob_people_perception Ros version : Kinetic
void FaceRecognizerNode::facePositionsCallback(const cob_perception_msgs::ColorDepthImageArray::ConstPtr& face_positions)
{ // Timer tim; // tim.start();
// receive head and face positions and recognize faces in the face region, finally publish detected and recognized faces
// --- convert color image patches of head regions and contained face bounding boxes ---
cv_bridge::CvImageConstPtr cv_ptr;
std::vector<cv::Mat> heads_color_images(face_positions->head_detections.size());
std::vector<cv::Mat> heads_depth_images(face_positions->head_detections.size());
std::vector<std::vector<cv::Rect> > face_bounding_boxes(face_positions->head_detections.size());
std::vector<cv::Rect> head_bounding_boxes(face_positions->head_detections.size());
for (unsigned int i = 0; i < face_positions->head_detections.size(); i++)
{
// color image
if (enable_face_recognition_ == true)
{
sensor_msgs::ImageConstPtr msgPtr = boost::shared_ptr<sensor_msgs::Image const>(&(face_positions->head_detections[i].color_image), voidDeleter);
try
{
cv_ptr = cv_bridge::toCvShare(msgPtr, sensor_msgs::image_encodings::BGR8);
} catch (cv_bridge::Exception& e)
{
ROS_ERROR("cv_bridge exception: %s", e.what());
return;
}
heads_color_images[i] = cv_ptr->image;
}
// depth image
sensor_msgs::ImageConstPtr msgPtr = boost::shared_ptr<sensor_msgs::Image const>(&(face_positions->head_detections[i].depth_image), voidDeleter);
try
{
cv_ptr = cv_bridge::toCvShare(msgPtr, sensor_msgs::image_encodings::TYPE_32FC3);
} catch (cv_bridge::Exception& e)
{
ROS_ERROR("cv_bridge exception: %s", e.what());
return;
}
heads_depth_images[i] = cv_ptr->image;
// face bounding boxes
face_bounding_boxes[i].resize(face_positions->head_detections[i].face_detections.size());
for (uint j = 0; j < face_bounding_boxes[i].size(); j++)
{
const cob_perception_msgs::Rect& source_rect = face_positions->head_detections[i].face_detections[j];
cv::Rect rect(source_rect.x, source_rect.y, source_rect.width, source_rect.height);
face_bounding_boxes[i][j] = rect;
}
// head bounding box
const cob_perception_msgs::Rect& source_rect = face_positions->head_detections[i].head_detection;
cv::Rect rect(source_rect.x, source_rect.y, source_rect.width, source_rect.height);
head_bounding_boxes[i] = rect;
}
// --- face recognition ---
std::vector<std::vector<std::string> > identification_labels;
bool identification_failed = false;
if (enable_face_recognition_ == true)
{
// recognize faces
//unsigned long result_state = face_recognizer_.recognizeFaces(heads_color_images, face_bounding_boxes, identification_labels);
//timeval t1,t2;
//gettimeofday(&t1,NULL);
unsigned long result_state = face_recognizer_.recognizeFaces(heads_color_images, heads_depth_images, face_bounding_boxes, identification_labels);
//gettimeofday(&t2,NULL);
//std::cout<<(t2.tv_sec - t1.tv_sec) * 1000.0<<std::endl;
if (result_state == ipa_Utils::RET_FAILED)
{
ROS_ERROR("FaceRecognizerNode::face_positions_callback: Please load a face recognition model at first.");
identification_failed = true;
}
}
if (enable_face_recognition_ == false || identification_failed == true)
{
// label all image unknown if face recognition disabled
identification_labels.resize(face_positions->head_detections.size());
for (uint i = 0; i < identification_labels.size(); i++)
{
identification_labels[i].resize(face_positions->head_detections[i].face_detections.size());
for (uint j = 0; j < identification_labels[i].size(); j++)
identification_labels[i][j] = "Unknown";
}
}
// --- publish detection message ---
FaceDetectionMessageHelper face_detection_message_helper;
cob_perception_msgs::DetectionArray detection_msg;
std::string row = "";
ofstream myfile;
face_detection_message_helper.prepareCartesionDetectionMessage(row,detection_msg, face_positions->header, heads_depth_images, head_bounding_boxes,
face_bounding_boxes, &identification_labels);
// publish message
face_recognition_publisher_.publish(detection_msg);
myfile.open ("/home/amal/catkin_ws/src/cob_people_perception/cob_people_detection/files/recognition.csv",fstream::app);
myfile << row ;
if (display_timing_ == true)
ROS_INFO("%d FaceRecognition: Time stamp of pointcloud message: %f. Delay: %f.", face_positions- ...