Skip to content

FIX: Use faster insertion in MRPT 2.15.0 #11

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
jlblancoc merged 1 commit into from
Oct 26, 2025
Merged

FIX: Use faster insertion in MRPT 2.15.0 #11

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
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
21 changes: 20 additions & 1 deletion mp2p_icp_filters/src/FilterBoundingBox.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -22,6 +22,7 @@
#include <mp2p_icp_filters/GetOrCreatePointLayer.h>
#include <mrpt/containers/yaml.h>
#include <mrpt/math/ops_containers.h> // dotProduct
#include <mrpt/version.h>

IMPLEMENTS_MRPT_OBJECT(FilterBoundingBox, mp2p_icp_filters::FilterBase, mp2p_icp_filters)

Expand Down Expand Up @@ -105,6 +106,18 @@ void FilterBoundingBox::filter(mp2p_icp::metric_map_t& inOut) const
outsidePc->reserve(outsidePc->size() + pc.size() / 10);
}

#if MRPT_VERSION >= 0x020f00 // 2.15.0
mrpt::maps::CPointsMap::InsertCtx ctxOutside, ctxInside;
if (insidePc)
{
ctxInside = insidePc->prepareForInsertPointsFrom(pc);
}
if (outsidePc)
{
ctxOutside = outsidePc->prepareForInsertPointsFrom(pc);
}
#endif

const auto& xs = pc.getPointsBufferRef_x();
const auto& ys = pc.getPointsBufferRef_y();
const auto& zs = pc.getPointsBufferRef_z();
Expand All @@ -114,10 +127,16 @@ void FilterBoundingBox::filter(mp2p_icp::metric_map_t& inOut) const
const bool isInside = params.bounding_box.containsPoint({xs[i], ys[i], zs[i]});

auto* targetPc = isInside ? insidePc.get() : outsidePc.get();

#if MRPT_VERSION >= 0x020f00 // 2.15.0
auto* ctx = isInside ? &ctxInside : &ctxOutside;
#endif
if (targetPc)
{
#if MRPT_VERSION >= 0x020f00 // 2.15.0
targetPc->insertPointFrom(pc, i, *ctx);
#else
targetPc->insertPointFrom(pc, i);
#endif
}
}

Expand Down
32 changes: 32 additions & 0 deletions mp2p_icp_filters/src/FilterByIntensity.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -125,6 +125,22 @@ void FilterByIntensity::filter(mp2p_icp::metric_map_t& inOut) const
ASSERT_EQUAL_(Is.size(), xs.size());
const size_t N = xs.size();

#if MRPT_VERSION >= 0x020f00 // 2.15.0
mrpt::maps::CPointsMap::InsertCtx ctxLow, ctxHigh, ctxMid;
if (outLow)
{
ctxLow = outLow->prepareForInsertPointsFrom(pc);
}
if (outHigh)
{
ctxHigh = outHigh->prepareForInsertPointsFrom(pc);
}
if (outMid)
{
ctxMid = outMid->prepareForInsertPointsFrom(pc);
}
#endif

size_t countLow = 0, countMid = 0, countHigh = 0;

for (size_t i = 0; i < N; i++)
Expand All @@ -133,25 +149,41 @@ void FilterByIntensity::filter(mp2p_icp::metric_map_t& inOut) const

mrpt::maps::CPointsMap* trg = nullptr;

#if MRPT_VERSION >= 0x020f00 // 2.15.0
mrpt::maps::CPointsMap::InsertCtx* ctx = nullptr;
#endif
if (I < params.low_threshold)
{
trg = outLow.get();
++countLow;
#if MRPT_VERSION >= 0x020f00 // 2.15.0
ctx = &ctxLow;
#endif
}
else if (I > params.high_threshold)
{
trg = outHigh.get();
++countHigh;
#if MRPT_VERSION >= 0x020f00 // 2.15.0
ctx = &ctxHigh;
#endif
}
else
{
trg = outMid.get();
++countMid;
#if MRPT_VERSION >= 0x020f00 // 2.15.0
ctx = &ctxMid;
#endif
}

