1 #include "volm_geo_index.h"
2 #include "volm_io.h"
3 #include <iomanip>
4 #include <iostream>
5 #include <utility>
6 //:
7 // \file
8
9 #include <bkml/bkml_write.h>
10 #include "vpgl/vpgl_utm.h"
11 #include "vgl/vgl_intersection.h"
12 #include <volm/volm_loc_hyp.h>
13 #ifdef _MSC_VER
14 # include "vcl_msvc_warnings.h"
15 #endif
16 #include "vgl/vgl_area.h"
17
~volm_geo_index_node()18 volm_geo_index_node::~volm_geo_index_node()
19 {
20 children_.clear();
21 }
22
get_label_index_name(std::string const & geo_index_name_pre,std::string const & identifier)23 std::string volm_geo_index_node::get_label_index_name(std::string const& geo_index_name_pre, std::string const& identifier)
24 {
25 if (identifier.compare("") == 0)
26 return geo_index_name_pre + "_" + this->get_string() + "_index_label.bin";
27 else
28 return geo_index_name_pre + "_" + this->get_string() + "_index_label_"+identifier+".bin";
29 }
30
construct_sub_tree(volm_geo_index_node_sptr parent,float min_size)31 void construct_sub_tree(volm_geo_index_node_sptr parent, float min_size)
32 {
33 float w = parent->extent_.width();
34 float h = parent->extent_.height();
35 if (w <= min_size || h <= min_size)
36 return;
37 // create 4 children
38 vgl_point_2d<double> p1 = parent->extent_.min_point();
39 vgl_point_2d<double> p2(p1.x()+w/2, p1.y()+h/2);
40 vgl_box_2d<double> b1(p1, p2);
41 volm_geo_index_node_sptr c1 = new volm_geo_index_node(b1, parent);
42 construct_sub_tree(c1, min_size);
43
44 p1 = vgl_point_2d<double>(parent->extent_.min_point().x(), parent->extent_.min_point().y()+h/2);
45 p2 = vgl_point_2d<double>(parent->extent_.min_point().x()+w/2, parent->extent_.max_point().y());
46 vgl_box_2d<double> b2(p1, p2);
47 volm_geo_index_node_sptr c2 = new volm_geo_index_node(b2, parent);
48 construct_sub_tree(c2, min_size);
49
50 p1 = vgl_point_2d<double>(parent->extent_.min_point().x()+w/2, parent->extent_.min_point().y()+h/2);
51 p2 = vgl_point_2d<double>(parent->extent_.max_point().x(), parent->extent_.max_point().y());
52 vgl_box_2d<double> b3(p1, p2);
53 volm_geo_index_node_sptr c3 = new volm_geo_index_node(b3, parent);
54 construct_sub_tree(c3, min_size);
55
56 p1 = vgl_point_2d<double>(parent->extent_.min_point().x()+w/2, parent->extent_.min_point().y());
57 p2 = vgl_point_2d<double>(parent->extent_.max_point().x(), parent->extent_.min_point().y()+h/2);
58 vgl_box_2d<double> b4(p1, p2);
59 volm_geo_index_node_sptr c4 = new volm_geo_index_node(b4, parent);
60 construct_sub_tree(c4, min_size);
61
62 parent->children_.push_back(c1);
63 parent->children_.push_back(c2);
64 parent->children_.push_back(c3);
65 parent->children_.push_back(c4);
66 }
67
68 // construct a tree such that the given tile is the root, and the hierarchy of its children form a quadtree space partition
69 // the stopping criterion is when a child's box is less equal than min_size arcseconds
construct_tree(volm_tile t,float min_size)70 volm_geo_index_node_sptr volm_geo_index::construct_tree(volm_tile t, float min_size)
71 {
72 vgl_point_2d<double> p1(t.lower_left_lon(), t.lower_left_lat());
73 vgl_point_2d<double> p2(p1.x()+t.scale_i_, p1.y()+t.scale_j_);
74 vgl_box_2d<double> box(p1, p2);
75 std::cout << "box: " << box << std::endl;
76 volm_geo_index_node_sptr root = new volm_geo_index_node(box);
77 // recursively add children
78 construct_sub_tree(root, min_size);
79 return root;
80 }
81
82
construct_sub_tree_poly(volm_geo_index_node_sptr parent,float min_size,vgl_polygon<double> const & poly)83 void construct_sub_tree_poly(volm_geo_index_node_sptr parent, float min_size, vgl_polygon<double> const& poly)
84 {
85 float w = parent->extent_.width();
86 float h = parent->extent_.height();
87 if (w <= min_size || h <= min_size)
88 return;
89 // create 4 children
90 vgl_point_2d<double> p1(parent->extent_.min_point().x(), parent->extent_.min_point().y());
91 vgl_point_2d<double> p2(p1.x()+w/2, p1.y()+h/2);
92 vgl_box_2d<double> b1(p1, p2);
93 if (vgl_intersection(b1, poly)) {
94 volm_geo_index_node_sptr c1 = new volm_geo_index_node(b1, parent);
95 construct_sub_tree_poly(c1, min_size, poly);
96 parent->children_.push_back(c1);
97 }
98
99 p1 = vgl_point_2d<double>(parent->extent_.min_point().x(), parent->extent_.min_point().y()+h/2);
100 p2 = vgl_point_2d<double>(parent->extent_.min_point().x()+w/2, parent->extent_.max_point().y());
101 vgl_box_2d<double> b2(p1, p2);
102 if (vgl_intersection(b2, poly)) {
103 volm_geo_index_node_sptr c2 = new volm_geo_index_node(b2, parent);
104 construct_sub_tree_poly(c2, min_size, poly);
105 parent->children_.push_back(c2);
106 }
107
108 p1 = vgl_point_2d<double>(parent->extent_.min_point().x()+w/2, parent->extent_.min_point().y()+h/2);
109 p2 = vgl_point_2d<double>(parent->extent_.max_point().x(), parent->extent_.max_point().y());
110 vgl_box_2d<double> b3(p1, p2);
111 if (vgl_intersection(b3, poly)) {
112 volm_geo_index_node_sptr c3 = new volm_geo_index_node(b3, parent);
113 construct_sub_tree_poly(c3, min_size, poly);
114 parent->children_.push_back(c3);
115 }
116
117 p1 = vgl_point_2d<double>(parent->extent_.min_point().x()+w/2, parent->extent_.min_point().y());
118 p2 = vgl_point_2d<double>(parent->extent_.max_point().x(), parent->extent_.min_point().y()+h/2);
119 vgl_box_2d<double> b4(p1, p2);
120 if (vgl_intersection(b4, poly)) {
121 volm_geo_index_node_sptr c4 = new volm_geo_index_node(b4, parent);
122 construct_sub_tree_poly(c4, min_size, poly);
123 parent->children_.push_back(c4);
124 }
125 }
126
127 // construct with a polygon with possibly multiple sheets, only keep the children who intersect one of the sheets of the polygon
construct_tree(volm_tile t,float min_size,vgl_polygon<double> const & poly)128 volm_geo_index_node_sptr volm_geo_index::construct_tree(volm_tile t, float min_size, vgl_polygon<double> const& poly)
129 {
130 vgl_box_2d<float> box = t.bbox();
131 vgl_box_2d<double> box_d = t.bbox_double();
132 std::cout << "box: " << box << std::endl;
133 volm_geo_index_node_sptr root;
134 if (vgl_intersection(box_d, poly)) {
135 root = new volm_geo_index_node(box_d);
136 // recursively add children
137 construct_sub_tree_poly(root, min_size, poly);
138 }
139 return root;
140 }
141
142 //: prune the children which do not intersect the poly
prune_tree(const volm_geo_index_node_sptr & root,vgl_polygon<double> const & poly)143 bool volm_geo_index::prune_tree(const volm_geo_index_node_sptr& root, vgl_polygon<double> const& poly)
144 {
145 if (!vgl_intersection(root->extent_, poly))
146 return false;
147
148 for (auto & i : root->children_) {
149 if (!i)
150 continue;
151 if (!prune_tree(i, poly)) // the child does not intersect poly
152 i = nullptr; // sptr deallocates this child
153 }
154
155 return true;
156 }
157
prune_tree(const volm_geo_index_node_sptr & root,vgl_polygon<float> const & poly)158 bool volm_geo_index::prune_tree(const volm_geo_index_node_sptr& root, vgl_polygon<float> const& poly)
159 {
160 vgl_polygon<double> poly2;
161 volm_io::convert_polygons(poly, poly2);
162 return volm_geo_index::prune_tree(root, poly2);
163 }
164
165 //: prune nodes whose bbox is not in the utm_zone
prune_by_zone(const volm_geo_index_node_sptr & root,unsigned utm_zone)166 bool volm_geo_index::prune_by_zone(const volm_geo_index_node_sptr& root, unsigned utm_zone)
167 {
168 vpgl_utm u; int zone1, zone2; double x, y;
169 u.transform(root->extent_.min_point().y(), root->extent_.min_point().x(), x, y, zone1);
170 u.transform(root->extent_.max_point().y(), root->extent_.max_point().x(), x, y, zone2);
171 if (zone1 != (int)utm_zone && zone2 != (int)utm_zone)
172 return false;
173
174 for (auto & i : root->children_) {
175 if (!i)
176 continue;
177 if (!prune_by_zone(i, utm_zone)) // the child is not in zone
178 i = nullptr; // sptr deallocates this child
179 }
180 return true;
181 }
182
depth(const volm_geo_index_node_sptr & node)183 unsigned volm_geo_index::depth(const volm_geo_index_node_sptr& node)
184 {
185 if (!node->children_.size())
186 return 0;
187 unsigned d = 0;
188 for (auto & i : node->children_) {
189 if (!i)
190 continue;
191 unsigned dd = depth(i);
192 if (dd > d)
193 d = dd;
194 }
195 return d+1;
196 }
197
198 //: kml requires first lat, then lon
write_to_kml_node(std::ofstream & ofs,const volm_geo_index_node_sptr & n,unsigned current_depth,unsigned depth,std::string explanation)199 void volm_geo_index::write_to_kml_node(std::ofstream& ofs, const volm_geo_index_node_sptr& n, unsigned current_depth, unsigned depth, std::string explanation)
200 {
201 if (!n)
202 return;
203 if (current_depth == depth) {
204 vnl_double_2 ul, ll, lr, ur;
205 // lat is y, lon is x
206 ll[0] = n->extent_.min_point().y(); ll[1] = n->extent_.min_point().x();
207 ul[0] = n->extent_.max_point().y(); ul[1] = n->extent_.min_point().x();
208 lr[0] = n->extent_.min_point().y(); lr[1] = n->extent_.max_point().x();
209 ur[0] = n->extent_.max_point().y(); ur[1] = n->extent_.max_point().x();
210 bkml_write::write_box(ofs, " ", std::move(explanation), ul, ur,ll,lr);
211 }
212 else {
213 for (const auto & i : n->children_)
214 write_to_kml_node(ofs, i, current_depth + 1, depth);
215 }
216 }
217
218 //: write the bboxes of the nodes at the given depth to kml file
write_to_kml(const volm_geo_index_node_sptr & root,unsigned depth,std::string const & file_name)219 void volm_geo_index::write_to_kml(const volm_geo_index_node_sptr& root, unsigned depth, std::string const& file_name)
220 {
221 std::ofstream ofs(file_name.c_str());
222 bkml_write::open_document(ofs);
223 write_to_kml_node(ofs, root, 0, depth);
224 bkml_write::close_document(ofs);
225 }
226
write_to_text(std::ofstream & ofs,const volm_geo_index_node_sptr & n)227 void write_to_text(std::ofstream& ofs, const volm_geo_index_node_sptr& n)
228 {
229 ofs << std::setprecision(6) << std::fixed << n->extent_.min_point().x() << ' '
230 << std::setprecision(6) << std::fixed << n->extent_.min_point().y() << ' '
231 << std::setprecision(6) << std::fixed << n->extent_.max_point().x() << ' '
232 << std::setprecision(6) << std::fixed << n->extent_.max_point().y() << '\n'
233 << n->children_.size() << '\n';
234 for (auto & i : n->children_) {
235 if (!i) ofs << " 0";
236 else ofs << " 1";
237 }
238 ofs << '\n';
239 for (auto & i : n->children_) {
240 if (i)
241 write_to_text(ofs, i);
242 }
243 }
244
write(const volm_geo_index_node_sptr & root,std::string const & file_name,float min_size)245 void volm_geo_index::write(const volm_geo_index_node_sptr& root, std::string const& file_name, float min_size)
246 {
247 std::ofstream ofs(file_name.c_str());
248 ofs << min_size << '\n';
249 write_to_text(ofs, root);
250 }
251
read_and_construct_node(std::ifstream & ifs,volm_geo_index_node_sptr parent)252 volm_geo_index_node_sptr read_and_construct_node(std::ifstream& ifs, volm_geo_index_node_sptr parent)
253 {
254 float x,y;
255 ifs >> x; ifs >> y; vgl_point_2d<double> min_pt(x,y);
256 ifs >> x; ifs >> y; vgl_point_2d<double> max_pt(x,y);
257 vgl_box_2d<double> b(min_pt, max_pt);
258 volm_geo_index_node_sptr n = new volm_geo_index_node(b, parent);
259 unsigned nc;
260 ifs >> nc;
261 std::vector<unsigned> existence(nc);
262 for (unsigned i = 0; i < nc; i++)
263 ifs >> existence[i];
264 for (unsigned i = 0; i < nc; i++) {
265 volm_geo_index_node_sptr child;
266 if (existence[i])
267 child = read_and_construct_node(ifs, n);
268 n->children_.push_back(child);
269 }
270 return n;
271 }
272
273 // even if a child has zero pointer, it's order in the children_ array is the same, this is to make sure that the children have consistent geographic meaning
read_and_construct(std::string const & file_name,float & min_size)274 volm_geo_index_node_sptr volm_geo_index::read_and_construct(std::string const& file_name, float& min_size)
275 {
276 std::ifstream ifs(file_name.c_str());
277 ifs >> min_size;
278 volm_geo_index_node_sptr dummy_parent;
279 volm_geo_index_node_sptr root = read_and_construct_node(ifs, dummy_parent);
280 return root;
281 }
282
get_leaves(const volm_geo_index_node_sptr & root,std::vector<volm_geo_index_node_sptr> & leaves)283 void volm_geo_index::get_leaves(const volm_geo_index_node_sptr& root, std::vector<volm_geo_index_node_sptr>& leaves)
284 {
285 if (!root)
286 return;
287 if (!root->children_.size())
288 leaves.push_back(root);
289 else {
290 bool at_least_one_child = false;
291 for (auto & i : root->children_) {
292 if (!i)
293 continue;
294 else {
295 get_leaves(i, leaves);
296 at_least_one_child = true;
297 }
298 }
299 if (!at_least_one_child)
300 leaves.push_back(root);
301 }
302 }
303
304 //: return all the leaves that intersect a given rectangular area
get_leaves(const volm_geo_index_node_sptr & root,std::vector<volm_geo_index_node_sptr> & leaves,vgl_box_2d<double> & area)305 void volm_geo_index::get_leaves(const volm_geo_index_node_sptr& root, std::vector<volm_geo_index_node_sptr>& leaves, vgl_box_2d<double>& area)
306 {
307 if (!root)
308 return;
309
310 if (vgl_area(vgl_intersection(root->extent_, area)) == 0.0f)
311 return;
312
313 if (!root->children_.size())
314 leaves.push_back(root);
315 else {
316 bool at_least_one_child = false;
317 for (auto & i : root->children_) {
318 if (!i)
319 continue;
320 else {
321 get_leaves(i, leaves, area);
322 at_least_one_child = true;
323 }
324 }
325 if (!at_least_one_child)
326 leaves.push_back(root);
327 }
328 }
329
get_string()330 std::string volm_geo_index_node::get_string()
331 {
332 std::stringstream str;
333 str << "node_"
334 << std::setprecision(6) << std::fixed << this->extent_.min_point().x() << '_'
335 << std::setprecision(6) << std::fixed << this->extent_.min_point().y() << '_'
336 << std::setprecision(6) << std::fixed << this->extent_.max_point().x() << '_'
337 << std::setprecision(6) << std::fixed << this->extent_.max_point().y();
338 return str.str();
339 }
340
write_hyps(const volm_geo_index_node_sptr & root,std::string const & file_name_pre)341 void volm_geo_index::write_hyps(const volm_geo_index_node_sptr& root, std::string const& file_name_pre)
342 {
343 if (!root)
344 return;
345 if (root->hyps_ && root->hyps_->size() > 0) {
346 std::string str = root->get_hyp_name(file_name_pre);
347 root->hyps_->write_hypotheses(str);
348 }
349 for (const auto & i : root->children_)
350 write_hyps(i, file_name_pre);
351 }
352
read_hyps(const volm_geo_index_node_sptr & root,std::string const & file_name_pre)353 void volm_geo_index::read_hyps(const volm_geo_index_node_sptr& root, std::string const& file_name_pre)
354 {
355 if (!root)
356 return;
357 std::string str = file_name_pre + "_" + root->get_string() + ".bin";
358 if (vul_file::exists(str)) {
359 volm_loc_hyp_sptr hyp = new volm_loc_hyp(str);
360 root->hyps_ = hyp;
361 }
362 for (const auto & i : root->children_)
363 read_hyps(i, file_name_pre);
364 }
365
hypo_size(const volm_geo_index_node_sptr & root)366 unsigned volm_geo_index::hypo_size(const volm_geo_index_node_sptr& root)
367 {
368 if (!root)
369 return 0;
370 unsigned cnt = 0;
371 if (root->hyps_)
372 cnt = root->hyps_->size();
373 for (const auto & i : root->children_)
374 cnt += hypo_size(i);
375 return cnt;
376 }
377
add_hypothesis(const volm_geo_index_node_sptr & root,double lon,double lat,double elev)378 bool volm_geo_index::add_hypothesis(const volm_geo_index_node_sptr& root, double lon, double lat, double elev)
379 {
380 if (!root)
381 return false;
382
383 if (root->hyps_) {
384 if (root->extent_.contains(lon, lat)) {
385 root->hyps_->add(lat, lon, elev);
386 return true;
387 }
388 } else if (!root->children_.size()) { // it's a leaf
389 if (root->extent_.contains(lon, lat)) {
390 root->hyps_ = new volm_loc_hyp;
391 root->hyps_->add(lat, lon, elev);
392 return true;
393 }
394 }
395
396 bool added = false;
397 for (unsigned i = 0; i < root->children_.size(); i++) {
398 if (root->children_[i] && root->children_[i]->extent_.contains(lon, lat))
399 added = add_hypothesis(root->children_[i], lon, lat, elev);
400 }
401 return added;
402 }
403
get_leaves_with_hyps(const volm_geo_index_node_sptr & root,std::vector<volm_geo_index_node_sptr> & leaves)404 void volm_geo_index::get_leaves_with_hyps(const volm_geo_index_node_sptr& root, std::vector<volm_geo_index_node_sptr>& leaves)
405 {
406 if (!root)
407 return;
408 if (!root->children_.size() && root->hyps_ && root->hyps_->size() > 0) {
409 leaves.push_back(root);
410 return;
411 }
412 bool at_least_one_child = false;
413 for (auto & i : root->children_) {
414 if (!i)
415 continue;
416 else {
417 get_leaves_with_hyps(i, leaves);
418 at_least_one_child = true;
419 }
420 }
421 if (!at_least_one_child && root->hyps_ && root->hyps_->size() > 0)
422 leaves.push_back(root);
423 }
424
425 //: return the leaf and the hyp id of the closest hyp to the given location
get_closest(const volm_geo_index_node_sptr & root,double lat,double lon,unsigned & hyp_id)426 volm_geo_index_node_sptr volm_geo_index::get_closest(const volm_geo_index_node_sptr& root, double lat, double lon, unsigned& hyp_id)
427 {
428 if (!root)
429 return nullptr;
430
431 vgl_point_3d<double> h;
432 if (root->hyps_) {
433 root->hyps_->get_closest(lat, lon, h, hyp_id);
434 return root;
435 }
436 if (!root->children_.size())
437 return nullptr;
438 for (unsigned i = 0; i < root->children_.size(); i++) {
439 if (!root->children_[i])
440 continue;
441 if (root->children_[i]->extent_.contains(lon, lat)) {
442 volm_geo_index_node_sptr c = get_closest(root->children_[i], lat, lon, hyp_id);
443 if (c)
444 return c;
445 }
446 }
447 return nullptr;
448 }
449
450 //: return true if a hyp exists within the given radius to the given point
exists(const volm_geo_index_node_sptr & root,double lat,double lon,double inc_in_sec_rad)451 bool volm_geo_index::exists(const volm_geo_index_node_sptr& root, double lat, double lon, double inc_in_sec_rad)
452 {
453 if (!root)
454 return false;
455 if (root->hyps_) {
456 unsigned id;
457 if (root->hyps_->exist(lat, lon, inc_in_sec_rad, id))
458 return true;
459 }
460 if (!root->children_.size())
461 return false;
462 for (unsigned i = 0; i < root->children_.size(); i++) {
463 if (!root->children_[i])
464 continue;
465 if (root->children_[i]->extent_.contains(lon, lat)) {
466 if (exists(root->children_[i], lat, lon, inc_in_sec_rad))
467 return true;
468 }
469 }
470
471 return false;
472 }
473