Visual Servoing Platform  version 3.2.0 under development (2019-01-22)
vpMbScanLine.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2019 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2 of the License, or
9  * (at your option) any later version.
10  * See the file LICENSE.txt at the root directory of this source
11  * distribution for additional information about the GNU GPL.
12  *
13  * For using ViSP with software that can not be combined with the GNU
14  * GPL, please contact Inria about acquiring a ViSP Professional
15  * Edition License.
16  *
17  * See http://visp.inria.fr for more information.
18  *
19  * This software was developed at:
20  * Inria Rennes - Bretagne Atlantique
21  * Campus Universitaire de Beaulieu
22  * 35042 Rennes Cedex
23  * France
24  *
25  * If you have questions regarding the use of this file, please contact
26  * Inria at visp@inria.fr
27  *
28  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30  *
31  * Description:
32  * Make the complete tracking of an object by using its CAD model
33  *
34  * Authors:
35  * Aurelien Yol
36  *
37  *****************************************************************************/
38 
39 #include <visp3/core/vpConfig.h>
40 
41 #if defined _MSC_VER && _MSC_VER >= 1200
42 #define NOMINMAX
43 #endif
44 
45 #include <algorithm>
46 #include <cmath>
47 #include <cstddef>
48 #include <iostream>
49 #include <utility>
50 
51 #include <visp3/core/vpMeterPixelConversion.h>
52 #include <visp3/mbt/vpMbScanLine.h>
53 
54 #if defined(DEBUG_DISP)
55 #include <visp3/gui/vpDisplayGDI.h>
56 #include <visp3/gui/vpDisplayX.h>
57 #endif
58 
59 #ifndef DOXYGEN_SHOULD_SKIP_THIS
60 
61 vpMbScanLine::vpMbScanLine()
62  : w(0), h(0), K(), maskBorder(0), mask(), primitive_ids(), visibility_samples(), depthTreshold(1e-06)
63 #if defined(DEBUG_DISP)
64  ,
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)
81  delete dispLineDebug;
82  if (dispMaskDebug != NULL)
83  delete dispMaskDebug;
84 #endif
85 }
96 void vpMbScanLine::drawLineY(const vpColVector &a, const vpColVector &b, const vpMbScanLineEdge &edge, 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  std::swap(x0, x1);
107  std::swap(y0, y1);
108  std::swap(z0, z1);
109  }
110 
111  // if (y0 >= h - 1 || y1 < 0 || y1 == y0)
112  if (y0 >= h - 1 || y1 < 0 || std::fabs(y1 - y0) <= std::numeric_limits<double>::epsilon())
113  return;
114 
115  const unsigned int _y0 = (std::max)((unsigned int)0, (unsigned int)(std::ceil(y0)));
116  const double _y1 = (std::min)((double)h, (double)y1);
117 
118  const bool b_sample_Y = (std::fabs(y0 - y1) > std::fabs(x0 - x1));
119 
120  for (unsigned int y = _y0; y < _y1; ++y) {
121  const double x = x0 + (x1 - x0) * (y - y0) / (y1 - y0);
122  const double alpha = getAlpha(y, y0 * z0, z0, y1 * z1, z1);
123  vpMbScanLineSegment s;
124  s.p = x;
125  s.type = POINT;
126  s.Z2 = s.Z1 = mix(z0, z1, alpha);
127  s.P2 = s.P1 = s.p * s.Z1;
128  s.ID = ID;
129  s.edge = edge;
130  s.b_sample_Y = b_sample_Y;
131  scanlines[y].push_back(s);
132  }
133 }
134 
145 void vpMbScanLine::drawLineX(const vpColVector &a, const vpColVector &b, const vpMbScanLineEdge &edge, const int ID,
146  std::vector<std::vector<vpMbScanLineSegment> > &scanlines)
147 {
148  double x0 = a[0] / a[2];
149  double y0 = a[1] / a[2];
150  double z0 = a[2];
151  double x1 = b[0] / b[2];
152  double y1 = b[1] / b[2];
153  double z1 = b[2];
154  if (x0 > x1) {
155  std::swap(x0, x1);
156  std::swap(y0, y1);
157  std::swap(z0, z1);
158  }
159 
160  // if (x0 >= w - 1 || x1 < 0 || x1 == x0)
161  if (x0 >= w - 1 || x1 < 0 || std::fabs(x1 - x0) <= std::numeric_limits<double>::epsilon())
162  return;
163 
164  const unsigned int _x0 = (std::max)((unsigned int)0, (unsigned int)(std::ceil(x0)));
165  const double _x1 = (std::min)((double)w, (double)x1);
166 
167  const bool b_sample_Y = (std::fabs(y0 - y1) > std::fabs(x0 - x1));
168 
169  for (unsigned int x = _x0; x < _x1; ++x) {
170  const double y = y0 + (y1 - y0) * (x - x0) / (x1 - x0);
171  const double alpha = getAlpha(x, x0 * z0, z0, x1 * z1, z1);
172  vpMbScanLineSegment s;
173  s.p = y;
174  s.type = POINT;
175  s.Z2 = s.Z1 = mix(z0, z1, alpha);
176  s.P2 = s.P1 = s.p * s.Z1;
177  s.ID = ID;
178  s.edge = edge;
179  s.b_sample_Y = b_sample_Y;
180  scanlines[x].push_back(s);
181  }
182 }
183 
191 void vpMbScanLine::drawPolygonY(const std::vector<std::pair<vpPoint, unsigned int> > &polygon, const int ID,
192  std::vector<std::vector<vpMbScanLineSegment> > &scanlines)
193 {
194  if (polygon.size() < 2)
195  return;
196 
197  if (polygon.size() == 2) {
198  vpColVector p1, p2;
199  createVectorFromPoint(polygon.front().first, p1, K);
200  createVectorFromPoint(polygon.back().first, p2, K);
201 
202  drawLineY(p1, p2, makeMbScanLineEdge(polygon.front().first, polygon.back().first), ID, scanlines);
203  return;
204  }
205 
206  std::vector<std::vector<vpMbScanLineSegment> > local_scanlines;
207  local_scanlines.resize(h);
208 
209  for (size_t i = 0; i < polygon.size(); ++i) {
210  vpColVector p1, p2;
211  createVectorFromPoint(polygon[i].first, p1, K);
212  createVectorFromPoint(polygon[(i + 1) % polygon.size()].first, p2, K);
213 
214  drawLineY(p1, p2, makeMbScanLineEdge(polygon[i].first, polygon[(i + 1) % polygon.size()].first), ID,
215  local_scanlines);
216  }
217 
218  createScanLinesFromLocals(scanlines, local_scanlines, h);
219 }
220 
228 void vpMbScanLine::drawPolygonX(const std::vector<std::pair<vpPoint, unsigned int> > &polygon, const int ID,
229  std::vector<std::vector<vpMbScanLineSegment> > &scanlines)
230 {
231  if (polygon.size() < 2)
232  return;
233 
234  if (polygon.size() == 2) {
235  vpColVector p1, p2;
236  createVectorFromPoint(polygon.front().first, p1, K);
237  createVectorFromPoint(polygon.back().first, p2, K);
238 
239  drawLineX(p1, p2, makeMbScanLineEdge(polygon.front().first, polygon.back().first), ID, scanlines);
240  return;
241  }
242 
243  std::vector<std::vector<vpMbScanLineSegment> > local_scanlines;
244  local_scanlines.resize(w);
245 
246  for (size_t i = 0; i < polygon.size(); ++i) {
247  vpColVector p1, p2;
248  createVectorFromPoint(polygon[i].first, p1, K);
249  createVectorFromPoint(polygon[(i + 1) % polygon.size()].first, p2, K);
250 
251  drawLineX(p1, p2, makeMbScanLineEdge(polygon[i].first, polygon[(i + 1) % polygon.size()].first), ID,
252  local_scanlines);
253  }
254 
255  createScanLinesFromLocals(scanlines, local_scanlines, w);
256 }
257 
267 void vpMbScanLine::createScanLinesFromLocals(std::vector<std::vector<vpMbScanLineSegment> > &scanlines,
268  std::vector<std::vector<vpMbScanLineSegment> > &localScanlines,
269  const unsigned int &size)
270 {
271  for (unsigned int j = 0; j < size; ++j) {
272  std::vector<vpMbScanLineSegment> &scanline = localScanlines[j];
273  sort(scanline.begin(), scanline.end(),
274  vpMbScanLineSegmentComparator()); // Not sure its necessary
275 
276  bool b_start = true;
277  for (size_t i = 0; i < scanline.size(); ++i) {
278  vpMbScanLineSegment s = scanline[i];
279  if (b_start) {
280  s.type = START;
281  s.P1 = s.p * s.Z1;
282  b_start = false;
283  } else {
284  vpMbScanLineSegment &prev = scanlines[j].back();
285  s.type = END;
286  s.P1 = prev.P1;
287  s.Z1 = prev.Z1;
288  s.P2 = s.p * s.Z2;
289  prev.P2 = s.P2;
290  prev.Z2 = s.Z2;
291  b_start = true;
292  }
293  scanlines[j].push_back(s);
294  }
295  }
296 }
297 
307 void vpMbScanLine::drawScene(const std::vector<std::vector<std::pair<vpPoint, unsigned int> > *> &polygons,
308  std::vector<int> listPolyIndices, const vpCameraParameters &cam, unsigned int width,
309  unsigned int height)
310 {
311  this->w = width;
312  this->h = height;
313  this->K = cam;
314 
315  visibility_samples.clear();
316 
317  std::vector<std::vector<vpMbScanLineSegment> > scanlinesY;
318  scanlinesY.resize(h);
319  std::vector<std::vector<vpMbScanLineSegment> > scanlinesX;
320  scanlinesX.resize(w);
321 
322  mask.resize(h, w, 0);
323 
324  vpImage<unsigned char> maskY(h, w, 0);
325  vpImage<unsigned char> maskX(h, w, 0);
326 
327  primitive_ids.resize(h, w, -1);
328 
329  for (unsigned int ID = 0; ID < polygons.size(); ++ID) {
330  drawPolygonY(*(polygons[ID]), listPolyIndices[ID], scanlinesY);
331  drawPolygonX(*(polygons[ID]), listPolyIndices[ID], scanlinesX);
332  }
333 
334  // Y
335  int last_ID = -1;
336  vpMbScanLineSegment last_visible;
337  for (unsigned int y = 0; y < scanlinesY.size(); ++y) {
338  std::vector<vpMbScanLineSegment> &scanline = scanlinesY[y];
339  sort(scanline.begin(), scanline.end(), vpMbScanLineSegmentComparator());
340 
341  std::vector<std::pair<double, vpMbScanLineSegment> > stack;
342  for (size_t i = 0; i < scanline.size(); ++i) {
343  const vpMbScanLineSegment &s = scanline[i];
344 
345  switch (s.type) {
346  case START:
347  stack.push_back(std::make_pair(s.Z1, s));
348  break;
349  case END:
350  for (size_t j = 0; j < stack.size(); ++j)
351  if (stack[j].second.ID == s.ID) {
352  if (j != stack.size() - 1)
353  stack[j] = stack.back();
354  stack.pop_back();
355  break;
356  }
357  break;
358  case POINT:
359  break;
360  }
361 
362  for (size_t j = 0; j < stack.size(); ++j) {
363  const vpMbScanLineSegment &s0 = stack[j].second;
364  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));
365  }
366  sort(stack.begin(), stack.end(), vpMbScanLineSegmentComparator());
367 
368  int new_ID = stack.empty() ? -1 : stack.front().second.ID;
369 
370  if (new_ID != last_ID || s.type == POINT) {
371  if (s.b_sample_Y)
372  switch (s.type) {
373  case POINT:
374  if (new_ID == -1 || s.Z1 - depthTreshold <= stack.front().first)
375  visibility_samples[s.edge].insert((int)y);
376  break;
377  case START:
378  if (new_ID == s.ID)
379  visibility_samples[s.edge].insert((int)y);
380  break;
381  case END:
382  if (last_ID == s.ID)
383  visibility_samples[s.edge].insert((int)y);
384  break;
385  }
386 
387  // This part will only be used for MbKltTracking
388  if (last_ID != -1) {
389  const unsigned int x0 = (std::max)((unsigned int)0, (unsigned int)(std::ceil(last_visible.p)));
390  const double x1 = (std::min)((double)w, (double)s.p);
391  for (unsigned int x = x0 + maskBorder; x < x1 - maskBorder; ++x) {
392  primitive_ids[(unsigned int)y][(unsigned int)x] = last_visible.ID;
393 
394  if (maskBorder != 0)
395  maskY[(unsigned int)y][(unsigned int)x] = 255;
396  else
397  mask[(unsigned int)y][(unsigned int)x] = 255;
398  }
399  }
400 
401  last_ID = new_ID;
402  if (!stack.empty()) {
403  last_visible = stack.front().second;
404  last_visible.p = s.p;
405  }
406  }
407  }
408  }
409 
410  // X
411  last_ID = -1;
412  for (unsigned int x = 0; x < scanlinesX.size(); ++x) {
413  std::vector<vpMbScanLineSegment> &scanline = scanlinesX[x];
414  sort(scanline.begin(), scanline.end(), vpMbScanLineSegmentComparator());
415 
416  std::vector<std::pair<double, vpMbScanLineSegment> > stack;
417  for (size_t i = 0; i < scanline.size(); ++i) {
418  const vpMbScanLineSegment &s = scanline[i];
419 
420  switch (s.type) {
421  case START:
422  stack.push_back(std::make_pair(s.Z1, s));
423  break;
424  case END:
425  for (size_t j = 0; j < stack.size(); ++j)
426  if (stack[j].second.ID == s.ID) {
427  if (j != stack.size() - 1)
428  stack[j] = stack.back();
429  stack.pop_back();
430  break;
431  }
432  break;
433  case POINT:
434  break;
435  }
436 
437  for (size_t j = 0; j < stack.size(); ++j) {
438  const vpMbScanLineSegment &s0 = stack[j].second;
439  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));
440  }
441  sort(stack.begin(), stack.end(), vpMbScanLineSegmentComparator());
442 
443  int new_ID = stack.empty() ? -1 : stack.front().second.ID;
444 
445  if (new_ID != last_ID || s.type == POINT) {
446  if (!s.b_sample_Y)
447  switch (s.type) {
448  case POINT:
449  if (new_ID == -1 || s.Z1 - depthTreshold <= stack.front().first)
450  visibility_samples[s.edge].insert((int)x);
451  break;
452  case START:
453  if (new_ID == s.ID)
454  visibility_samples[s.edge].insert((int)x);
455  break;
456  case END:
457  if (last_ID == s.ID)
458  visibility_samples[s.edge].insert((int)x);
459  break;
460  }
461 
462  // This part will only be used for MbKltTracking
463  if (maskBorder != 0 && last_ID != -1) {
464  const unsigned int y0 = (std::max)((unsigned int)0, (unsigned int)(std::ceil(last_visible.p)));
465  const double y1 = (std::min)((double)h, (double)s.p);
466  for (unsigned int y = y0 + maskBorder; y < y1 - maskBorder; ++y) {
467  // primitive_ids[(unsigned int)y][(unsigned int)x] =
468  // last_visible.ID;
469  maskX[(unsigned int)y][(unsigned int)x] = 255;
470  }
471  }
472 
473  last_ID = new_ID;
474  if (!stack.empty()) {
475  last_visible = stack.front().second;
476  last_visible.p = s.p;
477  }
478  }
479  }
480  }
481 
482  if (maskBorder != 0)
483  for (unsigned int i = 0; i < h; i++)
484  for (unsigned int j = 0; j < w; j++)
485  if (maskX[i][j] == 255 && maskY[i][j] == 255)
486  mask[i][j] = 255;
487 
488 #if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI)) && defined(DEBUG_DISP)
489  if (!dispMaskDebug->isInitialised()) {
490  dispMaskDebug->init(mask, 800, 600);
491  }
492 
493  vpDisplay::display(mask);
494 
495  for (unsigned int ID = 0; ID < polygons.size(); ++ID) {
496  for (unsigned int i = 0; i < polygons[ID]->size(); i++) {
497  vpPoint p1 = (*(polygons[ID]))[i].first;
498  vpPoint p2 = (*(polygons[ID]))[(i + 1) % polygons[ID]->size()].first;
499  double i1 = 0, j1 = 0, i2 = 0, j2 = 0;
500  p1.project();
501  p2.project();
502  vpMeterPixelConversion::convertPoint(K, p1.get_x(), p1.get_y(), j1, i1);
503  vpMeterPixelConversion::convertPoint(K, p2.get_x(), p2.get_y(), j2, i2);
504 
505  vpDisplay::displayLine(mask, i1, j1, i2, j2, vpColor::red, 3);
506  }
507  }
508 
509  vpDisplay::flush(mask);
510 
511  if (!dispLineDebug->isInitialised()) {
512  linedebugImg.resize(h, w, 0);
513  dispLineDebug->init(linedebugImg, 800, 100);
514  }
515  vpDisplay::display(linedebugImg);
516 #endif
517 }
518 
529 void vpMbScanLine::queryLineVisibility(const vpPoint &a, const vpPoint &b,
530  std::vector<std::pair<vpPoint, vpPoint> > &lines, const bool &displayResults)
531 {
532  vpColVector _a, _b;
533  createVectorFromPoint(a, _a, K);
534  createVectorFromPoint(b, _b, K);
535 
536  double x0 = _a[0] / _a[2];
537  double y0 = _a[1] / _a[2];
538  double z0 = _a[2];
539  double x1 = _b[0] / _b[2];
540  double y1 = _b[1] / _b[2];
541  double z1 = _b[2];
542 
543  vpMbScanLineEdge edge = makeMbScanLineEdge(a, b);
544  lines.clear();
545 
546  if (displayResults) {
547 #if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI)) && defined(DEBUG_DISP)
548  double i1(0.0), j1(0.0), i2(0.0), j2(0.0);
549  vpPoint a_(a), b_(b);
550  a_.project();
551  b_.project();
552  vpMeterPixelConversion::convertPoint(K, a_.get_x(), a_.get_y(), j1, i1);
553  vpMeterPixelConversion::convertPoint(K, b_.get_x(), b_.get_y(), j2, i2);
554 
555  vpDisplay::displayLine(linedebugImg, i1, j1, i2, j2, vpColor::yellow, 3);
556 #endif
557  }
558 
559  if (!visibility_samples.count(edge))
560  return;
561 
562  // Initialized as the biggest difference between the two points is on the
563  // X-axis
564  double *v0(&x0), *w0(&z0);
565  double *v1(&x1), *w1(&z1);
566  unsigned int size(w);
567 
568  if (std::fabs(y0 - y1) > std::fabs(x0 - x1)) // Test if the biggest difference is on the Y-axis
569  {
570  v0 = &y0;
571  v1 = &y1;
572  size = h;
573  }
574 
575  // Cannot call swap(a,b) since a and b are const
576  // The fix consists in 2 new points that contain the right points
577  vpPoint a_;
578  vpPoint b_;
579 
580  if (*v0 > *v1) {
581  std::swap(v0, v1);
582  std::swap(w0, w1);
583  // std::swap(a, b);
584  // Cannot call swap(a,b) since a and b are const
585  // Instead of swap we set the right address of the corresponding pointers
586  a_ = b;
587  b_ = a;
588  } else {
589  a_ = a;
590  b_ = b;
591  }
592 
593  // if (*v0 >= size - 1 || *v1 < 0 || *v1 == *v0)
594  if (*v0 >= size - 1 || *v1 < 0 || std::fabs(*v1 - *v0) <= std::numeric_limits<double>::epsilon())
595  return;
596 
597  const int _v0 = (std::max)(0, int(std::ceil(*v0)));
598  const int _v1 = (std::min)((int)(size - 1), (int)(std::ceil(*v1) - 1));
599 
600  const std::set<int> &visible_samples = visibility_samples[edge];
601  int last = _v0;
602  vpPoint line_start;
603  vpPoint line_end;
604  bool b_line_started = false;
605  for (std::set<int>::const_iterator it = visible_samples.begin(); it != visible_samples.end(); ++it) {
606  const int v = *it;
607  const double alpha = getAlpha(v, (*v0) * (*w0), (*w0), (*v1) * (*w1), (*w1));
608  // const vpPoint p = mix(a, b, alpha);
609  const vpPoint p = mix(a_, b_, alpha);
610  if (last + 1 != v) {
611  if (b_line_started)
612  lines.push_back(std::make_pair(line_start, line_end));
613  b_line_started = false;
614  }
615  if (v == _v0) {
616  // line_start = a;
617  line_start = a_;
618  line_end = p;
619  b_line_started = true;
620  } else if (v == _v1) {
621  // line_end = b;
622  line_end = b_;
623  if (!b_line_started)
624  line_start = p;
625  b_line_started = true;
626  } else {
627  line_end = p;
628  if (!b_line_started)
629  line_start = p;
630  b_line_started = true;
631  }
632  last = v;
633  }
634  if (b_line_started)
635  lines.push_back(std::make_pair(line_start, line_end));
636 
637  if (displayResults) {
638 #if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI)) && defined(DEBUG_DISP)
639  double i1(0.0), j1(0.0), i2(0.0), j2(0.0);
640  for (unsigned int i = 0; i < lines.size(); i++) {
641  lines[i].first.project();
642  lines[i].second.project();
643  vpMeterPixelConversion::convertPoint(K, lines[i].first.get_x(), lines[i].first.get_y(), j1, i1);
644  vpMeterPixelConversion::convertPoint(K, lines[i].second.get_x(), lines[i].second.get_y(), j2, i2);
645 
646  vpDisplay::displayLine(linedebugImg, i1, j1, i2, j2, vpColor::red, 3);
647  }
648  vpDisplay::flush(linedebugImg);
649 #endif
650  }
651 }
652 
661 vpMbScanLine::vpMbScanLineEdge vpMbScanLine::makeMbScanLineEdge(const vpPoint &a, const vpPoint &b)
662 {
663  vpColVector _a(3);
664  vpColVector _b(3);
665 
666  _a[0] = std::ceil((a.get_X() * 1e8) * 1e-6);
667  _a[1] = std::ceil((a.get_Y() * 1e8) * 1e-6);
668  _a[2] = std::ceil((a.get_Z() * 1e8) * 1e-6);
669 
670  _b[0] = std::ceil((b.get_X() * 1e8) * 1e-6);
671  _b[1] = std::ceil((b.get_Y() * 1e8) * 1e-6);
672  _b[2] = std::ceil((b.get_Z() * 1e8) * 1e-6);
673 
674  bool b_comp = false;
675  for (unsigned int i = 0; i < 3; ++i)
676  if (_a[i] < _b[i]) {
677  b_comp = true;
678  break;
679  } else if (_a[i] > _b[i])
680  break;
681 
682  if (b_comp)
683  return std::make_pair(_a, _b);
684 
685  return std::make_pair(_b, _a);
686 }
687 
695 void vpMbScanLine::createVectorFromPoint(const vpPoint &p, vpColVector &v, const vpCameraParameters &K)
696 {
697  v = vpColVector(3);
698 
699  v[0] = p.get_X() * K.get_px() + K.get_u0() * p.get_Z();
700  v[1] = p.get_Y() * K.get_py() + K.get_v0() * p.get_Z();
701  v[2] = p.get_Z();
702 }
703 
713 double vpMbScanLine::getAlpha(double x, double X0, double Z0, double X1, double Z1)
714 {
715  const double N = X0 - x * Z0;
716  const double D = x * (Z1 - Z0) - (X1 - X0);
717  double alpha = N / D;
718  if (vpMath::isNaN(alpha) || vpMath::isInf(alpha))
719  return 0.0;
720 
721  alpha = (std::min)(1.0, alpha);
722  alpha = (std::max)(0.0, alpha);
723  return alpha;
724 }
725 
735 double vpMbScanLine::mix(double a, double b, double alpha) { return a + (b - a) * alpha; }
736 
746 vpPoint vpMbScanLine::mix(const vpPoint &a, const vpPoint &b, double alpha)
747 {
748  vpPoint res;
749  res.set_X(a.get_X() + (b.get_X() - a.get_X()) * alpha);
750  res.set_Y(a.get_Y() + (b.get_Y() - a.get_Y()) * alpha);
751  res.set_Z(a.get_Z() + (b.get_Z() - a.get_Z()) * alpha);
752 
753  return res;
754 }
755 
764 double vpMbScanLine::norm(const vpPoint &a, const vpPoint &b)
765 {
766  return sqrt(vpMath::sqr(a.get_X() - b.get_X()) + vpMath::sqr(a.get_Y() - b.get_Y()) +
767  vpMath::sqr(a.get_Z() - b.get_Z()));
768 }
769 
770 #endif
double get_u0() const
static void convertPoint(const vpCameraParameters &cam, const double &x, const double &y, double &u, double &v)
static bool isNaN(const double value)
Definition: vpMath.cpp:84
Display for windows using GDI (available on any windows 32 platform).
Definition: vpDisplayGDI.h:129
Use the X11 console to display images on unix-like OS. Thus to enable this class X11 should be instal...
Definition: vpDisplayX.h:151
double get_py() const
double get_y() const
Get the point y coordinate in the image plane.
Definition: vpPoint.cpp:431
static bool isInf(const double value)
Definition: vpMath.cpp:110
void set_X(const double X)
Set the point X coordinate in the camera frame.
Definition: vpPoint.cpp:452
static void flush(const vpImage< unsigned char > &I)
static const vpColor red
Definition: vpColor.h:180
Class that defines what is a point.
Definition: vpPoint.h:58
vpCameraParameters cam
The camera parameters.
Definition: vpMbTracker.h:117
void set_Z(const double Z)
Set the point Z coordinate in the camera frame.
Definition: vpPoint.cpp:456
double get_v0() const
static double sqr(double x)
Definition: vpMath.h:108
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:454
double get_x() const
Get the point x coordinate in the image plane.
Definition: vpPoint.cpp:429
double get_px() const
double get_Y() const
Get the point Y coordinate in the camera frame.
Definition: vpPoint.cpp:413
double get_Z() const
Get the point Z coordinate in the camera frame.
Definition: vpPoint.cpp:415
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:188
double get_X() const
Get the point X coordinate in the camera frame.
Definition: vpPoint.cpp:411