From 11d440cd938396ffcc3d39a32a4edf0bfea4872c Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Sol=C3=A0?= <jsola@iri.upc.edu>
Date: Wed, 14 Sep 2022 01:31:27 +0200
Subject: [PATCH] New pclDownsample() which only uses in and out clouds
---
.../laser/processor/processor_odom_icp_3d.h | 3 ++
include/laser/utils/laser3d_tools.h | 29 ++++++++++---------
src/processor/processor_odom_icp_3d.cpp | 15 ++++++++++
test/gtest_laser3d_tools.cpp | 10 ++++++-
test/gtest_processor_odom_icp_3d.cpp | 14 +++++----
test/yaml/problem_odom_icp_3d.yaml | 2 +-
6 files changed, 52 insertions(+), 21 deletions(-)
diff --git a/include/laser/processor/processor_odom_icp_3d.h b/include/laser/processor/processor_odom_icp_3d.h
index cbeaf7967..d64b0e13c 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 61452b28e..eaa5bb185 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 52f970d76..c15bf47b2 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 416fb0a20..775f3bb8b 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 14b0e4729..b86c49456 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 cd8308a82..dd0fbad6a 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)
--
GitLab