1 // This is brl/bbas/bvgl/algo/bvgl_2d_geo_index.cxx
2 #include "bvgl_2d_geo_index.h"
3 //:
4 // \file
5 #include <bkml/bkml_write.h>
6 #include "vgl/vgl_intersection.h"
7 #include "vpgl/vpgl_utm.h"
8 #include "vgl/vgl_line_segment_2d.h"
9 #include "vgl/vgl_area.h"
10
11 #include <utility>
12
13 // function to check whether the given box intersect with a line defined by a vector of points
is_intersect(vgl_box_2d<double> const & box,std::vector<vgl_point_2d<double>> const & line)14 static bool is_intersect(vgl_box_2d<double> const& box, std::vector<vgl_point_2d<double> > const& line)
15 {
16 // Is the loop that follows correct?
17 unsigned n_line_segs = line.size()-1;
18 for (unsigned i = 0; i < n_line_segs; i++) {
19 vgl_point_2d<double> s = line[i]; vgl_point_2d<double> e = line[i+1];
20 vgl_line_segment_2d<double> line_segment(s,e);
21 vgl_point_2d<double> p0, p1;
22 if (vgl_intersection(box, line_segment, p0, p1) != 0)
23 return true;
24 if (box.contains(s) || box.contains(e))
25 return true;
26 }
27 return false;
28 }
29
30 // convert a polygon from float to double
convert_polygon(vgl_polygon<float> const & in_poly,vgl_polygon<double> & out_poly)31 bool bvgl_2d_geo_index::convert_polygon(vgl_polygon<float> const& in_poly, vgl_polygon<double>& out_poly)
32 {
33 out_poly.clear();
34 for (unsigned i = 0; i < in_poly.num_sheets(); i++)
35 out_poly.new_sheet();
36 for (unsigned i = 0; i < in_poly.num_sheets(); i++)
37 for (unsigned j = 0; j < (unsigned)in_poly[i].size(); j++)
38 out_poly[i].push_back(vgl_point_2d<double>((double)in_poly[i][j].x(), (double)in_poly[i][j].y()));
39 return true;
40 }
41
42 // convert a polygon from double to float
convert_polygon(vgl_polygon<double> const & in_poly,vgl_polygon<float> & out_poly)43 bool bvgl_2d_geo_index::convert_polygon(vgl_polygon<double> const& in_poly, vgl_polygon<float>& out_poly)
44 {
45 out_poly.clear();
46 for (unsigned i = 0; i < in_poly.num_sheets(); i++)
47 out_poly.new_sheet();
48 for (unsigned i = 0; i < in_poly.num_sheets(); i++)
49 for (unsigned j = 0; j < (unsigned)in_poly[i].size(); j++)
50 out_poly[i].push_back(vgl_point_2d<float>((float)in_poly[i][j].x(), (float)in_poly[i][j].y()));
51 return true;
52 }
53
54 // convert a 2d box from float to double
convert_box(vgl_box_2d<float> const & in,vgl_box_2d<double> & out)55 bool bvgl_2d_geo_index::convert_box(vgl_box_2d<float> const& in, vgl_box_2d<double>& out)
56 {
57 out.empty();
58 out.set_min_x((double)(in.min_x())); out.set_max_x((double)(in.max_x()));
59 out.set_min_y((double)(in.min_y())); out.set_max_y((double)(in.max_y()));
60 return true;
61 }
62
63 // convert a 2d box from double to float
convert_box(vgl_box_2d<double> const & in,vgl_box_2d<float> & out)64 bool bvgl_2d_geo_index::convert_box(vgl_box_2d<double> const& in, vgl_box_2d<float>& out)
65 {
66 out.empty();
67 out.set_min_x((float)(in.min_x())); out.set_max_x((float)(in.max_x()));
68 out.set_min_y((float)(in.min_y())); out.set_max_y((float)(in.max_y()));
69 return true;
70 }
71
72 // write node and its children to kml
write_to_kml_node(std::ofstream & ofs,const bvgl_2d_geo_index_node_sptr & n,unsigned const & current_depth,unsigned const & depth,const std::string & explanation,const std::string & name)73 void bvgl_2d_geo_index::write_to_kml_node(std::ofstream& ofs, const bvgl_2d_geo_index_node_sptr& n, unsigned const& current_depth, unsigned const& depth, const std::string& explanation, const std::string& name)
74 {
75 if (!n)
76 return;
77 if (current_depth == depth) {
78 // note that in the extent bounding box, x -- lon, y -- lat (kml format: lat lon)
79 vnl_double_2 ul, ll, lr, ur;
80 ll[0] = n->extent_.min_point().y(); ll[1] = n->extent_.min_point().x();
81 ul[0] = n->extent_.max_point().y(); ul[1] = n->extent_.min_point().x();
82 lr[0] = n->extent_.min_point().y(); lr[1] = n->extent_.max_point().x();
83 ur[0] = n->extent_.max_point().y(); ur[1] = n->extent_.max_point().x();
84 bkml_write::write_box(ofs, name, explanation, ul, ur, ll, lr);
85 }
86 else {
87 for (const auto & c_idx : n->children_)
88 write_to_kml_node(ofs, c_idx, current_depth+1, depth, explanation, name);
89 }
90 }
91
92 // write the quadtree node structure at certain depth
write_to_kml(const bvgl_2d_geo_index_node_sptr & root,unsigned const & depth,std::string const & kml_file,std::string explanation,std::string name)93 void bvgl_2d_geo_index::write_to_kml(const bvgl_2d_geo_index_node_sptr& root, unsigned const& depth, std::string const& kml_file, std::string explanation, std::string name)
94 {
95 std::ofstream ofs(kml_file.c_str());
96 bkml_write::open_document(ofs);
97 write_to_kml_node(ofs, root, 0, depth, std::move(explanation), std::move(name));
98 bkml_write::close_document(ofs);
99 }
100
101 // write node and its children to kml for quadtree having non geo coordinates
write_to_kml_node(std::ofstream & ofs,const bvgl_2d_geo_index_node_sptr & n,unsigned const & current_depth,unsigned const & depth,vpgl_lvcs_sptr const & lvcs,const std::string & explanation,const std::string & name)102 void bvgl_2d_geo_index::write_to_kml_node(std::ofstream& ofs, const bvgl_2d_geo_index_node_sptr& n, unsigned const& current_depth, unsigned const& depth, vpgl_lvcs_sptr const& lvcs, const std::string& explanation, const std::string& name)
103 {
104 if (!n)
105 return;
106 if (current_depth == depth) {
107 double min_lon, min_lat, max_lon, max_lat, gz;
108 lvcs->local_to_global(n->extent_.min_point().x(), n->extent_.min_point().y(), 0.0, vpgl_lvcs::wgs84, min_lon, min_lat, gz);
109 lvcs->local_to_global(n->extent_.max_point().x(), n->extent_.max_point().y(), 0.0, vpgl_lvcs::wgs84, max_lon, max_lat, gz);
110 vnl_double_2 ul, ll, lr, ur;
111 ll[0] = min_lat; ll[1] = min_lon;
112 ul[0] = max_lat; ul[1] = min_lon;
113 lr[0] = min_lat; lr[1] = max_lon;
114 ur[0] = max_lat; ur[1] = max_lon;
115 bkml_write::write_box(ofs, name, explanation, ul, ur, ll, lr);
116 }
117 else {
118 for (const auto & c_idx : n->children_)
119 write_to_kml_node(ofs, c_idx, current_depth+1, depth, lvcs, explanation, name);
120 }
121 }
122
write_to_kml(const bvgl_2d_geo_index_node_sptr & root,unsigned const & depth,std::string const & kml_file,vpgl_lvcs_sptr const & lvcs,std::string explanation,std::string name)123 void bvgl_2d_geo_index::write_to_kml(const bvgl_2d_geo_index_node_sptr& root, unsigned const& depth, std::string const& kml_file, vpgl_lvcs_sptr const& lvcs, std::string explanation, std::string name)
124 {
125 std::ofstream ofs(kml_file.c_str());
126 bkml_write::open_document(ofs);
127 write_to_kml_node(ofs, root, 0, depth, lvcs, std::move(explanation), std::move(name));
128 bkml_write::close_document(ofs);
129 }
130
131 // return the depth of the tree
depth(const bvgl_2d_geo_index_node_sptr & node)132 unsigned bvgl_2d_geo_index::depth(const bvgl_2d_geo_index_node_sptr& node)
133 {
134 if (node->children_.empty()) // already at leaf level
135 return 0;
136 unsigned d = 0;
137 for (auto & i : node->children_) {
138 if (!i)
139 continue;
140 unsigned dd = depth(i);
141 if (dd > d)
142 d = dd;
143 }
144 return d+1;
145 }
146
147 // write the tree structure
write_to_text(std::ofstream & ofs,const bvgl_2d_geo_index_node_sptr & n)148 void write_to_text(std::ofstream& ofs, const bvgl_2d_geo_index_node_sptr& n)
149 {
150 ofs << std::setprecision(8) << std::fixed << n->extent_.min_point().x() << ' '
151 << std::setprecision(8) << std::fixed << n->extent_.min_point().y() << ' '
152 << std::setprecision(8) << std::fixed << n->extent_.max_point().x() << ' '
153 << std::setprecision(8) << std::fixed << n->extent_.max_point().y() << '\n'
154 << n->children_.size() << '\n';
155 for (auto & i : n->children_) {
156 if (!i) ofs << " 0";
157 else ofs << " 1";
158 }
159 ofs << '\n';
160 for (auto & i : n->children_) {
161 if (i)
162 write_to_text(ofs, i);
163 }
164 }
165
write(const bvgl_2d_geo_index_node_sptr & root,std::string const & file_name,double const & min_size)166 void bvgl_2d_geo_index::write(const bvgl_2d_geo_index_node_sptr& root, std::string const& file_name, double const& min_size)
167 {
168 std::ofstream ofs(file_name.c_str());
169 ofs << min_size << '\n';
170 write_to_text(ofs, root);
171 }
172
173 // write the tree structure using lvcs
write_to_text(std::ofstream & ofs,const bvgl_2d_geo_index_node_sptr & n,vpgl_lvcs_sptr const & lvcs)174 void write_to_text(std::ofstream& ofs, const bvgl_2d_geo_index_node_sptr& n, vpgl_lvcs_sptr const& lvcs)
175 {
176 // transfer the extents
177 double min_lon, min_lat, max_lon, max_lat, gz;
178 lvcs->local_to_global(n->extent_.min_point().x(), n->extent_.min_point().y(), 0.0, vpgl_lvcs::wgs84, min_lon, min_lat, gz);
179 lvcs->local_to_global(n->extent_.max_point().x(), n->extent_.max_point().y(), 0.0, vpgl_lvcs::wgs84, max_lon, max_lat, gz);
180 ofs << std::setprecision(6) << std::fixed << min_lon << ' '
181 << std::setprecision(6) << std::fixed << min_lat << ' '
182 << std::setprecision(6) << std::fixed << max_lon << ' '
183 << std::setprecision(6) << std::fixed << max_lat << ' '
184 << n->children_.size() << '\n';
185 for (auto & i : n->children_) {
186 if (!i) ofs << " 0";
187 else ofs << " 1";
188 }
189 ofs << '\n';
190 for (auto & i : n->children_) {
191 if (i)
192 write_to_text(ofs, i, lvcs);
193 }
194 }
195
write(const bvgl_2d_geo_index_node_sptr & root,std::string const & file_name,double const & min_size,vpgl_lvcs_sptr const & lvcs)196 void bvgl_2d_geo_index::write(const bvgl_2d_geo_index_node_sptr& root, std::string const& file_name, double const& min_size, vpgl_lvcs_sptr const& lvcs)
197 {
198 std::ofstream ofs(file_name.c_str());
199 double min_size_x, min_size_y, gz;
200 lvcs->local_to_global(min_size, min_size, 0.0, vpgl_lvcs::wgs84, min_size_x, min_size_y, gz);
201 ofs << min_size_x << '\n';
202 write_to_text(ofs, root, lvcs);
203 }
204
205 // prune the tree leaves by given polygon
prune_tree(const bvgl_2d_geo_index_node_sptr & root,vgl_polygon<double> const & poly)206 bool bvgl_2d_geo_index::prune_tree(const bvgl_2d_geo_index_node_sptr& root, vgl_polygon<double> const& poly)
207 {
208 // note that the tree will not be pruned if the root bounding box does not intersect with the polygon
209 if (!vgl_intersection(root->extent_, poly))
210 return false;
211
212 for (auto & i : root->children_) {
213 if (!i)
214 continue;
215 if (!prune_tree(i, poly)) // the child does not intersect with the polygon
216 i = nullptr; // sptr de-allocates this child
217 }
218 return true;
219 }
220
prune_tree(const bvgl_2d_geo_index_node_sptr & root,vgl_polygon<float> const & poly)221 bool bvgl_2d_geo_index::prune_tree(const bvgl_2d_geo_index_node_sptr& root, vgl_polygon<float> const& poly)
222 {
223 vgl_polygon<double> poly_double;
224 bvgl_2d_geo_index::convert_polygon(poly, poly_double);
225 return bvgl_2d_geo_index::prune_tree(root, poly_double);
226 }
227
228 // return all the leaves
get_leaves(const bvgl_2d_geo_index_node_sptr & root,std::vector<bvgl_2d_geo_index_node_sptr> & leaves)229 void bvgl_2d_geo_index::get_leaves(const bvgl_2d_geo_index_node_sptr& root, std::vector<bvgl_2d_geo_index_node_sptr>& leaves)
230 {
231 if (!root)
232 return;
233 if (!root->children_.size())
234 leaves.push_back(root);
235 else {
236 bool at_least_one_child = false;
237 for (auto & i : root->children_) {
238 if (!i)
239 continue;
240 else {
241 get_leaves(i, leaves);
242 at_least_one_child = true;
243 }
244 }
245 if (!at_least_one_child)
246 leaves.push_back(root);
247 }
248 }
249
250 // return the leaves that intersect with given rectangular region
get_leaves(const bvgl_2d_geo_index_node_sptr & root,std::vector<bvgl_2d_geo_index_node_sptr> & leaves,vgl_box_2d<double> const & area)251 void bvgl_2d_geo_index::get_leaves(const bvgl_2d_geo_index_node_sptr& root, std::vector<bvgl_2d_geo_index_node_sptr>& leaves, vgl_box_2d<double> const& area)
252 {
253 if (!root) // empty tree
254 return;
255 if (vgl_area(vgl_intersection(root->extent_, area)) == 0.0f) // tree doesn't intersect with given region
256 return;
257
258 if (!root->children_.size()) // the node has no children and it intersects with box
259 leaves.push_back(root);
260 else {
261 bool at_least_one_child = false;
262 for (auto & i : root->children_) {
263 if (!i)
264 continue;
265 else {
266 get_leaves(i, leaves, area);
267 at_least_one_child = true;
268 }
269 }
270 if (!at_least_one_child) // the node has children but all children are empty
271 leaves.push_back(root);
272 }
273 }
274
get_leaves(const bvgl_2d_geo_index_node_sptr & root,std::vector<bvgl_2d_geo_index_node_sptr> & leaves,vgl_box_2d<float> const & area)275 void bvgl_2d_geo_index::get_leaves(const bvgl_2d_geo_index_node_sptr& root, std::vector<bvgl_2d_geo_index_node_sptr>& leaves, vgl_box_2d<float> const& area)
276 {
277 vgl_box_2d<double> box_double;
278 bvgl_2d_geo_index::convert_box(area, box_double);
279 get_leaves(root, leaves, box_double);
280 }
281
282 // return all leaves that intersect with polygon
get_leaves(const bvgl_2d_geo_index_node_sptr & root,std::vector<bvgl_2d_geo_index_node_sptr> & leaves,vgl_polygon<double> const & poly)283 void bvgl_2d_geo_index::get_leaves(const bvgl_2d_geo_index_node_sptr& root, std::vector<bvgl_2d_geo_index_node_sptr>& leaves, vgl_polygon<double> const& poly)
284 {
285 if (!root) // the node is empty
286 return;
287 // check whether the polygon intersects with current root
288 if (!vgl_intersection(root->extent_, poly)) // the node does not intersect with given polygon
289 return;
290 // the node intersects with the polygon
291 if (!root->children_.size()) { // the node intersects with the polygon and has no child
292 leaves.push_back(root);
293 }
294 else { // the node has children, go inside to its children
295 bool at_least_one_child = false;
296 for (auto & i : root->children_) {
297 if (!i) // the node has children but child i is empty
298 continue;
299 else {
300 get_leaves(i, leaves, poly); // check the intersection of child i and its following children with poly
301 at_least_one_child = true;
302 }
303 }
304 if (!at_least_one_child) // the node has children but all children are empty
305 leaves.push_back(root);
306 }
307 }
308
get_leaves(const bvgl_2d_geo_index_node_sptr & root,std::vector<bvgl_2d_geo_index_node_sptr> & leaves,vgl_polygon<float> const & poly)309 void bvgl_2d_geo_index::get_leaves(const bvgl_2d_geo_index_node_sptr& root, std::vector<bvgl_2d_geo_index_node_sptr>& leaves, vgl_polygon<float> const& poly)
310 {
311 vgl_polygon<double> poly_double;
312 bvgl_2d_geo_index::convert_polygon(poly, poly_double);
313 get_leaves(root, leaves, poly_double);
314 }
315
get_leaves(const bvgl_2d_geo_index_node_sptr & root,std::vector<bvgl_2d_geo_index_node_sptr> & leaves,std::vector<vgl_point_2d<double>> const & line)316 void bvgl_2d_geo_index::get_leaves(const bvgl_2d_geo_index_node_sptr& root, std::vector<bvgl_2d_geo_index_node_sptr>& leaves, std::vector<vgl_point_2d<double> > const& line)
317 {
318 if (!root) // the tree is empty
319 return;
320 if (!is_intersect(root->extent_, line)) // the root does not intersect with the line
321 return;
322 // the node intersects with the line
323 if (!root->children_.size()) { // the node intersects with the line and has no child
324 leaves.push_back(root);
325 }
326 else { // the node has children and check the intersection recursively
327 bool at_least_one_child = false;
328 for (auto & i : root->children_) {
329 if (!i) // the node has children but child i is empty
330 continue;
331 else {
332 get_leaves(i, leaves, line); // check the intersection of child i and its following children
333 at_least_one_child = true;
334 }
335 }
336 if (!at_least_one_child) // the node has children but all children are empty
337 leaves.push_back(root);
338 }
339 }
340
get_leaves(const bvgl_2d_geo_index_node_sptr & root,std::vector<bvgl_2d_geo_index_node_sptr> & leaves,std::vector<vgl_point_2d<float>> const & line)341 void bvgl_2d_geo_index::get_leaves(const bvgl_2d_geo_index_node_sptr& root, std::vector<bvgl_2d_geo_index_node_sptr>& leaves, std::vector<vgl_point_2d<float> > const& line)
342 {
343 std::vector<vgl_point_2d<double> > line_double;
344 auto num_pts = (unsigned)line.size();
345 for (unsigned i = 0; i < num_pts; i++)
346 line_double.emplace_back((double)line[i].x(), (double)line[i].y());
347 get_leaves(root, leaves, line_double);
348 }
349
get_leaf(const bvgl_2d_geo_index_node_sptr & root,bvgl_2d_geo_index_node_sptr & leaf,vgl_point_2d<double> const & point)350 void bvgl_2d_geo_index::get_leaf(const bvgl_2d_geo_index_node_sptr& root, bvgl_2d_geo_index_node_sptr& leaf, vgl_point_2d<double> const& point)
351 {
352 if (!root)
353 return;
354 if (!root->extent_.contains(point))
355 return;
356 if (!root->children_.size()) {
357 leaf = root;
358 }
359 else {
360 bool at_least_one_child = false;
361 for (auto & i : root->children_) {
362 if (!i)
363 continue;
364 else {
365 get_leaf(i, leaf, point);
366 at_least_one_child = true;
367 }
368 }
369 if (!at_least_one_child)
370 leaf = root;
371 }
372 }
373
get_leaf(const bvgl_2d_geo_index_node_sptr & root,bvgl_2d_geo_index_node_sptr & leaf,vgl_point_2d<float> const & point)374 void bvgl_2d_geo_index::get_leaf(const bvgl_2d_geo_index_node_sptr& root, bvgl_2d_geo_index_node_sptr& leaf, vgl_point_2d<float> const& point)
375 {
376 vgl_point_2d<double> pt_double( (double)(point.x()), (double)(point.y()) );
377 get_leaf(root, leaf, pt_double);
378 }
379