1 #include <iostream>
2 #include "bwm_corr.h"
3 //:
4 // \file
5 #include "bwm_observer_cam.h"
6 #include "io/bwm_io_structs.h"
7 #include "vgl/vgl_distance.h"
8 #ifdef _MSC_VER
9 # include "vcl_msvc_warnings.h"
10 #endif
11 #include "vsl/vsl_basic_xml_element.h"
12 #include <vsol/vsol_point_2d.h>
13
match(bwm_observer_cam * obs,vgl_point_2d<double> & pt)14 bool bwm_corr::match(bwm_observer_cam* obs, vgl_point_2d<double> &pt)
15 {
16 std::map<bwm_observer_cam*, vgl_point_2d<double> >::iterator iter = matches_.begin();
17
18 iter = matches_.find(obs);
19 if (iter != matches_.end()) {
20 pt = iter->second;
21 return true;
22 }
23
24 std::cerr << "Correspondent point is not found for this observer\n";
25 return false;
26 }
27 //need to use a tolerance to check equality of points
28 static bool
point_equal(vgl_point_2d<double> const & a,vgl_point_2d<double> const & b)29 point_equal(vgl_point_2d<double> const & a, vgl_point_2d<double> const & b)
30 {
31 double tol = 0.001;
32 double d = vgl_distance<double>(a, b);
33 return d<tol;
34 }
35
update_match(bwm_observer_cam * obs,vgl_point_2d<double> old_pt,vgl_point_2d<double> new_pt)36 bool bwm_corr::update_match(bwm_observer_cam* obs, vgl_point_2d<double> old_pt, vgl_point_2d<double> new_pt)
37 {
38 std::map<bwm_observer_cam*, vgl_point_2d<double> >::iterator
39 iter = matches_.find(obs);
40
41 if (iter != matches_.end()) {
42 vgl_point_2d<double> pt(iter->second);
43 if (point_equal(pt,old_pt)) {
44 iter->second = vgl_point_2d<double> (new_pt);
45 return true;
46 }
47 iter++;
48 }
49 return false;
50 }
51
set_match(bwm_observer_cam * obs,const double x,const double y)52 void bwm_corr::set_match(bwm_observer_cam* obs, const double x, const double y)
53 {
54 vgl_point_2d<double> pt(x, y);
55 //std::pair<bwm_observer_cam*, vgl_point_2d<double> > pair(obs, pt);
56 matches_[obs] = pt;
57 }
58
59 //: Deletes the observer's correspondence point
erase(bwm_observer_cam * obs)60 void bwm_corr::erase(bwm_observer_cam* obs)
61 {
62 matches_.erase(obs);
63 }
64
observers()65 std::vector<bwm_observer_cam*> bwm_corr::observers()
66 {
67 std::vector<bwm_observer_cam*> obs(0);
68 std::map<bwm_observer_cam*, vgl_point_2d<double> >::iterator iter = matches_.begin();
69 while (iter != matches_.end()) {
70 obs.push_back(iter->first);
71 iter++;
72 }
73 return obs;
74 }
75
obs_in(bwm_observer_cam * obs,vgl_point_2d<double> & corr)76 bool bwm_corr::obs_in(bwm_observer_cam* obs, vgl_point_2d<double> &corr)
77 {
78 std::map<bwm_observer_cam*, vgl_point_2d<double> >::iterator iter = matches_.begin();
79 while (iter != matches_.end()) {
80 if (obs == iter->first) {
81 corr = iter->second;
82 return true;
83 }
84 iter++;
85 }
86 return false;
87 }
88
89 //external functions
operator <<(std::ostream & s,bwm_corr const & c)90 std::ostream& operator<<(std::ostream& s, bwm_corr const& c)
91 {
92 s << "Number of Cameras: " << c.matches_.size() << '\n';
93 int i=0;
94 std::map<bwm_observer_cam*, vgl_point_2d<double> >::const_iterator
95 iter = c.matches_.begin();
96
97 // first write down the camera info
98 while (iter != c.matches_.end()) {
99 s << "Camera " << i++ << " :" << iter->first->camera_path() << std::endl;
100 }
101
102 iter = c.matches_.begin();
103 i=0;
104 if (c.mode()) {
105 while (iter != c.matches_.end()) {
106 s << "Camera [" << i++ << "]: [X: " << iter->second.x() << " Y: " << iter->second.y() << "]\n";
107 iter++;
108 }
109 s << "-----------------------------------------------------\n";
110 }
111 else
112 s << "WORLD POINT: " << c.world_pt_ << '\n';
113 return s;
114 }
115
x_write(std::ostream & os)116 void bwm_corr::x_write(std::ostream &os)
117 {
118 vsl_basic_xml_element corr(CORRESPONDENCES_TAG);
119 corr.x_write_open(os);
120
121 if (! mode()) {
122 vsl_basic_xml_element xml_element(CORRESP_PT_TAG);
123 xml_element.add_attribute("X", world_pt().x());
124 xml_element.add_attribute("Y", world_pt().y());
125 xml_element.add_attribute("Z", world_pt().z());
126 xml_element.x_write(os);
127 }
128
129 std::map<bwm_observer_cam*, vgl_point_2d<double> >::const_iterator
130 iter = matches_.begin();
131 while (iter != matches_.end()) {
132 vsl_basic_xml_element corr_elm(CORRESP_ELM_TAG);
133 corr_elm.x_write_open(os);
134
135 vsl_basic_xml_element corr_tab(CORR_CAMERA_TAG);
136 corr_tab.append_cdata(iter->first->tab_name());
137 corr_tab.x_write(os);
138
139 vsl_basic_xml_element xml_element(CORRESP_PT_TAG);
140 xml_element.add_attribute("X", iter->second.x());
141 xml_element.add_attribute("Y", iter->second.y());
142 xml_element.x_write(os);
143 corr_elm.x_write_close(os);
144 iter++;
145 }
146 corr.x_write_close(os);
147 }
148
match_list()149 std::vector<std::pair<std::string, vsol_point_2d> > bwm_corr::match_list()
150 {
151 std::vector<std::pair<std::string, vsol_point_2d> > list;
152 std::map<bwm_observer_cam*, vgl_point_2d<double> >::iterator iter = matches_.begin();
153 while (iter != matches_.end()) {
154 std::pair<std::string, vsol_point_2d> pair;
155 pair.first = iter->first->tab_name();
156 pair.second = vsol_point_2d(iter->second.x(), iter->second.y());
157 list.push_back(pair);
158 iter++;
159 }
160 return list;
161 }
162