Skip to content
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
27 changes: 19 additions & 8 deletions apps/mm2txt/main.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -193,19 +193,30 @@ void run_mm2txt()
saveToTxt(*genxyz, filName, printHeader);
}
#if MRPT_VERSION < 0x030000 // <3.0.0
else if (auto* xyzirt = dynamic_cast<const mrpt::maps::CPointsMapXYZIRT*>(pts); xyzirt)
{
xyzirt->saveXYZIRT_to_text_file(filName);
}
else if (auto* xyzi = dynamic_cast<const mrpt::maps::CPointsMapXYZI*>(pts); xyzi)
else
{
xyzi->saveXYZI_to_text_file(filName);
}
#endif
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wdeprecated-declarations"
if (auto* xyzirt = dynamic_cast<const mrpt::maps::CPointsMapXYZIRT*>(pts); xyzirt)
{
xyzirt->saveXYZIRT_to_text_file(filName);
}
else if (auto* xyzi = dynamic_cast<const mrpt::maps::CPointsMapXYZI*>(pts); xyzi)
{
xyzi->saveXYZI_to_text_file(filName);
}
else
{
pts->save3D_to_text_file(filName);
}
#pragma GCC diagnostic pop
}
#else
else
{
pts->save3D_to_text_file(filName);
}
#endif
}
}

Expand Down
40 changes: 14 additions & 26 deletions apps/txt2mm/main.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -37,6 +37,11 @@

const char* VALID_FORMATS = "(xyz|xyzi|xyzirt|xyzrgb|xyzrgb_normalized)";

// Replicated here from CGenericPointsMap until MRPT 2.15.3 is minimum required version:
constexpr static std::string_view POINT_FIELD_INTENSITY = "intensity";
constexpr static std::string_view POINT_FIELD_RING_ID = "ring";
constexpr static std::string_view POINT_FIELD_TIMESTAMP = "t";

using namespace std::string_literals;

