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
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
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) {
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
00425
00426
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
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
00520
00521
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
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
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
00587
00588
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
00619 cv_bridge::CvImagePtr cv_ptr_depth;
00620 try {
00621 cv_ptr_depth = cv_bridge::toCvCopy(depthImg, "16UC1");
00622
00623
00624
00625
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
00653
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
00701
00702
00703
00704
00705
00706
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
00731
00732
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
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
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
00831
00832
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
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
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
00918 for(int i = 0; i < height; ++i)
00919 for(int j = 0; j < width; ++j){
00920 z0 = data[i][j];
00921
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
00929 sqRangeDist = (z0-z1)*(z0-z1);
00930
00931 if(sqRangeDist < maxSqRangeDif) {
00932
00933
00934 totGausDist = gausApprox[(int)(sqRangeDist*precision)];
00935 result[i][j] += totGausDist*z1;
00936 Wp[i][j] += totGausDist;
00937
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 }