diff --git a/.gitignore b/.gitignore
index 1a57960199ca6913030d90819f96848498a900e9..d3c36fd6eacd20e7ab33c93d779abd1a10cf12f3 100644
--- a/.gitignore
+++ b/.gitignore
@@ -23,6 +23,7 @@ src/examples/map_polyline_example_write.yaml
 /CMakeCache.txt
 *.dat
 .DS_Store
+laser.found
 
 *.graffle
 /Default/
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 19ff2105a0d8891aa3dec67ba614fbc42b739232..2aff3964dc1f960767780c9e9571995b0cdac205 100644
--- a/CMakeLists.txt
+++ b/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,12 @@ 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)
+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})
@@ -213,6 +220,44 @@ 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
+    include/${PROJECT_NAME}/capture/capture_laser_3d.h
+    )
+  SET(HDRS_SENSOR
+    include/${PROJECT_NAME}/sensor/sensor_laser_3d.h
+    )
+  SET(HDRS_PROCESSOR ${HDRS_PROCESSOR}
+  include/${PROJECT_NAME}/processor/processor_odom_icp_3d.h
+    )
+  SET(SRCS_CAPTURE
+    src/capture/capture_laser_3d.cpp
+    )
+  SET(SRCS_CAPTURE
+    src/capture/capture_laser_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)
+
+
 # create the shared library
 ADD_LIBRARY(${PLUGIN_NAME}
   SHARED
@@ -244,6 +289,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=========================
diff --git a/include/laser/capture/capture_laser_3d.h b/include/laser/capture/capture_laser_3d.h
index 5a8a4e38f1e453174cc32f35e820829435c3bd7c..a82c35e0d620a92b29cedd300908eb625df24f4c 100644
--- a/include/laser/capture/capture_laser_3d.h
+++ b/include/laser/capture/capture_laser_3d.h
@@ -18,14 +18,14 @@ typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;
 class CaptureLaser3d : public CaptureBase
 {
   public:
-    CaptureLaser3d(TimeStamp _timestamp, PointCloud::Ptr _point_cloud);
+    CaptureLaser3d(const TimeStamp& _timestamp, SensorBasePtr _sensor, PointCloud::Ptr _point_cloud);
     ~CaptureLaser3d();
     PointCloud::Ptr getPointCloud() const;
-    
+
     
   private:
     PointCloud::Ptr point_cloud_;
-}
+};
 
 }  // namespace laser
 }  // namespace wolf
