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