Merge branch '27-new-branch-laser-3d-2' of...

Merge branch '27-new-branch-laser-3d-2' of ssh://gitlab.iri.upc.edu:2202/mobile_robotics/wolf_projects/wolf_lib/plugins/laser into 27-new-branch-laser-3d-2

CMakeLists.txt

@@ -85,11 +85,6 @@ find_package(PkgConfig)
 pkg_check_modules(csm QUIET csm)
 # link_directories(${csm_LIBRARY_DIRS})
 find_package(PCL 1.10 COMPONENTS common registration io)
-if (PCL_FOUND)
-include_directories(${PCL_INCLUDE_DIRS} )
-link_directories(${PCL_LIBRARY_DIRS})
-add_definitions(${PCL_DEFINITIONS})
-endif(PCL_FOUND)
 
 # ============ CONFIG.H ============ 
 set(_WOLF_ROOT_DIR ${CMAKE_SOURCE_DIR})
@@ -241,20 +236,6 @@ if(PCL_FOUND)
   SET(SRCS_PROCESSOR ${SRCS_PROCESSOR}
       src/processor/processor_odom_icp_3d.cpp
     )
-
-
-# SET(HDRS_PROCESSOR ${HDRS_PROCESSOR}
-#   include/${PROJECT_NAME}/processor/processor_loop_closure_falko.h
-#   )
-# SET(SRCS_PROCESSOR ${SRCS_PROCESSOR}
-#   src/processor/processor_loop_closure_falko.cpp
-#   )
-# SET(HDRS_FEATURE ${HDRS_FEATURE}
-#   include/${PROJECT_NAME}/feature/feature_scene_falko.h
-#   )
-# SET(SRCS_FEATURE ${SRCS_FEATURE}
-#   src/feature/feature_scene_falko.cpp
-#   )
 endif(PCL_FOUND)
 
 

include/laser/capture/capture_laser_3d.h

@@ -31,8 +31,6 @@
 
 namespace wolf
 {
-namespace laser
-{
 
 WOLF_PTR_TYPEDEFS(CaptureLaser3d);
 
@@ -41,14 +39,14 @@ class CaptureLaser3d : public CaptureBase
   public:
     CaptureLaser3d(const TimeStamp& _timestamp, SensorBasePtr _sensor, pcl::PointCloud<pcl::PointXYZ>::Ptr _point_cloud);
     ~CaptureLaser3d();
-    pcl::PointCloud<pcl::PointXYZ>::Ptr getPointCloud() const;
+    pcl::PointCloud<pcl::PointXYZ>::Ptr getPointCloud();
+    pcl::PointCloud<pcl::PointXYZ>::ConstPtr getPointCloud() const;
 
     
   private:
     pcl::PointCloud<pcl::PointXYZ>::Ptr point_cloud_;
 };
 
-}  // namespace laser
 }  // namespace wolf
 
 #endif  // CAPTURE_LASER_3d_H_
\ No newline at end of file

include/laser/processor/processor_odom_icp_3d.h

@@ -44,9 +44,6 @@
 namespace wolf
 {
 
-namespace laser
-{
-
 WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorOdomIcp3d);
 
 struct ParamsProcessorOdomIcp3d : public ParamsProcessorTracker, public ParamsMotionProvider
@@ -168,7 +165,7 @@ class ProcessorOdomIcp3d : public ProcessorTracker, public MotionProvider
 
     void establishFactors() override;
 };
-}  // namespace laser
+
 }  // namespace wolf
 
 #endif  // SRC_PROCESSOR_ODOM_ICP_3D_H_

include/laser/sensor/sensor_laser_3d.h

@@ -27,8 +27,6 @@
 
 namespace wolf
 {
-namespace laser
-{
 
 WOLF_STRUCT_PTR_TYPEDEFS(ParamsSensorLaser3d);
 
@@ -54,8 +52,6 @@ class SensorLaser3d : public SensorBase
     ParamsSensorLaser3dPtr params_laser3d_;
 };
 
-
-}  // namespace laser
 }  // namespace wolf
 
 #endif  // SENSOR_LASER_3d_H_
