test/gtest_laser3d_tools.cpp

+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program.  If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+
+// wolf laser
+#include "laser/utils/laser3d_tools.h"
+#include "laser/internal/config.h"
+
+// //Wolf
+#include "core/common/wolf.h"
+#include "core/utils/utils_gtest.h"
+#include <core/math/rotations.h>
+
+// pcl
+#include <pcl/registration/gicp.h>
+#include <pcl/registration/gicp6d.h>
+
+// Eigen
+#include <Eigen/Geometry>
+
+// std
+#include <iostream>
+#include <string>
+#include <filesystem>
+#include <unistd.h>
+
+//#define write_results
+
+//////  vsainz: i have to write a test
+
+using namespace wolf;
+using namespace Eigen;
+
+std::string laser_root_dir = _WOLF_LASER_ROOT_DIR;
+
+
+TEST(pairAlign, identity)
+{
+    // Load data
+    pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_ref = pcl::make_shared<pcl::PointCloud<pcl::PointXYZ>>();
+    loadData(laser_root_dir + "/test/data/1.pcd", *pcl_ref);
+
+    pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_other = pcl_ref;  // same PCld --> ground truth transform is identity
+    WOLF_INFO("Ground truth transform : \n", Matrix4d::Identity());
+
+    // Guess
+    Isometry3d transformation_guess = Translation3d(1, 1, 1) * AngleAxisd(0.2, Vector3d(1, 2, 3).normalized());
+    WOLF_INFO("Initial guess transform: \n", transformation_guess.matrix());
+
+    // final transform
+    Isometry3d transformation_final;
+
+    // Solver configuration
+    RegistrationPtr registration_solver =
+        pcl::make_shared<pcl::IterativeClosestPointNonLinear<pcl::PointXYZ, pcl::PointXYZ>>();
+    registration_solver->setTransformationEpsilon(1e-6);
+    // Set the maximum distance between two correspondences (src<->tgt) to 10cm
+    // Note: adjust this based on the size of your datasets
+    registration_solver->setMaxCorrespondenceDistance(0.5);  // visaub: changed to 100 cm
+    // Set the point representation
+    // 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,
+                    transformation_guess,
+                    transformation_final,
+                    registration_solver);  
+
+    WOLF_INFO("Final transform: \n", transformation_final.matrix());
+
+    ASSERT_MATRIX_APPROX(transformation_final.matrix(), Matrix4d::Identity(), 1e-2);
+}
+
+TEST(pairAlign, known_transformation)
+{
+    // Load data
+    pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_ref = pcl::make_shared<pcl::PointCloud<pcl::PointXYZ>>();
+    loadData(laser_root_dir + "/test/data/1.pcd", *pcl_ref);
+
+    // known transform
+    Affine3d transformation_known = Affine3d::Identity();
+    transformation_known.translation() << 1.0, 0.5, 0.2;
+    transformation_known.rotate(AngleAxisd(0.3, Vector3d::UnitZ()));
+
+    // Move point cloud
+    pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_other = pcl::make_shared<pcl::PointCloud<pcl::PointXYZ>>();
+    pcl::transformPointCloud(*pcl_ref, *pcl_other, transformation_known);
+
+    // Guess
+    Isometry3d transformation_guess = Isometry3d::Identity();
+
+    // final transform
+    Isometry3d transformation_final;
+    WOLF_INFO("Applied transformation: \n", transformation_known.matrix());
+
+    // Solver configuration
+    RegistrationPtr registration_solver =
+        pcl::make_shared<pcl::IterativeClosestPointNonLinear<pcl::PointXYZ, pcl::PointXYZ>>();
+    // pcl::GeneralizedIterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> registration_solver;
+
+    double max_trans_epsilon_meters = 0.001;
+    double max_rot_epsilon_degrees  = 0.1;
+    double trf_rotation_epsilon     = cos(toRad(max_rot_epsilon_degrees));
+
+    registration_solver->setTransformationEpsilon(max_trans_epsilon_meters * max_trans_epsilon_meters);
+    registration_solver->setTransformationRotationEpsilon(trf_rotation_epsilon);
+    // Set the maximum distance between two correspondences (src<->tgt) to 10cm
+    // Note: adjust this based on the size of your datasets
+    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,
+                    transformation_guess,
+                    transformation_final,
+                    registration_solver);
+
+    WOLF_INFO("Final transform: \n", transformation_final.matrix());
+
+    // auto convcrit = registration_solver->getConvergeCriteria();
+
+    // WOLF_INFO("convergence criteria: ", registration_solver->getConvergeCriteria()->getConvergenceState());
+
+    ASSERT_MATRIX_APPROX(transformation_final.matrix(), transformation_known.matrix(), 1e-2);
+}
+
+int main(int argc, char** argv)
+{
+    testing::InitGoogleTest(&argc, argv);
+    // ::testing::GTEST_FLAG(filter) = "pairAlign.known_transformation";
+    return RUN_ALL_TESTS();
+}

