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 atan2f(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  else
82  {
83  //cout << "INF edge -> Setting to 1.0\n";
84  return 1.0f; // TODO: Something more intelligent
85  }
86  }
87 
88  float neighbor_distance_squared = squaredEuclideanDistance(neighbor, point);
89  if (neighbor_distance_squared <= local_surface.max_neighbor_distance_squared)
90  return 0.0f;
91  float ret = 1.0f - std::sqrt (local_surface.max_neighbor_distance_squared / neighbor_distance_squared);
92  if (neighbor.range < point.range)
93  ret = -ret;
94  return ret;
95 }
96 
97 //float RangeImageBorderExtractor::getNormalBasedBorderScore(const RangeImageBorderExtractor::LocalSurface& local_surface,
98  //int x, int y, int offset_x, int offset_y) const
99 //{
100  //PointWithRange neighbor;
101  //range_image_->get1dPointAverage(x+offset_x, y+offset_y, offset_x, offset_y, parameters_.pixel_radius_borders, neighbor);
102  //if (std::isinf(neighbor.range))
103  //{
104  //if (neighbor.range < 0.0f)
105  //return 0.0f;
106  //else
107  //return 1.0f; // TODO: Something more intelligent (Compare normal with viewing direction)
108  //}
109 
110  //float normal_distance_to_plane_squared = local_surface.smallest_eigenvalue_no_jumps;
111  //float distance_to_plane = local_surface.normal_no_jumps.dot(local_surface.neighborhood_mean_no_jumps-neighbor.getVector3fMap());
112  //bool shadow_side = distance_to_plane < 0.0f;
113  //float distance_to_plane_squared = pow(distance_to_plane, 2);
114  //if (distance_to_plane_squared <= normal_distance_to_plane_squared)
115  //return 0.0f;
116  //float ret = 1.0f - (normal_distance_to_plane_squared/distance_to_plane_squared);
117  //if (shadow_side)
118  //ret = -ret;
119  ////cout << PVARC(normal_distance_to_plane_squared)<<PVAR(distance_to_plane_squared)<<" => "<<ret<<"\n";
120  //return ret;
121 //}
122 
123 bool RangeImageBorderExtractor::get3dDirection(const BorderDescription& border_description, Eigen::Vector3f& direction,
124  const LocalSurface* local_surface)
125 {
126  const BorderTraits border_traits = border_description.traits;
127 
128  int delta_x=0, delta_y=0;
129  if (border_traits[BORDER_TRAIT__OBSTACLE_BORDER_RIGHT])
130  ++delta_x;
131  if (border_traits[BORDER_TRAIT__OBSTACLE_BORDER_LEFT])
132  --delta_x;
133  if (border_traits[BORDER_TRAIT__OBSTACLE_BORDER_TOP])
134  --delta_y;
135  if (border_traits[BORDER_TRAIT__OBSTACLE_BORDER_BOTTOM])
136  ++delta_y;
137 
138  if (delta_x==0 && delta_y==0)
139  return false;
140 
141  int x=border_description.x, y=border_description.y;
142  const PointWithRange& point = range_image_->getPoint(x, y);
143  Eigen::Vector3f neighbor_point;
144  range_image_->calculate3DPoint(static_cast<float> (x+delta_x), static_cast<float> (y+delta_y), point.range, neighbor_point);
145  //cout << "Neighborhood point is "<<neighbor_point[0]<<", "<<neighbor_point[1]<<", "<<neighbor_point[2]<<".\n";
146 
147  if (local_surface!=nullptr)
148  {
149  // Get the point that lies on the local plane approximation
150  Eigen::Vector3f sensor_pos = range_image_->getSensorPos(),
151  viewing_direction = neighbor_point-sensor_pos;
152 
153  float lambda = (local_surface->normal_no_jumps.dot(local_surface->neighborhood_mean_no_jumps-sensor_pos)/
154  local_surface->normal_no_jumps.dot(viewing_direction));
155  neighbor_point = lambda*viewing_direction + sensor_pos;
156  //cout << "Neighborhood point projected onto plane is "<<neighbor_point[0]<<", "<<neighbor_point[1]<<", "<<neighbor_point[2]<<".\n";
157  }
158  //cout << point.x<<","<< point.y<<","<< point.z<<" -> "<< direction[0]<<","<< direction[1]<<","<< direction[2]<<"\n";
159  direction = neighbor_point-point.getVector3fMap();
160  direction.normalize();
161 
162  return true;
163 }
164 
166 {
167  int index = y*range_image_->width + x;
168  Eigen::Vector3f*& border_direction = border_directions_[index];
169  border_direction = nullptr;
170  const BorderDescription& border_description = border_descriptions_->points[index];
171  const BorderTraits& border_traits = border_description.traits;
172  if (!border_traits[BORDER_TRAIT__OBSTACLE_BORDER])
173  return;
174  border_direction = new Eigen::Vector3f(0.0f, 0.0f, 0.0f);
175  if (!get3dDirection(border_description, *border_direction, surface_structure_[index]))
176  {
177  delete border_direction;
178  border_direction = nullptr;
179  return;
180  }
181 }
182 
183 bool RangeImageBorderExtractor::changeScoreAccordingToShadowBorderValue(int x, int y, int offset_x, int offset_y, float* border_scores,
184  float* border_scores_other_direction, int& shadow_border_idx) const
185 {
186  float& border_score = border_scores[y*range_image_->width+x];
187 
188  shadow_border_idx = -1;
189  if (border_score<parameters_.minimum_border_probability)
190  return false;
191 
192  if (border_score == 1.0f)
193  { // INF neighbor?
194  if (range_image_->isMaxRange(x+offset_x, y+offset_y))
195  {
196  shadow_border_idx = (y+offset_y)*range_image_->width + x+offset_x;
197  return true;
198  }
199  }
200 
201  float best_shadow_border_score = 0.0f;
202 
203  for (int neighbor_distance=1; neighbor_distance<=parameters_.pixel_radius_borders; ++neighbor_distance)
204  {
205  int neighbor_x=x+neighbor_distance*offset_x, neighbor_y=y+neighbor_distance*offset_y;
206  if (!range_image_->isInImage(neighbor_x, neighbor_y))
207  continue;
208  float neighbor_shadow_border_score = border_scores_other_direction[neighbor_y*range_image_->width+neighbor_x];
209 
210  if (neighbor_shadow_border_score < best_shadow_border_score)
211  {
212  shadow_border_idx = neighbor_y*range_image_->width + neighbor_x;
213  best_shadow_border_score = neighbor_shadow_border_score;
214  }
215  }
216  if (shadow_border_idx >= 0)
217  {
218  //cout << PVARC(border_score)<<PVARN(best_shadow_border_score);
219  //border_score *= (std::max)(0.9f, powf(-best_shadow_border_score, 0.1f)); // TODO: Something better
220  border_score *= (std::max)(0.9f, 1-powf(1+best_shadow_border_score, 3));
221  if (border_score>=parameters_.minimum_border_probability)
222  return true;
223  }
224  shadow_border_idx = -1;
225  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
226  return false;
227 }
228 
229 float RangeImageBorderExtractor::updatedScoreAccordingToNeighborValues(int x, int y, const float* border_scores) const
230 {
231  float max_score_bonus = 0.5f;
232 
233  float border_score = border_scores[y*range_image_->width+x];
234 
235  // Check if an update can bring the score to a value higher than the minimum
236  if (border_score + max_score_bonus*(1.0f-border_score) < parameters_.minimum_border_probability)
237  return border_score;
238 
239  float average_neighbor_score=0.0f, weight_sum=0.0f;
240  for (int y2=y-1; y2<=y+1; ++y2)
241  {
242  for (int x2=x-1; x2<=x+1; ++x2)
243  {
244  if (!range_image_->isInImage(x2, y2) || (x2==x&&y2==y))
245  continue;
246  average_neighbor_score += border_scores[y2*range_image_->width+x2];
247  weight_sum += 1.0f;
248  }
249  }
250  average_neighbor_score /=weight_sum;
251 
252  if (average_neighbor_score*border_score < 0.0f)
253  return border_score;
254 
255  float new_border_score = border_score + max_score_bonus * average_neighbor_score * (1.0f-fabsf(border_score));
256 
257  //std::cout << PVARC(border_score)<<PVARN(new_border_score);
258  return new_border_score;
259 }
260 
261 bool RangeImageBorderExtractor::checkPotentialBorder(int x, int y, int offset_x, int offset_y, float* border_scores,
262  float* border_scores_other_direction, int& shadow_border_idx) const
263 {
264  float& border_score = border_scores[y*range_image_->width+x];
265  if (border_score<parameters_.minimum_border_probability)
266  return false;
267 
268  shadow_border_idx = -1;
269  float best_shadow_border_score = -0.5f*parameters_.minimum_border_probability;
270 
271  for (int neighbor_distance=1; neighbor_distance<=parameters_.pixel_radius_borders; ++neighbor_distance)
272  {
273  int neighbor_x=x+neighbor_distance*offset_x, neighbor_y=y+neighbor_distance*offset_y;
274  if (!range_image_->isInImage(neighbor_x, neighbor_y))
275  continue;
276  float neighbor_shadow_border_score = border_scores_other_direction[neighbor_y*range_image_->width+neighbor_x];
277 
278  if (neighbor_shadow_border_score < best_shadow_border_score)
279  {
280  shadow_border_idx = neighbor_y*range_image_->width + neighbor_x;
281  best_shadow_border_score = neighbor_shadow_border_score;
282  }
283  }
284  if (shadow_border_idx >= 0)
285  {
286  return true;
287  }
288  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
289  return false;
290 }
291 
292 bool RangeImageBorderExtractor::checkIfMaximum(int x, int y, int offset_x, int offset_y, float* border_scores, int shadow_border_idx) const
293 {
294  float border_score = border_scores[y*range_image_->width+x];
295  int neighbor_x=x-offset_x, neighbor_y=y-offset_y;
296  if (range_image_->isInImage(neighbor_x, neighbor_y) && border_scores[neighbor_y*range_image_->width+neighbor_x] > border_score)
297  return false;
298 
299  for (int neighbor_distance=1; neighbor_distance<=parameters_.pixel_radius_borders; ++neighbor_distance)
300  {
301  neighbor_x=x+neighbor_distance*offset_x; neighbor_y=y+neighbor_distance*offset_y;
302  if (!range_image_->isInImage(neighbor_x, neighbor_y))
303  continue;
304  int neighbor_index = neighbor_y*range_image_->width + neighbor_x;
305  if (neighbor_index==shadow_border_idx)
306  return true;
307 
308  float neighbor_border_score = border_scores[neighbor_index];
309  if (neighbor_border_score > border_score)
310  return false;
311  }
312  return true;
313 }
314 
315 bool RangeImageBorderExtractor::calculateMainPrincipalCurvature(int x, int y, int radius, float& magnitude,
316  Eigen::Vector3f& main_direction) const
317 {
318  magnitude = 0.0f;
319  int index = y*range_image_->width+x;
320  LocalSurface* local_surface = surface_structure_[index];
321  if (local_surface==nullptr)
322  return false;
323  //const PointWithRange& point = range_image_->getPointNoCheck(x,y);
324 
325  //Eigen::Vector3f& normal = local_surface->normal_no_jumps;
326  //Eigen::Matrix3f to_tangent_plane = Eigen::Matrix3f::Identity() - normal*normal.transpose();
327 
328  VectorAverage3f vector_average;
329  bool beams_valid[9];
330  for (int step=1; step<=radius; ++step)
331  {
332  int beam_idx = 0;
333  for (int y2=y-step; y2<=y+step; y2+=step)
334  {
335  for (int x2=x-step; x2<=x+step; x2+=step)
336  {
337  bool& beam_valid = beams_valid[beam_idx++];
338  if (step==1)
339  {
340  if (x2==x && y2==y)
341  beam_valid = false;
342  else
343  beam_valid = true;
344  }
345  else
346  if (!beam_valid)
347  continue;
348  //std::cout << x2-x<<","<<y2-y<<" ";
349 
350  if (!range_image_->isValid(x2,y2))
351  continue;
352 
353  int index2 = y2*range_image_->width + x2;
354 
355  const BorderTraits& border_traits = border_descriptions_->points[index2].traits;
356  if (border_traits[BORDER_TRAIT__VEIL_POINT] || border_traits[BORDER_TRAIT__SHADOW_BORDER])
357  {
358  beam_valid = false;
359  continue;
360  }
361 
362  //const PointWithRange& point2 = range_image_->getPoint(index2);
363  LocalSurface* local_surface2 = surface_structure_[index2];
364  if (local_surface2==nullptr)
365  continue;
366  Eigen::Vector3f& normal2 = local_surface2->normal_no_jumps;
367  //float distance_squared = squaredEuclideanDistance(point, point2);
368  //vector_average.add(to_tangent_plane*normal2);
369  vector_average.add(normal2);
370  }
371  }
372  }
373  //std::cout << "\n";
374  if (vector_average.getNoOfSamples() < 3)
375  return false;
376 
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]);
380  //magnitude = eigen_values[2];
381  //magnitude = 1.0f - powf(1.0f-magnitude, 5);
382  //magnitude = 1.0f - powf(1.0f-magnitude, 10);
383  //magnitude += magnitude - powf(magnitude,2);
384  //magnitude += magnitude - powf(magnitude,2);
385 
386  //magnitude = std::sqrt (local_surface->eigen_values[0]/local_surface->eigen_values.sum());
387  //magnitude = std::sqrt (local_surface->eigen_values_no_jumps[0]/local_surface->eigen_values_no_jumps.sum());
388 
389  //if (surface_structure_[y*range_image_->width+x+1]==NULL||surface_structure_[y*range_image_->width+x-1]==NULL)
390  //{
391  //magnitude = -std::numeric_limits<float>::infinity ();
392  //return false;
393  //}
394  //float angle2 = acosf(surface_structure_[y*range_image_->width+x+1]->normal.dot(local_surface->normal)),
395  //angle1 = acosf(surface_structure_[y*range_image_->width+x-1]->normal.dot(local_surface->normal));
396  //magnitude = angle2-angle1;
397 
398  return std::isfinite(magnitude);
399 }
400 
401 } // namespace end
bool isValid(int x, int y) const
Check if a point is inside of the image and has a finite range.
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:409
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.
This file defines compatibility wrappers for low level I/O functions.
Definition: convolution.h:44
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...
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 Eigen::Vector3f getSensorPos() const
Get the sensor position.
float squaredEuclideanDistance(const PointType1 &p1, const PointType2 &p2)
Calculate the squared euclidean distance between the two given points.
Definition: distances.h:174
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.
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.
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.
uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:412
bool isInImage(int x, int y) const
Check if a point is inside of the image.
unsigned int getNoOfSamples()
Get the number of added vectors.
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...
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...
Stores some information extracted from the neighborhood of a point.
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...
static float getObstacleBorderAngle(const BorderTraits &border_traits)
Take the information from BorderTraits to calculate the local direction of the border.
A structure to store if a point in a range image lies on a border between an obstacle and the backgro...
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 ...
void calculateBorderDirection(int x, int y)
Calculate the 3D direction of the border just using the border traits at this position (facing away f...
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...
Definition: point_types.h:306
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 isMaxRange(int x, int y) const
Check if a point is a max range (range=INFINITY) - please check isInImage or isObserved first! ...