00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
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
00057 assert (isFinite (query) && "Invalid (NaN, Inf) point coordinates given to nearestKSearch!");
00058
00059
00060 unsigned left, right, top, bottom;
00061
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
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
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
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;
00131 int yEnd = yBegin + 1;
00132
00133
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
00141
00142
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
00149 {
00150
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
00172 bool stop = false;
00173 do
00174 {
00175
00176 --xBegin;
00177 ++xEnd;
00178 --yBegin;
00179 ++yEnd;
00180
00181
00182 int xFrom = xBegin;
00183 int xTo = xEnd;
00184 clipRange (xFrom, xTo, 0, input_->width);
00185
00186
00187 if (xTo > xFrom)
00188 {
00189
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
00200
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
00211 int yFrom = yBegin + 1;
00212 int yTo = yEnd - 1;
00213 clipRange (yFrom, yTo, 0, input_->height);
00214
00215
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
00238 if (stop)
00239 getProjectedRadiusSearchBox (query, results.top ().distance, left, right, top, bottom);
00240
00241 }
00242
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
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
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
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
00508
00509 KR_ = projection_matrix_.topLeftCorner <3, 3> ();
00510
00511
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