Point Cloud Library (PCL) 1.12.0
Loading...
Searching...
No Matches
organized.hpp
1/*
2 * Software License Agreement (BSD License)
3 *
4 * Point Cloud Library (PCL) - www.pointclouds.org
5 * Copyright (c) 2010-2011, Willow Garage, Inc.
6 *
7 * All rights reserved.
8 *
9 * Redistribution and use in source and binary forms, with or without
10 * modification, are permitted provided that the following conditions
11 * are met:
12 *
13 * * Redistributions of source code must retain the above copyright
14 * notice, this list of conditions and the following disclaimer.
15 * * Redistributions in binary form must reproduce the above
16 * copyright notice, this list of conditions and the following
17 * disclaimer in the documentation and/or other materials provided
18 * with the distribution.
19 * * Neither the name of the copyright holder(s) nor the names of its
20 * contributors may be used to endorse or promote products derived
21 * from this software without specific prior written permission.
22 *
23 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34 * POSSIBILITY OF SUCH DAMAGE.
35 *
36 * $Id$
37 *
38 */
39
40#pragma once
41
42#include <pcl/search/organized.h>
43#include <pcl/common/point_tests.h> // for pcl::isFinite
44#include <pcl/common/projection_matrix.h> // for getCameraMatrixFromProjectionMatrix, ...
45#include <Eigen/Eigenvalues>
46
47//////////////////////////////////////////////////////////////////////////////////////////////
48template<typename PointT> int
50 const double radius,
51 Indices &k_indices,
52 std::vector<float> &k_sqr_distances,
53 unsigned int max_nn) const
54{
55 // NAN test
56 assert (isFinite (query) && "Invalid (NaN, Inf) point coordinates given to nearestKSearch!");
57
58 // search window
59 unsigned left, right, top, bottom;
60 //unsigned x, y, idx;
61 float squared_distance;
62 const float squared_radius = radius * radius;
63
64 k_indices.clear ();
65 k_sqr_distances.clear ();
66
67 this->getProjectedRadiusSearchBox (query, squared_radius, left, right, top, bottom);
68
69 // iterate over search box
70 if (max_nn == 0 || max_nn >= static_cast<unsigned int> (input_->size ()))
71 max_nn = static_cast<unsigned int> (input_->size ());
72
73 k_indices.reserve (max_nn);
74 k_sqr_distances.reserve (max_nn);
75
76 unsigned yEnd = (bottom + 1) * input_->width + right + 1;
77 unsigned idx = top * input_->width + left;
78 unsigned skip = input_->width - right + left - 1;
79 unsigned xEnd = idx - left + right + 1;
80
81 for (; xEnd != yEnd; idx += skip, xEnd += input_->width)
82 {
83 for (; idx < xEnd; ++idx)
84 {
85 if (!mask_[idx] || !isFinite ((*input_)[idx]))
86 continue;
87
88 float dist_x = (*input_)[idx].x - query.x;
89 float dist_y = (*input_)[idx].y - query.y;
90 float dist_z = (*input_)[idx].z - query.z;
91 squared_distance = dist_x * dist_x + dist_y * dist_y + dist_z * dist_z;
92 //squared_distance = ((*input_)[idx].getVector3fMap () - query.getVector3fMap ()).squaredNorm ();
93 if (squared_distance <= squared_radius)
94 {
95 k_indices.push_back (idx);
96 k_sqr_distances.push_back (squared_distance);
97 // already done ?
98 if (k_indices.size () == max_nn)
99 {
100 if (sorted_results_)
101 this->sortResults (k_indices, k_sqr_distances);
102 return (max_nn);
103 }
104 }
105 }
106 }
107 if (sorted_results_)
108 this->sortResults (k_indices, k_sqr_distances);
109 return (static_cast<int> (k_indices.size ()));
110}
111
112//////////////////////////////////////////////////////////////////////////////////////////////
113template<typename PointT> int
115 int k,
116 Indices &k_indices,
117 std::vector<float> &k_sqr_distances) const
118{
119 assert (isFinite (query) && "Invalid (NaN, Inf) point coordinates given to nearestKSearch!");
120 if (k < 1)
121 {
122 k_indices.clear ();
123 k_sqr_distances.clear ();
124 return (0);
125 }
126
127 Eigen::Vector3f queryvec (query.x, query.y, query.z);
128 // project query point on the image plane
129 //Eigen::Vector3f q = KR_ * query.getVector3fMap () + projection_matrix_.block <3, 1> (0, 3);
130 Eigen::Vector3f q (KR_ * queryvec + projection_matrix_.block <3, 1> (0, 3));
131 int xBegin = int(q [0] / q [2] + 0.5f);
132 int yBegin = int(q [1] / q [2] + 0.5f);
133 int xEnd = xBegin + 1; // end is the pixel that is not used anymore, like in iterators
134 int yEnd = yBegin + 1;
135
136 // the search window. This is supposed to shrink within the iterations
137 unsigned left = 0;
138 unsigned right = input_->width - 1;
139 unsigned top = 0;
140 unsigned bottom = input_->height - 1;
141
142 std::vector <Entry> results; // sorted from smallest to largest distance
143 results.reserve (k);
144 // add point laying on the projection of the query point.
145 if (xBegin >= 0 &&
146 xBegin < static_cast<int> (input_->width) &&
147 yBegin >= 0 &&
148 yBegin < static_cast<int> (input_->height))
149 testPoint (query, k, results, yBegin * input_->width + xBegin);
150 else // point lys
151 {
152 // find the box that touches the image border -> don't waste time evaluating boxes that are completely outside the image!
153 int dist = std::numeric_limits<int>::max ();
154
155 if (xBegin < 0)
156 dist = -xBegin;
157 else if (xBegin >= static_cast<int> (input_->width))
158 dist = xBegin - static_cast<int> (input_->width);
159
160 if (yBegin < 0)
161 dist = std::min (dist, -yBegin);
162 else if (yBegin >= static_cast<int> (input_->height))
163 dist = std::min (dist, yBegin - static_cast<int> (input_->height));
164
165 xBegin -= dist;
166 xEnd += dist;
167
168 yBegin -= dist;
169 yEnd += dist;
170 }
171
172
173 // stop used as isChanged as well as stop.
174 bool stop = false;
175 do
176 {
177 // increment box size
178 --xBegin;
179 ++xEnd;
180 --yBegin;
181 ++yEnd;
182
183 // the range in x-direction which intersects with the image width
184 int xFrom = xBegin;
185 int xTo = xEnd;
186 clipRange (xFrom, xTo, 0, input_->width);
187
188 // if x-extend is not 0
189 if (xTo > xFrom)
190 {
191 // if upper line of the rectangle is visible and x-extend is not 0
192 if (yBegin >= 0 && yBegin < static_cast<int> (input_->height))
193 {
194 index_t idx = yBegin * input_->width + xFrom;
195 index_t idxTo = idx + xTo - xFrom;
196 for (; idx < idxTo; ++idx)
197 stop = testPoint (query, k, results, idx) || stop;
198 }
199
200
201 // the row yEnd does NOT belong to the box -> last row = yEnd - 1
202 // if lower line of the rectangle is visible
203 if (yEnd > 0 && yEnd <= static_cast<int> (input_->height))
204 {
205 index_t idx = (yEnd - 1) * input_->width + xFrom;
206 index_t idxTo = idx + xTo - xFrom;
207
208 for (; idx < idxTo; ++idx)
209 stop = testPoint (query, k, results, idx) || stop;
210 }
211
212 // skip first row and last row (already handled above)
213 int yFrom = yBegin + 1;
214 int yTo = yEnd - 1;
215 clipRange (yFrom, yTo, 0, input_->height);
216
217 // if we have lines in between that are also visible
218 if (yFrom < yTo)
219 {
220 if (xBegin >= 0 && xBegin < static_cast<int> (input_->width))
221 {
222 index_t idx = yFrom * input_->width + xBegin;
223 index_t idxTo = yTo * input_->width + xBegin;
224
225 for (; idx < idxTo; idx += input_->width)
226 stop = testPoint (query, k, results, idx) || stop;
227 }
228
229 if (xEnd > 0 && xEnd <= static_cast<int> (input_->width))
230 {
231 index_t idx = yFrom * input_->width + xEnd - 1;
232 index_t idxTo = yTo * input_->width + xEnd - 1;
233
234 for (; idx < idxTo; idx += input_->width)
235 stop = testPoint (query, k, results, idx) || stop;
236 }
237
238 }
239 // stop here means that the k-nearest neighbor changed -> recalculate bounding box of ellipse.
240 if (stop)
241 getProjectedRadiusSearchBox (query, results.back ().distance, left, right, top, bottom);
242
243 }
244 // now we use it as stop flag -> if bounding box is completely within the already examined search box were done!
245 stop = (static_cast<int> (left) >= xBegin && static_cast<int> (left) < xEnd &&
246 static_cast<int> (right) >= xBegin && static_cast<int> (right) < xEnd &&
247 static_cast<int> (top) >= yBegin && static_cast<int> (top) < yEnd &&
248 static_cast<int> (bottom) >= yBegin && static_cast<int> (bottom) < yEnd);
249
250 } while (!stop);
251
252
253 const auto results_size = results.size ();
254 k_indices.resize (results_size);
255 k_sqr_distances.resize (results_size);
256 std::size_t idx = 0;
257 for(const auto& result : results)
258 {
259 k_indices [idx] = result.index;
260 k_sqr_distances [idx] = result.distance;
261 ++idx;
262 }
263
264 return (static_cast<int> (results_size));
265}
266
267////////////////////////////////////////////////////////////////////////////////////////////
268template<typename PointT> void
270 float squared_radius,
271 unsigned &minX,
272 unsigned &maxX,
273 unsigned &minY,
274 unsigned &maxY) const
275{
276 Eigen::Vector3f queryvec (point.x, point.y, point.z);
277 //Eigen::Vector3f q = KR_ * point.getVector3fMap () + projection_matrix_.block <3, 1> (0, 3);
278 Eigen::Vector3f q (KR_ * queryvec + projection_matrix_.block <3, 1> (0, 3));
279
280 float a = squared_radius * KR_KRT_.coeff (8) - q [2] * q [2];
281 float b = squared_radius * KR_KRT_.coeff (7) - q [1] * q [2];
282 float c = squared_radius * KR_KRT_.coeff (4) - q [1] * q [1];
283 int min, max;
284 // a and c are multiplied by two already => - 4ac -> - ac
285 float det = b * b - a * c;
286 if (det < 0)
287 {
288 minY = 0;
289 maxY = input_->height - 1;
290 }
291 else
292 {
293 float y1 = static_cast<float> ((b - std::sqrt (det)) / a);
294 float y2 = static_cast<float> ((b + std::sqrt (det)) / a);
295
296 min = std::min (static_cast<int> (std::floor (y1)), static_cast<int> (std::floor (y2)));
297 max = std::max (static_cast<int> (std::ceil (y1)), static_cast<int> (std::ceil (y2)));
298 minY = static_cast<unsigned> (std::min (static_cast<int> (input_->height) - 1, std::max (0, min)));
299 maxY = static_cast<unsigned> (std::max (std::min (static_cast<int> (input_->height) - 1, max), 0));
300 }
301
302 b = squared_radius * KR_KRT_.coeff (6) - q [0] * q [2];
303 c = squared_radius * KR_KRT_.coeff (0) - q [0] * q [0];
304
305 det = b * b - a * c;
306 if (det < 0)
307 {
308 minX = 0;
309 maxX = input_->width - 1;
310 }
311 else
312 {
313 float x1 = static_cast<float> ((b - std::sqrt (det)) / a);
314 float x2 = static_cast<float> ((b + std::sqrt (det)) / a);
315
316 min = std::min (static_cast<int> (std::floor (x1)), static_cast<int> (std::floor (x2)));
317 max = std::max (static_cast<int> (std::ceil (x1)), static_cast<int> (std::ceil (x2)));
318 minX = static_cast<unsigned> (std::min (static_cast<int> (input_->width)- 1, std::max (0, min)));
319 maxX = static_cast<unsigned> (std::max (std::min (static_cast<int> (input_->width) - 1, max), 0));
320 }
321}
322
323
324//////////////////////////////////////////////////////////////////////////////////////////////
325template<typename PointT> void
327{
328 pcl::getCameraMatrixFromProjectionMatrix (projection_matrix_, camera_matrix);
329}
330
331//////////////////////////////////////////////////////////////////////////////////////////////
332template<typename PointT> void
334{
335 // internally we calculate with double but store the result into float matrices.
336 projection_matrix_.setZero ();
337 if (input_->height == 1 || input_->width == 1)
338 {
339 PCL_ERROR ("[pcl::%s::estimateProjectionMatrix] Input dataset is not organized!\n", this->getName ().c_str ());
340 return;
341 }
342
343 const unsigned ySkip = (std::max) (input_->height >> pyramid_level_, unsigned (1));
344 const unsigned xSkip = (std::max) (input_->width >> pyramid_level_, unsigned (1));
345
346 Indices indices;
347 indices.reserve (input_->size () >> (pyramid_level_ << 1));
348
349 for (unsigned yIdx = 0, idx = 0; yIdx < input_->height; yIdx += ySkip, idx += input_->width * ySkip)
350 {
351 for (unsigned xIdx = 0, idx2 = idx; xIdx < input_->width; xIdx += xSkip, idx2 += xSkip)
352 {
353 if (!mask_ [idx2])
354 continue;
355
356 indices.push_back (idx2);
357 }
358 }
359
360 double residual_sqr = pcl::estimateProjectionMatrix<PointT> (input_, projection_matrix_, indices);
361
362 if (std::abs (residual_sqr) > eps_ * float (indices.size ()))
363 {
364 PCL_ERROR ("[pcl::%s::estimateProjectionMatrix] 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 ());
365 return;
366 }
367
368 // get left 3x3 sub matrix, which contains K * R, with K = camera matrix = [[fx s cx] [0 fy cy] [0 0 1]]
369 // and R being the rotation matrix
370 KR_ = projection_matrix_.topLeftCorner <3, 3> ();
371
372 // precalculate KR * KR^T needed by calculations during nn-search
373 KR_KRT_ = KR_ * KR_.transpose ();
374}
375
376//////////////////////////////////////////////////////////////////////////////////////////////
377template<typename PointT> bool
379{
380 Eigen::Vector3f projected = KR_ * point.getVector3fMap () + projection_matrix_.block <3, 1> (0, 3);
381 q.x = projected [0] / projected [2];
382 q.y = projected [1] / projected [2];
383 return (projected[2] != 0);
384}
385#define PCL_INSTANTIATE_OrganizedNeighbor(T) template class PCL_EXPORTS pcl::search::OrganizedNeighbor<T>;
386
int radiusSearch(const PointT &p_q, double radius, Indices &k_indices, std::vector< float > &k_sqr_distances, unsigned int max_nn=0) const override
Search for all neighbors of query point that are within a given radius.
Definition organized.hpp:49
int nearestKSearch(const PointT &p_q, int k, Indices &k_indices, std::vector< float > &k_sqr_distances) const override
Search for the k-nearest neighbors for a given query point.
void computeCameraMatrix(Eigen::Matrix3f &camera_matrix) const
Compute the camera matrix.
void estimateProjectionMatrix()
estimated the projection matrix from the input cloud.
bool projectPoint(const PointT &p, pcl::PointXY &q) const
projects a point into the image
void getProjectedRadiusSearchBox(const PointT &point, float squared_radius, unsigned &minX, unsigned &minY, unsigned &maxX, unsigned &maxY) const
Obtain a search box in 2D from a sphere with a radius in 3D.
bool isFinite(const PointT &pt)
Tests if the 3D components of a point are all finite param[in] pt point to be tested return true if f...
Definition point_tests.h:55
detail::int_type_t< detail::index_type_size, detail::index_type_signed > index_t
Type used for an index in PCL.
Definition types.h:112
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition types.h:133
double estimateProjectionMatrix(typename pcl::PointCloud< PointT >::ConstPtr cloud, Eigen::Matrix< float, 3, 4, Eigen::RowMajor > &projection_matrix, const Indices &indices)
Estimates the projection matrix P = K * (R|-R*t) from organized point clouds, with K = [[fx,...
PCL_EXPORTS void getCameraMatrixFromProjectionMatrix(const Eigen::Matrix< float, 3, 4, Eigen::RowMajor > &projection_matrix, Eigen::Matrix3f &camera_matrix)
Determines the camera matrix from the given projection matrix.
A 2D point structure representing Euclidean xy coordinates.
A point structure representing Euclidean xyz coordinates, and the RGB color.