1 // This file is part of OpenCV project.
2 // It is subject to the license terms in the LICENSE file found in the top-level directory
3 // of this distribution and at http://opencv.org/license.html.
4
5 #include "../precomp.hpp"
6 #include "../usac.hpp"
7 #include "opencv2/flann/miniflann.hpp"
8 #include <map>
9
10 namespace cv { namespace usac {
getCalibratedThreshold(double threshold,const Mat & K1,const Mat & K2)11 double Utils::getCalibratedThreshold (double threshold, const Mat &K1, const Mat &K2) {
12 return threshold / ((K1.at<double>(0, 0) + K1.at<double>(1, 1) +
13 K2.at<double>(0, 0) + K2.at<double>(1, 1)) / 4.0);
14 }
15
16 /*
17 * K1, K2 are 3x3 intrinsics matrices
18 * points is matrix of size |N| x 4
19 * Assume K = [k11 k12 k13
20 * 0 k22 k23
21 * 0 0 1]
22 */
calibratePoints(const Mat & K1,const Mat & K2,const Mat & points,Mat & calib_points)23 void Utils::calibratePoints (const Mat &K1, const Mat &K2, const Mat &points, Mat &calib_points) {
24 const auto * const points_ = (float *) points.data;
25 const auto * const k1 = (double *) K1.data;
26 const auto inv1_k11 = float(1 / k1[0]); // 1 / k11
27 const auto inv1_k12 = float(-k1[1] / (k1[0]*k1[4])); // -k12 / (k11*k22)
28 // (-k13*k22 + k12*k23) / (k11*k22)
29 const auto inv1_k13 = float((-k1[2]*k1[4] + k1[1]*k1[5]) / (k1[0]*k1[4]));
30 const auto inv1_k22 = float(1 / k1[4]); // 1 / k22
31 const auto inv1_k23 = float(-k1[5] / k1[4]); // -k23 / k22
32
33 const auto * const k2 = (double *) K2.data;
34 const auto inv2_k11 = float(1 / k2[0]);
35 const auto inv2_k12 = float(-k2[1] / (k2[0]*k2[4]));
36 const auto inv2_k13 = float((-k2[2]*k2[4] + k2[1]*k2[5]) / (k2[0]*k2[4]));
37 const auto inv2_k22 = float(1 / k2[4]);
38 const auto inv2_k23 = float(-k2[5] / k2[4]);
39
40 calib_points = Mat ( points.rows, 4, points.type());
41 auto * calib_points_ = (float *) calib_points.data;
42
43 for (int i = 0; i < points.rows; i++) {
44 const int idx = 4*i;
45 (*calib_points_++) = inv1_k11 * points_[idx ] + inv1_k12 * points_[idx+1] + inv1_k13;
46 (*calib_points_++) = inv1_k22 * points_[idx+1] + inv1_k23;
47 (*calib_points_++) = inv2_k11 * points_[idx+2] + inv2_k12 * points_[idx+3] + inv2_k13;
48 (*calib_points_++) = inv2_k22 * points_[idx+3] + inv2_k23;
49 }
50 }
51
52 /*
53 * K is 3x3 intrinsic matrix
54 * points is matrix of size |N| x 5, first two columns are image points [u_i, v_i]
55 * calib_norm_pts are K^-1 [u v 1]^T / ||K^-1 [u v 1]^T||
56 */
calibrateAndNormalizePointsPnP(const Mat & K,const Mat & pts,Mat & calib_norm_pts)57 void Utils::calibrateAndNormalizePointsPnP (const Mat &K, const Mat &pts, Mat &calib_norm_pts) {
58 const auto * const points = (float *) pts.data;
59 const auto * const k = (double *) K.data;
60 const auto inv_k11 = float(1 / k[0]);
61 const auto inv_k12 = float(-k[1] / (k[0]*k[4]));
62 const auto inv_k13 = float((-k[2]*k[4] + k[1]*k[5]) / (k[0]*k[4]));
63 const auto inv_k22 = float(1 / k[4]);
64 const auto inv_k23 = float(-k[5] / k[4]);
65
66 calib_norm_pts = Mat (pts.rows, 3, pts.type());
67 auto * calib_norm_pts_ = (float *) calib_norm_pts.data;
68
69 for (int i = 0; i < pts.rows; i++) {
70 const int idx = 5 * i;
71 const float k_inv_u = inv_k11 * points[idx] + inv_k12 * points[idx+1] + inv_k13;
72 const float k_inv_v = inv_k22 * points[idx+1] + inv_k23;
73 const float norm = 1.f / sqrtf(k_inv_u*k_inv_u + k_inv_v*k_inv_v + 1);
74 (*calib_norm_pts_++) = k_inv_u * norm;
75 (*calib_norm_pts_++) = k_inv_v * norm;
76 (*calib_norm_pts_++) = norm;
77 }
78 }
79
normalizeAndDecalibPointsPnP(const Mat & K_,Mat & pts,Mat & calib_norm_pts)80 void Utils::normalizeAndDecalibPointsPnP (const Mat &K_, Mat &pts, Mat &calib_norm_pts) {
81 const auto * const K = (double *) K_.data;
82 const auto k11 = (float)K[0], k12 = (float)K[1], k13 = (float)K[2],
83 k22 = (float)K[4], k23 = (float)K[5];
84 calib_norm_pts = Mat (pts.rows, 3, pts.type());
85 auto * points = (float *) pts.data;
86 auto * calib_norm_pts_ = (float *) calib_norm_pts.data;
87
88 for (int i = 0; i < pts.rows; i++) {
89 const int idx = 5 * i;
90 const float k_inv_u = points[idx ];
91 const float k_inv_v = points[idx+1];
92 const float norm = 1.f / sqrtf(k_inv_u*k_inv_u + k_inv_v*k_inv_v + 1);
93 (*calib_norm_pts_++) = k_inv_u * norm;
94 (*calib_norm_pts_++) = k_inv_v * norm;
95 (*calib_norm_pts_++) = norm;
96 points[idx ] = k11 * k_inv_u + k12 * k_inv_v + k13;
97 points[idx+1] = k22 * k_inv_v + k23;
98 }
99 }
100 /*
101 * decompose Projection Matrix to calibration, rotation and translation
102 * Assume K = [fx 0 tx
103 * 0 fy ty
104 * 0 0 1]
105 */
decomposeProjection(const Mat & P,Mat & K_,Mat & R,Mat & t,bool same_focal)106 void Utils::decomposeProjection (const Mat &P, Mat &K_, Mat &R, Mat &t, bool same_focal) {
107 const Mat M = P.colRange(0,3);
108 double scale = norm(M.row(2)); scale *= scale;
109 Matx33d K = Matx33d::eye();
110 K(1,2) = M.row(1).dot(M.row(2)) / scale;
111 K(0,2) = M.row(0).dot(M.row(2)) / scale;
112 K(1,1) = sqrt(M.row(1).dot(M.row(1)) / scale - K(1,2)*K(1,2));
113 K(0,0) = sqrt(M.row(0).dot(M.row(0)) / scale - K(0,2)*K(0,2));
114 if (same_focal)
115 K(0,0) = K(1,1) = (K(0,0) + K(1,1)) / 2;
116 R = K.inv() * M / sqrt(scale);
117 if (determinant(M) < 0) R *= -1;
118 t = R * M.inv() * P.col(3);
119 K_ = Mat(K);
120 }
121
getSkewSymmetric(const Vec3d & v)122 Matx33d Math::getSkewSymmetric(const Vec3d &v) {
123 return Matx33d(0, -v[2], v[1],
124 v[2], 0, -v[0],
125 -v[1], v[0], 0);
126 }
127
rotVec2RotMat(const Vec3d & v)128 Matx33d Math::rotVec2RotMat (const Vec3d &v) {
129 const double phi = sqrt(v[0]*v[0]+v[1]*v[1]+v[2]*v[2]);
130 const double x = v[0] / phi, y = v[1] / phi, z = v[2] / phi;
131 const double a = sin(phi), b = cos(phi);
132 // R = I + sin(phi) * skew(v) + (1 - cos(phi) * skew(v)^2
133 return Matx33d((b - 1)*y*y + (b - 1)*z*z + 1, -a*z - x*y*(b - 1), a*y - x*z*(b - 1),
134 a*z - x*y*(b - 1), (b - 1)*x*x + (b - 1)*z*z + 1, -a*x - y*z*(b - 1),
135 -a*y - x*z*(b - 1), a*x - y*z*(b - 1), (b - 1)*x*x + (b - 1)*y*y + 1);
136 }
137
rotMat2RotVec(const Matx33d & R)138 Vec3d Math::rotMat2RotVec (const Matx33d &R) {
139 // https://math.stackexchange.com/questions/83874/efficient-and-accurate-numerical-implementation-of-the-inverse-rodrigues-rotatio?rq=1
140 Vec3d rot_vec;
141 const double trace = R(0,0)+R(1,1)+R(2,2);
142 if (trace >= 3 - FLT_EPSILON) {
143 rot_vec = (0.5 * (trace-3)/12)*Vec3d(R(2,1)-R(1,2),
144 R(0,2)-R(2,0),
145 R(1,0)-R(0,1));
146 } else if (3 - FLT_EPSILON > trace && trace > -1 + FLT_EPSILON) {
147 double theta = acos((trace - 1) / 2);
148 rot_vec = (theta / (2 * sin(theta))) * Vec3d(R(2,1)-R(1,2),
149 R(0,2)-R(2,0),
150 R(1,0)-R(0,1));
151 } else {
152 int a;
153 if (R(0,0) > R(1,1))
154 a = R(0,0) > R(2,2) ? 0 : 2;
155 else
156 a = R(1,1) > R(2,2) ? 1 : 2;
157 Vec3d v;
158 int b = (a + 1) % 3, c = (a + 2) % 3;
159 double s = sqrt(R(a,a) - R(b,b) - R(c,c) + 1);
160 v[a] = s / 2;
161 v[b] = (R(b,a) + R(a,b)) / (2 * s);
162 v[c] = (R(c,a) + R(a,c)) / (2 * s);
163 rot_vec = M_PI * v / norm(v);
164 }
165 return rot_vec;
166 }
167
168 /*
169 * Eliminate matrix of m rows and n columns to be upper triangular.
170 */
eliminateUpperTriangular(std::vector<double> & a,int m,int n)171 bool Math::eliminateUpperTriangular (std::vector<double> &a, int m, int n) {
172 for (int r = 0; r < m; r++){
173 double pivot = a[r*n+r];
174 int row_with_pivot = r;
175
176 // find the maximum pivot value among r-th column
177 for (int k = r+1; k < m; k++)
178 if (fabs(pivot) < fabs(a[k*n+r])) {
179 pivot = a[k*n+r];
180 row_with_pivot = k;
181 }
182
183 // if pivot value is 0 continue
184 if (fabs(pivot) < DBL_EPSILON)
185 return false; // matrix is not full rank -> terminate
186
187 // swap row with maximum pivot value with current row
188 for (int c = r; c < n; c++)
189 std::swap(a[row_with_pivot*n+c], a[r*n+c]);
190
191 // eliminate other rows
192 for (int j = r+1; j < m; j++){
193 const int row_idx1 = j*n, row_idx2 = r*n;
194 const auto fac = a[row_idx1+r] / pivot;
195 a[row_idx1+r] = 0; // zero eliminated element
196 for (int c = r+1; c < n; c++)
197 a[row_idx1+c] -= fac * a[row_idx2+c];
198 }
199 }
200 return true;
201 }
202
203 //////////////////////////////////////// RANDOM GENERATOR /////////////////////////////
204 class UniformRandomGeneratorImpl : public UniformRandomGenerator {
205 private:
206 int subset_size = 0, max_range = 0;
207 std::vector<int> subset;
208 RNG rng;
209 public:
UniformRandomGeneratorImpl(int state)210 explicit UniformRandomGeneratorImpl (int state) : rng(state) {}
211
212 // interval is <0; max_range);
UniformRandomGeneratorImpl(int state,int max_range_,int subset_size_)213 UniformRandomGeneratorImpl (int state, int max_range_, int subset_size_) : rng(state) {
214 subset_size = subset_size_;
215 max_range = max_range_;
216 subset = std::vector<int>(subset_size_);
217 }
218
getRandomNumber()219 int getRandomNumber () override {
220 return rng.uniform(0, max_range);
221 }
222
getRandomNumber(int max_rng)223 int getRandomNumber (int max_rng) override {
224 return rng.uniform(0, max_rng);
225 }
226
227 // closed range
resetGenerator(int max_range_)228 void resetGenerator (int max_range_) override {
229 CV_CheckGE(0, max_range_, "max range must be greater than 0");
230 max_range = max_range_;
231 }
232
generateUniqueRandomSet(std::vector<int> & sample)233 void generateUniqueRandomSet (std::vector<int>& sample) override {
234 CV_CheckLE(subset_size, max_range, "RandomGenerator. Subset size must be LE than range!");
235 int j, num;
236 sample[0] = rng.uniform(0, max_range);
237 for (int i = 1; i < subset_size;) {
238 num = rng.uniform(0, max_range);
239 // check if value is in array
240 for (j = i - 1; j >= 0; j--)
241 if (num == sample[j])
242 // if so, generate again
243 break;
244 // success, value is not in array, so it is unique, add to sample.
245 if (j == -1) sample[i++] = num;
246 }
247 }
248
249 // interval is <0; max_range)
generateUniqueRandomSet(std::vector<int> & sample,int max_range_)250 void generateUniqueRandomSet (std::vector<int>& sample, int max_range_) override {
251 /*
252 * necessary condition:
253 * if subset size is bigger than range then array cannot be unique,
254 * so function has infinite loop.
255 */
256 CV_CheckLE(subset_size, max_range_, "RandomGenerator. Subset size must be LE than range!");
257 int num, j;
258 sample[0] = rng.uniform(0, max_range_);
259 for (int i = 1; i < subset_size;) {
260 num = rng.uniform(0, max_range_);
261 for (j = i - 1; j >= 0; j--)
262 if (num == sample[j])
263 break;
264 if (j == -1) sample[i++] = num;
265 }
266 }
267
268 // interval is <0, max_range)
generateUniqueRandomSet(std::vector<int> & sample,int subset_size_,int max_range_)269 void generateUniqueRandomSet (std::vector<int>& sample, int subset_size_, int max_range_) override {
270 CV_CheckLE(subset_size_, max_range_, "RandomGenerator. Subset size must be LE than range!");
271 int num, j;
272 sample[0] = rng.uniform(0, max_range_);
273 for (int i = 1; i < subset_size_;) {
274 num = rng.uniform(0, max_range_);
275 for (j = i - 1; j >= 0; j--)
276 if (num == sample[j])
277 break;
278 if (j == -1) sample[i++] = num;
279 }
280 }
generateUniqueRandomSubset(std::vector<int> & array1,int size1)281 const std::vector<int> &generateUniqueRandomSubset (std::vector<int> &array1, int size1) override {
282 CV_CheckLE(subset_size, size1, "RandomGenerator. Subset size must be LE than range!");
283 int temp_size1 = size1;
284 for (int i = 0; i < subset_size; i++) {
285 const int idx1 = rng.uniform(0, temp_size1);
286 subset[i] = array1[idx1];
287 std::swap(array1[idx1], array1[--temp_size1]);
288 }
289 return subset;
290 }
291
setSubsetSize(int subset_size_)292 void setSubsetSize (int subset_size_) override {
293 subset_size = subset_size_;
294 }
getSubsetSize() const295 int getSubsetSize () const override { return subset_size; }
clone(int state) const296 Ptr<RandomGenerator> clone (int state) const override {
297 return makePtr<UniformRandomGeneratorImpl>(state, max_range, subset_size);
298 }
299 };
300
create(int state)301 Ptr<UniformRandomGenerator> UniformRandomGenerator::create (int state) {
302 return makePtr<UniformRandomGeneratorImpl>(state);
303 }
create(int state,int max_range,int subset_size_)304 Ptr<UniformRandomGenerator> UniformRandomGenerator::create
305 (int state, int max_range, int subset_size_) {
306 return makePtr<UniformRandomGeneratorImpl>(state, max_range, subset_size_);
307 }
308
309 // @k_minth - desired k-th minimal element. For median is half of array
310 // closed working interval of array <@left; @right>
311 float quicksort_median (std::vector<float> &array, int k_minth, int left, int right);
quicksort_median(std::vector<float> & array,int k_minth,int left,int right)312 float quicksort_median (std::vector<float> &array, int k_minth, int left, int right) {
313 // length is 0, return single value
314 if (right - left == 0) return array[left];
315
316 // get pivot, the rightest value in array
317 const auto pivot = array[right];
318 int right_ = right - 1; // -1, not including pivot
319 // counter of values smaller equal than pivot
320 int j = left, values_less_eq_pivot = 1; // 1, inludes pivot already
321 for (; j <= right_;) {
322 if (array[j] <= pivot) {
323 j++;
324 values_less_eq_pivot++;
325 } else
326 // value is bigger than pivot, swap with right_ value
327 // swap values in array and decrease interval
328 std::swap(array[j], array[right_--]);
329 }
330 if (values_less_eq_pivot == k_minth) return pivot;
331 if (k_minth > values_less_eq_pivot)
332 return quicksort_median(array, k_minth - values_less_eq_pivot, j, right-1);
333 else
334 return quicksort_median(array, k_minth, left, j-1);
335 }
336
337 // find median using quicksort with complexity O(log n)
338 // Note, function changes order of values in array
findMedian(std::vector<float> & array)339 float Utils::findMedian (std::vector<float> &array) {
340 const int length = static_cast<int>(array.size());
341 if (length % 2) {
342 // odd number of values
343 return quicksort_median (array, length/2+1, 0, length-1);
344 } else {
345 // even: return average
346 return (quicksort_median(array, length/2 , 0, length-1) +
347 quicksort_median(array, length/2+1, 0, length-1))/2;
348 }
349 }
350
351 ///////////////////////////////////////////////////////////////////////////////////////////////////
352 ///////////////////////////////// Radius Search Graph /////////////////////////////////////////////
353 ///////////////////////////////////////////////////////////////////////////////////////////////////
354 class RadiusSearchNeighborhoodGraphImpl : public RadiusSearchNeighborhoodGraph {
355 private:
356 std::vector<std::vector<int>> graph;
357 public:
RadiusSearchNeighborhoodGraphImpl(const Mat & container_,int points_size,double radius,int flann_search_params,int num_kd_trees)358 RadiusSearchNeighborhoodGraphImpl (const Mat &container_, int points_size,
359 double radius, int flann_search_params, int num_kd_trees) {
360 // Radius search OpenCV works only with float data
361 CV_Assert(container_.type() == CV_32F);
362
363 FlannBasedMatcher flann(makePtr<flann::KDTreeIndexParams>(num_kd_trees), makePtr<flann::SearchParams>(flann_search_params));
364 std::vector<std::vector<DMatch>> neighbours;
365 flann.radiusMatch(container_, container_, neighbours, (float)radius);
366
367 // allocate graph
368 graph = std::vector<std::vector<int>> (points_size);
369
370 int pt = 0;
371 for (const auto &n : neighbours) {
372 auto &graph_row = graph[pt];
373 graph_row = std::vector<int>(n.size()-1);
374 int j = 0;
375 for (const auto &idx : n)
376 // skip neighbor which has the same index as requested point
377 if (idx.trainIdx != pt)
378 graph_row[j++] = idx.trainIdx;
379 pt++;
380 }
381 }
382
getNeighbors(int point_idx) const383 inline const std::vector<int> &getNeighbors(int point_idx) const override {
384 return graph[point_idx];
385 }
386 };
create(const Mat & points,int points_size,double radius_,int flann_search_params,int num_kd_trees)387 Ptr<RadiusSearchNeighborhoodGraph> RadiusSearchNeighborhoodGraph::create (const Mat &points,
388 int points_size, double radius_, int flann_search_params, int num_kd_trees) {
389 return makePtr<RadiusSearchNeighborhoodGraphImpl> (points, points_size, radius_,
390 flann_search_params, num_kd_trees);
391 }
392
393 ///////////////////////////////////////////////////////////////////////////////////////////////////
394 ///////////////////////////////// FLANN Graph /////////////////////////////////////////////
395 ///////////////////////////////////////////////////////////////////////////////////////////////////
396 class FlannNeighborhoodGraphImpl : public FlannNeighborhoodGraph {
397 private:
398 std::vector<std::vector<int>> graph;
399 std::vector<std::vector<double>> distances;
400 public:
FlannNeighborhoodGraphImpl(const Mat & container_,int points_size,int k_nearest_neighbors,bool get_distances,int flann_search_params_,int num_kd_trees)401 FlannNeighborhoodGraphImpl (const Mat &container_, int points_size, int k_nearest_neighbors,
402 bool get_distances, int flann_search_params_, int num_kd_trees) {
403 CV_Assert(k_nearest_neighbors <= points_size);
404 // FLANN works only with float data
405 CV_Assert(container_.type() == CV_32F);
406
407 flann::Index flannIndex (container_.reshape(1), flann::KDTreeIndexParams(num_kd_trees));
408 Mat dists, nearest_neighbors;
409
410 flannIndex.knnSearch(container_, nearest_neighbors, dists, k_nearest_neighbors+1,
411 flann::SearchParams(flann_search_params_));
412
413 // first nearest neighbor of point is this point itself.
414 // remove this first column
415 nearest_neighbors.colRange(1, k_nearest_neighbors+1).copyTo (nearest_neighbors);
416
417 graph = std::vector<std::vector<int>>(points_size, std::vector<int>(k_nearest_neighbors));
418 const auto * const nn = (int *) nearest_neighbors.data;
419 const auto * const dists_ptr = (float *) dists.data;
420
421 if (get_distances)
422 distances = std::vector<std::vector<double>>(points_size, std::vector<double>(k_nearest_neighbors));
423
424 for (int pt = 0; pt < points_size; pt++) {
425 std::copy(nn + k_nearest_neighbors*pt, nn + k_nearest_neighbors*pt + k_nearest_neighbors, &graph[pt][0]);
426 if (get_distances)
427 std::copy(dists_ptr + k_nearest_neighbors*pt, dists_ptr + k_nearest_neighbors*pt + k_nearest_neighbors,
428 &distances[pt][0]);
429 }
430 }
getNeighborsDistances(int idx) const431 const std::vector<double>& getNeighborsDistances (int idx) const override {
432 return distances[idx];
433 }
getNeighbors(int point_idx) const434 inline const std::vector<int> &getNeighbors(int point_idx) const override {
435 // CV_Assert(point_idx_ < num_vertices);
436 return graph[point_idx];
437 }
438 };
439
create(const Mat & points,int points_size,int k_nearest_neighbors_,bool get_distances,int flann_search_params_,int num_kd_trees)440 Ptr<FlannNeighborhoodGraph> FlannNeighborhoodGraph::create(const Mat &points,
441 int points_size, int k_nearest_neighbors_, bool get_distances,
442 int flann_search_params_, int num_kd_trees) {
443 return makePtr<FlannNeighborhoodGraphImpl>(points, points_size,
444 k_nearest_neighbors_, get_distances, flann_search_params_, num_kd_trees);
445 }
446
447 ///////////////////////////////////////////////////////////////////////////////////////////////////
448 ///////////////////////////////// Grid Neighborhood Graph /////////////////////////////////////////
449 ///////////////////////////////////////////////////////////////////////////////////////////////////
450 class GridNeighborhoodGraphImpl : public GridNeighborhoodGraph {
451 private:
452 // This struct is used for the nearest neighbors search by griding two images.
453 struct CellCoord {
454 int c1x, c1y, c2x, c2y;
CellCoordcv::usac::GridNeighborhoodGraphImpl::CellCoord455 CellCoord (int c1x_, int c1y_, int c2x_, int c2y_) {
456 c1x = c1x_; c1y = c1y_; c2x = c2x_; c2y = c2y_;
457 }
operator ==cv::usac::GridNeighborhoodGraphImpl::CellCoord458 bool operator==(const CellCoord &o) const {
459 return c1x == o.c1x && c1y == o.c1y && c2x == o.c2x && c2y == o.c2y;
460 }
operator <cv::usac::GridNeighborhoodGraphImpl::CellCoord461 bool operator<(const CellCoord &o) const {
462 if (c1x < o.c1x) return true;
463 if (c1x == o.c1x && c1y < o.c1y) return true;
464 if (c1x == o.c1x && c1y == o.c1y && c2x < o.c2x) return true;
465 return c1x == o.c1x && c1y == o.c1y && c2x == o.c2x && c2y < o.c2y;
466 }
467 };
468
469 std::map<CellCoord, std::vector<int >> neighbors_map;
470 std::vector<std::vector<int>> graph;
471 public:
GridNeighborhoodGraphImpl(const Mat & container_,int points_size,int cell_size_x_img1,int cell_size_y_img1,int cell_size_x_img2,int cell_size_y_img2,int max_neighbors)472 GridNeighborhoodGraphImpl (const Mat &container_, int points_size,
473 int cell_size_x_img1, int cell_size_y_img1, int cell_size_x_img2, int cell_size_y_img2,
474 int max_neighbors) {
475
476 const auto * const container = (float *) container_.data;
477 // <int, int, int, int> -> {neighbors set}
478 // Key is cell position. The value is indexes of neighbors.
479
480 const float cell_sz_x1 = 1.f / (float) cell_size_x_img1,
481 cell_sz_y1 = 1.f / (float) cell_size_y_img1,
482 cell_sz_x2 = 1.f / (float) cell_size_x_img2,
483 cell_sz_y2 = 1.f / (float) cell_size_y_img2;
484 const int dimension = container_.cols;
485 for (int i = 0; i < points_size; i++) {
486 const int idx = dimension * i;
487 neighbors_map[CellCoord((int)(container[idx ] * cell_sz_x1),
488 (int)(container[idx+1] * cell_sz_y1),
489 (int)(container[idx+2] * cell_sz_x2),
490 (int)(container[idx+3] * cell_sz_y2))].emplace_back(i);
491 }
492
493 //--------- create a graph ----------
494 graph = std::vector<std::vector<int>>(points_size);
495
496 // store neighbors cells into graph (2D vector)
497 for (const auto &cell : neighbors_map) {
498 const int neighbors_in_cell = static_cast<int>(cell.second.size());
499
500 // only one point in cell -> no neighbors
501 if (neighbors_in_cell < 2) continue;
502
503 const std::vector<int> &neighbors = cell.second;
504 // ---------- fill graph -----
505 for (int v_in_cell : neighbors) {
506 // there is always at least one neighbor
507 auto &graph_row = graph[v_in_cell];
508 graph_row = std::vector<int>(std::min(max_neighbors, neighbors_in_cell-1));
509 int j = 0;
510 for (int n : neighbors)
511 if (n != v_in_cell){
512 graph_row[j++] = n;
513 if (j >= max_neighbors)
514 break;
515 }
516 }
517 }
518 }
519
getNeighbors(int point_idx) const520 inline const std::vector<int> &getNeighbors(int point_idx) const override {
521 // Note, neighbors vector also includes point_idx!
522 // return neighbors_map[vertices_to_cells[point_idx]];
523 return graph[point_idx];
524 }
525 };
526
create(const Mat & points,int points_size,int cell_size_x_img1_,int cell_size_y_img1_,int cell_size_x_img2_,int cell_size_y_img2_,int max_neighbors)527 Ptr<GridNeighborhoodGraph> GridNeighborhoodGraph::create(const Mat &points,
528 int points_size, int cell_size_x_img1_, int cell_size_y_img1_,
529 int cell_size_x_img2_, int cell_size_y_img2_, int max_neighbors) {
530 return makePtr<GridNeighborhoodGraphImpl>(points, points_size,
531 cell_size_x_img1_, cell_size_y_img1_, cell_size_x_img2_, cell_size_y_img2_, max_neighbors);
532 }
533 }}