extractor.cpp

Go to the documentation of this file.

#include "extractor.hpp"

int main(int argc, char** argv) {
        

        ROS_INFO("Node launched.");
        
        ros::init(argc, argv, "extractor");
        
        ros::NodeHandle private_node_handle("~");
        
        extractorData startupData;
        
        startupData.read_addr = argv[0];
        startupData.read_addr = startupData.read_addr.substr(0, startupData.read_addr.size()-11);
                
        bool inputIsValid = startupData.obtainStartingData(private_node_handle);
        
        if (!inputIsValid) {
                ROS_ERROR("Configuration invalid.");
        }
                
        ROS_INFO("Startup data processed.");
        
        
        ros::NodeHandle nh;
        
        extractorNode extractor_node(nh, startupData);
        
        ROS_INFO("Node configured.");
        
        
        while (extractor_node.isStillListening()) {     
                extractor_node.extractBagFile();
        }
        
        return 0;
        
}

bool extractorData::obtainStartingData(ros::NodeHandle& nh) {
        
        wantsDepth = false;
        wantsRgb = false;
        wantsThermal = false;
        
        rawTimeFormat = true;
        
        nh.param<bool>("setupForMapping", setupForMapping, false);
        
        nh.param<bool>("splitBags", splitBags, false);
        
        nh.param<std::string>("bagSource", bagSource, "bagSource");
        nh.param<std::string>("outputFolder", outputFolder, "outputFolder");
        
        if (outputFolder == "outputFolder") {
                if (splitBags) {
                        outputFolder = bagSource.substr(0, bagSource.size());
                } else {
                        outputFolder = bagSource.substr(0, bagSource.size()-4);
                }
        }
        
        outputFolder = outputFolder + "_data";
        
        nh.param<int>("maxFrames", maxFrames, 10000);
        
        nh.param<bool>("debugMode", debugMode, false);
        nh.param<bool>("verboseMode", verboseMode, false);
        
        nh.param<std::string>("rgbTopic", rgbTopic, "rgb");
        nh.param<std::string>("depthTopic", depthTopic, "depth");
        nh.param<std::string>("thermalTopic", thermalTopic, "thermal");
        
        if (rgbTopic != "rgb") {
                rgbInfoTopic = rgbTopic.substr(0, rgbTopic.find_last_of("/") + 1) + "camera_info";
                if (verboseMode) { ROS_INFO("Assuming rgbInfoTopic is (%s)", rgbInfoTopic.c_str()); }
                wantsRgb = true;
        }
        
        if (thermalTopic != "thermal") {
                thermalInfoTopic = thermalTopic.substr(0, thermalTopic.find_last_of("/") + 1) + "camera_info";
                if (verboseMode) { ROS_INFO("Assuming thermalInfoTopic is (%s)", thermalInfoTopic.c_str()); }
                wantsThermal = true;
        }
        
        if (depthTopic != "depth") {
                depthInfoTopic = depthTopic.substr(0, depthTopic.find_last_of("/") + 1) + "camera_info";
                if (verboseMode) { ROS_INFO("Assuming depthInfoTopicrosc is (%s)", depthInfoTopic.c_str()); }
                wantsDepth = true;
        }
        
        nh.param<std::string>("thermalIntrinsics", thermalIntrinsics, "thermalIntrinsics");
        nh.param<bool>("undistortThermal", undistortThermal, true);


        if (undistortThermal) {
                if (thermalIntrinsics != "thermalIntrinsics") {
                        ROS_INFO("Thermal intrinsics at (%s) selected.", thermalIntrinsics.c_str());
                } else {
                        ROS_ERROR("read_addr = (%s)", read_addr.c_str());
                        thermalIntrinsics = read_addr + "data/calibration/csiro-aslab/miricle-1.yml";
                        ROS_ERROR("No thermal intrinsics supplied. Defaulting to (%s)", thermalIntrinsics.c_str());
                }
        }
        
        nh.param<bool>("internalTimestamp", internalTimestamp, false);
        nh.param<bool>("thermistorReading", thermistorReading, false);
        nh.param<bool>("recordDuplicates", recordDuplicates, false);
        
        return true;
}


bool extractorNode::isStillListening() {
                return stillListening;
}


