void RGBDFrameRecorder :: saveCurrentFrame(const RGBDImage& image) { std::string frame_dir = format("%s/view%04d", m_dir.absolutePath().toStdString().c_str(), m_frame_index); std::string raw_frame_dir = format("%s/raw", frame_dir.c_str(), m_frame_index); QDir dir (frame_dir.c_str()); dir.mkpath("raw"); std::string filename; if (!m_only_raw) { filename = cv::format("%s/color.png", frame_dir.c_str()); imwrite(filename, image.rgb()); } filename = cv::format("%s/raw/color.png", frame_dir.c_str()); imwrite(filename, image.rawRgb()); if (!m_only_raw && image.mappedDepth().data) { filename = cv::format("%s/mapped_depth.png", frame_dir.c_str()); imwrite_normalized(filename, image.mappedDepth()); filename = cv::format("%s/mapped_color.png", frame_dir.c_str()); imwrite(filename, image.mappedRgb()); filename = cv::format("%s/depth.yml", frame_dir.c_str()); imwrite_yml(filename, image.mappedDepth()); } if (!m_only_raw) { filename = cv::format("%s/raw/depth.png", frame_dir.c_str()); if (image.rawDepth().data) imwrite_normalized(filename.c_str(), image.rawDepth()); filename = cv::format("%s/depth.png", frame_dir.c_str()); if (image.depth().data) imwrite_normalized(filename.c_str(), image.depth()); filename = cv::format("%s/intensity.png", frame_dir.c_str()); if (image.intensity().data) imwrite_normalized(filename.c_str(), image.intensity()); } if (image.rawDepth().data) { if (m_use_binary_raw) { filename = cv::format("%s/raw/depth.raw", frame_dir.c_str()); imwrite_Mat1f_raw(filename.c_str(), image.rawDepth()); } else { filename = cv::format("%s/raw/depth.yml", frame_dir.c_str()); imwrite_yml(filename.c_str(), image.rawDepth()); } } if (image.rawIntensity().data) { if (m_use_binary_raw) { filename = cv::format("%s/raw/intensity.raw", frame_dir.c_str()); imwrite_Mat1f_raw(filename.c_str(), image.rawIntensity()); } else { filename = cv::format("%s/raw/intensity.yml", frame_dir.c_str()); imwrite_yml(filename.c_str(), image.rawIntensity()); } } if (!m_only_raw) { filename = cv::format("%s/raw/amplitude.png", frame_dir.c_str()); if (image.rawAmplitude().data) imwrite_normalized(filename.c_str(), image.rawAmplitude()); filename = cv::format("%s/amplitude.png", frame_dir.c_str()); if (image.amplitude().data) imwrite_normalized(filename.c_str(), image.amplitude()); } if (image.rawAmplitude().data) { if (m_use_binary_raw) { filename = cv::format("%s/raw/amplitude.raw", frame_dir.c_str()); imwrite_Mat1f_raw(filename.c_str(), image.rawAmplitude()); } else { filename = cv::format("%s/raw/amplitude.yml", frame_dir.c_str()); imwrite_yml(filename.c_str(), image.rawAmplitude()); } } ++m_frame_index; }
void RGBDFrameRecorder :: writeFrame(const RGBDImage& image, const std::string& frame_dir) { std::string raw_frame_dir = format("%s/raw", frame_dir.c_str(), m_frame_index); QDir dir (frame_dir.c_str()); dir.mkpath("raw"); std::string filename; if (m_save_rgb_pose && image.calibration()) { filename = cv::format("%s/rgb_pose.avs", frame_dir.c_str()); image.rgbPose().saveToAvsFile(filename.c_str()); } if (!m_only_raw) { filename = cv::format("%s/color.png", frame_dir.c_str()); imwrite(filename, image.rgb()); } if (m_save_pcl_point_cloud) { filename = cv::format("%s/cloud.pcd", frame_dir.c_str()); #ifdef NESTK_USE_PCL pcl::PointCloud<pcl::PointXYZ> cloud; rgbdImageToPointCloud(cloud, image); pcl::io::savePCDFileASCII(filename.c_str(), cloud); #endif } if (m_use_compressed_format) filename = cv::format("%s/raw/color.png", frame_dir.c_str()); else filename = cv::format("%s/raw/color.bmp", frame_dir.c_str()); imwrite(filename, image.rawRgb()); if (!m_only_raw && image.mappedDepth().data) { filename = cv::format("%s/mapped_depth.png", frame_dir.c_str()); imwrite_normalized(filename, image.mappedDepth()); filename = cv::format("%s/mapped_color.png", frame_dir.c_str()); imwrite(filename, image.mappedRgb()); filename = cv::format("%s/depth.yml", frame_dir.c_str()); imwrite_yml(filename, image.mappedDepth()); } if (!m_only_raw) { filename = cv::format("%s/raw/depth.png", frame_dir.c_str()); if (image.rawDepth().data) imwrite_normalized(filename.c_str(), image.rawDepth()); filename = cv::format("%s/depth.png", frame_dir.c_str()); if (image.depth().data) imwrite_normalized(filename.c_str(), image.depth()); filename = cv::format("%s/intensity.png", frame_dir.c_str()); if (image.intensity().data) imwrite_normalized(filename.c_str(), image.intensity()); } if (image.rawDepth().data) { if (m_use_binary_raw) { filename = cv::format("%s/raw/depth.raw", frame_dir.c_str()); imwrite_Mat1f_raw(filename.c_str(), image.rawDepth()); } else { filename = cv::format("%s/raw/depth.yml", frame_dir.c_str()); imwrite_yml(filename.c_str(), image.rawDepth()); } } if (m_save_intensity && image.rawIntensity().data) { if (m_use_binary_raw) { filename = cv::format("%s/raw/intensity.raw", frame_dir.c_str()); imwrite_Mat1f_raw(filename.c_str(), image.rawIntensity()); } else { filename = cv::format("%s/raw/intensity.yml", frame_dir.c_str()); imwrite_yml(filename.c_str(), image.rawIntensity()); } } if (!m_only_raw) { filename = cv::format("%s/raw/amplitude.png", frame_dir.c_str()); if (image.rawAmplitude().data) imwrite_normalized(filename.c_str(), image.rawAmplitude()); filename = cv::format("%s/amplitude.png", frame_dir.c_str()); if (image.amplitude().data) imwrite_normalized(filename.c_str(), image.amplitude()); } if (image.rawAmplitude().data) { if (m_use_binary_raw) { filename = cv::format("%s/raw/amplitude.raw", frame_dir.c_str()); imwrite_Mat1f_raw(filename.c_str(), image.rawAmplitude()); } else { filename = cv::format("%s/raw/amplitude.yml", frame_dir.c_str()); imwrite_yml(filename.c_str(), image.rawAmplitude()); } } }
void calibrate_kinect_depth(std::vector< DepthCalibrationPoint >& depth_values) { std::vector< std::vector<Point3f> > pattern_points; calibrationPattern(pattern_points, global::opt_pattern_width(), global::opt_pattern_height(), global::opt_square_size(), global::images_list.size()); for (int i_image = 0; i_image < global::images_list.size(); ++i_image) { // Generate depth calibration points QString filename = global::images_list[i_image]; QDir cur_image_dir (global::images_dir.absoluteFilePath(filename)); std::string full_filename = cur_image_dir.absoluteFilePath("raw/color.png").toStdString(); RGBDImage image; OpenniRGBDProcessor processor; processor.setFilterFlag(RGBDProcessorFlags::ComputeMapping, true); image.loadFromDir(cur_image_dir.absolutePath().toStdString(), &global::calibration, &processor); imshow_normalized("mapped depth", image.mappedDepth()); imshow("color", image.rgb()); std::vector<Point2f> current_view_corners; calibrationCorners(full_filename, "corners", global::opt_pattern_width(), global::opt_pattern_height(), current_view_corners, image.rgb(), 1, global::pattern_type); if (current_view_corners.size() != (global::opt_pattern_width()*global::opt_pattern_height())) { ntk_dbg(1) << "Corners not detected in " << cur_image_dir.absolutePath().toStdString(); continue; } // FIXME: why rvecs and tvecs from calibrateCamera seems to be nonsense ? // calling findExtrinsics another time to get good chessboard estimations. cv::Mat1f H; estimate_checkerboard_pose(pattern_points[0], current_view_corners, global::calibration.rgb_intrinsics, H); Pose3D pose; pose.setCameraParametersFromOpencv(global::calibration.rgb_intrinsics); pose.setCameraTransform(H); ntk_dbg_print(pose, 1); cv::Mat3b debug_img; image.rgb().copyTo(debug_img); foreach_idx(pattern_i, pattern_points[0]) { Point3f p = pose.projectToImage(pattern_points[0][pattern_i]); ntk_dbg_print(p, 1); float kinect_raw = image.mappedDepth()(p.y, p.x); ntk_dbg_print(kinect_raw, 1); if (kinect_raw < 1e-5) continue; float err = kinect_raw-p.z; cv::putText(debug_img, format("%d", (int)(err*1000)), Point(p.x, p.y), CV_FONT_NORMAL, 0.4, Scalar(255,0,0)); ntk_dbg_print(pattern_points[0][pattern_i], 1); ntk_dbg_print(p, 1); ntk_dbg_print(kinect_raw, 1); depth_values.push_back(DepthCalibrationPoint(kinect_raw, p.z)); } imshow("errors", debug_img); cv::waitKey(0); }