bool RelativePoseEstimatorICP :: estimateNewPose(const RGBDImage& image) { if (m_ref_cloud.points.size() < 1) { ntk_dbg(1) << "Reference cloud was empty"; return false; } PointCloud<PointXYZ>::Ptr target = m_ref_cloud.makeShared(); PointCloud<PointXYZ>::Ptr source (new PointCloud<PointXYZ>()); rgbdImageToPointCloud(*source, image); PointCloud<PointXYZ>::Ptr filtered_source (new PointCloud<PointXYZ>()); PointCloud<PointXYZ>::Ptr filtered_target (new PointCloud<PointXYZ>()); pcl::VoxelGrid<pcl::PointXYZ> grid; grid.setLeafSize (m_voxel_leaf_size, m_voxel_leaf_size, m_voxel_leaf_size); grid.setInputCloud(source); grid.filter(*filtered_source); grid.setInputCloud(target); grid.filter(*filtered_target); PointCloud<PointXYZ> cloud_reg; IterativeClosestPoint<PointXYZ, PointXYZ> reg; reg.setMaximumIterations (m_max_iterations); reg.setTransformationEpsilon (1e-5); reg.setMaxCorrespondenceDistance (m_distance_threshold); reg.setInputCloud (filtered_source); reg.setInputTarget (filtered_target); reg.align (cloud_reg); if (!reg.hasConverged()) { ntk_dbg(1) << "ICP did not converge, ignoring."; return false; } Eigen::Matrix4f t = reg.getFinalTransformation (); cv::Mat1f T(4,4); //toOpencv(t,T); for (int r = 0; r < 4; ++r) for (int c = 0; c < 4; ++c) T(r,c) = t(r,c); Pose3D icp_pose; icp_pose.setCameraTransform(T); ntk_dbg_print(icp_pose.cvTranslation(), 1); // Pose3D stores the transformation from 3D space to image. // So we have to invert everything so that unprojecting from // image to 3D gives the right transformation. // H2' = ICP * H1' m_current_pose.resetCameraTransform(); m_current_pose = *image.calibration()->depth_pose; m_current_pose.invert(); m_current_pose.applyTransformAfter(icp_pose); m_current_pose.invert(); return true; }
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 RelativePoseEstimatorICP :: setReferenceImage(const RGBDImage& ref_image) { rgbdImageToPointCloud(m_ref_cloud, ref_image); }