organized.hpp
Go to the documentation of this file.
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 Willow Garage, Inc. 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: organized.hpp 6124 2012-07-03 19:04:27Z aichim $
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 
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       squared_distance = (input_->points[idx].getVector3fMap () - query.getVector3fMap ()).squaredNorm ();
00092       if (squared_distance <= squared_radius)
00093       {
00094         k_indices.push_back (idx);
00095         k_sqr_distances.push_back (squared_distance);
00096         // already done ?
00097         if (k_indices.size () == max_nn)
00098         {
00099           if (sorted_results_)
00100             this->sortResults (k_indices, k_sqr_distances);
00101           return (max_nn);
00102         }
00103       }
00104     }
00105   }
00106   if (sorted_results_)
00107     this->sortResults (k_indices, k_sqr_distances);  
00108   return (static_cast<int> (k_indices.size ()));
00109 }
00110 
00112 template<typename PointT> int
00113 pcl::search::OrganizedNeighbor<PointT>::nearestKSearch (const PointT &query,
00114                                                         int k,
00115                                                         std::vector<int> &k_indices,
00116                                                         std::vector<float> &k_sqr_distances) const
00117 {
00118   assert (isFinite (query) && "Invalid (NaN, Inf) point coordinates given to nearestKSearch!");
00119   if (k < 1)
00120   {
00121     k_indices.clear ();
00122     k_sqr_distances.clear ();
00123     return (0);
00124   }
00125 
00126   // project query point on the image plane
00127   Eigen::Vector3f q = KR_ * query.getVector3fMap () + projection_matrix_.block <3, 1> (0, 3);
00128   int xBegin = int(q [0] / q [2] + 0.5f);
00129   int yBegin = int(q [1] / q [2] + 0.5f);
00130   int xEnd   = xBegin + 1; // end is the pixel that is not used anymore, like in iterators
00131   int yEnd   = yBegin + 1;
00132 
00133   // the search window. This is supposed to shrink within the iterations
00134   unsigned left = 0;
00135   unsigned right = input_->width - 1;
00136   unsigned top = 0;
00137   unsigned bottom = input_->height - 1;
00138 
00139   std::priority_queue <Entry> results;
00140   //std::vector<Entry> k_results;
00141   //k_results.reserve (k);
00142   // add point laying on the projection of the query point.
00143   if (xBegin >= 0 && 
00144       xBegin < static_cast<int> (input_->width) && 
00145       yBegin >= 0 && 
00146       yBegin < static_cast<int> (input_->height))
00147     testPoint (query, k, results, yBegin * input_->width + xBegin);
00148   else // point lys
00149   {
00150     // find the box that touches the image border -> dont waste time evaluating boxes that are completely outside the image!
00151     int dist = std::numeric_limits<int>::max ();
00152 
00153     if (xBegin < 0)
00154       dist = -xBegin;
00155     else if (xBegin >= static_cast<int> (input_->width))
00156       dist = xBegin - static_cast<int> (input_->width);
00157 
00158     if (yBegin < 0)
00159       dist = std::min (dist, -yBegin);
00160     else if (yBegin >= static_cast<int> (input_->height))
00161       dist = std::min (dist, yBegin - static_cast<int> (input_->height));
00162 
00163     xBegin -= dist;
00164     xEnd   += dist;
00165 
00166     yBegin -= dist;
00167     yEnd   += dist;
00168   }
00169 
00170   
00171   // stop used as isChanged as well as stop.
00172   bool stop = false;
00173   do
00174   {
00175     // increment box size
00176     --xBegin;
00177     ++xEnd;
00178     --yBegin;
00179     ++yEnd;
00180 
00181     // the range in x-direction which intersects with the image width
00182     int xFrom = xBegin;
00183     int xTo   = xEnd;
00184     clipRange (xFrom, xTo, 0, input_->width);
00185     
00186     // if x-extend is not 0
00187     if (xTo > xFrom)
00188     {
00189       // if upper line of the rectangle is visible and x-extend is not 0
00190       if (yBegin >= 0 && yBegin < static_cast<int> (input_->height))
00191       {
00192         int idx   = yBegin * input_->width + xFrom;
00193         int idxTo = idx + xTo - xFrom;
00194         for (; idx < idxTo; ++idx)
00195           stop = testPoint (query, k, results, idx) || stop;
00196       }
00197       
00198 
00199       // the row yEnd does NOT belong to the box -> last row = yEnd - 1
00200       // if lower line of the rectangle is visible
00201       if (yEnd > 0 && yEnd <= static_cast<int> (input_->height))
00202       {
00203         int idx   = (yEnd - 1) * input_->width + xFrom;
00204         int idxTo = idx + xTo - xFrom;
00205 
00206         for (; idx < idxTo; ++idx)
00207           stop = testPoint (query, k, results, idx) || stop;
00208       }
00209       
00210       // skip first row and last row (already handled above)
00211       int yFrom = yBegin + 1;
00212       int yTo   = yEnd - 1;
00213       clipRange (yFrom, yTo, 0, input_->height);
00214       
00215       // if we have lines in between that are also visible
00216       if (yFrom < yTo)
00217       {
00218         if (xBegin >= 0 && xBegin < static_cast<int> (input_->width))
00219         {
00220           int idx   = yFrom * input_->width + xBegin;
00221           int idxTo = yTo * input_->width + xBegin;
00222 
00223           for (; idx < idxTo; idx += input_->width)
00224             stop = testPoint (query, k, results, idx) || stop;
00225         }
00226         
00227         if (xEnd > 0 && xEnd <= static_cast<int> (input_->width))
00228         {
00229           int idx   = yFrom * input_->width + xEnd - 1;
00230           int idxTo = yTo * input_->width + xEnd - 1;
00231 
00232           for (; idx < idxTo; idx += input_->width)
00233             stop = testPoint (query, k, results, idx) || stop;
00234         }
00235         
00236       }
00237       // stop here means that the k-nearest neighbor changed -> recalculate bounding box of ellipse.
00238       if (stop)
00239         getProjectedRadiusSearchBox (query, results.top ().distance, left, right, top, bottom);
00240       
00241     }
00242     // now we use it as stop flag -> if bounding box is completely within the already examined search box were done!
00243     stop = (static_cast<int> (left)   >= xBegin && static_cast<int> (left)   < xEnd && 
00244             static_cast<int> (right)  >= xBegin && static_cast<int> (right)  < xEnd &&
00245             static_cast<int> (top)    >= yBegin && static_cast<int> (top)    < yEnd && 
00246             static_cast<int> (bottom) >= yBegin && static_cast<int> (bottom) < yEnd);
00247     
00248   } while (!stop);
00249 
00250   
00251   k_indices.resize (results.size ());
00252   k_sqr_distances.resize (results.size ());
00253   size_t idx = results.size () - 1;
00254   while (!results.empty ())
00255   {
00256     k_indices [idx] = results.top ().index;
00257     k_sqr_distances [idx] = results.top ().distance;
00258     results.pop ();
00259     --idx;
00260   }
00261   
00262   return (static_cast<int> (k_indices.size ()));
00263 }
00264 
00266 template<typename PointT> void
00267 pcl::search::OrganizedNeighbor<PointT>::getProjectedRadiusSearchBox (const PointT& point,
00268                                                                      float squared_radius,
00269                                                                      unsigned &minX,
00270                                                                      unsigned &maxX,
00271                                                                      unsigned &minY,
00272                                                                      unsigned &maxY) const
00273 {
00274   Eigen::Vector3f q = KR_ * point.getVector3fMap () + projection_matrix_.block <3, 1> (0, 3);
00275 
00276   float a = squared_radius * KR_KRT_.coeff (8) - q [2] * q [2];
00277   float b = squared_radius * KR_KRT_.coeff (7) - q [1] * q [2];
00278   float c = squared_radius * KR_KRT_.coeff (4) - q [1] * q [1];
00279   int min, max;
00280   // a and c are multiplied by two already => - 4ac -> - ac
00281   float det = b * b - a * c;
00282   if (det < 0)
00283   {
00284     minY = 0;
00285     maxY = input_->height - 1;
00286   }
00287   else
00288   {
00289     float y1 = (b - sqrt (det)) / a;
00290     float y2 = (b + sqrt (det)) / a;
00291 
00292     min = std::min (static_cast<int> (floor (y1)), static_cast<int> (floor (y2)));
00293     max = std::max (static_cast<int> (ceil (y1)), static_cast<int> (ceil (y2)));
00294     minY = static_cast<unsigned> (std::min (static_cast<int> (input_->height) - 1, std::max (0, min)));
00295     maxY = static_cast<unsigned> (std::max (std::min (static_cast<int> (input_->height) - 1, max), 0));
00296   }
00297 
00298   b = squared_radius * KR_KRT_.coeff (6) - q [0] * q [2];
00299   c = squared_radius * KR_KRT_.coeff (0) - q [0] * q [0];
00300 
00301   det = b * b - a * c;
00302   if (det < 0)
00303   {
00304     minX = 0;
00305     maxX = input_->width - 1;
00306   }
00307   else
00308   {
00309     float x1 = (b - sqrt (det)) / a;
00310     float x2 = (b + sqrt (det)) / a;
00311 
00312     min = std::min (static_cast<int> (floor (x1)), static_cast<int> (floor (x2)));
00313     max = std::max (static_cast<int> (ceil (x1)), static_cast<int> (ceil (x2)));
00314     minX = static_cast<unsigned> (std::min (static_cast<int> (input_->width)- 1, std::max (0, min)));
00315     maxX = static_cast<unsigned> (std::max (std::min (static_cast<int> (input_->width) - 1, max), 0));
00316   }
00317 }
00318 
00320 template<typename PointT> template <typename MatrixType> void
00321 pcl::search::OrganizedNeighbor<PointT>::makeSymmetric (MatrixType& matrix, bool use_upper_triangular) const
00322 {
00323   if (use_upper_triangular && (MatrixType::Flags & Eigen::RowMajorBit) )
00324   {
00325     matrix.coeffRef (4) = matrix.coeff (1);
00326     matrix.coeffRef (8) = matrix.coeff (2);
00327     matrix.coeffRef (9) = matrix.coeff (6);
00328     matrix.coeffRef (12) = matrix.coeff (3);
00329     matrix.coeffRef (13) = matrix.coeff (7);
00330     matrix.coeffRef (14) = matrix.coeff (11);
00331   }
00332   else
00333   {
00334     matrix.coeffRef (1) = matrix.coeff (4);
00335     matrix.coeffRef (2) = matrix.coeff (8);
00336     matrix.coeffRef (6) = matrix.coeff (9);
00337     matrix.coeffRef (3) = matrix.coeff (12);
00338     matrix.coeffRef (7) = matrix.coeff (13);
00339     matrix.coeffRef (11) = matrix.coeff (14);
00340   }
00341 }
00342 
00344 template<typename PointT> void
00345 pcl::search::OrganizedNeighbor<PointT>::computeCameraMatrix (Eigen::Matrix3f& camera_matrix) const
00346 {
00347   Eigen::Matrix3f cam = KR_KRT_ / KR_KRT_.coeff (8);
00348 
00349   memset (&(camera_matrix.coeffRef (0)), 0, sizeof (Eigen::Matrix3f::Scalar) * 9);
00350   camera_matrix.coeffRef (8) = 1.0;
00351   
00352   if (camera_matrix.Flags & Eigen::RowMajorBit)
00353   {
00354     camera_matrix.coeffRef (2) = cam.coeff (2);
00355     camera_matrix.coeffRef (5) = cam.coeff (5);
00356     camera_matrix.coeffRef (4) = sqrt (cam.coeff (4) - cam.coeff (5) * cam.coeff (5));
00357     camera_matrix.coeffRef (1) = (cam.coeff (1) - cam.coeff (2) * cam.coeff (5)) / camera_matrix.coeff (4);
00358     camera_matrix.coeffRef (0) = sqrt (cam.coeff (0) - camera_matrix.coeff (1) * camera_matrix.coeff (1) - cam.coeff (2) * cam.coeff (2));
00359   }
00360   else
00361   {
00362     camera_matrix.coeffRef (6) = cam.coeff (2);
00363     camera_matrix.coeffRef (7) = cam.coeff (5);
00364     camera_matrix.coeffRef (4) = sqrt (cam.coeff (4) - cam.coeff (5) * cam.coeff (5));
00365     camera_matrix.coeffRef (3) = (cam.coeff (1) - cam.coeff (2) * cam.coeff (5)) / camera_matrix.coeff (4);
00366     camera_matrix.coeffRef (0) = sqrt (cam.coeff (0) - camera_matrix.coeff (3) * camera_matrix.coeff (3) - cam.coeff (2) * cam.coeff (2));
00367   }
00368 }
00369 
00371 template<typename PointT> void
00372 pcl::search::OrganizedNeighbor<PointT>::estimateProjectionMatrix ()
00373 {
00374   // internally we calculate with double but store the result into float matrices.
00375   typedef double Scalar;
00376   projection_matrix_.setZero ();
00377   if (input_->height == 1 || input_->width == 1)
00378   {
00379     PCL_ERROR ("[pcl::%s::estimateProjectionMatrix] Input dataset is not organized!\n", this->getName ().c_str ());
00380     return;
00381   }
00382   
00383   const unsigned ySkip = (input_->height >> pyramid_level_);
00384   const unsigned xSkip = (input_->width >> pyramid_level_);
00385   Eigen::Matrix<Scalar, 4, 4, Eigen::RowMajor> A = Eigen::Matrix<Scalar, 4, 4, Eigen::RowMajor>::Zero ();
00386   Eigen::Matrix<Scalar, 4, 4, Eigen::RowMajor> B = Eigen::Matrix<Scalar, 4, 4, Eigen::RowMajor>::Zero ();
00387   Eigen::Matrix<Scalar, 4, 4, Eigen::RowMajor> C = Eigen::Matrix<Scalar, 4, 4, Eigen::RowMajor>::Zero ();
00388   Eigen::Matrix<Scalar, 4, 4, Eigen::RowMajor> D = Eigen::Matrix<Scalar, 4, 4, Eigen::RowMajor>::Zero ();
00389 
00390   for (unsigned yIdx = 0, idx = 0; yIdx < input_->height; yIdx += ySkip, idx += input_->width * (ySkip - 1))
00391   {
00392     for (unsigned xIdx = 0; xIdx < input_->width; xIdx += xSkip, idx += xSkip)
00393     {
00394       if (!mask_ [idx])
00395         continue;
00396 
00397       const PointT& point = input_->points[idx];
00398       if (pcl_isfinite (point.x))
00399       {
00400         Scalar xx = point.x * point.x;
00401         Scalar xy = point.x * point.y;
00402         Scalar xz = point.x * point.z;
00403         Scalar yy = point.y * point.y;
00404         Scalar yz = point.y * point.z;
00405         Scalar zz = point.z * point.z;
00406         Scalar xx_yy = xIdx * xIdx + yIdx * yIdx;
00407 
00408         A.coeffRef (0) += xx;
00409         A.coeffRef (1) += xy;
00410         A.coeffRef (2) += xz;
00411         A.coeffRef (3) += point.x;
00412 
00413         A.coeffRef (5) += yy;
00414         A.coeffRef (6) += yz;
00415         A.coeffRef (7) += point.y;
00416 
00417         A.coeffRef (10) += zz;
00418         A.coeffRef (11) += point.z;
00419         A.coeffRef (15) += 1.0;
00420 
00421         B.coeffRef (0) -= xx * xIdx;
00422         B.coeffRef (1) -= xy * xIdx;
00423         B.coeffRef (2) -= xz * xIdx;
00424         B.coeffRef (3) -= point.x * static_cast<double>(xIdx);
00425 
00426         B.coeffRef (5) -= yy * xIdx;
00427         B.coeffRef (6) -= yz * xIdx;
00428         B.coeffRef (7) -= point.y * static_cast<double>(xIdx);
00429 
00430         B.coeffRef (10) -= zz * xIdx;
00431         B.coeffRef (11) -= point.z * static_cast<double>(xIdx);
00432 
00433         B.coeffRef (15) -= xIdx;
00434 
00435         C.coeffRef (0) -= xx * yIdx;
00436         C.coeffRef (1) -= xy * yIdx;
00437         C.coeffRef (2) -= xz * yIdx;
00438         C.coeffRef (3) -= point.x * static_cast<double>(yIdx);
00439 
00440         C.coeffRef (5) -= yy * yIdx;
00441         C.coeffRef (6) -= yz * yIdx;
00442         C.coeffRef (7) -= point.y * static_cast<double>(yIdx);
00443 
00444         C.coeffRef (10) -= zz * yIdx;
00445         C.coeffRef (11) -= point.z * static_cast<double>(yIdx);
00446 
00447         C.coeffRef (15) -= yIdx;
00448 
00449         D.coeffRef (0) += xx * xx_yy;
00450         D.coeffRef (1) += xy * xx_yy;
00451         D.coeffRef (2) += xz * xx_yy;
00452         D.coeffRef (3) += point.x * xx_yy;
00453 
00454         D.coeffRef (5) += yy * xx_yy;
00455         D.coeffRef (6) += yz * xx_yy;
00456         D.coeffRef (7) += point.y * xx_yy;
00457 
00458         D.coeffRef (10) += zz * xx_yy;
00459         D.coeffRef (11) += point.z * xx_yy;
00460 
00461         D.coeffRef (15) += xx_yy;
00462       }
00463     }
00464   }
00465 
00466   makeSymmetric(A);
00467   makeSymmetric(B);
00468   makeSymmetric(C);
00469   makeSymmetric(D);
00470 
00471   Eigen::Matrix<Scalar, 12, 12, Eigen::RowMajor> X = Eigen::Matrix<Scalar, 12, 12, Eigen::RowMajor>::Zero ();
00472   X.topLeftCorner<4,4> () = A;
00473   X.block<4,4> (0, 8) = B;
00474   X.block<4,4> (8, 0) = B;
00475   X.block<4,4> (4, 4) = A;
00476   X.block<4,4> (4, 8) = C;
00477   X.block<4,4> (8, 4) = C;
00478   X.block<4,4> (8, 8) = D;
00479 
00480   Eigen::SelfAdjointEigenSolver<Eigen::Matrix<Scalar, 12, 12, Eigen::RowMajor> > ei_symm(X);
00481   Eigen::Matrix<Scalar, 12, 12, Eigen::RowMajor> eigen_vectors = ei_symm.eigenvectors();
00482 
00483   // check whether the residual MSE is low. If its high, the cloud was not captured from a projective device.
00484   Eigen::Matrix<Scalar, 1, 1> residual_sqr = eigen_vectors.col (0).transpose () * X *  eigen_vectors.col (0);
00485   if ( fabs (residual_sqr.coeff (0)) > eps_ * A.coeff (15))
00486   {
00487     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.coeff (0) / A.coeff (15), static_cast<int> (A.coeff (15)));
00488     return;
00489   }
00490 
00491   projection_matrix_.coeffRef (0) = static_cast <float> (eigen_vectors.coeff (0));
00492   projection_matrix_.coeffRef (1) = static_cast <float> (eigen_vectors.coeff (12));
00493   projection_matrix_.coeffRef (2) = static_cast <float> (eigen_vectors.coeff (24));
00494   projection_matrix_.coeffRef (3) = static_cast <float> (eigen_vectors.coeff (36));
00495   projection_matrix_.coeffRef (4) = static_cast <float> (eigen_vectors.coeff (48));
00496   projection_matrix_.coeffRef (5) = static_cast <float> (eigen_vectors.coeff (60));
00497   projection_matrix_.coeffRef (6) = static_cast <float> (eigen_vectors.coeff (72));
00498   projection_matrix_.coeffRef (7) = static_cast <float> (eigen_vectors.coeff (84));
00499   projection_matrix_.coeffRef (8) = static_cast <float> (eigen_vectors.coeff (96));
00500   projection_matrix_.coeffRef (9) = static_cast <float> (eigen_vectors.coeff (108));
00501   projection_matrix_.coeffRef (10) = static_cast <float> (eigen_vectors.coeff (120));
00502   projection_matrix_.coeffRef (11) = static_cast <float> (eigen_vectors.coeff (132));
00503 
00504   if (projection_matrix_.coeff (0) < 0)
00505     projection_matrix_ *= -1.0;
00506 
00507   // get left 3x3 sub matrix, which contains K * R, with K = camera matrix = [[fx s cx] [0 fy cy] [0 0 1]]
00508   // and R being the rotation matrix
00509   KR_ = projection_matrix_.topLeftCorner <3, 3> ();
00510 
00511   // precalculate KR * KR^T needed by calculations during nn-search
00512   KR_KRT_ = KR_ * KR_.transpose ();
00513 }
00514 
00516 template<typename PointT> bool
00517 pcl::search::OrganizedNeighbor<PointT>::projectPoint (const PointT& point, pcl::PointXY& q) const
00518 {
00519   Eigen::Vector3f projected = KR_ * point.getVector3fMap () + projection_matrix_.block <3, 1> (0, 3);
00520   q.x = projected [0] / projected [2];
00521   q.y = projected [1] / projected [2];
00522   return (projected[2] != 0);
00523 }
00524 #define PCL_INSTANTIATE_OrganizedNeighbor(T) template class PCL_EXPORTS pcl::search::OrganizedNeighbor<T>;
00525 
00526 #endif


pcl
Author(s): Open Perception
autogenerated on Mon Oct 6 2014 03:16:07