extractorNode::extractorNode(ros::NodeHandle& nh, extractorData startupData): 
  dataFile(NULL),
  timeFile(NULL),
  depthtimeFile(NULL),
  depthInternaltimeFile(NULL),
  imgtimeFile(NULL),
  imgInternaltimeFile(NULL),
  thermaltimeFile(NULL),
  thermalInternaltimeFile(NULL),
  count(0),
  filenum(0),
  skipFrames(skipFrames),
  maxFrames(maxFrames),
  d_fx(525),
  d_fy(525),
  d_cx(319.5),
  d_cy(239.5) {

        
        stillListening = true;

        
        configData = startupData;
        
        startTime = -1.0;
        
        
        
        
          
  outputFilename = (char*) malloc(256);
  outputFilenameThermal = (char*) malloc(256);
        outputdepth = (char*) malloc(256);
        //outputcloud = (char*) malloc(256);
        

    if (configData.wantsThermal) {
                topics.push_back(startupData.thermalTopic);
                topics.push_back(startupData.thermalInfoTopic);
                if (configData.verboseMode) { ROS_INFO("Subscribing to (%s) and (%s)...", startupData.thermalTopic.c_str(), startupData.thermalInfoTopic.c_str()); }
        }
        
        if (configData.wantsRgb) {
                topics.push_back(startupData.rgbTopic);
                topics.push_back(startupData.rgbInfoTopic);
                if (configData.verboseMode) { ROS_INFO("Subscribing to (%s) and (%s)...", startupData.rgbTopic.c_str(), startupData.rgbInfoTopic.c_str()); }
        }
        
        if (configData.wantsDepth) {
                topics.push_back(startupData.depthTopic);
                topics.push_back(startupData.depthInfoTopic);
                if (configData.verboseMode) { ROS_INFO("Subscribing to (%s) and (%s)...", startupData.depthTopic.c_str(), startupData.depthInfoTopic.c_str()); }
        }
        
        

        count_rgb = 0 ; 
        count_depth = 0 ;
        count_rgbinfo = 0 ; 
        count_depthinfo = 0;
        count_thermal = 0 ;
        count_thermalinfo = 0 ;

        makeDirectories();
        
        if (configData.verboseMode) { ROS_INFO("Directories made."); }
        
        if (configData.thermistorReading) {
                thermistorLogFile = startupData.outputFolder + "/thermistor.txt";
                ROS_INFO("Opening file for thermistor data: (%s)\n", thermistorLogFile.c_str());
                ofs_thermistor_log.open(thermistorLogFile.c_str());
        }
        
        
        //timePath << resultsDir << "time.txt";  
        
        if (configData.wantsThermal) {
                thermaltimePath << startupData.outputFolder << "/" << "thermal.txt"; 
                thermaltimeFile = fopen(thermaltimePath.str().c_str(), "w");
                
                if (configData.internalTimestamp) {
                        thermalInternaltimePath << startupData.outputFolder << "/" << "thermal-internal.txt"; 
                        thermalInternaltimeFile = fopen(thermalInternaltimePath.str().c_str(), "w");
                }
                
                if (configData.recordDuplicates) {
                        thermalDuplicatesPath << startupData.outputFolder << "/" << "thermal-duplicates.txt"; 
                        thermalDuplicatesFile = fopen(thermalDuplicatesPath.str().c_str(), "w");
                }
        }
        
        if (configData.wantsRgb) {
                imgtimePath << startupData.outputFolder << "/" << "rgb.txt";            
                imgtimeFile = fopen(imgtimePath.str().c_str(), "w");
                
                if (configData.internalTimestamp) {
                        imgInternaltimePath << startupData.outputFolder << "/" << "rgb-internal.txt"; 
                        imgInternaltimeFile = fopen(imgInternaltimePath.str().c_str(), "w");
                }
                
        }
        
        if (configData.wantsDepth) {
                depthtimePath << startupData.outputFolder << "/" << "depth.txt";  
                depthtimeFile = fopen(depthtimePath.str().c_str(), "w");
                
                if (configData.internalTimestamp) {
                        depthInternaltimePath << startupData.outputFolder << "/" << "depth-internal.txt"; 
                        depthInternaltimeFile = fopen(depthInternaltimePath.str().c_str(), "w");
                }
        }
  
        char bagName[256];
        int idx = startupData.bagSource.find_last_of("/\\");
  
        sprintf(bagName, "%s", (startupData.bagSource.substr(idx+1, startupData.bagSource.length())).c_str());
  
        printf("%s << bagName = (%s)\n", __FUNCTION__, bagName);
  
        if (configData.wantsRgb) {
                fprintf(imgtimeFile,"# color images\n");
                fflush(imgtimeFile);
                fprintf(imgtimeFile,"# file: '%s'\n", startupData.bagSource.c_str());
                fflush(imgtimeFile);
                fprintf(imgtimeFile,"# timestamp filename\n");
                fflush(imgtimeFile);
                
                if (configData.internalTimestamp) {
                        fprintf(imgInternaltimeFile,"# color images\n");
                        fflush(imgInternaltimeFile);
                        fprintf(imgInternaltimeFile,"# file: '%s'\n", startupData.bagSource.c_str());
                        fflush(imgInternaltimeFile);
                        fprintf(imgInternaltimeFile,"# timestamp filename\n");
                        fflush(imgInternaltimeFile);
                }
                
                
        }
        
        if (configData.wantsDepth) {
                fprintf(depthtimeFile,"# depth images\n");
                fflush(depthtimeFile);
                fprintf(depthtimeFile,"# file: '%s'\n", startupData.bagSource.c_str());
                fflush(depthtimeFile);
                fprintf(depthtimeFile,"# timestamp filename\n");
                fflush(depthtimeFile);
                
                if (configData.internalTimestamp) {
                        fprintf(depthInternaltimeFile,"# depth images\n");
                        fflush(depthInternaltimeFile);
                        fprintf(depthInternaltimeFile,"# file: '%s'\n", startupData.bagSource.c_str());
                        fflush(depthInternaltimeFile);
                        fprintf(depthInternaltimeFile,"# timestamp filename\n");
                        fflush(depthInternaltimeFile);
                }
                
        }
        
        if (configData.wantsThermal) {
                fprintf(thermaltimeFile,"# thermal images\n");
                fflush(thermaltimeFile);
                fprintf(thermaltimeFile,"# file: '%s'\n", startupData.bagSource.c_str());
                fflush(thermaltimeFile);
                fprintf(thermaltimeFile,"# timestamp filename\n");
                fflush(thermaltimeFile);
                
                if (configData.internalTimestamp) {
                        fprintf(thermalInternaltimeFile,"# thermal images\n");
                        fflush(thermalInternaltimeFile);
                        fprintf(thermalInternaltimeFile,"# file: '%s'\n", startupData.bagSource.c_str());
                        fflush(thermalInternaltimeFile);
                        fprintf(thermalInternaltimeFile,"# timestamp filename\n");
                        fflush(thermalInternaltimeFile);
                }
                
                
                
                if (configData.recordDuplicates) {
                        fprintf(thermalDuplicatesFile,"# thermal duplicates\n");
                        fflush(thermalDuplicatesFile);
                        fprintf(thermalDuplicatesFile,"# file: '%s'\n", startupData.bagSource.c_str());
                        fflush(thermalDuplicatesFile);
                        fprintf(thermalDuplicatesFile,"# (is potentially a duplicate)\n");
                        fflush(thermalDuplicatesFile);
                }
        }
                
                
        if (configData.verboseMode) { ROS_INFO("Here [152]\n");}
        
        if (configData.wantsThermal) {
                if (configData.undistortThermal) {
                        
                        ROS_INFO("Reading in calibration data");
                        
                        if (configData.thermalIntrinsics[0] != '/') {
                                configData.thermalIntrinsics = configData.read_addr + configData.thermalIntrinsics;
                        }
                        
                        ROS_INFO("thermal intrinsics = %s", configData.thermalIntrinsics.c_str());
                        
                        
                        FileStorage fs(configData.thermalIntrinsics, FileStorage::READ);
                        fs["imageSize"] >> imageSize;
                        fs["cameraMatrix"] >> cameraMatrix;
                        fs["distCoeffs"] >> distCoeffs;
                        fs["newCamMat"] >> newCamMat;
                        fs.release();
                        
                        ROS_INFO("Calibration data read.");

                        cameraSize = Size(imageSize.at<unsigned short>(0, 0), imageSize.at<unsigned short>(0, 1));
                        
                        ROS_INFO("Camera params determined.");
                        
                        initUndistortRectifyMap(cameraMatrix, distCoeffs,  Mat() , newCamMat, cameraSize, CV_32FC1, map1, map2);
                   
                        
                        
                }
        }
        
        if (configData.splitBags) {
                
                
                DIR * dirp;
                struct dirent * entry;
                
                dirp = opendir(configData.bagSource.c_str());
                
                if (dirp == NULL) {
                        ROS_ERROR("Opening of directory (%s) failed.", configData.bagSource.c_str());
                        return;
                }

                while ((entry = readdir(dirp)) != NULL) {
                        if (entry->d_type == DT_REG) { // If the entry is a regular file
                                splitBagNames.push_back(string(entry->d_name));
                        }
                }

                closedir(dirp);

                sort(splitBagNames.begin(), splitBagNames.end());

                if(splitBagNames.size() <= 0)   {
                        ROS_ERROR("File counting error.");
                        return;
                }

                ROS_INFO("No. of bags in folder = %d", splitBagNames.size());
                
                for (unsigned int iii = 0; iii < splitBagNames.size(); iii++) {
                        ROS_INFO("Bag(%d) = (%s)", iii, splitBagNames.at(iii).c_str());
                }
                
                ROS_INFO("Prepared for split bags (%s)", bagName);

        } else {
                ROS_INFO("Prepared for single bag (%s)", bagName);
                splitBagNames.push_back(bagName);
        }
        
}

