diff --git a/CMakeLists.txt b/CMakeLists.txt
index 24eb11f6..1171f470 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -159,7 +159,7 @@ SET(rtabmap_ros_lib_src
    src/nodelets/disparity_to_depth.cpp 
    src/nodelets/obstacles_detection.cpp
    src/nodelets/point_cloud_aggregator.cpp
-   src/nodelets/OdometryROS.cpp
+   src/OdometryROS.cpp
    src/MsgConversion.cpp
    src/MapsManager.cpp
 )   
diff --git a/src/CoreWrapper.h b/include/rtabmap_ros/CoreWrapper.h
similarity index 100%
rename from src/CoreWrapper.h
rename to include/rtabmap_ros/CoreWrapper.h
diff --git a/src/GuiWrapper.h b/include/rtabmap_ros/GuiWrapper.h
similarity index 100%
rename from src/GuiWrapper.h
rename to include/rtabmap_ros/GuiWrapper.h
diff --git a/src/MapsManager.h b/include/rtabmap_ros/MapsManager.h
similarity index 100%
rename from src/MapsManager.h
rename to include/rtabmap_ros/MapsManager.h
diff --git a/src/nodelets/OdometryROS.h b/include/rtabmap_ros/OdometryROS.h
similarity index 100%
rename from src/nodelets/OdometryROS.h
rename to include/rtabmap_ros/OdometryROS.h
diff --git a/src/PreferencesDialogROS.h b/include/rtabmap_ros/PreferencesDialogROS.h
similarity index 100%
rename from src/PreferencesDialogROS.h
rename to include/rtabmap_ros/PreferencesDialogROS.h
diff --git a/src/CoreNode.cpp b/src/CoreNode.cpp
index 4429c125..df608f12 100644
--- a/src/CoreNode.cpp
+++ b/src/CoreNode.cpp
@@ -25,7 +25,7 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
 
-#include "CoreWrapper.h"
+#include "rtabmap_ros/CoreWrapper.h"
 #include <rtabmap/utilite/ULogger.h>
 #include <rtabmap/utilite/UDirectory.h>
 #include <rtabmap/utilite/UStl.h>
diff --git a/src/CoreWrapper.cpp b/src/CoreWrapper.cpp
index 45793c79..00d2b739 100644
--- a/src/CoreWrapper.cpp
+++ b/src/CoreWrapper.cpp
@@ -25,7 +25,7 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
 
-#include "CoreWrapper.h"
+#include "rtabmap_ros/CoreWrapper.h"
 
 #include <stdio.h>
 #include <ros/ros.h>
diff --git a/src/GuiNode.cpp b/src/GuiNode.cpp
index 4190ca3a..6fd74d2f 100644
--- a/src/GuiNode.cpp
+++ b/src/GuiNode.cpp
@@ -25,7 +25,7 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
 
-#include "GuiWrapper.h"
+#include "rtabmap_ros/GuiWrapper.h"
 #include "rtabmap/utilite/ULogger.h"
 
 #include <QApplication>
diff --git a/src/GuiWrapper.cpp b/src/GuiWrapper.cpp
index 5682ae71..0b8c1db2 100644
--- a/src/GuiWrapper.cpp
+++ b/src/GuiWrapper.cpp
@@ -25,7 +25,7 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
 
-#include "GuiWrapper.h"
+#include "rtabmap_ros/GuiWrapper.h"
 #include <QApplication>
 #include <QDir>
 
@@ -54,8 +54,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap_ros/GetMap.h"
 #include "rtabmap_ros/SetGoal.h"
 #include "rtabmap_ros/SetLabel.h"
-
-#include "PreferencesDialogROS.h"
+#include "rtabmap_ros/PreferencesDialogROS.h"
 
 #include <pcl_ros/transforms.h>
 #include <pcl_conversions/pcl_conversions.h>
diff --git a/src/MapAssemblerNode.cpp b/src/MapAssemblerNode.cpp
index a8055d53..8e7c295d 100644
--- a/src/MapAssemblerNode.cpp
+++ b/src/MapAssemblerNode.cpp
@@ -28,7 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <ros/ros.h>
 #include "rtabmap_ros/MapData.h"
 #include "rtabmap_ros/MsgConversion.h"
-#include "MapsManager.h"
+#include "rtabmap_ros/MapsManager.h"
 #include <rtabmap/core/util3d_transforms.h>
 #include <rtabmap/core/util3d.h>
 #include <rtabmap/core/util3d_filtering.h>
diff --git a/src/MapsManager.cpp b/src/MapsManager.cpp
index c6f2012b..7b1e2b9c 100644
--- a/src/MapsManager.cpp
+++ b/src/MapsManager.cpp
@@ -25,7 +25,7 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
 
