1 // This is oxl/mvl/ClosestImagePointFinder.cxx
2 //:
3 // \file
4
5 #include <vector>
6 #include <map>
7 #include <iostream>
8 #include <functional>
9 #include <utility>
10 #include "ClosestImagePointFinder.h"
11 #ifdef _MSC_VER
12 # include "vcl_msvc_warnings.h"
13 #endif
14
15 #include "vgl/vgl_box_2d.h"
16 #include "vgl/vgl_homg_point_2d.h"
17 #include "vnl/vnl_math.h"
18
19 #include <mvl/HomgInterestPointSet.h>
20
21 class vcl_multimap_double_int : public std::multimap<double, int, std::less <double> >
22 {
23 typedef std::multimap<double, int, std::less <double> > base;
24 public:
25 iterator insert(double key, int value);
26 void clear();
27 };
28
insert(double key,int value)29 vcl_multimap_double_int::iterator vcl_multimap_double_int::insert(double key, int value)
30 {
31 //std::cerr << " ins \t" << value << '\t' << key << '\n';
32 return base::insert(std::pair<const double, int>(key, value));
33 }
34
clear()35 void vcl_multimap_double_int::clear() { base::erase(begin(), end()); }
36
37
38 //: Initialize to allow fast lookups of corners in the given set.
ClosestImagePointFinder(const HomgInterestPointSet & corners)39 ClosestImagePointFinder::ClosestImagePointFinder(const HomgInterestPointSet& corners):
40 px_(corners.size()),
41 py_(corners.size())
42 {
43 // Make a map of image2 y value to corner2
44 y2i_ = new vcl_multimap_double_int;
45 for (unsigned i = 0; i < corners.size(); ++i) {
46 double x = corners.get_2d(i)[0];
47 double y = corners.get_2d(i)[1];
48 px_[i] = x;
49 py_[i] = y;
50 y2i_->insert(y, i);
51 }
52 }
53
54 //: Initialize to allow fast lookups of corners in the given set.
ClosestImagePointFinder(std::vector<vgl_homg_point_2d<double>> const & corners)55 ClosestImagePointFinder::ClosestImagePointFinder(std::vector<vgl_homg_point_2d<double> > const& corners)
56 : px_(corners.size()), py_(corners.size())
57 {
58 y2i_ = new vcl_multimap_double_int;
59 for (unsigned i = 0; i < corners.size(); ++i)
60 {
61 px_[i] = corners[i].x();
62 py_[i] = corners[i].y();
63 y2i_->insert(py_[i], i);
64 }
65 }
66
~ClosestImagePointFinder()67 ClosestImagePointFinder::~ClosestImagePointFinder()
68 {
69 delete y2i_;
70 }
71
get_all_within_search_region(double cx,double cy,double w,double h,std::vector<int> * out)72 void ClosestImagePointFinder::get_all_within_search_region(double cx, double cy, double w, double h, std::vector<int>* out)
73 {
74 get_all_within_search_region(vgl_box_2d<double>(cx - w, cx + w, cy - h, cy + h), out);
75 }
76
get_all_within_search_region(vgl_box_2d<double> const & disparity_bounds,std::vector<int> * out)77 void ClosestImagePointFinder::get_all_within_search_region(vgl_box_2d<double> const& disparity_bounds, std::vector<int>* out)
78 {
79 // Look at `point2's between y0 and y1
80 auto potential = y2i_->lower_bound(disparity_bounds.min_y());
81 auto end = y2i_->upper_bound(disparity_bounds.max_y() + 1);
82 #if 0
83 std::cerr << "map:";
84 for (vcl_multimap_double_int::iterator p = y2i_->begin(); p != y2i_->end(); ++p)
85 std::cerr << ' '<< (*p).second << '[' << (*p).first << ']';
86 std::cerr << '\n';
87 #endif // 0
88 out->erase(out->begin(), out->end());
89 for (; potential != end; ++potential) {
90 int point2_index = (*potential).second;
91 if (disparity_bounds.contains(px_[point2_index], py_[point2_index]))
92 out->push_back(point2_index);
93 }
94 }
95
96 //: Returns number that were within range.
get_closest_within_region(double cx,double cy,double w,double h,int * out,double mindist_sq)97 int ClosestImagePointFinder::get_closest_within_region(double cx, double cy, double w, double h, int* out, double mindist_sq)
98 {
99 // setup_checklist_disparity
100 vgl_box_2d<double> disparity_bounds(cx - w, cx + w, cy - h, cy + h);
101
102 // Look at `point2's between y0 and y1
103 auto potential = y2i_->lower_bound(disparity_bounds.min_y());
104 auto end = y2i_->upper_bound(disparity_bounds.max_y() + 1);
105
106 double orig_mindist_sq = mindist_sq;
107
108 last_index_ = -1;
109 int inrange = 0;
110 for (; potential != end; ++potential) {
111 int point2_index = (*potential).second;
112 double x = px_[point2_index];
113 double y = py_[point2_index];
114 if (disparity_bounds.contains(x, y)) {
115 double dx = x - cx;
116 double dy = y - cy;
117 double d2 = dx*dx + dy*dy;
118 if (d2 < orig_mindist_sq)
119 ++inrange;
120 if (d2 < mindist_sq) {
121 mindist_sq = d2;
122 last_index_ = point2_index;
123 }
124 }
125 }
126 last_d2_ = mindist_sq;
127 last_inrange_ = inrange;
128 if (out)
129 *out = last_index_;
130
131 return inrange;
132 }
133
get_closest_within_region(double cx,double cy,double w,double h,int * out)134 int ClosestImagePointFinder::get_closest_within_region(double cx, double cy, double w, double h, int* out)
135 {
136 double d = std::max(w,h);
137 return get_closest_within_region(cx, cy, w, h, out, d*d);
138 }
139
140 //: Returns number that were within range.
get_closest_within_distance(double cx,double cy,double r,int * out)141 int ClosestImagePointFinder::get_closest_within_distance(double cx, double cy, double r, int* out)
142 {
143 return get_closest_within_region(cx, cy, r*2, r*2, out, r*r);
144 }
145