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))
A point structure representing Euclidean xyz coordinates, padded with an extra range float...
float updatedScoreAccordingToNeighborValues(int x, int y, const float *border_scores) const
Returns a new score for the given pixel that is >= the original value, based on the neighbors values...
float max_neighbor_distance_squared
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.
int pixel_radius_plane_extraction
bool calculateMainPrincipalCurvature(int x, int y, int radius, float &magnitude, Eigen::Vector3f &main_direction) const
Calculate the main principal curvature (the largest eigenvalue and corresponding eigenvector for the ...
int pixel_radius_principal_curvature
unsigned int getNoOfSamples()
Get the number of added vectors.
bool isValid(int x, int y) const
Check if a point is inside of the image and has a finite range.
void calculateBorderDirection(int x, int y)
Calculate the 3D direction of the border just using the border traits at this position (facing away f...
Stores some information extracted from the neighborhood of a point.
Eigen::Vector3f normal_no_jumps
float minimum_border_probability
bool checkIfMaximum(int x, int y, int offset_x, int offset_y, float *border_scores, int shadow_border_idx) const
Check if a potential border point is a maximum regarding the border score.
LocalSurface ** surface_structure_
static float getObstacleBorderAngle(const BorderTraits &border_traits)
Take the information from BorderTraits to calculate the local direction of the border.
float squaredEuclideanDistance(const PointType1 &p1, const PointType2 &p2)
Calculate the squared euclidean distance between the two given points.
Eigen::Vector3f ** border_directions_
bool checkPotentialBorder(int x, int y, int offset_x, int offset_y, float *border_scores_left, float *border_scores_right, int &shadow_border_idx) const
Check if a potential border point has a corresponding shadow border.
bool changeScoreAccordingToShadowBorderValue(int x, int y, int offset_x, int offset_y, float *border_scores, float *border_scores_other_direction, int &shadow_border_idx) const
Find the best corresponding shadow border and lower score according to the shadow borders value...
const RangeImage * range_image_
Parameters used in this class.
bool get3dDirection(const BorderDescription &border_description, Eigen::Vector3f &direction, const LocalSurface *local_surface=NULL)
Calculate a 3d direction from a border point by projecting the direction in the range image - returns...
const PointWithRange & getPoint(int image_x, int image_y) const
Return the 3D point with range at the given image position.
float getNeighborDistanceChangeScore(const LocalSurface &local_surface, int x, int y, int offset_x, int offset_y, int pixel_radius=1) const
Calculate a border score based on how distant the neighbor is, compared to the closest neighbors /par...
Calculates the weighted average and the covariance matrix.
std::ostream & operator<<(std::ostream &os, const BivariatePolynomialT< real > &p)
Eigen::Vector3f neighborhood_mean_no_jumps
uint32_t width
The point cloud width (if organized as an image-structure).
void add(const Eigen::Matrix< real, dimension, 1 > &sample, real weight=1.0)
Add a new sample.
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
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...
int pixel_radius_border_direction
std::bitset< 32 > BorderTraits
Data type to store extended information about a transition from foreground to backgroundSpecification...
const Eigen::Vector3f getSensorPos() const
Get the sensor position.
bool isInImage(int x, int y) const
Check if a point is inside of the image.
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! ...
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.
PointCloudOut * border_descriptions_