Skip to content

Commit ac160ae

Browse files
jlblancocjlblancoc
committed
NDT maps: more render options (enable colormaps,etc.)
1 parent a950918 commit ac160ae

File tree

2 files changed

+105
-9
lines changed

2 files changed

+105
-9
lines changed

‎mola_metric_maps/include/mola_metric_maps/NDT.h

Lines changed: 9 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -482,6 +482,15 @@ class NDT : public mrpt::maps::CMetricMap,
482482
mrpt::img::TColorf planes_color{1.0f, .0f, 1.0f};
483483
bool normals_visible = true;
484484
mrpt::img::TColorf normals_color{1.0f, 0.0f, 0.0f};
485+
486+
/** Colormap to use. Can be set to cmNone for uniform color */
487+
mrpt::img::TColormap points_colormap = mrpt::img::cmHOT;
488+
mrpt::img::TColormap planes_colormap = mrpt::img::cmHOT;
489+
490+
/** If colormap!=mrpt::img::cmNONE, use this coordinate
491+
* as color index: 0=x 1=y 2=z
492+
*/
493+
uint8_t recolorizeByCoordinateIndex = 2;
485494
};
486495
TRenderOptions renderOptions;
487496

‎mola_metric_maps/src/NDT.cpp

Lines changed: 96 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -224,27 +224,84 @@ void NDT::getVisualizationInto(mrpt::opengl::CSetOfObjects& outObj) const
224224
MRPT_START
225225
if (!genericMapParams.enableSaveAs3DObject) return;
226226

227+
// Calculate histograms / stats:
228+
auto bb = this->boundingBox();
229+
230+
// handle planar maps (avoids error in histogram below):
231+
for (int i = 0; i < 3; i++)
232+
if (bb.max[i] - bb.min[i] < 0.1f) bb.max[i] = bb.min[i] + 0.1f;
233+
234+
// Use a histogram to discard outliers from the colormap extremes:
235+
constexpr size_t nBins = 100;
236+
// for x,y,z
237+
std::array<mrpt::math::CHistogram, 3> hists = {
238+
mrpt::math::CHistogram(bb.min.x, bb.max.x, nBins),
239+
mrpt::math::CHistogram(bb.min.y, bb.max.y, nBins),
240+
mrpt::math::CHistogram(bb.min.z, bb.max.z, nBins)};
241+
242+
size_t nPoints = 0;
243+
244+
const auto lambdaVisitPointsForHist =
245+
[&hists, &nPoints](const mrpt::math::TPoint3Df& pt)
246+
{
247+
for (int i = 0; i < 3; i++) hists[i].add(pt[i]);
248+
nPoints++;
249+
};
250+
251+
this->visitAllPoints(lambdaVisitPointsForHist);
252+
253+
float recolorMin = .0, recolorMax = 1.f;
254+
if (nPoints)
255+
{
256+
// Analyze the histograms and get confidence intervals:
257+
std::vector<double> coords;
258+
std::vector<double> hits;
259+
260+
const int idx = renderOptions.recolorizeByCoordinateIndex;
261+
ASSERT_(idx >= 0 && idx < 3);
262+
263+
constexpr double confidenceInterval = 0.02;
264+
265+
hists[idx].getHistogramNormalized(coords, hits);
266+
mrpt::math::confidenceIntervalsFromHistogram(
267+
coords, hits, recolorMin, recolorMax, confidenceInterval);
268+
}
269+
const float recolorK =
270+
recolorMax != recolorMin ? 1.0f / (recolorMax - recolorMin) : 1.0f;
271+
272+
// points:
227273
MRPT_TODO("option to hide points already with NDT");
228274
if (renderOptions.points_visible)
229275
{
230-
auto obj = mrpt::opengl::CPointCloud::Create();
276+
auto obj = mrpt::opengl::CPointCloudColoured::Create();
231277

232-
const auto lambdaVisitPoints = [&obj](const mrpt::math::TPoint3Df& pt)
233-
{ obj->insertPoint(pt); };
278+
const auto lambdaVisitPoints = [&obj](const mrpt::math::TPoint3Df& pt) {
279+
obj->insertPoint({pt.x, pt.y, pt.z, 0, 0, 0});
280+
};
234281
this->visitAllPoints(lambdaVisitPoints);
235282

236-
obj->setColor(renderOptions.points_color);
283+
if (renderOptions.points_colormap == mrpt::img::cmNONE)
284+
obj->setColor(renderOptions.points_color);
285+
else
286+
{
287+
if (!obj->empty())
288+
obj->recolorizeByCoordinate(
289+
recolorMin, recolorMax,
290+
renderOptions.recolorizeByCoordinateIndex,
291+
renderOptions.points_colormap);
292+
}
293+
237294
obj->setPointSize(renderOptions.point_size);
238-
obj->enableColorFromZ(false);
239295
outObj.insert(obj);
240296
}
241297

