diff --git a/.gitlab-ci.yml b/.gitlab-ci.yml
index 694447df19317c8b9a11e9ffe375b205a3ec5417..45acf90fed839847508a7a4dbd6edc4e35c6d59e 100644
--- a/.gitlab-ci.yml
+++ b/.gitlab-ci.yml
@@ -4,6 +4,16 @@ stages:
   - demos
   
 ############ YAML ANCHORS ############
+.print_variables_template: &print_variables_definition
+  # Print variables
+  - echo $WOLF_CORE_BRANCH
+  - echo $WOLF_LASER_BRANCH
+  - echo $WOLF_ROS_NODE_NODE
+  - echo $CI_COMMIT_BRANCH
+  - echo $WOLF_ROS_LASER_NODE
+  - echo $WOLF_ROS_LASER_BRANCH
+  - echo $LASERSCANUTILS_BRANCH
+
 .preliminaries_template: &preliminaries_definition
   ## Install ssh-agent if not already installed, it is required by Docker.
   ## (change apt-get to yum if you use an RPM-based image)
@@ -91,9 +101,12 @@ stages:
   - if [ -d laser_scan_utils ]; then
   -   echo "directory laser_scan_utils exists"
   -   cd laser_scan_utils
+  -   git checkout main
+  -   git pull
+  -   git checkout $LASERSCANUTILS_BRANCH
   -   git pull
   - else
-  -   git clone https://gitlab.iri.upc.edu/labrobotica/algorithms/laser_scan_utils.git
+  -   git clone -b $LASERSCANUTILS_BRANCH https://gitlab.iri.upc.edu/labrobotica/algorithms/laser_scan_utils.git
   -   cd laser_scan_utils
   - fi
   - mkdir -pv build
@@ -160,17 +173,13 @@ stages:
 .clone_wolfrosnode_template: &clone_wolfrosnode_definition
   - roscd
   - cd ../src
-  - git clone ssh://git@gitlab.iri.upc.edu:2202/mobile_robotics/wolf_projects/wolf_ros/wolf_ros_node.git
-  - cd wolf_ros_node
-  - git checkout $WOLF_ROS_CORE_BRANCH
+  - git -b $WOLF_ROS_NODE_BRANCH clone ssh://git@gitlab.iri.upc.edu:2202/mobile_robotics/wolf_projects/wolf_ros/wolf_ros_node.git
 
 .build_and_test_template: &build_and_test_definition
   - roscd
   - cd ../src
-  - git clone ssh://git@gitlab.iri.upc.edu:2202/${CI_PROJECT_PATH}.git
-  - cd wolf_ros_laser
-  - git checkout $CI_COMMIT_BRANCH
-  - cd ../..
+  - git clone -b $CI_COMMIT_BRANCHssh://git@gitlab.iri.upc.edu:2202/${CI_PROJECT_PATH}.git
+  - cd ..
   - catkin_make
 
 ############ LICENSE HEADERS ############
@@ -178,9 +187,8 @@ license_headers:
   stage: license
   image: labrobotica/wolf_deps_ros:20.04
   cache: []
-  except:
-    - master
   before_script:
+    - *print_variables_definition
     - *preliminaries_definition
     - *install_wolf_definition
   script:
@@ -194,9 +202,8 @@ build_and_test:bionic:
     - key: wolf_and_deps-bionic
       paths:
       - ci_deps
-  except:
-    - master
   before_script:
+    - *print_variables_definition
     - *preliminaries_definition
     - *install_wolf_definition
     - *install_csm_definition
@@ -215,9 +222,8 @@ build_and_test:focal:
     - key: wolf_and_deps-focal
       paths:
       - ci_deps
-  except:
-    - master
   before_script:
+    - *print_variables_definition
     - *preliminaries_definition
     - *install_wolf_definition
     - *install_csm_definition
@@ -234,7 +240,8 @@ demo_laser:
   variables:
     WOLF_CORE_BRANCH: $WOLF_CORE_BRANCH
     WOLF_LASER_BRANCH: $WOLF_LASER_BRANCH
-    WOLF_ROS_CORE_BRANCH: $WOLF_ROS_CORE_BRANCH
+    WOLF_ROS_NODE_BRANCH: $WOLF_ROS_NODE_BRANCH
     WOLF_ROS_LASER_BRANCH: $CI_COMMIT_BRANCH
+    LASERSCANUTILS_BRANCH: $LASERSCANUTILS_BRANCH
   trigger: 
     project: mobile_robotics/wolf_projects/wolf_ros/demos/wolf_demo_laser2d
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 7f704f3c32a4322c3080c9d4dd7e9830d879ef32..90765e7c8f12d53ae8b327f9480ccb399ba8d80e 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -1,13 +1,11 @@
-cmake_minimum_required(VERSION 2.8.3)
+CMAKE_MINIMUM_REQUIRED(VERSION 3.10)
 project(wolf_ros_laser)
 
 ## Compile as C++14
