template <typename PointT, typename NormalT> bool pcl::RegionGrowing<PointT, NormalT>::validatePoint (int initial_seed, int point, int nghbr, bool& is_a_seed) const { is_a_seed = true; float cosine_threshold = cosf (theta_threshold_); float data[4]; data[0] = input_->points[point].data[0]; data[1] = input_->points[point].data[1]; data[2] = input_->points[point].data[2]; data[3] = input_->points[point].data[3]; Eigen::Map<Eigen::Vector3f> initial_point (static_cast<float*> (data)); Eigen::Map<Eigen::Vector3f> initial_normal (static_cast<float*> (normals_->points[point].normal)); //check the angle between normals if (smooth_mode_flag_ == true) { Eigen::Map<Eigen::Vector3f> nghbr_normal (static_cast<float*> (normals_->points[nghbr].normal)); float dot_product = fabsf (nghbr_normal.dot (initial_normal)); if (dot_product < cosine_threshold) { return (false); } } else { Eigen::Map<Eigen::Vector3f> nghbr_normal (static_cast<float*> (normals_->points[nghbr].normal)); Eigen::Map<Eigen::Vector3f> initial_seed_normal (static_cast<float*> (normals_->points[initial_seed].normal)); float dot_product = fabsf (nghbr_normal.dot (initial_seed_normal)); if (dot_product < cosine_threshold) return (false); } // check the curvature if needed if (curvature_flag_ && normals_->points[nghbr].curvature > curvature_threshold_) { is_a_seed = false; } // check the residual if needed float data_1[4]; data_1[0] = input_->points[nghbr].data[0]; data_1[1] = input_->points[nghbr].data[1]; data_1[2] = input_->points[nghbr].data[2]; data_1[3] = input_->points[nghbr].data[3]; Eigen::Map<Eigen::Vector3f> nghbr_point (static_cast<float*> (data_1)); float residual = fabsf (initial_normal.dot (initial_point - nghbr_point)); if (residual_flag_ && residual > residual_threshold_) is_a_seed = false; return (true); }
bool pcl::RegionGrowingHSV<PointT, NormalT>::validatePoint(int initial_seed, int point, int nghbr, bool& is_a_seed) const { is_a_seed = true; // check the color difference std::vector<float> point_color; point_color.resize(3, 0); std::vector<float> nghbr_color; nghbr_color.resize(3, 0); point_color[0] = input_->points[point].h; point_color[1] = input_->points[point].s; point_color[2] = input_->points[point].v; nghbr_color[0] = input_->points[nghbr].h; nghbr_color[1] = input_->points[nghbr].s; nghbr_color[2] = input_->points[nghbr].v; bool similar_enough = calculateColorimetricalDifference(point_color, nghbr_color, false); if (!similar_enough) return (false); float cosine_threshold = cosf(theta_threshold_); // check the angle between normals if needed if (normal_flag_) { float data[4]; data[0] = input_->points[point].data[0]; data[1] = input_->points[point].data[1]; data[2] = input_->points[point].data[2]; data[3] = input_->points[point].data[3]; Eigen::Map<Eigen::Vector3f> initial_point(static_cast<float*> (data)); Eigen::Map<Eigen::Vector3f> initial_normal(static_cast<float*> (normals_->points[point].normal)); if (smooth_mode_flag_ == true) { Eigen::Map<Eigen::Vector3f> nghbr_normal(static_cast<float*> (normals_->points[nghbr].normal)); float dot_product = fabsf(nghbr_normal.dot(initial_normal)); if (dot_product < cosine_threshold) return (false); } else { Eigen::Map<Eigen::Vector3f> nghbr_normal(static_cast<float*> (normals_->points[nghbr].normal)); Eigen::Map<Eigen::Vector3f> initial_seed_normal(static_cast<float*> (normals_->points[initial_seed].normal)); float dot_product = fabsf(nghbr_normal.dot(initial_seed_normal)); if (dot_product < cosine_threshold) return (false); } } // check the curvature if needed if (curvature_flag_ && normals_->points[nghbr].curvature > curvature_threshold_) is_a_seed = false; // check the residual if needed if (residual_flag_) { float data_p[4]; data_p[0] = input_->points[point].data[0]; data_p[1] = input_->points[point].data[1]; data_p[2] = input_->points[point].data[2]; data_p[3] = input_->points[point].data[3]; float data_n[4]; data_n[0] = input_->points[nghbr].data[0]; data_n[1] = input_->points[nghbr].data[1]; data_n[2] = input_->points[nghbr].data[2]; data_n[3] = input_->points[nghbr].data[3]; Eigen::Map<Eigen::Vector3f> nghbr_point(static_cast<float*> (data_n)); Eigen::Map<Eigen::Vector3f> initial_point(static_cast<float*> (data_p)); Eigen::Map<Eigen::Vector3f> initial_normal(static_cast<float*> (normals_->points[point].normal)); float residual = fabsf(initial_normal.dot(initial_point - nghbr_point)); if (residual > residual_threshold_) is_a_seed = false; } return (true); }
template <typename PointT> bool pcl::RegionGrowingRGB<PointT>::validatePoint (int initial_seed, int point, int nghbr, bool& is_a_seed) const { is_a_seed = true; // check the color difference std::vector<unsigned int> point_color; point_color.resize (3, 0); std::vector<unsigned int> nghbr_color; nghbr_color.resize (3, 0); point_color[0] = cloud_for_segmentation_->points[point].r; point_color[1] = cloud_for_segmentation_->points[point].g; point_color[2] = cloud_for_segmentation_->points[point].b; nghbr_color[0] = cloud_for_segmentation_->points[nghbr].r; nghbr_color[1] = cloud_for_segmentation_->points[nghbr].g; nghbr_color[2] = cloud_for_segmentation_->points[nghbr].b; float difference = calculateColorimetricalDifference (point_color, nghbr_color); if (difference > color_p2p_threshold_) return (false); float cosine_threshold = cosf (theta_threshold_); // check the angle between normals if needed if (normal_flag_) { Eigen::Map<Eigen::Vector3f> initial_point (static_cast<float*> (cloud_for_segmentation_->points[point].data)); Eigen::Map<Eigen::Vector3f> initial_normal (static_cast<float*> (normals_->points[point].normal)); if (smooth_mode_ == true) { Eigen::Map<Eigen::Vector3f> nghbr_normal (static_cast<float*> (normals_->points[nghbr].normal)); float dot_product = fabsf (nghbr_normal.dot (initial_normal)); if (dot_product < cosine_threshold) return (false); } else { Eigen::Map<Eigen::Vector3f> nghbr_normal (static_cast<float*> (normals_->points[nghbr].normal)); Eigen::Map<Eigen::Vector3f> initial_seed_normal (static_cast<float*> (normals_->points[initial_seed].normal)); float dot_product = fabsf (nghbr_normal.dot (initial_seed_normal)); if (dot_product < cosine_threshold) return (false); } } // check the curvature if needed if (curvature_flag_ && normals_->points[nghbr].curvature > curvature_threshold_) is_a_seed = false; // check the residual if needed if (residual_flag_) { Eigen::Map<Eigen::Vector3f> nghbr_point (static_cast<float*> (cloud_for_segmentation_->points[nghbr].data)); Eigen::Map<Eigen::Vector3f> initial_point (static_cast<float*> (cloud_for_segmentation_->points[point].data)); Eigen::Map<Eigen::Vector3f> initial_normal (static_cast<float*> (normals_->points[point].normal)); float residual = fabsf (initial_normal.dot (initial_point - nghbr_point)); if (residual > residual_threshold_) is_a_seed = false; } return (true); }