.gitignore

@@ -23,6 +23,7 @@ src/examples/map_polyline_example_write.yaml
 /CMakeCache.txt
 *.dat
 .DS_Store
+laser.found
 
 *.graffle
 /Default/

CMakeLists.txt

@@ -76,6 +76,7 @@ option(_WOLF_TRACE "Enable wolf tracing macro" ON)
 
 # ============ DEPENDENCIES ============ 
 FIND_PACKAGE(wolfcore REQUIRED CONFIG)
+# message(STATUS ${wolfcore_DIR})
 FIND_PACKAGE(laser_scan_utils REQUIRED CONFIG)
 # found only to set the XXX_FOUND variables
 # -> It would be better to look for them in the the laser_scan_utils target
@@ -83,6 +84,7 @@ FIND_PACKAGE(falkolib QUIET)
 find_package(PkgConfig)
 pkg_check_modules(csm QUIET csm)
 # link_directories(${csm_LIBRARY_DIRS})
+find_package(PCL 1.10 COMPONENTS common registration io)
 
 # ============ CONFIG.H ============ 
 set(_WOLF_ROOT_DIR ${CMAKE_SOURCE_DIR})
@@ -213,6 +215,33 @@ if(csm_FOUND)
     )
 endif(csm_FOUND)
 
+# PCL
+if(PCL_FOUND)
+  # put here all files, HEADERS and SOURCES
+  # message("Found PCL. Compiling some extra classes.")
+  # message(STATUS "PCL DIRS:" ${PCL_DIR})
+  # message(STATUS "PCL include DIRS:" ${PCL_INCLUDE_DIRS})
+  SET(HDRS_CAPTURE ${HDRS_CAPTURE}
+    include/${PROJECT_NAME}/capture/capture_laser_3d.h
+    )
+  SET(HDRS_PROCESSOR ${HDRS_PROCESSOR}
+    include/${PROJECT_NAME}/processor/processor_odom_icp_3d.h
+    )
+  SET(HDRS_SENSOR ${HDRS_SENSOR}
+    include/${PROJECT_NAME}/sensor/sensor_laser_3d.h
+    )
+  SET(SRCS_CAPTURE ${SRCS_CAPTURE}
+    src/capture/capture_laser_3d.cpp
+    )
+    SET(SRCS_PROCESSOR ${SRCS_PROCESSOR}
+    src/processor/processor_odom_icp_3d.cpp
+  )
+  SET(SRCS_PROCESSOR ${SRCS_PROCESSOR}
+      src/sensor/sensor_laser_3d.cpp
+    )
+endif(PCL_FOUND)
+
+
 # create the shared library
 ADD_LIBRARY(${PLUGIN_NAME}
   SHARED
@@ -244,6 +273,9 @@ TARGET_LINK_LIBRARIES(${PLUGIN_NAME} laser_scan_utils)
 IF (falkolib_FOUND)
 	TARGET_LINK_LIBRARIES(${PLUGIN_NAME} ${falkolib_LIBRARY})
 ENDIF(falkolib_FOUND)
+IF (PCL_FOUND)
+	TARGET_LINK_LIBRARIES(${PLUGIN_NAME} ${PCL_LIBRARIES})
+ENDIF(PCL_FOUND)
 
 #Build tests
 #===============EXAMPLE=========================

include/laser/capture/capture_laser_3d.h

+//--------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--------
+#ifndef CAPTURE_LASER_3d_H_
+#define CAPTURE_LASER_3d_H_
+
+// WOLF includes
+#include <core/capture/capture_base.h>
+
+// PCL includes
+#include <pcl/point_types.h>
+#include <pcl/point_cloud.h>
+
+namespace wolf
+{
+
+WOLF_PTR_TYPEDEFS(CaptureLaser3d);
+
+class CaptureLaser3d : public CaptureBase
+{
+  public:
+    CaptureLaser3d(const TimeStamp& _timestamp, SensorBasePtr _sensor, pcl::PointCloud<pcl::PointXYZ>::Ptr _point_cloud_raw);
+    ~CaptureLaser3d();
+    pcl::PointCloud<pcl::PointXYZ>::Ptr getPCLDownsampled();
+    pcl::PointCloud<pcl::PointXYZ>::ConstPtr getPCLDownsampled() const;
+    pcl::PointCloud<pcl::PointXYZ>::Ptr getPCLRaw();
+    pcl::PointCloud<pcl::PointXYZ>::ConstPtr getPCLRaw() const;
+    void setPCLDownsampled(pcl::PointCloud<pcl::PointXYZ>::Ptr _pcl_ptr);
+    
+  private:
+    pcl::PointCloud<pcl::PointXYZ>::Ptr point_cloud_raw_;
+    pcl::PointCloud<pcl::PointXYZ>::Ptr point_cloud_;
+};
+
+}  // namespace wolf
+
+#endif  // CAPTURE_LASER_3d_H_
\ No newline at end of file

include/laser/processor/processor_odom_icp_3d.h

+//--------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--------
+
+#ifndef SRC_PROCESSOR_ODOM_ICP_3D_H_
+#define SRC_PROCESSOR_ODOM_ICP_3D_H_
+
+/**************************
+ *      WOLF includes     *
+ **************************/
+
+#include "laser/capture/capture_laser_3d.h"
+#include "laser/sensor/sensor_laser_3d.h"
+#include "laser/utils/laser3d_tools.h"
+
+#include "core/common/wolf.h"
+#include "core/processor/processor_tracker.h"
+#include "core/processor/motion_provider.h"
+
+/**************************
+ *      PCL includes      *
+ **************************/
+#include <pcl/point_types.h>
+#include <pcl/point_cloud.h>
+#include <pcl/registration/registration.h>
+#include <pcl/filters/voxel_grid.h>
+
+namespace wolf
+{
+
+WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorOdomIcp3d);
+
+struct ParamsProcessorOdomIcp3d : public ParamsProcessorTracker, public ParamsMotionProvider
+{
+    double max_time_span;
+    double max_fitness_score; // maximum Euclidean fitness score (e.g., mean of squared distances from the source to the target)
+
+    bool   pcl_downsample;
+    double pcl_downsample_voxel_size;
+
+    string icp_algorithm;                           // "icp", "icpnl", "gicp"
+    int    icp_max_iterations;
+    double icp_transformation_translation_epsilon;  // squared value of translation epsilon
+    double icp_transformation_rotation_epsilon;     // cosinus of rotation angle epsilon
+    double icp_max_correspondence_distance;
+
+    // add more params as required
+
+    ParamsProcessorOdomIcp3d() = default;
+    ParamsProcessorOdomIcp3d(std::string _unique_name, const ParamsServer& _server)
+        : ParamsProcessorTracker(_unique_name, _server), ParamsMotionProvider(_unique_name, _server)
+    {
+        max_time_span = _server.getParam<double>(prefix + _unique_name + "/keyframe_vote/max_time_span");
+        max_fitness_score = _server.getParam<double>(prefix + _unique_name + "/keyframe_vote/max_fitness_score");
+
+        pcl_downsample            = _server.getParam<bool>(prefix + _unique_name + "/pcl_downsample");
+        pcl_downsample_voxel_size = _server.getParam<double>(prefix + _unique_name + "/pcl_downsample_voxel_size");
+
+        icp_algorithm      = _server.getParam<string>(prefix + _unique_name + "/icp_algorithm");
+        icp_max_iterations = _server.getParam<int>(prefix + _unique_name + "/icp_max_iterations");
+        icp_transformation_translation_epsilon =
+            _server.getParam<double>(prefix + _unique_name + "/icp_transformation_translation_epsilon");
+        icp_transformation_rotation_epsilon =
+            _server.getParam<double>(prefix + _unique_name + "/icp_transformation_rotation_epsilon");
+        icp_max_correspondence_distance =
+            _server.getParam<double>(prefix + _unique_name + "/icp_max_correspondence_distance");
+    }
+
+    std::string print() const override
+    {
+        // TODO
+        return "\n" + ParamsProcessorTracker::print() + "\n" + ParamsMotionProvider::print() + "\n";
+        // + "initial_guess: "             + initial_guess                                         + "\n"
+        // + "keyframe_vote/min_dist: "    + std::to_string(vfk_min_dist)                          + "\n"
+        // + "keyframe_vote/min_angle: "   + std::to_string(vfk_min_angle)                         + "\n"
+        // + "keyframe_vote/min_time: "    + std::to_string(vfk_min_time)                          + "\n"
+        // + "keyframe_vote/min_error: "   + std::to_string(vfk_min_error)                         + "\n"
+        // + "keyframe_vote/max_points: "  + std::to_string(vfk_max_points)                        + "\n";
+    }
+};
+
+WOLF_PTR_TYPEDEFS(ProcessorOdomIcp3d);
+
+
+class ProcessorOdomIcp3d : public ProcessorTracker, public MotionProvider
+{
+  protected:
+    // Useful sensor stuff
+    SensorLaser3dPtr  sensor_laser_;  // casted pointer to parent
+    CaptureLaser3dPtr origin_laser_, last_laser_, incoming_laser_;
+    Eigen::Isometry3d T_origin_last_, T_last_incoming_, T_origin_incoming_, T_robot_sensor_, T_sensor_robot_;
+    RegistrationPtr   registration_solver_;
+    Eigen::Matrix6d   transform_cov_;
+
+  public:
+    ParamsProcessorOdomIcp3dPtr params_odom_icp_;
+
+  public:
+    ProcessorOdomIcp3d(ParamsProcessorOdomIcp3dPtr _params);
+    ~ProcessorOdomIcp3d() override;
+    WOLF_PROCESSOR_CREATE(ProcessorOdomIcp3d, ParamsProcessorOdomIcp3d);
+    void configure(SensorBasePtr _sensor) override;
+
+    // API required by MotionProvider
+    VectorComposite getState(const StateStructure& _structure = "") const override;
+    TimeStamp       getTimeStamp() const override;
+    VectorComposite getState(const TimeStamp& _ts, const StateStructure& _structure = "") const override;
+
+  protected:
+    // API required by ProcessorTracker
+    void preProcess() override;
+
+    /** \brief Compute transform from origin to incoming.
+     *
+     * The transform is the result of aligning the scans in origin and incoming.
+     * This alignement takes as prior the current transform from origin to last.
+     *
+     * \return the number of features tracked. This is always zero since this processor is not tracking features.
+     */
+    unsigned int processKnown() override;
+    /** \brief Compute transform from last to incoming.
+     *
+     * The transform is the result of aligning the scans in last and incoming.
+     * This alignement takes as prior the identity transform.
+     *
+     * \param _max_features number of features to extract. This is ignored since this processor is not extracting features.
+     *
+     * \return the number of features extracted. This is always zero since this processor is not extracting features.
+     */
+    unsigned int processNew(const int& _max_features) override;
+
+    bool voteForKeyFrame() const override;
+
+    /** \brief advance pointers and update internal values
+     *
+     * We do several things:
+     *   - Advance derived pointers
+     *   - Advance isometries
+     *   - Update extrinsic isometries in case they have changed
+     *   - Compute odometry
+     */
+    void advanceDerived() override;
+    /** \brief advance pointers and update internal values
+     *
+     * We do several things:
+     *   - Reset derived pointers
+     *   - Reset isometries
+     *   - Update extrinsic isometries in case they have changed
+     *   - Compute odometry
+     */
+    void resetDerived() override;
+
+    void establishFactors() override;
+
+    void PCLdownsample(CaptureLaser3dPtr _capture_laser, bool _pcl_downsample = true, double _pcl_voxel_size = 0.05);
+    
+};
+
+}  // namespace wolf
+
+#endif  // SRC_PROCESSOR_ODOM_ICP_3D_H_

