Skip to content

Commit 0fb9d7d

Browse files
committed
Code clean up (remove now dead code; mrpt backwards compatibility)
1 parent 63cc0fd commit 0fb9d7d

4 files changed

Lines changed: 3 additions & 93 deletions

File tree

mp2p_icp_filters/include/mp2p_icp_filters/FilterDeskew.h

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -61,8 +61,8 @@ enum class MotionCompensationMethod : uint8_t
6161
* reference time point, which can be the start or middle point of the scan. This can be
6262
* controlled by adding a FilterAdjustTimestamps before this block.
6363
*
64-
* - The input layer must contain a point cloud in the format
65-
* mrpt::maps::CPointsMapXYZIRT or mrpt::maps::CGenericPointsMap so timestamps are present.
64+
* - The input layer must contain a point cloud of type mrpt::maps::CGenericPointsMap
65+
* so timestamps are present.
6666
*
6767
* - If the input layer is of a different type, or the `t` field is missing,
6868
* an exception will be thrown by default, unless the option

mp2p_icp_filters/src/FilterDecimateVoxels.cpp

Lines changed: 0 additions & 31 deletions
Original file line numberDiff line numberDiff line change
@@ -25,11 +25,6 @@
2525
#include <mrpt/math/ops_containers.h> // dotProduct
2626
#include <mrpt/random/RandomGenerators.h>
2727

28-
//
29-
#include <mrpt/maps/CPointsMapXYZI.h>
30-
#include <mrpt/maps/CPointsMapXYZIRT.h>
31-
#include <mrpt/version.h>
32-
3328
IMPLEMENTS_MRPT_OBJECT(FilterDecimateVoxels, mp2p_icp_filters::FilterBase, mp2p_icp_filters)
3429

3530
using namespace mp2p_icp_filters;
@@ -174,11 +169,9 @@ void FilterDecimateVoxels::filter(mp2p_icp::metric_map_t& inOut) const
174169
const auto& xs = pcPtrs[mapIdx]->getPointsBufferRef_x();
175170
const auto& ys = pcPtrs[mapIdx]->getPointsBufferRef_y();
176171

177-
#if MRPT_VERSION >= 0x020f00 // 2.15.0
178172
outPc->registerPointFieldsFrom(*pcPtrs[mapIdx]);
179173
mrpt::maps::CPointsMap::InsertCtx ctxOut =
180174
outPc->prepareForInsertPointsFrom(*pcPtrs[mapIdx]);
181-
#endif
182175

183176
for (size_t i = 0; i < xs.size(); i++)
184177
{
@@ -188,13 +181,7 @@ void FilterDecimateVoxels::filter(mp2p_icp::metric_map_t& inOut) const
188181
}
189182
else
190183
{
191-
#if MRPT_VERSION >= 0x020f03 // 2.15.3
192184
outPc->insertPointFrom(i, ctxOut);
193-
#elif MRPT_VERSION >= 0x020f00 // 2.15.0
194-
outPc->insertPointFrom(*pcPtrs[mapIdx], i, ctxOut);
195-
#else
196-
outPc->insertPointFrom(*pcPtrs[mapIdx], i);
197-
#endif
198185
}
199186
}
200187
}
@@ -233,9 +220,7 @@ void FilterDecimateVoxels::filter(mp2p_icp::metric_map_t& inOut) const
233220
std::set<PointCloudToVoxelGridSingle::indices_t, PointCloudToVoxelGridSingle::IndicesHash>
234221
flattenUsedBins;
235222

236-
#if MRPT_VERSION >= 0x020f00 // 2.15.0
237223
std::map<const mrpt::maps::CPointsMap*, mrpt::maps::CPointsMap::InsertCtx> ctxs;
238-
#endif
239224

