/** * \brief Internal conversion function, writes given timestamp to output file stream * \param timestamp timestamp for current message * \param stream Output file stream * */ void CBlfLogConverter::WriteTimestamp(ULONGLONG timestamp, std::ofstream& stream) const { // Be careful: in this function milliseconds uses for 10^-4 sec ULONGLONG hours = timestamp; hours /= 100000; ULONGLONG millisecond = hours % 10000; hours /= 10000; ULONGLONG seconds = hours % 60; hours /= 60; ULONGLONG minutes = hours % 60; hours /= 60; stream << std::dec << hours <<":" << minutes << ":" << seconds << ":" ; // Outputs milliseconds with adjusting by zero stream.setf(ios::right, ios::adjustfield); stream.fill('0'); stream << std::setw(4)<< std::dec << millisecond; }
void saveImagesToDisk(ros::NodeHandle& nh, ros::Subscriber& sub) { //pcl::ScopeTime t1 ("save images"); { boost::mutex::scoped_lock lock(rgb_mutex_); memcpy( cv_rgb_.data, &rgb_data_[0], 3*rows_*cols_*sizeof(unsigned char)); new_rgb_ = false; } capture_->stop(); sub.shutdown(); sub = nh.subscribe("/camera/depth/image_raw", 1, &OpenNIShoter::frameDepthCallback, this); new_depth_ = false; capture_->start(); do { if (new_depth_ == true ) { boost::mutex::scoped_lock lock(depth_mutex_); memcpy( cv_depth_.data, &depth_data_[0], rows_*cols_*sizeof(uint16_t)); new_depth_ = false; break; } boost::this_thread::sleep (boost::posix_time::millisec (10)); } while (1); capture_->stop(); sub.shutdown(); sub = nh.subscribe("/camera/ir/image", 1, &OpenNIShoter::frameIRCallback, this); new_ir_ = false; capture_->start(); do { if (new_ir_ == true ) { boost::mutex::scoped_lock lock(ir_mutex_); memcpy( cv_ir_raw_.data, &ir_data_[0], rows_*cols_*sizeof(uint16_t)); cv_ir_raw_.convertTo(cv_ir_, CV_8U); cv::equalizeHist( cv_ir_, cv_ir_ ); int max = 0; for (int i=0; i< rows_; i++) { for (int j=0; j< cols_; j++) { if (ir_data_[i+j*rows_] > max) max = ir_data_[i+j*rows_]; } } std::cout << "max IR val: " << max << std::endl; new_ir_ = false; break; } boost::this_thread::sleep (boost::posix_time::millisec (10)); } while (1); capture_->stop(); sub.shutdown(); sub = nh.subscribe("/camera/rgb/image_raw", 1, &OpenNIShoter::frameRGBCallback, this); capture_->start(); sprintf(depth_file, "/Depth/%018ld.png", timestamp_depth_ ); sprintf(rgb_file, "/RGB/%018ld.png", timestamp_rgb_); sprintf(ir_file, "/IR/%018ld.png", timestamp_ir_); cv::imwrite( write_folder_ + depth_file, cv_depth_); cv::imwrite( write_folder_ + rgb_file, cv_rgb_); cv::imwrite( write_folder_ + ir_file, cv_ir_); sprintf(depth_file_PNG, "/Depth/%018ld.png", timestamp_depth_); sprintf(rgb_file_PNG, "/RGB/%018ld.png", timestamp_rgb_); sprintf(ir_file_PNG, "/IR/%018ld.png", timestamp_ir_); off_rgb_.width(18); off_rgb_.fill('0'); off_rgb_ << timestamp_rgb_; off_rgb_ << " " << rgb_file_PNG << std::endl; off_depth_.width(18); off_depth_.fill('0'); off_depth_ << timestamp_depth_; off_depth_ << " " << depth_file_PNG << std::endl; off_ir_.width(18); off_ir_.fill('0'); off_ir_ << timestamp_ir_; off_ir_ << " " << ir_file_PNG << std::endl; }