if (trg)
{
#if MRPT_VERSION >= 0x020f00 // 2.15.0
trg->insertPointFrom(pc, i, *ctx);
#else
trg->insertPointFrom(pc, i);
#endif
}
}

Expand Down
19 changes: 19 additions & 0 deletions mp2p_icp_filters/src/FilterByRange.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -22,6 +22,7 @@
#include <mp2p_icp_filters/GetOrCreatePointLayer.h>
#include <mrpt/containers/yaml.h>
#include <mrpt/math/TPoint3D.h>
#include <mrpt/version.h>

IMPLEMENTS_MRPT_OBJECT(FilterByRange, mp2p_icp_filters::FilterBase, mp2p_icp_filters)

Expand Down Expand Up @@ -111,6 +112,19 @@ void FilterByRange::filter(mp2p_icp::metric_map_t& inOut) const
const float sqrMin = mrpt::square(params.range_min);
const float sqrMax = mrpt::square(params.range_max);

#if MRPT_VERSION >= 0x020f00 // 2.15.0
std::optional<mrpt::maps::CPointsMap::InsertCtx> ctxBetween;
std::optional<mrpt::maps::CPointsMap::InsertCtx> ctxOutside;
if (outBetween)
{
ctxBetween = outBetween->prepareForInsertPointsFrom(pc);
}
if (outOutside)
{
ctxOutside = outOutside->prepareForInsertPointsFrom(pc);
}
#endif

for (size_t i = 0; i < xs.size(); i++)
{
bool isInside;
Expand All @@ -135,7 +149,12 @@ void FilterByRange::filter(mp2p_icp::metric_map_t& inOut) const

if (targetPc)
{
#if MRPT_VERSION >= 0x020f00 // 2.15.0
const auto& ctx = isInside ? ctxBetween : ctxOutside;
targetPc->insertPointFrom(pc, i, ctx.value());
#else
targetPc->insertPointFrom(pc, i);
#endif
}
}

Expand Down
26 changes: 26 additions & 0 deletions mp2p_icp_filters/src/FilterByRing.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -119,6 +119,19 @@ void FilterByRing::filter(mp2p_icp::metric_map_t& inOut) const
params.input_pointcloud_layer.c_str());
}

#if MRPT_VERSION >= 0x020f00 // 2.15.0
mrpt::maps::CPointsMap::InsertCtx ctxSelected;
if (outSelected)
{
ctxSelected = outSelected->prepareForInsertPointsFrom(pc);
}
mrpt::maps::CPointsMap::InsertCtx ctxNotSelected;
if (outNonSel)
{
ctxNotSelected = outNonSel->prepareForInsertPointsFrom(pc);
}
#endif

const auto& Rs = *ptrR;
ASSERT_EQUAL_(Rs.size(), xs.size());
const size_t N = xs.size();
Expand All @@ -130,21 +143,34 @@ void FilterByRing::filter(mp2p_icp::metric_map_t& inOut) const
const auto R = Rs[i];

mrpt::maps::CPointsMap* trg = nullptr;
#if MRPT_VERSION >= 0x020f00 // 2.15.0
mrpt::maps::CPointsMap::InsertCtx* ctx = nullptr;
#endif

if (params.selected_ring_ids.count(R) != 0)
{
trg = outSelected.get();
++countSel;
#if MRPT_VERSION >= 0x020f00 // 2.15.0
ctx = &ctxSelected;
#endif
}
else
{
trg = outNonSel.get();
++countNon;
#if MRPT_VERSION >= 0x020f00 // 2.15.0
ctx = &ctxNotSelected;
#endif
}

if (trg)
{
#if MRPT_VERSION >= 0x020f00 // 2.15.0
trg->insertPointFrom(pc, i, *ctx);
#else
trg->insertPointFrom(pc, i);
#endif
}
}

Expand Down
40 changes: 39 additions & 1 deletion mp2p_icp_filters/src/FilterCurvature.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -131,6 +131,24 @@ void FilterCurvature::filter(mp2p_icp::metric_map_t& inOut) const

const size_t N = xs.size();

