find position of object in a bag

edit

Hi guys,

I need to find(estimate) the 3D position of the 'people in the scene' -- (this is in the bag file) with respect to the camera.

My code is : (I didn't write it)

//ROS
#include <ros/ros.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/image_encodings.h>
#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>
//OpenCV
#include <opencv2/opencv.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
//BG SUBTRACTION
#include <opencv2/video/background_segm.hpp>

cv::Ptr<cv::BackgroundSubtractor> pMog;
cv::Mat fgMask;

void rgbCallback(const sensor_msgs::ImageConstPtr& msg)
{
  cv_bridge::CvImageConstPtr cv_ptr;
  try
  {
    cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
  }
  catch (cv_bridge::Exception& ex)
  {
    ROS_ERROR("cv_bridge exception: %s", ex.what());
    exit(-1);
  }


  pMog->operator()(cv_ptr->image, fgMask);


  cv::imshow("RGB", cv_ptr->image);
  cv::imshow("FgMask", fgMask);
  cv::waitKey(30);
}


void depthCallback(const sensor_msgs::ImageConstPtr& msg)
{
  cv_bridge::CvImageConstPtr cv_ptr;
  try
  {
     cv_ptr = cv_bridge::toCvCopy(msg, 
                sensor_msgs::image_encodings::TYPE_32FC1);
  }
  catch (cv_bridge::Exception& ex)
  {
    ROS_ERROR("cv_bridge exception: %s", ex.what());
    exit(-1);
  }

  cv::imshow("DEPTH", cv_ptr->image);
  cv::waitKey(30);
}

int main(int argc, char **argv) {
    ros::init(argc, argv, "kinectgrabber");

    pMog = new cv::BackgroundSubtractorMOG(); //MOG Approach

    ros::NodeHandle n;
    ros::Subscriber sub = 
        n.subscribe("/camera/rgb/image_color", 1, rgbCallback);
    //ros::Subscriber depth = 
    //  n.subscribe("/camera/depth/image", 1, depthCallback);
    ros::spin();
    return 0;
}

I have a bag file (it's a short video with two man walking by) and I have to make the code read the bag file print out the 3D position of the people in the scene.

Any help is very much appreciated. Thank you in advance.