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-2011, 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 * 00038 */ 00039 00040 #ifndef PCL_FEATURES_IMPL_BOARD_H_ 00041 #define PCL_FEATURES_IMPL_BOARD_H_ 00042 00043 #include <pcl/features/board.h> 00044 #include <utility> 00045 #include <pcl/common/transforms.h> 00046 00047 ////////////////////////////////////////////////////////////////////////////////////////////// 00048 template<typename PointInT, typename PointNT, typename PointOutT> void 00049 pcl::BOARDLocalReferenceFrameEstimation<PointInT, PointNT, PointOutT>::directedOrthogonalAxis ( 00050 Eigen::Vector3f const &axis, 00051 Eigen::Vector3f const &axis_origin, 00052 Eigen::Vector3f const &point, 00053 Eigen::Vector3f &directed_ortho_axis) 00054 { 00055 Eigen::Vector3f projection; 00056 projectPointOnPlane (point, axis_origin, axis, projection); 00057 directed_ortho_axis = projection - axis_origin; 00058 00059 directed_ortho_axis.normalize (); 00060 00061 // check if the computed x axis is orthogonal to the normal 00062 //assert(areEquals((float)(directed_ortho_axis.dot(axis)), 0.0f, 1E-3f)); 00063 } 00064 00065 ////////////////////////////////////////////////////////////////////////////////////////////// 00066 template<typename PointInT, typename PointNT, typename PointOutT> void 00067 pcl::BOARDLocalReferenceFrameEstimation<PointInT, PointNT, PointOutT>::projectPointOnPlane ( 00068 Eigen::Vector3f const &point, 00069 Eigen::Vector3f const &origin_point, 00070 Eigen::Vector3f const &plane_normal, 00071 Eigen::Vector3f &projected_point) 00072 { 00073 float t; 00074 Eigen::Vector3f xo; 00075 00076 xo = point - origin_point; 00077 t = plane_normal.dot (xo); 00078 00079 projected_point = point - (t * plane_normal); 00080 } 00081 00082 ////////////////////////////////////////////////////////////////////////////////////////////// 00083 template<typename PointInT, typename PointNT, typename PointOutT> float 00084 pcl::BOARDLocalReferenceFrameEstimation<PointInT, PointNT, PointOutT>::getAngleBetweenUnitVectors ( 00085 Eigen::Vector3f const &v1, 00086 Eigen::Vector3f const &v2, 00087 Eigen::Vector3f const &axis) 00088 { 00089 Eigen::Vector3f angle_orientation; 00090 angle_orientation = v1.cross (v2); 00091 float angle_radians = acosf (std::max (-1.0f, std::min (1.0f, v1.dot (v2)))); 00092 00093 angle_radians = angle_orientation.dot (axis) < 0.f ? (2 * static_cast<float> (M_PI) - angle_radians) : angle_radians; 00094 00095 return (angle_radians); 00096 } 00097 00098 ////////////////////////////////////////////////////////////////////////////////////////////// 00099 template<typename PointInT, typename PointNT, typename PointOutT> void 00100 pcl::BOARDLocalReferenceFrameEstimation<PointInT, PointNT, PointOutT>::randomOrthogonalAxis ( 00101 Eigen::Vector3f const &axis, 00102 Eigen::Vector3f &rand_ortho_axis) 00103 { 00104 if (!areEquals (axis.z (), 0.0f)) 00105 { 00106 rand_ortho_axis.x () = (static_cast<float> (rand ()) / static_cast<float> (RAND_MAX)) * 2.0f - 1.0f; 00107 rand_ortho_axis.y () = (static_cast<float> (rand ()) / static_cast<float> (RAND_MAX)) * 2.0f - 1.0f; 00108 rand_ortho_axis.z () = -(axis.x () * rand_ortho_axis.x () + axis.y () * rand_ortho_axis.y ()) / axis.z (); 00109 } 00110 else if (!areEquals (axis.y (), 0.0f)) 00111 { 00112 rand_ortho_axis.x () = (static_cast<float> (rand ()) / static_cast<float> (RAND_MAX)) * 2.0f - 1.0f; 00113 rand_ortho_axis.z () = (static_cast<float> (rand ()) / static_cast<float> (RAND_MAX)) * 2.0f - 1.0f; 00114 rand_ortho_axis.y () = -(axis.x () * rand_ortho_axis.x () + axis.z () * rand_ortho_axis.z ()) / axis.y (); 00115 } 00116 else if (!areEquals (axis.x (), 0.0f)) 00117 { 00118 rand_ortho_axis.y () = (static_cast<float> (rand ()) / static_cast<float> (RAND_MAX)) * 2.0f - 1.0f; 00119 rand_ortho_axis.z () = (static_cast<float> (rand ()) / static_cast<float> (RAND_MAX)) * 2.0f - 1.0f; 00120 rand_ortho_axis.x () = -(axis.y () * rand_ortho_axis.y () + axis.z () * rand_ortho_axis.z ()) / axis.x (); 00121 } 00122 00123 rand_ortho_axis.normalize (); 00124 00125 // check if the computed x axis is orthogonal to the normal 00126 //assert(areEquals(rand_ortho_axis.dot(axis), 0.0f, 1E-6f)); 00127 } 00128 00129 ////////////////////////////////////////////////////////////////////////////////////////////// 00130 template<typename PointInT, typename PointNT, typename PointOutT> void 00131 pcl::BOARDLocalReferenceFrameEstimation<PointInT, PointNT, PointOutT>::planeFitting ( 00132 Eigen::Matrix<float, 00133 Eigen::Dynamic, 3> const &points, 00134 Eigen::Vector3f ¢er, 00135 Eigen::Vector3f &norm) 00136 { 00137 // ----------------------------------------------------- 00138 // Plane Fitting using Singular Value Decomposition (SVD) 00139 // ----------------------------------------------------- 00140 00141 int n_points = static_cast<int> (points.rows ()); 00142 if (n_points == 0) 00143 { 00144 return; 00145 } 00146 00147 //find the center by averaging the points positions 00148 center.setZero (); 00149 00150 for (int i = 0; i < n_points; ++i) 00151 { 00152 center += points.row (i); 00153 } 00154 00155 center /= static_cast<float> (n_points); 00156 00157 //copy points - average (center) 00158 Eigen::Matrix<float, Eigen::Dynamic, 3> A (n_points, 3); //PointData 00159 for (int i = 0; i < n_points; ++i) 00160 { 00161 A (i, 0) = points (i, 0) - center.x (); 00162 A (i, 1) = points (i, 1) - center.y (); 00163 A (i, 2) = points (i, 2) - center.z (); 00164 } 00165 00166 Eigen::JacobiSVD<Eigen::MatrixXf> svd (A, Eigen::ComputeFullV); 00167 norm = svd.matrixV ().col (2); 00168 } 00169 00170 ////////////////////////////////////////////////////////////////////////////////////////////// 00171 template<typename PointInT, typename PointNT, typename PointOutT> void 00172 pcl::BOARDLocalReferenceFrameEstimation<PointInT, PointNT, PointOutT>::normalDisambiguation ( 00173 pcl::PointCloud<PointNT> const &normal_cloud, 00174 std::vector<int> const &normal_indices, 00175 Eigen::Vector3f &normal) 00176 { 00177 Eigen::Vector3f normal_mean; 00178 normal_mean.setZero (); 00179 00180 for (size_t i = 0; i < normal_indices.size (); ++i) 00181 { 00182 const PointNT& curPt = normal_cloud[normal_indices[i]]; 00183 00184 normal_mean += curPt.getNormalVector3fMap (); 00185 } 00186 00187 normal_mean.normalize (); 00188 00189 if (normal.dot (normal_mean) < 0) 00190 { 00191 normal = -normal; 00192 } 00193 } 00194 00195 ////////////////////////////////////////////////////////////////////////////////////////////// 00196 template<typename PointInT, typename PointNT, typename PointOutT> float 00197 pcl::BOARDLocalReferenceFrameEstimation<PointInT, PointNT, PointOutT>::computePointLRF (const int &index, 00198 Eigen::Matrix3f &lrf) 00199 { 00200 //find Z axis 00201 00202 //extract support points for Rz radius 00203 std::vector<int> neighbours_indices; 00204 std::vector<float> neighbours_distances; 00205 int n_neighbours = this->searchForNeighbors (index, search_parameter_, neighbours_indices, neighbours_distances); 00206 00207 //check if there are enough neighbor points, otherwise compute a random X axis and use normal as Z axis 00208 if (n_neighbours < 6) 00209 { 00210 //PCL_WARN( 00211 // "[pcl::%s::computePointLRF] Warning! Neighborhood has less than 6 vertices. Aborting description of point with index %d\n", 00212 // getClassName().c_str(), index); 00213 00214 //setting lrf to NaN 00215 lrf.setConstant (std::numeric_limits<float>::quiet_NaN ()); 00216 00217 return (std::numeric_limits<float>::max ()); 00218 } 00219 00220 //copy neighbours coordinates into eigen matrix 00221 Eigen::Matrix<float, Eigen::Dynamic, 3> neigh_points_mat (n_neighbours, 3); 00222 for (int i = 0; i < n_neighbours; ++i) 00223 { 00224 neigh_points_mat.row (i) = (*surface_)[neighbours_indices[i]].getVector3fMap (); 00225 } 00226 00227 Eigen::Vector3f x_axis, y_axis; 00228 //plane fitting to find direction of Z axis 00229 Eigen::Vector3f fitted_normal; //z_axis 00230 Eigen::Vector3f centroid; 00231 planeFitting (neigh_points_mat, centroid, fitted_normal); 00232 00233 //disambiguate Z axis with normal mean 00234 normalDisambiguation (*normals_, neighbours_indices, fitted_normal); 00235 00236 //setting LRF Z axis 00237 lrf.row (2).matrix () = fitted_normal; 00238 00239 ///////////////////////////////////////////////////////////////////////////////////////// 00240 //find X axis 00241 00242 //extract support points for Rx radius 00243 if (tangent_radius_ != 0.0f && search_parameter_ != tangent_radius_) 00244 { 00245 n_neighbours = this->searchForNeighbors (index, tangent_radius_, neighbours_indices, neighbours_distances); 00246 } 00247 00248 //find point with the "most different" normal (with respect to fittedNormal) 00249 00250 float min_normal_cos = std::numeric_limits<float>::max (); 00251 int min_normal_index = -1; 00252 00253 bool margin_point_found = false; 00254 Eigen::Vector3f best_margin_point; 00255 bool best_point_found_on_margins = false; 00256 00257 float radius2 = tangent_radius_ * tangent_radius_; 00258 00259 float margin_distance2 = margin_thresh_ * margin_thresh_ * radius2; 00260 00261 float max_boundary_angle = 0; 00262 00263 if (find_holes_) 00264 { 00265 randomOrthogonalAxis (fitted_normal, x_axis); 00266 00267 lrf.row (0).matrix () = x_axis; 00268 00269 for (int i = 0; i < check_margin_array_size_; i++) 00270 { 00271 check_margin_array_[i] = false; 00272 margin_array_min_angle_[i] = std::numeric_limits<float>::max (); 00273 margin_array_max_angle_[i] = -std::numeric_limits<float>::max (); 00274 margin_array_min_angle_normal_[i] = -1.0; 00275 margin_array_max_angle_normal_[i] = -1.0; 00276 } 00277 max_boundary_angle = (2 * static_cast<float> (M_PI)) / static_cast<float> (check_margin_array_size_); 00278 } 00279 00280 for (int curr_neigh = 0; curr_neigh < n_neighbours; ++curr_neigh) 00281 { 00282 const int& curr_neigh_idx = neighbours_indices[curr_neigh]; 00283 const float& neigh_distance_sqr = neighbours_distances[curr_neigh]; 00284 if (neigh_distance_sqr <= margin_distance2) 00285 { 00286 continue; 00287 } 00288 00289 //point normalIndex is inside the ring between marginThresh and Radius 00290 margin_point_found = true; 00291 00292 Eigen::Vector3f normal_mean = normals_->at (curr_neigh_idx).getNormalVector3fMap (); 00293 00294 float normal_cos = fitted_normal.dot (normal_mean); 00295 if (normal_cos < min_normal_cos) 00296 { 00297 min_normal_index = curr_neigh_idx; 00298 min_normal_cos = normal_cos; 00299 best_point_found_on_margins = false; 00300 } 00301 00302 if (find_holes_) 00303 { 00304 //find angle with respect to random axis previously calculated 00305 Eigen::Vector3f indicating_normal_vect; 00306 directedOrthogonalAxis (fitted_normal, input_->at (index).getVector3fMap (), 00307 surface_->at (curr_neigh_idx).getVector3fMap (), indicating_normal_vect); 00308 float angle = getAngleBetweenUnitVectors (x_axis, indicating_normal_vect, fitted_normal); 00309 00310 int check_margin_array_idx = std::min (static_cast<int> (floor (angle / max_boundary_angle)), check_margin_array_size_ - 1); 00311 check_margin_array_[check_margin_array_idx] = true; 00312 00313 if (angle < margin_array_min_angle_[check_margin_array_idx]) 00314 { 00315 margin_array_min_angle_[check_margin_array_idx] = angle; 00316 margin_array_min_angle_normal_[check_margin_array_idx] = normal_cos; 00317 } 00318 if (angle > margin_array_max_angle_[check_margin_array_idx]) 00319 { 00320 margin_array_max_angle_[check_margin_array_idx] = angle; 00321 margin_array_max_angle_normal_[check_margin_array_idx] = normal_cos; 00322 } 00323 } 00324 00325 } //for each neighbor 00326 00327 if (!margin_point_found) 00328 { 00329 //find among points with neighDistance <= marginThresh*radius 00330 for (int curr_neigh = 0; curr_neigh < n_neighbours; curr_neigh++) 00331 { 00332 const int& curr_neigh_idx = neighbours_indices[curr_neigh]; 00333 const float& neigh_distance_sqr = neighbours_distances[curr_neigh]; 00334 00335 if (neigh_distance_sqr > margin_distance2) 00336 continue; 00337 00338 Eigen::Vector3f normal_mean = normals_->at (curr_neigh_idx).getNormalVector3fMap (); 00339 00340 float normal_cos = fitted_normal.dot (normal_mean); 00341 00342 if (normal_cos < min_normal_cos) 00343 { 00344 min_normal_index = curr_neigh_idx; 00345 min_normal_cos = normal_cos; 00346 } 00347 }//for each neighbor 00348 00349 // Check if we are not in a degenerate case (all the neighboring normals are NaNs) 00350 if (min_normal_index == -1) 00351 { 00352 lrf.setConstant (std::numeric_limits<float>::quiet_NaN ()); 00353 return (std::numeric_limits<float>::max ()); 00354 } 00355 //find orthogonal axis directed to minNormalIndex point projection on plane with fittedNormal as axis 00356 directedOrthogonalAxis (fitted_normal, input_->at (index).getVector3fMap (), 00357 surface_->at (min_normal_index).getVector3fMap (), x_axis); 00358 y_axis = fitted_normal.cross (x_axis); 00359 00360 lrf.row (0).matrix () = x_axis; 00361 lrf.row (1).matrix () = y_axis; 00362 //z axis already set 00363 00364 00365 return (min_normal_cos); 00366 } 00367 00368 if (!find_holes_) 00369 { 00370 if (best_point_found_on_margins) 00371 { 00372 //if most inclined normal is on support margin 00373 directedOrthogonalAxis (fitted_normal, input_->at (index).getVector3fMap (), best_margin_point, x_axis); 00374 y_axis = fitted_normal.cross (x_axis); 00375 00376 lrf.row (0).matrix () = x_axis; 00377 lrf.row (1).matrix () = y_axis; 00378 //z axis already set 00379 00380 return (min_normal_cos); 00381 } 00382 00383 // Check if we are not in a degenerate case (all the neighboring normals are NaNs) 00384 if (min_normal_index == -1) 00385 { 00386 lrf.setConstant (std::numeric_limits<float>::quiet_NaN ()); 00387 return (std::numeric_limits<float>::max ()); 00388 } 00389 00390 directedOrthogonalAxis (fitted_normal, input_->at (index).getVector3fMap (), 00391 surface_->at (min_normal_index).getVector3fMap (), x_axis); 00392 y_axis = fitted_normal.cross (x_axis); 00393 00394 lrf.row (0).matrix () = x_axis; 00395 lrf.row (1).matrix () = y_axis; 00396 //z axis already set 00397 00398 return (min_normal_cos); 00399 }// if(!find_holes_) 00400 00401 //check if there is at least a hole 00402 bool is_hole_present = false; 00403 for (int i = 0; i < check_margin_array_size_; i++) 00404 { 00405 if (!check_margin_array_[i]) 00406 { 00407 is_hole_present = true; 00408 break; 00409 } 00410 } 00411 00412 if (!is_hole_present) 00413 { 00414 if (best_point_found_on_margins) 00415 { 00416 //if most inclined normal is on support margin 00417 directedOrthogonalAxis (fitted_normal, input_->at (index).getVector3fMap (), best_margin_point, x_axis); 00418 y_axis = fitted_normal.cross (x_axis); 00419 00420 lrf.row (0).matrix () = x_axis; 00421 lrf.row (1).matrix () = y_axis; 00422 //z axis already set 00423 00424 return (min_normal_cos); 00425 } 00426 00427 // Check if we are not in a degenerate case (all the neighboring normals are NaNs) 00428 if (min_normal_index == -1) 00429 { 00430 lrf.setConstant (std::numeric_limits<float>::quiet_NaN ()); 00431 return (std::numeric_limits<float>::max ()); 00432 } 00433 00434 //find orthogonal axis directed to minNormalIndex point projection on plane with fittedNormal as axis 00435 directedOrthogonalAxis (fitted_normal, input_->at (index).getVector3fMap (), 00436 surface_->at (min_normal_index).getVector3fMap (), x_axis); 00437 y_axis = fitted_normal.cross (x_axis); 00438 00439 lrf.row (0).matrix () = x_axis; 00440 lrf.row (1).matrix () = y_axis; 00441 //z axis already set 00442 00443 return (min_normal_cos); 00444 }//if (!is_hole_present) 00445 00446 //case hole found 00447 //find missing region 00448 float angle = 0.0; 00449 int hole_end; 00450 int hole_first; 00451 00452 //find first no border pie 00453 int first_no_border = -1; 00454 if (check_margin_array_[check_margin_array_size_ - 1]) 00455 { 00456 first_no_border = 0; 00457 } 00458 else 00459 { 00460 for (int i = 0; i < check_margin_array_size_; i++) 00461 { 00462 if (check_margin_array_[i]) 00463 { 00464 first_no_border = i; 00465 break; 00466 } 00467 } 00468 } 00469 00470 //float steep_prob = 0.0; 00471 float max_hole_prob = -std::numeric_limits<float>::max (); 00472 00473 //find holes 00474 for (int ch = first_no_border; ch < check_margin_array_size_; ch++) 00475 { 00476 if (!check_margin_array_[ch]) 00477 { 00478 //border beginning found 00479 hole_first = ch; 00480 hole_end = hole_first + 1; 00481 while (!check_margin_array_[hole_end % check_margin_array_size_]) 00482 { 00483 ++hole_end; 00484 } 00485 //border end found, find angle 00486 00487 if ((hole_end - hole_first) > 0) 00488 { 00489 //check if hole can be a shapeness hole 00490 int previous_hole = (((hole_first - 1) < 0) ? (hole_first - 1) + check_margin_array_size_ : (hole_first - 1)) 00491 % check_margin_array_size_; 00492 int following_hole = (hole_end) % check_margin_array_size_; 00493 float normal_begin = margin_array_max_angle_normal_[previous_hole]; 00494 float normal_end = margin_array_min_angle_normal_[following_hole]; 00495 normal_begin -= min_normal_cos; 00496 normal_end -= min_normal_cos; 00497 normal_begin = normal_begin / (1.0f - min_normal_cos); 00498 normal_end = normal_end / (1.0f - min_normal_cos); 00499 normal_begin = 1.0f - normal_begin; 00500 normal_end = 1.0f - normal_end; 00501 00502 //evaluate P(Hole); 00503 float hole_width = 0.0f; 00504 if (following_hole < previous_hole) 00505 { 00506 hole_width = margin_array_min_angle_[following_hole] + 2 * static_cast<float> (M_PI) 00507 - margin_array_max_angle_[previous_hole]; 00508 } 00509 else 00510 { 00511 hole_width = margin_array_min_angle_[following_hole] - margin_array_max_angle_[previous_hole]; 00512 } 00513 float hole_prob = hole_width / (2 * static_cast<float> (M_PI)); 00514 00515 //evaluate P(zmin|Hole) 00516 float steep_prob = (normal_end + normal_begin) / 2.0f; 00517 00518 //check hole prob and after that, check steepThresh 00519 00520 if (hole_prob > hole_size_prob_thresh_) 00521 { 00522 if (steep_prob > steep_thresh_) 00523 { 00524 if (hole_prob > max_hole_prob) 00525 { 00526 max_hole_prob = hole_prob; 00527 00528 float angle_weight = ((normal_end - normal_begin) + 1.0f) / 2.0f; 00529 if (following_hole < previous_hole) 00530 { 00531 angle = margin_array_max_angle_[previous_hole] + (margin_array_min_angle_[following_hole] + 2 00532 * static_cast<float> (M_PI) - margin_array_max_angle_[previous_hole]) * angle_weight; 00533 } 00534 else 00535 { 00536 angle = margin_array_max_angle_[previous_hole] + (margin_array_min_angle_[following_hole] 00537 - margin_array_max_angle_[previous_hole]) * angle_weight; 00538 } 00539 } 00540 } 00541 } 00542 } //(hole_end-hole_first) > 0 00543 00544 if (hole_end >= check_margin_array_size_) 00545 { 00546 break; 00547 } 00548 else 00549 { 00550 ch = hole_end - 1; 00551 } 00552 } 00553 } 00554 00555 if (max_hole_prob > -std::numeric_limits<float>::max ()) 00556 { 00557 //hole found 00558 Eigen::AngleAxisf rotation = Eigen::AngleAxisf (angle, fitted_normal); 00559 x_axis = rotation * x_axis; 00560 00561 min_normal_cos -= 10.0f; 00562 } 00563 else 00564 { 00565 if (best_point_found_on_margins) 00566 { 00567 //if most inclined normal is on support margin 00568 directedOrthogonalAxis (fitted_normal, input_->at (index).getVector3fMap (), best_margin_point, x_axis); 00569 } 00570 else 00571 { 00572 // Check if we are not in a degenerate case (all the neighboring normals are NaNs) 00573 if (min_normal_index == -1) 00574 { 00575 lrf.setConstant (std::numeric_limits<float>::quiet_NaN ()); 00576 return (std::numeric_limits<float>::max ()); 00577 } 00578 00579 //find orthogonal axis directed to minNormalIndex point projection on plane with fittedNormal as axis 00580 directedOrthogonalAxis (fitted_normal, input_->at (index).getVector3fMap (), 00581 surface_->at (min_normal_index).getVector3fMap (), x_axis); 00582 } 00583 } 00584 00585 y_axis = fitted_normal.cross (x_axis); 00586 00587 lrf.row (0).matrix () = x_axis; 00588 lrf.row (1).matrix () = y_axis; 00589 //z axis already set 00590 00591 return (min_normal_cos); 00592 } 00593 00594 ////////////////////////////////////////////////////////////////////////////////////////////// 00595 template<typename PointInT, typename PointNT, typename PointOutT> void 00596 pcl::BOARDLocalReferenceFrameEstimation<PointInT, PointNT, PointOutT>::computeFeature (PointCloudOut &output) 00597 { 00598 //check whether used with search radius or search k-neighbors 00599 if (this->getKSearch () != 0) 00600 { 00601 PCL_ERROR( 00602 "[pcl::%s::computeFeature] Error! Search method set to k-neighborhood. Call setKSearch(0) and setRadiusSearch( radius ) to use this class.\n", 00603 getClassName().c_str()); 00604 return; 00605 } 00606 00607 this->resetData (); 00608 for (size_t point_idx = 0; point_idx < indices_->size (); ++point_idx) 00609 { 00610 Eigen::Matrix3f currentLrf; 00611 PointOutT &rf = output[point_idx]; 00612 00613 //rf.confidence = computePointLRF (*indices_[point_idx], currentLrf); 00614 //if (rf.confidence == std::numeric_limits<float>::max ()) 00615 if (computePointLRF ((*indices_)[point_idx], currentLrf) == std::numeric_limits<float>::max ()) 00616 { 00617 output.is_dense = false; 00618 } 00619 00620 for (int d = 0; d < 3; ++d) 00621 { 00622 rf.x_axis[d] = currentLrf (0, d); 00623 rf.y_axis[d] = currentLrf (1, d); 00624 rf.z_axis[d] = currentLrf (2, d); 00625 } 00626 } 00627 } 00628 00629 #define PCL_INSTANTIATE_BOARDLocalReferenceFrameEstimation(T,NT,OutT) template class PCL_EXPORTS pcl::BOARDLocalReferenceFrameEstimation<T,NT,OutT>; 00630 00631 #endif // PCL_FEATURES_IMPL_BOARD_H_