thermalvis: thermalvis: listener.cpp Source File

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