diff --git a/include/laser/processor/processor_odom_icp_3d.h b/include/laser/processor/processor_odom_icp_3d.h
new file mode 100644
index 0000000000000000000000000000000000000000..d0eed4511121a98d4dfaf3588c43377242c373f8
--- /dev/null
+++ b/include/laser/processor/processor_odom_icp_3d.h
@@ -0,0 +1,134 @@
+
+#ifndef SRC_PROCESSOR_ODOM_ICP_3D_H_
+#define SRC_PROCESSOR_ODOM_ICP_3D_H_
+
+/**************************
+ *      WOLF includes     *
+ **************************/
+// #include "laser/processor/params_icp.h"   //vsainz: parameters not specified yet
+#include "laser/sensor/sensor_laser_3d.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/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>
+
+namespace wolf
+{
+
+WOLF_PTR_TYPEDEFS(ProcessorOdomIcp3d);
+WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorOdomIcp3d);
+
+struct ParamsProcessorOdomIcp3d : public ParamsProcessorTracker, public ParamsMotionProvider
+{
+};  // vsainz: empty, for now
+
+class ProcessorOdomIcp3d : public ProcessorTracker, public MotionProvider
+{
+  protected:
+    // Useful sensor stuff
+    SensorLaser3dPtr sensor_laser_;  // casted pointer to parent
+
+    // ICP algorithm
+    // wip
+
+    // temporary and carry-on transformations
+
+    Eigen::VectorXd odom_origin_;
+    Eigen::VectorXd odom_last_;
+    Eigen::VectorXd odom_incoming_;
+
+    Eigen::Isometry3d rl_T_sl_, ro_T_so_;
+
+  public:
+    ParamsProcessorOdomIcp3dPtr params_odom_icp_;
+
+  public:
+    ProcessorOdomIcp3d(ParamsProcessorOdomIcp3dPtr _params);
+    WOLF_PROCESSOR_CREATE(ProcessorOdomIcp3d, ParamsProcessorOdomIcp3d);
+    ~ProcessorOdomIcp3d() override;
+    void configure(SensorBasePtr _sensor) override;
+
+    VectorComposite getState(const StateStructure& _structure = "") const override;
+    TimeStamp       getTimeStamp() const override;
+    VectorComposite getState(const TimeStamp& _ts, const StateStructure& _structure = "") const override;
+    void            setProblem(ProblemPtr _problem_ptr) override;
+
+    Eigen::Vector3d getOriginLastTransform(double& dt) const;
+
+  protected:
+    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;
+    FeatureBasePtr emplaceFeature(CaptureBasePtr _capture_laser);
+    FactorBasePtr  emplaceFactor(FeatureBasePtr _feature);
+
+    bool voteForKeyFrameDistance() const;
+    bool voteForKeyFrameAngle() const;
+    bool voteForKeyFrameTime() const;
+    bool voteForKeyFrameMatchQuality() const;
+
+    template <typename D1, typename D2>
+    void convert2dTo3d(const Eigen::Isometry2d& T2, Eigen::MatrixBase<D1>& p3, Eigen::QuaternionBase<D2>& q3) const;
+
+    template <typename D1>
+    void              convert2dTo3d(const Eigen::Isometry2d& T2, Eigen::MatrixBase<D1>& x3) const;
+    Eigen::Isometry2d computeIsometry2d(const Eigen::VectorXd& x1, const Eigen::VectorXd& x2) const;
+
+    void updateExtrinsicsIsometries();
+};
+
+}  // namespace wolf
+
+
+#endif  // SRC_PROCESSOR_ODOM_ICP_3D_H_
diff --git a/include/laser/sensor/sensor_laser_3d.h b/include/laser/sensor/sensor_laser_3d.h
new file mode 100644
index 0000000000000000000000000000000000000000..496f5466b9a1283194f9bfefe0088061e370b8a3
--- /dev/null
+++ b/include/laser/sensor/sensor_laser_3d.h
@@ -0,0 +1,36 @@
+#ifndef SENSOR_LASER_3d_H_
+#define SENSOR_LASER_3d_H_
+
+// WOLF includes
+#include <core/sensor/sensor_base.h>
+
+
+namespace wolf
+{
+namespace laser
+{
+
+WOLF_STRUCT_PTR_TYPEDEFS(ParamsSensorLaser3d);
+
+struct ParamsSensorLaser3d : public ParamsSensorBase
+{};    // Not yet defined
+
+WOLF_PTR_TYPEDEFS(SensorLaser3d);
+
+
+class SensorLaser3d : public SensorBase
+{
+  public:
+    SensorLaser3d(StateBlockPtr p_ptr, StateBlockPtr _o_ptr);
+    ~SensorLaser3d();
+};
+
+
+
+WOLF_SENSOR_CREATE(SensorLaser3d, ParamsSensorLaser3d, 0);
+
+
+}  // namespace laser
+}  // namespace wolf
+
+#endif  // SENSOR_LASER_3d_H_
\ No newline at end of file
diff --git a/include/laser/utils/laser3d_tools.h b/include/laser/utils/laser3d_tools.h
new file mode 100644
index 0000000000000000000000000000000000000000..5c82a908f0b51c75a31ca297f04187ab5172a8dc
--- /dev/null
+++ b/include/laser/utils/laser3d_tools.h
@@ -0,0 +1,80 @@
+
+
+#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>
+
+
+using PointCloud = pcl::PointCloud<pcl::PointXYZ>;
+typedef boost::shared_ptr<PointCloud>       PointCloudBoostPtr;
+typedef boost::shared_ptr<const PointCloud> PointCloudBoostConstPtr;
+
+namespace wolf
+{
+namespace laser
+{
+
+// convenient typedefs
+
+// _cloud_ref: first PointCloud
+// _cloud_otherref: first PointCloud
+inline void pairAlign(const PointCloudBoostConstPtr                                      _cloud_ref,
+                      const PointCloudBoostConstPtr                                      _cloud_other,
+                      const Eigen::Isometry3d                                           &_transform_guess,
+                      Eigen::Isometry3d                                                 &_transform_final,
+                      pcl::IterativeClosestPointNonLinear<pcl::PointXYZ, pcl::PointXYZ> &_registration_solver,
+                      bool                                                               _downsample = false)
+{
+    // Internal PointCloud pointers (boost::shared_ptr)
+    PointCloudBoostPtr cloud_ref = boost::make_shared<PointCloud>();  // std::make_shared<PoinCloud> cloud_ref;
+    PointCloudBoostPtr cloud_other =
+        boost::make_shared<PointCloud>();  // std::make_shared<PoinCloud> cloud_other(new PointCloud);
+
+    // Downsample for consistency and speed
+    // \note enable this for large datasets
+    pcl::VoxelGrid<pcl::PointXYZ> grid;  // downsample for performance
+    if (_downsample)
+    {
+        grid.setLeafSize(0.05, 0.05, 0.05);
+        // grid.setInputCloud(boost::static_pointer_cast<PointCloud>(_cloud_ref));
+        grid.setInputCloud(_cloud_ref);
+        grid.filter(*cloud_ref);
+
+        grid.setInputCloud(_cloud_other);
+        grid.filter(*cloud_other);
+    }
+    else
+    {
+        *cloud_ref   = *_cloud_ref;
+        *cloud_other = *_cloud_other;
+    }
+
+    // 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
+    _registration_solver.align(*cloud_ref, transform_guess);
+    transform_pcl = _registration_solver.getFinalTransformation();
+
+    // Output the transformation as Isometry of double(s)
+    _transform_final = Eigen::Isometry3d(transform_pcl.cast<double>());
+
+    // _transform_final = _transform_final.inverse();   // Maybe we messed up
+}
+
+}  // namespace laser
+}  // namespace wolf
diff --git a/src/sensor/sensor_laser_3d.cpp b/src/sensor/sensor_laser_3d.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..e70bcd0b4785d421c9b9b4677a3ff6c25ce255bb
--- /dev/null
+++ b/src/sensor/sensor_laser_3d.cpp
@@ -0,0 +1,15 @@
+#include "laser/sensor/sensor_laser_3d.h"
+
+namespace wolf
+{
+namespace laser
+{
+
+SensorLaser3d::SensorLaser3d(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr) : 
+SensorBase("SensorLaser3d", _p_ptr, _o_ptr, nullptr, 0) {}
+//  vsainz: Parameters should be set somewhere, not yet!!
+
+SensorLaser3d::~SensorLaser3d() {}
+
+}  // namespace laser
+}  // namespace wolf
\ No newline at end of file
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index 1d3f41289deb112f091202b31669f289598b7832..c64c1d39019e0dde7e7ff9e6dac6e544c649f7b4 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -24,3 +24,7 @@ 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)
+endif(PCL_FOUND)
\ No newline at end of file
diff --git a/test/data/1.pcd b/test/data/1.pcd
new file mode 100644
index 0000000000000000000000000000000000000000..9b79b908a6430311c645e014e0b8174088601cdd
Binary files /dev/null and b/test/data/1.pcd differ
diff --git a/test/data/2.pcd b/test/data/2.pcd
new file mode 100644
index 0000000000000000000000000000000000000000..575b487f86c7234f2370fc4e3c210cdca9f732ad
Binary files /dev/null and b/test/data/2.pcd differ
diff --git a/test/gtest_laser3d_tools.cpp b/test/gtest_laser3d_tools.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..eb9c5f2d8792f16eebf1c27bffda629f433b9fdf
--- /dev/null
+++ b/test/gtest_laser3d_tools.cpp
@@ -0,0 +1,129 @@
+
+// wolf laser
+#include "laser/utils/laser3d_tools.h"
+#include "laser/internal/config.h"
+
+// //Wolf
+#include "core/common/wolf.h"
+// #include "core/math/rotations.h"
+#include "core/utils/utils_gtest.h"
+
+// Eigen
+#include <Eigen/Geometry>
+
+// //std
+#include <iostream>
+#include <string>
+#include <filesystem>
+#include <unistd.h>
+// #include <iostream>
+// #include <fstream>
+// #include <iomanip>
+
+//#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;
+
+
+
+void loadData(std::string fname, PointCloud& cloud)
+{
+    pcl::io::loadPCDFile(fname, cloud);
+    // remove NAN points from the cloud
+    pcl::Indices indices;
+    pcl::removeNaNFromPointCloud(cloud, cloud, indices);
+};
+
+TEST(laser3d_tools, pairAlign_identity)
+{
+    //    typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;
+    using PointCloud = pcl::PointCloud<pcl::PointXYZ>;
+
+    // Load data
+    PointCloudBoostPtr pcl_ref = boost::make_shared<PointCloud>();
+    loadData(laser_root_dir + "/test/data/1.pcd", *pcl_ref);
+
+    PointCloudBoostPtr pcl_other = pcl_ref;
+    // loadData("data/2.pcd", pcl_other);
+
+    // Guess
+    Eigen::Isometry3d transformation_guess = Eigen::Translation3d(1,1,1) * Eigen::AngleAxisd(0.1,Eigen::Vector3d(1,2,3).normalized());
+    WOLF_INFO("Initial guess transform: \n", transformation_guess.matrix());
+
+    // final transform
+    Eigen::Isometry3d transformation_final;
+
+    // Solver configuration
+    pcl::IterativeClosestPointNonLinear<pcl::PointXYZ, pcl::PointXYZ> registration_solver;
+    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);
+
+    // Alignment
+    wolf::laser::pairAlign(pcl_ref, pcl_other, transformation_guess, transformation_final, registration_solver, true); //false);
+
+    WOLF_INFO("Final transform: \n", transformation_final.matrix());
+
+    ASSERT_MATRIX_APPROX(transformation_final.matrix(), Eigen::Matrix4d::Identity(), 1e-2);
+}
+
+TEST(laser3d_tools, pairAlign_known_transformation)
+{
+    //    typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;
+    using PointCloud = pcl::PointCloud<pcl::PointXYZ>;
+
+    // Load data
+    PointCloudBoostPtr pcl_ref = boost::make_shared<PointCloud>();
+    loadData(laser_root_dir + "/test/data/1.pcd", *pcl_ref);
+
+    // known transform
+    Eigen::Affine3d transformation_known = Eigen::Affine3d::Identity();
+    transformation_known.translation() << 1.0, 0.5, 0.2;
+    transformation_known.rotate (Eigen::AngleAxisd (0.3, Eigen::Vector3d::UnitZ()));
+
+    // Move point cloud 
+    PointCloudBoostPtr pcl_other = boost::make_shared<PointCloud>();
+    pcl::transformPointCloud (*pcl_ref, *pcl_other, transformation_known);
+
+    // Guess
+    Eigen::Isometry3d transformation_guess = Eigen::Isometry3d::Identity();
+
+    // final transform
+    Eigen::Isometry3d transformation_final;
+    WOLF_INFO("Applied transformation: \n", transformation_known.matrix());
+    
+    // Solver configuration
+    pcl::IterativeClosestPointNonLinear<pcl::PointXYZ, pcl::PointXYZ> registration_solver;
+    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);
+
+    // Alignment
+    wolf::laser::pairAlign(pcl_ref, pcl_other, transformation_guess, transformation_final, registration_solver, true); //false);
+
+    WOLF_INFO("Final transform: \n", transformation_final.matrix());
+
+    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) = "Conversions.R2q_norm_of_q";
+    return RUN_ALL_TESTS();
+}