1 #include <cmath>
2 #include <iostream>
3 #include <algorithm>
4 #include "vmal_track_lines.h"
5
6 #include "vnl/vnl_math.h" // for pi
7 #include "vnl/vnl_double_2.h"
8
9 #include <vtol/vtol_edge_2d.h>
10
11 #ifdef _MSC_VER
12 # include "vcl_msvc_warnings.h"
13 #endif
14
15 #include <vmal/vmal_lines_correlation.h>
16 #include <vmal/vmal_refine_lines.h>
17 #include <vmal/vmal_operators.h>
18 #include <vmal/vmal_convert_vtol.h>
19
20 #define PI vnl_math::pi
21
22 vmal_track_lines::vmal_track_lines() = default;
23
24 vmal_track_lines::~vmal_track_lines() = default;
25
track_lines(const std::vector<std::vector<vtol_edge_2d_sptr> * > * fit_lines,const std::vector<std::vector<vtol_edge_2d_sptr> * > * transformed_lines,const std::vector<vil1_image> & images,const std::vector<vnl_double_3x3> & homo,const vmal_multi_view_data_edge_sptr & matches)26 void vmal_track_lines::track_lines(const std::vector<std::vector<vtol_edge_2d_sptr>*>* fit_lines,
27 const std::vector<std::vector<vtol_edge_2d_sptr>*>* transformed_lines,
28 const std::vector<vil1_image> &images,
29 const std::vector<vnl_double_3x3> &homo,
30 const vmal_multi_view_data_edge_sptr& matches)
31 {
32 theta_=0.0873;//0.0873;
33 radius_=5.0;
34 vmal_multi_view_data_edge_sptr tmp_matches;
35 tmp_matches=new vmal_multi_view_data<vtol_edge_2d_sptr>(matches->get_nb_views());
36
37 if (fit_lines->size()==(transformed_lines->size()+1))
38 {
39 //int match_num=0;
40 unsigned int min_line=0;
41 double min_dist=-1;
42 bool match=false;
43 bool replace=false;
44 vtol_edge_2d_sptr cur_fl;
45 vtol_edge_2d_sptr cur_tl;
46
47 for (unsigned int i=0;i<(*transformed_lines)[0]->size();i++)
48 {
49 bool found=true;
50 unsigned int match_line=i;
51 unsigned int view_num=0;
52 tmp_matches->new_track();
53 while ((view_num < transformed_lines->size()) && found)
54 {
55 found=false;
56 cur_tl=(*(*transformed_lines)[view_num])[match_line];
57 //double tl1x=cur_tl->v1()->cast_to_vertex_2d()->x();
58 //double tl2x=cur_tl->v2()->cast_to_vertex_2d()->x();
59 //double tl1y=cur_tl->v1()->cast_to_vertex_2d()->y();
60 //double tl2y=cur_tl->v2()->cast_to_vertex_2d()->y();
61 vtol_edge_2d_sptr other_match;
62 for (unsigned int j=0; j<(*fit_lines)[view_num+1]->size(); ++j)
63 {
64 cur_fl=(*(*fit_lines)[view_num+1])[j];
65 //double fl1x=cur_fl->v1()->cast_to_vertex_2d()->x();
66 //double fl2x=cur_fl->v2()->cast_to_vertex_2d()->x();
67 //double fl1y=cur_fl->v1()->cast_to_vertex_2d()->y();
68 //double fl2y=cur_fl->v2()->cast_to_vertex_2d()->y();
69
70 double angle=seg_angle(cur_tl,cur_fl);
71 if (angle<theta_)
72 {
73 if (belong(cur_tl,cur_fl))
74 {
75 double cur_dist;
76 //int size=(*fit_lines)[view_num]->size();
77 vtol_edge_2d_sptr pred_fl=(*(*fit_lines)[view_num])[match_line];
78 cost_function(pred_fl,
79 cur_tl,
80 cur_fl,
81 images[view_num],
82 images[view_num+1],
83 homo[view_num], cur_dist);
84 if (min_dist==-1)//Initial value
85 min_dist=cur_dist;
86 if ((cur_dist<=min_dist) && (cur_dist!=-1))
87 {
88 //look if this line have already been matched.
89 //If so, perform a test between those two lines
90 //to see which one is the best. It can also decide
91 //that they both suit.
92 if (tmp_matches->get_pred_match(view_num,cur_fl,other_match))
93 {
94 vtol_edge_2d_sptr t_other_match=find_transfo(other_match,*(*fit_lines)[view_num],*(*transformed_lines)[view_num]);
95 double other_dist;
96 cost_function(other_match,
97 t_other_match,
98 cur_fl,
99 images[view_num],
100 images[view_num+1],
101 homo[view_num], other_dist);
102 //double ol1x=t_other_match->v1()->cast_to_vertex_2d()->x();
103 //double ol2x=t_other_match->v2()->cast_to_vertex_2d()->x();
104 //double ol1y=t_other_match->v1()->cast_to_vertex_2d()->y();
105 //double ol2y=t_other_match->v2()->cast_to_vertex_2d()->y();
106
107 //int choice=is_cur_best(cur_tl,cur_fl,t_other_match);
108 int choice;
109 if (other_dist<cur_dist)
110 choice=-1;
111 else
112 choice=1;
113 if (choice==1) //the new is better
114 {
115 found=true;
116 min_dist=cur_dist;
117 min_line=j;
118 replace=true;
119 }
120 else if (choice==0) //they both suit
121 {
122 found=true;
123 min_dist=cur_dist;
124 min_line=j;
125 replace=false;
126 }
127 else if (choice==-1) //the old match is the best
128 {
129 found=false;
130 min_dist=cur_dist;
131 min_line=j;
132 replace=false;
133 }
134 }
135 else
136 {
137 found=true;
138 min_dist=cur_dist;
139 min_line=j;
140 replace=false;
141 }
142 }
143 }
144 }
145 }
146 if (found)
147 {
148 if (replace)
149 {
150 tmp_matches->remove(view_num+1, (*(*fit_lines)[view_num+1])[min_line]);
151 replace=false;
152 }
153 tmp_matches->set(view_num,(*(*fit_lines)[view_num])[match_line]);
154 match=true;
155 view_num++;
156 }
157 match_line=min_line;
158 min_dist=-1 ;
159 }
160 if (match)
161 {
162 vtol_edge_2d_sptr p=(*(*fit_lines)[view_num])[match_line];
163 tmp_matches->set(view_num,p);
164 match=false;
165 tmp_matches->close_track();
166 }
167 }
168 }
169 sort_lines(tmp_matches,matches);
170 matches->print(std::cerr);
171 }
172
seg_angle(const vtol_edge_2d_sptr & trans_line,const vtol_edge_2d_sptr & fit_line)173 double vmal_track_lines::seg_angle(const vtol_edge_2d_sptr& trans_line,const vtol_edge_2d_sptr& fit_line)
174 {
175 double vect_tlx=(trans_line->v2()->cast_to_vertex_2d()->x())-(trans_line->v1()->cast_to_vertex_2d()->x());
176 double vect_tly=(trans_line->v2()->cast_to_vertex_2d()->y())-(trans_line->v1()->cast_to_vertex_2d()->y());
177
178 double vect_flx=(fit_line->v2()->cast_to_vertex_2d()->x())-(fit_line->v1()->cast_to_vertex_2d()->x());
179 double vect_fly=(fit_line->v2()->cast_to_vertex_2d()->y())-(fit_line->v1()->cast_to_vertex_2d()->y());
180
181 vnl_double_2 vect_tl(vect_tlx,vect_tly);
182
183 vnl_double_2 vect_fl(vect_flx,vect_fly);
184
185 double alpha=angle(vect_tl,vect_fl);
186
187 return alpha;
188 }
189
belong(const vtol_edge_2d_sptr & trans_line,const vtol_edge_2d_sptr & fit_line) const190 bool vmal_track_lines::belong(const vtol_edge_2d_sptr& trans_line,const vtol_edge_2d_sptr& fit_line) const
191 {
192 double tl1x=trans_line->v1()->cast_to_vertex_2d()->x();
193 double tl2x=trans_line->v2()->cast_to_vertex_2d()->x();
194 double vect_tlx=tl2x-tl1x;
195
196 double tl1y=trans_line->v1()->cast_to_vertex_2d()->y();
197 double tl2y=trans_line->v2()->cast_to_vertex_2d()->y();
198 double vect_tly=tl2y-tl1y;
199
200 vnl_double_2 vect_tl(vect_tlx,vect_tly);
201 vnl_double_2 norma=vect_tl.normalize();
202
203 double fl1x=fit_line->v1()->cast_to_vertex_2d()->x();
204 double fl2x=fit_line->v2()->cast_to_vertex_2d()->x();
205
206 double fl1y=fit_line->v1()->cast_to_vertex_2d()->y();
207 double fl2y=fit_line->v2()->cast_to_vertex_2d()->y();
208
209 //defined the first bounding segment
210 double bound1_tl1x=tl1x+(-norma[1]*radius_);
211 double bound1_tl1y=tl1y+(norma[0]*radius_);
212 double bound1_tl2x=tl2x+(-norma[1]*radius_);
213 double bound1_tl2y=tl2y+(norma[0]*radius_);
214
215 //defined the second bounding segment
216 double bound2_tl1x=tl1x+(norma[1]*radius_);
217 double bound2_tl1y=tl1y+(-norma[0]*radius_);
218 double bound2_tl2x=tl2x+(norma[1]*radius_);
219 double bound2_tl2y=tl2y+(-norma[0]*radius_);
220
221
222 if (vmal_operators::cross_seg(bound1_tl1x, bound1_tl1y, bound2_tl1x, bound2_tl1y,
223 fl1x, fl1y, fl2x, fl2y))
224
225 return true;
226 else if (vmal_operators::cross_seg(bound1_tl2x, bound1_tl2y, bound2_tl2x,bound2_tl2y,
227 fl1x, fl1y,fl2x, fl2y))
228 return true;
229 else //test if at least one of the extremities of the fit line
230 //is inside the bounding rectangle.
231 {
232 double x1,y1,x2,y2;
233 vmal_operators::project_point(fl1x,fl1y,bound1_tl1x,bound1_tl1y,bound1_tl2x,bound1_tl2y,&x1,&y1);
234 vmal_operators::project_point(fl1x,fl1y,bound2_tl1x,bound2_tl1y,bound2_tl2x,bound2_tl2y,&x2,&y2);
235 if ((x1!=-1)&&(x2!=-1))
236 {
237 if (((x1-fl1x)*(x2-fl1x)+(y1-fl1y)*(y2-fl1y))<0)
238 return true;
239 }
240 vmal_operators::project_point(fl2x,fl2y,bound1_tl1x,bound1_tl1y,bound1_tl2x,bound1_tl2y,&x1,&y1);
241 vmal_operators::project_point(fl2x,fl2y,bound2_tl1x,bound2_tl1y,bound2_tl2x,bound2_tl2y,&x2,&y2);
242 if ((x1!=-1)&&(x2!=-1))
243 {
244 if (((x1-fl2x)*(x2-fl2x)+(y1-fl2y)*(y2-fl2y))<0)
245 return true;
246 }
247 }
248 return false;
249 }
250
dist(const vtol_edge_2d_sptr & trans_line,const vtol_edge_2d_sptr & fit_line)251 double vmal_track_lines::dist(const vtol_edge_2d_sptr& trans_line,const vtol_edge_2d_sptr& fit_line)
252 {
253 double tl1x=trans_line->v1()->cast_to_vertex_2d()->x();
254 double tl2x=trans_line->v2()->cast_to_vertex_2d()->x();
255 double tl1y=trans_line->v1()->cast_to_vertex_2d()->y();
256 double tl2y=trans_line->v2()->cast_to_vertex_2d()->y();
257 double vect_tlx=tl2x-tl1x;
258 double vect_tly=tl2y-tl1y;
259 vnl_double_2 vect_tl(vect_tlx,vect_tly);
260 vect_tl=vect_tl.normalize();
261
262 double fl1x=fit_line->v1()->cast_to_vertex_2d()->x();
263 double fl2x=fit_line->v2()->cast_to_vertex_2d()->x();
264 double fl1y=fit_line->v1()->cast_to_vertex_2d()->y();
265 double fl2y=fit_line->v2()->cast_to_vertex_2d()->y();
266 double vect_flx=fl2x-fl1x;
267 double vect_fly=fl2y-fl1y;
268 vnl_double_2 vect_fl(vect_flx,vect_fly);
269 vect_fl=vect_fl.normalize();
270
271 double dist;
272 double distover;
273
274 double x1,y1;
275 double x2,y2;
276 double x3,y3;
277 double x4,y4;
278
279 double dist1=vmal_operators::project_point(fl1x,fl1y,tl1x,tl1y,tl2x,tl2y,&x1,&y1);
280 double dist2=vmal_operators::project_point(fl2x,fl2y,tl1x,tl1y,tl2x,tl2y,&x2,&y2);
281 double dist3=vmal_operators::project_point(tl1x,tl1y,fl1x,fl1y,fl2x,fl2y,&x3,&y3);
282 double dist4=vmal_operators::project_point(tl2x,tl2y,fl1x,fl1y,fl2x,fl2y,&x4,&y4);
283
284 if (dist1==-1)
285 return -1;
286
287 if (x1!=-1 && x2!=-1) // the first segment totally project on the second
288 {
289 if (x3!=-1)
290 {
291 dist=dist3+std::min(dist1,dist2);
292 if (dist1<dist2)
293 distover=std::sqrt((x1-x2)*(x1-x2)+(y1-y2)*(y1-y2))+
294 std::sqrt((x3-fl1x)*(x3-fl1x)+(y3-fl1y)*(y3-fl1y));
295 else
296 distover=std::sqrt((x1-x2)*(x1-x2)+(y1-y2)*(y1-y2))+
297 std::sqrt((x3-fl2x)*(x3-fl2x)+(y3-fl2y)*(y3-fl2y));
298 }
299 else if (x4!=-1)
300 {
301 dist=dist4+std::min(dist1,dist2);
302 if (dist1<dist2)
303 distover=std::sqrt((x1-x2)*(x1-x2)+(y1-y2)*(y1-y2))+
304 std::sqrt((x4-fl1x)*(x4-fl1x)+(y4-fl1y)*(y4-fl1y));
305 else
306 distover=std::sqrt((x1-x2)*(x1-x2)+(y1-y2)*(y1-y2))+
307 std::sqrt((x4-fl2x)*(x4-fl2x)+(y4-fl2y)*(y4-fl2y));
308 }
309 else
310 {
311 dist=dist1+dist2;
312 distover=std::sqrt((x1-x2)*(x1-x2)+(y1-y2)*(y1-y2))+
313 std::sqrt((fl1x-fl2x)*(fl1x-fl2x)+(fl1y-fl2y)*(fl1y-fl2y));
314 }
315 }
316 else if (x1!=-1) //case 2
317 {
318 dist=dist1;
319
320 if ((x3!=-1)&&(x4!=-1))
321 {
322 dist+=std::min(dist3,dist4);
323 if (dist3<dist4)
324 distover=std::sqrt((x3-fl1x)*(x3-fl1x)+(y3-fl1y)*(y3-fl1y))+
325 std::sqrt((x1-tl1x)*(x1-tl1x)+(y1-tl1y)*(y1-tl1y));
326 else
327 distover=std::sqrt((x4-fl1x)*(x4-fl1x)+(y4-fl1y)*(y4-fl1y))+
328 std::sqrt((x1-tl2x)*(x1-tl2x)+(y1-tl2y)*(y1-tl2y));
329 }
330 else if (x3!=-1)
331 {
332 dist+=dist3;
333 distover=std::sqrt((x3-fl1x)*(x3-fl1x)+(y3-fl1y)*(y3-fl1y))+
334 std::sqrt((x1-tl1x)*(x1-tl1x)+(y1-tl1y)*(y1-tl1y));
335 }
336 else if (x4!=-1)
337 {
338 dist+=dist4;
339 distover=std::sqrt((x4-fl1x)*(x4-fl1x)+(y4-fl1y)*(y4-fl1y))+
340 std::sqrt((x1-tl2x)*(x1-tl2x)+(y1-tl2y)*(y1-tl2y));
341 }
342 else
343 {
344 // dist=2*dist;
345 // distover=std::min(std::sqrt((x1-tl2x)*(x1-tl2x)+(y1-tl2y)*(y1-tl2y)),
346 // std::sqrt((x1-tl1x)*(x1-tl1x)+(y1-tl1y)*(y1-tl1y)));
347 dist=-1;
348 distover=1;
349 }
350 }
351 else if (x2!=-1)
352 {
353 dist=dist2;
354
355 if ((x3!=-1)&&(x4!=-1))
356 {
357 dist+=std::min(dist3,dist4);
358 if (dist3<dist4)
359 distover=std::sqrt((x3-fl2x)*(x3-fl2x)+(y3-fl2y)*(y3-fl2y))+
360 std::sqrt((x2-tl1x)*(x2-tl1x)+(y2-tl1y)*(y2-tl1y));
361 else
362 distover=std::sqrt((x4-fl2x)*(x4-fl2x)+(y4-fl2y)*(y4-fl2y))+
363 std::sqrt((x2-tl2x)*(x2-tl2x)+(y2-tl2y)*(y2-tl2y));
364 }
365 else if (x3!=-1)
366 {
367 dist+=dist3;
368 distover=std::sqrt((x3-fl2x)*(x3-fl2x)+(y3-fl2y)*(y3-fl2y))+
369 std::sqrt((x2-tl1x)*(x2-tl1x)+(y2-tl1y)*(y2-tl1y));
370 }
371 else if (x4!=-1)
372 {
373 dist+=dist4;
374 distover=std::sqrt((x4-fl2x)*(x4-fl2x)+(y4-fl2y)*(y4-fl2y))+
375 std::sqrt((x2-tl2x)*(x2-tl2x)+(y2-tl2y)*(y2-tl2y));
376 }
377 else
378 {
379 dist=-1;
380 distover=1;
381 }
382 }
383 else
384 {
385 dist=dist3+dist4;
386 distover=std::sqrt((x3-x4)*(x3-x4)+(y3-y4)*(y3-y4))+
387 std::sqrt((tl1x-tl2x)*(tl1x-tl2x)+(tl1y-tl2y)*(tl1y-tl2y));
388 }
389
390 #if 0
391 double length=std::sqrt((tl1x-tl2x)*(tl1x-tl2x)+(tl1y-tl2y)*(tl1y-tl2y))
392 + std::sqrt((fl1x-fl2x)*(fl1x-fl2x)+(fl1y-fl2y)*(fl1y-fl2y));
393 #endif
394
395 return dist/distover;
396 }
397
is_cur_best(const vtol_edge_2d_sptr & trans_line,const vtol_edge_2d_sptr & fit_line,const vtol_edge_2d_sptr & other_line)398 int vmal_track_lines::is_cur_best(const vtol_edge_2d_sptr& trans_line,const vtol_edge_2d_sptr& fit_line,const vtol_edge_2d_sptr& other_line)
399 {
400 double tl1x=trans_line->v1()->cast_to_vertex_2d()->x();
401 double tl2x=trans_line->v2()->cast_to_vertex_2d()->x();
402 double tl1y=trans_line->v1()->cast_to_vertex_2d()->y();
403 double tl2y=trans_line->v2()->cast_to_vertex_2d()->y();
404 #if 0
405 double fl1x=fit_line->v1()->cast_to_vertex_2d()->x();
406 double fl2x=fit_line->v2()->cast_to_vertex_2d()->x();
407 double fl1y=fit_line->v1()->cast_to_vertex_2d()->y();
408 double fl2y=fit_line->v2()->cast_to_vertex_2d()->y();
409 #endif
410 double ol1x=other_line->v1()->cast_to_vertex_2d()->x();
411 double ol2x=other_line->v2()->cast_to_vertex_2d()->x();
412 double ol1y=other_line->v1()->cast_to_vertex_2d()->y();
413 double ol2y=other_line->v2()->cast_to_vertex_2d()->y();
414
415 double x1,y1;
416 double x2,y2;
417 double x3,y3;
418 double x4,y4;
419
420 vmal_operators::project_point(tl1x,tl1y,ol1x,ol1y,ol2x,ol2y,&x1,&y1);
421 vmal_operators::project_point(tl2x,tl2y,ol1x,ol1y,ol2x,ol2y,&x2,&y2);
422 vmal_operators::project_point(ol1x,ol1y,tl1x,tl1y,tl2x,tl2y,&x3,&y3);
423 vmal_operators::project_point(ol2x,ol2y,tl1x,tl1y,tl2x,tl2y,&x4,&y4);
424
425 #if 0
426 if ((x1==-1)&&(x2==-1)&&(x3==-1)&&(x4==-1))
427 return 0;
428 else
429 #endif
430 {
431 double dist1=dist(trans_line,fit_line);
432 double dist2=dist(other_line,fit_line);
433 if (dist1<dist2)
434 return 1;
435 else
436 return -1;
437 }
438 }
439
find_transfo(const vtol_edge_2d_sptr & line,std::vector<vtol_edge_2d_sptr> & fit_lines,const std::vector<vtol_edge_2d_sptr> & transformed_lines)440 vtol_edge_2d_sptr vmal_track_lines::find_transfo(const vtol_edge_2d_sptr& line,
441 std::vector<vtol_edge_2d_sptr>& fit_lines,
442 const std::vector<vtol_edge_2d_sptr>& transformed_lines
443 )
444 {
445 std::vector<vtol_edge_2d_sptr>::iterator iter;
446 int i=0;
447 for (iter=fit_lines.begin(); iter!=fit_lines.end(); ++iter)
448 {
449 if (*(*iter)==*line)
450 return transformed_lines[i];
451 i++;
452 }
453 return nullptr;
454 }
455
sort_lines(const vmal_multi_view_data_edge_sptr & matches,const vmal_multi_view_data_edge_sptr & sorted_matches)456 void vmal_track_lines::sort_lines(const vmal_multi_view_data_edge_sptr& matches,
457 const vmal_multi_view_data_edge_sptr& sorted_matches)
458 {
459 bool still_track;
460 std::map<int,vtol_edge_2d_sptr,std::less<int> > track;
461 still_track=matches->get_first_track(track);
462 while (still_track)
463 {
464 sorted_matches->new_track();
465 std::map<int,vtol_edge_2d_sptr,std::less<int> >::iterator iter1;
466 auto iter2=track.begin();
467 iter2++;
468 for (iter1=track.begin(); iter2!=track.end(); ++iter1)
469 {
470 int key1=(*iter1).first;
471 vtol_edge_2d_sptr value0=(*iter1).second;
472 int key2=(*iter2).first;
473 vtol_edge_2d_sptr value1=(*iter2).second;
474
475 vtol_edge_2d_sptr out0;
476 vtol_edge_2d_sptr out1;
477
478 sort_a_pair_of_line(value0, value1, out0, out1);
479
480 sorted_matches->set(key1,out0);
481 sorted_matches->set(key2,out1);
482 iter2++;
483 }
484 sorted_matches->close_track();
485 still_track=matches->get_next_track(track);
486 }
487 }
488
sort_a_pair_of_line(const vtol_edge_2d_sptr & line0,const vtol_edge_2d_sptr & line1,vtol_edge_2d_sptr & new_line0,vtol_edge_2d_sptr & new_line1)489 void vmal_track_lines::sort_a_pair_of_line(const vtol_edge_2d_sptr& line0,
490 const vtol_edge_2d_sptr& line1,
491 vtol_edge_2d_sptr &new_line0,
492 vtol_edge_2d_sptr &new_line1)
493 {
494 double cur_1x=line0->v1()->cast_to_vertex_2d()->x();
495 double cur_2x=line0->v2()->cast_to_vertex_2d()->x();
496 double cur_1y=line0->v1()->cast_to_vertex_2d()->y();
497 double cur_2y=line0->v2()->cast_to_vertex_2d()->y();
498 vnl_double_2 cur(cur_2x-cur_1x,cur_2y-cur_1y);
499
500 double next_1x=line1->v1()->cast_to_vertex_2d()->x();
501 double next_2x=line1->v2()->cast_to_vertex_2d()->x();
502 double next_1y=line1->v1()->cast_to_vertex_2d()->y();
503 double next_2y=line1->v2()->cast_to_vertex_2d()->y();
504 vnl_double_2 next(next_2x-next_1x,next_2y-next_1y);
505
506 if (dot_product(cur,next)<0)
507 {
508 new_line0=new vtol_edge_2d(cur_1x,cur_1y,cur_2x,cur_2y);
509 new_line1=new vtol_edge_2d(next_2x,next_2y,next_1x,next_1y);
510 }
511 else
512 {
513 new_line0=new vtol_edge_2d(cur_1x,cur_1y,cur_2x,cur_2y);
514 new_line1=new vtol_edge_2d(next_1x,next_1y,next_2x,next_2y);
515 }
516 }
517
518
lines_correlation(const vtol_edge_2d_sptr & line0,const vtol_edge_2d_sptr & line1,const vnl_double_3x3 & H,vil1_memory_image_of<vxl_byte> & image0,vil1_memory_image_of<vxl_byte> & image1)519 double vmal_track_lines::lines_correlation(const vtol_edge_2d_sptr& line0,
520 const vtol_edge_2d_sptr& line1,
521 const vnl_double_3x3 & H,
522 vil1_memory_image_of<vxl_byte> &image0,
523 vil1_memory_image_of<vxl_byte> &image1)
524 {
525 vtol_edge_2d_sptr s_line0;
526 vtol_edge_2d_sptr s_line1;
527 // sort the lines so that their end-points match
528 sort_a_pair_of_line(line0, line1, s_line0, s_line1);
529
530 vnl_double_3 s_line0_p, s_line0_q;
531 vnl_double_3 s_line1_p, s_line1_q;
532
533 convert_line_double_3(s_line0, s_line0_p, s_line0_q);
534 convert_line_double_3(s_line1, s_line1_p, s_line1_q);
535
536 vnl_double_3 r_line0_p, r_line0_q;
537 vnl_double_3 r_line1_p, r_line1_q;
538
539 // refine the lines so that their length are the same
540 vmal_refine_lines ref;
541 ref.refine_lines_min_h(s_line0_p, s_line0_q,
542 s_line1_p, s_line1_q,
543 H,
544 r_line0_p, r_line0_q,
545 r_line1_p, r_line1_q);
546
547 vmal_lines_correlation correl;
548 vnl_double_3 trans;
549 double res;
550 res=correl.find_min_corr(r_line0_p, r_line0_q,
551 r_line1_p, r_line1_q,
552 image0, image1,
553 trans);
554 return res;
555 }
556
557
cost_function(const vtol_edge_2d_sptr & line0,const vtol_edge_2d_sptr &,const vtol_edge_2d_sptr & line1,const vil1_image & image0,const vil1_image & image1,const vnl_double_3x3 homo,double & result)558 void vmal_track_lines::cost_function(const vtol_edge_2d_sptr& line0,
559 const vtol_edge_2d_sptr& /*t_line0*/,
560 const vtol_edge_2d_sptr& line1,
561 const vil1_image &image0,
562 const vil1_image &image1,
563 const vnl_double_3x3 homo,
564 double &result)
565 {
566 vil1_memory_image_of<vxl_byte> i0;
567 vil1_memory_image_of<vxl_byte> i1;
568 convert_grey_memory_image(image0,i0);
569 convert_grey_memory_image(image1,i1);
570 result=lines_correlation(line0, line1, homo, i0, i1);
571 #if 0 // TODO ?
572 result=dist(t_line0, line1);
573 double alpha=0.5;
574 result=result*alpha+(1-alpha);
575 #endif
576 }
577