extractorNode::~extractorNode() {
  if(dataFile)
    fclose(dataFile);
  if(timeFile)
    fclose(timeFile);
    
    if (configData.wantsRgb) {
                if(imgtimeFile) fclose(imgtimeFile);
                if(imgInternaltimeFile) fclose(imgInternaltimeFile);
        }
        
        if (configData.wantsDepth) {
                if(depthtimeFile) fclose(depthtimeFile);
                if(depthInternaltimeFile) fclose(depthInternaltimeFile);
        }
        
        if (configData.wantsThermal) {
                if(thermaltimeFile) fclose(thermaltimeFile);
                if(thermalInternaltimeFile) fclose(thermalInternaltimeFile);
                
                if (configData.recordDuplicates) {
                        if(thermalDuplicatesFile) fclose(thermalDuplicatesFile);  
                }
                
        }
        
        if (configData.thermistorReading) {
                if (ofs_thermistor_log.is_open()) ofs_thermistor_log.close();
        }
    
        
}

void extractorNode::serverCallback(thermalvis::extractorConfig &config, uint32_t level) {
            
        //configData.maxFrames = config.maxFrames;
        //configData.debugMode = config.debugMode;
        // no point putting anything in here coz there is no ROS spinning...?
        
}

void extractorNode::makeDirectories() {
        
        char buffer[256];
        sprintf(buffer,"mkdir -p %s",configData.outputFolder.c_str());
        system(buffer);
        
        if (configData.setupForMapping) {
                sprintf(buffer,"mkdir -p %s/%s",configData.outputFolder.c_str(),"meshes/");
                system(buffer);
                sprintf(buffer,"mkdir -p %s/%s",configData.outputFolder.c_str(),"worlds/");
                system(buffer);
                sprintf(buffer,"mkdir -p %s/%s",configData.outputFolder.c_str(),"emeshes/");
                system(buffer);
                sprintf(buffer,"mkdir -p %s/%s",configData.outputFolder.c_str(),"cmeshes/");
                system(buffer);
                sprintf(buffer,"mkdir -p %s/%s",configData.outputFolder.c_str(),"smeshes/");
                system(buffer);
                sprintf(buffer,"mkdir -p %s/%s",configData.outputFolder.c_str(),"poses/");
                system(buffer);
                sprintf(buffer,"mkdir -p %s/%s",configData.outputFolder.c_str(),"cloud/");
                system(buffer);
        }
        
        if (configData.wantsRgb) {
                sprintf(buffer,"mkdir -p %s/%s",configData.outputFolder.c_str(),"rgb/");
                system(buffer);
        }
        
        if (configData.wantsThermal) {
                sprintf(buffer,"mkdir -p %s/%s",configData.outputFolder.c_str(),"thermal/");
                system(buffer);
        }
        
        if (configData.wantsDepth) {
                sprintf(buffer,"mkdir -p %s/%s",configData.outputFolder.c_str(),"depth/");
                system(buffer);
        }
        
        char buffer_check[256];
        sprintf(buffer_check,"%s", &buffer[9]);
        
        if (configData.verboseMode) { ROS_INFO("Checking that last directory (%s) has been created..", buffer_check); }
        
        while (!boost::filesystem::exists( buffer_check ) ) {
                
        }
        
}

