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 = std::sqrt (eigen_values[2]);
398 if (!pcl_isfinite(magnitude))
A point structure representing Euclidean xyz coordinates, padded with an extra range float.
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
This file defines compatibility wrappers for low level I/O functions.
void calculate3DPoint(float image_x, float image_y, float range, PointWithRange &point) const
Calculate the 3D point according to the given image point and range.
void get1dPointAverage(int x, int y, int delta_x, int delta_y, int no_of_points, PointWithRange &average_point) const
Calculates the average 3D position of the no_of_points points described by the start point x,...
float squaredEuclideanDistance(const PointType1 &p1, const PointType2 &p2)
Calculate the squared euclidean distance between the two given points.
bool isInImage(int x, int y) const
Check if a point is inside of the image.
std::ostream & operator<<(std::ostream &os, const BivariatePolynomialT< real > &p)
void add(const Eigen::Matrix< real, dimension, 1 > &sample, real weight=1.0)
Add a new sample.
bool isValid(int x, int y) const
Check if a point is inside of the image and has a finite range.
const Eigen::Vector3f getSensorPos() const
Get the sensor position.
uint32_t width
The point cloud width (if organized as an image-structure).
unsigned int getNoOfSamples()
Get the number of added vectors.
void doPCA(Eigen::Matrix< real, dimension, 1 > &eigen_values, Eigen::Matrix< real, dimension, 1 > &eigen_vector1, Eigen::Matrix< real, dimension, 1 > &eigen_vector2, Eigen::Matrix< real, dimension, 1 > &eigen_vector3) const
Do Principal component analysis.
const PointWithRange & getPoint(int image_x, int image_y) const
Return the 3D point with range at the given image position.
A structure to store if a point in a range image lies on a border between an obstacle and the backgro...
bool isMaxRange(int x, int y) const
Check if a point is a max range (range=INFINITY) - please check isInImage or isObserved first!
Calculates the weighted average and the covariance matrix.
std::bitset< 32 > BorderTraits
Data type to store extended information about a transition from foreground to backgroundSpecification...