-#include "MapsManager.h"
+#include "rtabmap_ros/MapsManager.h"
 
 #include <rtabmap/utilite/ULogger.h>
 #include <rtabmap/utilite/UTimer.h>
diff --git a/src/nodelets/OdometryROS.cpp b/src/OdometryROS.cpp
similarity index 98%
rename from src/nodelets/OdometryROS.cpp
rename to src/OdometryROS.cpp
index 317265db..cd5e0eba 100644
--- a/src/nodelets/OdometryROS.cpp
+++ b/src/OdometryROS.cpp
@@ -25,7 +25,7 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
 
-#include "OdometryROS.h"
+#include "rtabmap_ros/OdometryROS.h"
 
 #include <sensor_msgs/Image.h>
 #include <sensor_msgs/image_encodings.h>
@@ -426,7 +426,7 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp)
 		}
 
 		// local map / reference frame
-		if(odomLocalMap_.getNumSubscribers() && dynamic_cast<OdometryF2M*>(odometry_))
+		if(odomLocalMap_.getNumSubscribers() && odometry_->isF2M())
 		{
 			pcl::PointCloud<pcl::PointXYZ> cloud;
 			const std::multimap<int, cv::Point3f> & map = ((OdometryF2M*)odometry_)->getMap().getWords3();
@@ -443,7 +443,8 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp)
 
 		if(odomLastFrame_.getNumSubscribers())
 		{
-			if(dynamic_cast<OdometryF2M*>(odometry_))
+			// check which type of Odometry is using
+			if(odometry_->isF2M()) // If it's Frame to Map Odometry
 			{
 				const std::multimap<int, cv::Point3f> & words3  = ((OdometryF2M*)odometry_)->getLastFrame().getWords3();
 				if(words3.size())
@@ -463,10 +464,10 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp)
 					odomLastFrame_.publish(cloudMsg);
 				}
 			}
-			else
+			else if(odometry_->isF2F()) // if Using Frame to Frame Odometry
 			{
-				//Frame to Frame
 				const Signature & refFrame = ((OdometryF2F*)odometry_)->getRefFrame();
+
 				if(refFrame.getWords3().size())
 				{
 					pcl::PointCloud<pcl::PointXYZ> cloud;
@@ -482,6 +483,8 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp)
 					cloudMsg.header.frame_id = odomFrameId_;
 					odomLastFrame_.publish(cloudMsg);
 				}
+			}else{
+				NODELET_ERROR("ERROR, Wrong Type of Odometry, Shouldn't happen");
 			}
 		}
 	}
@@ -549,7 +552,7 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp)
 
 bool OdometryROS::isOdometryF2M() const
 {
-	return dynamic_cast<OdometryF2M*>(odometry_) != 0;
+	return odometry_->isF2M();
 }
 
 bool OdometryROS::reset(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
diff --git a/src/PreferencesDialogROS.cpp b/src/PreferencesDialogROS.cpp
index dbd7957f..d93e7152 100644
--- a/src/PreferencesDialogROS.cpp
+++ b/src/PreferencesDialogROS.cpp
@@ -25,7 +25,7 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
 
-#include "PreferencesDialogROS.h"
+#include "rtabmap_ros/PreferencesDialogROS.h"
 #include <rtabmap/core/Parameters.h>
 #include <QDir>
 #include <QFileInfo>
diff --git a/src/nodelets/rgbd_odometry.cpp b/src/nodelets/rgbd_odometry.cpp
index b0f480cf..8492b018 100644
--- a/src/nodelets/rgbd_odometry.cpp
+++ b/src/nodelets/rgbd_odometry.cpp
@@ -25,7 +25,7 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
 
-#include "OdometryROS.h"
+#include <rtabmap_ros/OdometryROS.h>
 
 #include <pluginlib/class_list_macros.h>
 #include <nodelet/nodelet.h>
@@ -208,7 +208,7 @@ private:
 				return;
 			}
 
-			ros::Time stamp = image->header.stamp>depth->header.stamp?image->header.stamp:depth->header.stamp;
+			ros::Time stamp = image->header.stamp > depth->header.stamp? image->header.stamp : depth->header.stamp;
 
 			Transform localTransform = getTransform(this->frameId(), image->header.frame_id, stamp);
 			if(localTransform.isNull())
diff --git a/src/nodelets/stereo_odometry.cpp b/src/nodelets/stereo_odometry.cpp
index de1a1657..3c8bca95 100644
--- a/src/nodelets/stereo_odometry.cpp
+++ b/src/nodelets/stereo_odometry.cpp
@@ -25,7 +25,7 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
 
-#include "OdometryROS.h"
+#include "rtabmap_ros/OdometryROS.h"
 #include "pluginlib/class_list_macros.h"
 #include "nodelet/nodelet.h"