240225
grid.visit_voxels(
241226
[&](const PointCloudToVoxelGridSingle::indices_t& idx,
@@ -269,11 +254,7 @@ void FilterDecimateVoxels::filter(mp2p_icp::metric_map_t& inOut) const
269254
outPc->registerPointFieldsFrom(*pc);
270255
ctx = outPc->prepareForInsertPointsFrom(*pc);
271256
}
272-
#if MRPT_VERSION >= 0x020f03 // 2.15.3
273257
outPc->insertPointFrom(*vxl.pointIdx, ctx);
274-
#else
275-
outPc->insertPointFrom(*pc, *vxl.pointIdx, ctx);
276-
#endif
277258
// Actual flatten in "z":
278259
outPc->getPointsBufferRef_float_field("z")->back() =
279260
static_cast<float>(*params.flatten_to);
@@ -288,11 +269,7 @@ void FilterDecimateVoxels::filter(mp2p_icp::metric_map_t& inOut) const
288269
outPc->registerPointFieldsFrom(*pc);
289270
ctx = outPc->prepareForInsertPointsFrom(*pc);
290271
}
291-
#if MRPT_VERSION >= 0x020f03 // 2.15.3
292272
outPc->insertPointFrom(*vxl.pointIdx, ctx);
293-
#else
294-
outPc->insertPointFrom(*pc, *vxl.pointIdx, ctx);
295-
#endif
296273
}
297274
});
298275
}
@@ -326,10 +303,8 @@ void FilterDecimateVoxels::filter(mp2p_icp::metric_map_t& inOut) const
326303
std::set<PointCloudToVoxelGrid::indices_t, PointCloudToVoxelGrid::IndicesHash>
327304
flattenUsedBins;
328305

329-
#if MRPT_VERSION >= 0x020f00 // 2.15.0
330306
outPc->registerPointFieldsFrom(pc);
331307
auto ctx = outPc->prepareForInsertPointsFrom(pc);
332-
#endif
333308

334309
grid.visit_voxels(
335310
[&](const PointCloudToVoxelGrid::indices_t& idx,
@@ -440,13 +415,7 @@ void FilterDecimateVoxels::filter(mp2p_icp::metric_map_t& inOut) const
440415
}
441416
else
442417
{
443-
#if MRPT_VERSION >= 0x020f03 // 2.15.3
444418
outPc->insertPointFrom(insertPtIdx, ctx);
445-
#elif MRPT_VERSION >= 0x020f00 // 2.15.0
446-
outPc->insertPointFrom(pc, insertPtIdx, ctx);
447-
#else
448-
outPc->insertPointFrom(pc, insertPtIdx);
449-
#endif
450419
}
451420
}
452421
});

mp2p_icp_filters/src/FilterDeskew.cpp

Lines changed: 0 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -29,8 +29,6 @@
2929
#include <mrpt/containers/yaml.h>
3030
#include <mrpt/core/Clock.h>
3131
#include <mrpt/core/exceptions.h>
32-
#include <mrpt/maps/CPointsMapXYZIRT.h>
33-
#include <mrpt/maps/CSimplePointsMap.h>
3432
#include <mrpt/poses/Lie/SO.h>
3533
#include <mrpt/version.h>
3634

mp2p_icp_map/src/pointcloud_sanity_check.cpp

Lines changed: 1 addition & 58 deletions
Original file line numberDiff line numberDiff line change
@@ -21,21 +21,14 @@
2121

2222
#include <mp2p_icp/pointcloud_sanity_check.h>
2323
#include <mrpt/core/exceptions.h>
24-
#include <mrpt/version.h>
25-
26-
#if MRPT_VERSION >= 0x20f00 // 2.15.0
2724
#include <mrpt/maps/CPointsMap.h>
28-
#else
29-
#include <mrpt/maps/CPointsMapXYZI.h>
30-
#include <mrpt/maps/CPointsMapXYZIRT.h>
31-
#endif
25+
#include <mrpt/version.h>
3226

