TEST (PCL, IterativeClosestPoint) { IterativeClosestPoint<PointXYZ, PointXYZ> reg; reg.setInputCloud (cloud_source.makeShared ()); reg.setInputTarget (cloud_target.makeShared ()); reg.setMaximumIterations (50); reg.setTransformationEpsilon (1e-8); reg.setMaxCorrespondenceDistance (0.05); // Register reg.align (cloud_reg); EXPECT_EQ (int (cloud_reg.points.size ()), int (cloud_source.points.size ())); Eigen::Matrix4f transformation = reg.getFinalTransformation (); EXPECT_NEAR (transformation (0, 0), 0.8806, 1e-3); EXPECT_NEAR (transformation (0, 1), 0.036481287330389023, 1e-2); EXPECT_NEAR (transformation (0, 2), -0.4724, 1e-3); EXPECT_NEAR (transformation (0, 3), 0.03453, 1e-3); EXPECT_NEAR (transformation (1, 0), -0.02354, 1e-3); EXPECT_NEAR (transformation (1, 1), 0.9992, 1e-3); EXPECT_NEAR (transformation (1, 2), 0.03326, 1e-3); EXPECT_NEAR (transformation (1, 3), -0.001519, 1e-3); EXPECT_NEAR (transformation (2, 0), 0.4732, 1e-3); EXPECT_NEAR (transformation (2, 1), -0.01817, 1e-3); EXPECT_NEAR (transformation (2, 2), 0.8808, 1e-3); EXPECT_NEAR (transformation (2, 3), 0.04116, 1e-3); EXPECT_EQ (transformation (3, 0), 0); EXPECT_EQ (transformation (3, 1), 0); EXPECT_EQ (transformation (3, 2), 0); EXPECT_EQ (transformation (3, 3), 1); }
float Eye3D::dynamic_EMD(const PointCloudPtr target, const vector<float> &wInput, PointCloudPtr source, const vector<float> &wOutput) { _viz->clear(); float radiusArg = 0.5; IterativeClosestPoint<PointT, PointT> icp; icp.setInputCloud(source); icp.setInputWeight(wInput); icp.setInputTarget(target); icp.setOutputWeight(wOutput); pcl::PointCloud<PointT> result; // Set the transformation epsilon (criterion 2) icp.setMaximumIterations (150); icp.setMaxCorrespondenceDistance (100); icp.setTransformationEpsilon (1e-11); // Set the euclidean distance difference epsilon (criterion 3) icp.setEuclideanFitnessEpsilon (1e-11); icp.setRANSACIterations(0); icp.align(result); if (!icp.hasConverged()) { std::cout << "ICP failed to converged!"<<std::endl; return -1; } if (visualize_EMD) { /// visualize the align result int size = result.points.size(); // rank the weights std::map<float, int> w2rank; std::vector<float> wInputRank; m_util::rank(wInput, &w2rank); for(float w: wInput){ wInputRank.push_back(w2rank[w] + 1); } w2rank.clear(); std::vector<float> wOutputRank; m_util::rank(wOutput, &w2rank); for(float w: wOutput){ wOutputRank.push_back(w2rank[w] + 1); } int f = 50; for (int i = 0; i < size; i++) { _viz->add_sphere(target->points[i].x/f,target->points[i].y/f, target->points[i].z/f, wInputRank[i]* radiusArg, 0, 255, 0); // _viz->add_sphere(source->points[i].x/f, source->points[i].y/f, // source->points[i].z/f, wOutputRank[i]* radiusArg, 0, 0, 255); _viz->add_sphere(result.points[i].x/f, result.points[i].y/f, result.points[i].z, wOutputRank[i] * radiusArg, 255, 0, 0); } // icp.visualize_correspondence(_viz,0); _viz->reset_camera(); } return icp.best_EMD_; }
TEST (PCL, IterativeClosestPointWithRejectors) { IterativeClosestPoint<PointXYZ, PointXYZ> reg; reg.setMaximumIterations (50); reg.setTransformationEpsilon (1e-8); reg.setMaxCorrespondenceDistance (0.15); // Add a median distance rejector pcl::registration::CorrespondenceRejectorMedianDistance::Ptr rej_med (new pcl::registration::CorrespondenceRejectorMedianDistance); rej_med->setMedianFactor (4.0); reg.addCorrespondenceRejector (rej_med); // Also add a SaC rejector pcl::registration::CorrespondenceRejectorSampleConsensus<PointXYZ>::Ptr rej_samp (new pcl::registration::CorrespondenceRejectorSampleConsensus<PointXYZ>); reg.addCorrespondenceRejector (rej_samp); size_t ntransforms = 10; for (size_t t = 0; t < ntransforms; t++) { // Sample a fixed offset between cloud pairs Eigen::Affine3f delta_transform; sampleRandomTransform (delta_transform, 0., 0.05); // Sample random global transform for each pair, to make sure we aren't biased around the origin Eigen::Affine3f net_transform; sampleRandomTransform (net_transform, 2*M_PI, 10.); PointCloud<PointXYZ>::ConstPtr source (cloud_source.makeShared ()); PointCloud<PointXYZ>::Ptr source_trans (new PointCloud<PointXYZ>); PointCloud<PointXYZ>::Ptr target_trans (new PointCloud<PointXYZ>); pcl::transformPointCloud (*source, *source_trans, delta_transform.inverse () * net_transform); pcl::transformPointCloud (*source, *target_trans, net_transform); reg.setInputSource (source_trans); reg.setInputTarget (target_trans); // Register reg.align (cloud_reg); Eigen::Matrix4f trans_final = reg.getFinalTransformation (); // Translation should be within 1cm for (int y = 0; y < 4; y++) EXPECT_NEAR (trans_final (y, 3), delta_transform (y, 3), 1E-2); // Rotation within .1 for (int y = 0; y < 4; y++) for (int x = 0; x < 3; x++) EXPECT_NEAR (trans_final (y, x), delta_transform (y, x), 1E-1); } }
bool IncrementalPoseEstimatorFromRgbFeatures::optimizeWithICP( pcl::PointCloud<pcl::PointXYZ>::ConstPtr cloud_source, Pose3D& depth_pose, int closest_view_index) { pcl::PointCloud<pcl::PointXYZ>::ConstPtr cloud_target = m_image_data[closest_view_index].sampled_cloud; pcl::PointCloud<pcl::PointXYZ> cloud_reg; Pose3D new_depth_pose = depth_pose; ntk_dbg_print(cloud_source->points.size(), 1); ntk_dbg_print(cloud_target->points.size(), 1); IterativeClosestPoint<PointXYZ, PointXYZ> reg; reg.setInputCloud (cloud_target); reg.setInputTarget (cloud_source); reg.setMaximumIterations (50); reg.setTransformationEpsilon (1e-5); reg.setMaxCorrespondenceDistance (0.5); reg.align (cloud_reg); if (!reg.hasConverged()) { ntk_dbg(1) << "ICP did not converge, ignoring."; return false; } ntk_dbg_print(reg.getFitnessScore(), 1); 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); new_depth_pose.applyTransformAfter(icp_pose); depth_pose = new_depth_pose; return true; }
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; }