Point Cloud Library (PCL)
1.7.0
|
00001 /* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Point Cloud Library (PCL) - www.pointclouds.org 00005 * Copyright (c) 2010, Willow Garage, Inc. 00006 * Copyright (c) 2012-, Open Perception, Inc. 00007 * 00008 * All rights reserved. 00009 * 00010 * Redistribution and use in source and binary forms, with or without 00011 * modification, are permitted provided that the following conditions 00012 * are met: 00013 * 00014 * * Redistributions of source code must retain the above copyright 00015 * notice, this list of conditions and the following disclaimer. 00016 * * Redistributions in binary form must reproduce the above 00017 * copyright notice, this list of conditions and the following 00018 * disclaimer in the documentation and/or other materials provided 00019 * with the distribution. 00020 * * Neither the name of the copyright holder(s) nor the names of its 00021 * contributors may be used to endorse or promote products derived 00022 * from this software without specific prior written permission. 00023 * 00024 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00025 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00026 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00027 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00028 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00029 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00030 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00031 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00032 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00033 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00034 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00035 * POSSIBILITY OF SUCH DAMAGE. 00036 * 00037 * Author: Bastian Steder 00038 */ 00039 00040 #include <pcl/range_image/range_image.h> 00041 00042 namespace pcl { 00043 00044 ////////// STATIC ////////// 00045 float RangeImageBorderExtractor::getObstacleBorderAngle(const BorderTraits& border_traits) 00046 { 00047 float x=0.0f, y=0.0f; 00048 if (border_traits[BORDER_TRAIT__OBSTACLE_BORDER_RIGHT]) 00049 ++x; 00050 if (border_traits[BORDER_TRAIT__OBSTACLE_BORDER_LEFT]) 00051 --x; 00052 if (border_traits[BORDER_TRAIT__OBSTACLE_BORDER_TOP]) 00053 --y; 00054 if (border_traits[BORDER_TRAIT__OBSTACLE_BORDER_BOTTOM]) 00055 ++y; 00056 00057 return atan2f(y, x); 00058 } 00059 00060 inline std::ostream& operator << (std::ostream& os, const RangeImageBorderExtractor::Parameters& p) 00061 { 00062 os << PVARC(p.pixel_radius_borders)<<PVARC(p.pixel_radius_plane_extraction)<<PVARC(p.pixel_radius_border_direction) 00063 << PVARC(p.minimum_border_probability)<<PVARN(p.pixel_radius_principal_curvature); 00064 return (os); 00065 } 00066 00067 ////////// NON-STATIC ////////// 00068 00069 00070 float RangeImageBorderExtractor::getNeighborDistanceChangeScore( 00071 const RangeImageBorderExtractor::LocalSurface& local_surface, 00072 int x, int y, int offset_x, int offset_y, int pixel_radius) const 00073 { 00074 const PointWithRange& point = range_image_->getPoint(x, y); 00075 PointWithRange neighbor; 00076 range_image_->get1dPointAverage(x+offset_x, y+offset_y, offset_x, offset_y, pixel_radius, neighbor); 00077 if (pcl_isinf(neighbor.range)) 00078 { 00079 if (neighbor.range < 0.0f) 00080 return 0.0f; 00081 else 00082 { 00083 //cout << "INF edge -> Setting to 1.0\n"; 00084 return 1.0f; // TODO: Something more intelligent 00085 } 00086 } 00087 00088 float neighbor_distance_squared = squaredEuclideanDistance(neighbor, point); 00089 if (neighbor_distance_squared <= local_surface.max_neighbor_distance_squared) 00090 return 0.0f; 00091 float ret = 1.0f - sqrtf(local_surface.max_neighbor_distance_squared / neighbor_distance_squared); 00092 if (neighbor.range < point.range) 00093 ret = -ret; 00094 return ret; 00095 } 00096 00097 //float RangeImageBorderExtractor::getNormalBasedBorderScore(const RangeImageBorderExtractor::LocalSurface& local_surface, 00098 //int x, int y, int offset_x, int offset_y) const 00099 //{ 00100 //PointWithRange neighbor; 00101 //range_image_->get1dPointAverage(x+offset_x, y+offset_y, offset_x, offset_y, parameters_.pixel_radius_borders, neighbor); 00102 //if (pcl_isinf(neighbor.range)) 00103 //{ 00104 //if (neighbor.range < 0.0f) 00105 //return 0.0f; 00106 //else 00107 //return 1.0f; // TODO: Something more intelligent (Compare normal with viewing direction) 00108 //} 00109 00110 //float normal_distance_to_plane_squared = local_surface.smallest_eigenvalue_no_jumps; 00111 //float distance_to_plane = local_surface.normal_no_jumps.dot(local_surface.neighborhood_mean_no_jumps-neighbor.getVector3fMap()); 00112 //bool shadow_side = distance_to_plane < 0.0f; 00113 //float distance_to_plane_squared = pow(distance_to_plane, 2); 00114 //if (distance_to_plane_squared <= normal_distance_to_plane_squared) 00115 //return 0.0f; 00116 //float ret = 1.0f - (normal_distance_to_plane_squared/distance_to_plane_squared); 00117 //if (shadow_side) 00118 //ret = -ret; 00119 ////cout << PVARC(normal_distance_to_plane_squared)<<PVAR(distance_to_plane_squared)<<" => "<<ret<<"\n"; 00120 //return ret; 00121 //} 00122 00123 bool RangeImageBorderExtractor::get3dDirection(const BorderDescription& border_description, Eigen::Vector3f& direction, 00124 const LocalSurface* local_surface) 00125 { 00126 const BorderTraits border_traits = border_description.traits; 00127 00128 int delta_x=0, delta_y=0; 00129 if (border_traits[BORDER_TRAIT__OBSTACLE_BORDER_RIGHT]) 00130 ++delta_x; 00131 if (border_traits[BORDER_TRAIT__OBSTACLE_BORDER_LEFT]) 00132 --delta_x; 00133 if (border_traits[BORDER_TRAIT__OBSTACLE_BORDER_TOP]) 00134 --delta_y; 00135 if (border_traits[BORDER_TRAIT__OBSTACLE_BORDER_BOTTOM]) 00136 ++delta_y; 00137 00138 if (delta_x==0 && delta_y==0) 00139 return false; 00140 00141 int x=border_description.x, y=border_description.y; 00142 const PointWithRange& point = range_image_->getPoint(x, y); 00143 Eigen::Vector3f neighbor_point; 00144 range_image_->calculate3DPoint(static_cast<float> (x+delta_x), static_cast<float> (y+delta_y), point.range, neighbor_point); 00145 //cout << "Neighborhood point is "<<neighbor_point[0]<<", "<<neighbor_point[1]<<", "<<neighbor_point[2]<<".\n"; 00146 00147 if (local_surface!=NULL) 00148 { 00149 // Get the point that lies on the local plane approximation 00150 Eigen::Vector3f sensor_pos = range_image_->getSensorPos(), 00151 viewing_direction = neighbor_point-sensor_pos; 00152 00153 float lambda = (local_surface->normal_no_jumps.dot(local_surface->neighborhood_mean_no_jumps-sensor_pos)/ 00154 local_surface->normal_no_jumps.dot(viewing_direction)); 00155 neighbor_point = lambda*viewing_direction + sensor_pos; 00156 //cout << "Neighborhood point projected onto plane is "<<neighbor_point[0]<<", "<<neighbor_point[1]<<", "<<neighbor_point[2]<<".\n"; 00157 } 00158 //cout << point.x<<","<< point.y<<","<< point.z<<" -> "<< direction[0]<<","<< direction[1]<<","<< direction[2]<<"\n"; 00159 direction = neighbor_point-point.getVector3fMap(); 00160 direction.normalize(); 00161 00162 return true; 00163 } 00164 00165 void RangeImageBorderExtractor::calculateBorderDirection(int x, int y) 00166 { 00167 int index = y*range_image_->width + x; 00168 Eigen::Vector3f*& border_direction = border_directions_[index]; 00169 border_direction = NULL; 00170 const BorderDescription& border_description = border_descriptions_->points[index]; 00171 const BorderTraits& border_traits = border_description.traits; 00172 if (!border_traits[BORDER_TRAIT__OBSTACLE_BORDER]) 00173 return; 00174 border_direction = new Eigen::Vector3f(0.0f, 0.0f, 0.0f); 00175 if (!get3dDirection(border_description, *border_direction, surface_structure_[index])) 00176 { 00177 delete border_direction; 00178 border_direction = NULL; 00179 return; 00180 } 00181 } 00182 00183 bool RangeImageBorderExtractor::changeScoreAccordingToShadowBorderValue(int x, int y, int offset_x, int offset_y, float* border_scores, 00184 float* border_scores_other_direction, int& shadow_border_idx) const 00185 { 00186 float& border_score = border_scores[y*range_image_->width+x]; 00187 00188 shadow_border_idx = -1; 00189 if (border_score<parameters_.minimum_border_probability) 00190 return false; 00191 00192 if (border_score == 1.0f) 00193 { // INF neighbor? 00194 if (range_image_->isMaxRange(x+offset_x, y+offset_y)) 00195 { 00196 shadow_border_idx = (y+offset_y)*range_image_->width + x+offset_x; 00197 return true; 00198 } 00199 } 00200 00201 float best_shadow_border_score = 0.0f; 00202 00203 for (int neighbor_distance=1; neighbor_distance<=parameters_.pixel_radius_borders; ++neighbor_distance) 00204 { 00205 int neighbor_x=x+neighbor_distance*offset_x, neighbor_y=y+neighbor_distance*offset_y; 00206 if (!range_image_->isInImage(neighbor_x, neighbor_y)) 00207 continue; 00208 float neighbor_shadow_border_score = border_scores_other_direction[neighbor_y*range_image_->width+neighbor_x]; 00209 00210 if (neighbor_shadow_border_score < best_shadow_border_score) 00211 { 00212 shadow_border_idx = neighbor_y*range_image_->width + neighbor_x; 00213 best_shadow_border_score = neighbor_shadow_border_score; 00214 } 00215 } 00216 if (shadow_border_idx >= 0) 00217 { 00218 //cout << PVARC(border_score)<<PVARN(best_shadow_border_score); 00219 //border_score *= (std::max)(0.9f, powf(-best_shadow_border_score, 0.1f)); // TODO: Something better 00220 border_score *= (std::max)(0.9f, 1-powf(1+best_shadow_border_score, 3)); 00221 if (border_score>=parameters_.minimum_border_probability) 00222 return true; 00223 } 00224 shadow_border_idx = -1; 00225 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 00226 return false; 00227 } 00228 00229 float RangeImageBorderExtractor::updatedScoreAccordingToNeighborValues(int x, int y, const float* border_scores) const 00230 { 00231 float max_score_bonus = 0.5f; 00232 00233 float border_score = border_scores[y*range_image_->width+x]; 00234 00235 // Check if an update can bring the score to a value higher than the minimum 00236 if (border_score + max_score_bonus*(1.0f-border_score) < parameters_.minimum_border_probability) 00237 return border_score; 00238 00239 float average_neighbor_score=0.0f, weight_sum=0.0f; 00240 for (int y2=y-1; y2<=y+1; ++y2) 00241 { 00242 for (int x2=x-1; x2<=x+1; ++x2) 00243 { 00244 if (!range_image_->isInImage(x2, y2) || (x2==x&&y2==y)) 00245 continue; 00246 average_neighbor_score += border_scores[y2*range_image_->width+x2]; 00247 weight_sum += 1.0f; 00248 } 00249 } 00250 average_neighbor_score /=weight_sum; 00251 00252 if (average_neighbor_score*border_score < 0.0f) 00253 return border_score; 00254 00255 float new_border_score = border_score + max_score_bonus * average_neighbor_score * (1.0f-fabsf(border_score)); 00256 00257 //std::cout << PVARC(border_score)<<PVARN(new_border_score); 00258 return new_border_score; 00259 } 00260 00261 bool RangeImageBorderExtractor::checkPotentialBorder(int x, int y, int offset_x, int offset_y, float* border_scores, 00262 float* border_scores_other_direction, int& shadow_border_idx) const 00263 { 00264 float& border_score = border_scores[y*range_image_->width+x]; 00265 if (border_score<parameters_.minimum_border_probability) 00266 return false; 00267 00268 shadow_border_idx = -1; 00269 float best_shadow_border_score = -0.5f*parameters_.minimum_border_probability; 00270 00271 for (int neighbor_distance=1; neighbor_distance<=parameters_.pixel_radius_borders; ++neighbor_distance) 00272 { 00273 int neighbor_x=x+neighbor_distance*offset_x, neighbor_y=y+neighbor_distance*offset_y; 00274 if (!range_image_->isInImage(neighbor_x, neighbor_y)) 00275 continue; 00276 float neighbor_shadow_border_score = border_scores_other_direction[neighbor_y*range_image_->width+neighbor_x]; 00277 00278 if (neighbor_shadow_border_score < best_shadow_border_score) 00279 { 00280 shadow_border_idx = neighbor_y*range_image_->width + neighbor_x; 00281 best_shadow_border_score = neighbor_shadow_border_score; 00282 } 00283 } 00284 if (shadow_border_idx >= 0) 00285 { 00286 return true; 00287 } 00288 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 00289 return false; 00290 } 00291 00292 bool RangeImageBorderExtractor::checkIfMaximum(int x, int y, int offset_x, int offset_y, float* border_scores, int shadow_border_idx) const 00293 { 00294 float border_score = border_scores[y*range_image_->width+x]; 00295 int neighbor_x=x-offset_x, neighbor_y=y-offset_y; 00296 if (range_image_->isInImage(neighbor_x, neighbor_y) && border_scores[neighbor_y*range_image_->width+neighbor_x] > border_score) 00297 return false; 00298 00299 for (int neighbor_distance=1; neighbor_distance<=parameters_.pixel_radius_borders; ++neighbor_distance) 00300 { 00301 neighbor_x=x+neighbor_distance*offset_x; neighbor_y=y+neighbor_distance*offset_y; 00302 if (!range_image_->isInImage(neighbor_x, neighbor_y)) 00303 continue; 00304 int neighbor_index = neighbor_y*range_image_->width + neighbor_x; 00305 if (neighbor_index==shadow_border_idx) 00306 return true; 00307 00308 float neighbor_border_score = border_scores[neighbor_index]; 00309 if (neighbor_border_score > border_score) 00310 return false; 00311 } 00312 return true; 00313 } 00314 00315 bool RangeImageBorderExtractor::calculateMainPrincipalCurvature(int x, int y, int radius, float& magnitude, 00316 Eigen::Vector3f& main_direction) const 00317 { 00318 magnitude = 0.0f; 00319 int index = y*range_image_->width+x; 00320 LocalSurface* local_surface = surface_structure_[index]; 00321 if (local_surface==NULL) 00322 return false; 00323 //const PointWithRange& point = range_image_->getPointNoCheck(x,y); 00324 00325 //Eigen::Vector3f& normal = local_surface->normal_no_jumps; 00326 //Eigen::Matrix3f to_tangent_plane = Eigen::Matrix3f::Identity() - normal*normal.transpose(); 00327 00328 VectorAverage3f vector_average; 00329 bool beams_valid[9]; 00330 for (int step=1; step<=radius; ++step) 00331 { 00332 int beam_idx = 0; 00333 for (int y2=y-step; y2<=y+step; y2+=step) 00334 { 00335 for (int x2=x-step; x2<=x+step; x2+=step) 00336 { 00337 bool& beam_valid = beams_valid[beam_idx++]; 00338 if (step==1) 00339 { 00340 if (x2==x && y2==y) 00341 beam_valid = false; 00342 else 00343 beam_valid = true; 00344 } 00345 else 00346 if (!beam_valid) 00347 continue; 00348 //std::cout << x2-x<<","<<y2-y<<" "; 00349 00350 if (!range_image_->isValid(x2,y2)) 00351 continue; 00352 00353 int index2 = y2*range_image_->width + x2; 00354 00355 const BorderTraits& border_traits = border_descriptions_->points[index2].traits; 00356 if (border_traits[BORDER_TRAIT__VEIL_POINT] || border_traits[BORDER_TRAIT__SHADOW_BORDER]) 00357 { 00358 beam_valid = false; 00359 continue; 00360 } 00361 00362 //const PointWithRange& point2 = range_image_->getPoint(index2); 00363 LocalSurface* local_surface2 = surface_structure_[index2]; 00364 if (local_surface2==NULL) 00365 continue; 00366 Eigen::Vector3f& normal2 = local_surface2->normal_no_jumps; 00367 //float distance_squared = squaredEuclideanDistance(point, point2); 00368 //vector_average.add(to_tangent_plane*normal2); 00369 vector_average.add(normal2); 00370 } 00371 } 00372 } 00373 //std::cout << "\n"; 00374 if (vector_average.getNoOfSamples() < 3) 00375 return false; 00376 00377 Eigen::Vector3f eigen_values, eigen_vector1, eigen_vector2; 00378 vector_average.doPCA(eigen_values, eigen_vector1, eigen_vector2, main_direction); 00379 magnitude = sqrtf(eigen_values[2]); 00380 //magnitude = eigen_values[2]; 00381 //magnitude = 1.0f - powf(1.0f-magnitude, 5); 00382 //magnitude = 1.0f - powf(1.0f-magnitude, 10); 00383 //magnitude += magnitude - powf(magnitude,2); 00384 //magnitude += magnitude - powf(magnitude,2); 00385 00386 //magnitude = sqrtf(local_surface->eigen_values[0]/local_surface->eigen_values.sum()); 00387 //magnitude = sqrtf(local_surface->eigen_values_no_jumps[0]/local_surface->eigen_values_no_jumps.sum()); 00388 00389 //if (surface_structure_[y*range_image_->width+x+1]==NULL||surface_structure_[y*range_image_->width+x-1]==NULL) 00390 //{ 00391 //magnitude = -std::numeric_limits<float>::infinity (); 00392 //return false; 00393 //} 00394 //float angle2 = acosf(surface_structure_[y*range_image_->width+x+1]->normal.dot(local_surface->normal)), 00395 //angle1 = acosf(surface_structure_[y*range_image_->width+x-1]->normal.dot(local_surface->normal)); 00396 //magnitude = angle2-angle1; 00397 00398 if (!pcl_isfinite(magnitude)) 00399 return false; 00400 00401 return true; 00402 } 00403 00404 } // namespace end