-add_compile_options(-std=c++14)
+add_compile_options(-std=c++14) 
+# -fPIC and -rdynamic ensure unique singleton instance across shared libraries (for factories) see: https://stackoverflow.com/a/8626922
+SET(CMAKE_CXX_FLAGS "-fPIC -rdynamic")
 
-# SET(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_SOURCE_DIR}/wolf_ros_wrapper/cmake_modules")
-## Find catkin macros and libraries
-## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
-## is used, also find other catkin packages
 find_package(catkin REQUIRED COMPONENTS
   roscpp
   sensor_msgs
@@ -20,102 +18,11 @@ find_package(catkin REQUIRED COMPONENTS
 )
 
 ## System dependencies are found with CMake's conventions
-# find_package(Boost REQUIRED COMPONENTS system)
-# find_package(Ceres REQUIRED)
-# find_package(Eigen3 REQUIRED)
 find_package(wolfcore REQUIRED)
 find_package(wolflaser REQUIRED)
-find_package(falkolib QUIET)
 
-## Uncomment this if the package has a setup.py. This macro ensures
-## modules and global scripts declared therein get installed
-## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
-# catkin_python_setup()
-
-################################################
-## Declare ROS messages, services and actions ##
-################################################
-
-## To declare and build messages, services or actions from within this
-## package, follow these steps:
-## * Let MSG_DEP_SET be the set of packages whose message types you use in
-##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
-## * In the file package.xml:
-##   * add a build_depend tag for "message_generation"
-##   * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
-##   * If MSG_DEP_SET isn't empty the following dependency has been pulled in
-##     but can be declared for certainty nonetheless:
-##     * add a exec_depend tag for "message_runtime"
-## * In this file (CMakeLists.txt):
-##   * add "message_generation" and every package in MSG_DEP_SET to
-##     find_package(catkin REQUIRED COMPONENTS ...)
-##   * add "message_runtime" and every package in MSG_DEP_SET to
-##     catkin_package(CATKIN_DEPENDS ...)
-##   * uncomment the add_*_files sections below as needed
-##     and list every .msg/.srv/.action file to be processed
-##   * uncomment the generate_messages entry below
-##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
-
-## Generate messages in the 'msg' folder
-# add_message_files(
-#   FILES
-#   Message1.msg
-#   Message2.msg
-# )
-
-## Generate services in the 'srv' folder
-# add_service_files(
-#   FILES
-#   Service1.srv
-#   Service2.srv
-# )
-
-## Generate actions in the 'action' folder
-# add_action_files(
-#   FILES
-#   Action1.action
-#   Action2.action
-# )
-
-## Generate added messages and services with any dependencies listed here
-#generate_messages(
-#  DEPENDENCIES
-#  iri_gnss_msgs
-#)
-
-################################################
-## Declare ROS dynamic reconfigure parameters ##
-################################################
-
-## To declare and build dynamic reconfigure parameters within this
-## package, follow these steps:
-## * In the file package.xml:
-##   * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
-## * In this file (CMakeLists.txt):
-##   * add "dynamic_reconfigure" to
-##     find_package(catkin REQUIRED COMPONENTS ...)
-##   * uncomment the "generate_dynamic_reconfigure_options" section below
-##     and list every .cfg file to be processed
-
-## Generate dynamic reconfigure parameters in the 'cfg' folder
-#generate_dynamic_reconfigure_options(
-#  cfg/WolfROS.cfg
-#)
-
-###################################
-## catkin specific configuration ##
-###################################
-## The catkin_package macro generates cmake config files for your package
-## Declare things to be passed to dependent projects
-## INCLUDE_DIRS: uncomment this if your package contains header files
-## LIBRARIES: libraries you create in this project that dependent projects also need
-## CATKIN_DEPENDS: catkin_packages dependent projects also need
-## DEPENDS: system dependencies of this project that dependent projects also need
+# produces the XXXConfig.cmake file among other things
 catkin_package(
-#  INCLUDE_DIRS include
-#  LIBRARIES wolf_ros1
-#  CATKIN_DEPENDS roscpp sensor_msgs std_msgs
-#  DEPENDS system_lib
 )
 
 ###########
@@ -125,111 +32,44 @@ catkin_package(
 ## Specify additional locations of header files
 ## Your package locations should be listed before other locations
 include_directories(
-    include
-  ${EIGEN_INCLUDE_DIRS}
-  ${wolfcore_INCLUDE_DIRS}
-  ${wolflaser_INCLUDE_DIRS}
-  ${laser_scan_utils_INCLUDE_DIRS}
+  include
   ${catkin_INCLUDE_DIRS}
-  ${CERES_INCLUDE_DIRS}
 )
-# link_directories(/usr/local/lib/iri-algorithms)
-## Declare a C++ library
-# add_library(${PROJECT_NAME}
-#   src/${PROJECT_NAME}/wolf_ros.cpp
-# )
-add_library(publisher_${PROJECT_NAME})
-add_library(subscriber_${PROJECT_NAME})
 
-target_sources(publisher_${PROJECT_NAME} PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/src/publisher_laser_map.cpp)
-target_sources(subscriber_${PROJECT_NAME} PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/src/subscriber_laser2d.cpp)
+add_library(publisher_${PROJECT_NAME}
+  ${CMAKE_CURRENT_SOURCE_DIR}/src/publisher_laser_map.cpp
+)
+add_library(subscriber_${PROJECT_NAME}
+  ${CMAKE_CURRENT_SOURCE_DIR}/src/subscriber_laser2d.cpp
+)
+
+get_target_property(wolflaser_INCLUDE_DIR wolflaser INTERFACE_INCLUDE_DIRECTORIES)
+message("wolflaser_INCLUDE_DIRS: ${wolflaser_INCLUDE_DIRS}")
+find_file(FALKO laser/processor/processor_loop_closure_falko.h PATHS ${wolflaser_INCLUDE_DIR})
+find_file(ICP laser/processor/processor_odom_icp.h PATHS ${wolflaser_INCLUDE_DIR})
 
-if (falkolib_FOUND)
-	message("Found Falkolib. Compiling publisher_falko.")
+if (FALKO)
+	message("Found 'laser/processor/processor_loop_closure_falko.h'. Compiling publisher_falko.")
 	target_sources(publisher_${PROJECT_NAME} PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/src/publisher_falko.cpp)
+else()
+  message("Didn't find 'laser/processor/processor_loop_closure_falko.h'. Not compiling publisher_falko.")
 endif()
 
-find_file(ICP wolflaser_INCLUDE_DIRS laser/processor/processor_odom_icp.h)
-if (NOT ICP_NOTFOUND)
-	message("Found 'processor_odom_icp.h'. Compiling publisher_odom_icp.")
+if (ICP)
+	message("Found 'laser/processor/processor_odom_icp.h'. Compiling publisher_odom_icp.")
 	target_sources(publisher_${PROJECT_NAME} PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/src/publisher_odom_icp.cpp)
+else()
+  message("Didn't find 'laser/processor/processor_odom_icp.h'. Not compiling publisher_odom_icp.")
 endif ()
 
-## Add cmake target dependencies of the library
-## as an example, code may need to be generated before libraries
-## either from message generation or dynamic reconfigure
-# add_dependencies(${PROJECT_NAME}_node ${PROJECT_NAME}_gencfg)
-# add_dependencies(${PROJECT_NAME}_visualizer ${PROJECT_NAME}_gencfg)
-
-## Declare a C++ executable
-## With catkin_make all packages are built within a single CMake context
-## The recommended prefix ensures that target names across packages don't collide
-
-## Rename C++ executable without prefix
-## The above recommended prefix causes long target names, the following renames the
-## target back to the shorter version for ease of user use
-## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg"
-# set_target_properties(${PROJECT_NAME} PROPERTIES OUTPUT_NAME node PREFIX "")
-
-## Add cmake target dependencies of the executable
-## same as for the library above
-#add_dependencies(${PROJECT_NAME}_node ${PROJECT_NAME}_gencfg)
-#add_dependencies(${PROJECT_NAME}_visualizer ${PROJECT_NAME}_gencfg)
-
 ## Specify libraries to link a library or executable target against
 target_link_libraries(subscriber_${PROJECT_NAME}
-            ${wolfcore_LIBRARIES}
-            ${wolflaser_LIBRARIES}
+            wolfcore
+            wolflaser
+            ${catkin_LIBRARIES}
             )
 target_link_libraries(publisher_${PROJECT_NAME}
-            ${wolfcore_LIBRARIES}
-            ${wolflaser_LIBRARIES}
+            wolfcore
+            wolflaser
             ${catkin_LIBRARIES}
-            )
-#############
-## Install ##
-#############
-
-# all install targets should use catkin DESTINATION variables
-# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
-
-## Mark executable scripts (Python etc.) for installation
-## in contrast to setup.py, you can choose the destination
-# install(PROGRAMS
-#   scripts/my_python_script
-#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-# )
-
-## Mark executables and/or libraries for installation
-# install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}
-#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-# )
-
-## Mark cpp header files for installation
-# install(DIRECTORY include/${PROJECT_NAME}/
-#   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
-#   FILES_MATCHING PATTERN "*.h"
-#   PATTERN ".svn" EXCLUDE
-# )
-
-## Mark other files for installation (e.g. launch and bag files, etc.)
-# install(FILES
-#   # myfile1
-#   # myfile2
-#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
-# )
-
-#############
-## Testing ##
-#############
-
-## Add gtest based cpp test target and link libraries
-# catkin_add_gtest(${PROJECT_NAME}-test test/test_wolf_ros.cpp)
-# if(TARGET ${PROJECT_NAME}-test)
-#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
-# endif()
-
-## Add folders to be run by python nosetests
-# catkin_add_nosetests(test)
+            )
\ No newline at end of file
diff --git a/include/publisher_falko.h b/include/publisher_falko.h
index e4a965e387773e06fad6bedadb6c9cfdd396a925..b660abe0d27303a86cf965d63af7ba9a0ea5d9d1 100644
--- a/include/publisher_falko.h
+++ b/include/publisher_falko.h
@@ -51,12 +51,12 @@ class PublisherFalko : public Publisher
     std_msgs::ColorRGBA marker_color_target_;
     std_msgs::ColorRGBA marker_color_reference_;
 
-    bool          extrinsics_;
-    SensorBasePtr sensor_;
-    std::string   frame_id_, map_frame_id_;
+    bool                  extrinsics_;
+    ProcessorBaseConstPtr processor_;
+    std::string           map_frame_id_;
 
-    FrameBasePtr last_frame;
-    FrameBasePtr KF_old;
+    FrameBaseConstPtr last_frame;
+    FrameBaseConstPtr KF_old;
 
     ros::Publisher pub_keypoints_;
     ros::Publisher pub_marker_scene_target_;
@@ -64,10 +64,10 @@ class PublisherFalko : public Publisher
     ros::Publisher pub_marker_associations_;
 
     // Reference scene info
-    std::map<CaptureBasePtr, visualization_msgs::Marker> map_markers_reference_scenes_;
+    std::map<CaptureBaseConstPtr, visualization_msgs::Marker> map_markers_reference_scenes_;
 
   public:
-    PublisherFalko(const std::string &_unique_name, const ParamsServer &_server, const ProblemPtr _problem);
+    PublisherFalko(const std::string &_unique_name, const ParamsServer &_server, ProblemConstPtr _problem);
     WOLF_PUBLISHER_CREATE(PublisherFalko);
 
     virtual ~PublisherFalko(){};
@@ -76,26 +76,31 @@ class PublisherFalko : public Publisher
 
     void publishDerived() override;
 
-    void publishScene(std::vector<falkolib::FALKO> &_keypoints, laserscanutils::LaserScan &_scan,
-                      Eigen::Vector3d &_p_rob, Eigen::Quaterniond &_q_rob, ros::Publisher &_pub_lines,
-                      std_msgs::ColorRGBA _marker_color, int & _id, CaptureBasePtr cap);
+    void publishScene(const std::vector<falkolib::FALKO> &_keypoints, 
+                      const laserscanutils::LaserScan &_scan,
+                      const Eigen::Vector3d &_p_rob, 
+                      const Eigen::Quaterniond &_q_rob, 
+                      const ros::Publisher &_pub_lines,
+                      std_msgs::ColorRGBA _marker_color, 
+                      int _id, 
+                      CaptureBaseConstPtr cap);
 
     void publishEmpty();
 
-    void publishAssociations(std::vector<std::pair<int, int>> &associations_, CaptureBasePtr &cap_target,
-                             CaptureBasePtr &cap_reference);
+    void publishAssociations(const std::vector<std::pair<int, int>> &associations_, 
+                             CaptureBaseConstPtr cap_target,
+                             CaptureBaseConstPtr cap_reference);
 
-    void getKeypointsAndScan(FrameBasePtr &_frame, laserscanutils::LaserScan &_scan,
+    void getKeypointsAndScan(FrameBaseConstPtr _frame, 
+                             laserscanutils::LaserScan &_scan,
                              std::vector<falkolib::FALKO> &_keypoints);
 
     void getPosition(Eigen::Vector3d &p, Eigen::Quaterniond &q, VectorComposite current_state);
 
   protected:
-    //Eigen::Quaterniond    q_frame_;
-    //Eigen::Vector3d       t_frame_;
     int                   id_old_ = 1;
 
-    std::map<FrameBasePtr, VectorComposite> map_frame_states;
+    std::map<FrameBaseConstPtr, VectorComposite> map_frame_states;
 };
 
 } // namespace wolf