include/laser/sensor/sensor_laser_3d.h

+//--------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--------
+#ifndef SENSOR_LASER_3d_H_
+#define SENSOR_LASER_3d_H_
+
+// WOLF includes
+#include <core/sensor/sensor_base.h>
+
+namespace wolf
+{
+
+WOLF_STRUCT_PTR_TYPEDEFS(ParamsSensorLaser3d);
+
+struct ParamsSensorLaser3d : public ParamsSensorBase
+{
+    ParamsSensorLaser3d() = default;
+    ParamsSensorLaser3d(std::string _unique_name, const ParamsServer& _server)
+        : ParamsSensorBase(_unique_name, _server)
+    {
+    }
+    ~ParamsSensorLaser3d() override = default;
+};
+
+WOLF_PTR_TYPEDEFS(SensorLaser3d);
+
+class SensorLaser3d : public SensorBase
+{
+  public:
+    SensorLaser3d(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr);
+    SensorLaser3d(const Eigen::VectorXd& _extrinsics, ParamsSensorLaser3dPtr _params);
+    ~SensorLaser3d();
+    WOLF_SENSOR_CREATE(SensorLaser3d, ParamsSensorLaser3d, 7);
+    ParamsSensorLaser3dPtr params_laser3d_;
+};
+
+}  // namespace wolf
+
+#endif  // SENSOR_LASER_3d_H_
\ No newline at end of file

