extractor.cpp
Go to the documentation of this file.
00001 
00005 #include "extractor.hpp"
00006 
00007 int main(int argc, char** argv) {
00008         
00009 
00010         ROS_INFO("Node launched.");
00011         
00012         ros::init(argc, argv, "extractor");
00013         
00014         ros::NodeHandle private_node_handle("~");
00015         
00016         extractorData startupData;
00017         
00018         startupData.read_addr = argv[0];
00019         startupData.read_addr = startupData.read_addr.substr(0, startupData.read_addr.size()-11);
00020                 
00021         bool inputIsValid = startupData.obtainStartingData(private_node_handle);
00022         
00023         if (!inputIsValid) {
00024                 ROS_ERROR("Configuration invalid.");
00025         }
00026                 
00027         ROS_INFO("Startup data processed.");
00028         
00029         
00030         ros::NodeHandle nh;
00031         
00032         extractorNode extractor_node(nh, startupData);
00033         
00034         ROS_INFO("Node configured.");
00035         
00036         
00037         while (extractor_node.isStillListening()) {     
00038                 extractor_node.extractBagFile();
00039         }
00040         
00041         return 0;
00042         
00043 }
00044 
00045 bool extractorData::obtainStartingData(ros::NodeHandle& nh) {
00046         
00047         wantsDepth = false;
00048         wantsRgb = false;
00049         wantsThermal = false;
00050         
00051         rawTimeFormat = true;
00052         
00053         nh.param<bool>("setupForMapping", setupForMapping, false);
00054         
00055         nh.param<bool>("splitBags", splitBags, false);
00056         
00057         nh.param<std::string>("bagSource", bagSource, "bagSource");
00058         nh.param<std::string>("outputFolder", outputFolder, "outputFolder");
00059         
00060         if (outputFolder == "outputFolder") {
00061                 if (splitBags) {
00062                         outputFolder = bagSource.substr(0, bagSource.size());
00063                 } else {
00064                         outputFolder = bagSource.substr(0, bagSource.size()-4);
00065                 }
00066         }
00067         
00068         outputFolder = outputFolder + "_data";
00069         
00070         nh.param<int>("maxFrames", maxFrames, 10000);
00071         
00072         nh.param<bool>("debugMode", debugMode, false);
00073         nh.param<bool>("verboseMode", verboseMode, false);
00074         
00075         nh.param<std::string>("rgbTopic", rgbTopic, "rgb");
00076         nh.param<std::string>("depthTopic", depthTopic, "depth");
00077         nh.param<std::string>("thermalTopic", thermalTopic, "thermal");
00078         
00079         if (rgbTopic != "rgb") {
00080                 rgbInfoTopic = rgbTopic.substr(0, rgbTopic.find_last_of("/") + 1) + "camera_info";
00081                 if (verboseMode) { ROS_INFO("Assuming rgbInfoTopic is (%s)", rgbInfoTopic.c_str()); }
00082                 wantsRgb = true;
00083         }
00084         
00085         if (thermalTopic != "thermal") {
00086                 thermalInfoTopic = thermalTopic.substr(0, thermalTopic.find_last_of("/") + 1) + "camera_info";
00087                 if (verboseMode) { ROS_INFO("Assuming thermalInfoTopic is (%s)", thermalInfoTopic.c_str()); }
00088                 wantsThermal = true;
00089         }
00090         
00091         if (depthTopic != "depth") {
00092                 depthInfoTopic = depthTopic.substr(0, depthTopic.find_last_of("/") + 1) + "camera_info";
00093                 if (verboseMode) { ROS_INFO("Assuming depthInfoTopicrosc is (%s)", depthInfoTopic.c_str()); }
00094                 wantsDepth = true;
00095         }
00096         
00097         nh.param<std::string>("thermalIntrinsics", thermalIntrinsics, "thermalIntrinsics");
00098         nh.param<bool>("undistortThermal", undistortThermal, true);
00099 
00100 
00101         if (undistortThermal) {
00102                 if (thermalIntrinsics != "thermalIntrinsics") {
00103                         ROS_INFO("Thermal intrinsics at (%s) selected.", thermalIntrinsics.c_str());
00104                 } else {
00105                         ROS_ERROR("read_addr = (%s)", read_addr.c_str());
00106                         thermalIntrinsics = read_addr + "data/calibration/csiro-aslab/miricle-1.yml";
00107                         ROS_ERROR("No thermal intrinsics supplied. Defaulting to (%s)", thermalIntrinsics.c_str());
00108                 }
00109         }
00110         
00111         nh.param<bool>("internalTimestamp", internalTimestamp, false);
00112         nh.param<bool>("thermistorReading", thermistorReading, false);
00113         nh.param<bool>("recordDuplicates", recordDuplicates, false);
00114         
00115         return true;
00116 }
00117 
00118 
00119 bool extractorNode::isStillListening() {
00120                 return stillListening;
00121 }
00122 
00123 
00124 extractorNode::extractorNode(ros::NodeHandle& nh, extractorData startupData): 
00125   dataFile(NULL),
00126   timeFile(NULL),
00127   depthtimeFile(NULL),
00128   depthInternaltimeFile(NULL),
00129   imgtimeFile(NULL),
00130   imgInternaltimeFile(NULL),
00131   thermaltimeFile(NULL),
00132   thermalInternaltimeFile(NULL),
00133   count(0),
00134   filenum(0),
00135   skipFrames(skipFrames),
00136   maxFrames(maxFrames),
00137   d_fx(525),
00138   d_fy(525),
00139   d_cx(319.5),
00140   d_cy(239.5) {
00141 
00142         
00143         stillListening = true;
00144 
00145         
00146         configData = startupData;
00147         
00148         startTime = -1.0;
00149         
00150         
00151         
00152         
00153           
00154   outputFilename = (char*) malloc(256);
00155   outputFilenameThermal = (char*) malloc(256);
00156         outputdepth = (char*) malloc(256);
00157         //outputcloud = (char*) malloc(256);
00158         
00159 
00160     if (configData.wantsThermal) {
00161                 topics.push_back(startupData.thermalTopic);
00162                 topics.push_back(startupData.thermalInfoTopic);
00163                 if (configData.verboseMode) { ROS_INFO("Subscribing to (%s) and (%s)...", startupData.thermalTopic.c_str(), startupData.thermalInfoTopic.c_str()); }
00164         }
00165         
00166         if (configData.wantsRgb) {
00167                 topics.push_back(startupData.rgbTopic);
00168                 topics.push_back(startupData.rgbInfoTopic);
00169                 if (configData.verboseMode) { ROS_INFO("Subscribing to (%s) and (%s)...", startupData.rgbTopic.c_str(), startupData.rgbInfoTopic.c_str()); }
00170         }
00171         
00172         if (configData.wantsDepth) {
00173                 topics.push_back(startupData.depthTopic);
00174                 topics.push_back(startupData.depthInfoTopic);
00175                 if (configData.verboseMode) { ROS_INFO("Subscribing to (%s) and (%s)...", startupData.depthTopic.c_str(), startupData.depthInfoTopic.c_str()); }
00176         }
00177         
00178         
00179 
00180         count_rgb = 0 ; 
00181         count_depth = 0 ;
00182         count_rgbinfo = 0 ; 
00183         count_depthinfo = 0;
00184         count_thermal = 0 ;
00185         count_thermalinfo = 0 ;
00186 
00187         makeDirectories();
00188         
00189         if (configData.verboseMode) { ROS_INFO("Directories made."); }
00190         
00191         if (configData.thermistorReading) {
00192                 thermistorLogFile = startupData.outputFolder + "/thermistor.txt";
00193                 ROS_INFO("Opening file for thermistor data: (%s)\n", thermistorLogFile.c_str());
00194                 ofs_thermistor_log.open(thermistorLogFile.c_str());
00195         }
00196         
00197         
00198         //timePath << resultsDir << "time.txt";  
00199         
00200         if (configData.wantsThermal) {
00201                 thermaltimePath << startupData.outputFolder << "/" << "thermal.txt"; 
00202                 thermaltimeFile = fopen(thermaltimePath.str().c_str(), "w");
00203                 
00204                 if (configData.internalTimestamp) {
00205                         thermalInternaltimePath << startupData.outputFolder << "/" << "thermal-internal.txt"; 
00206                         thermalInternaltimeFile = fopen(thermalInternaltimePath.str().c_str(), "w");
00207                 }
00208                 
00209                 if (configData.recordDuplicates) {
00210                         thermalDuplicatesPath << startupData.outputFolder << "/" << "thermal-duplicates.txt"; 
00211                         thermalDuplicatesFile = fopen(thermalDuplicatesPath.str().c_str(), "w");
00212                 }
00213         }
00214         
00215         if (configData.wantsRgb) {
00216                 imgtimePath << startupData.outputFolder << "/" << "rgb.txt";            
00217                 imgtimeFile = fopen(imgtimePath.str().c_str(), "w");
00218                 
00219                 if (configData.internalTimestamp) {
00220                         imgInternaltimePath << startupData.outputFolder << "/" << "rgb-internal.txt"; 
00221                         imgInternaltimeFile = fopen(imgInternaltimePath.str().c_str(), "w");
00222                 }
00223                 
00224         }
00225         
00226         if (configData.wantsDepth) {
00227                 depthtimePath << startupData.outputFolder << "/" << "depth.txt";  
00228                 depthtimeFile = fopen(depthtimePath.str().c_str(), "w");
00229                 
00230                 if (configData.internalTimestamp) {
00231                         depthInternaltimePath << startupData.outputFolder << "/" << "depth-internal.txt"; 
00232                         depthInternaltimeFile = fopen(depthInternaltimePath.str().c_str(), "w");
00233                 }
00234         }
00235   
00236         char bagName[256];
00237         int idx = startupData.bagSource.find_last_of("/\\");
00238   
00239         sprintf(bagName, "%s", (startupData.bagSource.substr(idx+1, startupData.bagSource.length())).c_str());
00240   
00241         printf("%s << bagName = (%s)\n", __FUNCTION__, bagName);
00242   
00243         if (configData.wantsRgb) {
00244                 fprintf(imgtimeFile,"# color images\n");
00245                 fflush(imgtimeFile);
00246                 fprintf(imgtimeFile,"# file: '%s'\n", startupData.bagSource.c_str());
00247                 fflush(imgtimeFile);
00248                 fprintf(imgtimeFile,"# timestamp filename\n");
00249                 fflush(imgtimeFile);
00250                 
00251                 if (configData.internalTimestamp) {
00252                         fprintf(imgInternaltimeFile,"# color images\n");
00253                         fflush(imgInternaltimeFile);
00254                         fprintf(imgInternaltimeFile,"# file: '%s'\n", startupData.bagSource.c_str());
00255                         fflush(imgInternaltimeFile);
00256                         fprintf(imgInternaltimeFile,"# timestamp filename\n");
00257                         fflush(imgInternaltimeFile);
00258                 }
00259                 
00260                 
00261         }
00262         
00263         if (configData.wantsDepth) {
00264                 fprintf(depthtimeFile,"# depth images\n");
00265                 fflush(depthtimeFile);
00266                 fprintf(depthtimeFile,"# file: '%s'\n", startupData.bagSource.c_str());
00267                 fflush(depthtimeFile);
00268                 fprintf(depthtimeFile,"# timestamp filename\n");
00269                 fflush(depthtimeFile);
00270                 
00271                 if (configData.internalTimestamp) {
00272                         fprintf(depthInternaltimeFile,"# depth images\n");
00273                         fflush(depthInternaltimeFile);
00274                         fprintf(depthInternaltimeFile,"# file: '%s'\n", startupData.bagSource.c_str());
00275                         fflush(depthInternaltimeFile);
00276                         fprintf(depthInternaltimeFile,"# timestamp filename\n");
00277                         fflush(depthInternaltimeFile);
00278                 }
00279                 
00280         }
00281         
00282         if (configData.wantsThermal) {
00283                 fprintf(thermaltimeFile,"# thermal images\n");
00284                 fflush(thermaltimeFile);
00285                 fprintf(thermaltimeFile,"# file: '%s'\n", startupData.bagSource.c_str());
00286                 fflush(thermaltimeFile);
00287                 fprintf(thermaltimeFile,"# timestamp filename\n");
00288                 fflush(thermaltimeFile);
00289                 
00290                 if (configData.internalTimestamp) {
00291                         fprintf(thermalInternaltimeFile,"# thermal images\n");
00292                         fflush(thermalInternaltimeFile);
00293                         fprintf(thermalInternaltimeFile,"# file: '%s'\n", startupData.bagSource.c_str());
00294                         fflush(thermalInternaltimeFile);
00295                         fprintf(thermalInternaltimeFile,"# timestamp filename\n");
00296                         fflush(thermalInternaltimeFile);
00297                 }
00298                 
00299                 
00300                 
00301                 if (configData.recordDuplicates) {
00302                         fprintf(thermalDuplicatesFile,"# thermal duplicates\n");
00303                         fflush(thermalDuplicatesFile);
00304                         fprintf(thermalDuplicatesFile,"# file: '%s'\n", startupData.bagSource.c_str());
00305                         fflush(thermalDuplicatesFile);
00306                         fprintf(thermalDuplicatesFile,"# (is potentially a duplicate)\n");
00307                         fflush(thermalDuplicatesFile);
00308                 }
00309         }
00310                 
00311                 
00312         if (configData.verboseMode) { ROS_INFO("Here [152]\n");}
00313         
00314         if (configData.wantsThermal) {
00315                 if (configData.undistortThermal) {
00316                         
00317                         ROS_INFO("Reading in calibration data");
00318                         
00319                         if (configData.thermalIntrinsics[0] != '/') {
00320                                 configData.thermalIntrinsics = configData.read_addr + configData.thermalIntrinsics;
00321                         }
00322                         
00323                         ROS_INFO("thermal intrinsics = %s", configData.thermalIntrinsics.c_str());
00324                         
00325                         
00326                         FileStorage fs(configData.thermalIntrinsics, FileStorage::READ);
00327                         fs["imageSize"] >> imageSize;
00328                         fs["cameraMatrix"] >> cameraMatrix;
00329                         fs["distCoeffs"] >> distCoeffs;
00330                         fs["newCamMat"] >> newCamMat;
00331                         fs.release();
00332                         
00333                         ROS_INFO("Calibration data read.");
00334 
00335                         cameraSize = Size(imageSize.at<unsigned short>(0, 0), imageSize.at<unsigned short>(0, 1));
00336                         
00337                         ROS_INFO("Camera params determined.");
00338                         
00339                         initUndistortRectifyMap(cameraMatrix, distCoeffs,  Mat() , newCamMat, cameraSize, CV_32FC1, map1, map2);
00340                    
00341                         
00342                         
00343                 }
00344         }
00345         
00346         if (configData.splitBags) {
00347                 
00348                 
00349                 DIR * dirp;
00350                 struct dirent * entry;
00351                 
00352                 dirp = opendir(configData.bagSource.c_str());
00353                 
00354                 if (dirp == NULL) {
00355                         ROS_ERROR("Opening of directory (%s) failed.", configData.bagSource.c_str());
00356                         return;
00357                 }
00358 
00359                 while ((entry = readdir(dirp)) != NULL) {
00360                         if (entry->d_type == DT_REG) { // If the entry is a regular file
00361                                 splitBagNames.push_back(string(entry->d_name));
00362                         }
00363                 }
00364 
00365                 closedir(dirp);
00366 
00367                 sort(splitBagNames.begin(), splitBagNames.end());
00368 
00369                 if(splitBagNames.size() <= 0)   {
00370                         ROS_ERROR("File counting error.");
00371                         return;
00372                 }
00373 
00374                 ROS_INFO("No. of bags in folder = %d", splitBagNames.size());
00375                 
00376                 for (unsigned int iii = 0; iii < splitBagNames.size(); iii++) {
00377                         ROS_INFO("Bag(%d) = (%s)", iii, splitBagNames.at(iii).c_str());
00378                 }
00379                 
00380                 ROS_INFO("Prepared for split bags (%s)", bagName);
00381 
00382         } else {
00383                 ROS_INFO("Prepared for single bag (%s)", bagName);
00384                 splitBagNames.push_back(bagName);
00385         }
00386         
00387 }
00388 
00389 extractorNode::~extractorNode() {
00390   if(dataFile)
00391     fclose(dataFile);
00392   if(timeFile)
00393     fclose(timeFile);
00394     
00395     if (configData.wantsRgb) {
00396                 if(imgtimeFile) fclose(imgtimeFile);
00397                 if(imgInternaltimeFile) fclose(imgInternaltimeFile);
00398         }
00399         
00400         if (configData.wantsDepth) {
00401                 if(depthtimeFile) fclose(depthtimeFile);
00402                 if(depthInternaltimeFile) fclose(depthInternaltimeFile);
00403         }
00404         
00405         if (configData.wantsThermal) {
00406                 if(thermaltimeFile) fclose(thermaltimeFile);
00407                 if(thermalInternaltimeFile) fclose(thermalInternaltimeFile);
00408                 
00409                 if (configData.recordDuplicates) {
00410                         if(thermalDuplicatesFile) fclose(thermalDuplicatesFile);  
00411                 }
00412                 
00413         }
00414         
00415         if (configData.thermistorReading) {
00416                 if (ofs_thermistor_log.is_open()) ofs_thermistor_log.close();
00417         }
00418     
00419         
00420 }
00421 
00422 void extractorNode::serverCallback(thermalvis::extractorConfig &config, uint32_t level) {
00423             
00424         //configData.maxFrames = config.maxFrames;
00425         //configData.debugMode = config.debugMode;
00426         // no point putting anything in here coz there is no ROS spinning...?
00427         
00428 }
00429 
00430 void extractorNode::makeDirectories() {
00431         
00432         char buffer[256];
00433         sprintf(buffer,"mkdir -p %s",configData.outputFolder.c_str());
00434         system(buffer);
00435         
00436         if (configData.setupForMapping) {
00437                 sprintf(buffer,"mkdir -p %s/%s",configData.outputFolder.c_str(),"meshes/");
00438                 system(buffer);
00439                 sprintf(buffer,"mkdir -p %s/%s",configData.outputFolder.c_str(),"worlds/");
00440                 system(buffer);
00441                 sprintf(buffer,"mkdir -p %s/%s",configData.outputFolder.c_str(),"emeshes/");
00442                 system(buffer);
00443                 sprintf(buffer,"mkdir -p %s/%s",configData.outputFolder.c_str(),"cmeshes/");
00444                 system(buffer);
00445                 sprintf(buffer,"mkdir -p %s/%s",configData.outputFolder.c_str(),"smeshes/");
00446                 system(buffer);
00447                 sprintf(buffer,"mkdir -p %s/%s",configData.outputFolder.c_str(),"poses/");
00448                 system(buffer);
00449                 sprintf(buffer,"mkdir -p %s/%s",configData.outputFolder.c_str(),"cloud/");
00450                 system(buffer);
00451         }
00452         
00453         if (configData.wantsRgb) {
00454                 sprintf(buffer,"mkdir -p %s/%s",configData.outputFolder.c_str(),"rgb/");
00455                 system(buffer);
00456         }
00457         
00458         if (configData.wantsThermal) {
00459                 sprintf(buffer,"mkdir -p %s/%s",configData.outputFolder.c_str(),"thermal/");
00460                 system(buffer);
00461         }
00462         
00463         if (configData.wantsDepth) {
00464                 sprintf(buffer,"mkdir -p %s/%s",configData.outputFolder.c_str(),"depth/");
00465                 system(buffer);
00466         }
00467         
00468         char buffer_check[256];
00469         sprintf(buffer_check,"%s", &buffer[9]);
00470         
00471         if (configData.verboseMode) { ROS_INFO("Checking that last directory (%s) has been created..", buffer_check); }
00472         
00473         while (!boost::filesystem::exists( buffer_check ) ) {
00474                 
00475         }
00476         
00477 }
00478 
00479 void extractorNode::extractBagFile()
00480 {
00481         
00482         for (unsigned int zzz = 0; zzz < splitBagNames.size(); zzz++) {
00483                 
00484                 string currentBagName;
00485                         
00486                 
00487                 if (configData.splitBags) {
00488                         currentBagName = configData.bagSource + "/" + splitBagNames.at(zzz);
00489                         bag[zzz].open(currentBagName, rosbag::bagmode::Read);
00490                 } else {
00491                         currentBagName = configData.bagSource;
00492                         bag[zzz].open(configData.bagSource, rosbag::bagmode::Read);
00493                 }
00494                 
00495                 ROS_INFO("Currently processing bag(%d) = (%s)...", zzz, currentBagName.c_str());
00496                 
00497                 
00498   
00499                 view = new rosbag::View (bag[zzz], rosbag::TopicQuery(topics));
00500 
00501                 uint32_t msg_size = view->size() ;
00502                 
00503                 if (configData.verboseMode) { ROS_INFO("the msg size is %d",msg_size); }
00504                 
00505                 int minCount = 0;
00506          
00507                 BOOST_FOREACH(rosbag::MessageInstance const m, *view) {
00508                   
00509                 minCount = 99999;
00510 
00511                 if (configData.wantsRgb) {
00512                         if ((m.getTopic() == configData.rgbTopic) && ((count_rgb <= configData.maxFrames) || (configData.maxFrames == 0))) {
00513 
00514                                 //ROS_WARN("Processing RGB topic...");
00515 
00516                                 rgbImg = m.instantiate<sensor_msgs::Image>();
00517                                 sprintf(outputFilename, "%s/rgb/frame%06d.%s", configData.outputFolder.c_str(), count_rgb, "png");
00518 
00519                                 //ROS_INFO("Image goes to (%s)", outputFilename);
00520 
00521                                 // copy sensor msg Image to opencv 
00522                                 cv_bridge::CvImagePtr cv_ptr;
00523                                 try {
00524                                 cv_ptr = cv_bridge::toCvCopy(rgbImg, "bgr8");
00525                                 } catch (cv_bridge::Exception& e) {
00526                                 ROS_ERROR("cv_bridge exception: %s", e.what());
00527                                 return;
00528                                 }
00529 
00530                                 if (configData.debugMode) {
00531                                 cv::imshow("rgb", cv_ptr->image);
00532                                 cv::waitKey(1);
00533                                 }
00534 
00535 
00536                                 cv::imwrite(outputFilename,cv_ptr->image);
00537                                 ++count_rgb ;
00538 
00539                                 if (count_rgb < minCount) {
00540                                 minCount = count_rgb;
00541                                 }
00542                         }
00543 
00544                         if ((m.getTopic() == configData.rgbInfoTopic) && ((count_rgbinfo <= configData.maxFrames) || (configData.maxFrames == 0))) {
00545 
00546                                 //ROS_WARN("Processing RGB info topic...");
00547 
00548 
00549 
00550 
00551                                 rgbInfo = m.instantiate<sensor_msgs::CameraInfo>();
00552                                 
00553                                 if (configData.internalTimestamp) {
00554                                         uint64_t internal_time;
00555                                         memcpy(&internal_time, &rgbInfo->R[0], 8);
00556 
00557                                         uint32_t internal_sec, internal_nsec;
00558 
00559                                         internal_sec = internal_time / 1000000;
00560                                         internal_nsec = (internal_time % 1000000) * 1000;
00561                                         
00562                                         if (configData.rawTimeFormat) {
00563                                                 fprintf(imgInternaltimeFile,"%ld ", internal_time);
00564                                         } else {
00565                                                 fprintf(imgInternaltimeFile,"%010d.%09d ", internal_sec, internal_nsec);
00566                                         }
00567                                         
00568                                         
00569                                         fprintf(imgInternaltimeFile,"rgb/frame%06d.png\n", count_rgbinfo);
00570 
00571                                         fflush(imgInternaltimeFile);
00572                                 }
00573 
00574                                 //fprintf(imgtimeFile,"%ld ",rgbInfo->header.stamp.toNSec());
00575 
00576                                 
00577                                 if (configData.rawTimeFormat) {
00578                                         fprintf(imgtimeFile,"%ld ",rgbInfo->header.stamp.toNSec());
00579                                 } else {
00580                                         fprintf(imgtimeFile,"%010d.%06d ",rgbInfo->header.stamp.sec, rgbInfo->header.stamp.nsec/1000);
00581                                 }
00582                                 
00583                                 
00584 
00585                                 /*
00586                                 if (rgbInfo->binning_x!=0) {    
00587                                 unsigned long internalimg_time = (((unsigned long) rgbInfo->binning_y) << 32) | rgbInfo->binning_x;
00588                                 fprintf(imgtimeFile,"%ld \n",internalimg_time);
00589                                 }
00590                                 */
00591 
00592                                 fprintf(imgtimeFile,"rgb/frame%06d.png\n", count_rgbinfo);
00593 
00594                                 fflush(imgtimeFile);
00595 
00596                                 
00597 
00598                                 ++count_rgbinfo;
00599 
00600                                 if (count_rgbinfo < minCount) {
00601                                         minCount = count_rgbinfo;
00602                                 }
00603                         }
00604                 }
00605                 
00606                 
00607                 if (configData.wantsDepth) {
00608                         if ((m.getTopic() == configData.depthTopic) && ((count_depth <= configData.maxFrames) || (configData.maxFrames == 0))) {
00609                                 
00610                                 depthImg = m.instantiate<sensor_msgs::Image>();
00611                                 
00612                                 sprintf(outputdepth, "%s/depth/frame%06d.%s", configData.outputFolder.c_str(), count_depth, "png");
00613 
00614                                 if (startTime == -1.0) {
00615                                         startTime = depthImg->header.stamp.toSec();
00616                                 }
00617                                 
00618                                 // copy sensor msg Image to opencv 
00619                                 cv_bridge::CvImagePtr cv_ptr_depth;
00620                                 try {
00621                                         cv_ptr_depth = cv_bridge::toCvCopy(depthImg, "16UC1");
00622                                         // but says 32FC1 ... :(
00623                                         // http://answers.ros.org/question/10591/publish-kinect-depth-information-to-a-ros-topic/
00624                                         // cv_ptr_depth_ = cv_bridge::toCvCopy(depth_image_msgs, sensor_msgs::image_encodings::TYPE_32FC1);
00625                                         // cv_image = self.bridge.imgmsg_to_cv(data, "32FC1")
00626                                 } catch (cv_bridge::Exception& e) {
00627                                         ROS_ERROR("cv_bridge exception: %s", e.what());
00628                                         return;
00629                                 }
00630 
00631                                 cv::Mat rawDepth;
00632                                 rawDepth = cv::Mat(cv_ptr_depth->image);
00633                                 
00634                                 for (unsigned int iii = 0; iii < rawDepth.rows; iii++) {
00635                                         for (unsigned int jjj = 0; jjj < rawDepth.cols; jjj++) {
00636                                                 if (5.0 * double(rawDepth.at<unsigned short>(iii,jjj)) > 65535.0) {
00637                                                         rawDepth.at<unsigned short>(iii,jjj) = 0;
00638                                                 } else {
00639                                                         rawDepth.at<unsigned short>(iii,jjj) = 5 * rawDepth.at<unsigned short>(iii,jjj);
00640                                                 }
00641                                                 
00642                                         }
00643                                 }
00644                                 
00645                                 if (configData.debugMode) {
00646                                         cv::imshow("depth", rawDepth*5);
00647                                         cv::waitKey(1);
00648                                 }
00649                                 
00650 
00651                                 cv::imwrite(outputdepth,rawDepth);
00652                                 //fprintf(depthtimeFile,"%ld \n",depthImg->header.stamp.toNSec());
00653                                 //fflush(depthtimeFile);
00654                                 
00655                                 ++count_depth;
00656                                 
00657                                 if (count_depth < minCount) {
00658                                         minCount = count_depth;
00659                                 }
00660                         }
00661                 
00662                 
00663                         if ((m.getTopic() == configData.depthInfoTopic) && ((count_depthinfo <= configData.maxFrames) || (configData.maxFrames == 0))) {
00664                                 depthInfo = m.instantiate<sensor_msgs::CameraInfo>();
00665                                 
00666                                 if (configData.internalTimestamp) {
00667                                         uint64_t internal_time;
00668                                         memcpy(&internal_time, &depthInfo->R[0], 8);
00669                                         
00670                                         uint32_t internal_sec, internal_nsec;
00671                                         
00672                                         internal_sec = internal_time / 1000000;
00673                                         internal_nsec = (internal_time % 1000000) * 1000;
00674                                         
00675                                         if (configData.rawTimeFormat) {
00676                                                 fprintf(depthInternaltimeFile,"%ld ", internal_time);
00677                                         } else {
00678                                                 fprintf(depthInternaltimeFile,"%010d.%09d ", internal_sec, internal_nsec);
00679                                         }
00680                                         
00681                                         
00682                                         
00683                                         fprintf(depthInternaltimeFile,"depth/frame%06d.png\n", count_depthinfo);
00684                                 
00685                                         fflush(depthInternaltimeFile);
00686                                 }
00687 
00688                                 if (configData.rawTimeFormat) {
00689                                         fprintf(depthtimeFile,"%ld ",depthInfo->header.stamp.toNSec());
00690                                 } else {
00691                                         fprintf(depthtimeFile,"%010d.%06d ",depthInfo->header.stamp.sec, depthInfo->header.stamp.nsec/1000);
00692                                 }
00693                                 
00694                                 
00695                                 
00696                                 
00697                                 
00698                                 
00700                                 //fprintf(depthtimeFile,"%010d.%06d ",depthInfo->header.stamp.sec, depthInfo->header.stamp.nsec/1000);
00701                                 //fprintf(depthInternaltimeFile,"%010d.%09d ",depthInfo->binning_x, depthInfo->binning_y);
00702                                 
00703                                 /*
00704                                 if (depthInfo->binning_x!=0) {  
00705                                         unsigned long internaldepth_time = (((unsigned long) depthInfo->binning_y) << 32) | depthInfo->binning_x;
00706                                         fprintf(depthtimeFile,"%ld \n",internaldepth_time);
00707                                 }
00708                                 */
00709                                 
00710                                 fprintf(depthtimeFile,"depth/frame%06d.png\n", count_depthinfo);
00711                                 
00712                                 fflush(depthtimeFile);
00713                                 
00714                                 
00715                                 
00716                                 ++count_depthinfo;
00717                                 
00718                                 if (count_depthinfo < minCount) {
00719                                         minCount = count_depthinfo;
00720                                 }
00721                         }
00722                 }
00723 
00724                 if (configData.wantsThermal) {
00725                         if ((m.getTopic() == configData.thermalTopic) && ((count_thermal <= configData.maxFrames) || (configData.maxFrames == 0))) {
00726                                 thermalImg = m.instantiate<sensor_msgs::Image>();
00727 
00728                                 sprintf(outputFilenameThermal, "%s/thermal/frame%06d.%s", configData.outputFolder.c_str(), count_thermal, "png");
00729                                 
00730                                 //ROS_INFO("Image goes to (%s)", outputFilenameThermal);
00731 
00732                                 // copy sensor msg Image to opencv 
00733                                 cv_bridge::CvImagePtr cv_ptr_thermal;
00734                                 try {
00735                                         cv_ptr_thermal = cv_bridge::toCvCopy(thermalImg, "bgr8");
00736                                 } catch (cv_bridge::Exception& e) {
00737                                         ROS_ERROR("cv_bridge exception: %s", e.what());
00738                                         return;
00739                                 }
00740 
00741                                 cv::Mat rawThermal, grayThermal, dispThermal, writeThermal;
00742                                 rawThermal = cv::Mat(cv_ptr_thermal->image);
00743 
00744                                 if (rawThermal.type() == CV_16UC3) {
00745                                         cv::cvtColor(rawThermal, grayThermal, CV_RGB2GRAY);
00746                                 } else {
00747                                         grayThermal = cv::Mat(rawThermal);
00748                                 }
00749                                 
00750                                 if (configData.undistortThermal) {
00751                                         remap(grayThermal, writeThermal, map1, map2, INTER_LINEAR, BORDER_CONSTANT, 0);
00752                                 } else {
00753                                         writeThermal = grayThermal;
00754                                 }
00755                                 
00756                                 if (configData.debugMode) {
00757                                         adaptiveDownsample(writeThermal, dispThermal);
00758                                         
00759                                         cv::imshow("thermal", dispThermal);
00760                                         cv::waitKey(1);                 
00761                                 }
00762 
00763                                 
00764                                 cv::imwrite(outputFilenameThermal,writeThermal);
00765                                 //fprintf(thermaltimeFile,"%ld \n",thermalImg->header.stamp.toNSec());
00766                                 
00767                                 if (configData.recordDuplicates) {
00768                                         int equalityScore = -1;
00769                                 
00770                                         if ((matricesAreEqual(oldThermal, rawThermal) || (rawThermal.rows != oldThermal.rows))) {
00771                                                 equalityScore = 1;
00772                                         } else {
00773                                                 equalityScore = 0;
00774                                         }
00775                                         
00776                                         rawThermal.copyTo(oldThermal);
00777                                         
00778                                         fprintf(thermalDuplicatesFile,"%d\n", equalityScore);
00779                                         fflush(thermalDuplicatesFile);
00780                                 }
00781                                 
00782                                 
00783                                 
00784                                 
00785                                 //fflush(thermaltimeFile);
00786                                 ++count_thermal;
00787                                 
00788                                 if (count_thermal < minCount) {
00789                                         minCount = count_thermal;
00790                                 }
00791                         }
00792                         
00793                         
00794                         if ((m.getTopic() == configData.thermalInfoTopic) && ((count_thermalinfo <= configData.maxFrames) || (configData.maxFrames == 0))) {
00795                           thermalInfo = m.instantiate<sensor_msgs::CameraInfo>();
00796 
00797                                 
00798                                 
00799                                 
00800                                 if (configData.internalTimestamp) {
00801                                         
00802                                         uint64_t internal_time;
00803                                         memcpy(&internal_time, &thermalInfo->R[0], 8);
00804                                 
00805                                         uint32_t internal_sec, internal_nsec;
00806                                 
00807                                         internal_sec = internal_time / 1000000;
00808                                         internal_nsec = (internal_time % 1000000) * 1000;
00809                                         
00810                                         if (configData.rawTimeFormat) {
00811                                                 fprintf(thermalInternaltimeFile,"%ld ", internal_time);
00812                                         } else {
00813                                                 fprintf(thermalInternaltimeFile,"%010u.%09u ", internal_sec, internal_nsec);
00814                                         }
00815                                         
00816                                         
00817                                         
00818                                         fprintf(thermalInternaltimeFile,"thermal/frame%06d.png\n", count_thermalinfo);
00819                                 
00820                                         fflush(thermalInternaltimeFile);
00821                                 }
00822                                 
00823                                 if (configData.rawTimeFormat) {
00824                                         fprintf(thermaltimeFile,"%ld ",thermalInfo->header.stamp.toNSec());
00825                                 } else {
00826                                         fprintf(thermaltimeFile,"%010d.%06d ",thermalInfo->header.stamp.sec, thermalInfo->header.stamp.nsec/1000);
00827                                 }
00828 
00829                                 /*
00830                                 if (thermalInfo->binning_x!=0) {        
00831                                         unsigned long internalimg_time = (((unsigned long) thermalInfo->binning_y) << 32) | thermalInfo->binning_x;
00832                                         fprintf(thermaltimeFile,"%ld \n",internalimg_time);
00833                                 }
00834                                 */
00835                                 
00836                                 fprintf(thermaltimeFile,"thermal/frame%06d.png\n", count_thermalinfo);
00837                                 
00838                                 fflush(thermaltimeFile);
00839                                 
00840                                 
00841                                 
00842                                 
00843                                 if (configData.thermistorReading) {
00844                                         float thermistorVal;
00845                                 
00846                                         memcpy(&thermistorVal, &thermalInfo->binning_x, 4);
00847                                         
00848                                         ofs_thermistor_log << thermistorVal << endl;
00849                                 }
00850                                 
00851                                 
00852                                 ++count_thermalinfo;
00853                                 
00854                                 if (count_thermalinfo < minCount) {
00855                                         minCount = count_thermalinfo;
00856                                 }
00857                                 
00858                                 if ((count_thermalinfo > 0) && (count_thermalinfo % 100 == 0)) {
00859                                         ROS_INFO("At least (%d) sets of messages extracted..", count_thermalinfo);
00860                                 }
00861                         }
00862                 }
00863 
00864                 if ((minCount >= configData.maxFrames) && (configData.maxFrames > 0)) {
00865                         ROS_WARN("Breaking loop because frame count limit (%d) has been broken", configData.maxFrames);
00866                         break;
00867                 }
00868 
00869           }
00870           
00871           bag[zzz].close();
00872                 
00873         }
00874         
00875         
00876   
00877   stillListening = false;
00878   
00879 }
00880 
00881 float** extractorNode::ApplyBilateralFilter(float** data, const int width, const int height, float sigmaP, float sigmaR)
00882 {
00883   float z0, z1;
00884   float sigmaPSq = sigmaP*sigmaP;
00885   float sigmaRSq = sigmaR*sigmaR;
00886  
00887   float maxSqRangeDif = .2*.2;
00888   //precision to 1 cm
00889   int precision = 100000;
00890   int numGausVals = precision*maxSqRangeDif;
00891   float gausApprox[numGausVals];
00892 
00893   for(int i = 0; i < numGausVals; ++i) {
00894     float value = (float(i)/numGausVals) * maxSqRangeDif;
00895     gausApprox[i] = exp(-value/sigmaRSq);
00896   }
00897 
00898   float** result = new float*[height];
00899   float** Wp = new float*[height];
00900   for(int i = 0; i < height; ++i){
00901     result[i] = new float[width];
00902     Wp[i] = new float[width];
00903     for(int j = 0; j < width; ++j) {
00904       //initiate on self, assures nan's represented by -1 remain
00905       result[i][j] = data[i][j];
00906       Wp[i][j] = 1;
00907     }
00908   }
00909 
00910   float pixGausDist[FILTER_DISTANCE][FILTER_DISTANCE],sqRangeDist,totGausDist;
00911 
00912   for(int v = 0; v < FILTER_DISTANCE; ++v)
00913     for(int w = 0; w < FILTER_DISTANCE; ++w){
00914       pixGausDist[v][w] =  exp(-(v*v +w*w)/(sigmaPSq));
00915     }
00916 
00917   //lose unfit data, apply bilateral filtering, store results in writeData
00918   for(int i = 0; i < height; ++i)
00919     for(int j = 0; j < width; ++j){
00920       z0 = data[i][j];
00921       //if the point exists
00922       if(z0 > 0 ){
00923         for(int v = 0; v < FILTER_DISTANCE; ++v)
00924           for(int w = 0; w < FILTER_DISTANCE; ++w) {
00925             if((v != 0 || w != 0) && (i+v)<height && (j+w)<width) {
00926               z1 = data[i+v][j+w];
00927               if(z1 > 0) {
00928                 //calculate gausian point distance
00929                 sqRangeDist = (z0-z1)*(z0-z1);
00930                 //ranges that differ more than .5m do not provide additional information
00931                 if(sqRangeDist < maxSqRangeDif) {
00932                   //realGausDist = exp(-realEDist/(sigmaRSq));
00933                   //totGausDist = pixGausDist[v][w]*realGausDist;
00934                   totGausDist = gausApprox[(int)(sqRangeDist*precision)];
00935                   result[i][j] += totGausDist*z1;
00936                   Wp[i][j] += totGausDist;
00937                   //gausian values of the opposite pixel will be equal
00938                   result[i+v][j+w] += totGausDist*z0;
00939                   Wp[i+v][j+w] += totGausDist;
00940                 }
00941               }
00942             }
00943           }
00944       }
00945     }
00946 
00947   for(int i = 0; i < height; ++i){
00948     for(int j = 0; j < width; ++j)
00949       result[i][j] = result[i][j]/Wp[i][j];
00950     delete[] Wp[i];
00951   }
00952   delete Wp;
00953 
00954   return result;
00955 }


thermalvis
Author(s): Stephen Vidas
autogenerated on Sun Jan 5 2014 11:38:44