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 * 00007 * All rights reserved. 00008 * 00009 * Redistribution and use in source and binary forms, with or without 00010 * modification, are permitted provided that the following conditions 00011 * are met: 00012 * 00013 * * Redistributions of source code must retain the above copyright 00014 * notice, this list of conditions and the following disclaimer. 00015 * * Redistributions in binary form must reproduce the above 00016 * copyright notice, this list of conditions and the following 00017 * disclaimer in the documentation and/or other materials provided 00018 * with the distribution. 00019 * * Neither the name of the copyright holder(s) nor the names of its 00020 * contributors may be used to endorse or promote products derived 00021 * from this software without specific prior written permission. 00022 * 00023 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00024 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00025 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00026 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00027 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00028 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00029 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00030 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00031 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00032 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00033 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00034 * POSSIBILITY OF SUCH DAMAGE. 00035 * 00036 * $Id$ 00037 * 00038 */ 00039 00040 #ifndef PCL_SEARCH_IMPL_ORGANIZED_NEIGHBOR_SEARCH_H_ 00041 #define PCL_SEARCH_IMPL_ORGANIZED_NEIGHBOR_SEARCH_H_ 00042 00043 #include <pcl/search/organized.h> 00044 #include <pcl/common/eigen.h> 00045 #include <pcl/common/time.h> 00046 #include <Eigen/Eigenvalues> 00047 00048 ////////////////////////////////////////////////////////////////////////////////////////////// 00049 template<typename PointT> int 00050 pcl::search::OrganizedNeighbor<PointT>::radiusSearch (const PointT &query, 00051 const double radius, 00052 std::vector<int> &k_indices, 00053 std::vector<float> &k_sqr_distances, 00054 unsigned int max_nn) const 00055 { 00056 // NAN test 00057 assert (isFinite (query) && "Invalid (NaN, Inf) point coordinates given to nearestKSearch!"); 00058 00059 // search window 00060 unsigned left, right, top, bottom; 00061 //unsigned x, y, idx; 00062 float squared_distance; 00063 double squared_radius; 00064 00065 k_indices.clear (); 00066 k_sqr_distances.clear (); 00067 00068 squared_radius = radius * radius; 00069 00070 this->getProjectedRadiusSearchBox (query, static_cast<float> (squared_radius), left, right, top, bottom); 00071 00072 // iterate over search box 00073 if (max_nn == 0 || max_nn >= static_cast<unsigned int> (input_->points.size ())) 00074 max_nn = static_cast<unsigned int> (input_->points.size ()); 00075 00076 k_indices.reserve (max_nn); 00077 k_sqr_distances.reserve (max_nn); 00078 00079 unsigned yEnd = (bottom + 1) * input_->width + right + 1; 00080 register unsigned idx = top * input_->width + left; 00081 unsigned skip = input_->width - right + left - 1; 00082 unsigned xEnd = idx - left + right + 1; 00083 00084 for (; xEnd != yEnd; idx += skip, xEnd += input_->width) 00085 { 00086 for (; idx < xEnd; ++idx) 00087 { 00088 if (!mask_[idx] || !isFinite (input_->points[idx])) 00089 continue; 00090 00091 float dist_x = input_->points[idx].x - query.x; 00092 float dist_y = input_->points[idx].y - query.y; 00093 float dist_z = input_->points[idx].z - query.z; 00094 squared_distance = dist_x * dist_x + dist_y * dist_y + dist_z * dist_z; 00095 //squared_distance = (input_->points[idx].getVector3fMap () - query.getVector3fMap ()).squaredNorm (); 00096 if (squared_distance <= squared_radius) 00097 { 00098 k_indices.push_back (idx); 00099 k_sqr_distances.push_back (squared_distance); 00100 // already done ? 00101 if (k_indices.size () == max_nn) 00102 { 00103 if (sorted_results_) 00104 this->sortResults (k_indices, k_sqr_distances); 00105 return (max_nn); 00106 } 00107 } 00108 } 00109 } 00110 if (sorted_results_) 00111 this->sortResults (k_indices, k_sqr_distances); 00112 return (static_cast<int> (k_indices.size ())); 00113 } 00114 00115 ////////////////////////////////////////////////////////////////////////////////////////////// 00116 template<typename PointT> int 00117 pcl::search::OrganizedNeighbor<PointT>::nearestKSearch (const PointT &query, 00118 int k, 00119 std::vector<int> &k_indices, 00120 std::vector<float> &k_sqr_distances) const 00121 { 00122 assert (isFinite (query) && "Invalid (NaN, Inf) point coordinates given to nearestKSearch!"); 00123 if (k < 1) 00124 { 00125 k_indices.clear (); 00126 k_sqr_distances.clear (); 00127 return (0); 00128 } 00129 00130 Eigen::Vector3f queryvec (query.x, query.y, query.z); 00131 // project query point on the image plane 00132 //Eigen::Vector3f q = KR_ * query.getVector3fMap () + projection_matrix_.block <3, 1> (0, 3); 00133 Eigen::Vector3f q (KR_ * queryvec + projection_matrix_.block <3, 1> (0, 3)); 00134 int xBegin = int(q [0] / q [2] + 0.5f); 00135 int yBegin = int(q [1] / q [2] + 0.5f); 00136 int xEnd = xBegin + 1; // end is the pixel that is not used anymore, like in iterators 00137 int yEnd = yBegin + 1; 00138 00139 // the search window. This is supposed to shrink within the iterations 00140 unsigned left = 0; 00141 unsigned right = input_->width - 1; 00142 unsigned top = 0; 00143 unsigned bottom = input_->height - 1; 00144 00145 std::priority_queue <Entry> results; 00146 //std::vector<Entry> k_results; 00147 //k_results.reserve (k); 00148 // add point laying on the projection of the query point. 00149 if (xBegin >= 0 && 00150 xBegin < static_cast<int> (input_->width) && 00151 yBegin >= 0 && 00152 yBegin < static_cast<int> (input_->height)) 00153 testPoint (query, k, results, yBegin * input_->width + xBegin); 00154 else // point lys 00155 { 00156 // find the box that touches the image border -> dont waste time evaluating boxes that are completely outside the image! 00157 int dist = std::numeric_limits<int>::max (); 00158 00159 if (xBegin < 0) 00160 dist = -xBegin; 00161 else if (xBegin >= static_cast<int> (input_->width)) 00162 dist = xBegin - static_cast<int> (input_->width); 00163 00164 if (yBegin < 0) 00165 dist = std::min (dist, -yBegin); 00166 else if (yBegin >= static_cast<int> (input_->height)) 00167 dist = std::min (dist, yBegin - static_cast<int> (input_->height)); 00168 00169 xBegin -= dist; 00170 xEnd += dist; 00171 00172 yBegin -= dist; 00173 yEnd += dist; 00174 } 00175 00176 00177 // stop used as isChanged as well as stop. 00178 bool stop = false; 00179 do 00180 { 00181 // increment box size 00182 --xBegin; 00183 ++xEnd; 00184 --yBegin; 00185 ++yEnd; 00186 00187 // the range in x-direction which intersects with the image width 00188 int xFrom = xBegin; 00189 int xTo = xEnd; 00190 clipRange (xFrom, xTo, 0, input_->width); 00191 00192 // if x-extend is not 0 00193 if (xTo > xFrom) 00194 { 00195 // if upper line of the rectangle is visible and x-extend is not 0 00196 if (yBegin >= 0 && yBegin < static_cast<int> (input_->height)) 00197 { 00198 int idx = yBegin * input_->width + xFrom; 00199 int idxTo = idx + xTo - xFrom; 00200 for (; idx < idxTo; ++idx) 00201 stop = testPoint (query, k, results, idx) || stop; 00202 } 00203 00204 00205 // the row yEnd does NOT belong to the box -> last row = yEnd - 1 00206 // if lower line of the rectangle is visible 00207 if (yEnd > 0 && yEnd <= static_cast<int> (input_->height)) 00208 { 00209 int idx = (yEnd - 1) * input_->width + xFrom; 00210 int idxTo = idx + xTo - xFrom; 00211 00212 for (; idx < idxTo; ++idx) 00213 stop = testPoint (query, k, results, idx) || stop; 00214 } 00215 00216 // skip first row and last row (already handled above) 00217 int yFrom = yBegin + 1; 00218 int yTo = yEnd - 1; 00219 clipRange (yFrom, yTo, 0, input_->height); 00220 00221 // if we have lines in between that are also visible 00222 if (yFrom < yTo) 00223 { 00224 if (xBegin >= 0 && xBegin < static_cast<int> (input_->width)) 00225 { 00226 int idx = yFrom * input_->width + xBegin; 00227 int idxTo = yTo * input_->width + xBegin; 00228 00229 for (; idx < idxTo; idx += input_->width) 00230 stop = testPoint (query, k, results, idx) || stop; 00231 } 00232 00233 if (xEnd > 0 && xEnd <= static_cast<int> (input_->width)) 00234 { 00235 int idx = yFrom * input_->width + xEnd - 1; 00236 int idxTo = yTo * input_->width + xEnd - 1; 00237 00238 for (; idx < idxTo; idx += input_->width) 00239 stop = testPoint (query, k, results, idx) || stop; 00240 } 00241 00242 } 00243 // stop here means that the k-nearest neighbor changed -> recalculate bounding box of ellipse. 00244 if (stop) 00245 getProjectedRadiusSearchBox (query, results.top ().distance, left, right, top, bottom); 00246 00247 } 00248 // now we use it as stop flag -> if bounding box is completely within the already examined search box were done! 00249 stop = (static_cast<int> (left) >= xBegin && static_cast<int> (left) < xEnd && 00250 static_cast<int> (right) >= xBegin && static_cast<int> (right) < xEnd && 00251 static_cast<int> (top) >= yBegin && static_cast<int> (top) < yEnd && 00252 static_cast<int> (bottom) >= yBegin && static_cast<int> (bottom) < yEnd); 00253 00254 } while (!stop); 00255 00256 00257 k_indices.resize (results.size ()); 00258 k_sqr_distances.resize (results.size ()); 00259 size_t idx = results.size () - 1; 00260 while (!results.empty ()) 00261 { 00262 k_indices [idx] = results.top ().index; 00263 k_sqr_distances [idx] = results.top ().distance; 00264 results.pop (); 00265 --idx; 00266 } 00267 00268 return (static_cast<int> (k_indices.size ())); 00269 } 00270 00271 //////////////////////////////////////////////////////////////////////////////////////////// 00272 template<typename PointT> void 00273 pcl::search::OrganizedNeighbor<PointT>::getProjectedRadiusSearchBox (const PointT& point, 00274 float squared_radius, 00275 unsigned &minX, 00276 unsigned &maxX, 00277 unsigned &minY, 00278 unsigned &maxY) const 00279 { 00280 Eigen::Vector3f queryvec (point.x, point.y, point.z); 00281 //Eigen::Vector3f q = KR_ * point.getVector3fMap () + projection_matrix_.block <3, 1> (0, 3); 00282 Eigen::Vector3f q (KR_ * queryvec + projection_matrix_.block <3, 1> (0, 3)); 00283 00284 float a = squared_radius * KR_KRT_.coeff (8) - q [2] * q [2]; 00285 float b = squared_radius * KR_KRT_.coeff (7) - q [1] * q [2]; 00286 float c = squared_radius * KR_KRT_.coeff (4) - q [1] * q [1]; 00287 int min, max; 00288 // a and c are multiplied by two already => - 4ac -> - ac 00289 float det = b * b - a * c; 00290 if (det < 0) 00291 { 00292 minY = 0; 00293 maxY = input_->height - 1; 00294 } 00295 else 00296 { 00297 float y1 = static_cast<float> ((b - sqrt (det)) / a); 00298 float y2 = static_cast<float> ((b + sqrt (det)) / a); 00299 00300 min = std::min (static_cast<int> (floor (y1)), static_cast<int> (floor (y2))); 00301 max = std::max (static_cast<int> (ceil (y1)), static_cast<int> (ceil (y2))); 00302 minY = static_cast<unsigned> (std::min (static_cast<int> (input_->height) - 1, std::max (0, min))); 00303 maxY = static_cast<unsigned> (std::max (std::min (static_cast<int> (input_->height) - 1, max), 0)); 00304 } 00305 00306 b = squared_radius * KR_KRT_.coeff (6) - q [0] * q [2]; 00307 c = squared_radius * KR_KRT_.coeff (0) - q [0] * q [0]; 00308 00309 det = b * b - a * c; 00310 if (det < 0) 00311 { 00312 minX = 0; 00313 maxX = input_->width - 1; 00314 } 00315 else 00316 { 00317 float x1 = static_cast<float> ((b - sqrt (det)) / a); 00318 float x2 = static_cast<float> ((b + sqrt (det)) / a); 00319 00320 min = std::min (static_cast<int> (floor (x1)), static_cast<int> (floor (x2))); 00321 max = std::max (static_cast<int> (ceil (x1)), static_cast<int> (ceil (x2))); 00322 minX = static_cast<unsigned> (std::min (static_cast<int> (input_->width)- 1, std::max (0, min))); 00323 maxX = static_cast<unsigned> (std::max (std::min (static_cast<int> (input_->width) - 1, max), 0)); 00324 } 00325 } 00326 00327 00328 ////////////////////////////////////////////////////////////////////////////////////////////// 00329 template<typename PointT> void 00330 pcl::search::OrganizedNeighbor<PointT>::computeCameraMatrix (Eigen::Matrix3f& camera_matrix) const 00331 { 00332 pcl::getCameraMatrixFromProjectionMatrix (projection_matrix_, camera_matrix); 00333 } 00334 00335 ////////////////////////////////////////////////////////////////////////////////////////////// 00336 template<typename PointT> void 00337 pcl::search::OrganizedNeighbor<PointT>::estimateProjectionMatrix () 00338 { 00339 // internally we calculate with double but store the result into float matrices. 00340 projection_matrix_.setZero (); 00341 if (input_->height == 1 || input_->width == 1) 00342 { 00343 PCL_ERROR ("[pcl::%s::estimateProjectionMatrix] Input dataset is not organized!\n", this->getName ().c_str ()); 00344 return; 00345 } 00346 00347 const unsigned ySkip = (std::max) (input_->height >> pyramid_level_, unsigned (1)); 00348 const unsigned xSkip = (std::max) (input_->width >> pyramid_level_, unsigned (1)); 00349 00350 std::vector<int> indices; 00351 indices.reserve (input_->size () >> (pyramid_level_ << 1)); 00352 00353 for (unsigned yIdx = 0, idx = 0; yIdx < input_->height; yIdx += ySkip, idx += input_->width * (ySkip - 1)) 00354 { 00355 for (unsigned xIdx = 0; xIdx < input_->width; xIdx += xSkip, idx += xSkip) 00356 { 00357 if (!mask_ [idx]) 00358 continue; 00359 00360 indices.push_back (idx); 00361 } 00362 } 00363 00364 double residual_sqr = pcl::estimateProjectionMatrix<PointT> (input_, projection_matrix_, indices); 00365 00366 if (fabs (residual_sqr) > eps_ * float (indices.size ())) 00367 { 00368 PCL_ERROR ("[pcl::%s::radiusSearch] Input dataset is not from a projective device!\nResidual (MSE) %f, using %d valid points\n", this->getName ().c_str (), residual_sqr / double (indices.size()), indices.size ()); 00369 return; 00370 } 00371 00372 // get left 3x3 sub matrix, which contains K * R, with K = camera matrix = [[fx s cx] [0 fy cy] [0 0 1]] 00373 // and R being the rotation matrix 00374 KR_ = projection_matrix_.topLeftCorner <3, 3> (); 00375 00376 // precalculate KR * KR^T needed by calculations during nn-search 00377 KR_KRT_ = KR_ * KR_.transpose (); 00378 } 00379 00380 ////////////////////////////////////////////////////////////////////////////////////////////// 00381 template<typename PointT> bool 00382 pcl::search::OrganizedNeighbor<PointT>::projectPoint (const PointT& point, pcl::PointXY& q) const 00383 { 00384 Eigen::Vector3f projected = KR_ * point.getVector3fMap () + projection_matrix_.block <3, 1> (0, 3); 00385 q.x = projected [0] / projected [2]; 00386 q.y = projected [1] / projected [2]; 00387 return (projected[2] != 0); 00388 } 00389 #define PCL_INSTANTIATE_OrganizedNeighbor(T) template class PCL_EXPORTS pcl::search::OrganizedNeighbor<T>; 00390 00391 #endif