2222#include < mp2p_icp/metricmap.h>
2323#include < mp2p_icp_filters/FilterDeskew.h>
2424#include < mp2p_icp_filters/sm2mm.h>
25- #include < mrpt/maps/CPointsMapXYZIRT .h>
25+ #include < mrpt/maps/CGenericPointsMap .h>
2626#include < mrpt/maps/CSimplePointsMap.h>
2727#include < mrpt/math/TPoint3D.h>
2828#include < mrpt/math/TTwist3D.h>
@@ -47,7 +47,7 @@ struct SimulationParams
4747 mp2p_icp_filters::MotionCompensationMethod deskew_method =
4848 mp2p_icp_filters::MotionCompensationMethod::None;
4949
50- std::string outputMapClass = " mrpt::maps::CPointsMapXYZIRT " ;
50+ std::string outputMapClass = " mrpt::maps::CGenericPointsMap " ;
5151};
5252
5353struct SimulationResult
@@ -238,11 +238,13 @@ Scenario simulate_scenario(const SimulationParams& p)
238238 return s;
239239}
240240
241- mrpt::maps::CPointsMapXYZIRT ::Ptr simulate_skewed_points (
241+ mrpt::maps::CGenericPointsMap ::Ptr simulate_skewed_points (
242242 const mrpt::Clock::time_point& stamp, const mrpt::poses::CPose3DInterpolator& gtKeyframes,
243243 const std::vector<mrpt::math::TPoint3D>& gtPoints, const double lidar_scan_period)
244244{
245- auto pts = mrpt::maps::CPointsMapXYZIRT::Create ();
245+ auto pts = mrpt::maps::CGenericPointsMap::Create ();
246+ pts->registerField_float (mrpt::maps::CPointsMap::POINT_FIELD_TIMESTAMP);
247+ pts->registerField_float (mrpt::maps::CPointsMap::POINT_FIELD_INTENSITY);
246248
247249 // Simulate a scan where each point is acquired at a different time during the scan period.
248250 // Assume points are acquired sequentially over [stamp, stamp + lidar_scan_period)
@@ -269,11 +271,9 @@ mrpt::maps::CPointsMapXYZIRT::Ptr simulate_skewed_points(
269271 // Add to map, with time offset
270272 pts->insertPointFast (pt_local.x , pt_local.y , pt_local.z );
271273
272- #if MRPT_VERSION >= 0x020f00 // 2.15.0
273- pts->insertPointField_float (" t" , static_cast <float >(rel_time));
274- #else
275- pts->insertPointField_Timestamp (static_cast <float >(rel_time));
276- #endif
274+ pts->insertPointField_float (
275+ mrpt::maps::CPointsMap::POINT_FIELD_TIMESTAMP, static_cast <float >(rel_time));
276+ pts->insertPointField_float (mrpt::maps::CPointsMap::POINT_FIELD_INTENSITY, 1 .0f );
277277
278278#if 0
279279 std::cout << "PT[" << i << "] x: " << pt_local.x << ", y: " << pt_local.y
@@ -556,8 +556,9 @@ mrpt::maps::CSimplePointsMap simulate_gt_local_points(
556556 } // end for each timestep
557557
558558 // Now, reconstruct the points within the SM:
559- const auto sm2mmPipeline = mrpt::containers::yaml::FromText (mrpt::format (
560- R"yaml(
559+ const auto sm2mmPipeline = mrpt::containers::yaml::FromText (
560+ mrpt::format (
561+ R"yaml(
561562# --------------------------------------------------------
562563# 1) Generator (observation -> local frame metric maps)
563564# --------------------------------------------------------
@@ -594,7 +595,7 @@ mrpt::maps::CSimplePointsMap simulate_gt_local_points(
594595 # one or more layers to remove
595596 pointcloud_layer_to_remove: ["raw"]
596597 )yaml" ,
597- mrpt::typemeta::enum2str (p.deskew_method ).c_str (), p.outputMapClass .c_str ()));
598+ mrpt::typemeta::enum2str (p.deskew_method ).c_str (), p.outputMapClass .c_str ()));
598599
599600 mp2p_icp::metric_map_t mm;
600601 mp2p_icp_filters::sm2mm_options_t sm2mm_opts;
@@ -619,8 +620,8 @@ mrpt::maps::CSimplePointsMap simulate_gt_local_points(
619620 return fields;
620621 }();
621622
622- ASSERT_EQUAL_ (outFields.count (" intensity " ), 1 );
623- ASSERT_EQUAL_ (outFields.count (" t " ), 1 );
623+ ASSERT_EQUAL_ (outFields.count (std::string (mrpt::maps::CPointsMap::POINT_FIELD_INTENSITY) ), 1 );
624+ ASSERT_EQUAL_ (outFields.count (std::string (mrpt::maps::CPointsMap::POINT_FIELD_TIMESTAMP) ), 1 );
624625
625626 for (size_t i = 0 ; i < deskewedPts->size (); i++)
626627 {
0 commit comments