1 // This is//external/acal/acal_match_tree.h
2 #ifndef acal_match_tree_h
3 #define acal_match_tree_h
4 
5 //:
6 // \file
7 // \brief A tree of match tracks with nodes as cameras. Tree edges are correspondence pairs
8 // \author J.L. Mundy
9 // \date January 14, 2019
10 //
11 // \verbatim
12 //  Modifications
13 //   <none yet>
14 // \endverbatim
15 
16 #include <iostream>
17 #include <fstream>
18 #include <string>
19 #include <vector>
20 #include <memory>
21 #include <map>
22 #include <math.h>
23 
24 #include <vgl/vgl_point_2d.h>
25 #include <vgl/vgl_vector_2d.h>
26 #include "acal_match_utils.h"
27 
28 
29 class acal_match_node : public std::enable_shared_from_this<acal_match_node>
30 {
31  public:
32 
33   //: constructor
cam_id_(node_id)34   acal_match_node(size_t node_id = 0) : cam_id_(node_id) {}
35 
36   //: property accessors
size()37   size_t size() const { return children_.size(); }
is_leaf()38   bool is_leaf() const { return children_.empty(); }
is_root()39   bool is_root() const { return !has_parent_; }
has_parent()40   bool has_parent() const { return has_parent_; }
41 
42   //: parent accessors
parent()43   std::shared_ptr<acal_match_node> parent() const { return parent_.lock(); }
parent(std::shared_ptr<acal_match_node> node)44   void parent(std::shared_ptr<acal_match_node> node) {
45     has_parent_ = true;
46     parent_ = node;
47   }
48 
49   //: create & add a child node to "this" node
add_child(size_t child_id,std::vector<acal_match_pair> const & self_to_child_matches)50   void add_child(size_t child_id, std::vector<acal_match_pair> const& self_to_child_matches) {
51     auto child = std::make_shared<acal_match_node>(child_id);
52     child->parent(shared_from_this());
53     children_.push_back(child);
54     self_to_child_matches_.push_back(self_to_child_matches);
55   }
56 
57   //: parent & children node id
58   size_t parent_id() const;
59   std::vector<size_t> children_ids() const;
60 
61   //: equality operators
62   bool operator==(acal_match_node const& other) const;
63   bool operator!=(acal_match_node const& other) const { return !(*this == other); }
64 
65    //: find the index (cindx) of a node in the set of parent's children
child_index(std::shared_ptr<acal_match_node> const & node,size_t & cidx)66   bool child_index(std::shared_ptr<acal_match_node> const& node, size_t& cidx) {
67     size_t nc = children_.size();
68     bool found = false;
69     for (size_t i = 0; i < nc && !found; ++i)
70       if (children_[i]->cam_id_ == node->cam_id_) {
71         cidx = i;
72         found = true;
73       }
74     return found;
75   }
76 
77   // public members
78   size_t cam_id_ = 0;
79   std::vector<std::shared_ptr<acal_match_node> > children_;
80   std::vector<std::vector<acal_match_pair> > self_to_child_matches_;
81 
82  private:
83 
84   // private members
85   bool has_parent_ = false;
86   std::weak_ptr<acal_match_node> parent_;
87 
88 };
89 
90 // streaming operator
91 std::ostream& operator<<(std::ostream& os, acal_match_node const& node);
92 
93 
94 
95 class acal_match_tree
96 {
97  public:
98   //: default constructor
99   acal_match_tree() = default;
100 
101   //: initalizing constructor
acal_match_tree(size_t root_id)102   acal_match_tree(size_t root_id) {
103     root_->cam_id_ = root_id;
104   }
105   //: copy constructor
106   acal_match_tree(acal_match_tree const& mt);
107 
108   //: construct a new tree with the specified nodes removed
109   acal_match_tree(acal_match_tree const& tree, std::vector<size_t> nodes_to_remove);
110 
111   //: size accessor
size()112   size_t size() {return n_;}
113 
update_tree_size()114   void update_tree_size() {
115     n_ = 0;
116     n_nodes(root_, n_);
117   }
118 
119   //: add a child node and reconcile the correspondence pairs globally over the full tree
120   bool add_child_node(size_t parent_id, size_t child_id, std::vector<acal_match_pair> const& parent_to_child_matches);
121 
122   //: find a leaf node - depth first search
123   static std::shared_ptr<acal_match_node>  find_leaf_node(std::shared_ptr<acal_match_node>& node);
124 
125   //: delete a leaf node, returns false if not a leaf
126   static bool delete_leaf(std::shared_ptr<acal_match_node>& leaf_node);
127 
128   //: delete a subtree including the subtree root,  returns false if deletion fails
129   static bool delete_subtree(std::shared_ptr<acal_match_node>& subtree_root);
130 
131   //: copy a subtree - used in the copy constructor
132   static void copy_subtree(std::shared_ptr<acal_match_node> const& node_from, std::shared_ptr<acal_match_node>& node_to);
133 
134   //: prune correspondence pairs to insure consistent tracks over the full tree
135   static bool prune_tree(std::shared_ptr<acal_match_node> const& mutated_parent_node, size_t child_index);
136 
137   //: recursively remove correspondence pairs that are not present in reduced node corrs
138   static void prune_node(std::shared_ptr<acal_match_node> const& node, std::vector<acal_corr> const& reduced_node_corrs);
139 
140   //: recursively find a node camera id and return the node pointer
141   static std::shared_ptr<acal_match_node> find(std::shared_ptr<acal_match_node> const& node, size_t cam_id);
142 
143   //: recursively find the number of nodes below a node in the tree
144   static void n_nodes(std::shared_ptr<acal_match_node> const& node, size_t& n);
145 
146   // debug methods
147   //: save the tree in dot format for display
148   bool save_tree_dot_format(std::string const& path) const;
149 
150   //: recursively write the tree nodes and edges in dot format
151   bool write_dot(std::ostream& ostr, std::shared_ptr<acal_match_node> const& node, size_t root_id) const;
152 
153   //: collect correspondences recursively
154   static void collect_correspondences(std::shared_ptr<acal_match_node>& node, std::map<size_t, std::vector<vgl_point_2d<double> > >& corrs);
155 
156   //: reorganize correspondences in track format
157   std::vector< std::map<size_t, vgl_point_2d<double> > > tracks() ;
158 
159   //: return nodes
160   std::vector<std::shared_ptr<acal_match_node> > nodes() const;
161 
162   //: return sorted cam ids
163   std::vector<size_t> cam_ids() const;
164 
165   //: print tree
166   void print(std::ostream& os) const;
167 
168   //: equality operators
169   bool operator==(acal_match_tree const& other) const;
170   bool operator!=(acal_match_tree const& other) const { return !(*this == other); }
171 
172   //: assignment operator
173   acal_match_tree& operator = (acal_match_tree const& other) {
174     copy_subtree(other.root_, this->root_);
175     this->update_tree_size();
176     return *this;
177   }
178   // members
179   size_t n_ = 1;
180   size_t min_n_tracks_ = 1;
181   std::shared_ptr<acal_match_node> root_ = std::make_shared<acal_match_node>();
182 
183  private:
184 
185   // recursively process tree info
186   void nodes_recursive(std::vector<std::shared_ptr<acal_match_node> >& nodes, std::shared_ptr<acal_match_node> node) const;
187   void cam_ids_recursive(std::vector<size_t>& ids, std::shared_ptr<acal_match_node> node) const;
188   void print_recursive(std::ostream& os, std::shared_ptr<acal_match_node> node, std::string indent = "  ") const;
189 
190 };
191 
192 // streaming operator
193 std::ostream& operator<<(std::ostream& os, acal_match_tree const& tree);
194 
195 
196 #endif
197