$search

extractor.cpp

Go to the documentation of this file.

#include "extractor.hpp"

#include <rosbag/bag.h>
#include <rosbag/view.h>

#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>

#include <opencv/cv.h>
#include <opencv/highgui.h>

#include <cv_bridge/cv_bridge.h> 
#include <image_transport/image_transport.h>

#include <boost/foreach.hpp>
#include <time.h>
#include <math.h>

#define SAVEPLY false
#define FILTER_DISTANCE 1
#define SIGMA_P 15
#define SIGMA_R .05

typedef message_filters::sync_policies::ApproximateTime< sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::CameraInfo, sensor_msgs::CameraInfo > SyncPolicy;

template <class M>
class BagSubscriber : public message_filters::SimpleFilter<M>
{
public:
  void newMessage(const boost::shared_ptr<M const> &msg)
  {
    signalMessage(msg);
  }
};

extractor::extractor(int skipFrames, int maxFrames): 
  dataFile(NULL),
  timeFile(NULL),
  count(0),
  filenum(0),
  skipFrames(skipFrames),
  maxFrames(maxFrames)
{
  cout.precision(15);
  
  //synchronizer.registerCallback(boost::bind(&DepthListener::cameraCallback,this, _1, _2));
}
  
extractor::~extractor()
{
  if(dataFile)
    fclose(dataFile);
  if(timeFile)
    fclose(timeFile);
}