\ No newline at end of file

include/laser/utils/laser3d_tools.h

@@ -37,8 +37,6 @@
 
 namespace wolf
 {
-namespace laser
-{
 // _cloud_ref: first PointCloud
 // _cloud_other: second PointCloud
 inline void pairAlign(const pcl::PointCloud<pcl::PointXYZ>::Ptr                          _cloud_ref,
@@ -91,5 +89,4 @@ inline void pairAlign(const pcl::PointCloud<pcl::PointXYZ>::Ptr
     // _transform_final = _transform_final.inverse();   // Maybe we messed up
 }
 
-}  // namespace laser
 }  // namespace wolf

src/capture/capture_laser_3d.cpp

@@ -23,8 +23,6 @@
 
 namespace wolf
 {
-namespace laser
-{
 
 CaptureLaser3d::CaptureLaser3d(const TimeStamp& _timestamp, SensorBasePtr _sensor, pcl::PointCloud<pcl::PointXYZ>::Ptr _point_cloud)
     : CaptureBase("CaptureLaser3d", _timestamp, _sensor), point_cloud_(_point_cloud)
@@ -33,10 +31,14 @@ CaptureLaser3d::CaptureLaser3d(const TimeStamp& _timestamp, SensorBasePtr _senso
 
 CaptureLaser3d::~CaptureLaser3d() {}
 
-pcl::PointCloud<pcl::PointXYZ>::Ptr CaptureLaser3d::getPointCloud() const
+pcl::PointCloud<pcl::PointXYZ>::Ptr CaptureLaser3d::getPointCloud()
+{
+    return point_cloud_;
+}
+
+pcl::PointCloud<pcl::PointXYZ>::ConstPtr CaptureLaser3d::getPointCloud() const
 {
     return point_cloud_;
 }
 
-}  // namespace laser
 }  // 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

@@ -25,8 +25,7 @@
 
 namespace wolf
 {
-namespace laser
-{
+
 ProcessorOdomIcp3d::ProcessorOdomIcp3d(ParamsProcessorOdomIcp3dPtr _params)
     : ProcessorTracker("ProcessorOdomIcp3d", "PO", 3, _params), MotionProvider("PO", _params)
 {
@@ -187,5 +186,4 @@ void ProcessorOdomIcp3d::establishFactors()
         feature, feature, origin_laser_->getFrame(), shared_from_this(), false, TOP_MOTION);
 };
 
-}  // namespace laser
 }  // namespace wolf
\ No newline at end of file

src/sensor/sensor_laser_3d.cpp

@@ -23,8 +23,6 @@
 
 namespace wolf
 {
-namespace laser
-{
 
 SensorLaser3d::SensorLaser3d(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr)
     : SensorBase("SensorLaser3d", _p_ptr, _o_ptr, nullptr, 0)
@@ -44,5 +42,4 @@ SensorBase("SensorLaser3d",
 
 SensorLaser3d::~SensorLaser3d() {}
 
-}  // namespace laser
 }  // namespace wolf
\ No newline at end of file

test/gtest_laser3d_tools.cpp

@@ -86,7 +86,7 @@ TEST(pairAlign, identity)
     registration_solver.setMaximumIterations(200);
 
     // Alignment
-    wolf::laser::pairAlign(pcl_ref,
+    wolf::pairAlign(pcl_ref,
                            pcl_other,
                            transformation_guess,
                            transformation_final,
@@ -136,7 +136,7 @@ TEST(pairAlign, known_transformation)
     registration_solver.setMaximumIterations(100);
 
     // Alignment
-    wolf::laser::pairAlign(pcl_ref,
+    wolf::pairAlign(pcl_ref,
                            pcl_other,
                            transformation_guess,
                            transformation_final,

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
 #include "laser/processor/processor_odom_icp_3d.h"
 #include "core/processor/factory_processor.h"