Point Cloud Library (PCL) 1.12.0
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 //////////
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
60inline 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
120bool 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_)[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
180bool 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;
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
226float 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
258bool 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];
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
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;
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
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;
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_)[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::uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:398
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...
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 isValid(int x, int y) const
Check if a point is inside of the image and has a finite 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,...
const PointWithRange & getPoint(int image_x, int image_y) const
Return the 3D point with range at the given image position.
bool isMaxRange(int x, int y) const
Check if a point is a max range (range=INFINITY) - please check isInImage or isObserved first!
bool isInImage(int x, int y) const
Check if a point is inside of the image.
const Eigen::Vector3f getSensorPos() const
Get the sensor position.
Calculates the weighted average and the covariance matrix.
void add(const VectorType &sample, real weight=1.0)
Add a new sample.
void doPCA(VectorType &eigen_values, VectorType &eigen_vector1, VectorType &eigen_vector2, VectorType &eigen_vector3) const
Do Principal component analysis.
unsigned int getNoOfSamples()
Get the number of added vectors.
std::bitset< 32 > BorderTraits
Data type to store extended information about a transition from foreground to backgroundSpecification...
Definition: point_types.h:307
@ BORDER_TRAIT__OBSTACLE_BORDER_TOP
Definition: point_types.h:316
@ BORDER_TRAIT__OBSTACLE_BORDER_LEFT
Definition: point_types.h:317
@ BORDER_TRAIT__OBSTACLE_BORDER_RIGHT
Definition: point_types.h:316
@ BORDER_TRAIT__OBSTACLE_BORDER_BOTTOM
Definition: point_types.h:317
@ BORDER_TRAIT__OBSTACLE_BORDER
Definition: point_types.h:314
@ BORDER_TRAIT__SHADOW_BORDER
Definition: point_types.h:314
@ BORDER_TRAIT__VEIL_POINT
Definition: point_types.h:314
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.