Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
vpMbScanLine.cpp
1/****************************************************************************
2 *
3 * ViSP, open source Visual Servoing Platform software.
4 * Copyright (C) 2005 - 2023 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 https://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
61vpMbScanLine::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
77vpMbScanLine::~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}
96void 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 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
145void 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 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
191void 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
228void 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
267void 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
307void 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 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 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
529void 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
661vpMbScanLine::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
695void 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
713double 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
735double vpMbScanLine::mix(double a, double b, double alpha) { return a + (b - a) * alpha; }
736
746vpPoint 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
764double 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
Generic class defining intrinsic camera parameters.
Implementation of column vector and the associated operations.
static const vpColor red
Definition vpColor.h:211
static const vpColor yellow
Definition vpColor.h:219
Display for windows using GDI (available on any windows 32 platform).
Use the X11 console to display images on unix-like OS. Thus to enable this class X11 should be instal...
Definition vpDisplayX.h:132
static void display(const vpImage< unsigned char > &I)
static void displayLine(const vpImage< unsigned char > &I, const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, unsigned int thickness=1, bool segment=true)
static void flush(const vpImage< unsigned char > &I)
Definition of the vpImage class member functions.
Definition vpImage.h:135
static bool isNaN(double value)
Definition vpMath.cpp:93
static double sqr(double x)
Definition vpMath.h:124
static bool isInf(double value)
Definition vpMath.cpp:137
static void convertPoint(const vpCameraParameters &cam, const double &x, const double &y, double &u, double &v)
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
Definition vpPoint.h:77
double get_y() const
Get the point y coordinate in the image plane.
Definition vpPoint.cpp:469
double get_Y() const
Get the point cY coordinate in the camera frame.
Definition vpPoint.cpp:451
void set_X(double cX)
Set the point cX coordinate in the camera frame.
Definition vpPoint.cpp:490
double get_x() const
Get the point x coordinate in the image plane.
Definition vpPoint.cpp:467
void set_Y(double cY)
Set the point cY coordinate in the camera frame.
Definition vpPoint.cpp:492
double get_Z() const
Get the point cZ coordinate in the camera frame.
Definition vpPoint.cpp:453
void set_Z(double cZ)
Set the point cZ coordinate in the camera frame.
Definition vpPoint.cpp:494
double get_X() const
Get the point cX coordinate in the camera frame.
Definition vpPoint.cpp:449