Point Cloud Library (PCL) 1.12.0
Loading...
Searching...
No Matches
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
42namespace pcl {
43
44////////// STATIC //////////
59
66
67////////// NON-STATIC //////////
68
69
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
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
122{
124
125 int delta_x=0, delta_y=0;
127 ++delta_x;
129 --delta_x;
131 --delta_y;
133 ++delta_y;
134
135 if (delta_x==0 && delta_y==0)
136 return false;
137
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(),
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));
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_)[index];
170 return;
171 border_direction = new Eigen::Vector3f(0.0f, 0.0f, 0.0f);
173 {
174 delete border_direction;
175 border_direction = nullptr;
176 return;
177 }
178}
179
180bool RangeImageBorderExtractor::changeScoreAccordingToShadowBorderValue(int x, int y, int offset_x, int offset_y, float* border_scores,
182{
183 float& border_score = border_scores[y*range_image_->width+x];
184
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
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;
206
208 {
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));
219 return true;
220 }
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
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
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;
244 weight_sum += 1.0f;
245 }
246 }
248
250 return border_score;
251
253
254 //std::cout << PVARC(border_score)<<PVARN(new_border_score);
255 return new_border_score;
256}
257
258bool RangeImageBorderExtractor::checkPotentialBorder(int x, int y, int offset_x, int offset_y, float* border_scores,
260{
261 float& border_score = border_scores[y*range_image_->width+x];
263 return false;
264
267
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;
274
276 {
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
289bool 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;
294 return false;
295
297 {
299 if (!range_image_->isInImage(neighbor_x, neighbor_y))
300 continue;
303 return true;
304
307 return false;
308 }
309 return true;
310}
311
312bool 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;
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
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 {
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_)[index2].traits;
354 {
355 beam_valid = false;
356 continue;
357 }
358
359 //const PointWithRange& point2 = range_image_->getPoint(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);
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;
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
Iterator class for point clouds with or without given indices.
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.
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.
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.
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 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 ...
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.
void calculateBorderDirection(int x, int y)
Calculate the 3D direction of the border just using the border traits at this position (facing away f...
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...
std::bitset< 32 > BorderTraits
Data type to store extended information about a transition from foreground to backgroundSpecification...
@ BORDER_TRAIT__OBSTACLE_BORDER_TOP
@ BORDER_TRAIT__OBSTACLE_BORDER_LEFT
@ BORDER_TRAIT__OBSTACLE_BORDER_RIGHT
@ BORDER_TRAIT__OBSTACLE_BORDER_BOTTOM
@ BORDER_TRAIT__OBSTACLE_BORDER
@ BORDER_TRAIT__SHADOW_BORDER
@ BORDER_TRAIT__VEIL_POINT
float squaredEuclideanDistance(const PointType1 &p1, const PointType2 &p2)
Calculate the squared euclidean distance between the two given points.
Definition distances.h:182
std::ostream & operator<<(std::ostream &os, const BivariatePolynomialT< real > &p)
#define PVARN(s)
Definition pcl_macros.h:268
#define PVARC(s)
Definition pcl_macros.h:272
A structure to store if a point in a range image lies on a border between an obstacle and the backgro...
A point structure representing Euclidean xyz coordinates, padded with an extra range float.
Stores some information extracted from the neighborhood of a point.