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); }