diff --git a/CMakeLists.txt b/CMakeLists.txt
index 6f4bc7d..e49d4df 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -1,5 +1,6 @@
 cmake_minimum_required(VERSION 2.8.3)
 project(rf2o_laser_odometry)
+set (CMAKE_CXX_STANDARD 14)  # Require C++14
 
 ## Find catkin macros and libraries
 ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
diff --git a/include/rf2o_laser_odometry/CLaserOdometry2D.h b/include/rf2o_laser_odometry/CLaserOdometry2D.h
index 68f0be7..200f148 100644
--- a/include/rf2o_laser_odometry/CLaserOdometry2D.h
+++ b/include/rf2o_laser_odometry/CLaserOdometry2D.h
@@ -26,15 +26,19 @@
 // MRPT related headers
 #include <mrpt/version.h>
 #if MRPT_VERSION>=0x130
-#	include <mrpt/obs/CObservation2DRangeScan.h>
+#   include <mrpt/obs/CObservation2DRangeScan.h>
 #   include <mrpt/obs/CObservationOdometry.h>
     using namespace mrpt::obs;
 #else
-#	include <mrpt/slam/CObservation2DRangeScan.h>
+#   include <mrpt/slam/CObservation2DRangeScan.h>
 #   include <mrpt/slam/CObservationOdometry.h>
     using namespace mrpt::slam;
 #endif
-#include <mrpt/system/threads.h>
+
+#if MRPT_VERSION<0x150
+#   include <mrpt/system/threads.h>
+#endif
+
 #include <mrpt/system/os.h>
 #include <mrpt/poses/CPose3D.h>
 #include <mrpt/utils.h>
diff --git a/src/CLaserOdometry2D.cpp b/src/CLaserOdometry2D.cpp
index 82e4901..a4803d9 100644
--- a/src/CLaserOdometry2D.cpp
+++ b/src/CLaserOdometry2D.cpp
@@ -144,8 +144,8 @@ void CLaserOdometry2D::Init()
 
 
     //Set the initial pose
-    laser_pose = CPose2D(robotInitialPose + LaserPoseOnTheRobot);
-    laser_oldpose = CPose2D(robotInitialPose + LaserPoseOnTheRobot);
+    laser_pose = robotInitialPose + LaserPoseOnTheRobot;
+    laser_oldpose = robotInitialPose + LaserPoseOnTheRobot;
 
 
     // Init module (internal)
@@ -943,7 +943,7 @@ void CLaserOdometry2D::PoseUpdate()
 	laser_oldpose = laser_pose;
 	math::CMatrixDouble33 aux_acu = acu_trans;
 	poses::CPose2D pose_aux_2D(acu_trans(0,2), acu_trans(1,2), kai_loc(2)/fps);
-    laser_pose = laser_pose + pose_aux_2D;
+        laser_pose = laser_pose + CPose3D(pose_aux_2D);