3327
bool mp2p_icp::pointcloud_sanity_check(const mrpt::maps::CPointsMap& pc, bool printWarnings)
3428
{
3529
bool ok = true;
3630
const size_t n = pc.size();
3731

38-
#if MRPT_VERSION >= 0x20f00 // 2.15.0
3932
for (const auto& field : pc.getPointFieldNames_float())
4033
{
4134
const auto& vec = pc.getPointsBufferRef_float_field(field);
@@ -52,11 +45,7 @@ bool mp2p_icp::pointcloud_sanity_check(const mrpt::maps::CPointsMap& pc, bool pr
5245
}
5346
for (const auto& field : pc.getPointFieldNames_uint16())
5447
{
55-
#if MRPT_VERSION >= 0x20f04 // 2.15.4
5648
const auto& vec = pc.getPointsBufferRef_uint16_field(field);
57-
#else
58-
const auto& vec = pc.getPointsBufferRef_uint_field(field);
59-
#endif
6049
if (vec && !vec->empty() && vec->size() != n)
6150
{
6251
ok = false;
@@ -69,7 +58,6 @@ bool mp2p_icp::pointcloud_sanity_check(const mrpt::maps::CPointsMap& pc, bool pr
6958
}
7059
}
7160

72-
#if MRPT_VERSION >= 0x20f03 // 2.15.3
7361
for (const auto& field : pc.getPointFieldNames_double())
7462
{
7563
const auto& vec = pc.getPointsBufferRef_double_field(field);
@@ -98,51 +86,6 @@ bool mp2p_icp::pointcloud_sanity_check(const mrpt::maps::CPointsMap& pc, bool pr
9886
}
9987
}
10088
}
101-
#endif
10289

103-
#else
104-
if (auto* pcIRT = dynamic_cast<const mrpt::maps::CPointsMapXYZIRT*>(&pc); pcIRT)
105-
{
106-
if (pcIRT->hasIntensityField() && pcIRT->getPointsBufferRef_intensity()->size() != n)
107-
{
108-
ok = false;
109-
if (printWarnings)
110-
std::cerr << "[mp2p_icp] XYZIRT WARNING: Intensity channel has "
111-
"incorrect length="
112-
<< pcIRT->getPointsBufferRef_intensity()->size() << " expected=" << n
113-
<< std::endl;
114-
}
115-
if (pcIRT->hasRingField() && pcIRT->getPointsBufferRef_ring()->size() != n)
116-
{
117-
ok = false;
118-
if (printWarnings)
119-
std::cerr << "[mp2p_icp] XYZIRT WARNING: Ring channel has "
120-
"incorrect length="
121-
<< pcIRT->getPointsBufferRef_ring()->size() << " expected=" << n
122-
<< std::endl;
123-
}
124-
if (pcIRT->hasTimeField() && pcIRT->getPointsBufferRef_timestamp()->size() != n)
125-
{
126-
ok = false;
127-
if (printWarnings)
128-
std::cerr << "[mp2p_icp] XYZIRT WARNING: Timestamp channel has "
129-
"incorrect length="
130-
<< pcIRT->getPointsBufferRef_timestamp()->size() << " expected=" << n
131-
<< std::endl;
132-
}
133-
}
134-
else if (auto* pcI = dynamic_cast<const mrpt::maps::CPointsMapXYZI*>(&pc); pcI)
135-
{
136-
if (pcI->getPointsBufferRef_intensity() && pcI->getPointsBufferRef_intensity()->size() != n)
137-
{
138-
ok = false;
139-
if (printWarnings)
140-
std::cerr << "[mp2p_icp] XYZI WARNING: Intensity channel has "
141-
"incorrect length="
142-
<< pcI->getPointsBufferRef_intensity()->size() << " expected=" << n
143-
<< std::endl;
144-
}
145-
}
146-
#endif
14790
return ok;
14891
}

0 commit comments

Comments
 (0)