Point Cloud Library (PCL)  1.9.1-dev
range_image_border_extractor.hpp
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2010, Willow Garage, Inc.
6  * Copyright (c) 2012-, Open Perception, Inc.
7  *
8  * All rights reserved.
9  *
10  * Redistribution and use in source and binary forms, with or without
11  * modification, are permitted provided that the following conditions
12  * are met:
13  *
14  * * Redistributions of source code must retain the above copyright
15  * notice, this list of conditions and the following disclaimer.
16  * * Redistributions in binary form must reproduce the above
17  * copyright notice, this list of conditions and the following
18  * disclaimer in the documentation and/or other materials provided
19  * with the distribution.
20  * * Neither the name of the copyright holder(s) nor the names of its
21  * contributors may be used to endorse or promote products derived
22  * from this software without specific prior written permission.
23  *
24  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
25  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
26  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
27  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
28  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
29  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
30  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
31  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
32  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
33  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
34  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
35  * POSSIBILITY OF SUCH DAMAGE.
36  *
37  * Author: Bastian Steder
38  */
39 
40 #include <pcl/range_image/range_image.h>
41 
42 namespace pcl {
43 
44 ////////// STATIC //////////
46 {
47  float x=0.0f, y=0.0f;
48  if (border_traits[BORDER_TRAIT__OBSTACLE_BORDER_RIGHT])
49  ++x;
50  if (border_traits[BORDER_TRAIT__OBSTACLE_BORDER_LEFT])
51  --x;
52  if (border_traits[BORDER_TRAIT__OBSTACLE_BORDER_TOP])
53  --y;
54  if (border_traits[BORDER_TRAIT__OBSTACLE_BORDER_BOTTOM])
55  ++y;
56 
57  return std::atan2(y, x);
58 }
59 
60 inline std::ostream& operator << (std::ostream& os, const RangeImageBorderExtractor::Parameters& p)
61 {
64  return (os);
65 }
66 
67 ////////// NON-STATIC //////////
68 
69 
71  const RangeImageBorderExtractor::LocalSurface& local_surface,
72  int x, int y, int offset_x, int offset_y, int pixel_radius) const
73 {
74  const PointWithRange& point = range_image_->getPoint(x, y);
75  PointWithRange neighbor;
76  range_image_->get1dPointAverage(x+offset_x, y+offset_y, offset_x, offset_y, pixel_radius, neighbor);
77  if (std::isinf(neighbor.range))
78  {
79  if (neighbor.range < 0.0f)
80  return 0.0f;
81  //std::cout << "INF edge -> Setting to 1.0\n";
82  return 1.0f; // TODO: Something more intelligent
83  }
84 
85  float neighbor_distance_squared = squaredEuclideanDistance(neighbor, point);
86  if (neighbor_distance_squared <= local_surface.max_neighbor_distance_squared)
87  return 0.0f;
88  float ret = 1.0f - std::sqrt (local_surface.max_neighbor_distance_squared / neighbor_distance_squared);
89  if (neighbor.range < point.range)
90  ret = -ret;
91  return ret;
92 }
93 
94 //float RangeImageBorderExtractor::getNormalBasedBorderScore(const RangeImageBorderExtractor::LocalSurface& local_surface,
95  //int x, int y, int offset_x, int offset_y) const
96 //{
97  //PointWithRange neighbor;
98  //range_image_->get1dPointAverage(x+offset_x, y+offset_y, offset_x, offset_y, parameters_.pixel_radius_borders, neighbor);
99  //if (std::isinf(neighbor.range))
100  //{
101  //if (neighbor.range < 0.0f)
102  //return 0.0f;
103  //else
104  //return 1.0f; // TODO: Something more intelligent (Compare normal with viewing direction)
105  //}
106 
107  //float normal_distance_to_plane_squared = local_surface.smallest_eigenvalue_no_jumps;
108  //float distance_to_plane = local_surface.normal_no_jumps.dot(local_surface.neighborhood_mean_no_jumps-neighbor.getVector3fMap());
109  //bool shadow_side = distance_to_plane < 0.0f;
110  //float distance_to_plane_squared = pow(distance_to_plane, 2);
111  //if (distance_to_plane_squared <= normal_distance_to_plane_squared)
112  //return 0.0f;
113  //float ret = 1.0f - (normal_distance_to_plane_squared/distance_to_plane_squared);
114  //if (shadow_side)
115  //ret = -ret;
116  ////std::cout << PVARC(normal_distance_to_plane_squared)<<PVAR(distance_to_plane_squared)<<" => "<<ret<<"\n";
117  //return ret;
118 //}
119 
120 bool RangeImageBorderExtractor::get3dDirection(const BorderDescription& border_description, Eigen::Vector3f& direction,
121  const LocalSurface* local_surface)
122 {
123  const BorderTraits border_traits = border_description.traits;
124 
125  int delta_x=0, delta_y=0;
126  if (border_traits[BORDER_TRAIT__OBSTACLE_BORDER_RIGHT])
127  ++delta_x;
128  if (border_traits[BORDER_TRAIT__OBSTACLE_BORDER_LEFT])
129  --delta_x;
130  if (border_traits[BORDER_TRAIT__OBSTACLE_BORDER_TOP])
131  --delta_y;
132  if (border_traits[BORDER_TRAIT__OBSTACLE_BORDER_BOTTOM])
133  ++delta_y;
134 
135  if (delta_x==0 && delta_y==0)
136  return false;
137 
138  int x=border_description.x, y=border_description.y;
139  const PointWithRange& point = range_image_->getPoint(x, y);
140  Eigen::Vector3f neighbor_point;
141  range_image_->calculate3DPoint(static_cast<float> (x+delta_x), static_cast<float> (y+delta_y), point.range, neighbor_point);
142  //std::cout << "Neighborhood point is "<<neighbor_point[0]<<", "<<neighbor_point[1]<<", "<<neighbor_point[2]<<".\n";
143 
144  if (local_surface!=nullptr)
145  {
146  // Get the point that lies on the local plane approximation
147  Eigen::Vector3f sensor_pos = range_image_->getSensorPos(),
148  viewing_direction = neighbor_point-sensor_pos;
149 
150  float lambda = (local_surface->normal_no_jumps.dot(local_surface->neighborhood_mean_no_jumps-sensor_pos)/
151  local_surface->normal_no_jumps.dot(viewing_direction));
152  neighbor_point = lambda*viewing_direction + sensor_pos;
153  //std::cout << "Neighborhood point projected onto plane is "<<neighbor_point[0]<<", "<<neighbor_point[1]<<", "<<neighbor_point[2]<<".\n";
154  }
155  //std::cout << point.x<<","<< point.y<<","<< point.z<<" -> "<< direction[0]<<","<< direction[1]<<","<< direction[2]<<"\n";
156  direction = neighbor_point-point.getVector3fMap();
157  direction.normalize();
158 
159  return true;
160 }
161 
163 {
164  int index = y*range_image_->width + x;
165  Eigen::Vector3f*& border_direction = border_directions_[index];
166  border_direction = nullptr;
167  const BorderDescription& border_description = border_descriptions_->points[index];
168  const BorderTraits& border_traits = border_description.traits;
169  if (!border_traits[BORDER_TRAIT__OBSTACLE_BORDER])
170  return;
171  border_direction = new Eigen::Vector3f(0.0f, 0.0f, 0.0f);
172  if (!get3dDirection(border_description, *border_direction, surface_structure_[index]))
173  {
174  delete border_direction;
175  border_direction = nullptr;
176  return;
177  }
178 }
179 
180 bool RangeImageBorderExtractor::changeScoreAccordingToShadowBorderValue(int x, int y, int offset_x, int offset_y, float* border_scores,
181  float* border_scores_other_direction, int& shadow_border_idx) const
182 {
183  float& border_score = border_scores[y*range_image_->width+x];
184 
185  shadow_border_idx = -1;
186  if (border_score<parameters_.minimum_border_probability)
187  return false;
188 
189  if (border_score == 1.0f)
190  { // INF neighbor?
191  if (range_image_->isMaxRange(x+offset_x, y+offset_y))
192  {
193  shadow_border_idx = (y+offset_y)*range_image_->width + x+offset_x;
194  return true;
195  }
196  }
197 
198  float best_shadow_border_score = 0.0f;
199 
200  for (int neighbor_distance=1; neighbor_distance<=parameters_.pixel_radius_borders; ++neighbor_distance)
201  {
202  int neighbor_x=x+neighbor_distance*offset_x, neighbor_y=y+neighbor_distance*offset_y;
203  if (!range_image_->isInImage(neighbor_x, neighbor_y))
204  continue;
205  float neighbor_shadow_border_score = border_scores_other_direction[neighbor_y*range_image_->width+neighbor_x];
206 
207  if (neighbor_shadow_border_score < best_shadow_border_score)
208  {
209  shadow_border_idx = neighbor_y*range_image_->width + neighbor_x;
210  best_shadow_border_score = neighbor_shadow_border_score;
211  }
212  }
213  if (shadow_border_idx >= 0)
214  {
215  //std::cout << PVARC(border_score)<<PVARN(best_shadow_border_score);
216  //border_score *= (std::max)(0.9f, powf(-best_shadow_border_score, 0.1f)); // TODO: Something better
217  border_score *= (std::max)(0.9f, 1-powf(1+best_shadow_border_score, 3));
218  if (border_score>=parameters_.minimum_border_probability)
219  return true;
220  }
221  shadow_border_idx = -1;
222  border_score = 0.0f; // Since there was no shadow border found we set this value to zero, so that it does not influence the maximum search
223  return false;
224 }
225 
226 float RangeImageBorderExtractor::updatedScoreAccordingToNeighborValues(int x, int y, const float* border_scores) const
227 {
228  float max_score_bonus = 0.5f;
229 
230  float border_score = border_scores[y*range_image_->width+x];
231 
232  // Check if an update can bring the score to a value higher than the minimum
233  if (border_score + max_score_bonus*(1.0f-border_score) < parameters_.minimum_border_probability)
234  return border_score;
235 
236  float average_neighbor_score=0.0f, weight_sum=0.0f;
237  for (int y2=y-1; y2<=y+1; ++y2)
238  {
239  for (int x2=x-1; x2<=x+1; ++x2)
240  {
241  if (!range_image_->isInImage(x2, y2) || (x2==x&&y2==y))
242  continue;
243  average_neighbor_score += border_scores[y2*range_image_->width+x2];
244  weight_sum += 1.0f;
245  }
246  }
247  average_neighbor_score /=weight_sum;
248 
249  if (average_neighbor_score*border_score < 0.0f)
250  return border_score;
251 
252  float new_border_score = border_score + max_score_bonus * average_neighbor_score * (1.0f-std::abs(border_score));
253 
254  //std::cout << PVARC(border_score)<<PVARN(new_border_score);
255  return new_border_score;
256 }
257 
258 bool RangeImageBorderExtractor::checkPotentialBorder(int x, int y, int offset_x, int offset_y, float* border_scores,
259  float* border_scores_other_direction, int& shadow_border_idx) const
260 {
261  float& border_score = border_scores[y*range_image_->width+x];
262  if (border_score<parameters_.minimum_border_probability)
263  return false;
264 
265  shadow_border_idx = -1;
266  float best_shadow_border_score = -0.5f*parameters_.minimum_border_probability;
267 
268  for (int neighbor_distance=1; neighbor_distance<=parameters_.pixel_radius_borders; ++neighbor_distance)
269  {
270  int neighbor_x=x+neighbor_distance*offset_x, neighbor_y=y+neighbor_distance*offset_y;
271  if (!range_image_->isInImage(neighbor_x, neighbor_y))
272  continue;
273  float neighbor_shadow_border_score = border_scores_other_direction[neighbor_y*range_image_->width+neighbor_x];
274 
275  if (neighbor_shadow_border_score < best_shadow_border_score)
276  {
277  shadow_border_idx = neighbor_y*range_image_->width + neighbor_x;
278  best_shadow_border_score = neighbor_shadow_border_score;
279  }
280  }
281  if (shadow_border_idx >= 0)
282  {
283  return true;
284  }
285  border_score = 0.0f; // Since there was no shadow border found we set this value to zero, so that it does not influence the maximum search
286  return false;
287 }
288 
289 bool RangeImageBorderExtractor::checkIfMaximum(int x, int y, int offset_x, int offset_y, float* border_scores, int shadow_border_idx) const
290 {
291  float border_score = border_scores[y*range_image_->width+x];
292  int neighbor_x=x-offset_x, neighbor_y=y-offset_y;
293  if (range_image_->isInImage(neighbor_x, neighbor_y) && border_scores[neighbor_y*range_image_->width+neighbor_x] > border_score)
294  return false;
295 
296  for (int neighbor_distance=1; neighbor_distance<=parameters_.pixel_radius_borders; ++neighbor_distance)
297  {
298  neighbor_x=x+neighbor_distance*offset_x; neighbor_y=y+neighbor_distance*offset_y;
299  if (!range_image_->isInImage(neighbor_x, neighbor_y))
300  continue;
301  int neighbor_index = neighbor_y*range_image_->width + neighbor_x;
302  if (neighbor_index==shadow_border_idx)
303  return true;
304 
305  float neighbor_border_score = border_scores[neighbor_index];
306  if (neighbor_border_score > border_score)
307  return false;
308  }
309  return true;
310 }
311 
312 bool RangeImageBorderExtractor::calculateMainPrincipalCurvature(int x, int y, int radius, float& magnitude,
313  Eigen::Vector3f& main_direction) const
314 {
315  magnitude = 0.0f;
316  int index = y*range_image_->width+x;
317  LocalSurface* local_surface = surface_structure_[index];
318  if (local_surface==nullptr)
319  return false;
320  //const PointWithRange& point = range_image_->getPointNoCheck(x,y);
321 
322  //Eigen::Vector3f& normal = local_surface->normal_no_jumps;
323  //Eigen::Matrix3f to_tangent_plane = Eigen::Matrix3f::Identity() - normal*normal.transpose();
324 
325  VectorAverage3f vector_average;
326  bool beams_valid[9];
327  for (int step=1; step<=radius; ++step)
328  {
329  int beam_idx = 0;
330  for (int y2=y-step; y2<=y+step; y2+=step)
331  {
332  for (int x2=x-step; x2<=x+step; x2+=step)
333  {
334  bool& beam_valid = beams_valid[beam_idx++];
335  if (step==1)
336  {
337  if (x2==x && y2==y)
338  beam_valid = false;
339  else
340  beam_valid = true;
341  }
342  else
343  if (!beam_valid)
344  continue;
345  //std::cout << x2-x<<","<<y2-y<<" ";
346 
347  if (!range_image_->isValid(x2,y2))
348  continue;
349 
350  int index2 = y2*range_image_->width + x2;
351 
352  const BorderTraits& border_traits = border_descriptions_->points[index2].traits;
353  if (border_traits[BORDER_TRAIT__VEIL_POINT] || border_traits[BORDER_TRAIT__SHADOW_BORDER])
354  {
355  beam_valid = false;
356  continue;
357  }
358 
359  //const PointWithRange& point2 = range_image_->getPoint(index2);
360  LocalSurface* local_surface2 = surface_structure_[index2];
361  if (local_surface2==nullptr)
362  continue;
363  Eigen::Vector3f& normal2 = local_surface2->normal_no_jumps;
364  //float distance_squared = squaredEuclideanDistance(point, point2);
365  //vector_average.add(to_tangent_plane*normal2);
366  vector_average.add(normal2);
367  }
368  }
369  }
370  //std::cout << "\n";
371  if (vector_average.getNoOfSamples() < 3)
372  return false;
373 
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]);
377  //magnitude = eigen_values[2];
378  //magnitude = 1.0f - powf(1.0f-magnitude, 5);
379  //magnitude = 1.0f - powf(1.0f-magnitude, 10);
380  //magnitude += magnitude - powf(magnitude,2);
381  //magnitude += magnitude - powf(magnitude,2);
382 
383  //magnitude = std::sqrt (local_surface->eigen_values[0]/local_surface->eigen_values.sum());
384  //magnitude = std::sqrt (local_surface->eigen_values_no_jumps[0]/local_surface->eigen_values_no_jumps.sum());
385 
386  //if (surface_structure_[y*range_image_->width+x+1]==NULL||surface_structure_[y*range_image_->width+x-1]==NULL)
387  //{
388  //magnitude = -std::numeric_limits<float>::infinity ();
389  //return false;
390  //}
391  //float angle2 = std::acos(surface_structure_[y*range_image_->width+x+1]->normal.dot(local_surface->normal)),
392  //angle1 = std::acos(surface_structure_[y*range_image_->width+x-1]->normal.dot(local_surface->normal));
393  //magnitude = angle2-angle1;
394 
395  return std::isfinite(magnitude);
396 }
397 
398 } // namespace end
std::bitset< 32 > BorderTraits
Data type to store extended information about a transition from foreground to backgroundSpecification...
Definition: point_types.h:311
A point structure representing Euclidean xyz coordinates, padded with an extra range float...
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
Definition: point_cloud.h:426
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...
#define PVARC(s)
Definition: pcl_macros.h:172
This file defines compatibility wrappers for low level I/O functions.
Definition: convolution.h:45
bool get3dDirection(const BorderDescription &border_description, Eigen::Vector3f &direction, const LocalSurface *local_surface=nullptr)
Calculate a 3d direction from a border point by projecting the direction in the range image - returns...
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...
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.
Definition: distances.h:178
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.
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...
uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:429
#define PVARN(s)
Definition: pcl_macros.h:168
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.
Stores some information extracted from the neighborhood of a point.
const PointWithRange & getPoint(int image_x, int image_y) const
Return the 3D point with range at the given image position.
static float getObstacleBorderAngle(const BorderTraits &border_traits)
Take the information from BorderTraits to calculate the local direction of the border.
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.
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 calculateBorderDirection(int x, int y)
Calculate the 3D direction of the border just using the border traits at this position (facing away f...
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.
Calculates the weighted average and the covariance matrix.
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 ...