298+
// planes:
242299
if (renderOptions.planes_visible)
243300
{
244301
auto obj = mrpt::opengl::CSetOfTriangles::Create();
245302

246303
const auto lambdaVisitVoxel =
247-
[&obj, this](
304+
[&obj, recolorK, recolorMin, this](
248305
[[maybe_unused]] const global_index3d_t& idx,
249306
const VoxelData& v)
250307
{
@@ -264,7 +321,18 @@ void NDT::getVisualizationInto(mrpt::opengl::CSetOfObjects& outObj) const
264321
const auto vy = ndt->eigVectors.at(2).cast<float>() * s;
265322

266323
mrpt::opengl::TTriangle t;
267-
t.setColor(renderOptions.planes_color);
324+
325+
if (renderOptions.planes_colormap == mrpt::img::cmNONE)
326+
t.setColor(renderOptions.planes_color);
327+
else
328+
{
329+
t.setColor(mrpt::img::colormap(
330+
renderOptions.planes_colormap,
331+
recolorK *
332+
(center[renderOptions.recolorizeByCoordinateIndex] -
333+
recolorMin)));
334+
}
335+
268336
t.vertices[0].xyzrgba.pt = center + vx + vy;
269337
t.vertices[1].xyzrgba.pt = center - vx - vy;
270338
t.vertices[2].xyzrgba.pt = center + vx - vy;
@@ -282,6 +350,7 @@ void NDT::getVisualizationInto(mrpt::opengl::CSetOfObjects& outObj) const
282350
outObj.insert(obj);
283351
}
284352

353+
// normals:
285354
if (renderOptions.normals_visible)
286355
{
287356
auto obj = mrpt::opengl::CSetOfLines::Create();
@@ -302,7 +371,7 @@ void NDT::getVisualizationInto(mrpt::opengl::CSetOfObjects& outObj) const
302371

303372
// eigenVector[0] is the plane normal.
304373
// [1] and [2] are parallel to the plane surface:
305-
const float s = voxel_size_ * 0.5f;
374+
const float s = voxel_size_ * 0.2f;
306375
const auto vz = ndt->eigVectors.at(0).cast<float>() * s;
307376

308377
const auto p1 = center;
@@ -777,11 +846,14 @@ void NDT::TLikelihoodOptions::readFromStream(mrpt::serialization::CArchive& in)
777846
void NDT::TRenderOptions::writeToStream(
778847
mrpt::serialization::CArchive& out) const
779848
{
780-
const int8_t version = 0;
849+
const int8_t version = 1;
781850
out << version;
782851
out << points_visible << point_size << points_color;
783852
out << planes_visible << planes_color;
784853
out << normals_visible << normals_color;
854+
out << static_cast<int8_t>(points_colormap)
855+
<< static_cast<int8_t>(planes_colormap)
856+
<< recolorizeByCoordinateIndex; // v1
785857
}
786858

787859
void NDT::TRenderOptions::readFromStream(mrpt::serialization::CArchive& in)
@@ -791,10 +863,17 @@ void NDT::TRenderOptions::readFromStream(mrpt::serialization::CArchive& in)
791863
switch (version)
792864
{
793865
case 0:
866+
case 1:
794867
{
795868
in >> points_visible >> point_size >> points_color;
796869
in >> planes_visible >> planes_color;
797870
in >> normals_visible >> normals_color;
871+
if (version >= 1)
872+
{
873+
in.ReadAsAndCastTo<int8_t>(this->points_colormap);
874+
in.ReadAsAndCastTo<int8_t>(this->planes_colormap);
875+
in >> recolorizeByCoordinateIndex;
876+
}
798877
}
799878
break;
800879
default:
@@ -842,6 +921,10 @@ void NDT::TRenderOptions::dumpToTextStream(std::ostream& out) const
842921
LOADABLEOPTS_DUMP_VAR(normals_color.R, float);
843922
LOADABLEOPTS_DUMP_VAR(normals_color.G, float);
844923
LOADABLEOPTS_DUMP_VAR(normals_color.B, float);
924+
925+
LOADABLEOPTS_DUMP_VAR(points_colormap, int);
926+
LOADABLEOPTS_DUMP_VAR(planes_colormap, int);
927+
LOADABLEOPTS_DUMP_VAR(recolorizeByCoordinateIndex, int);
845928
}
846929

847930
void NDT::TInsertionOptions::loadFromConfigFile(
@@ -879,6 +962,10 @@ void NDT::TRenderOptions::loadFromConfigFile(
879962
MRPT_LOAD_CONFIG_VAR(normals_color.R, float, c, s);
880963
MRPT_LOAD_CONFIG_VAR(normals_color.G, float, c, s);
881964
MRPT_LOAD_CONFIG_VAR(normals_color.B, float, c, s);
965+
966+
points_colormap = c.read_enum(s, "points_colormap", this->points_colormap);
967+
planes_colormap = c.read_enum(s, "planes_colormap", this->planes_colormap);
968+
MRPT_LOAD_CONFIG_VAR(recolorizeByCoordinateIndex, int, c, s);
882969
}
883970

884971
void NDT::internal_insertPointCloud3D(

0 commit comments

Comments
 (0)