MRPT  2.0.4
CMeshFast.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-2020, 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 
12 #include <mrpt/img/color_maps.h>
14 #include <mrpt/opengl/CMeshFast.h>
16 #include <mrpt/poses/CPose3D.h>
18 #include <Eigen/Dense>
19 
20 using namespace mrpt;
21 using namespace mrpt::opengl;
22 using namespace mrpt::img;
23 using namespace mrpt::poses;
24 using namespace mrpt::math;
25 using namespace std;
26 
28 
29 void CMeshFast::updatePoints() const
30 {
32 
33  const auto cols = Z.cols();
34  const auto rows = Z.rows();
35 
36  if ((m_colorFromZ) || (m_isImage)) updateColorsMatrix();
37 
38  ASSERT_((cols > 0) && (rows > 0));
39  ASSERT_((xMax > xMin) && (yMax > yMin));
40  X.setSize(rows, cols);
41  Y.setSize(rows, cols);
42  const float sCellX = (xMax - xMin) / (rows - 1);
43  const float sCellY = (yMax - yMin) / (cols - 1);
44 
45  for (int iX = 0; iX < rows; iX++)
46  for (int iY = 0; iY < cols; iY++)
47  {
48  X(iX, iY) = xMin + iX * sCellX;
49  Y(iX, iY) = yMin + iY * sCellY;
50  }
51 
52  pointsUpToDate = true;
53 }
54 
56 {
57  using mrpt::img::TColor;
58  using mrpt::img::TColorf;
59 
60  if (!pointsUpToDate) updatePoints();
61 
62  ASSERT_EQUAL_(X.size(), Y.size());
63  ASSERT_EQUAL_(X.size(), Z.size());
64 
67  vbd.clear();
68  cbd.clear();
69 
70  for (int i = 0; i < X.rows(); i++)
71  {
72  for (int j = 0; j < X.cols(); j++)
73  {
74  TColor col;
75 
76  if (m_isImage && m_textureImage.isColor())
77  col = TColor(C_r(i, j), C_g(i, j), C_b(i, j), m_color.A);
78  else if (m_isImage)
79  col = TColor(C(i, j), C(i, j), C(i, j), m_color.A);
80  else if (m_colorFromZ)
81  {
82  float rz, gz, bz;
83  colormap(m_colorMap, C(i, j) / 255.0f, rz, gz, bz);
84  col = TColorf(rz, gz, bz, m_color.A / 255.f).asTColor();
85  }
86  else
87  col = m_color;
88 
89  cbd.emplace_back(col);
90  vbd.emplace_back(X(i, j), Y(i, j), Z(i, j));
91  }
92  }
93 }
94 
96 {
98 
99  // Make a copy:
100  m_textureImage = img;
101 
102  // Delete content in Z
103  Z.setZero(img.getHeight(), img.getWidth());
104 
105  // Update flags/states
106  m_modified_Image = true;
107  m_enableTransparency = false;
108  m_colorFromZ = false;
109  m_isImage = true;
110  pointsUpToDate = false;
111 
113 
114  MRPT_END
115 }
116 
118  const CImage& img, const mrpt::math::CMatrixDynamic<float>& in_Z)
119 {
120  MRPT_START
121 
122  ASSERT_(
123  (img.getWidth() == static_cast<size_t>(in_Z.cols())) &&
124  (img.getHeight() == static_cast<size_t>(in_Z.rows())));
125 
126  Z = in_Z;
127 
128  // Make a copy:
129  m_textureImage = img;
130 
131  // Update flags/states
132  m_modified_Image = true;
133  m_enableTransparency = false;
134  m_colorFromZ = false;
135  m_isImage = true;
136  pointsUpToDate = false;
137 
139 
140  MRPT_END
141 }
142 
143 uint8_t CMeshFast::serializeGetVersion() const { return 0; }
145 {
146  writeToStreamRender(out);
147 
148  out << m_textureImage;
149  out << m_isImage;
150  out << xMin << xMax << yMin << yMax;
151  out << X << Y << Z; // We don't need to serialize C, it's computed
152  out << m_enableTransparency;
153  out << m_colorFromZ;
154  out << int16_t(m_colorMap);
155  out << m_pointSize;
156 }
157 
159  mrpt::serialization::CArchive& in, uint8_t version)
160 {
161  switch (version)
162  {
163  case 0:
164  {
165  readFromStreamRender(in);
166 
167  in >> m_textureImage;
168  in >> m_isImage;
169 
170  in >> xMin;
171  in >> xMax;
172  in >> yMin;
173  in >> yMax;
174 
175  in >> X >> Y >> Z;
176  in >> m_enableTransparency;
177  in >> m_colorFromZ;
178 
179  int16_t i;
180  in >> i;
181  m_colorMap = TColormap(i);
182  in >> m_pointSize;
183  m_modified_Z = true;
184  }
185 
186  pointsUpToDate = false;
187  break;
188 
189  default:
191  };
193 }
194 
196 {
197  if (!m_modified_Z && !m_modified_Image) return;
198 
200 
201  if (m_isImage)
202  {
203  const int cols = m_textureImage.getWidth();
204  const int rows = m_textureImage.getHeight();
205 
206  ASSERT_EQUAL_(cols, Z.cols());
207  ASSERT_EQUAL_(rows, Z.rows());
208 
209  if (m_textureImage.isColor())
210  {
211  C_r.setSize(rows, cols);
212  C_g.setSize(rows, cols);
213  C_b.setSize(rows, cols);
214  m_textureImage.getAsRGBMatrices(C_r, C_g, C_b);
215  }
216  else
217  {
218  C.setSize(rows, cols);
219  m_textureImage.getAsMatrix(C);
220  }
221  }
222  else
223  {
224  const size_t cols = Z.cols();
225  const size_t rows = Z.rows();
226 
227  C.setSize(rows, cols);
228 
229  // Color is proportional to difference between height of a cell and
230  // the mean of the nearby cells MEANS:
231  Eigen::MatrixXf Zf = Z.asEigen().cast<float>();
232  mrpt::math::normalize(Zf, 0.01f, 0.99f);
233 
234  Zf *= 255;
235 
236  C = Zf.cast<uint8_t>();
237  }
238 
239  m_modified_Image = false;
240  m_modified_Z = false; // Done
241  pointsUpToDate = false;
242 }
243 
245 {
246  Z = in_Z;
247  m_modified_Z = true;
248  pointsUpToDate = false;
249 
250  // Delete previously loaded images
251  m_isImage = false;
252 
254 }
255 
258 {
259  bb_min.x = xMin;
260  bb_min.y = yMin;
261  bb_min.z = Z.minCoeff();
262 
263  bb_max.x = xMax;
264  bb_max.y = yMax;
265  bb_max.z = Z.maxCoeff();
266 
267  // Convert to coordinates of my parent:
268  m_pose.composePoint(bb_min, bb_min);
269  m_pose.composePoint(bb_max, bb_max);
270 }
void updateColorsMatrix() const
Called internally to assure C is updated.
Definition: CMeshFast.cpp:195
void colormap(const TColormap &color_map, const float color_index, float &r, float &g, float &b)
Transform a float number in the range [0,1] into RGB components.
Definition: color_maps.cpp:114
TColormap
Different colormaps for use in mrpt::img::colormap()
Definition: color_maps.h:30
#define MRPT_START
Definition: exceptions.h:241
void serializeFrom(mrpt::serialization::CArchive &in, uint8_t serial_version) override
Pure virtual method for reading (deserializing) from an abstract archive.
Definition: CMeshFast.cpp:158
std::vector< mrpt::math::TPoint3Df > m_vertex_buffer_data
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
To be added to all CSerializable-classes implementation files.
void notifyChange() const
Call to enable calling renderUpdateBuffers() before the next render() rendering iteration.
A planar (XY) grid where each cell has an associated height and, optionally, a texture map...
Definition: CMeshFast.h:38
This file implements several operations that operate element-wise on individual or pairs of container...
uint8_t serializeGetVersion() const override
Must return the current versioning number of the object.
Definition: CMeshFast.cpp:143
The base class of 3D objects that can be directly rendered through OpenGL.
Definition: CRenderizable.h:48
void serializeTo(mrpt::serialization::CArchive &out) const override
Pure virtual method for writing (serializing) to an abstract archive.
Definition: CMeshFast.cpp:144
void assignImageAndZ(const mrpt::img::CImage &img, const mrpt::math::CMatrixDynamic< float > &in_Z)
Assigns a texture image and Z simultaneously, and disable transparency.
Definition: CMeshFast.cpp:117
size_t getHeight() const override
Returns the height of the image in pixels.
Definition: CImage.cpp:849
STL namespace.
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:97
std::vector< mrpt::img::TColor > m_color_buffer_data
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
This base provides a set of functions for maths stuff.
size_t getWidth() const override
Returns the width of the image in pixels.
Definition: CImage.cpp:818
void normalize(CONTAINER &c, Scalar valMin, Scalar valMax)
Scales all elements such as the minimum & maximum values are shifted to the given values...
#define ASSERT_EQUAL_(__A, __B)
Assert comparing two values, reporting their actual values upon failure.
Definition: exceptions.h:137
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...
Definition: CMeshFast.cpp:256
void setZ(const mrpt::math::CMatrixDynamic< float > &in_Z)
This method sets the matrix of heights for each position (cell) in the mesh grid. ...
Definition: CMeshFast.cpp:244
void onUpdateBuffers_Points() override
Must be implemented in derived classes to update the geometric entities to be drawn in "m_*_buffer" f...
Definition: CMeshFast.cpp:55
size_type rows() const
Number of rows in the matrix.
size_type cols() const
Number of columns in the matrix.
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:54
mrpt::vision::TStereoCalibResults out
#define MRPT_END
Definition: exceptions.h:245
An RGBA color - floats in the range [0,1].
Definition: TColor.h:88
The namespace for 3D scene representation and rendering.
Definition: CGlCanvasBase.h:13
const auto bb_max
const auto bb_min
TColor asTColor() const
Returns the 0-255 integer version of this color: RGBA_u8.
Definition: TColor.h:101
A RGB color - 8bit.
Definition: TColor.h:25
This template class provides the basic functionality for a general 2D any-size, resizable container o...
void assignImage(const mrpt::img::CImage &img)
Assigns a texture image, and disable transparency.
Definition: CMeshFast.cpp:95
A class for storing images as grayscale or RGB bitmaps.
Definition: img/CImage.h:148



Page generated by Doxygen 1.8.14 for MRPT 2.0.4 Git: 02a75dd25 Thu Jun 4 01:49:55 2020 +0200 at jue jun 4 02:00:12 CEST 2020