void extractorNode::extractBagFile()
{
        
        for (unsigned int zzz = 0; zzz < splitBagNames.size(); zzz++) {
                
                string currentBagName;
                        
                
                if (configData.splitBags) {
                        currentBagName = configData.bagSource + "/" + splitBagNames.at(zzz);
                        bag[zzz].open(currentBagName, rosbag::bagmode::Read);
                } else {
                        currentBagName = configData.bagSource;
                        bag[zzz].open(configData.bagSource, rosbag::bagmode::Read);
                }
                
                ROS_INFO("Currently processing bag(%d) = (%s)...", zzz, currentBagName.c_str());
                
                
  
                view = new rosbag::View (bag[zzz], rosbag::TopicQuery(topics));

                uint32_t msg_size = view->size() ;
                
                if (configData.verboseMode) { ROS_INFO("the msg size is %d",msg_size); }
                
                int minCount = 0;
         
                BOOST_FOREACH(rosbag::MessageInstance const m, *view) {
                  
                minCount = 99999;

                if (configData.wantsRgb) {
                        if ((m.getTopic() == configData.rgbTopic) && ((count_rgb <= configData.maxFrames) || (configData.maxFrames == 0))) {

                                //ROS_WARN("Processing RGB topic...");

                                rgbImg = m.instantiate<sensor_msgs::Image>();
                                sprintf(outputFilename, "%s/rgb/frame%06d.%s", configData.outputFolder.c_str(), count_rgb, "png");

                                //ROS_INFO("Image goes to (%s)", outputFilename);

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

                                if (configData.debugMode) {
                                cv::imshow("rgb", cv_ptr->image);
                                cv::waitKey(1);
                                }


                                cv::imwrite(outputFilename,cv_ptr->image);
                                ++count_rgb ;

                                if (count_rgb < minCount) {
                                minCount = count_rgb;
                                }
                        }

                        if ((m.getTopic() == configData.rgbInfoTopic) && ((count_rgbinfo <= configData.maxFrames) || (configData.maxFrames == 0))) {

                                //ROS_WARN("Processing RGB info topic...");




                                rgbInfo = m.instantiate<sensor_msgs::CameraInfo>();
                                
                                if (configData.internalTimestamp) {
                                        uint64_t internal_time;
                                        memcpy(&internal_time, &rgbInfo->R[0], 8);

                                        uint32_t internal_sec, internal_nsec;

                                        internal_sec = internal_time / 1000000;
                                        internal_nsec = (internal_time % 1000000) * 1000;
                                        
                                        if (configData.rawTimeFormat) {
                                                fprintf(imgInternaltimeFile,"%ld ", internal_time);
                                        } else {
                                                fprintf(imgInternaltimeFile,"%010d.%09d ", internal_sec, internal_nsec);
                                        }
                                        
                                        
                                        fprintf(imgInternaltimeFile,"rgb/frame%06d.png\n", count_rgbinfo);

                                        fflush(imgInternaltimeFile);
                                }

                                //fprintf(imgtimeFile,"%ld ",rgbInfo->header.stamp.toNSec());

                                
                                if (configData.rawTimeFormat) {
                                        fprintf(imgtimeFile,"%ld ",rgbInfo->header.stamp.toNSec());
                                } else {
                                        fprintf(imgtimeFile,"%010d.%06d ",rgbInfo->header.stamp.sec, rgbInfo->header.stamp.nsec/1000);
                                }
                                
                                

                                /*
                                if (rgbInfo->binning_x!=0) {    
                                unsigned long internalimg_time = (((unsigned long) rgbInfo->binning_y) << 32) | rgbInfo->binning_x;
                                fprintf(imgtimeFile,"%ld \n",internalimg_time);
                                }
                                */

                                fprintf(imgtimeFile,"rgb/frame%06d.png\n", count_rgbinfo);

                                fflush(imgtimeFile);

                                

                                ++count_rgbinfo;

                                if (count_rgbinfo < minCount) {
                                        minCount = count_rgbinfo;
                                }
                        }
                }
                
                
                if (configData.wantsDepth) {
                        if ((m.getTopic() == configData.depthTopic) && ((count_depth <= configData.maxFrames) || (configData.maxFrames == 0))) {
                                
                                depthImg = m.instantiate<sensor_msgs::Image>();
                                
                                sprintf(outputdepth, "%s/depth/frame%06d.%s", configData.outputFolder.c_str(), count_depth, "png");

                                if (startTime == -1.0) {
                                        startTime = depthImg->header.stamp.toSec();
                                }
                                
                                // copy sensor msg Image to opencv 
                                cv_bridge::CvImagePtr cv_ptr_depth;
                                try {
                                        cv_ptr_depth = cv_bridge::toCvCopy(depthImg, "16UC1");
                                        // but says 32FC1 ... :(
                                        // http://answers.ros.org/question/10591/publish-kinect-depth-information-to-a-ros-topic/
                                        // cv_ptr_depth_ = cv_bridge::toCvCopy(depth_image_msgs, sensor_msgs::image_encodings::TYPE_32FC1);
                                        // cv_image = self.bridge.imgmsg_to_cv(data, "32FC1")
                                } catch (cv_bridge::Exception& e) {
                                        ROS_ERROR("cv_bridge exception: %s", e.what());
                                        return;
                                }

                                cv::Mat rawDepth;
                                rawDepth = cv::Mat(cv_ptr_depth->image);
                                
                                for (unsigned int iii = 0; iii < rawDepth.rows; iii++) {
                                        for (unsigned int jjj = 0; jjj < rawDepth.cols; jjj++) {
                                                if (5.0 * double(rawDepth.at<unsigned short>(iii,jjj)) > 65535.0) {
                                                        rawDepth.at<unsigned short>(iii,jjj) = 0;
                                                } else {
                                                        rawDepth.at<unsigned short>(iii,jjj) = 5 * rawDepth.at<unsigned short>(iii,jjj);
                                                }
                                                
                                        }
                                }
                                
                                if (configData.debugMode) {
                                        cv::imshow("depth", rawDepth*5);
                                        cv::waitKey(1);
                                }
                                

                                cv::imwrite(outputdepth,rawDepth);
                                //fprintf(depthtimeFile,"%ld \n",depthImg->header.stamp.toNSec());
                                //fflush(depthtimeFile);
                                
                                ++count_depth;
                                
                                if (count_depth < minCount) {
                                        minCount = count_depth;
                                }
                        }
                
                
                        if ((m.getTopic() == configData.depthInfoTopic) && ((count_depthinfo <= configData.maxFrames) || (configData.maxFrames == 0))) {
                                depthInfo = m.instantiate<sensor_msgs::CameraInfo>();
                                
                                if (configData.internalTimestamp) {
                                        uint64_t internal_time;
                                        memcpy(&internal_time, &depthInfo->R[0], 8);
                                        
                                        uint32_t internal_sec, internal_nsec;
                                        
                                        internal_sec = internal_time / 1000000;
                                        internal_nsec = (internal_time % 1000000) * 1000;
                                        
                                        if (configData.rawTimeFormat) {
                                                fprintf(depthInternaltimeFile,"%ld ", internal_time);
                                        } else {
                                                fprintf(depthInternaltimeFile,"%010d.%09d ", internal_sec, internal_nsec);
                                        }
                                        
                                        
                                        
                                        fprintf(depthInternaltimeFile,"depth/frame%06d.png\n", count_depthinfo);
                                
                                        fflush(depthInternaltimeFile);
                                }

                                if (configData.rawTimeFormat) {
                                        fprintf(depthtimeFile,"%ld ",depthInfo->header.stamp.toNSec());
                                } else {
                                        fprintf(depthtimeFile,"%010d.%06d ",depthInfo->header.stamp.sec, depthInfo->header.stamp.nsec/1000);
                                }
                                
                                
                                
                                
                                
                                
                                //fprintf(depthtimeFile,"%010d.%06d ",depthInfo->header.stamp.sec, depthInfo->header.stamp.nsec/1000);
                                //fprintf(depthInternaltimeFile,"%010d.%09d ",depthInfo->binning_x, depthInfo->binning_y);
                                
                                /*
                                if (depthInfo->binning_x!=0) {  
                                        unsigned long internaldepth_time = (((unsigned long) depthInfo->binning_y) << 32) | depthInfo->binning_x;
                                        fprintf(depthtimeFile,"%ld \n",internaldepth_time);
                                }
                                */
                                
                                fprintf(depthtimeFile,"depth/frame%06d.png\n", count_depthinfo);
                                
                                fflush(depthtimeFile);
                                
                                
                                
                                ++count_depthinfo;
                                
                                if (count_depthinfo < minCount) {
                                        minCount = count_depthinfo;
                                }
                        }
                }

                if (configData.wantsThermal) {
                        if ((m.getTopic() == configData.thermalTopic) && ((count_thermal <= configData.maxFrames) || (configData.maxFrames == 0))) {
                                thermalImg = m.instantiate<sensor_msgs::Image>();

                                sprintf(outputFilenameThermal, "%s/thermal/frame%06d.%s", configData.outputFolder.c_str(), count_thermal, "png");
                                
                                //ROS_INFO("Image goes to (%s)", outputFilenameThermal);

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

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

                                if (rawThermal.type() == CV_16UC3) {
                                        cv::cvtColor(rawThermal, grayThermal, CV_RGB2GRAY);
                                } else {
                                        grayThermal = cv::Mat(rawThermal);
                                }
                                
                                if (configData.undistortThermal) {
                                        remap(grayThermal, writeThermal, map1, map2, INTER_LINEAR, BORDER_CONSTANT, 0);
                                } else {
                                        writeThermal = grayThermal;
                                }
                                
                                if (configData.debugMode) {
                                        adaptiveDownsample(writeThermal, dispThermal);
                                        
                                        cv::imshow("thermal", dispThermal);
                                        cv::waitKey(1);                 
                                }

                                
                                cv::imwrite(outputFilenameThermal,writeThermal);
                                //fprintf(thermaltimeFile,"%ld \n",thermalImg->header.stamp.toNSec());
                                
                                if (configData.recordDuplicates) {
                                        int equalityScore = -1;
                                
                                        if ((matricesAreEqual(oldThermal, rawThermal) || (rawThermal.rows != oldThermal.rows))) {
                                                equalityScore = 1;
                                        } else {
                                                equalityScore = 0;
                                        }
                                        
                                        rawThermal.copyTo(oldThermal);
                                        
                                        fprintf(thermalDuplicatesFile,"%d\n", equalityScore);
                                        fflush(thermalDuplicatesFile);
                                }
                                
                                
                                
                                
                                //fflush(thermaltimeFile);
                                ++count_thermal;
                                
                                if (count_thermal < minCount) {
                                        minCount = count_thermal;
                                }
                        }
                        
                        
                        if ((m.getTopic() == configData.thermalInfoTopic) && ((count_thermalinfo <= configData.maxFrames) || (configData.maxFrames == 0))) {
                          thermalInfo = m.instantiate<sensor_msgs::CameraInfo>();

                                
                                
                                
                                if (configData.internalTimestamp) {
                                        
                                        uint64_t internal_time;
                                        memcpy(&internal_time, &thermalInfo->R[0], 8);
                                
                                        uint32_t internal_sec, internal_nsec;
                                
                                        internal_sec = internal_time / 1000000;
                                        internal_nsec = (internal_time % 1000000) * 1000;
                                        
                                        if (configData.rawTimeFormat) {
                                                fprintf(thermalInternaltimeFile,"%ld ", internal_time);
                                        } else {
                                                fprintf(thermalInternaltimeFile,"%010u.%09u ", internal_sec, internal_nsec);
                                        }
                                        
                                        
                                        
                                        fprintf(thermalInternaltimeFile,"thermal/frame%06d.png\n", count_thermalinfo);
                                
                                        fflush(thermalInternaltimeFile);
                                }
                                
                                if (configData.rawTimeFormat) {
                                        fprintf(thermaltimeFile,"%ld ",thermalInfo->header.stamp.toNSec());
                                } else {
                                        fprintf(thermaltimeFile,"%010d.%06d ",thermalInfo->header.stamp.sec, thermalInfo->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(thermaltimeFile,"thermal/frame%06d.png\n", count_thermalinfo);
                                
                                fflush(thermaltimeFile);
                                
                                
                                
                                
                                if (configData.thermistorReading) {
                                        float thermistorVal;
                                
                                        memcpy(&thermistorVal, &thermalInfo->binning_x, 4);
                                        
                                        ofs_thermistor_log << thermistorVal << endl;
                                }
                                
                                
                                ++count_thermalinfo;
                                
                                if (count_thermalinfo < minCount) {
                                        minCount = count_thermalinfo;
                                }
                                
                                if ((count_thermalinfo > 0) && (count_thermalinfo % 100 == 0)) {
                                        ROS_INFO("At least (%d) sets of messages extracted..", count_thermalinfo);
                                }
                        }
                }

                if ((minCount >= configData.maxFrames) && (configData.maxFrames > 0)) {
                        ROS_WARN("Breaking loop because frame count limit (%d) has been broken", configData.maxFrames);
                        break;
                }

          }
          
          bag[zzz].close();
                
        }
        
        
  
  stillListening = false;
  
}

float** extractorNode::ApplyBilateralFilter(float** data, const int width, const int height, float sigmaP, float sigmaR)
{
  float z0, z1;
  float sigmaPSq = sigmaP*sigmaP;
  float sigmaRSq = sigmaR*sigmaR;
 
  float maxSqRangeDif = .2*.2;
  //precision to 1 cm
  int precision = 100000;
  int numGausVals = precision*maxSqRangeDif;
  float gausApprox[numGausVals];

  for(int i = 0; i < numGausVals; ++i) {
    float value = (float(i)/numGausVals) * maxSqRangeDif;
    gausApprox[i] = exp(-value/sigmaRSq);
  }

  float** result = new float*[height];
  float** Wp = new float*[height];
  for(int i = 0; i < height; ++i){
    result[i] = new float[width];
    Wp[i] = new float[width];
    for(int j = 0; j < width; ++j) {
      //initiate on self, assures nan's represented by -1 remain
      result[i][j] = data[i][j];
      Wp[i][j] = 1;
    }
  }

  float pixGausDist[FILTER_DISTANCE][FILTER_DISTANCE],sqRangeDist,totGausDist;

  for(int v = 0; v < FILTER_DISTANCE; ++v)
    for(int w = 0; w < FILTER_DISTANCE; ++w){
      pixGausDist[v][w] =  exp(-(v*v +w*w)/(sigmaPSq));
    }

  //lose unfit data, apply bilateral filtering, store results in writeData
  for(int i = 0; i < height; ++i)
    for(int j = 0; j < width; ++j){
      z0 = data[i][j];
      //if the point exists
      if(z0 > 0 ){
        for(int v = 0; v < FILTER_DISTANCE; ++v)
          for(int w = 0; w < FILTER_DISTANCE; ++w) {
            if((v != 0 || w != 0) && (i+v)<height && (j+w)<width) {
              z1 = data[i+v][j+w];
              if(z1 > 0) {
                //calculate gausian point distance
                sqRangeDist = (z0-z1)*(z0-z1);
                //ranges that differ more than .5m do not provide additional information
                if(sqRangeDist < maxSqRangeDif) {
                  //realGausDist = exp(-realEDist/(sigmaRSq));
                  //totGausDist = pixGausDist[v][w]*realGausDist;
                  totGausDist = gausApprox[(int)(sqRangeDist*precision)];
                  result[i][j] += totGausDist*z1;
                  Wp[i][j] += totGausDist;
                  //gausian values of the opposite pixel will be equal
                  result[i+v][j+w] += totGausDist*z0;
                  Wp[i+v][j+w] += totGausDist;
                }
              }
            }
          }
      }
    }

  for(int i = 0; i < height; ++i){
    for(int j = 0; j < width; ++j)
      result[i][j] = result[i][j]/Wp[i][j];
    delete[] Wp[i];
  }
  delete Wp;

  return result;
}