10 #include <mrpt/maps/CSimplePointsMap.h>
11 #include <mrpt/opengl/COpenGLScene.h>
12 #include <mrpt/version.h>
20 #include "../parse_utils.h"
24 using namespace mvsim;
29 mrpt::math::CMatrixFloat applyConvolution(
30 const mrpt::math::CMatrixFloat& data,
const mrpt::math::CMatrixDouble& kernel)
33 const size_t rows = data.rows();
34 const size_t cols = data.cols();
35 const size_t kernelSize = kernel.rows();
36 const size_t kernelRadius = kernelSize / 2;
39 ASSERT_EQUAL_(kernelSize,
static_cast<size_t>(kernel.cols()));
41 ASSERTMSG_(std::abs(kernel.sum() - 1.0) < 5e-3,
"Kernel must be normalized (sum to 1)");
44 mrpt::math::CMatrixFloat result(rows, cols);
47 for (
size_t i = 0; i < rows; ++i)
49 for (
size_t j = 0; j < cols; ++j)
54 for (
int ki = -kernelRadius; ki <= static_cast<int>(kernelRadius); ++ki)
56 for (
int kj = -kernelRadius; kj <= static_cast<int>(kernelRadius); ++kj)
62 if (ni >= 0 && ni <
static_cast<int>(rows) && nj >= 0 &&
63 nj < static_cast<int>(cols))
65 sum += data(ni, nj) * kernel(ki + kernelRadius, kj + kernelRadius);
88 std::string sElevationImgFile;
89 params[
"elevation_image"] =
TParamEntry(
"%s", &sElevationImgFile);
90 std::string sTextureImgFile;
91 params[
"texture_image"] =
TParamEntry(
"%s", &sTextureImgFile);
92 int texture_rotate = 0;
93 params[
"texture_image_rotate"] =
TParamEntry(
"%i", &texture_rotate);
95 std::string sElevationMatrixData;
96 params[
"elevation_data_matrix"] =
TParamEntry(
"%s", &sElevationMatrixData);
98 std::string sDemTextFile;
99 params[
"dem_xyzrgb_file"] =
TParamEntry(
"%s", &sDemTextFile);
101 mrpt::math::TPoint3Df dem_xyz_offset = {0, 0, 0};
102 params[
"dem_offset_x"] =
TParamEntry(
"%f", &dem_xyz_offset.x);
103 params[
"dem_offset_y"] =
TParamEntry(
"%f", &dem_xyz_offset.y);
104 params[
"dem_offset_z"] =
TParamEntry(
"%f", &dem_xyz_offset.z);
106 double img_min_z = 0.0, img_max_z = 5.0;
107 params[
"elevation_image_min_z"] =
TParamEntry(
"%lf", &img_min_z);
108 params[
"elevation_image_max_z"] =
TParamEntry(
"%lf", &img_max_z);
110 double corner_min_x = std::numeric_limits<double>::max();
111 double corner_min_y = std::numeric_limits<double>::max();
113 params[
"corner_min_x"] =
TParamEntry(
"%lf", &corner_min_x);
114 params[
"corner_min_y"] =
TParamEntry(
"%lf", &corner_min_y);
116 mrpt::img::TColor mesh_color(0xa0, 0xe0, 0xa0);
117 params[
"mesh_color"] =
TParamEntry(
"%color", &mesh_color);
125 std::string convolution_kernel_str;
126 params[
"apply_kernel"] =
TParamEntry(
"%s", &convolution_kernel_str);
131 mrpt::math::CMatrixFloat elevation_data;
132 std::optional<mrpt::img::CImage> mesh_image;
134 if (!sElevationImgFile.empty())
138 mrpt::img::CImage imgElev;
139 if (!imgElev.loadFromFile(sElevationImgFile, 0 ))
140 throw std::runtime_error(mrpt::format(
141 "[ElevationMap] ERROR: Cannot read elevation image '%s'",
142 sElevationImgFile.c_str()));
146 imgElev.getAsMatrix(elevation_data);
147 ASSERT_(img_min_z != img_max_z);
149 const double vmin = elevation_data.minCoeff();
150 const double vmax = elevation_data.maxCoeff();
151 mrpt::math::CMatrixFloat
f = elevation_data;
153 f *= (img_max_z - img_min_z) / (vmax - vmin);
154 mrpt::math::CMatrixFloat m(elevation_data.rows(), elevation_data.cols());
155 m.setConstant(img_min_z);
157 elevation_data = std::move(
f);
159 else if (!sElevationMatrixData.empty())
163 std::stringstream sErrors;
164 if (!elevation_data.fromMatlabStringFormat(sElevationMatrixData, sErrors))
166 THROW_EXCEPTION_FMT(
"Error parsing <elevation_data_matrix>: %s", sErrors.str().c_str());
172 !sDemTextFile.empty(),
173 "Either <elevation_image>, <elevation_data_matrix> or <dem_xyzrgb_file> must be "
178 mrpt::math::CMatrixDouble data;
179 data.loadFromTextFile(sDemTextFile);
180 ASSERTMSG_(data.cols() == 6,
"DEM txt file format error: expected 6 columns (x,y,z,r,g,b)");
183 data.col(0).array() += dem_xyz_offset.x;
184 data.col(1).array() += dem_xyz_offset.y;
185 data.col(2).array() += dem_xyz_offset.z;
189 const double minx = data.col(0).minCoeff();
190 const double maxx = data.col(0).maxCoeff();
191 const double miny = data.col(1).minCoeff();
192 const double maxy = data.col(1).maxCoeff();
197 const auto nx =
static_cast<unsigned int>(std::ceil((maxx - minx) /
resolution_));
198 const auto ny =
static_cast<unsigned int>(std::ceil((maxy - miny) /
resolution_));
201 mrpt::system::LVL_DEBUG,
202 "[ElevationMap] Loaded %u points, min_corner=(%lf,%lf), max_corner=(%lf,%lf), "
204 static_cast<unsigned>(data.rows()), minx, miny, maxx, maxy, nx, ny);
209 mrpt::maps::CSimplePointsMap pts;
210 pts.reserve(data.rows());
212 for (
int i = 0; i < data.rows(); i++)
213 pts.insertPoint(data(i, 0) - minx, data(i, 1) - miny, data(i, 2));
215 pts.kdTreeEnsureIndexBuilt2D();
216 elevation_data.resize(nx, ny);
220 mesh_image.emplace();
221 mesh_image->resize(ny, nx, mrpt::img::CH_RGB);
223 for (
unsigned int cx = 0; cx < nx; cx++)
226 for (
unsigned int cy = 0; cy < ny; cy++)
229 float closestSqrErr = 0;
230 const auto idxPt = pts.kdTreeClosestPoint2D(lx, ly, closestSqrErr);
232 elevation_data(cx, cy) = data(idxPt, 2 );
233 const uint8_t
R = data(idxPt, 3);
234 const uint8_t G = data(idxPt, 4);
235 const uint8_t B = data(idxPt, 5);
237 auto*
dest = &mesh_image->ptrLine<uint8_t>(cx)[3 * cy];
248 if (!mesh_image && !sTextureImgFile.empty())
251 mesh_image.emplace();
253 if (!mesh_image->loadFromFile(sTextureImgFile))
254 throw std::runtime_error(mrpt::format(
255 "[ElevationMap] ERROR: Cannot read texture image '%s'", sTextureImgFile.c_str()));
258 switch (texture_rotate)
267 mrpt::img::CImage im;
268 mesh_image->rotateImage(
269 im, mrpt::DEG2RAD(texture_rotate), mesh_image->getWidth() / 2,
270 mesh_image->getHeight() / 2);
271 mesh_image = std::move(im);
275 THROW_EXCEPTION(
"texture_image_rotate can only be: 0, 90, -90, 180");
280 if (!convolution_kernel_str.empty())
282 mrpt::math::CMatrixDouble kernel;
283 std::stringstream ss(
mvsim::trim(convolution_kernel_str));
286 kernel.loadFromTextFile(ss);
287 ASSERT_(kernel.cols() == kernel.rows());
288 ASSERT_(kernel.cols() > 1);
290 catch (
const std::exception& e)
293 "Error parsing kernel as matrix: '%s'.\nError: %s", convolution_kernel_str.c_str(),
298 mrpt::system::LVL_DEBUG,
"[ElevationMap] Applying filtering convolution filter %ux%u",
299 static_cast<unsigned>(kernel.rows()),
static_cast<unsigned>(kernel.cols()));
301 elevation_data = applyConvolution(elevation_data, kernel);
306 const double LX = (elevation_data.rows() - 1) *
resolution_;
307 const double LY = (elevation_data.cols() - 1) *
resolution_;
309 if (corner_min_x == std::numeric_limits<double>::max()) corner_min_x = -0.5 * LX;
310 if (corner_min_y == std::numeric_limits<double>::max()) corner_min_y = -0.5 * LY;
328 auto gl_mesh = mrpt::opengl::CMesh::Create();
331 gl_mesh->enableTransparency(
false);
335 gl_mesh->assignImageAndZ(*mesh_image, elevation_data);
340 gl_mesh->setZ(elevation_data);
341 gl_mesh->setColor_u8(mesh_color);
344 gl_mesh->setGridLimits(corner_min_x, corner_min_x + LX, corner_min_y, corner_min_y + LY);
347 gl_mesh->setLocalRepresentativePoint(
348 mrpt::math::TPoint3Df(corner_min_x + 0.5 * LX, corner_min_y + 0.5 * LY, .0
f));
355 const size_t NX =
static_cast<size_t>(std::ceil(LX / subSize));
356 const size_t NY =
static_cast<size_t>(std::ceil(LY / subSize));
357 for (
size_t iX = 0; iX < NX; iX++)
363 const size_t startIx = iX * M;
364 const size_t lenIx_p = std::min<size_t>(M, elevation_data.rows() - startIx);
365 const size_t lenIx = std::min<size_t>(M + 1, elevation_data.rows() - startIx);
367 for (
size_t iY = 0; iY < NY; iY++)
369 const size_t startIy = iY * M;
370 const size_t lenIy_p = std::min<size_t>(M, elevation_data.cols() - startIy);
371 const size_t lenIy = std::min<size_t>(M + 1, elevation_data.cols() - startIy);
374 const auto subEle = elevation_data.extractMatrix(lenIx, lenIy, startIx, startIy);
377 auto gl_mesh = mrpt::opengl::CMesh::Create();
380 gl_mesh->enableTransparency(
false);
384 gl_mesh->assignImageAndZ(*mesh_image, subEle);
389 gl_mesh->setZ(subEle);
390 gl_mesh->setColor_u8(mesh_color);
393 gl_mesh->setGridLimits(
394 corner_min_x + iX * subSize,
395 corner_min_x + iX * subSize + lenIx_p *
resolution_,
396 corner_min_y + iY * subSize,
397 corner_min_y + iY * subSize + lenIy_p *
resolution_);
400 gl_mesh->setLocalRepresentativePoint(mrpt::math::TPoint3Df(
401 corner_min_x + (iX + 0.5) * subSize, corner_min_y + (iY + 0.5) * subSize,
409 const mrpt::optional_ref<mrpt::opengl::COpenGLScene>& viz,
410 const mrpt::optional_ref<mrpt::opengl::COpenGLScene>& physical,
411 [[maybe_unused]]
bool childrenOnly)
415 "ERROR: Can't render Mesh before loading it! Have you called "
416 "loadConfigFrom() first?");
424 glMesh->setPose(
parent()->applyWorldRenderOffset(mrpt::poses::CPose3D::Identity()));
426 viz->get().insert(glMesh);
427 physical->get().insert(glMesh);
451 const mrpt::math::TPoint3D& p1,
const mrpt::math::TPoint3D& p2,
const mrpt::math::TPoint3D& p3,
454 const double det = (p2.x - p3.x) * (p1.y - p3.y) +
455 (p3.y - p2.y) * (p1.x - p3.x);
458 const double l1 = ((p2.x - p3.x) * (y - p3.y) + (p3.y - p2.y) * (x - p3.x)) / det;
459 const double l2 = ((p3.x - p1.x) * (y - p3.y) + (p1.y - p3.y) * (x - p3.x)) / det;
460 const double l3 = 1.0 - l1 - l2;
462 return l1 * p1.z + l2 * p2.z + l3 * p3.z;
477 const double sCellX = (x1 - x0) / (nCellsX - 1);
478 const double sCellY = (y1 - y0) / (nCellsY - 1);
481 const int cx00 = ::floor((pt.x - x0) / sCellX);
482 const int cy00 = ::floor((pt.y - y0) / sCellY);
484 if (cx00 < 0 || cx00 >=
int(nCellsX - 1) || cy00 < 0 || cy00 >=
int(nCellsY - 1))
491 const double z11 =
meshCacheZ_(cx00 + 1, cy00 + 1);
498 const mrpt::math::TPoint3D p00(.0, .0, z00);
499 const mrpt::math::TPoint3D p01(.0, sCellY, z01);
500 const mrpt::math::TPoint3D p10(sCellX, .0, z10);
501 const mrpt::math::TPoint3D p11(sCellX, sCellY, z11);
503 const double lx = pt.x - (x0 + cx00 * sCellX);
504 const double ly = pt.y - (y0 + cy00 * sCellY);
507 return calcz(p00, p01, p11, lx, ly);
509 return calcz(p00, p10, p11, lx, ly);