40 #include <pcl/range_image/range_image.h>
72 int x,
int y,
int offset_x,
int offset_y,
int pixel_radius)
const
77 if (pcl_isinf(neighbor.
range))
79 if (neighbor.
range < 0.0f)
128 int delta_x=0, delta_y=0;
138 if (delta_x==0 && delta_y==0)
141 int x=border_description.
x, y=border_description.
y;
143 Eigen::Vector3f neighbor_point;
147 if (local_surface!=NULL)
151 viewing_direction = neighbor_point-sensor_pos;
155 neighbor_point = lambda*viewing_direction + sensor_pos;
159 direction = neighbor_point-point.getVector3fMap();
160 direction.normalize();
169 border_direction = NULL;
174 border_direction =
new Eigen::Vector3f(0.0f, 0.0f, 0.0f);
177 delete border_direction;
178 border_direction = NULL;
184 float* border_scores_other_direction,
int& shadow_border_idx)
const
188 shadow_border_idx = -1;
192 if (border_score == 1.0f)
201 float best_shadow_border_score = 0.0f;
205 int neighbor_x=x+neighbor_distance*offset_x, neighbor_y=y+neighbor_distance*offset_y;
208 float neighbor_shadow_border_score = border_scores_other_direction[neighbor_y*
range_image_->
width+neighbor_x];
210 if (neighbor_shadow_border_score < best_shadow_border_score)
213 best_shadow_border_score = neighbor_shadow_border_score;
216 if (shadow_border_idx >= 0)
220 border_score *= (std::max)(0.9f, 1-powf(1+best_shadow_border_score, 3));
224 shadow_border_idx = -1;
231 float max_score_bonus = 0.5f;
239 float average_neighbor_score=0.0f, weight_sum=0.0f;
240 for (
int y2=y-1; y2<=y+1; ++y2)
242 for (
int x2=x-1; x2<=x+1; ++x2)
250 average_neighbor_score /=weight_sum;
252 if (average_neighbor_score*border_score < 0.0f)
255 float new_border_score = border_score + max_score_bonus * average_neighbor_score * (1.0f-fabsf(border_score));
258 return new_border_score;
262 float* border_scores_other_direction,
int& shadow_border_idx)
const
268 shadow_border_idx = -1;
273 int neighbor_x=x+neighbor_distance*offset_x, neighbor_y=y+neighbor_distance*offset_y;
276 float neighbor_shadow_border_score = border_scores_other_direction[neighbor_y*
range_image_->
width+neighbor_x];
278 if (neighbor_shadow_border_score < best_shadow_border_score)
281 best_shadow_border_score = neighbor_shadow_border_score;
284 if (shadow_border_idx >= 0)
295 int neighbor_x=x-offset_x, neighbor_y=y-offset_y;
301 neighbor_x=x+neighbor_distance*offset_x; neighbor_y=y+neighbor_distance*offset_y;
305 if (neighbor_index==shadow_border_idx)
308 float neighbor_border_score = border_scores[neighbor_index];
309 if (neighbor_border_score > border_score)
316 Eigen::Vector3f& main_direction)
const
321 if (local_surface==NULL)
330 for (
int step=1; step<=radius; ++step)
333 for (
int y2=y-step; y2<=y+step; y2+=step)
335 for (
int x2=x-step; x2<=x+step; x2+=step)
337 bool& beam_valid = beams_valid[beam_idx++];
364 if (local_surface2==NULL)
369 vector_average.
add(normal2);
377 Eigen::Vector3f eigen_values, eigen_vector1, eigen_vector2;
378 vector_average.
doPCA(eigen_values, eigen_vector1, eigen_vector2, main_direction);
379 magnitude = sqrtf(eigen_values[2]);
398 if (!pcl_isfinite(magnitude))