// CLI flags:
Expand Down Expand Up @@ -161,12 +166,8 @@ int main(int argc, char** argv)
else if (format == "xyzi")
{
ASSERT_GE_(nCols, 4U);
#if MRPT_VERSION >= 0x20f00 // 2.15.0
auto pts = mrpt::maps::CGenericPointsMap::Create();
pts->registerField_float("intensity");
#else
auto pts = mrpt::maps::CPointsMapXYZI::Create();
#endif
pts->registerField_float(POINT_FIELD_INTENSITY);
pts->reserve(nRows);
if (nCols > 4)
{
Expand All @@ -178,26 +179,18 @@ int main(int argc, char** argv)
for (size_t i = 0; i < nRows; i++)
{
pts->insertPointFast(data(i, idxX + 0), data(i, idxX + 1), data(i, idxX + 2));
#if MRPT_VERSION >= 0x20f00 // 2.15.0
pts->insertPointField_float("intensity", data(i, idxI));
#else
pts->insertPointField_Intensity(data(i, idxI));
#endif
pts->insertPointField_float(POINT_FIELD_INTENSITY, data(i, idxI));
}

pc = pts;
}
else if (format == "xyzirt")
{
ASSERT_GE_(nCols, 6U);
#if MRPT_VERSION >= 0x20f00 // 2.15.0
auto pts = mrpt::maps::CGenericPointsMap::Create();
pts->registerField_float("intensity");
pts->registerField_uint16("ring");
pts->registerField_float("timestamp");
#else
auto pts = mrpt::maps::CPointsMapXYZI::Create();
#endif
pts->registerField_float(POINT_FIELD_INTENSITY);
pts->registerField_uint16(POINT_FIELD_RING_ID);
pts->registerField_float(POINT_FIELD_TIMESTAMP);
pts->reserve(nRows);
if (nCols > 6)
{
Expand All @@ -209,15 +202,10 @@ int main(int argc, char** argv)
for (size_t i = 0; i < nRows; i++)
{
pts->insertPointFast(data(i, idxX + 0), data(i, idxX + 1), data(i, idxX + 2));
#if MRPT_VERSION >= 0x20f00 // 2.15.0
pts->insertPointField_float("intensity", data(i, idxI));
pts->insertPointField_uint16("ring", static_cast<uint16_t>(data(i, idxR)));
pts->insertPointField_float("timestamp", data(i, idxT));
#else
pts->insertPointField_Intensity(data(i, idxI));
pts->insertPointField_Ring(data(i, idxR));
pts->insertPointField_Timestamp(data(i, idxT));
#endif
pts->insertPointField_float(POINT_FIELD_INTENSITY, data(i, idxI));
pts->insertPointField_uint16(
POINT_FIELD_RING_ID, static_cast<uint16_t>(data(i, idxR)));
pts->insertPointField_float(POINT_FIELD_TIMESTAMP, data(i, idxT));
}

pc = pts;
Expand Down
13 changes: 13 additions & 0 deletions mp2p_icp_filters/include/mp2p_icp_filters/FilterBase.h
Original file line number Diff line number Diff line change
Expand Up @@ -29,6 +29,7 @@
#include <mrpt/obs/obs_frwds.h>
#include <mrpt/rtti/CObject.h>
#include <mrpt/system/COutputLogger.h>
#include <mrpt/version.h>

namespace mrpt::system
{
Expand Down Expand Up @@ -117,6 +118,18 @@ void apply_filter_pipeline(
const std::string& filename,
const mrpt::system::VerbosityLevel& vLevel = mrpt::system::LVL_INFO);

// For convenience, define shortcut names for common point cloud field names:
#if MRPT_VERSION >= 0x20f03 // 2.15.3
constexpr auto POINT_FIELD_INTENSITY = mrpt::maps::CPointsMap::POINT_FIELD_INTENSITY;
constexpr auto POINT_FIELD_RING_ID = mrpt::maps::CPointsMap::POINT_FIELD_RING_ID;
constexpr auto POINT_FIELD_TIMESTAMP = mrpt::maps::CPointsMap::POINT_FIELD_TIMESTAMP;
#else
// Define here locally, until MRPT 2.15.3 is the minimum required version:
constexpr static std::string_view POINT_FIELD_INTENSITY = "intensity";
constexpr static std::string_view POINT_FIELD_RING_ID = "ring";
constexpr static std::string_view POINT_FIELD_TIMESTAMP = "t";
#endif

/** @} */

} // namespace mp2p_icp_filters
7 changes: 2 additions & 5 deletions mp2p_icp_filters/src/FilterByIntensity.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -109,11 +109,8 @@ void FilterByIntensity::filter(mp2p_icp::metric_map_t& inOut) const

const auto& xs = pc.getPointsBufferRef_x();

#if MRPT_VERSION >= 0x020f00 // 2.15.0
const auto* ptrI = pc.getPointsBufferRef_float_field("intensity");
#else
const auto* ptrI = pc.getPointsBufferRef_intensity();
#endif
const auto* ptrI = pc.getPointsBufferRef_float_field(POINT_FIELD_INTENSITY);

if (!ptrI || ptrI->empty())
{
THROW_EXCEPTION_FMT(
Expand Down
22 changes: 16 additions & 6 deletions mp2p_icp_filters/src/FilterDecimateVoxels.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -261,14 +261,27 @@ void FilterDecimateVoxels::filter(mp2p_icp::metric_map_t& inOut) const
// First time we see this (x,y) cell:
flattenUsedBins.insert(flattenIdx);

outPc->insertPointFast(
vxl.point->x, vxl.point->y, static_cast<float>(*params.flatten_to));
const auto* pc = vxl.source.value();

auto& ctx = ctxs[pc];
if (!ctx.xs_src)
{
outPc->registerPointFieldsFrom(*pc);
ctx = outPc->prepareForInsertPointsFrom(*pc);
}
#if MRPT_VERSION >= 0x020f03 // 2.15.3
outPc->insertPointFrom(*vxl.pointIdx, ctx);
#else
outPc->insertPointFrom(*pc, *vxl.pointIdx, ctx);
#endif
// Actual flatten in "z":
outPc->getPointsBufferRef_float_field("z")->back() =
static_cast<float>(*params.flatten_to);
}
else
{
const auto* pc = vxl.source.value();

#if MRPT_VERSION >= 0x020f00 // 2.15.0
auto& ctx = ctxs[pc];
if (!ctx.xs_src)
{
Expand All @@ -279,9 +292,6 @@ void FilterDecimateVoxels::filter(mp2p_icp::metric_map_t& inOut) const
outPc->insertPointFrom(*vxl.pointIdx, ctx);
#else
outPc->insertPointFrom(*pc, *vxl.pointIdx, ctx);
#endif
#else
outPc->insertPointFrom(*pc, *vxl.pointIdx);
#endif
}
});
Expand Down
6 changes: 1 addition & 5 deletions mp2p_icp_filters/src/FilterNormalizeIntensity.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -62,11 +62,7 @@ void FilterNormalizeIntensity::filter(mp2p_icp::metric_map_t& inOut) const

auto& pc = *pcPtr;

#if MRPT_VERSION >= 0x020f00 // 2.15.0
auto* IsPtr = pc.getPointsBufferRef_float_field("intensity");
#else
auto* IsPtr = pc.getPointsBufferRef_intensity();
#endif
auto* IsPtr = pc.getPointsBufferRef_float_field(POINT_FIELD_INTENSITY);

ASSERTMSG_(
IsPtr != nullptr,
Expand Down
8 changes: 4 additions & 4 deletions mp2p_icp_filters/src/Generator.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -30,8 +30,8 @@
#include <mrpt/config/CConfigFileMemory.h>
#include <mrpt/containers/yaml.h>
#include <mrpt/core/get_env.h>
#include <mrpt/maps/CGenericPointsMap.h>
#include <mrpt/maps/CMultiMetricMap.h>
#include <mrpt/maps/CPointsMapXYZIRT.h>
#include <mrpt/maps/CSimplePointsMap.h>
#include <mrpt/obs/CObservation2DRangeScan.h>
#include <mrpt/obs/CObservation3DRangeScan.h>
Expand Down Expand Up @@ -185,13 +185,13 @@ bool Generator::filterVelodyneScan( //
{
mrpt::maps::CPointsMap::Ptr outPc = GetOrCreatePointLayer(
out, params.target_layer, false /*does not allow empty name*/,
"mrpt::maps::CPointsMapXYZIRT" /* creation class if not existing */);
"mrpt::maps::CGenericPointsMap" /* creation class if not existing */);
ASSERT_(outPc);

auto m = std::dynamic_pointer_cast<mrpt::maps::CPointsMapXYZIRT>(outPc);
auto m = std::dynamic_pointer_cast<mrpt::maps::CGenericPointsMap>(outPc);
ASSERTMSG_(
m,
"Output layer must be of type mrpt::maps::CPointsMapXYZIRT for the "
"Output layer must be of type mrpt::maps::CGenericPointsMap for the "
"specialized filterVelodyneScan() generator.");

m->insertObservation(pc, robotPose);
Expand Down
6 changes: 3 additions & 3 deletions tests/test-mp2p_FilterByExpression.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -54,7 +54,7 @@ void FilterByExpression_SpatialFiltering()

void FilterByExpression_IntensityAndLogic()
{
constexpr auto INTENSITY = mrpt::maps::CPointsMap::POINT_FIELD_INTENSITY;
constexpr auto INTENSITY = POINT_FIELD_INTENSITY;

auto pc = mrpt::maps::CGenericPointsMap::Create();
pc->registerField_float(INTENSITY);
Expand Down Expand Up @@ -88,12 +88,12 @@ void FilterByExpression_IntensityAndLogic()
void FilterByExpression_CustomFields()
{
auto pc = mrpt::maps::CGenericPointsMap::Create();
pc->registerField_uint16(mrpt::maps::CPointsMap::POINT_FIELD_RING_ID);
pc->registerField_uint16(POINT_FIELD_RING_ID);
pc->registerField_uint8("SEMANTICS");
pc->registerField_double("latitude");

pc->insertPoint(0, 0, 0);
pc->insertPointField_uint16(mrpt::maps::CPointsMap::POINT_FIELD_RING_ID, 32);
pc->insertPointField_uint16(POINT_FIELD_RING_ID, 32);
pc->insertPointField_uint8("SEMANTICS", 12);
pc->insertPointField_double("latitude", 3.14);

Expand Down
19 changes: 7 additions & 12 deletions tests/test-mp2p_FilterDecimateVoxels.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -20,7 +20,7 @@

#include <mp2p_icp/metricmap.h>
#include <mp2p_icp_filters/FilterDecimateVoxels.h>
#include <mrpt/maps/CPointsMapXYZI.h>
#include <mrpt/maps/CGenericPointsMap.h>
#include <mrpt/math/ops_containers.h>
#include <mrpt/system/filesystem.h>
#include <mrpt/version.h>
Expand All @@ -31,7 +31,9 @@ using namespace mp2p_icp;
// Helper to create a consistent point map for testing
mrpt::maps::CPointsMap::Ptr createTestPoints(size_t n_x, size_t n_y)
{
auto pc = mrpt::maps::CPointsMapXYZI::Create();
auto pc = mrpt::maps::CGenericPointsMap::Create();
pc->registerField_float(POINT_FIELD_INTENSITY);

// Create a 2D grid of points, 10x10. Each point has a small, known Z-offset.
for (size_t i = 0; i < n_x; ++i)
{
Expand All @@ -43,12 +45,7 @@ mrpt::maps::CPointsMap::Ptr createTestPoints(size_t n_x, size_t n_y)
float intensity = static_cast<float>(i * n_y + j);

pc->insertPointFast(x, y, z);
#if MRPT_VERSION >= 0x020f00
pc->insertPointField_float(
mrpt::maps::CPointsMapXYZI::POINT_FIELD_INTENSITY, intensity);
#else
pc->insertPointField_Intensity(intensity);
#endif
pc->insertPointField_float(POINT_FIELD_INTENSITY, intensity);
}
}
return pc;
Expand Down Expand Up @@ -196,10 +193,8 @@ void test_decimate_method(
ASSERT_NEAR_(bb.min.z, 0.0f, 0.2f);
}

// Since we created CPointsMapXYZI, the output should also have intensity.
#if MRPT_VERSION >= 0x020f00
ASSERT_(output_pc->hasPointField(mrpt::maps::CPointsMapXYZI::POINT_FIELD_INTENSITY));
#endif
// The output should also have intensity.
ASSERT_(output_pc->hasPointField(POINT_FIELD_INTENSITY));
std::cout << " Success ✅." << std::endl;
}

Expand Down
12 changes: 5 additions & 7 deletions tests/test-mp2p_Filters.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -38,7 +38,7 @@ using namespace mp2p_icp;
namespace
{

constexpr std::string_view INTENSITY = "intensity";
constexpr std::string_view INTENSITY = POINT_FIELD_INTENSITY;

// Helper to create a test point cloud with known positions and intensities
mrpt::maps::CPointsMap::Ptr createTestPointsWithIntensity(
Expand Down Expand Up @@ -204,10 +204,9 @@ void test_FilterByIntensity_ThreeLayers()
ASSERT_EQUAL_(low_pc->size() + mid_pc->size() + high_pc->size(), input_pc->size());

// Verify intensity ranges
#if MRPT_VERSION >= 0x020f00
const auto* ptrI_low = low_pc->getPointsBufferRef_float_field("intensity");
const auto* ptrI_mid = mid_pc->getPointsBufferRef_float_field("intensity");
const auto* ptrI_high = high_pc->getPointsBufferRef_float_field("intensity");
const auto* ptrI_low = low_pc->getPointsBufferRef_float_field(POINT_FIELD_INTENSITY);
const auto* ptrI_mid = mid_pc->getPointsBufferRef_float_field(POINT_FIELD_INTENSITY);
const auto* ptrI_high = high_pc->getPointsBufferRef_float_field(POINT_FIELD_INTENSITY);

if (ptrI_low)
{
Expand All @@ -233,7 +232,6 @@ void test_FilterByIntensity_ThreeLayers()
ASSERT_GT_(I, 0.7f);
}
}
#endif

std::cout << "Success ✅" << std::endl;
}
Expand Down Expand Up @@ -614,7 +612,7 @@ void test_FilterMLS_DistinctCloudProjection()
{
avg_z += z;
}
avg_z /= mls_pc->size();
avg_z /= static_cast<double>(mls_pc->size());

// Average z should be closer to 2.0 than the distinct cloud's z=3.0
ASSERT_LT_(std::abs(avg_z - 2.0f), 0.5f);
Expand Down
Loading