static void distance_to_origin_test() { check_within_d(distance_to_origin(make_point(0, 0)), 0.0, EPSILON); check_within_d(distance_to_origin(make_point(1, 0)), 1.0, EPSILON); check_within_d(distance_to_origin(make_point(-1, 0)), 1.0, EPSILON); check_within_d(distance_to_origin(make_point(0, 2)), 2.0, EPSILON); check_within_d(distance_to_origin(make_point(0, -2)), 2.0, EPSILON); check_within_d(distance_to_origin(make_point(3, 4)), 5.0, EPSILON); check_within_d(distance_to_origin(make_point(3, -4)), 5.0, EPSILON); }
inline bool is_edge(const pcl::PointCloud<PointT> &in, int current_idx, std::vector<int> &k_indices, std::vector<float> &square_distance){ #ifdef USE_SQUERE_DISTANCE const float point_distance = distance_to_origin(in.points.at(current_idx)); #else const float point_distance = in.points.at(current_idx).z; #endif float min_z = point_distance; float max_z = 0; //point_distance; float max_y = in.points.at(current_idx).y; int best_candidate = current_idx; const float z_threshold = 0.3; #ifdef USE_Y_THRESHOLD const float y_threshold = 0.10; const float y_min_t = in.points.at(current_idx).y - y_threshold; const float y_max_t = in.points.at(current_idx).y + y_threshold; #endif for(int n = 0; n < k_indices.size(); ++n){ #ifdef USE_Y_THRESHOLD if(! between( y_min_t, in.points.at(k_indices.at(n)).y, y_max_t)) continue; #endif #ifdef USE_SQUERE_DISTANCE float distance_n = distance_to_origin(in.points.at(k_indices.at(n))); #else float distance_n = in.points.at(k_indices.at((n))).z; #endif // if(distance_n > point_distance){ // max_z = distance_n; // best_candidate = k_indices.at(n); // } max_z = MAX(max_z, distance_n -point_distance); // if(in.points.at(k_indices.at((n))).x > max_x) // { // max_x = in.points.at(k_indices.at((n))).x; // } // if(in.points.at(k_indices.at((n))).x < min_x) // { // min_x = in.points.at(k_indices.at((n))).x; // } // if(in.points.at(k_indices.at(n)).y > max_y) // { // max_y = in.points.at(k_indices.at(n)).y; // } // if(in.points.at(k_indices.at(n)).y < min_y) // { // min_y = in.points.at(k_indices.at((n))).y; // } // if(in.points.at(k_indices.at((n))).z < min_z) // { // min_z = in.points.at(k_indices.at((n))).z; // } } if(max_z > z_threshold) return true; return false; //if( max_z - threshold > in.points.at(current_idx).z) if((is_depth_step(in.points.at(current_idx), in.points.at(best_candidate))) //|| max_y == in.points.at(current_idx).y /* no neighbor above */ ) { #ifdef DEBUG std::cout << "threshold: " << z_threshold << " max_z: " << max_z << " point: " << in.points.at(current_idx) << " best: " << in.points.at(best_candidate) << " delta: " << delta(in.points.at(current_idx), in.points.at(best_candidate)) << std::endl; #endif return true; } #ifdef DEBUG //std::cout << "from " << k_indices.size() << " neigbors non matchs the condition"<< std::endl; #endif return false; }