diff --git a/include/laser/processor/processor_odom_icp_3d.h b/include/laser/processor/processor_odom_icp_3d.h index cbeaf79675d7a3ea01bb39b52a92a50bdbcf94fc..d64b0e13ccd72ddfc16a951852dada996f67444f 100644 --- a/include/laser/processor/processor_odom_icp_3d.h +++ b/include/laser/processor/processor_odom_icp_3d.h @@ -169,6 +169,9 @@ class ProcessorOdomIcp3d : public ProcessorTracker, public MotionProvider void resetDerived() override; void establishFactors() override; + + void PCLdownsample(CaptureLaser3dPtr _capture_laser, bool _pcl_downsample = true, double _pcl_voxel_size = 0.05); + }; } // namespace wolf diff --git a/include/laser/utils/laser3d_tools.h b/include/laser/utils/laser3d_tools.h index 61452b28ea3a92673318ffbf2a8f273c82cce24d..eaa5bb185b0377f717cc4d72a896bb2ba55cc1d9 100644 --- a/include/laser/utils/laser3d_tools.h +++ b/include/laser/utils/laser3d_tools.h @@ -87,23 +87,26 @@ void loadData(std::string _fname, pcl::PointCloud<pcl::PointXYZ> &_cloud) pcl::removeNaNFromPointCloud(_cloud, _cloud, indices); } }; -void PCLdownsample(CaptureLaser3dPtr _capture_laser, bool _pcl_downsample = true, double _pcl_voxel_size = 0.05) +void pclDownsample(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr _input, pcl::PointCloud<pcl::PointXYZ>::Ptr _output, double _pcl_voxel_size = 0.05) { - if (_pcl_downsample) - { pcl::VoxelGrid<pcl::PointXYZ> grid; grid.setLeafSize(_pcl_voxel_size, _pcl_voxel_size, _pcl_voxel_size); - grid.setInputCloud(_capture_laser->getPCLRaw()); - - pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_ptr = pcl::make_shared<pcl::PointCloud<pcl::PointXYZ>>(); - grid.filter(*pcl_ptr); - _capture_laser->setPCLDownsampled(pcl_ptr); - } - else - { - _capture_laser->setPCLDownsampled(_capture_laser->getPCLRaw()); - } + grid.setInputCloud(_input); + grid.filter(*_output); } + +// void PCLdownsample(CaptureLaser3dPtr _capture_laser, bool _pcl_downsample = true, double _pcl_voxel_size = 0.05) +// { +// if (_pcl_downsample) +// { +// pclDownsample(_capture_laser->getPCLRaw(), _capture_laser->getPCLDownsampled(), _pcl_voxel_size); +// } +// else +// { +// _capture_laser->setPCLDownsampled(_capture_laser->getPCLRaw()); +// } +// } + // _cloud_ref: first PointCloud // _cloud_other: second PointCloud inline void pairAlign(const pcl::PointCloud<pcl::PointXYZ>::Ptr _cloud_ref, diff --git a/src/processor/processor_odom_icp_3d.cpp b/src/processor/processor_odom_icp_3d.cpp index 52f970d7677fe9c69ab02f14465c71409788ca55..c15bf47b2a81b479eb4772fe9d019c52a5379ec4 100644 --- a/src/processor/processor_odom_icp_3d.cpp +++ b/src/processor/processor_odom_icp_3d.cpp @@ -244,6 +244,21 @@ void ProcessorOdomIcp3d::establishFactors() } }; +void ProcessorOdomIcp3d::PCLdownsample(CaptureLaser3dPtr _capture_laser, bool _pcl_downsample, double _pcl_voxel_size) +{ + if (_pcl_downsample) + { + pcl::PointCloud<pcl::PointXYZ>::Ptr _output = pcl::make_shared<pcl::PointCloud<pcl::PointXYZ>>(); + pclDownsample(_capture_laser->getPCLRaw(), _output, _pcl_voxel_size); + _capture_laser->setPCLDownsampled(_output); + } + else + { + _capture_laser->setPCLDownsampled(_capture_laser->getPCLRaw()); + } +} + + } // namespace wolf #include "core/processor/factory_processor.h" namespace wolf diff --git a/test/gtest_laser3d_tools.cpp b/test/gtest_laser3d_tools.cpp index 416fb0a205d2d3c805989bdf995af9c84f6c2da3..775f3bb8bcc1a6b50e34101ed175584822336535 100644 --- a/test/gtest_laser3d_tools.cpp +++ b/test/gtest_laser3d_tools.cpp @@ -79,6 +79,9 @@ TEST(pairAlign, identity) // reg.setPointRepresentation(pcl::make_shared<const MyPointRepresentation>(point_representation)); registration_solver->setMaximumIterations(200); + pclDownsample(pcl_ref, pcl_ref); + pclDownsample(pcl_other, pcl_other); + // Alignment wolf::pairAlign(pcl_ref, pcl_other, @@ -129,6 +132,11 @@ TEST(pairAlign, known_transformation) registration_solver->setMaxCorrespondenceDistance(0.5); // visaub: changed to 100 cm registration_solver->setMaximumIterations(100); + // Downsample + pclDownsample(pcl_ref, pcl_ref, 0.1); + pclDownsample(pcl_other, pcl_other, 0.1); + + // Alignment wolf::pairAlign(pcl_ref, pcl_other, @@ -148,6 +156,6 @@ TEST(pairAlign, known_transformation) int main(int argc, char** argv) { testing::InitGoogleTest(&argc, argv); - ::testing::GTEST_FLAG(filter) = "pairAlign.known_transformation"; + // ::testing::GTEST_FLAG(filter) = "pairAlign.known_transformation"; return RUN_ALL_TESTS(); } diff --git a/test/gtest_processor_odom_icp_3d.cpp b/test/gtest_processor_odom_icp_3d.cpp index 14b0e47295794c4ebd35cd57102a5babc4e0319f..b86c49456e5b261fee8ffe818ad9e665ad90d8ed 100644 --- a/test/gtest_processor_odom_icp_3d.cpp +++ b/test/gtest_processor_odom_icp_3d.cpp @@ -93,8 +93,9 @@ TEST(Init, register_in_factories) TEST_F(Test_ProcessorOdomIcp3D_yaml, process_make_keyframes_always_same_pcl) { std::vector<VectorComposite> X(10); - unsigned int i = 0; - for (TimeStamp t = 0; t < 6; t += 0.5) // will make one KF every 2 captures + auto last_kf_id = P->getLastFrame() ? P->getLastFrame()->id() : 0; + unsigned int i = 0; + for (TimeStamp t = 0; t < 10; t += 1) { // Load pointcloud from file pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_ref0 = pcl::make_shared<pcl::PointCloud<pcl::PointXYZ>>(); @@ -104,10 +105,11 @@ TEST_F(Test_ProcessorOdomIcp3D_yaml, process_make_keyframes_always_same_pcl) C0 = std::make_shared<CaptureLaser3d>(t, S, pcl_ref0); C0->process(); - // store state at integer timestamps (where the KFs are) - if (std::fabs(t.get() - (double)i) < 0.1) + // store state where the KFs are + if (p->getOrigin()->getFrame()->id() != last_kf_id) { - X[i] = p->getLast()->getFrame()->getState("PO"); + last_kf_id = P->getLastFrame()->id(); + X[i] = p->getLast()->getFrame()->getState("PO"); i++; } } @@ -165,6 +167,6 @@ TEST(Test_ProcessorOdomIcp3D_yaml, load_single_binary){ int main(int argc, char** argv) { testing::InitGoogleTest(&argc, argv); - ::testing::GTEST_FLAG(filter) = "Test_ProcessorOdomIcp3D_yaml.process_kitti"; + // ::testing::GTEST_FLAG(filter) = "Test_ProcessorOdomIcp3D_yaml.process_kitti"; return RUN_ALL_TESTS(); } \ No newline at end of file diff --git a/test/yaml/problem_odom_icp_3d.yaml b/test/yaml/problem_odom_icp_3d.yaml index cd8308a82bfbf527c656c0f5fe1074e019d2568b..dd0fbad6a20c0b0b9ae9efbec467c9150b0e1c60 100644 --- a/test/yaml/problem_odom_icp_3d.yaml +++ b/test/yaml/problem_odom_icp_3d.yaml @@ -42,7 +42,7 @@ config: state_getter: true state_priority: 1 pcl_downsample: true - pcl_downsample_voxel_size: 0.05 + pcl_downsample_voxel_size: 0.20 icp_algorithm: "icp_nl" # "icp", "icp_nl", "gicp" icp_max_iterations: 20 icp_transformation_rotation_epsilon: 0.9999 # this is cos(alpha)