how can I use tf message filter with cob_perception_msgs::ColorDepthImageArray?

asked 2018-06-30 03:11:19 -0600

amal gravatar image

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- ...
(more)
edit retag flag offensive close merge delete