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, typename NormalT> bool
pcl::RegionGrowingRGB2<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<unsigned int> point_color;
point_color.resize (3, 0);
std::vector<unsigned int> nghbr_color;
nghbr_color.resize (3, 0);
point_color[0] = input_->points[point].r;
point_color[1] = input_->points[point].g;
point_color[2] = input_->points[point].b;
nghbr_color[0] = input_->points[nghbr].r;
nghbr_color[1] = input_->points[nghbr].g;
nghbr_color[2] = input_->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_)
{
// 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 = 0;//fabsf (initial_normal.dot (initial_point - nghbr_point));
if (residual > residual_threshold_)
is_a_seed = false;
}
return (true);
}
