MRPT  1.9.9
CVectorField2D.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2019, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include "opengl-precomp.h" // Precompiled header
11 
14 
15 #include "opengl_internals.h"
16 
17 using namespace mrpt;
18 using namespace mrpt::opengl;
19 
20 using namespace mrpt::math;
21 using namespace std;
22 
24 
25 /** Constructor */
26 CVectorField2D::CVectorField2D() : xcomp(0, 0), ycomp(0, 0)
27 
28 {
29  m_point_color = m_color;
30  m_field_color = m_color;
31 }
32 
33 /** Constructor with a initial set of lines. */
35  CMatrixFloat Matrix_x, CMatrixFloat Matrix_y, float xmin, float xmax,
36  float ymin, float ymax)
37  : m_LineWidth(1.0), m_pointSize(1.0), m_antiAliasing(true)
38 {
39  MRPT_UNUSED_PARAM(Matrix_x);
40  MRPT_UNUSED_PARAM(Matrix_y);
41  MRPT_UNUSED_PARAM(xmin);
42  MRPT_UNUSED_PARAM(xmax);
43  MRPT_UNUSED_PARAM(ymin);
44  MRPT_UNUSED_PARAM(ymax);
47 }
48 
49 /*---------------------------------------------------------------
50  render
51  ---------------------------------------------------------------*/
53 {
54 #if MRPT_HAS_OPENGL_GLUT
55 
56  // Enable antialiasing:
57  glPushAttrib(GL_COLOR_BUFFER_BIT | GL_LINE_BIT);
58  if (m_antiAliasing || m_color.A != 255)
59  {
60  glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
61  glEnable(GL_BLEND);
62  }
63  if (m_antiAliasing)
64  {
65  glEnable(GL_LINE_SMOOTH);
66  glEnable(GL_POINT_SMOOTH);
67  }
68 
69  glLineWidth(m_LineWidth);
70  glPointSize(m_pointSize);
71 
73 
74  glDisable(GL_LIGHTING); // Disable lights when drawing lines
75  glBegin(GL_POINTS);
76  glColor4ub(
78 
79  const float x_cell_size = (xMax - xMin) / (xcomp.cols() - 1);
80  const float y_cell_size = (yMax - yMin) / (ycomp.rows() - 1);
81 
82  for (int i = 0; i < xcomp.cols(); i++)
83  for (int j = 0; j < xcomp.rows(); j++)
84  {
85  glVertex3f(xMin + i * x_cell_size, yMin + j * y_cell_size, 0);
86  }
87 
88  glEnd();
89 
90  glBegin(GL_LINES);
91  glColor4ub(
93  for (int i = 0; i < xcomp.cols(); i++)
94  for (int j = 0; j < xcomp.rows(); j++)
95  {
96  glVertex3f(xMin + i * x_cell_size, yMin + j * y_cell_size, 0);
97  glVertex3f(
98  xMin + i * x_cell_size + xcomp(j, i),
99  yMin + j * y_cell_size + ycomp(j, i), 0);
100  }
101  glEnd();
102 
103  glBegin(GL_TRIANGLES);
104  glColor4ub(
106  for (int i = 0; i < xcomp.cols(); i++)
107  for (int j = 0; j < xcomp.rows(); j++)
108  {
109  const float tri_side =
110  0.25 *
111  sqrt(xcomp(j, i) * xcomp(j, i) + ycomp(j, i) * ycomp(j, i));
112  const float ang = ::atan2(ycomp(j, i), xcomp(j, i)) - 1.5708;
113  glVertex3f(
114  -sin(ang) * 0.866 * tri_side + xMin + i * x_cell_size +
115  xcomp(j, i),
116  cos(ang) * 0.866 * tri_side + yMin + j * y_cell_size +
117  ycomp(j, i),
118  0);
119  glVertex3f(
120  cos(ang) * 0.5 * tri_side + xMin + i * x_cell_size +
121  xcomp(j, i),
122  sin(ang) * 0.5 * tri_side + yMin + j * y_cell_size +
123  ycomp(j, i),
124  0);
125  glVertex3f(
126  -cos(ang) * 0.5 * tri_side + xMin + i * x_cell_size +
127  xcomp(j, i),
128  -sin(ang) * 0.5 * tri_side + yMin + j * y_cell_size +
129  ycomp(j, i),
130  0);
131  }
132  glEnd();
133 
135  glEnable(GL_LIGHTING); // Disable lights when drawing lines
136 
137  // End of antialiasing:
138  glPopAttrib();
139 
140 #endif
141 }
142 
143 /*---------------------------------------------------------------
144  Implements the writing to a CStream capability of
145  CSerializable objects
146  ---------------------------------------------------------------*/
147 uint8_t CVectorField2D::serializeGetVersion() const { return 0; }
149 {
151 
152  out << xcomp << ycomp;
153  out << xMin << xMax << yMin << yMax;
154  out << m_LineWidth;
155  out << m_pointSize;
156  out << m_antiAliasing;
157  out << m_point_color;
158  out << m_field_color;
159 }
160 
162  mrpt::serialization::CArchive& in, uint8_t version)
163 {
164  switch (version)
165  {
166  case 0:
168 
169  in >> xcomp >> ycomp;
170  in >> xMin >> xMax >> yMin >> yMax;
171  in >> m_LineWidth;
172  in >> m_pointSize;
173  in >> m_antiAliasing;
174  in >> m_point_color;
175  in >> m_field_color;
176  break;
177 
178  default:
180  break;
181  };
183 }
184 
187 {
188  bb_min.x = xMin;
189  bb_min.y = yMin;
190  bb_min.z = 0;
191 
192  bb_max.x = xMax;
193  bb_max.y = yMax;
194  bb_max.z = 0;
195 
196  const float x_cell_size = (xMax - xMin) / (xcomp.cols() - 1);
197  const float y_cell_size = (yMax - yMin) / (ycomp.rows() - 1);
198 
199  for (int i = 0; i < xcomp.cols(); i++)
200  for (int j = 0; j < xcomp.rows(); j++)
201  {
202  const float tri_side =
203  0.25 *
204  sqrt(xcomp(j, i) * xcomp(j, i) + ycomp(j, i) * ycomp(j, i));
205  const float ang = ::atan2(ycomp(j, i), xcomp(j, i)) - 1.5708;
206 
207  if (-sin(ang) * 0.866 * tri_side + xMin + i * x_cell_size +
208  xcomp(j, i) <
209  bb_min.x)
210  bb_min.x = -sin(ang) * 0.866 * tri_side + xMin +
211  i * x_cell_size + xcomp(j, i);
212 
213  if (cos(ang) * 0.866 * tri_side + yMin + j * y_cell_size +
214  ycomp(j, i) <
215  bb_min.y)
216  bb_min.y = cos(ang) * 0.866 * tri_side + yMin +
217  j * y_cell_size + ycomp(j, i);
218 
219  if (-sin(ang) * 0.866 * tri_side + xMin + i * x_cell_size +
220  xcomp(j, i) >
221  bb_max.x)
222  bb_max.x = -sin(ang) * 0.866 * tri_side + xMin +
223  i * x_cell_size + xcomp(j, i);
224 
225  if (cos(ang) * 0.866 * tri_side + yMin + j * y_cell_size +
226  ycomp(j, i) >
227  bb_max.y)
228  bb_max.y = cos(ang) * 0.866 * tri_side + yMin +
229  j * y_cell_size + ycomp(j, i);
230  }
231 
232  // Convert to coordinates of my parent:
235 }
236 
238 {
239  ASSERT_(xcomp.size() > 0);
240 
241  const float ratio_xp =
242  xcomp.maxCoeff() * (xcomp.cols() - 1) / (xMax - xMin);
243  const float ratio_xn =
244  xcomp.minCoeff() * (xcomp.cols() - 1) / (xMax - xMin);
245  const float ratio_yp =
246  ycomp.maxCoeff() * (ycomp.rows() - 1) / (yMax - yMin);
247  const float ratio_yn =
248  ycomp.minCoeff() * (ycomp.rows() - 1) / (yMax - yMin);
249  const float norm_factor =
250  0.85 / max(max(ratio_xp, abs(ratio_xn)), max(ratio_yp, abs(ratio_yn)));
251 
252  xcomp *= norm_factor;
253  ycomp *= norm_factor;
255 }
Scalar maxCoeff() const
Maximum value in the matrix/vector.
void notifyChange() const
Must be called to notify that the object has changed (so, the display list must be updated) ...
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
To be added to all CSerializable-classes implementation files.
mrpt::math::CMatrixF ycomp
Y component of the vector field.
mrpt::img::TColor m_point_color
STL namespace.
uint8_t B
Definition: TColor.h:46
uint8_t G
Definition: TColor.h:46
mrpt::poses::CPose3D m_pose
6D pose wrt the parent coordinate reference.
Definition: CRenderizable.h:54
A renderizable object suitable for rendering with OpenGL&#39;s display lists.
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:97
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
This base provides a set of functions for maths stuff.
mrpt::math::CMatrixF xcomp
X component of the vector field.
void writeToStreamRender(mrpt::serialization::CArchive &out) const
size_type rows() const
Number of rows in the matrix.
size_type cols() const
Number of columns in the matrix.
A 2D vector field representation, consisting of points and arrows drawn on a plane (invisible grid)...
uint8_t R
Definition: TColor.h:46
void getBoundingBox(mrpt::math::TPoint3D &bb_min, mrpt::math::TPoint3D &bb_max) const override
Evaluates the bounding box of this object (including possible children) in the coordinate frame of th...
mrpt::img::TColor m_color
Color components in the range [0,255].
Definition: CRenderizable.h:51
void serializeFrom(mrpt::serialization::CArchive &in, uint8_t serial_version) override
Pure virtual method for reading (deserializing) from an abstract archive.
static void checkOpenGLError()
Checks glGetError and throws an exception if an error situation is found.
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
void serializeTo(mrpt::serialization::CArchive &out) const override
Pure virtual method for writing (serializing) to an abstract archive.
bool m_antiAliasing
By default is true.
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:54
mrpt::vision::TStereoCalibResults out
matrix_size_t size() const
Get a 2-vector with [NROWS NCOLS] (as in MATLAB command size(x))
The namespace for 3D scene representation and rendering.
Definition: CGlCanvasBase.h:15
const auto bb_max
void adjustVectorFieldToGrid()
Adjust the vector field in the scene (vectors magnitude) according to the grid size.
Scalar minCoeff() const
Minimum value in the matrix/vector.
float m_pointSize
By default is 1.0.
void composePoint(double lx, double ly, double lz, double &gx, double &gy, double &gz, mrpt::optional_ref< mrpt::math::CMatrixDouble33 > out_jacobian_df_dpoint=std::nullopt, mrpt::optional_ref< mrpt::math::CMatrixDouble36 > out_jacobian_df_dpose=std::nullopt, mrpt::optional_ref< mrpt::math::CMatrixDouble36 > out_jacobian_df_dse3=std::nullopt, bool use_small_rot_approx=false) const
An alternative, slightly more efficient way of doing with G and L being 3D points and P this 6D pose...
const auto bb_min
void render_dl() const override
Render.
mrpt::img::TColor m_field_color
Lightweight 3D point.
Definition: TPoint3D.h:90
This template class provides the basic functionality for a general 2D any-size, resizable container o...
void readFromStreamRender(mrpt::serialization::CArchive &in)
float m_LineWidth
By default is 1.0.
uint8_t A
Definition: TColor.h:46
uint8_t serializeGetVersion() const override
Must return the current versioning number of the object.
#define MRPT_UNUSED_PARAM(a)
Determines whether this is an X86 or AMD64 platform.
Definition: common.h:186



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 7e629e01a Sat Dec 14 00:05:55 2019 +0100 at sáb dic 14 00:15:10 CET 2019