include/laser/utils/laser3d_tools.h

+//--------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--------
+
+#ifndef LASER3D_TOOLS_H
+#define LASER3D_TOOLS_H
+
+/**************************
+ *      WOLF includes     *
+ **************************/
+#include "laser/capture/capture_laser_3d.h"
+#include "laser/sensor/sensor_laser_3d.h"
+
+#include <core/utils/logging.h>
+
+/**************************
+ *      PCL includes      *
+ **************************/
+#include <pcl/point_types.h>
+#include <pcl/point_cloud.h>
+#include <pcl/common/transforms.h>
+#include <pcl/io/pcd_io.h>
+#include <pcl/filters/voxel_grid.h>
+#include <pcl/registration/icp.h>
+#include <pcl/registration/icp_nl.h>
+#include <pcl/registration/transforms.h>
+
+/**************************
+ *    Standard includes   *
+ **************************/
+#include <iostream>
+#include <fstream>
+
+typedef pcl::Registration<pcl::PointXYZ, pcl::PointXYZ>::Ptr RegistrationPtr;
+
+namespace wolf
+{
+void loadData(std::string _fname, pcl::PointCloud<pcl::PointXYZ> &_cloud)
+{
+    if (_fname.compare(_fname.size() - 4, 4, ".pcd") == 0)
+    {
+        pcl::io::loadPCDFile(_fname, _cloud);
+    }
+    else if (_fname.compare(_fname.size() - 4, 4, ".bin") == 0)
+    {  // file is binary
+        // pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_ptr = pcl::make_shared<pcl::PointCloud<pcl::PointXYZ>>();
+
+        std::ifstream stream_input;
+
+        stream_input.open(_fname, std::ios::in);
+        for (int i = 0; stream_input.good() && !stream_input.eof(); i++)
+        {
+            pcl::PointXYZI point_raw;
+            stream_input.read((char *)&point_raw.x, 3 * sizeof(float));
+            stream_input.read((char *)&point_raw.intensity, sizeof(float));
+
+            pcl::PointXYZ tPoint;
+
+            tPoint.x = point_raw.x;
+            tPoint.y = point_raw.y;
+            tPoint.z = point_raw.z;
+
+            _cloud.push_back(tPoint);
+        }
+        stream_input.close();
+
+        // remove NAN points from the cloud
+        pcl::Indices indices;
+        pcl::removeNaNFromPointCloud(_cloud, _cloud, indices);
+    }
+};
+void pclDownsample(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr _input, pcl::PointCloud<pcl::PointXYZ>::Ptr _output, double _pcl_voxel_size = 0.05)
+{
+        pcl::VoxelGrid<pcl::PointXYZ> grid;
+        grid.setLeafSize(_pcl_voxel_size, _pcl_voxel_size, _pcl_voxel_size);
+        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,
+                      const pcl::PointCloud<pcl::PointXYZ>::Ptr _cloud_other,
+                      const Eigen::Isometry3d                  &_transform_guess,
+                      Eigen::Isometry3d                        &_transform_final,
+                      RegistrationPtr                          &_registration_solver)
+{
+    // DURATION --------------------------------------------------------
+    auto start = std::chrono::high_resolution_clock::now();
+    // DURATION --------------------------------------------------------
+
+    // Set input clouds
+    _registration_solver->setInputSource(_cloud_ref);
+    _registration_solver->setInputTarget(_cloud_other);
+
+    // Declare variables
+    Eigen::Matrix4f transform_pcl;
+
+    // Change to float
+    Eigen::Matrix4f transform_guess = _transform_guess.matrix().cast<float>();
+
+    // Alignment
+    pcl::PointCloud<pcl::PointXYZ> cloud_out;
+    _registration_solver->align(cloud_out, transform_guess);
+    transform_pcl = _registration_solver->getFinalTransformation();
+
+    // Output the transformation as Isometry of double(s)
+    _transform_final = Eigen::Isometry3d(transform_pcl.cast<double>());
+
+    // DURATION --------------------------------------------------------
+    auto duration =
+        std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::high_resolution_clock::now() - start);
+    WOLF_INFO("Laser3d_tools.h: pairAlign: duration: ", 1e-3 * (duration).count(), " ms");
+    // DURATION --------------------------------------------------------
+}
+
+}  // namespace wolf
+
+#endif  // LASER3D_TOOLS_H
\ No newline at end of file