diff --git a/include/publisher_laser_map.h b/include/publisher_laser_map.h
index 1af60603fbdc5e6d76f042ec00ead531066540af..c018cfb8fa86893f082e182d1ecf9a98603966de 100644
--- a/include/publisher_laser_map.h
+++ b/include/publisher_laser_map.h
@@ -48,7 +48,7 @@ namespace wolf
 
 struct ScanData
 {
-    CaptureLaser2dPtr capture_;
+    CaptureLaser2dConstPtr capture_;
     laserscanutils::LaserScanParams params_;
     Eigen::MatrixXf local_points_;
     pcl::PointCloud<pcl::PointXYZRGB> local_pc_;
@@ -78,14 +78,14 @@ class PublisherLaserMap: public Publisher
         int pc_r_, pc_g_, pc_b_;
 
         // std::maps
-        std::map<FrameBasePtr,std::list<ScanData>> scans_;
-        std::map<FrameBasePtr,Eigen::Vector3d> current_trajectory_, last_trajectory_occ_, last_trajectory_pc_;
+        std::map<FrameBaseConstPtr,std::list<ScanData>> scans_;
+        std::map<FrameBaseConstPtr,Eigen::Vector3d> current_trajectory_, last_trajectory_occ_, last_trajectory_pc_;
 
 
     public:
         PublisherLaserMap(const std::string& _unique_name,
-                              const ParamsServer& _server,
-                              const ProblemPtr _problem);
+                          const ParamsServer& _server,
+                          ProblemConstPtr _problem);
         WOLF_PUBLISHER_CREATE(PublisherLaserMap);
 
         virtual ~PublisherLaserMap(){};
