40 #include <pcl/range_image/range_image.h>
57 return std::atan2(y, x);
72 int x,
int y,
int offset_x,
int offset_y,
int pixel_radius)
const
77 if (std::isinf(neighbor.
range))
79 if (neighbor.
range < 0.0f)
125 int delta_x=0, delta_y=0;
135 if (delta_x==0 && delta_y==0)
138 int x=border_description.
x, y=border_description.
y;
140 Eigen::Vector3f neighbor_point;
144 if (local_surface!=
nullptr)
148 viewing_direction = neighbor_point-sensor_pos;
152 neighbor_point = lambda*viewing_direction + sensor_pos;
156 direction = neighbor_point-point.getVector3fMap();
157 direction.normalize();
166 border_direction =
nullptr;
171 border_direction =
new Eigen::Vector3f(0.0f, 0.0f, 0.0f);
174 delete border_direction;
175 border_direction =
nullptr;
181 float* border_scores_other_direction,
int& shadow_border_idx)
const
185 shadow_border_idx = -1;
189 if (border_score == 1.0f)
198 float best_shadow_border_score = 0.0f;
202 int neighbor_x=x+neighbor_distance*offset_x, neighbor_y=y+neighbor_distance*offset_y;
205 float neighbor_shadow_border_score = border_scores_other_direction[neighbor_y*
range_image_->
width+neighbor_x];
207 if (neighbor_shadow_border_score < best_shadow_border_score)
210 best_shadow_border_score = neighbor_shadow_border_score;
213 if (shadow_border_idx >= 0)
217 border_score *= (std::max)(0.9f, 1-powf(1+best_shadow_border_score, 3));
221 shadow_border_idx = -1;
228 float max_score_bonus = 0.5f;
236 float average_neighbor_score=0.0f, weight_sum=0.0f;
237 for (
int y2=y-1; y2<=y+1; ++y2)
239 for (
int x2=x-1; x2<=x+1; ++x2)
247 average_neighbor_score /=weight_sum;
249 if (average_neighbor_score*border_score < 0.0f)
252 float new_border_score = border_score + max_score_bonus * average_neighbor_score * (1.0f-std::abs(border_score));
255 return new_border_score;
259 float* border_scores_other_direction,
int& shadow_border_idx)
const
265 shadow_border_idx = -1;
270 int neighbor_x=x+neighbor_distance*offset_x, neighbor_y=y+neighbor_distance*offset_y;
273 float neighbor_shadow_border_score = border_scores_other_direction[neighbor_y*
range_image_->
width+neighbor_x];
275 if (neighbor_shadow_border_score < best_shadow_border_score)
278 best_shadow_border_score = neighbor_shadow_border_score;
281 if (shadow_border_idx >= 0)
292 int neighbor_x=x-offset_x, neighbor_y=y-offset_y;
298 neighbor_x=x+neighbor_distance*offset_x; neighbor_y=y+neighbor_distance*offset_y;
302 if (neighbor_index==shadow_border_idx)
305 float neighbor_border_score = border_scores[neighbor_index];
306 if (neighbor_border_score > border_score)
313 Eigen::Vector3f& main_direction)
const
318 if (local_surface==
nullptr)
327 for (
int step=1; step<=radius; ++step)
330 for (
int y2=y-step; y2<=y+step; y2+=step)
332 for (
int x2=x-step; x2<=x+step; x2+=step)
334 bool& beam_valid = beams_valid[beam_idx++];
352 const BorderTraits& border_traits = (*border_descriptions_)[index2].traits;
361 if (local_surface2==
nullptr)
366 vector_average.
add(normal2);
374 Eigen::Vector3f eigen_values, eigen_vector1, eigen_vector2;
375 vector_average.
doPCA(eigen_values, eigen_vector1, eigen_vector2, main_direction);
376 magnitude = std::sqrt (eigen_values[2]);
395 return std::isfinite(magnitude);