src/capture/capture_laser_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--------
+#include "laser/capture/capture_laser_3d.h"
+
+namespace wolf
+{
+
+CaptureLaser3d::CaptureLaser3d(const TimeStamp&                    _timestamp,
+                               SensorBasePtr                       _sensor,
+                               pcl::PointCloud<pcl::PointXYZ>::Ptr _point_cloud_raw)
+    : CaptureBase("CaptureLaser3d", _timestamp, _sensor), point_cloud_raw_(_point_cloud_raw)
+{
+    point_cloud_ = nullptr;
+}
+
+CaptureLaser3d::~CaptureLaser3d() {}
+
+void CaptureLaser3d::setPCLDownsampled(pcl::PointCloud<pcl::PointXYZ>::Ptr _pcl_ptr)
+{
+    point_cloud_ = _pcl_ptr;
+}
+
+pcl::PointCloud<pcl::PointXYZ>::Ptr CaptureLaser3d::getPCLDownsampled()
+{
+    return point_cloud_;
+}
+
+pcl::PointCloud<pcl::PointXYZ>::ConstPtr CaptureLaser3d::getPCLDownsampled() const
+{
+    return point_cloud_;
+}
+
+pcl::PointCloud<pcl::PointXYZ>::Ptr CaptureLaser3d::getPCLRaw()
+{
+    return point_cloud_raw_;
+}
+
+pcl::PointCloud<pcl::PointXYZ>::ConstPtr CaptureLaser3d::getPCLRaw() const
+{
+    return point_cloud_raw_;
+}
+
+}  // namespace wolf
\ No newline at end of file

src/processor/processor_loop_closure_icp.cpp

