Point Cloud Library (PCL)  1.7.1
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 (pcl_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 - sqrtf(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 (pcl_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!=NULL)
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 = NULL;
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 = NULL;
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==NULL)
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==NULL)
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 = sqrtf(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 = sqrtf(local_surface->eigen_values[0]/local_surface->eigen_values.sum());
387  //magnitude = sqrtf(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  if (!pcl_isfinite(magnitude))
399  return false;
400 
401  return true;
402 }
403 
404 } // namespace end