1 //:
2 // \file
3 // \author Ozge C Ozcanli (ozge@lems.brown.edu)
4 // \date October 16, 2008
5 
6 #include <iostream>
7 #include <cmath>
8 #include "brec_hierarchy_edge.h"
9 #include <bsta/algo/bsta_gaussian_updater.h>
10 #include "vgl/vgl_point_2d.h"
11 #include "vnl/vnl_quaternion.h"
12 
13 #include <bxml/bxml_find.h>
14 #ifdef _MSC_VER
15 #  include "vcl_msvc_warnings.h"
16 #endif
17 
18 #include "vnl/vnl_cross_product_matrix.h"
19 #include "vnl/vnl_double_3.h"
20 
21 double
22 //brec_hierarchy_edge::prob_density(const vnl_vector_fixed<float,2>& pt)
prob_density(const float dist,const float angle)23 brec_hierarchy_edge::prob_density(const float dist, const float angle)
24 {
25   //: return non-normalized value
26   //return float(loc_model_.var()*vnl_math::twopi*loc_model_.prob_density(pt));
27   double d_dens = dist_model_.prob_density(dist);
28   double d_var = dist_model_.var();
29   d_dens = d_var*2.0f*vnl_math::pi*d_dens;
30 
31   double a_dens = angle_model_.prob_density(angle);
32   double a_var = angle_model_.var();
33   a_dens = a_var*2.0f*(vnl_math::pi)*a_dens;
34 
35   //return weight_*d_dens*a_dens;
36   return d_dens*a_dens;  // not using weighted density (weights were given by mean-shit mode finding for 1D angle and distance distributions separately)
37 }
38 
39 double
prob_density_dist(const float dist)40 brec_hierarchy_edge::prob_density_dist(const float dist)
41 {
42   //: return non-normalized value
43   return dist_model_.var()*vnl_math::twopi*dist_model_.prob_density(dist);
44 }
45 
46 double
prob_density_angle(const float angle)47 brec_hierarchy_edge::prob_density_angle(const float angle)
48 {
49   //: return non-normalized value
50   return angle_model_.var()*vnl_math::twopi*angle_model_.prob_density(angle);
51 }
52 
53 #if 0
54 //: if the model is updated then the to_central flag is made false since the edge becomes a non-central edge
55 void
56 brec_hierarchy_edge::update_model(const vnl_vector_fixed<float,2>& sample)
57 {
58   to_central_ = false;
59   bsta_update_gaussian(loc_model_, 1.0f, sample, 25.0f); // min stad_dev = 5 pixels
60 }
61 #endif // 0
62 
63 //: if the model is updated then the to_central flag is made false since the edge becomes a non-central edge
64 void
update_dist_model(const float dist)65 brec_hierarchy_edge::update_dist_model(const float dist)
66 {
67   to_central_ = false;
68   //bsta_update_gaussian(dist_model_, 1.0f, dist, 4.0f);  // min stad_dev = 2 pixels
69   bsta_update_gaussian(dist_model_, 1.0f, dist, min_stad_dev_dist_*min_stad_dev_dist_);
70 }
71 
72 void
update_angle_model(const float angle)73 brec_hierarchy_edge::update_angle_model(const float angle)
74 {
75   to_central_ = false;
76   //float min_stad_dev = float(vnl_math::pi_over_180)*10.0f;  // min stad_dev = 10 degrees
77   auto min_stad_dev = float(vnl_math::pi_over_180*min_stad_dev_angle_);  // min stad_dev = 10 degrees
78   bsta_update_gaussian(angle_model_, 1.0f, angle, min_stad_dev*min_stad_dev);
79 }
80 
81 void
calculate_dist_angle(const brec_part_instance_sptr & pi,vnl_vector_fixed<float,2> & dif_to_center,float & dist,float & angle)82 brec_hierarchy_edge::calculate_dist_angle(const brec_part_instance_sptr& pi, vnl_vector_fixed<float,2>& dif_to_center, float& dist, float& angle)
83 {
84   // if pi is a composed part we need to get its central part
85   brec_part_instance_sptr pp = pi;
86   while (pp->kind_ == brec_part_instance_kind::COMPOSED) {
87     pp = pp->central_part()->cast_to_instance();
88   }
89 
90   vnl_vector_fixed<float, 2> v = pp->direction_vector();  // get orientation vector of central part: pi
91 
92   dist = (float)dif_to_center.magnitude();
93   vnl_vector_fixed<float, 2> v1_hat = dif_to_center.normalize();
94   angle = (float)std::acos(dot_product(v, v1_hat));
95 
96   //: if angle is ~ 180 degrees return a positive angle
97   if ((std::abs(angle-vnl_math::pi) < 0.17) || (angle < 0.17))  // allow for a 10 degree interval around 180 degrees and 0 degree
98     return;
99 
100   //: now we want this angle positive or negative, depending on which side of v does v1 lie
101   vnl_double_3 v_3(v[0], v[1], 0.0);
102   vnl_double_3 v1_hat_3(v1_hat[0], v1_hat[1], 0.0);
103 
104   vnl_double_3x3 V = vnl_cross_product_matrix(v_3);
105 
106   vnl_double_3 v_v1_hat = V*v1_hat_3;
107   if (v_v1_hat[2] < 0)
108     angle = -angle;  // negate the angle
109 
110   return;
111 }
112 
113 vgl_box_2d<float>
get_probe_box(const brec_part_instance_sptr & central_p)114 brec_hierarchy_edge::get_probe_box(const brec_part_instance_sptr& central_p)
115 {
116   float cx = central_p->x_; float cy = central_p->y_;
117 
118   // if pi is a composed part we need to get its central part
119   brec_part_instance_sptr pp = central_p;
120   while (pp->kind_ == brec_part_instance_kind::COMPOSED) {
121     pp = pp->central_part()->cast_to_instance();
122   }
123   vnl_vector_fixed<float, 2> v = pp->direction_vector();  // get orientation vector of central part: pi
124 
125   //: define a rotation about z axis (in the image plane)
126   double mean_angle = this->mean_angle();
127   double var_angle = this->var_angle();
128   vnl_quaternion<float> q(0.0f, 0.0f, float(mean_angle-var_angle));
129 
130   double mean_dist = this->mean_dist();
131   vnl_vector_fixed<float,3> v3d(v[0], v[1], 0.0f);
132   vnl_vector_fixed<float,3> out = q.rotate(v3d);
133   vnl_vector_fixed<float,3> out_dist = out*float(mean_dist);
134 
135   float mx = cx + out_dist[0];
136   float my = cy + out_dist[1];
137   auto rad = (float)std::ceil(std::sqrt(var_dist())+3);
138   float si = mx - rad;
139   float upper_i = mx + rad;
140   float sj = my - rad;
141   float upper_j = my + rad;
142 
143   vgl_point_2d<float> pr0(si, sj), pr1(si, upper_j), pr2(upper_i, upper_j), pr3(upper_i, sj);
144   vgl_box_2d<float> probe;
145   probe.add(pr0); probe.add(pr1); probe.add(pr2); probe.add(pr3);
146 
147   //: create these boxes for each var_angle() and take union of all boxes
148   vnl_quaternion<float> q2(0.0f, 0.0f, float(mean_angle+var_angle));
149   vnl_vector_fixed<float,3> out2 = q2.rotate(v3d);
150   vnl_vector_fixed<float,3> out_dist2 = out2*float(mean_dist);
151 
152   mx = cx + out_dist2[0];
153   my = cy + out_dist2[1];
154   si = mx - rad;
155   upper_i = mx + rad;
156   sj = my - rad;
157   upper_j = my + rad;
158   pr0.set(si, sj); pr1.set(si, upper_j); pr2.set(upper_i, upper_j); pr3.set(upper_i, sj);
159   probe.add(pr0); probe.add(pr1); probe.add(pr2); probe.add(pr3);
160 
161   return probe;
162 }
163 
164 //: samples the position of the part linked with this edge wrt to the position (x,y)
sample_position(const brec_part_instance_sptr & central_p,float x,float y,vnl_random & rng)165 vnl_vector_fixed<float,2> brec_hierarchy_edge::sample_position(const brec_part_instance_sptr& central_p, float x, float y, vnl_random& rng)
166 {
167   // if central_p is a composed part we need to get its central part
168   brec_part_instance_sptr pp = central_p;
169   while (pp->kind_ == brec_part_instance_kind::COMPOSED) {
170     pp = pp->central_part()->cast_to_instance();
171   }
172   vnl_vector_fixed<float, 2> v = pp->direction_vector();  // get orientation vector of central part
173 
174   //: define a rotation about z axis (in the image plane)
175   vnl_quaternion<float> q(0.0f, 0.0f, float(angle_model_.sample(rng)));
176 
177   vnl_vector_fixed<float,3> v3d(v[0], v[1], 0.0f);
178   vnl_vector_fixed<float,3> out = q.rotate(v3d);
179   vnl_vector_fixed<float,3> out_dist = out*float(dist_model_.sample(rng));
180 
181   float mx = x + out_dist[0];
182   float my = y + out_dist[1];
183   vnl_vector_fixed<float, 2> out_v(mx, my);
184   return out_v;
185 }
186 
mean_position(const brec_part_instance_sptr & central_p,float x,float y)187 vnl_vector_fixed<float,2> brec_hierarchy_edge::mean_position(const brec_part_instance_sptr& central_p, float x, float y)
188 {
189   // if central_p is a composed part we need to get its central part
190   brec_part_instance_sptr pp = central_p;
191   while (pp->kind_ == brec_part_instance_kind::COMPOSED) {
192     pp = pp->central_part()->cast_to_instance();
193   }
194   vnl_vector_fixed<float, 2> v = pp->direction_vector();  // get orientation vector of central part
195 
196   //: define a rotation about z axis (in the image plane)
197   vnl_quaternion<float> q(0.0f, 0.0f, float(angle_model_.mean()));
198 
199   vnl_vector_fixed<float,3> v3d(v[0], v[1], 0.0f);
200   vnl_vector_fixed<float,3> out = q.rotate(v3d);
201   vnl_vector_fixed<float,3> out_dist = out*float(dist_model_.mean());
202 
203   float mx = x + out_dist[0];
204   float my = y + out_dist[1];
205   vnl_vector_fixed<float, 2> out_v(mx, my);
206   return out_v;
207 }
208 
xml_element()209 bxml_data_sptr brec_hierarchy_edge::xml_element()
210 {
211   bxml_element* data = new bxml_element("edge");
212 
213   data->set_attribute("dist_mean",dist_model_.mean());
214   data->set_attribute("dist_var",dist_model_.var());
215   data->set_attribute("angle_mean",angle_model_.mean());
216   data->set_attribute("angle_var",angle_model_.var());
217 
218   data->set_attribute("min_stad_dev_dist",min_stad_dev_dist_);
219   data->set_attribute("min_stad_dev_angle",min_stad_dev_angle_);
220   data->set_attribute("weight",weight_);
221 
222   if (to_central_)
223     data->set_attribute("to_central",1);
224   else
225     data->set_attribute("to_central",0);
226 
227   data->append_text("\n ");
228   return data;
229 }
230 
xml_parse_element(bxml_data_sptr data)231 bool brec_hierarchy_edge::xml_parse_element(bxml_data_sptr data)
232 {
233   bxml_element query("edge");
234   bxml_data_sptr base_root = bxml_find_by_name(data, query);
235 
236   if (!base_root)
237     return false;
238 
239   float dist_mean, dist_var, angle_mean, angle_var;
240   if (base_root->type() == bxml_data::ELEMENT) {
241     bool o = (((bxml_element*)base_root.ptr())->get_attribute("dist_mean", dist_mean) &&
242              ((bxml_element*)base_root.ptr())->get_attribute("dist_var", dist_var) &&
243              ((bxml_element*)base_root.ptr())->get_attribute("angle_mean", angle_mean) &&
244              ((bxml_element*)base_root.ptr())->get_attribute("angle_var", angle_var) &&
245              ((bxml_element*)base_root.ptr())->get_attribute("weight", weight_) &&
246              ((bxml_element*)base_root.ptr())->get_attribute("min_stad_dev_dist", min_stad_dev_dist_) &&
247              ((bxml_element*)base_root.ptr())->get_attribute("min_stad_dev_angle", min_stad_dev_angle_));
248 
249     int to_central_int;
250     o = o && ((bxml_element*)base_root.ptr())->get_attribute("to_central", to_central_int);
251 
252     if (!o)
253       return false;
254 
255     to_central_ = to_central_int == 0 ? false : true;
256 
257     dist_model_.set_mean(dist_mean);
258     dist_model_.set_var(dist_var);
259     angle_model_.set_mean(angle_mean);
260     angle_model_.set_var(angle_var);
261 
262     return true;
263   }
264   else
265     return false;
266 }
267