Visual Servoing Platform  version 3.0.1
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Groups Pages
vpMbScanLine.cpp
1 /****************************************************************************
2  *
3  * This file is part of the ViSP software.
4  * Copyright (C) 2005 - 2017 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or
7  * modify it under the terms of the GNU General Public License
8  * ("GPL") version 2 as published by the Free Software Foundation.
9  * See the file LICENSE.txt at the root directory of this source
10  * distribution for additional information about the GNU GPL.
11  *
12  * For using ViSP with software that can not be combined with the GNU
13  * GPL, please contact Inria about acquiring a ViSP Professional
14  * Edition License.
15  *
16  * See http://visp.inria.fr for more information.
17  *
18  * This software was developed at:
19  * Inria Rennes - Bretagne Atlantique
20  * Campus Universitaire de Beaulieu
21  * 35042 Rennes Cedex
22  * France
23  *
24  * If you have questions regarding the use of this file, please contact
25  * Inria at visp@inria.fr
26  *
27  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
28  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
29  *
30  * Description:
31  * Make the complete tracking of an object by using its CAD model
32  *
33  * Authors:
34  * Aurelien Yol
35  *
36  *****************************************************************************/
37 
38 #include <visp3/core/vpConfig.h>
39 
40 #if defined _MSC_VER && _MSC_VER >= 1200
41 # define NOMINMAX
42 #endif
43 
44 #include <cmath>
45 #include <algorithm>
46 #include <cstddef>
47 #include <iostream>
48 #include <utility>
49 
50 #include <visp3/mbt/vpMbScanLine.h>
51 #include <visp3/core/vpMeterPixelConversion.h>
52 
53 #if defined(DEBUG_DISP)
54 #include <visp3/gui/vpDisplayGDI.h>
55 #include <visp3/gui/vpDisplayX.h>
56 #endif
57 
58 
59 #ifndef DOXYGEN_SHOULD_SKIP_THIS
60 
61 vpMbScanLine::vpMbScanLine()
62  : w(0), h(0), K(), maskBorder(0), mask(), primitive_ids(),
63  visibility_samples(), depthTreshold(1e-06)
64 #if defined(DEBUG_DISP)
65  ,dispMaskDebug(NULL), dispLineDebug(NULL), linedebugImg()
66 #endif
67 {
68 #if defined(VISP_HAVE_X11) && defined(DEBUG_DISP)
69  dispLineDebug = new vpDisplayX();
70  dispMaskDebug = new vpDisplayX();
71 #elif defined(VISP_HAVE_GDI) && defined(DEBUG_DISP)
72  dispLineDebug = new vpDisplayGDI();
73  dispMaskDebug = new vpDisplayGDI();
74 #endif
75 }
76 
77 vpMbScanLine::~vpMbScanLine()
78 {
79 #if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI)) && defined(DEBUG_DISP)
80  if (dispLineDebug != NULL) delete dispLineDebug;
81  if (dispMaskDebug != NULL) delete dispMaskDebug;
82 #endif
83 }
93 void vpMbScanLine::drawLineY(const vpColVector &a,
94  const vpColVector &b,
95  const vpMbScanLineEdge &edge,
96  const int ID,
97  std::vector<std::vector<vpMbScanLineSegment> > &scanlines)
98 {
99  double x0 = a[0] / a[2];
100  double y0 = a[1] / a[2];
101  double z0 = a[2];
102  double x1 = b[0] / b[2];
103  double y1 = b[1] / b[2];
104  double z1 = b[2];
105  if (y0 > y1)
106  {
107  std::swap(x0, x1);
108  std::swap(y0, y1);
109  std::swap(z0, z1);
110  }
111 
112  //if (y0 >= h - 1 || y1 < 0 || y1 == y0)
113  if (y0 >= h - 1 || y1 < 0 || std::fabs(y1 - y0) <= std::numeric_limits<double>::epsilon())
114  return;
115 
116  const unsigned int _y0 = std::max<unsigned int>(0, (unsigned int)(std::ceil(y0)));
117  const double _y1 = std::min<double>(h, y1);
118 
119  const bool b_sample_Y = (std::fabs(y0 - y1) > std::fabs(x0 - x1));
120 
121  for(unsigned int y = _y0 ; y < _y1 ; ++y)
122  {
123  const double x = x0 + (x1 - x0) * (y - y0) / (y1 - y0);
124  const double alpha = getAlpha(y, y0 * z0, z0, y1 * z1, z1);
125  vpMbScanLineSegment s;
126  s.p = x;
127  s.type = POINT;
128  s.Z2 = s.Z1 = mix(z0, z1, alpha);
129  s.P2 = s.P1 = s.p * s.Z1;
130  s.ID = ID;
131  s.edge = edge;
132  s.b_sample_Y = b_sample_Y;
133  scanlines[y].push_back(s);
134  }
135 }
136 
146 void vpMbScanLine::drawLineX(const vpColVector &a,
147  const vpColVector &b,
148  const vpMbScanLineEdge &edge,
149  const int ID,
150  std::vector<std::vector<vpMbScanLineSegment> > &scanlines)
151 {
152  double x0 = a[0] / a[2];
153  double y0 = a[1] / a[2];
154  double z0 = a[2];
155  double x1 = b[0] / b[2];
156  double y1 = b[1] / b[2];
157  double z1 = b[2];
158  if (x0 > x1)
159  {
160  std::swap(x0, x1);
161  std::swap(y0, y1);
162  std::swap(z0, z1);
163  }
164 
165  //if (x0 >= w - 1 || x1 < 0 || x1 == x0)
166  if (x0 >= w - 1 || x1 < 0 || std::fabs(x1 - x0) <= std::numeric_limits<double>::epsilon())
167  return;
168 
169  const unsigned int _x0 = std::max<unsigned int>(0, (unsigned int)(std::ceil(x0)));
170  const double _x1 = std::min<double>(w, x1);
171 
172  const bool b_sample_Y = (std::fabs(y0 - y1) > std::fabs(x0 - x1));
173 
174  for(unsigned int x = _x0 ; x < _x1 ; ++x)
175  {
176  const double y = y0 + (y1 - y0) * (x - x0) / (x1 - x0);
177  const double alpha = getAlpha(x, x0 * z0, z0, x1 * z1, z1);
178  vpMbScanLineSegment s;
179  s.p = y;
180  s.type = POINT;
181  s.Z2 = s.Z1 = mix(z0, z1, alpha);
182  s.P2 = s.P1 = s.p * s.Z1;
183  s.ID = ID;
184  s.edge = edge;
185  s.b_sample_Y = b_sample_Y;
186  scanlines[x].push_back(s);
187  }
188 }
189 
190 
198 void
199 vpMbScanLine::drawPolygonY(const std::vector<std::pair<vpPoint, unsigned int> > &polygon,
200  const int ID,
201  std::vector<std::vector<vpMbScanLineSegment> > &scanlines)
202 {
203  if (polygon.size() < 2)
204  return;
205 
206  if (polygon.size() == 2)
207  {
208  vpColVector p1, p2;
209  createVectorFromPoint(polygon.front().first, p1, K);
210  createVectorFromPoint(polygon.back().first, p2, K);
211 
212  drawLineY(p1,
213  p2,
214  makeMbScanLineEdge(polygon.front().first, polygon.back().first),
215  ID,
216  scanlines);
217  return;
218  }
219 
220  std::vector<std::vector<vpMbScanLineSegment> > local_scanlines;
221  local_scanlines.resize(h);
222 
223  for(size_t i = 0 ; i < polygon.size() ; ++i)
224  {
225  vpColVector p1, p2;
226  createVectorFromPoint(polygon[i].first, p1, K);
227  createVectorFromPoint(polygon[(i + 1) % polygon.size()].first, p2, K);
228 
229  drawLineY(p1, p2, makeMbScanLineEdge(polygon[i].first, polygon[(i + 1) % polygon.size()].first), ID, local_scanlines);
230  }
231 
232  createScanLinesFromLocals(scanlines,local_scanlines,h);
233 }
234 
242 void
243 vpMbScanLine::drawPolygonX(const std::vector<std::pair<vpPoint, unsigned int> > &polygon,
244  const int ID,
245  std::vector<std::vector<vpMbScanLineSegment> > &scanlines)
246 {
247  if (polygon.size() < 2)
248  return;
249 
250  if (polygon.size() == 2)
251  {
252  vpColVector p1, p2;
253  createVectorFromPoint(polygon.front().first, p1, K);
254  createVectorFromPoint(polygon.back().first, p2, K);
255 
256  drawLineX(p1,
257  p2,
258  makeMbScanLineEdge(polygon.front().first, polygon.back().first),
259  ID,
260  scanlines);
261  return;
262  }
263 
264  std::vector<std::vector<vpMbScanLineSegment> > local_scanlines;
265  local_scanlines.resize(w);
266 
267  for(size_t i = 0 ; i < polygon.size() ; ++i)
268  {
269  vpColVector p1, p2;
270  createVectorFromPoint(polygon[i].first, p1, K);
271  createVectorFromPoint(polygon[(i + 1) % polygon.size()].first, p2, K);
272 
273  drawLineX(p1, p2, makeMbScanLineEdge(polygon[i].first, polygon[(i + 1) % polygon.size()].first), ID, local_scanlines);
274  }
275 
276  createScanLinesFromLocals(scanlines,local_scanlines,w);
277 }
278 
288 void
289 vpMbScanLine::createScanLinesFromLocals(std::vector<std::vector<vpMbScanLineSegment> > &scanlines,
290  std::vector<std::vector<vpMbScanLineSegment> > &localScanlines,
291  const unsigned int &size)
292 {
293  for(unsigned int j = 0 ; j < size ; ++j)
294  {
295  std::vector<vpMbScanLineSegment> &scanline = localScanlines[j];
296  sort(scanline.begin(), scanline.end(), vpMbScanLineSegmentComparator()); // Not sure its necessary
297 
298  bool b_start = true;
299  for(size_t i = 0 ; i < scanline.size() ; ++i)
300  {
301  vpMbScanLineSegment s = scanline[i];
302  if (b_start)
303  {
304  s.type = START;
305  s.P1 = s.p * s.Z1;
306  b_start = false;
307  }
308  else
309  {
310  vpMbScanLineSegment &prev = scanlines[j].back();
311  s.type = END;
312  s.P1 = prev.P1;
313  s.Z1 = prev.Z1;
314  s.P2 = s.p * s.Z2;
315  prev.P2 = s.P2;
316  prev.Z2 = s.Z2;
317  b_start = true;
318  }
319  scanlines[j].push_back(s);
320  }
321  }
322 }
323 
333 void
334 vpMbScanLine::drawScene(const std::vector<std::vector<std::pair<vpPoint, unsigned int> > * > &polygons,
335  std::vector<int> listPolyIndices,
336  const vpCameraParameters &cam, unsigned int width, unsigned int height)
337 {
338  this->w = width;
339  this->h = height;
340  this->K = cam;
341 
342  visibility_samples.clear();
343 
344  std::vector<std::vector<vpMbScanLineSegment> > scanlinesY;
345  scanlinesY.resize(h);
346  std::vector<std::vector<vpMbScanLineSegment> > scanlinesX;
347  scanlinesX.resize(w);
348 
349  mask.resize(h,w,0);
350 
351  vpImage<unsigned char> maskY(h,w,0);
352  vpImage<unsigned char> maskX(h,w,0);
353 
354  primitive_ids.resize(h, w, -1);
355 
356  for(unsigned int ID = 0 ; ID < polygons.size() ; ++ID)
357  {
358  drawPolygonY(*(polygons[ID]), listPolyIndices[ID], scanlinesY);
359  drawPolygonX(*(polygons[ID]), listPolyIndices[ID], scanlinesX);
360  }
361 
362  // Y
363  int last_ID = -1;
364  vpMbScanLineSegment last_visible;
365  for(unsigned int y = 0 ; y < scanlinesY.size() ; ++y)
366  {
367  std::vector<vpMbScanLineSegment> &scanline = scanlinesY[y];
368  sort(scanline.begin(), scanline.end(), vpMbScanLineSegmentComparator());
369 
370  std::vector<std::pair<double, vpMbScanLineSegment> > stack;
371  for(size_t i = 0 ; i < scanline.size() ; ++i)
372  {
373  const vpMbScanLineSegment &s = scanline[i];
374 
375  switch(s.type)
376  {
377  case START:
378  stack.push_back(std::make_pair(s.Z1, s));
379  break;
380  case END:
381  for(size_t j = 0 ; j < stack.size() ; ++j)
382  if (stack[j].second.ID == s.ID)
383  {
384  stack[j] = stack.back();
385  stack.pop_back();
386  break;
387  }
388  break;
389  case POINT:
390  break;
391  }
392 
393  for(size_t j = 0 ; j < stack.size() ; ++j)
394  {
395  const vpMbScanLineSegment &s0 = stack[j].second;
396  stack[j].first = mix(s0.Z1, s0.Z2, getAlpha(s.type == POINT ? s.p : (s.p + 0.5), s0.P1, s0.Z1, s0.P2, s0.Z2));
397  }
398  sort(stack.begin(), stack.end(), vpMbScanLineSegmentComparator());
399 
400  int new_ID = stack.empty() ? -1 : stack.front().second.ID;
401 
402  if (new_ID != last_ID || s.type == POINT)
403  {
404  if (s.b_sample_Y)
405  switch(s.type)
406  {
407  case POINT:
408  if (new_ID == -1 || s.Z1 - depthTreshold <= stack.front().first)
409  visibility_samples[s.edge].insert((int)y);
410  break;
411  case START:
412  if (new_ID == s.ID)
413  visibility_samples[s.edge].insert((int)y);
414  break;
415  case END:
416  if (last_ID == s.ID)
417  visibility_samples[s.edge].insert((int)y);
418  break;
419  }
420 
421  // This part will only be used for MbKltTracking
422  if (last_ID != -1)
423  {
424  const unsigned int x0 = std::max<unsigned int>(0, (unsigned int)(std::ceil(last_visible.p)));
425  const double x1 = std::min<double>(w, s.p);
426  for(unsigned int x = x0 + maskBorder ; x < x1 - maskBorder; ++x)
427  {
428  primitive_ids[(unsigned int)y][(unsigned int)x] = last_visible.ID;
429 
430  if(maskBorder != 0)
431  maskY[(unsigned int)y][(unsigned int)x] = 255;
432  else
433  mask[(unsigned int)y][(unsigned int)x] = 255;
434  }
435  }
436 
437  last_ID = new_ID;
438  if (!stack.empty())
439  {
440  last_visible = stack.front().second;
441  last_visible.p = s.p;
442  }
443  }
444  }
445  }
446 
447  // X
448  last_ID = -1;
449  for(unsigned int x = 0 ; x < scanlinesX.size() ; ++x)
450  {
451  std::vector<vpMbScanLineSegment> &scanline = scanlinesX[x];
452  sort(scanline.begin(), scanline.end(), vpMbScanLineSegmentComparator());
453 
454  std::vector<std::pair<double, vpMbScanLineSegment> > stack;
455  for(size_t i = 0 ; i < scanline.size() ; ++i)
456  {
457  const vpMbScanLineSegment &s = scanline[i];
458 
459  switch(s.type)
460  {
461  case START:
462  stack.push_back(std::make_pair(s.Z1, s));
463  break;
464  case END:
465  for(size_t j = 0 ; j < stack.size() ; ++j)
466  if (stack[j].second.ID == s.ID)
467  {
468  stack[j] = stack.back();
469  stack.pop_back();
470  break;
471  }
472  break;
473  case POINT:
474  break;
475  }
476 
477  for(size_t j = 0 ; j < stack.size() ; ++j)
478  {
479  const vpMbScanLineSegment &s0 = stack[j].second;
480  stack[j].first = mix(s0.Z1, s0.Z2, getAlpha(s.type == POINT ? s.p : (s.p + 0.5), s0.P1, s0.Z1, s0.P2, s0.Z2));
481  }
482  sort(stack.begin(), stack.end(), vpMbScanLineSegmentComparator());
483 
484  int new_ID = stack.empty() ? -1 : stack.front().second.ID;
485 
486  if (new_ID != last_ID || s.type == POINT)
487  {
488  if (!s.b_sample_Y)
489  switch(s.type)
490  {
491  case POINT:
492  if (new_ID == -1 || s.Z1 - depthTreshold <= stack.front().first)
493  visibility_samples[s.edge].insert((int)x);
494  break;
495  case START:
496  if (new_ID == s.ID)
497  visibility_samples[s.edge].insert((int)x);
498  break;
499  case END:
500  if (last_ID == s.ID)
501  visibility_samples[s.edge].insert((int)x);
502  break;
503  }
504 
505  // This part will only be used for MbKltTracking
506  if (maskBorder != 0 && last_ID != -1)
507  {
508  const unsigned int y0 = std::max<unsigned int>(0, (unsigned int)(std::ceil(last_visible.p)));
509  const double y1 = std::min<double>(h, s.p);
510  for(unsigned int y = y0 + maskBorder ; y < y1 - maskBorder; ++y)
511  {
512  //primitive_ids[(unsigned int)y][(unsigned int)x] = last_visible.ID;
513  maskX[(unsigned int)y][(unsigned int)x] = 255;
514  }
515  }
516 
517  last_ID = new_ID;
518  if (!stack.empty())
519  {
520  last_visible = stack.front().second;
521  last_visible.p = s.p;
522  }
523  }
524  }
525  }
526 
527  if(maskBorder != 0)
528  for(unsigned int i = 0 ; i < h ; i++)
529  for(unsigned int j = 0 ; j < w ; j++)
530  if(maskX[i][j] == 255 && maskY[i][j] == 255)
531  mask[i][j] = 255;
532 
533 #if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI)) && defined(DEBUG_DISP)
534  if(!dispMaskDebug->isInitialised()){
535  dispMaskDebug->init(mask, 800, 600);
536  }
537 
538  vpDisplay::display(mask);
539 
540  for(unsigned int ID = 0 ; ID < polygons.size() ; ++ID)
541  {
542  for(unsigned int i = 0 ; i < polygons[ID]->size() ; i++){
543  vpPoint p1 = (*(polygons[ID]))[i].first;
544  vpPoint p2 = (*(polygons[ID]))[(i+1)%polygons[ID]->size()].first;
545  double i1=0,j1=0,i2=0,j2=0;
546  p1.project();
547  p2.project();
550 
551  vpDisplay::displayLine(mask,i1,j1,i2,j2,vpColor::red,3);
552  }
553  }
554 
555  vpDisplay::flush(mask);
556 
557 
558  if(!dispLineDebug->isInitialised()){
559  linedebugImg.resize(h,w,0);
560  dispLineDebug->init(linedebugImg, 800, 100);
561  }
562  vpDisplay::display(linedebugImg);
563 #endif
564 
565 }
566 
575 void
576 vpMbScanLine::queryLineVisibility(const vpPoint &a, const vpPoint &b,
577  std::vector<std::pair<vpPoint, vpPoint> > &lines,
578  const bool &displayResults)
579 {
580  vpColVector _a, _b;
581  createVectorFromPoint(a, _a, K);
582  createVectorFromPoint(b, _b, K);
583 
584  double x0 = _a[0] / _a[2];
585  double y0 = _a[1] / _a[2];
586  double z0 = _a[2];
587  double x1 = _b[0] / _b[2];
588  double y1 = _b[1] / _b[2];
589  double z1 = _b[2];
590 
591  vpMbScanLineEdge edge = makeMbScanLineEdge(a, b);
592  lines.clear();
593 
594  if(displayResults){
595 #if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI)) && defined(DEBUG_DISP)
596  double i1(0.0), j1(0.0), i2(0.0), j2(0.0);
597  vpPoint a_(a), b_(b);
598  a_.project();
599  b_.project();
600  vpMeterPixelConversion::convertPoint(K,a_.get_x(), a_.get_y(),j1,i1);
601  vpMeterPixelConversion::convertPoint(K,b_.get_x(), b_.get_y(),j2,i2);
602 
603  vpDisplay::displayLine(linedebugImg,i1,j1,i2,j2,vpColor::yellow,3);
604 #endif
605  }
606 
607  if (!visibility_samples.count(edge))
608  return;
609 
610  // Initialized as the biggest difference between the two points is on the X-axis
611  double *v0(&x0), *w0(&z0);
612  double *v1(&x1), *w1(&z1);
613  unsigned int size(w);
614 
615  if (std::fabs(y0 - y1) > std::fabs(x0 - x1)) // Test if the biggest difference is on the Y-axis
616  {
617  v0 = &y0;
618  v1 = &y1;
619  size = h;
620  }
621 
622  // Cannot call swap(a,b) since a and b are const
623  // The fix consists in 2 new points that contain the right points
624  vpPoint a_;
625  vpPoint b_;
626 
627  if (*v0 > *v1)
628  {
629  std::swap(v0, v1);
630  std::swap(w0, w1);
631  //std::swap(a, b);
632  // Cannot call swap(a,b) since a and b are const
633  // Instead of swap we set the right address of the corresponding pointers
634  a_ = b;
635  b_ = a;
636  }
637  else {
638  a_ = a;
639  b_ = b;
640  }
641 
642  //if (*v0 >= size - 1 || *v1 < 0 || *v1 == *v0)
643  if (*v0 >= size - 1 || *v1 < 0 || std::fabs(*v1 - *v0) <= std::numeric_limits<double>::epsilon())
644  return;
645 
646  const int _v0 = std::max(0, int(std::ceil(*v0)));
647  const int _v1 = std::min<int>((int)(size - 1), (int)(std::ceil(*v1) - 1));
648 
649  const std::set<int> &visible_samples = visibility_samples[edge];
650  int last = _v0;
651  vpPoint line_start;
652  vpPoint line_end;
653  bool b_line_started = false;
654  for(std::set<int>::const_iterator it = visible_samples.begin() ; it != visible_samples.end() ; ++it)
655  {
656  const int v = *it;
657  const double alpha = getAlpha(v, (*v0) * (*w0), (*w0), (*v1) * (*w1), (*w1));
658  //const vpPoint p = mix(a, b, alpha);
659  const vpPoint p = mix(a_, b_, alpha);
660  if (last + 1 != v)
661  {
662  if(b_line_started)
663  lines.push_back(std::make_pair(line_start, line_end));
664  b_line_started = false;
665  }
666  if (v == _v0)
667  {
668  //line_start = a;
669  line_start = a_;
670  line_end = p;
671  b_line_started = true;
672  }
673  else if (v == _v1)
674  {
675  //line_end = b;
676  line_end = b_;
677  if (!b_line_started)
678  line_start = p;
679  b_line_started = true;
680  }
681  else
682  {
683  line_end = p;
684  if (!b_line_started)
685  line_start = p;
686  b_line_started = true;
687  }
688  last = v;
689  }
690  if (b_line_started)
691  lines.push_back(std::make_pair(line_start, line_end));
692 
693  if(displayResults){
694 #if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI)) && defined(DEBUG_DISP)
695  double i1(0.0), j1(0.0), i2(0.0), j2(0.0);
696  for(unsigned int i = 0 ; i < lines.size() ; i++){
697  lines[i].first.project();
698  lines[i].second.project();
699  vpMeterPixelConversion::convertPoint(K,lines[i].first.get_x(), lines[i].first.get_y(),j1,i1);
700  vpMeterPixelConversion::convertPoint(K,lines[i].second.get_x(), lines[i].second.get_y(),j2,i2);
701 
702  vpDisplay::displayLine(linedebugImg,i1,j1,i2,j2,vpColor::red,3);
703  }
704  vpDisplay::flush(linedebugImg);
705 #endif
706  }
707 }
708 
717 vpMbScanLine::vpMbScanLineEdge
718 vpMbScanLine::makeMbScanLineEdge(const vpPoint &a, const vpPoint &b)
719 {
720  vpColVector _a(3);
721  vpColVector _b(3);
722 
723  _a[0] = std::ceil((a.get_X() * 1e8) * 1e-6);
724  _a[1] = std::ceil((a.get_Y() * 1e8) * 1e-6);
725  _a[2] = std::ceil((a.get_Z() * 1e8) * 1e-6);
726 
727  _b[0] = std::ceil((b.get_X() * 1e8) * 1e-6);
728  _b[1] = std::ceil((b.get_Y() * 1e8) * 1e-6);
729  _b[2] = std::ceil((b.get_Z() * 1e8) * 1e-6);
730 
731  bool b_comp = false;
732  for(unsigned int i = 0 ; i < 3 ; ++i)
733  if (_a[i] < _b[i])
734  {
735  b_comp = true;
736  break;
737  }
738  else if(_a[i] > _b[i])
739  break;
740 
741  if (b_comp)
742  return std::make_pair(_a, _b);
743 
744  return std::make_pair(_b, _a);
745 }
746 
754 void
755 vpMbScanLine::createVectorFromPoint(const vpPoint &p, vpColVector &v, const vpCameraParameters &K)
756 {
757  v = vpColVector(3);
758 
759  v[0] = p.get_X() * K.get_px() + K.get_u0() * p.get_Z();
760  v[1] = p.get_Y() * K.get_py() + K.get_v0() * p.get_Z();
761  v[2] = p.get_Z();
762 }
763 
773 double
774 vpMbScanLine::getAlpha(double x, double X0, double Z0, double X1, double Z1)
775 {
776  const double N = X0 - x * Z0;
777  const double D = x * (Z1 - Z0) - (X1 - X0);
778  double alpha = N / D;
779  if (vpMath::isNaN(alpha) || vpMath::isInf(alpha))
780  return 0.0;
781 
782  alpha = std::min(1.0,alpha);
783  alpha = std::max(0.0,alpha);
784  return alpha;
785 }
786 
796 double
797 vpMbScanLine::mix(double a, double b, double alpha)
798 {
799  return a + (b - a) * alpha;
800 }
801 
811 vpPoint
812 vpMbScanLine::mix(const vpPoint &a, const vpPoint &b, double alpha)
813 {
814  vpPoint res;
815  res.set_X(a.get_X() + ( b.get_X() - a.get_X() ) * alpha);
816  res.set_Y(a.get_Y() + ( b.get_Y() - a.get_Y() ) * alpha);
817  res.set_Z(a.get_Z() + ( b.get_Z() - a.get_Z() ) * alpha);
818 
819  return res;
820 }
821 
830 double
831 vpMbScanLine::norm(const vpPoint &a, const vpPoint &b)
832 {
833  return sqrt(vpMath::sqr(a.get_X()-b.get_X()) + vpMath::sqr(a.get_Y() - b.get_Y()) + vpMath::sqr(a.get_Z() - b.get_Z()));
834 }
835 
836 #endif
double get_u0() const
static void convertPoint(const vpCameraParameters &cam, const double &x, const double &y, double &u, double &v)
Point coordinates conversion from normalized coordinates in meter to pixel coordinates ...
static bool isNaN(const double value)
Definition: vpMath.cpp:85
Display for windows using GDI (available on any windows 32 platform).
Definition: vpDisplayGDI.h:128
Use the X11 console to display images on unix-like OS. Thus to enable this class X11 should be instal...
Definition: vpDisplayX.h:153
double get_py() const
double get_y() const
Get the point y coordinate in the image plane.
Definition: vpPoint.cpp:458
static bool isInf(const double value)
Definition: vpMath.cpp:112
void set_X(const double X)
Set the point X coordinate in the camera frame.
Definition: vpPoint.cpp:478
static void flush(const vpImage< unsigned char > &I)
static const vpColor red
Definition: vpColor.h:163
Class that defines what is a point.
Definition: vpPoint.h:59
void set_Z(const double Z)
Set the point Z coordinate in the camera frame.
Definition: vpPoint.cpp:482
double get_v0() const
static double sqr(double x)
Definition: vpMath.h:110
static void display(const vpImage< unsigned char > &I)
Generic class defining intrinsic camera parameters.
void set_Y(const double Y)
Set the point Y coordinate in the camera frame.
Definition: vpPoint.cpp:480
double get_x() const
Get the point x coordinate in the image plane.
Definition: vpPoint.cpp:456
double get_px() const
double get_Y() const
Get the point Y coordinate in the camera frame.
Definition: vpPoint.cpp:440
double get_Z() const
Get the point Z coordinate in the camera frame.
Definition: vpPoint.cpp:442
Implementation of column vector and the associated operations.
Definition: vpColVector.h:72
static void displayLine(const vpImage< unsigned char > &I, const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, unsigned int thickness=1)
static const vpColor yellow
Definition: vpColor.h:171
double get_X() const
Get the point X coordinate in the camera frame.
Definition: vpPoint.cpp:438