void extractor::extractBagFile(const string &bagFileName, const string &saveDirName)
{
  rosbag::Bag bag;
  bag.open(bagFileName, rosbag::bagmode::Read);
  resultsDir = saveDirName;

  char buffer[100];
  sprintf(buffer,"mkdir -p %s/%s",saveDirName.c_str(),"img/");
  system(buffer);

  string imgTopic = "/thermalvis/streamer/image_raw";
  string infoTopic = "/thermalvis/streamer/camera_info";

  stringstream imgPath;
  char *outputFilename;
  outputFilename = (char*) malloc(256);

    stringstream timePath;
    timePath << resultsDir << "/" << "time.txt";  

    timeFile = fopen(timePath.str().c_str(), "w");

  // Image topics to load
  std::vector<std::string> topics;
  topics.push_back(imgTopic);
  topics.push_back(infoTopic);
  
  rosbag::View view(bag, rosbag::TopicQuery(topics));

  uint32_t msg_size = view.size() ;
  ROS_INFO("the msg size is %d",msg_size);

  // Set up fake subscribers to capture images
  //BagSubscriber<sensor_msgs::Image> subRgb, subDepth, subThermal;
  //BagSubscriber<sensor_msgs::CameraInfo> subRgbInfo, subDepthInfo, subThermalInfo;
  // Use time synchronizer to make sure we get properly synchronized images
  //message_filters::Synchronizer<SyncPolicy> sync(SyncPolicy(35), subRgb, subDepth, subThermal, subRgbInfo, subDepthInfo, subThermalInfo);
  //boost::bind(&PointExtractor::cameraCallback,this, _1, _2, _3);
  //sync.registerCallback(boost::bind(&PointExtractor::cameraCallback,this, _1, _2, _3, _4, _5, _6)); // Steve
 
  int count_img = 0 ; 
  int count_info = 0 ; 
  
  char bagName[256];
  int idx = bagFileName.find_last_of("/\\");
  
  sprintf(bagName, "%s", (bagFileName.substr(idx+1, bagFileName.length())).c_str());
        
        fprintf(timeFile,"# images\n");
        fflush(timeFile);
        fprintf(timeFile,"# file: '%s'\n", bagName);
        fflush(timeFile);
        fprintf(timeFile,"# timestamp filename\n");
        fflush(timeFile);
 
  BOOST_FOREACH(rosbag::MessageInstance const m, view)
  {

        
        if (m.getTopic() == imgTopic){
                sensor_msgs::Image::ConstPtr img = m.instantiate<sensor_msgs::Image>();

                sprintf(outputFilename, "%s/thermal/frame%06d.%s", resultsDir.c_str(), count, "png");
                
                ROS_INFO("Image goes to (%s)", outputFilename);

                // copy sensor msg Image to opencv 
                cv_bridge::CvImagePtr cv_ptr_img;
                try {
                        cv_ptr_img = cv_bridge::toCvCopy(img, "bgr8");
                } catch (cv_bridge::Exception& e) {
                        ROS_ERROR("cv_bridge exception: %s", e.what());
                        return;
                }

                cv::Mat rawThermal, grayThermal, dispThermal;
                rawThermal = cv::Mat(cv_ptr_img->image);

                if (rawThermal.type() == CV_16UC3) {
                        cv::cvtColor(rawThermal, grayThermal, CV_RGB2GRAY);
                } else {
                        grayThermal = cv::Mat(rawThermal);
                }
                cv::imwrite(outputFilename,grayThermal);
                //fprintf(thermaltimeFile,"%ld \n",thermalImg->header.stamp.toNSec());
                //fflush(thermaltimeFile);
                ++count;
        }
    
    
    if (m.getTopic() == infoTopic){
      sensor_msgs::CameraInfo::ConstPtr info = m.instantiate<sensor_msgs::CameraInfo>();

                //fprintf(thermaltimeFile,"%ld ",thermalInfo->header.stamp.toNSec());
                fprintf(timeFile,"%010d.%06d ",info->header.stamp.sec, info->header.stamp.nsec/1000);

                /*
                if (thermalInfo->binning_x!=0) {        
                        unsigned long internalimg_time = (((unsigned long) thermalInfo->binning_y) << 32) | thermalInfo->binning_x;
                        fprintf(thermaltimeFile,"%ld \n",internalimg_time);
                }
                */
                
                fprintf(timeFile,"thermal/frame%06d.png\n", count_info);
                
                fflush(timeFile);
                
                ++count_info;
    }
    

  }
  bag.close();
}
/*
void PointExtractor::cameraCallback(const sensor_msgs::Image::ConstPtr& rgbImgMsg, 
                                   const sensor_msgs::Image::ConstPtr& depthImgMsg,
                                   const sensor_msgs::Image::ConstPtr& thermalImgMsg,
                                   const sensor_msgs::CameraInfo::ConstPtr& rgbInfoMsg,
                                   const sensor_msgs::CameraInfo::ConstPtr& depthInfoMsg,
                                   const sensor_msgs::CameraInfo::ConstPtr& thermalInfoMsg
                                   )
{
  if(count == 0){
    stringstream timePath;
    stringstream internaltimePath;
    timePath << resultsDir << "time.txt";  
    internaltimePath << resultsDir << "internaltime.txt";    

    timeFile = fopen(timePath.str().c_str(), "w");
    internaltimeFile = fopen(internaltimePath.str().c_str(), "w");
  }

  if(count%(maxFrames*skipFrames) == 0) {     
    if(dataFile)
      fclose(dataFile);
    
    stringstream dataPath;
    dataPath << resultsDir << "data/kinectdataD" << filenum << ".bin";

    filenum++;
    
    dataFile = fopen(dataPath.str().c_str(), "w");
    if (!dataFile){
      printf("Unable to open file\n");
      exit(0);
    }
    else {
      startTime = depthImgMsg->header.stamp.toSec();
      if(!SAVEPLY)
        fwrite((const void*)&startTime,sizeof(double),1,dataFile);  
    }
  }
  if( count%skipFrames == 0) {
    fprintf(timeFile,"%ld %ld %ld\n",rgbImgMsg->header.stamp.toNSec(), depthImgMsg->header.stamp.toNSec() , thermalImgMsg->header.stamp.toNSec() );

    if (rgbInfoMsg->binning_x!=0 && depthInfoMsg->binning_x!=0)
     {  
        unsigned long internalimg_time = (((unsigned long) rgbInfoMsg->binning_y) << 32) | rgbInfoMsg->binning_x;
        unsigned long internaldepth_time = (((unsigned long) depthInfoMsg->binning_y) << 32) | depthInfoMsg->binning_x;
        fprintf(internaltimeFile,"%ld %ld\n",internalimg_time, internaldepth_time);
     }

    stringstream imgPath;
    char *outputFilename;
    outputFilename = (char*) malloc(256);
                        
    //imgPath << resultsDir << "img/kinectImg%06d" << count << ".jpg";
    sprintf(outputFilename, "%s/img/frame%06d.%s", resultsDir.c_str(), count, "jpg");
    
    // copy sensor msg Image to opencv 
    cv_bridge::CvImagePtr cv_ptr;
    try {
      cv_ptr = cv_bridge::toCvCopy(rgbImgMsg, "bgr8");
    } catch (cv_bridge::Exception& e) {
      ROS_ERROR("cv_bridge exception: %s", e.what());
      return;
    }
    cv::imwrite(outputFilename,cv_ptr->image);
    
    //stringstream thermalPath;
    //thermalPath << resultsDir << "thermal/thermalImg" << count << ".png";
    char *outputFilenameThermal;
    outputFilenameThermal = (char*) malloc(256);
    sprintf(outputFilenameThermal, "%s/thermal/frame%06d.%s", resultsDir.c_str(), count, "png");
    
    // copy sensor msg Image to opencv 
    cv_bridge::CvImagePtr cv_ptr_thermal;
    try {
      cv_ptr_thermal = cv_bridge::toCvCopy(thermalImgMsg, "bgr8");
    } catch (cv_bridge::Exception& e) {
      ROS_ERROR("cv_bridge exception: %s", e.what());
      return;
    }
    
    cv::Mat rawThermal, grayThermal, dispThermal;
    
    rawThermal = cv::Mat(cv_ptr_thermal->image);
    
    if (rawThermal.type() == CV_16UC3) {
                cv::cvtColor(rawThermal, grayThermal, CV_RGB2GRAY);
        } else {
                grayThermal = cv::Mat(rawThermal);
        }
        
    cv::imwrite(outputFilenameThermal,grayThermal);

    int time = clock();
    //CameraHandle(depthCamMsg);
    getPointCloud(depthImgMsg, rgbImgMsg);
    printf("Time depth: %f ms\n" ,((static_cast<float>(clock()-time)/CLOCKS_PER_SEC)*1000));
    //if(SAVEPLY)
    //  exit(0);
  }
  
  ++count;
}
*/

---

thermalvis
Author(s): Stephen Vidas
autogenerated on Tue Mar 5 12:25:24 2013