@@ -94,9 +94,9 @@ class PublisherLaserMap: public Publisher
 
     protected:
         void updateTrajectory();
-        bool trajectoryChanged(const std::map<FrameBasePtr,Eigen::Vector3d>& _last_trajectory,
-                               const std::map<FrameBasePtr,Eigen::Vector3d>& _current_trajectory) const;
-        void storeScan(FrameBasePtr frame, CaptureLaser2dPtr capture);
+        bool trajectoryChanged(const std::map<FrameBaseConstPtr,Eigen::Vector3d>& _last_trajectory,
+                               const std::map<FrameBaseConstPtr,Eigen::Vector3d>& _current_trajectory) const;
+        void storeScan(FrameBaseConstPtr frame, CaptureLaser2dConstPtr capture);
 
         // occmap
         void resetOccMap();
diff --git a/include/publisher_odom_icp.h b/include/publisher_odom_icp.h
index 1c60816d10d6d4d37b3bd5683c6da02e526b4550..1c129cbbb8eded621214cd5f28437d5a3b631b37 100644
--- a/include/publisher_odom_icp.h
+++ b/include/publisher_odom_icp.h
@@ -42,14 +42,14 @@ class PublisherOdomIcp: public Publisher
 {
     protected:
 
-        ProcessorOdomIcpPtr processor_odom_icp_;
+        ProcessorOdomIcpConstPtr processor_odom_icp_;
         geometry_msgs::PoseStamped msg_;
 
     public:
 
         PublisherOdomIcp(const std::string& _unique_name,
                          const ParamsServer& _server,
-                         const ProblemPtr _problem);
+                         ProblemConstPtr _problem);
         WOLF_PUBLISHER_CREATE(PublisherOdomIcp);
 
         virtual ~PublisherOdomIcp(){};