test/gtest_processor_odom_icp_3d.cpp

+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program.  If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+
+// wolf laser
+#include "laser/sensor/sensor_laser_3d.h"
+#include "laser/processor/processor_odom_icp_3d.h"
+#include "laser/internal/config.h"
+
+// wolf core
+#include "core/common/wolf.h"
+#include <core/sensor/factory_sensor.h>
+#include "core/processor/factory_processor.h"
+
+// wolf yaml
+#include <core/utils/params_server.h>
+#include <core/yaml/parser_yaml.h>
+#include "core/yaml/yaml_conversion.h"
+
+// testing
+#include <core/utils/utils_gtest.h>
+#include <core/utils/logging.h>
+
+// pcl includes
+#include <pcl/point_cloud.h>
+#include <pcl/io/pcd_io.h>
+#include <pcl/filters/voxel_grid.h>
+
+// other includes
+#include <string> 
+
+using namespace wolf;
+
+std::string laser_root_dir = _WOLF_LASER_ROOT_DIR;
+
+
+
+
+class Test_ProcessorOdomIcp3D_yaml : public testing::Test
+{
+  public:
+    ProblemPtr            P;
+    SensorLaser3dPtr      S;
+    ProcessorOdomIcp3dPtr p;
+    FrameBasePtr          F0, F1, F;
+    CaptureLaser3dPtr     C0, C1, C2, C3;
+
+    VectorXd data;
+    MatrixXd data_cov;
+
+  protected:
+    void SetUp() override
+    {
+        WOLF_INFO(laser_root_dir);
+
+        ParserYaml   parser = ParserYaml("problem_odom_icp_3d.yaml", laser_root_dir + "/test/yaml/");
+        ParamsServer server = ParamsServer(parser.getParams());
+        P                   = Problem::autoSetup(server);
+
+        S = std::static_pointer_cast<SensorLaser3d>(P->getHardware()->getSensorList().front());
+        p = std::static_pointer_cast<ProcessorOdomIcp3d>(S->getProcessorList().front());
+
+        P->print(4, 1, 1, 1);
+    }
+};
+
+TEST(Init, register_in_factories)
+{
+    FactorySensor::registerCreator("SensorLaser3d", SensorLaser3d::create);
+    AutoConfFactorySensor::registerCreator("SensorLaser3d", SensorLaser3d::create);
+    FactoryProcessor::registerCreator("ProcessorOdomIcp3d", ProcessorOdomIcp3d::create);
+    AutoConfFactoryProcessor::registerCreator("ProcessorOdomIcp3d", ProcessorOdomIcp3d::create);
+}
+
+TEST_F(Test_ProcessorOdomIcp3D_yaml, process_make_keyframes_always_same_pcl)
+{
+    std::vector<VectorComposite> X(10);
+    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>>();
+        loadData(laser_root_dir + "/test/data/1.pcd", *pcl_ref0);
+
+        // make Capture and process
+        C0 = std::make_shared<CaptureLaser3d>(t, S, pcl_ref0);
+        C0->process();
+
+        // store state where the KFs are
+        if (p->getOrigin()->getFrame()->id() != last_kf_id)
+        {
+            last_kf_id = P->getLastFrame()->id();
+            X[i]       = p->getLast()->getFrame()->getState("PO");
+            i++;
+        }
+    }
+
+    P->print(4, 1, 1, 1);
+
+    ASSERT_MATRIX_APPROX(X.at(0).vector("PO"), X.at(1).vector("PO"), 1e-8);  // pointclouds are the same
+    ASSERT_MATRIX_APPROX(X.at(0).vector("PO"), X.at(2).vector("PO"), 1e-8);  // pointclouds are the same
+    ASSERT_MATRIX_APPROX(X.at(0).vector("PO"), X.at(3).vector("PO"), 1e-8);  // pointclouds are the same
+    ASSERT_MATRIX_APPROX(X.at(0).vector("PO"), X.at(4).vector("PO"), 1e-8);  // pointclouds are the same
+    ASSERT_MATRIX_APPROX(X.at(0).vector("PO"), X.at(5).vector("PO"), 1e-8);  // pointclouds are the same
+} 
+
+TEST_F(Test_ProcessorOdomIcp3D_yaml, process_kitti)
+{   // Loads different kitti frames
+    // source:      2011_09_26_drive_0005_sync/velodyne_points
+    std::vector<VectorComposite> X(15);
+    unsigned int                 i = 0;
+    std::string fname;   //ex: 0000000050.bin
+    TimeStamp t = 0.0;     // should be read from velodyne_points/timestamps.txt
+    for (int j = 100; j <= 105; j++)    // from scan 100 to 105
+    {
+        // Load pointcloud from file
+        pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_ref_j = pcl::make_shared<pcl::PointCloud<pcl::PointXYZ>>();
+
+        fname = "0000000000" + std::to_string(j) + ".bin" ;
+        fname = fname.substr(fname.length() - 14, fname.length());
+        loadData(laser_root_dir + "/test/data/kitti/" + fname, *pcl_ref_j);
+        
+        WOLF_INFO("Loaded PointCloud "+fname);
+
+        // make Capture and process
+        C0 = std::make_shared<CaptureLaser3d>(t, S, pcl_ref_j);
+        C0->process();
+
+        // store state at integer timestamps (where the KFs are)
+        if (std::fabs(t.get() - (double)i) < 0.1)
+        {
+            X[i] = p->getLast()->getFrame()->getState("PO");
+            i++;
+        }
+
+        t += 0.1;
+    }
+
+    P->print(4, 1, 1, 1);
+}
+
+/*
+TEST(Test_ProcessorOdomIcp3D_yaml, load_single_binary){
+    pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_ref_i = pcl::make_shared<pcl::PointCloud<pcl::PointXYZ>>();
+    loadData(laser_root_dir + "/test/data/kitti/0000000101.bin", *pcl_ref_i);
+}*/
+
+int main(int argc, char** argv)
+{
+    testing::InitGoogleTest(&argc, argv);
+    // ::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

+config:
+  problem:
+    frame_structure: "PO"
+    dimension: 3
+    prior:
+      mode: "factor"
+      $state:
+        P: [0,0,0]
+        O: [0,0,0,1]
+      $sigma:
+        P: [.1,.1,.1]
+        O: [.1,.1,.1]
+      time_tolerance: 0.05
+    tree_manager: 
+      type: "None"
+  map:
+    type: "MapBase"
+    plugin: "core"
+  sensors:
+   -
+    name: "Lidar"
+    type: "SensorLaser3d"   # No
+    plugin: "laser"
+    extrinsic:
+      pose: [0.5,0,0, 0,0,0,1]
+
+  processors:
+   -
+    name: "Odometry ICP 3d"
+    type: "ProcessorOdomIcp3d"
+    sensor_name: "Lidar"
+    plugin: "laser"
+    time_tolerance: 0.05
+    max_new_features: 0
+    apply_loss_function: false
+    keyframe_vote:
+      voting_active: true
+      min_features_for_keyframe : 0
+      max_time_span: 0.000099   # KF every frame
+      max_fitness_score: 0.0001 # maximum Euclidean fitness score (e.g., mean of squared distances from the source to the target)
+      angle_turned: 1.0
+    state_getter: true
+    state_priority: 1
+    pcl_downsample: true
+    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)
+    icp_transformation_translation_epsilon: 1e-9  # this is translation squared
+    icp_max_correspondence_distance:  0.05
+
+