#if MRPT_VERSION >= 0x020f00 // 2.15.0
mrpt::maps::CPointsMap::InsertCtx ctxLarger;
if (outPcLarger)
{
ctxLarger = outPcLarger->prepareForInsertPointsFrom(pc);
}
mrpt::maps::CPointsMap::InsertCtx ctxSmaller;
if (outPcSmaller)
{
ctxSmaller = outPcSmaller->prepareForInsertPointsFrom(pc);
}
mrpt::maps::CPointsMap::InsertCtx ctxOther;
if (outPcOther)
{
ctxOther = outPcOther->prepareForInsertPointsFrom(pc);
}
#endif

const uint16_t nRings = 1 + *std::max_element(ringPerPt.begin(), ringPerPt.end());

const auto estimPtsPerRing = N / nRings;
Expand Down Expand Up @@ -202,14 +220,18 @@ void FilterCurvature::filter(mp2p_icp::metric_map_t& inOut) const
if (idxs.size() <= 3)
{
// If we have too few points, just accept them as they are so few we
// cannot run the clasification method below.
// cannot run the clarification method below.
for (size_t idx = 0; idx < idxs.size(); idx++)
{
const size_t i = idxs[idx];
counterLarger++;
if (outPcLarger)
{
#if MRPT_VERSION >= 0x020f00 // 2.15.0
outPcLarger->insertPointFrom(pc, i, ctxLarger);
#else
outPcLarger->insertPointFrom(pc, i);
#endif
}
}
continue;
Expand All @@ -236,14 +258,22 @@ void FilterCurvature::filter(mp2p_icp::metric_map_t& inOut) const
counterLarger++;
if (outPcLarger)
{
#if MRPT_VERSION >= 0x020f00 // 2.15.0
outPcLarger->insertPointFrom(pc, i, ctxLarger);
#else
outPcLarger->insertPointFrom(pc, i);
#endif
}
}
else
{
if (outPcOther)
{
#if MRPT_VERSION >= 0x020f00 // 2.15.0
outPcOther->insertPointFrom(pc, i, ctxOther);
#else
outPcOther->insertPointFrom(pc, i);
#endif
}
}
continue;
Expand All @@ -261,15 +291,23 @@ void FilterCurvature::filter(mp2p_icp::metric_map_t& inOut) const
counterLarger++;
if (outPcLarger)
{
#if MRPT_VERSION >= 0x020f00 // 2.15.0
outPcLarger->insertPointFrom(pc, i, ctxLarger);
#else
outPcLarger->insertPointFrom(pc, i);
#endif
}
}
else
{
counterLess++;
if (outPcSmaller)
{
#if MRPT_VERSION >= 0x020f00 // 2.15.0
outPcSmaller->insertPointFrom(pc, i, ctxSmaller);
#else
outPcSmaller->insertPointFrom(pc, i);
#endif
}
}
}
Expand Down
9 changes: 9 additions & 0 deletions mp2p_icp_filters/src/FilterDecimateAdaptive.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -22,6 +22,7 @@
#include <mp2p_icp_filters/GetOrCreatePointLayer.h>
#include <mrpt/containers/yaml.h>
#include <mrpt/core/round.h>
#include <mrpt/version.h>

#if defined(MP2P_HAS_TBB)
#include <tbb/enumerable_thread_specific.h>
Expand Down Expand Up @@ -174,6 +175,10 @@ void FilterDecimateAdaptive::filter(mp2p_icp::metric_map_t& inOut) const

#endif

#if MRPT_VERSION >= 0x020f00 // 2.15.0
mrpt::maps::CPointsMap::InsertCtx ctx = outPc->prepareForInsertPointsFrom(pc);
#endif

// Perform resampling:
// -------------------
constexpr int FRACTIONARY_BIT_COUNT = 12;
Expand Down Expand Up @@ -216,7 +221,11 @@ void FilterDecimateAdaptive::filter(mp2p_icp::metric_map_t& inOut) const
if (!ith.exhausted)
{
auto ptIdx = ith.voxel->indices[ith.nextIdx++];
#if MRPT_VERSION >= 0x020f00 // 2.15.0
outPc->insertPointFrom(pc, ptIdx, ctx);
#else
outPc->insertPointFrom(pc, ptIdx);
#endif
anyInsertInTheRound = true;

if (ith.nextIdx >= ith.voxel->indices.size())
Expand Down
Loading