@@ -85,11 +85,8 @@ std::map<double,MatchLoopClosurePtr> ProcessorLoopClosureIcp::findLoopClosures(C
         // Frame can have more than one laser capture
         for (auto cap_ref : frm->getCapturesOfType<CaptureLaser2d>())
         {
-            auto cap_ref_laser = std::dynamic_pointer_cast<CaptureLaser2d>(cap_ref);
-            assert(cap_ref_laser != nullptr);
-
             // Match Scans
-            auto match = matchScans(cap_ref_laser, cap_laser);
+            auto match = matchScans(cap_ref, cap_laser);
 
             // IT'S A MATCH!
             if (match != nullptr)

src/processor/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/processor/processor_odom_icp_3d.h"
+#include "laser/utils/laser3d_tools.h"
+
+// wolf core
+#include "core/factor/factor_relative_pose_3d_with_extrinsics.h"
+#include "core/utils/logging.h"
+
+// pcl
+#include <pcl/registration/icp.h>
+#include <pcl/registration/gicp.h>
+#include <pcl/registration/icp_nl.h>
+
+namespace wolf
+{
+
+ProcessorOdomIcp3d::ProcessorOdomIcp3d(ParamsProcessorOdomIcp3dPtr _params)
+    : ProcessorTracker("ProcessorOdomIcp3d", "PO", 3, _params), MotionProvider("PO", _params)
+{
+    T_origin_incoming_.setIdentity();
+    T_origin_last_.setIdentity();
+    T_last_incoming_.setIdentity();
+    T_robot_sensor_.setIdentity();
+    T_sensor_robot_.setIdentity();
+
+    if (std::strcmp(_params->icp_algorithm.c_str(), "icp_nl") == 0)
+    {
+        WOLF_INFO("ProcessorOdomIcp3d: Using icp_nl");
+        registration_solver_ = pcl::make_shared<pcl::IterativeClosestPointNonLinear<pcl::PointXYZ, pcl::PointXYZ>>();
+    }
+    else if (std::strcmp(_params->icp_algorithm.c_str(), "icp") == 0)
+    {
+        WOLF_INFO("ProcessorOdomIcp3d: Using icp");
+        registration_solver_ = pcl::make_shared<pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ>>();
+    }
+    else if (std::strcmp(_params->icp_algorithm.c_str(), "gicp") == 0)
+    {
+        WOLF_INFO("ProcessorOdomIcp3d: Using gicp");
+        registration_solver_ = pcl::make_shared<pcl::GeneralizedIterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ>>();
+    }
+    else
+    {
+        WOLF_ERROR("ProcessorOdomIcp3d: Unrecognized ICP algorithm \"", _params->icp_algorithm, "\". Valid options are \"icp\", \"icp_nl\" and \"gicp\" only.");
+        throw("ICP algorithm not recognized");
+    }
+    registration_solver_->setMaximumIterations(_params->icp_max_iterations);
+    registration_solver_->setTransformationRotationEpsilon(_params->icp_transformation_rotation_epsilon);
+    registration_solver_->setTransformationEpsilon(_params->icp_transformation_translation_epsilon);
+    registration_solver_->setMaxCorrespondenceDistance(_params->icp_max_correspondence_distance);
+
+    params_odom_icp_ = _params;
+    transform_cov_.setIdentity();
+}
+
+ProcessorOdomIcp3d::~ProcessorOdomIcp3d() {}
+
+void ProcessorOdomIcp3d::configure(SensorBasePtr _sensor)
+{
+    sensor_laser_ = std::static_pointer_cast<SensorLaser3d>(_sensor);
+
+    T_robot_sensor_ = Eigen::Translation3d(_sensor->getStateBlock('P')->getState()) *
+                      Eigen::Quaterniond(_sensor->getStateBlock('O')->getState().data());
+
+    T_sensor_robot_ = T_robot_sensor_.inverse();
+}
+
+void ProcessorOdomIcp3d::preProcess()
+{
+    incoming_laser_ = std::static_pointer_cast<CaptureLaser3d>(incoming_ptr_);
+    double   pcl_voxel_size = params_odom_icp_->pcl_downsample_voxel_size;
+    bool     pcl_downsample = params_odom_icp_->pcl_downsample;
+    PCLdownsample(incoming_laser_, pcl_downsample, pcl_voxel_size);
+}
+
+/** \brief Tracker function
+ * \return The number of successful tracks.
+ *
+ * This is the tracker function to be implemented in derived classes.
+ * It operates on the \b incoming capture pointed by incoming_ptr_.
+ *
+ * This should do one of the following, depending on the design of the tracker -- see use_landmarks_:
+ *   - Track Features against other Features in the \b origin Capture. Tips:
+ *     - An intermediary step of matching against Features in the \b last Capture makes tracking easier.
+ *     - Once tracked against last, then the link to Features in \b origin is provided by the Features' Factors in \b
+ * last.
+ *     - If required, correct the drift by re-comparing against the Features in origin.
+ *     - The Factors in \b last need to be transferred to \b incoming (moved, not copied).
+ *   - Track Features against Landmarks in the Map. Tips:
+ *     - The links to the Landmarks are in the Features' Factors in last.
+ *     - The Factors in \b last need to be transferred to \b incoming (moved, not copied).
+ *
+ * The function must generate the necessary Features in the \b incoming Capture,
+ * of the correct type, derived from FeatureBase.
+ *
+ * It must also generate the factors, of the correct type, derived from FactorBase
+ * (through FactorAnalytic or FactorSparse).
+ */
+unsigned int ProcessorOdomIcp3d::processKnown()
+{
+    if (origin_ptr_ && (incoming_ptr_ != origin_ptr_))
+    {
+        origin_laser_ = std::static_pointer_cast<CaptureLaser3d>(origin_ptr_);
+        pairAlign(origin_laser_->getPCLDownsampled(),
+                  incoming_laser_->getPCLDownsampled(),
+                  T_origin_last_,
+                  T_origin_incoming_,
+                  registration_solver_);
+    }
+    return 0;
+};
+
+/**\brief Process new Features or Landmarks
+ *
+ */
+unsigned int ProcessorOdomIcp3d::processNew(const int& _max_features)
+{
+    if (last_ptr_)
+    {
+        last_laser_ = std::static_pointer_cast<CaptureLaser3d>(last_ptr_);
+        pairAlign(last_laser_->getPCLDownsampled(),
+                  incoming_laser_->getPCLDownsampled(),
+                  Eigen::Isometry3d::Identity(),
+                  T_last_incoming_,
+                  registration_solver_);
+    }
+    return 0;
+};
+
+/** \brief Vote for KeyFrame generation
+ *
+ * If a KeyFrame criterion is validated, this function returns true,
+ * meaning that it wants to create a KeyFrame at the \b last Capture.
+ *
+ * WARNING! This function only votes! It does not create KeyFrames!
+ */
+bool ProcessorOdomIcp3d::voteForKeyFrame() const
+{
+    // 1. vote by time
+    if (last_ptr_->getTimeStamp() - origin_ptr_->getTimeStamp() > params_odom_icp_->max_time_span)
+    {
+        WOLF_DEBUG("ProcessorOdomIcp3d: vote KF by time");
+        return true;
+    }
+    // TODO:
+    //  2. vote by distance
+    //  3. vote by angle
+    //  4. vote by nbr. of captures
+
+    // 5. vote by fitness score
+    if (registration_solver_->getFitnessScore() > params_odom_icp_->max_fitness_score)
+    {
+        WOLF_DEBUG("ProcessorOdomIcp3d: vote KF by fitness score");
+        return true;
+    }
+    
+    return false;
+};
+
+/** \brief Advance the incoming Capture to become the last.
+ *
+ * Call this when the tracking and keyframe policy work is done and
+ * we need to get ready to accept a new incoming Capture.
+ */
+void ProcessorOdomIcp3d::advanceDerived()
+{
+    T_origin_last_  = T_origin_incoming_;
+    last_laser_     = incoming_laser_;
+    incoming_laser_ = nullptr;
+};
+
+/** \brief Reset the tracker using the \b last Capture as the new \b origin.
+ */
+void ProcessorOdomIcp3d::resetDerived()
+{
+    T_origin_last_  = T_last_incoming_;
+    origin_laser_   = last_laser_;
+    last_laser_     = incoming_laser_;
+    incoming_laser_ = nullptr;
+};
+
+// Queries to the processor:
+TimeStamp ProcessorOdomIcp3d::getTimeStamp() const
+{
+    return last_laser_->getTimeStamp();
+}
+
+VectorComposite ProcessorOdomIcp3d::getState(const StateStructure& _structure) const
+{
+    VectorComposite   state_origin = origin_ptr_->getFrame()->getState("PO");
+    Eigen::Isometry3d T_world_origin_robot =
+        Eigen::Translation3d(state_origin.at('P')) * Eigen::Quaterniond(state_origin.at('O').data());
+    Eigen::Isometry3d T_world_last_robot = T_world_origin_robot * T_robot_sensor_ * T_origin_last_ * T_sensor_robot_;
+    VectorComposite   state_last;
+    state_last['P'] = T_world_last_robot.translation();
+    state_last['O'] = Eigen::Quaterniond(T_world_last_robot.rotation()).coeffs();
+    return state_last;
+}
+
+VectorComposite ProcessorOdomIcp3d::getState(const TimeStamp& _ts, const StateStructure& _structure) const
+{
+    // TODO get the appropriate capture
+    return getState(_structure);
+}
+
+/** \brief Creates and adds factors from last_ to origin_
+ *
+ */
+void ProcessorOdomIcp3d::establishFactors()
+{
+    if (last_ptr_ != origin_ptr_) // skip if it's the same frame (it happens the SECOND_TIME)
+    {
+        // emplace a feature of type FeatureMotion undet the capture last_ptr_ or last_laser_
+        Eigen::Vector7d measurement_of_motion;
+        measurement_of_motion.head(3) = T_origin_last_.translation();
+        measurement_of_motion.tail(4) = Eigen::Quaterniond(T_origin_last_.rotation()).coeffs();
+        auto feature =
+            FeatureBase::emplace<FeatureBase>(last_ptr_, "FeatureBase", measurement_of_motion, transform_cov_);
+
+        // emplace a factor of type FactorRelativePose3dWithExtrinsics under the feature above
+        auto factor = FactorBase::emplace<FactorRelativePose3dWithExtrinsics>(
+            feature, feature, origin_ptr_->getFrame(), shared_from_this(), false, TOP_MOTION);
+    }
+};
+
+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
+{
+WOLF_REGISTER_PROCESSOR(ProcessorOdomIcp3d);
+WOLF_REGISTER_PROCESSOR_AUTO(ProcessorOdomIcp3d);
+}  // namespace wolf

src/sensor/sensor_laser_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--------
+#include "laser/sensor/sensor_laser_3d.h"
+
+#include <core/state_block/state_block_derived.h>
+#include <core/state_block/state_quaternion.h>
+
+namespace wolf
+{
+
+SensorLaser3d::SensorLaser3d(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr)
+    : SensorBase("SensorLaser3d", _p_ptr, _o_ptr, nullptr, 0)
+{
+}
+
+SensorLaser3d::SensorLaser3d(const Eigen::VectorXd& _extrinsics, ParamsSensorLaser3dPtr _params)
+    : SensorBase("SensorLaser3d",
+                 std::make_shared<StatePoint3d>(_extrinsics.head(3), true),
+                 std::make_shared<StateQuaternion>(_extrinsics.tail(4), true),
+                 nullptr,
+                 0),
+      params_laser3d_(_params)
+{
+    //
+}
+
+SensorLaser3d::~SensorLaser3d() {}
+
+}  // namespace wolf
+
+// Register in the FactorySensor and the ParameterFactory
+#include "core/sensor/factory_sensor.h"
+namespace wolf {
+WOLF_REGISTER_SENSOR(SensorLaser3d)
+WOLF_REGISTER_SENSOR_AUTO(SensorLaser3d)
+} // namespace wolf
+

test/CMakeLists.txt

@@ -24,3 +24,8 @@ if(falkolib_FOUND)
     	wolf_add_gtest(gtest_processor_loop_closure_falko_icp gtest_processor_loop_closure_falko_icp.cpp)
   	endif(csm_FOUND)
 endif(falkolib_FOUND)
+
+if(PCL_FOUND)
+	wolf_add_gtest(gtest_laser3d_tools gtest_laser3d_tools.cpp)
+	wolf_add_gtest(gtest_processor_odom_icp_3d gtest_processor_odom_icp_3d.cpp)
+endif(PCL_FOUND)
\ No newline at end of file

test/data/kitti/timestamps.txt

+2011-09-26 13:04:32.335337762
+2011-09-26 13:04:32.438625628
+2011-09-26 13:04:32.541951663
+2011-09-26 13:04:32.645280345
+2011-09-26 13:04:32.748584715
+2011-09-26 13:04:32.851882576
+2011-09-26 13:04:32.955179251
+2011-09-26 13:04:33.058455038
+2011-09-26 13:04:33.161788132
+2011-09-26 13:04:33.265142234
+2011-09-26 13:04:33.368407119
+2011-09-26 13:04:33.471679129
+2011-09-26 13:04:33.574987540
+2011-09-26 13:04:33.678287762
+2011-09-26 13:04:33.781556152
+2011-09-26 13:04:33.884954722
+2011-09-26 13:04:33.988245317
+2011-09-26 13:04:34.091397260
+2011-09-26 13:04:34.194682360
+2011-09-26 13:04:34.297989343
+2011-09-26 13:04:34.401283278
+2011-09-26 13:04:34.504527128
+2011-09-26 13:04:34.607802417
+2011-09-26 13:04:34.711087760
+2011-09-26 13:04:34.814355620
+2011-09-26 13:04:34.917648931
+2011-09-26 13:04:35.020923233
+2011-09-26 13:04:35.124177585
+2011-09-26 13:04:35.227469163
+2011-09-26 13:04:35.330748917
+2011-09-26 13:04:35.433963723
+2011-09-26 13:04:35.537190894
+2011-09-26 13:04:35.640452958
+2011-09-26 13:04:35.743742739
+2011-09-26 13:04:35.847022176
+2011-09-26 13:04:35.950271780
+2011-09-26 13:04:36.053488779
+2011-09-26 13:04:36.156731687
+2011-09-26 13:04:36.259997486
+2011-09-26 13:04:36.363209987
+2011-09-26 13:04:36.466424567
+2011-09-26 13:04:36.569624701
+2011-09-26 13:04:36.672925196
+2011-09-26 13:04:36.776165764
+2011-09-26 13:04:36.879301220
+2011-09-26 13:04:36.982498156
+2011-09-26 13:04:37.085692252
+2011-09-26 13:04:37.188918035
+2011-09-26 13:04:37.292135658
+2011-09-26 13:04:37.395349614
+2011-09-26 13:04:37.498575284
+2011-09-26 13:04:37.601778062
+2011-09-26 13:04:37.704961679
+2011-09-26 13:04:37.808162495
+2011-09-26 13:04:37.911378801
+2011-09-26 13:04:38.014581331
+2011-09-26 13:04:38.117808833
+2011-09-26 13:04:38.221042124
+2011-09-26 13:04:38.324276085
+2011-09-26 13:04:38.427558211
+2011-09-26 13:04:38.530837254
+2011-09-26 13:04:38.634156788
+2011-09-26 13:04:38.737468483
+2011-09-26 13:04:38.840772177
+2011-09-26 13:04:38.944160820
+2011-09-26 13:04:39.047556446
+2011-09-26 13:04:39.150937092
+2011-09-26 13:04:39.254330815
+2011-09-26 13:04:39.357768089
+2011-09-26 13:04:39.461236131
+2011-09-26 13:04:39.564688516
+2011-09-26 13:04:39.668137104
+2011-09-26 13:04:39.771585858
+2011-09-26 13:04:39.875048796
+2011-09-26 13:04:39.978517137
+2011-09-26 13:04:40.082008601
+2011-09-26 13:04:40.185503214
+2011-09-26 13:04:40.288992178
+2011-09-26 13:04:40.392504444
+2011-09-26 13:04:40.495973075
+2011-09-26 13:04:40.599400084
+2011-09-26 13:04:40.702844822
+2011-09-26 13:04:40.806273004
+2011-09-26 13:04:40.909693242
+2011-09-26 13:04:41.013114779
+2011-09-26 13:04:41.116475689
+2011-09-26 13:04:41.219850175
+2011-09-26 13:04:41.323247270
+2011-09-26 13:04:41.426587321
+2011-09-26 13:04:41.529889840
+2011-09-26 13:04:41.633198896
+2011-09-26 13:04:41.736532013
+2011-09-26 13:04:41.839873160
+2011-09-26 13:04:41.943155264
+2011-09-26 13:04:42.046403723
+2011-09-26 13:04:42.149658272
+2011-09-26 13:04:42.252880524
+2011-09-26 13:04:42.356117971
+2011-09-26 13:04:42.459365092
+2011-09-26 13:04:42.562622210
+2011-09-26 13:04:42.665904890
+2011-09-26 13:04:42.769154732
+2011-09-26 13:04:42.872380298
+2011-09-26 13:04:42.975612623
+2011-09-26 13:04:43.078937121
+2011-09-26 13:04:43.182197917
+2011-09-26 13:04:43.285382470
+2011-09-26 13:04:43.388602972
+2011-09-26 13:04:43.491806052
+2011-09-26 13:04:43.595032884
+2011-09-26 13:04:43.698724998
+2011-09-26 13:04:43.801931003
+2011-09-26 13:04:43.904677436
+2011-09-26 13:04:44.007882621
+2011-09-26 13:04:44.111073932
+2011-09-26 13:04:44.214291904
+2011-09-26 13:04:44.317542613
+2011-09-26 13:04:44.420786651
+2011-09-26 13:04:44.523975691
+2011-09-26 13:04:44.627157621
+2011-09-26 13:04:44.730388368
+2011-09-26 13:04:44.833617097
+2011-09-26 13:04:44.936830345
+2011-09-26 13:04:45.040056894
+2011-09-26 13:04:45.143307359
+2011-09-26 13:04:45.246573491
+2011-09-26 13:04:45.349799334
+2011-09-26 13:04:45.453006344
+2011-09-26 13:04:45.556158193
+2011-09-26 13:04:45.659241506
+2011-09-26 13:04:45.762386345
+2011-09-26 13:04:45.865443920
+2011-09-26 13:04:45.968411521
+2011-09-26 13:04:46.071456700
+2011-09-26 13:04:46.174492262
+2011-09-26 13:04:46.277515679
+2011-09-26 13:04:46.380561323
+2011-09-26 13:04:46.483618987
+2011-09-26 13:04:46.586664270
+2011-09-26 13:04:46.689720187
+2011-09-26 13:04:46.792812032
+2011-09-26 13:04:46.895899467
+2011-09-26 13:04:46.998993630
+2011-09-26 13:04:47.102132861
+2011-09-26 13:04:47.205293239
+2011-09-26 13:04:47.308506549
+2011-09-26 13:04:47.411779993
+2011-09-26 13:04:47.515076291
+2011-09-26 13:04:47.618330665
+2011-09-26 13:04:47.721590862
+2011-09-26 13:04:47.824916201
+2011-09-26 13:04:47.928236690
+2011-09-26 13:04:48.031571686
+2011-09-26 13:04:48.134922273