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);
}
static int ipm_main(struct csa *csa)
{ int m = csa->m;
int n = csa->n;
int i, j, status;
double temp;
/* choose initial point using Mehrotra's heuristic */
if (csa->parm->msg_lev >= GLP_MSG_ALL)
xprintf("Guessing initial point...\n");
initial_point(csa);
/* main loop starts here */
if (csa->parm->msg_lev >= GLP_MSG_ALL)
xprintf("Optimization begins...\n");
for (;;)
{ /* perform basic computations at the current point */
basic_info(csa);
/* save initial value of rmu */
if (csa->iter == 0) csa->rmu0 = csa->rmu;
/* accumulate values of min(phi[k]) and save the best point */
xassert(csa->iter <= ITER_MAX);
if (csa->iter == 0 || csa->phi_min[csa->iter-1] > csa->phi)
{ csa->phi_min[csa->iter] = csa->phi;
csa->best_iter = csa->iter;
for (j = 1; j <= n; j++) csa->best_x[j] = csa->x[j];
for (i = 1; i <= m; i++) csa->best_y[i] = csa->y[i];
for (j = 1; j <= n; j++) csa->best_z[j] = csa->z[j];
csa->best_obj = csa->obj;
}
else
csa->phi_min[csa->iter] = csa->phi_min[csa->iter-1];
/* display information at the current point */
if (csa->parm->msg_lev >= GLP_MSG_ON)
xprintf("%3d: obj = %17.9e; rpi = %8.1e; rdi = %8.1e; gap ="
" %8.1e\n", csa->iter, csa->obj, csa->rpi, csa->rdi,
csa->gap);
/* check if the current point is optimal */
if (csa->rpi < 1e-8 && csa->rdi < 1e-8 && csa->gap < 1e-8)
{ if (csa->parm->msg_lev >= GLP_MSG_ALL)
xprintf("OPTIMAL SOLUTION FOUND\n");
status = 0;
break;
}
/* check if the problem has no feasible solution */
temp = 1e5 * csa->phi_min[csa->iter];
if (temp < 1e-8) temp = 1e-8;
if (csa->phi >= temp)
{ if (csa->parm->msg_lev >= GLP_MSG_ALL)
xprintf("PROBLEM HAS NO FEASIBLE PRIMAL/DUAL SOLUTION\n")
;
status = 1;
break;
}
/* check for very slow convergence or divergence */
if (((csa->rpi >= 1e-8 || csa->rdi >= 1e-8) && csa->rmu /
csa->rmu0 >= 1e6) ||
(csa->iter >= 30 && csa->phi_min[csa->iter] >= 0.5 *
csa->phi_min[csa->iter - 30]))
{ if (csa->parm->msg_lev >= GLP_MSG_ALL)
xprintf("NO CONVERGENCE; SEARCH TERMINATED\n");
status = 2;
break;
}
/* check for maximal number of iterations */
if (csa->iter == ITER_MAX)
{ if (csa->parm->msg_lev >= GLP_MSG_ALL)
xprintf("ITERATION LIMIT EXCEEDED; SEARCH TERMINATED\n");
status = 3;
break;
}
/* start the next iteration */
csa->iter++;
/* factorize normal equation system */
for (j = 1; j <= n; j++) csa->D[j] = csa->x[j] / csa->z[j];
decomp_NE(csa);
/* compute the next point using Mehrotra's predictor-corrector
technique */
if (make_step(csa))
{ if (csa->parm->msg_lev >= GLP_MSG_ALL)
xprintf("NUMERIC INSTABILITY; SEARCH TERMINATED\n");
status = 4;
break;
}
}
/* restore the best point */
if (status != 0)
{ for (j = 1; j <= n; j++) csa->x[j] = csa->best_x[j];
for (i = 1; i <= m; i++) csa->y[i] = csa->best_y[i];
for (j = 1; j <= n; j++) csa->z[j] = csa->best_z[j];
if (csa->parm->msg_lev >= GLP_MSG_ALL)
xprintf("Best point %17.9e was reached on iteration %d\n",
csa->best_obj, csa->best_iter);
}
/* return to the calling program */
return status;
}
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);
}
