Visual Servoing Platform  version 3.4.0
vpMbScanLine.h
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  * Compute the visibility of 3D polygons already transformed in the camera
33  *frame
34  *
35  * Authors:
36  * Aurelien Yol
37  *
38  *****************************************************************************/
39 
40 #ifndef vpMbScanLine_HH
41 #define vpMbScanLine_HH
42 
43 #include <deque>
44 #include <limits> // numeric_limits
45 #include <list>
46 #include <map>
47 #include <set>
48 #include <vector>
49 
50 #include <visp3/core/vpCameraParameters.h>
51 #include <visp3/core/vpColVector.h>
52 #include <visp3/core/vpImage.h>
53 #include <visp3/core/vpImageConvert.h>
54 #include <visp3/core/vpImagePoint.h>
55 #include <visp3/core/vpPoint.h>
56 
57 //#define DEBUG_DISP // Uncomment to get visibility debug display
58 
59 #if defined(DEBUG_DISP)
60 #include <visp3/core/vpDisplay.h>
61 #endif
62 
63 #ifndef DOXYGEN_SHOULD_SKIP_THIS
64 
71 class VISP_EXPORT vpMbScanLine
72 {
73 public:
76  typedef enum { START = 1, END = 0, POINT = 2 } vpMbScanLineType;
77 
80  typedef std::pair<vpColVector, vpColVector> vpMbScanLineEdge;
81 
83  struct vpMbScanLineSegment {
84  vpMbScanLineSegment() : type(START), edge(), p(0), P1(0), P2(0), Z1(0), Z2(0), ID(0), b_sample_Y(false) {}
85  vpMbScanLineType type;
86  vpMbScanLineEdge edge;
87  double p; // This value can be either x or y-coordinate value depending if
88  // the structure is used in X or Y-axis scanlines computation.
89  double P1, P2; // Same comment as previous value.
90  double Z1, Z2;
91  int ID;
92  bool b_sample_Y;
93  };
94 
96  struct vpMbScanLineEdgeComparator {
97  inline bool operator()(const vpMbScanLineEdge &l0, const vpMbScanLineEdge &l1) const
98  {
99  for (unsigned int i = 0; i < 3; ++i)
100  if (l0.first[i] < l1.first[i])
101  return true;
102  else if (l0.first[i] > l1.first[i])
103  return false;
104  for (unsigned int i = 0; i < 3; ++i)
105  if (l0.second[i] < l1.second[i])
106  return true;
107  else if (l0.second[i] > l1.second[i])
108  return false;
109  return false;
110  }
111  };
112 
114  struct vpMbScanLineSegmentComparator {
115  inline bool operator()(const vpMbScanLineSegment &a, const vpMbScanLineSegment &b) const
116  {
117  // return a.p == b.p ? a.type < b.type : a.p < b.p;
118  return (std::fabs(a.p - b.p) <= std::numeric_limits<double>::epsilon()) ? a.type < b.type : a.p < b.p;
119  }
120 
121  inline bool operator()(const std::pair<double, vpMbScanLineSegment> &a,
122  const std::pair<double, vpMbScanLineSegment> &b) const
123  {
124  return a.first < b.first;
125  }
126  };
127 
128 private:
129  unsigned int w, h;
131  unsigned int maskBorder;
133  vpImage<int> primitive_ids;
134  std::map<vpMbScanLineEdge, std::set<int>, vpMbScanLineEdgeComparator> visibility_samples;
135  double depthTreshold;
136 
137 public:
138 #if defined(DEBUG_DISP)
139  vpDisplay *dispMaskDebug;
140  vpDisplay *dispLineDebug;
141  vpImage<unsigned char> linedebugImg;
142 #endif
143 
144  vpMbScanLine();
145  virtual ~vpMbScanLine();
146 
147  void drawScene(const std::vector<std::vector<std::pair<vpPoint, unsigned int> > *> &polygons,
148  std::vector<int> listPolyIndices, const vpCameraParameters &K, unsigned int w, unsigned int h);
149 
157  double getDepthTreshold() { return depthTreshold; }
158  unsigned int getMaskBorder() { return maskBorder; }
159  const vpImage<unsigned char> &getMask() const { return mask; }
160  const vpImage<int> &getPrimitiveIDs() const { return primitive_ids; }
161 
162  void queryLineVisibility(const vpPoint &a, const vpPoint &b, std::vector<std::pair<vpPoint, vpPoint> > &lines,
163  const bool &displayResults = false);
164 
172  void setDepthTreshold(const double &treshold) { depthTreshold = treshold; }
173  void setMaskBorder(const unsigned int &mb) { maskBorder = mb; }
174 
175 private:
176  void createScanLinesFromLocals(std::vector<std::vector<vpMbScanLineSegment> > &scanlines,
177  std::vector<std::vector<vpMbScanLineSegment> > &localScanlines,
178  const unsigned int &size);
179 
180  void drawLineY(const vpColVector &a, const vpColVector &b, const vpMbScanLineEdge &line_ID, const int ID,
181  std::vector<std::vector<vpMbScanLineSegment> > &scanlines);
182 
183  void drawLineX(const vpColVector &a, const vpColVector &b, const vpMbScanLineEdge &line_ID, const int ID,
184  std::vector<std::vector<vpMbScanLineSegment> > &scanlines);
185 
186  void drawPolygonY(const std::vector<std::pair<vpPoint, unsigned int> > &polygon, const int ID,
187  std::vector<std::vector<vpMbScanLineSegment> > &scanlines);
188 
189  void drawPolygonX(const std::vector<std::pair<vpPoint, unsigned int> > &polygon, const int ID,
190  std::vector<std::vector<vpMbScanLineSegment> > &scanlines);
191 
192  // Static functions
193  static vpMbScanLineEdge makeMbScanLineEdge(const vpPoint &a, const vpPoint &b);
194  static void createVectorFromPoint(const vpPoint &p, vpColVector &v, const vpCameraParameters &K);
195  static double getAlpha(double x, double X0, double Z0, double X1, double Z1);
196  static double mix(double a, double b, double alpha);
197  static vpPoint mix(const vpPoint &a, const vpPoint &b, double alpha);
198  static double norm(const vpPoint &a, const vpPoint &b);
199 };
200 
201 #endif // doxygen should skip this
202 
203 #endif
Class that defines generic functionnalities for display.
Definition: vpDisplay.h:177
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
Definition: vpPoint.h:81
Generic class defining intrinsic camera parameters.
Implementation of column vector and the associated operations.
Definition: vpColVector.h:130