New pclDownsample() which only uses in and out clouds

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

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,

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

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();
 }

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

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)