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