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