diff --git a/src/publisher_falko.cpp b/src/publisher_falko.cpp
index 53481413f881c738c5812eb1e8458775f2b33a2e..fbe7214132f0076b4b94eafbd184b15cb6fa7e02 100644
--- a/src/publisher_falko.cpp
+++ b/src/publisher_falko.cpp
@@ -25,8 +25,9 @@
 
 namespace wolf {
 
-PublisherFalko::PublisherFalko(const std::string &_unique_name, const ParamsServer &_server,
-                               const ProblemPtr _problem)
+PublisherFalko::PublisherFalko(const std::string &_unique_name, 
+                               const ParamsServer &_server,
+                               ProblemConstPtr _problem)
 : Publisher(_unique_name, _server, _problem)
 {
     Eigen::Vector4d marker_color_v;
@@ -45,11 +46,10 @@ PublisherFalko::PublisherFalko(const std::string &_unique_name, const ParamsServ
     max_points_ = getParamWithDefault<int>(_server, prefix_ + "/max_points", 1e3);
 
     extrinsics_ = _server.getParam<bool>(prefix_ + "/extrinsics");
-    sensor_     = _problem->getSensor(_server.getParam<std::string>(prefix_ + "/sensor"));
-    // dynamic_ptr_cast(sensor_)
-    if (!sensor_)
-        throw std::runtime_error("sensor not found");
-    frame_id_ = _server.getParam<std::string>(prefix_ + "/frame_id");
+    processor_  = _problem->findProcessor(_server.getParam<std::string>(prefix_ + "/processor_name"));
+
+    if (!processor_)
+        throw std::runtime_error("processor not found");
 }
 
 void PublisherFalko::initialize(ros::NodeHandle &nh, const std::string &topic)
@@ -79,7 +79,7 @@ void PublisherFalko::publishDerived()
     if (!KF)
         return;
 
-    auto cap = KF->getCaptureOf(sensor_, "CaptureLaser2d");
+    auto cap = KF->getCaptureOf(processor_->getSensor(), "CaptureLaser2d");
     if (not cap)
         return;
 
@@ -89,9 +89,7 @@ void PublisherFalko::publishDerived()
 
     // state not ready
     if (current_state.count('P') == 0 or current_state.count('O') == 0 or not loc_ts.ok())
-    {
         return;
-    }
 
     // 2D robot position and yaw
     Eigen::Vector3d    p = Eigen::Vector3d::Zero();
@@ -110,23 +108,6 @@ void PublisherFalko::publishDerived()
     if (p(0) == 0 and p(1) == 0)
         return;
 
-    // LOOK FOR REFERENCE SCENE
-    auto processor_list = sensor_->getProcessorList();
-
-    ProcessorBasePtr processor_falko;
-    for (auto processor : processor_list)
-    {
-        if (processor->getType() == "ProcessorLoopClosureFalko")
-        {
-            processor_falko = processor;
-            break;
-        }
-    }
-    if (!processor_falko)
-        return;
-
-    // FactorBasePtrList factor_list;
-    // KF->getFactorList(factor_list);
     if (KF == KF_old and KF_old != nullptr)
         return;
 
@@ -137,10 +118,9 @@ void PublisherFalko::publishDerived()
 
     id = 1;
 
-    for (const FactorBasePtr &factor : KF->getConstrainedByList())
-        if (factor->getProcessor() == processor_falko)
+    for (auto factor : KF->getConstrainedByList())
+        if (factor->getProcessor() == processor_)
         {
-
             auto ftr         = factor->getFeature();
             auto frame_other = ftr->getFrame();
 
@@ -166,31 +146,25 @@ void PublisherFalko::publishDerived()
             std::cout << "keypoints target: " << keypoints.size() << std::endl;
             std::cout << "keypoints reference: " << keypoints_other.size() << std::endl;
 
-            // std::cout << "scan : " << std::endl;
-            // for (int i = 0; i < scan.ranges_raw_.size(); i++)
-            //     {
-            //         std::cout << scan.ranges_raw_[i] << ",";
-            //     }
-            // std::cout << "scan other : " << std::endl;
-            // for (int i = 0; i < scan_other.ranges_raw_.size(); i++)
-            //     {
-            //         std::cout << scan_other.ranges_raw_[i] << ",";
-            //     }
-
             publishScene(keypoints_other, scan_other, p_other, q_other, pub_marker_scene_reference_,
                          marker_color_reference_, id, cap);
 
             // Publish associations between scenes
-            auto matchings_list =
-                    std::static_pointer_cast<ProcessorLoopClosureFalkoAhtBsc>(processor_falko)->match_list_;
+            auto processor_aht_bsc = std::dynamic_pointer_cast<const ProcessorLoopClosureFalkoAhtBsc>(processor_);
 
-            for (auto match : matchings_list)
+            if (processor_aht_bsc)
             {
-                if (match->capture_target == cap)
+                auto matchings_list = processor_aht_bsc->getMatchList();
+            
+                for (auto match : matchings_list)
                 {
-                    auto match_falko  = std::static_pointer_cast<MatchLoopClosureFalko>(match);
-                    auto associations = match_falko->match_falko_->associations;
-                    publishAssociations(associations, cap, match->capture_reference);
+                    if (match->capture_target == cap)
+                    {
+                        auto match_falko  = std::static_pointer_cast<const MatchLoopClosureFalko>(match);
+                        publishAssociations(match_falko->match_falko_->associations, 
+                                            cap, 
+                                            match->capture_reference);
+                    }
                 }
             }
         }
@@ -200,8 +174,9 @@ void PublisherFalko::publishDerived()
     return;
 }
 
-void PublisherFalko::publishAssociations(std::vector<std::pair<int, int>> &associations_, CaptureBasePtr &cap_target,
-                                         CaptureBasePtr &cap_reference)
+void PublisherFalko::publishAssociations(const std::vector<std::pair<int, int>> &associations_, 
+                                         CaptureBaseConstPtr cap_target,
+                                         CaptureBaseConstPtr cap_reference)
 {
 
     std_msgs::ColorRGBA marker_color;
@@ -255,11 +230,16 @@ void PublisherFalko::publishAssociations(std::vector<std::pair<int, int>> &assoc
     pub_marker_associations_.publish(asso_marker);
 }
 
-void PublisherFalko::publishScene(std::vector<falkolib::FALKO> &_keypoints, laserscanutils::LaserScan &_scan,
-                                  Eigen::Vector3d &_p_rob, Eigen::Quaterniond &_q_rob, ros::Publisher &_pub_lines,
-                                  std_msgs::ColorRGBA _marker_color, int &_id, CaptureBasePtr cap)
+void PublisherFalko::publishScene(const std::vector<falkolib::FALKO> &_keypoints, 
+                                  const laserscanutils::LaserScan &_scan,
+                                  const Eigen::Vector3d &_p_rob, 
+                                  const Eigen::Quaterniond &_q_rob, 
+                                  const ros::Publisher &_pub_lines,
+                                  std_msgs::ColorRGBA _marker_color,
+                                  int _id,
+                                  CaptureBaseConstPtr cap)
 {
-    auto sensor_laser = std::static_pointer_cast<SensorLaser2d>(sensor_);
+    auto sensor_laser = std::static_pointer_cast<const SensorLaser2d>(processor_->getSensor());
     auto scan_params  = sensor_laser->getScanParams();
 
     std::vector<double> angle_discretization;
@@ -312,7 +292,7 @@ void PublisherFalko::publishScene(std::vector<falkolib::FALKO> &_keypoints, lase
     if (_id == 0)
     {
         map_markers_reference_scenes_.insert(
-                std::pair<CaptureBasePtr, visualization_msgs::Marker>(cap, marker_msg_falko_scene));
+                std::pair<CaptureBaseConstPtr, visualization_msgs::Marker>(cap, marker_msg_falko_scene));
     }
 
     // Close the area
@@ -351,11 +331,12 @@ void PublisherFalko::publishEmpty()
     }
 }
 
-void PublisherFalko::getKeypointsAndScan(FrameBasePtr &_frame, laserscanutils::LaserScan &_scan,
+void PublisherFalko::getKeypointsAndScan(FrameBaseConstPtr _frame, 
+                                         laserscanutils::LaserScan &_scan,
                                          std::vector<falkolib::FALKO> &_keypoints)
 {
-    auto cap       = _frame->getCaptureOf(sensor_, "CaptureLaser2d");
-    auto cap_laser = std::dynamic_pointer_cast<CaptureLaser2d>(cap);
+    auto cap       = _frame->getCaptureOf(processor_->getSensor(), "CaptureLaser2d");
+    auto cap_laser = std::dynamic_pointer_cast<const CaptureLaser2d>(cap);
 
     _scan = cap_laser->getScan();
 
@@ -363,7 +344,7 @@ void PublisherFalko::getKeypointsAndScan(FrameBasePtr &_frame, laserscanutils::L
     {
         if (feat->getType() == "FeatureSceneFalko")
         {
-            auto feat_falko = std::static_pointer_cast<FeatureSceneFalkoBsc>(feat);
+            auto feat_falko = std::static_pointer_cast<const FeatureSceneFalkoBsc>(feat);
             _keypoints      = feat_falko->getScene()->keypoints_list_;
         }
     }
@@ -372,9 +353,9 @@ void PublisherFalko::getKeypointsAndScan(FrameBasePtr &_frame, laserscanutils::L
 void PublisherFalko::getPosition(Eigen::Vector3d &p, Eigen::Quaterniond &q, VectorComposite _state)
 {
 
-    p.head(2) = _state['P']; //+ Eigen::Rotation2Dd(_state['O'](0)) * sensor_->getP()->getState().head(2);
+    p.head(2) = _state['P']; //+ Eigen::Rotation2Dd(_state['O'](0)) * processor_->getSensor()->getP()->getState().head(2);
     q         = Eigen::Quaterniond(
-            Eigen::AngleAxisd(_state['O'](0) + sensor_->getO()->getState()(0), Eigen::Vector3d::UnitZ()));
+            Eigen::AngleAxisd(_state['O'](0) + processor_->getSensor()->getO()->getState()(0), Eigen::Vector3d::UnitZ()));
 }
 
 WOLF_REGISTER_PUBLISHER(PublisherFalko)
diff --git a/src/publisher_laser_map.cpp b/src/publisher_laser_map.cpp
index e1bb8019fae198e6739ce5a82e865303ec9aa1a3..b699f1f42f1eee61a87d464a4a4de2140faaa424 100644
--- a/src/publisher_laser_map.cpp
+++ b/src/publisher_laser_map.cpp
@@ -35,8 +35,8 @@ namespace wolf
 {
 
 PublisherLaserMap::PublisherLaserMap(const std::string& _unique_name,
-                                             const ParamsServer& _server,
-                                             const ProblemPtr _problem) :
+                                     const ParamsServer& _server,
+                                     ProblemConstPtr _problem) :
     Publisher(_unique_name, _server, _problem),
     n_cells_(Eigen::Vector2i::Constant(100)),
     resized_(false),
@@ -135,7 +135,7 @@ void PublisherLaserMap::updateTrajectory()
     scans_.clear();
 
     // copy new trajectory poses and scans
-    auto frame_map = problem_->getTrajectory()->getFrameMap(); // copy for thread-safe
+    auto frame_map = problem_->getTrajectory()->getFrameMap();
     for (auto frame_pair : frame_map)
     {
         if (frame_pair.second->isRemoving())
@@ -149,7 +149,7 @@ void PublisherLaserMap::updateTrajectory()
         auto cap_list = frame_pair.second->getCaptureList();
         for (auto cap : cap_list)
         {
-            auto cap_laser = std::dynamic_pointer_cast<CaptureLaser2d>(cap);
+            auto cap_laser = std::dynamic_pointer_cast<const CaptureLaser2d>(cap);
             if (cap_laser and not cap_laser->isRemoving())
                 storeScan(frame_pair.second, cap_laser);
         }
@@ -163,11 +163,11 @@ void PublisherLaserMap::updateTrajectory()
     }
 }
 
-void PublisherLaserMap::storeScan(FrameBasePtr frame, CaptureLaser2dPtr capture)
+void PublisherLaserMap::storeScan(FrameBaseConstPtr frame, CaptureLaser2dConstPtr capture)
 {
     ScanData scan_data;
     scan_data.capture_ = capture;
-    scan_data.params_ = std::static_pointer_cast<SensorLaser2d>(capture->getSensor())->getScanParams();
+    scan_data.params_ = std::static_pointer_cast<const SensorLaser2d>(capture->getSensor())->getScanParams();
 
     // local points
     Eigen::Vector2d laser_extrinsics_p = capture->getSensor()->getP()->getState();
@@ -211,8 +211,8 @@ void PublisherLaserMap::storeScan(FrameBasePtr frame, CaptureLaser2dPtr capture)
     scans_.at(frame).push_back(scan_data);
 }
 
-bool PublisherLaserMap::trajectoryChanged(const std::map<FrameBasePtr,Eigen::Vector3d>& _last_trajectory,
-                                          const std::map<FrameBasePtr,Eigen::Vector3d>& _current_trajectory) const
+bool PublisherLaserMap::trajectoryChanged(const std::map<FrameBaseConstPtr,Eigen::Vector3d>& _last_trajectory,
+                                          const std::map<FrameBaseConstPtr,Eigen::Vector3d>& _current_trajectory) const
 {
     // check if trajectory changed enough
     for (auto frame_pos : _last_trajectory)
diff --git a/src/publisher_odom_icp.cpp b/src/publisher_odom_icp.cpp
index c9ed4ffb0c6b8af35be2c3ee82b1454227f7b456..eef4c179c0ad711788dff2a7a106ab14bb500716 100644
--- a/src/publisher_odom_icp.cpp
+++ b/src/publisher_odom_icp.cpp
@@ -32,7 +32,7 @@ namespace wolf
 
 PublisherOdomIcp::PublisherOdomIcp(const std::string& _unique_name,
                                    const ParamsServer& _server,
-                                   const ProblemPtr _problem) :
+                                   ProblemConstPtr _problem) :
         Publisher(_unique_name, _server, _problem),
         processor_odom_icp_(nullptr)
 {
@@ -45,7 +45,7 @@ PublisherOdomIcp::PublisherOdomIcp(const std::string& _unique_name,
         {
             if (proc->getName() == processor_name)
             {
-                processor_odom_icp_ = std::dynamic_pointer_cast<ProcessorOdomIcp>(proc);
+                processor_odom_icp_ = std::dynamic_pointer_cast<const ProcessorOdomIcp>(proc);
                 if (not processor_odom_icp_)
                     throw std::runtime_error("PublisherOdomIcp requires a processor of type 'ProcessorOdomIcp'.");
                 break;