1 #include <iostream>
2 #include "bwm_3d_corr.h"
3 //:
4 // \file
5 #include "vgl/vgl_distance.h"
6 #ifdef _MSC_VER
7 # include "vcl_msvc_warnings.h"
8 #endif
9
match(std::string const & site,vgl_point_3d<double> & pt) const10 bool bwm_3d_corr::match(std::string const& site, vgl_point_3d<double> &pt) const
11 {
12 std::map<std::string, vgl_point_3d<double> >::const_iterator iter = matches_.begin();
13
14 iter = matches_.find(site);
15 if (iter != matches_.end()) {
16 pt = iter->second;
17 return true;
18 }
19
20 std::cerr << "Correspondence is not found for this site\n";
21 return false;
22 }
23
24 //need to use a tolerance to check equality of points
25 static bool
point_equal(vgl_point_3d<double> const & a,vgl_point_3d<double> const & b)26 point_equal(vgl_point_3d<double> const & a, vgl_point_3d<double> const & b)
27 {
28 double tol = 0.001;
29 double d = vgl_distance<double>(a, b);
30 return d<tol;
31 }
32
update_match(std::string const & site,vgl_point_3d<double> old_pt,vgl_point_3d<double> new_pt)33 bool bwm_3d_corr::update_match(std::string const& site, vgl_point_3d<double> old_pt, vgl_point_3d<double> new_pt)
34 {
35 std::map<std::string , vgl_point_3d<double> >::iterator
36 iter = matches_.find(site);
37
38 if (iter != matches_.end()) {
39 vgl_point_3d<double> pt(iter->second);
40 if (point_equal(pt,old_pt)) {
41 iter->second = vgl_point_3d<double> (new_pt);
42 return true;
43 }
44 ++iter;
45 }
46 return false;
47 }
48
set_match(std::string const & site,const double x,const double y,const double z)49 void bwm_3d_corr::set_match(std::string const& site, const double x, const double y, const double z)
50 {
51 vgl_point_3d<double> pt(x, y, z);
52 matches_[site] = pt;
53 }
54
55 //: Deletes the correspondence with respect to the given site
erase(std::string const & site)56 void bwm_3d_corr::erase(std::string const& site)
57 {
58 matches_.erase(site);
59 }
60
61 //: return the list of sites being corresponded
sites() const62 std::vector<std::string> bwm_3d_corr::sites() const
63 {
64 std::vector<std::string> sites(0);
65 std::map<std::string, vgl_point_3d<double> >::const_iterator iter = matches_.begin();
66 while (iter != matches_.end()) {
67 sites.push_back(iter->first);
68 ++iter;
69 }
70 return sites;
71 }
72
73 //: test if the site exists in the set of correspondences and if so return the associated 3-d point
site_in(std::string const & site,vgl_point_3d<double> & corr) const74 bool bwm_3d_corr::site_in(std::string const& site, vgl_point_3d<double> &corr) const
75 {
76 std::map<std::string, vgl_point_3d<double> >::const_iterator iter = matches_.begin();
77 while (iter != matches_.end()) {
78 if (site == iter->first) {
79 corr = iter->second;
80 return true;
81 }
82 ++iter;
83 }
84 return false;
85 }
86
87
88 // return the set of correspondences as pairs
match_list() const89 std::vector<std::pair<std::string, vgl_point_3d<double> > > bwm_3d_corr::match_list() const
90 {
91 std::vector<std::pair<std::string, vgl_point_3d<double> > > mlist;
92 std::map<std::string, vgl_point_3d<double> >::const_iterator iter = matches_.begin();
93 while (iter != matches_.end()) {
94 std::pair<std::string, vgl_point_3d<double> > pair;
95 pair.first = iter->first;
96 pair.second = vgl_point_3d<double> (iter->second.x(), iter->second.y(), iter->second.z());
97 mlist.push_back(pair);
98 ++iter;
99 }
100 return mlist;
101 }
102 // return the set of corresponded points over all sites
matching_pts() const103 std::vector<vgl_point_3d<double> > bwm_3d_corr::matching_pts() const
104 {
105 std::vector<vgl_point_3d<double> > matches;
106 std::map<std::string, vgl_point_3d<double> >::const_iterator iter =
107 matches_.begin();
108 while (iter != matches_.end()){
109 matches.push_back(iter->second);
110 ++iter;
111 }
112 return matches;
113 }
114
115 // output stream
operator <<(std::ostream & s,bwm_3d_corr const & c)116 std::ostream& operator<<(std::ostream& s, bwm_3d_corr const& c)
117 {
118 std::vector<std::pair<std::string, vgl_point_3d<double> > > mlist = c.match_list();
119
120 s << "Number of Sites: " << mlist.size() << '\n';
121 std::vector<std::pair<std::string, vgl_point_3d<double> > >::const_iterator iter = mlist.begin();
122 while (iter != mlist.end()) {
123 s << "Site[ " << iter->first << " ]:( X: " << iter->second.x()
124 << " Y: " << iter->second.y() << " Z: " << iter->second.z() << " )\n";
125 ++iter;
126 }
127
128 return s;
129 }
130