39 #include <visp3/core/vpConfig.h>
41 #if defined _MSC_VER && _MSC_VER >= 1200
51 #include <visp3/core/vpMeterPixelConversion.h>
52 #include <visp3/mbt/vpMbScanLine.h>
54 #if defined(DEBUG_DISP)
55 #include <visp3/gui/vpDisplayGDI.h>
56 #include <visp3/gui/vpDisplayX.h>
59 #ifndef DOXYGEN_SHOULD_SKIP_THIS
61 vpMbScanLine::vpMbScanLine()
62 : w(0), h(0), K(), maskBorder(0), mask(), primitive_ids(), visibility_samples(), depthTreshold(1e-06)
63 #if defined(DEBUG_DISP)
65 dispMaskDebug(NULL), dispLineDebug(NULL), linedebugImg()
68 #if defined(VISP_HAVE_X11) && defined(DEBUG_DISP)
71 #elif defined(VISP_HAVE_GDI) && defined(DEBUG_DISP)
77 vpMbScanLine::~vpMbScanLine()
79 #if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI)) && defined(DEBUG_DISP)
80 if (dispLineDebug != NULL)
82 if (dispMaskDebug != NULL)
96 void vpMbScanLine::drawLineY(
const vpColVector &a,
const vpColVector &b,
const vpMbScanLineEdge &edge,
const int ID,
97 std::vector<std::vector<vpMbScanLineSegment> > &scanlines)
99 double x0 = a[0] / a[2];
100 double y0 = a[1] / a[2];
102 double x1 = b[0] / b[2];
103 double y1 = b[1] / b[2];
112 if (y0 >= h - 1 || y1 < 0 || std::fabs(y1 - y0) <= std::numeric_limits<double>::epsilon())
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);
118 const bool b_sample_Y = (std::fabs(y0 - y1) > std::fabs(x0 - x1));
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;
126 s.Z2 = s.Z1 = mix(z0, z1, alpha);
127 s.P2 = s.P1 = s.p * s.Z1;
130 s.b_sample_Y = b_sample_Y;
131 scanlines[y].push_back(s);
145 void vpMbScanLine::drawLineX(
const vpColVector &a,
const vpColVector &b,
const vpMbScanLineEdge &edge,
const int ID,
146 std::vector<std::vector<vpMbScanLineSegment> > &scanlines)
148 double x0 = a[0] / a[2];
149 double y0 = a[1] / a[2];
151 double x1 = b[0] / b[2];
152 double y1 = b[1] / b[2];
161 if (x0 >= w - 1 || x1 < 0 || std::fabs(x1 - x0) <= std::numeric_limits<double>::epsilon())
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);
167 const bool b_sample_Y = (std::fabs(y0 - y1) > std::fabs(x0 - x1));
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;
175 s.Z2 = s.Z1 = mix(z0, z1, alpha);
176 s.P2 = s.P1 = s.p * s.Z1;
179 s.b_sample_Y = b_sample_Y;
180 scanlines[x].push_back(s);
191 void vpMbScanLine::drawPolygonY(
const std::vector<std::pair<vpPoint, unsigned int> > &polygon,
const int ID,
192 std::vector<std::vector<vpMbScanLineSegment> > &scanlines)
194 if (polygon.size() < 2)
197 if (polygon.size() == 2) {
199 createVectorFromPoint(polygon.front().first, p1, K);
200 createVectorFromPoint(polygon.back().first, p2, K);
202 drawLineY(p1, p2, makeMbScanLineEdge(polygon.front().first, polygon.back().first), ID, scanlines);
206 std::vector<std::vector<vpMbScanLineSegment> > local_scanlines;
207 local_scanlines.resize(h);
209 for (
size_t i = 0; i < polygon.size(); ++i) {
211 createVectorFromPoint(polygon[i].first, p1, K);
212 createVectorFromPoint(polygon[(i + 1) % polygon.size()].first, p2, K);
214 drawLineY(p1, p2, makeMbScanLineEdge(polygon[i].first, polygon[(i + 1) % polygon.size()].first), ID,
218 createScanLinesFromLocals(scanlines, local_scanlines, h);
228 void vpMbScanLine::drawPolygonX(
const std::vector<std::pair<vpPoint, unsigned int> > &polygon,
const int ID,
229 std::vector<std::vector<vpMbScanLineSegment> > &scanlines)
231 if (polygon.size() < 2)
234 if (polygon.size() == 2) {
236 createVectorFromPoint(polygon.front().first, p1, K);
237 createVectorFromPoint(polygon.back().first, p2, K);
239 drawLineX(p1, p2, makeMbScanLineEdge(polygon.front().first, polygon.back().first), ID, scanlines);
243 std::vector<std::vector<vpMbScanLineSegment> > local_scanlines;
244 local_scanlines.resize(w);
246 for (
size_t i = 0; i < polygon.size(); ++i) {
248 createVectorFromPoint(polygon[i].first, p1, K);
249 createVectorFromPoint(polygon[(i + 1) % polygon.size()].first, p2, K);
251 drawLineX(p1, p2, makeMbScanLineEdge(polygon[i].first, polygon[(i + 1) % polygon.size()].first), ID,
255 createScanLinesFromLocals(scanlines, local_scanlines, w);
267 void vpMbScanLine::createScanLinesFromLocals(std::vector<std::vector<vpMbScanLineSegment> > &scanlines,
268 std::vector<std::vector<vpMbScanLineSegment> > &localScanlines,
269 const unsigned int &size)
271 for (
unsigned int j = 0; j < size; ++j) {
272 std::vector<vpMbScanLineSegment> &scanline = localScanlines[j];
273 sort(scanline.begin(), scanline.end(),
274 vpMbScanLineSegmentComparator());
277 for (
size_t i = 0; i < scanline.size(); ++i) {
278 vpMbScanLineSegment s = scanline[i];
284 vpMbScanLineSegment &prev = scanlines[j].back();
293 scanlines[j].push_back(s);
307 void vpMbScanLine::drawScene(
const std::vector<std::vector<std::pair<vpPoint, unsigned int> > *> &polygons,
315 visibility_samples.clear();
317 std::vector<std::vector<vpMbScanLineSegment> > scanlinesY;
318 scanlinesY.resize(h);
319 std::vector<std::vector<vpMbScanLineSegment> > scanlinesX;
320 scanlinesX.resize(w);
322 mask.resize(h, w, 0);
327 primitive_ids.resize(h, w, -1);
329 for (
unsigned int ID = 0; ID < polygons.size(); ++ID) {
330 drawPolygonY(*(polygons[ID]), listPolyIndices[ID], scanlinesY);
331 drawPolygonX(*(polygons[ID]), listPolyIndices[ID], scanlinesX);
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());
341 std::vector<std::pair<double, vpMbScanLineSegment> > stack;
342 for (
size_t i = 0; i < scanline.size(); ++i) {
343 const vpMbScanLineSegment &s = scanline[i];
347 stack.push_back(std::make_pair(s.Z1, s));
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();
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));
366 sort(stack.begin(), stack.end(), vpMbScanLineSegmentComparator());
368 int new_ID = stack.empty() ? -1 : stack.front().second.ID;
370 if (new_ID != last_ID || s.type == POINT) {
374 if (new_ID == -1 || s.Z1 - depthTreshold <= stack.front().first)
375 visibility_samples[s.edge].insert((
int)y);
379 visibility_samples[s.edge].insert((
int)y);
383 visibility_samples[s.edge].insert((
int)y);
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;
395 maskY[(
unsigned int)y][(
unsigned int)x] = 255;
397 mask[(
unsigned int)y][(
unsigned int)x] = 255;
402 if (!stack.empty()) {
403 last_visible = stack.front().second;
404 last_visible.p = s.p;
412 for (
unsigned int x = 0; x < scanlinesX.size(); ++x) {
413 std::vector<vpMbScanLineSegment> &scanline = scanlinesX[x];
414 sort(scanline.begin(), scanline.end(), vpMbScanLineSegmentComparator());
416 std::vector<std::pair<double, vpMbScanLineSegment> > stack;
417 for (
size_t i = 0; i < scanline.size(); ++i) {
418 const vpMbScanLineSegment &s = scanline[i];
422 stack.push_back(std::make_pair(s.Z1, s));
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();
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));
441 sort(stack.begin(), stack.end(), vpMbScanLineSegmentComparator());
443 int new_ID = stack.empty() ? -1 : stack.front().second.ID;
445 if (new_ID != last_ID || s.type == POINT) {
449 if (new_ID == -1 || s.Z1 - depthTreshold <= stack.front().first)
450 visibility_samples[s.edge].insert((
int)x);
454 visibility_samples[s.edge].insert((
int)x);
458 visibility_samples[s.edge].insert((
int)x);
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) {
469 maskX[(
unsigned int)y][(
unsigned int)x] = 255;
474 if (!stack.empty()) {
475 last_visible = stack.front().second;
476 last_visible.p = s.p;
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)
488 #if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI)) && defined(DEBUG_DISP)
489 if (!dispMaskDebug->isInitialised()) {
490 dispMaskDebug->init(mask, 800, 600);
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;
511 if (!dispLineDebug->isInitialised()) {
512 linedebugImg.resize(h, w, 0);
513 dispLineDebug->init(linedebugImg, 800, 100);
529 void vpMbScanLine::queryLineVisibility(
const vpPoint &a,
const vpPoint &b,
530 std::vector<std::pair<vpPoint, vpPoint> > &lines,
const bool &displayResults)
533 createVectorFromPoint(a, _a, K);
534 createVectorFromPoint(b, _b, K);
536 double x0 = _a[0] / _a[2];
537 double y0 = _a[1] / _a[2];
539 double x1 = _b[0] / _b[2];
540 double y1 = _b[1] / _b[2];
543 vpMbScanLineEdge edge = makeMbScanLineEdge(a, b);
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);
559 if (!visibility_samples.count(edge))
564 double *v0(&x0), *w0(&z0);
565 double *v1(&x1), *w1(&z1);
566 unsigned int size(w);
568 if (std::fabs(y0 - y1) > std::fabs(x0 - x1))
594 if (*v0 >= size - 1 || *v1 < 0 || std::fabs(*v1 - *v0) <= std::numeric_limits<double>::epsilon())
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));
600 const std::set<int> &visible_samples = visibility_samples[edge];
604 bool b_line_started =
false;
605 for (std::set<int>::const_iterator it = visible_samples.begin(); it != visible_samples.end(); ++it) {
607 const double alpha = getAlpha(v, (*v0) * (*w0), (*w0), (*v1) * (*w1), (*w1));
609 const vpPoint p = mix(a_, b_, alpha);
612 lines.push_back(std::make_pair(line_start, line_end));
613 b_line_started =
false;
619 b_line_started =
true;
620 }
else if (v == _v1) {
625 b_line_started =
true;
630 b_line_started =
true;
635 lines.push_back(std::make_pair(line_start, line_end));
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();
661 vpMbScanLine::vpMbScanLineEdge vpMbScanLine::makeMbScanLineEdge(
const vpPoint &a,
const vpPoint &b)
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);
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);
675 for (
unsigned int i = 0; i < 3; ++i)
679 }
else if (_a[i] > _b[i])
683 return std::make_pair(_a, _b);
685 return std::make_pair(_b, _a);
713 double vpMbScanLine::getAlpha(
double x,
double X0,
double Z0,
double X1,
double Z1)
715 const double N = X0 - x * Z0;
716 const double D = x * (Z1 - Z0) - (X1 - X0);
717 double alpha = N / D;
721 alpha = (std::min)(1.0, alpha);
722 alpha = (std::max)(0.0, alpha);
735 double vpMbScanLine::mix(
double a,
double b,
double alpha) {
return a + (b - a) * alpha; }
Generic class defining intrinsic camera parameters.
Implementation of column vector and the associated operations.
static const vpColor yellow
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...
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)
static bool isNaN(double value)
static double sqr(double x)
static bool isInf(double value)
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 ...
double get_y() const
Get the point y coordinate in the image plane.
double get_Y() const
Get the point cY coordinate in the camera frame.
void set_X(double cX)
Set the point cX coordinate in the camera frame.
double get_x() const
Get the point x coordinate in the image plane.
void set_Y(double cY)
Set the point cY coordinate in the camera frame.
double get_Z() const
Get the point cZ coordinate in the camera frame.
void set_Z(double cZ)
Set the point cZ coordinate in the camera frame.
double get_X() const
Get the point cX coordinate in the camera frame.