diff --git a/lsd_slam_core/CMakeLists.txt b/lsd_slam_core/CMakeLists.txt
index 7b4f8f22..95206de8 100644
--- a/lsd_slam_core/CMakeLists.txt
+++ b/lsd_slam_core/CMakeLists.txt
@@ -16,8 +16,11 @@ find_package(catkin REQUIRED COMPONENTS
   image_transport
   roscpp
   rosbag
+  cmake_modules
+  lsd_slam_msgs
 )
 
+find_package(OpenCV REQUIRED)
 find_package(Eigen REQUIRED)
 find_package(X11 REQUIRED)
 include(cmake/FindG2O.cmake)
@@ -42,7 +45,7 @@ generate_dynamic_reconfigure_options(
 catkin_package(
   LIBRARIES lsdslam
   DEPENDS Eigen SuiteSparse
-  CATKIN_DEPENDS libg2o 
+  CATKIN_DEPENDS libg2o
 )
 
 # SSE flags
@@ -87,28 +90,32 @@ set(SOURCE_FILES
 
 include_directories(
   include
-  ${EIGEN3_INCLUDE_DIR}
+  ${EIGEN_INCLUDE_DIR}
   ${PROJECT_SOURCE_DIR}/src
   ${PROJECT_SOURCE_DIR}/thirdparty/Sophus
   ${CSPARSE_INCLUDE_DIR} #Has been set by SuiteParse
   ${CHOLMOD_INCLUDE_DIR} #Has been set by SuiteParse
+  ${OpenCV_INCLUDE_DIRS}
+  ${catkin_INCLUDE_DIRS}
 )
 
 
 # build shared library.
 add_library(lsdslam SHARED ${SOURCE_FILES})
-target_link_libraries(lsdslam ${FABMAP_LIB} ${G2O_LIBRARIES} ${catkin_LIBRARIES} csparse cxsparse )
+add_dependencies(lsdslam ${catkin_EXPORTED_TARGETS})
+target_link_libraries(lsdslam ${FABMAP_LIB} ${G2O_LIBRARIES} ${catkin_LIBRARIES} cxsparse X11)
 #rosbuild_link_boost(lsdslam thread)
 
-
 # build live ros node
 add_executable(live_slam src/main_live_odometry.cpp)
-target_link_libraries(live_slam lsdslam ${catkin_LIBRARIES} ${G2O_LIBRARIES})
+add_dependencies(live_slam ${catkin_EXPORTED_TARGETS})
+target_link_libraries(live_slam lsdslam ${catkin_LIBRARIES} ${G2O_LIBRARIES} ${OpenCV_LIBRARIES})
 
 
 # build image node
 add_executable(dataset src/main_on_images.cpp)
-target_link_libraries(dataset lsdslam ${catkin_LIBRARIES} ${G2O_LIBRARIES})
+add_dependencies(dataset ${catkin_EXPORTED_TARGETS})
+target_link_libraries(dataset lsdslam ${catkin_LIBRARIES} ${G2O_LIBRARIES} ${OpenCV_LIBRARIES})
 
 # TODO add INSTALL
 
diff --git a/lsd_slam_core/package.xml b/lsd_slam_core/package.xml
index 4bc245b4..e10d44bb 100644
--- a/lsd_slam_core/package.xml
+++ b/lsd_slam_core/package.xml
@@ -16,20 +16,22 @@
   <build_depend>dynamic_reconfigure</build_depend>
   <build_depend>sensor_msgs</build_depend>
   <build_depend>roscpp</build_depend>
-  <build_depend>lsd_slam_viewer</build_depend>
+  <build_depend>lsd_slam_msgs</build_depend>
   <build_depend>rosbag</build_depend>
   <build_depend>eigen</build_depend>
   <build_depend>suitesparse</build_depend>
   <build_depend>libg2o</build_depend>
+  <build_depend>cmake_modules</build_depend>
 
   <run_depend>cv_bridge</run_depend>
   <run_depend>dynamic_reconfigure</run_depend>
   <run_depend>sensor_msgs</run_depend>
   <run_depend>roscpp</run_depend>
-  <run_depend>lsd_slam_viewer</run_depend>
+  <run_depend>lsd_slam_msgs</run_depend>
   <run_depend>rosbag</run_depend>
   <run_depend>eigen</run_depend>
   <run_depend>suitesparse</run_depend>
   <run_depend>libg2o</run_depend>
-  
+  <run_depend>cmake_modules</run_depend>
+
 </package> 
diff --git a/lsd_slam_core/src/DepthEstimation/DepthMap.cpp b/lsd_slam_core/src/DepthEstimation/DepthMap.cpp
index 831b8aae..c2d9f52c 100644
--- a/lsd_slam_core/src/DepthEstimation/DepthMap.cpp
+++ b/lsd_slam_core/src/DepthEstimation/DepthMap.cpp
@@ -1111,13 +1111,13 @@ void DepthMap::updateKeyframe(std::deque< std::shared_ptr<Frame> > referenceFram
 	{
 		cv::Mat keyFrameImage(activeKeyFrame->height(), activeKeyFrame->width(), CV_32F, const_cast<float*>(activeKeyFrameImageData));
 		keyFrameImage.convertTo(debugImageHypothesisHandling, CV_8UC1);
-		cv::cvtColor(debugImageHypothesisHandling, debugImageHypothesisHandling, CV_GRAY2RGB);
+		cv::cvtColor(debugImageHypothesisHandling, debugImageHypothesisHandling, cv::COLOR_GRAY2RGB);
 
 		cv::Mat oldest_refImage(oldest_referenceFrame->height(), oldest_referenceFrame->width(), CV_32F, const_cast<float*>(oldest_referenceFrame->image(0)));
 		cv::Mat newest_refImage(newest_referenceFrame->height(), newest_referenceFrame->width(), CV_32F, const_cast<float*>(newest_referenceFrame->image(0)));
 		cv::Mat rfimg = 0.5f*oldest_refImage + 0.5f*newest_refImage;
 		rfimg.convertTo(debugImageStereoLines, CV_8UC1);
-		cv::cvtColor(debugImageStereoLines, debugImageStereoLines, CV_GRAY2RGB);
+		cv::cvtColor(debugImageStereoLines, debugImageStereoLines, cv::COLOR_GRAY2RGB);
 	}
 
 	struct timeval tv_start, tv_end;
@@ -1238,7 +1238,7 @@ void DepthMap::createKeyFrame(Frame* new_keyframe)
 	{
 		cv::Mat keyFrameImage(new_keyframe->height(), new_keyframe->width(), CV_32F, const_cast<float*>(new_keyframe->image(0)));
 		keyFrameImage.convertTo(debugImageHypothesisPropagation, CV_8UC1);
-		cv::cvtColor(debugImageHypothesisPropagation, debugImageHypothesisPropagation, CV_GRAY2RGB);
+		cv::cvtColor(debugImageHypothesisPropagation, debugImageHypothesisPropagation, cv::COLOR_GRAY2RGB);
 	}
 
 
@@ -1403,7 +1403,7 @@ int DepthMap::debugPlotDepthMap()
 
 	cv::Mat keyFrameImage(activeKeyFrame->height(), activeKeyFrame->width(), CV_32F, const_cast<float*>(activeKeyFrameImageData));
 	keyFrameImage.convertTo(debugImageDepth, CV_8UC1);
-	cv::cvtColor(debugImageDepth, debugImageDepth, CV_GRAY2RGB);
+	cv::cvtColor(debugImageDepth, debugImageDepth, cv::COLOR_GRAY2RGB);
 
 	// debug plot & publish sparse version?
 	int refID = referenceFrameByID_offset;
diff --git a/lsd_slam_core/src/GlobalMapping/FabMap.cpp b/lsd_slam_core/src/GlobalMapping/FabMap.cpp
index 7e267f39..76f2eb62 100644
--- a/lsd_slam_core/src/GlobalMapping/FabMap.cpp
+++ b/lsd_slam_core/src/GlobalMapping/FabMap.cpp
@@ -129,7 +129,7 @@ void FabMap::compareAndAdd(Frame* keyframe, int* out_newID, int* out_loopID)
 	cv::Mat frame;
 	cv::Mat keyFrameImage(keyframe->height(), keyframe->width(), CV_32F, const_cast<float*>(keyframe->image()));
 	keyFrameImage.convertTo(frame, CV_8UC1);
-	//cv::cvtColor(frame, frame, CV_GRAY2RGB);
+	//cv::cvtColor(frame, frame, cv::COLOR_GRAY2RGB);
 	
 	// Generate FabMap bag-of-words data (image descriptor)
 	cv::Mat bow;
diff --git a/lsd_slam_core/src/GlobalMapping/KeyFrameGraph.cpp b/lsd_slam_core/src/GlobalMapping/KeyFrameGraph.cpp
index 093d81aa..1dd5805d 100644
--- a/lsd_slam_core/src/GlobalMapping/KeyFrameGraph.cpp
+++ b/lsd_slam_core/src/GlobalMapping/KeyFrameGraph.cpp
@@ -69,8 +69,10 @@ KeyFrameGraph::KeyFrameGraph()
 	typedef g2o::LinearSolverCSparse<BlockSolver::PoseMatrixType> LinearSolver;
 	//typedef g2o::LinearSolverPCG<BlockSolver::PoseMatrixType> LinearSolver;
 	LinearSolver* solver = new LinearSolver();
-	BlockSolver* blockSolver = new BlockSolver(solver);
-	g2o::OptimizationAlgorithmLevenberg* algorithm = new g2o::OptimizationAlgorithmLevenberg(blockSolver);
+//	BlockSolver* blockSolver = new BlockSolver(solver);
+    BlockSolver* blockSolver = new BlockSolver(std::unique_ptr<LinearSolver>(solver));
+//    g2o::OptimizationAlgorithmLevenberg* algorithm = new g2o::OptimizationAlgorithmLevenberg(blockSolver);
+    g2o::OptimizationAlgorithmLevenberg* algorithm = new g2o::OptimizationAlgorithmLevenberg(std::unique_ptr<BlockSolver>(blockSolver));
 	graph.setAlgorithm(algorithm);
 	
     graph.setVerbose(false); // printOptimizationInfo
diff --git a/lsd_slam_core/src/GlobalMapping/g2oTypeSim3Sophus.h b/lsd_slam_core/src/GlobalMapping/g2oTypeSim3Sophus.h
index af7690b8..02a28f47 100644
--- a/lsd_slam_core/src/GlobalMapping/g2oTypeSim3Sophus.h
+++ b/lsd_slam_core/src/GlobalMapping/g2oTypeSim3Sophus.h
@@ -93,7 +93,7 @@ class EdgeSim3 : public g2o::BaseBinaryEdge<7, Sophus::Sim3d, VertexSim3, Vertex
 	
 	virtual bool setMeasurementData(const double* m)
 	{
-		Eigen::Map<const g2o::Vector7d> v(m);
+		Eigen::Map<const Eigen::Matrix<double, 7 ,1> > v(m);
 		setMeasurement(Sophus::Sim3d::exp(v));
 		return true;
 	}
diff --git a/lsd_slam_core/src/IOWrapper/ImageDisplay.h b/lsd_slam_core/src/IOWrapper/ImageDisplay.h
index e8446450..c6482bfd 100644
--- a/lsd_slam_core/src/IOWrapper/ImageDisplay.h
+++ b/lsd_slam_core/src/IOWrapper/ImageDisplay.h
@@ -48,7 +48,7 @@ inline void displayImage(const char* windowName, const float* image, int width,
 	cv::Mat floatWrapper(height, width, CV_32F, const_cast<float*>(image));
 	cv::Mat tempImage(height, width, CV_8UC1);
 	floatWrapper.convertTo(tempImage, CV_8UC1);
-	cv::cvtColor(tempImage, tempImage, CV_GRAY2RGB);
+	cv::cvtColor(tempImage, tempImage, cv::COLOR_GRAY2RGB);
 	displayImage(windowName, tempImage);
 }
 
diff --git a/lsd_slam_core/src/IOWrapper/ROS/ROSOutput3DWrapper.cpp b/lsd_slam_core/src/IOWrapper/ROS/ROSOutput3DWrapper.cpp
index 78faa88c..c6c6bf2a 100644
--- a/lsd_slam_core/src/IOWrapper/ROS/ROSOutput3DWrapper.cpp
+++ b/lsd_slam_core/src/IOWrapper/ROS/ROSOutput3DWrapper.cpp
@@ -25,8 +25,8 @@
 
 
 #include "std_msgs/Float32MultiArray.h"
-#include "lsd_slam_viewer/keyframeGraphMsg.h"
-#include "lsd_slam_viewer/keyframeMsg.h"
+#include "lsd_slam_msgs/keyframeGraphMsg.h"
+#include "lsd_slam_msgs/keyframeMsg.h"
 
 #include "DataStructures/Frame.h"
 #include "GlobalMapping/KeyFrameGraph.h"
@@ -44,13 +44,13 @@ ROSOutput3DWrapper::ROSOutput3DWrapper(int width, int height)
 	this->height = height;
 
 	liveframe_channel = nh_.resolveName("lsd_slam/liveframes");
-	liveframe_publisher = nh_.advertise<lsd_slam_viewer::keyframeMsg>(liveframe_channel,1);
+	liveframe_publisher = nh_.advertise<lsd_slam_msgs::keyframeMsg>(liveframe_channel,1);
 
 	keyframe_channel = nh_.resolveName("lsd_slam/keyframes");
-	keyframe_publisher = nh_.advertise<lsd_slam_viewer::keyframeMsg>(keyframe_channel,1);
+	keyframe_publisher = nh_.advertise<lsd_slam_msgs::keyframeMsg>(keyframe_channel,1);
 
 	graph_channel = nh_.resolveName("lsd_slam/graph");
-	graph_publisher = nh_.advertise<lsd_slam_viewer::keyframeGraphMsg>(graph_channel,1);
+	graph_publisher = nh_.advertise<lsd_slam_msgs::keyframeGraphMsg>(graph_channel,1);
 
 	debugInfo_channel = nh_.resolveName("lsd_slam/debug");
 	debugInfo_publisher = nh_.advertise<std_msgs::Float32MultiArray>(debugInfo_channel,1);
@@ -69,7 +69,7 @@ ROSOutput3DWrapper::~ROSOutput3DWrapper()
 
 void ROSOutput3DWrapper::publishKeyframe(Frame* f)
 {
-	lsd_slam_viewer::keyframeMsg fMsg;
+	lsd_slam_msgs::keyframeMsg fMsg;
 
 
 	boost::shared_lock<boost::shared_mutex> lock = f->getActiveLock();
@@ -113,7 +113,7 @@ void ROSOutput3DWrapper::publishKeyframe(Frame* f)
 
 void ROSOutput3DWrapper::publishTrackedFrame(Frame* kf)
 {
-	lsd_slam_viewer::keyframeMsg fMsg;
+	lsd_slam_msgs::keyframeMsg fMsg;
 
 
 	fMsg.id = kf->id();
@@ -163,7 +163,7 @@ void ROSOutput3DWrapper::publishTrackedFrame(Frame* kf)
 
 void ROSOutput3DWrapper::publishKeyframeGraph(KeyFrameGraph* graph)
 {
-	lsd_slam_viewer::keyframeGraphMsg gMsg;
+	lsd_slam_msgs::keyframeGraphMsg gMsg;
 
 	graph->edgesListsMutex.lock();
 	gMsg.numConstraints = graph->edgesAll.size();
diff --git a/lsd_slam_core/src/LiveSLAMWrapper.cpp b/lsd_slam_core/src/LiveSLAMWrapper.cpp
index 45423d62..1f6b3c1d 100644
--- a/lsd_slam_core/src/LiveSLAMWrapper.cpp
+++ b/lsd_slam_core/src/LiveSLAMWrapper.cpp
@@ -118,7 +118,7 @@ void LiveSLAMWrapper::newImageCallback(const cv::Mat& img, Timestamp imgTime)
 	if (img.channels() == 1)
 		grayImg = img;
 	else
-		cvtColor(img, grayImg, CV_RGB2GRAY);
+		cvtColor(img, grayImg, cv::COLOR_RGB2GRAY);
 	
 
 	// Assert that we work with 8 bit images
diff --git a/lsd_slam_core/src/main_on_images.cpp b/lsd_slam_core/src/main_on_images.cpp
index 1aa06861..ee5b70d9 100644
--- a/lsd_slam_core/src/main_on_images.cpp
+++ b/lsd_slam_core/src/main_on_images.cpp
@@ -221,7 +221,7 @@ int main( int argc, char** argv )
 
 	for(unsigned int i=0;i<files.size();i++)
 	{
-		cv::Mat imageDist = cv::imread(files[i], CV_LOAD_IMAGE_GRAYSCALE);
+		cv::Mat imageDist = cv::imread(files[i], cv::IMREAD_GRAYSCALE);
 
 		if(imageDist.rows != h_inp || imageDist.cols != w_inp)
 		{
diff --git a/lsd_slam_core/src/util/globalFuncs.cpp b/lsd_slam_core/src/util/globalFuncs.cpp
index c02ca083..dae31659 100644
--- a/lsd_slam_core/src/util/globalFuncs.cpp
+++ b/lsd_slam_core/src/util/globalFuncs.cpp
@@ -49,11 +49,11 @@ void printMessageOnCVImage(cv::Mat &image, std::string line1,std::string line2)
 		for(int y=image.rows-30; y<image.rows;y++)
 			image.at<cv::Vec3b>(y,x) *= 0.5;
 
-	cv::putText(image, line2, cvPoint(10,image.rows-5),
-	    CV_FONT_HERSHEY_SIMPLEX, 0.4, cv::Scalar(200,200,250), 1, 8);
+	cv::putText(image, line2, cv::Point(10,image.rows-5),
+	    cv::FONT_HERSHEY_SIMPLEX, 0.4, cv::Scalar(200,200,250), 1, 8);
 
-	cv::putText(image, line1, cvPoint(10,image.rows-18),
-	    CV_FONT_HERSHEY_SIMPLEX, 0.4, cv::Scalar(200,200,250), 1, 8);
+	cv::putText(image, line1, cv::Point(10,image.rows-18),
+	    cv::FONT_HERSHEY_SIMPLEX, 0.4, cv::Scalar(200,200,250), 1, 8);
 }
 
 
@@ -71,7 +71,7 @@ cv::Mat getDepthRainbowPlot(const float* idepth, const float* idepthVar, const f
 		cv::Mat keyFrameImage(height, width, CV_32F, const_cast<float*>(gray));
 		cv::Mat keyFrameImage8u;
 		keyFrameImage.convertTo(keyFrameImage8u, CV_8UC1);
-		cv::cvtColor(keyFrameImage8u, res, CV_GRAY2RGB);
+		cv::cvtColor(keyFrameImage8u, res, cv::COLOR_GRAY2RGB);
 	}
 	else
 		fillCvMat(&res,cv::Vec3b(255,170,168));
@@ -136,7 +136,7 @@ cv::Mat getVarRedGreenPlot(const float* idepthVar, const float* gray, int width,
 		cv::Mat keyFrameImage(height, width, CV_32F, const_cast<float*>(gray));
 		cv::Mat keyFrameImage8u;
 		keyFrameImage.convertTo(keyFrameImage8u, CV_8UC1);
-		cv::cvtColor(keyFrameImage8u, res, CV_GRAY2RGB);
+		cv::cvtColor(keyFrameImage8u, res, cv::COLOR_GRAY2RGB);
 	}
 	else
 		fillCvMat(&res,cv::Vec3b(255,170,168));
diff --git a/lsd_slam_core/thirdparty/openFabMap/samples/openFABMAPcli.cpp b/lsd_slam_core/thirdparty/openFabMap/samples/openFABMAPcli.cpp
index 1b31e9c7..b0421823 100644
--- a/lsd_slam_core/thirdparty/openFabMap/samples/openFABMAPcli.cpp
+++ b/lsd_slam_core/thirdparty/openFabMap/samples/openFABMAPcli.cpp
@@ -884,7 +884,7 @@ void drawRichKeypoints(const cv::Mat& src, std::vector<cv::KeyPoint>& kpts, cv::
 	
 	cv::Mat grayFrame;
 	cvtColor(src, grayFrame, CV_RGB2GRAY);
-	cvtColor(grayFrame, dst, CV_GRAY2RGB);
+	cvtColor(grayFrame, dst, cv::COLOR_GRAY2RGB);
 	
 	if (kpts.size() == 0) {
 		return;
diff --git a/lsd_slam_msgs/CMakeLists.txt b/lsd_slam_msgs/CMakeLists.txt
new file mode 100644
index 00000000..a3741c77
--- /dev/null
+++ b/lsd_slam_msgs/CMakeLists.txt
@@ -0,0 +1,216 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(lsd_slam_msgs)
+
+## Compile as C++11, supported in ROS Kinetic and newer
+# add_compile_options(-std=c++11)
+
+## 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
+  rospy
+  std_msgs
+  message_generation
+)
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+
+
+## 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
+# )
+add_message_files(
+  FILES
+  keyframeMsg.msg
+  keyframeGraphMsg.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
+#   std_msgs
+# )
+generate_messages(
+  DEPENDENCIES
+  std_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/DynReconf1.cfg
+#   cfg/DynReconf2.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
+catkin_package(
+#  INCLUDE_DIRS include
+#  LIBRARIES lsd_slam_msgs
+ CATKIN_DEPENDS message_runtime std_msgs
+#  DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories(
+# include
+  ${catkin_INCLUDE_DIRS}
+)
+
+## Declare a C++ library
+# add_library(${PROJECT_NAME}
+#   src/${PROJECT_NAME}/lsd_slam_msgs.cpp
+# )
+
+## 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} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## 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
+# add_executable(${PROJECT_NAME}_node src/lsd_slam_msgs_node.cpp)
+
+## 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_node"
+# set_target_properties(${PROJECT_NAME}_node 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}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Specify libraries to link a library or executable target against
+# target_link_libraries(${PROJECT_NAME}_node
+#   ${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
+# catkin_install_python(PROGRAMS
+#   scripts/my_python_script
+#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables for installation
+## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
+# install(TARGETS ${PROJECT_NAME}_node
+#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark libraries for installation
+## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
+# install(TARGETS ${PROJECT_NAME}
+#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   RUNTIME DESTINATION ${CATKIN_GLOBAL_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_lsd_slam_msgs.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)
diff --git a/lsd_slam_msgs/msg/keyframeGraphMsg.msg b/lsd_slam_msgs/msg/keyframeGraphMsg.msg
new file mode 100644
index 00000000..b355e342
--- /dev/null
+++ b/lsd_slam_msgs/msg/keyframeGraphMsg.msg
@@ -0,0 +1,8 @@
+# data as serialization of sim(3)'s: (int id, float[7] camToWorld)
+uint32 numFrames
+uint8[] frameData
+
+
+# constraints (int from, int to, float err)
+uint32 numConstraints
+uint8[] constraintsData
diff --git a/lsd_slam_msgs/msg/keyframeMsg.msg b/lsd_slam_msgs/msg/keyframeMsg.msg
new file mode 100644
index 00000000..004a1665
--- /dev/null
+++ b/lsd_slam_msgs/msg/keyframeMsg.msg
@@ -0,0 +1,22 @@
+int32 id
+float64 time
+bool isKeyframe
+
+# camToWorld as serialization of sophus sim(3).
+# may change with keyframeGraph-updates.
+float32[7] camToWorld 
+
+
+# camera parameter (fx fy cx cy), width, height
+# will never change, but required for display.
+float32 fx
+float32 fy
+float32 cx
+float32 cy
+uint32 height
+uint32 width
+
+
+# data as InputPointDense (float idepth, float idepth_var, uchar color[4]), width x height
+# may be empty, in that case no associated pointcloud is ever shown.
+uint8[] pointcloud
diff --git a/lsd_slam_msgs/package.xml b/lsd_slam_msgs/package.xml
new file mode 100644
index 00000000..153c3c85
--- /dev/null
+++ b/lsd_slam_msgs/package.xml
@@ -0,0 +1,69 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>lsd_slam_msgs</name>
+  <version>0.0.0</version>
+  <description>The lsd_slam_msgs package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag -->
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="tekniker@todo.todo">tekniker</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>TODO</license>
+
+
+  <!-- Url tags are optional, but multiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <!-- <url type="website">http://wiki.ros.org/lsd_slam_msgs</url> -->
+
+
+  <!-- Author tags are optional, multiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintainers, but could be -->
+  <!-- Example: -->
+  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+  <!-- The *depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
+  <!--   <depend>roscpp</depend> -->
+  <!--   Note that this is equivalent to the following: -->
+  <!--   <build_depend>roscpp</build_depend> -->
+  <!--   <exec_depend>roscpp</exec_depend> -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use build_export_depend for packages you need in order to build against this package: -->
+  <!--   <build_export_depend>message_generation</build_export_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use exec_depend for packages you need at runtime: -->
+  <!--   <exec_depend>message_runtime</exec_depend> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <!-- Use doc_depend for packages you need only for building documentation: -->
+  <!--   <doc_depend>doxygen</doc_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>std_msgs</build_depend>
+  <build_depend>message_generation</build_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>rospy</build_depend>
+  <build_export_depend>std_msgs</build_export_depend>
+  <exec_depend>std_msgs</exec_depend>
+  <exec_depend>message_generation</exec_depend>
+  <exec_depend>message_runtime</exec_depend>
+  <exec_depend>roscpp</exec_depend>
+  <exec_depend>rospy</exec_depend>
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>
diff --git a/lsd_slam_viewer/CMakeLists.txt b/lsd_slam_viewer/CMakeLists.txt
index f0c2512b..5baaf524 100644
--- a/lsd_slam_viewer/CMakeLists.txt
+++ b/lsd_slam_viewer/CMakeLists.txt
@@ -19,14 +19,20 @@ find_package(catkin REQUIRED COMPONENTS
   sensor_msgs
   roscpp
   rosbag
-  message_generation
+#  message_generation
   roslib
+  cmake_modules
+  lsd_slam_msgs
 )
 
 find_package(OpenGL REQUIRED)
 set(QT_USE_QTOPENGL TRUE)
 set(QT_USE_QTXML TRUE)
 find_package(QGLViewer REQUIRED)
+message(STATUS "QGLViewer version: ${QGLViewer_VERSION}")
+message(STATUS "QGLViewer include directories: ${QGLViewer_INCLUDE_DIRS}")
+message(STATUS "QGLViewer libraries: ${QGLViewer_LIBRARIES}")
+set(QGLVIEWER_LIBRARY /usr/lib/x86_64-linux-gnu/libQGLViewer-qt4.so)
 find_package(Eigen REQUIRED)
 find_package(OpenCV REQUIRED)
 find_package(Boost REQUIRED COMPONENTS thread)
@@ -41,8 +47,8 @@ set(CMAKE_CXX_FLAGS
    "${CMAKE_CXX_FLAGS} -march=native -Wall -std=c++0x"
 )
 
-add_message_files(DIRECTORY msg FILES keyframeMsg.msg keyframeGraphMsg.msg)
-generate_messages(DEPENDENCIES)
+#add_message_files(DIRECTORY msg FILES keyframeMsg.msg keyframeGraphMsg.msg)
+#generate_messages(DEPENDENCIES)
 
 generate_dynamic_reconfigure_options(
   cfg/LSDSLAMViewerParams.cfg
@@ -69,6 +75,7 @@ include_directories(
 )  
 
 add_executable(viewer src/main_viewer.cpp ${SOURCE_FILES} ${HEADER_FILES})
+add_dependencies(viewer lsd_slam_viewer_generate_messages_cpp)
 target_link_libraries(viewer ${QGLViewer_LIBRARIES}
 			     ${QGLVIEWER_LIBRARY} 
 			     ${catkin_LIBRARIES}
@@ -84,4 +91,4 @@ target_link_libraries(viewer ${QGLViewer_LIBRARIES}
 #			     ${QT_LIBRARIES}
 #			     GL glut GLU
 #		      )
-
+#add_dependencies(videoStitch lsd_slam_viewer_generate_messages_cpp)
diff --git a/lsd_slam_viewer/package.xml b/lsd_slam_viewer/package.xml
index b411459c..b62058e1 100644
--- a/lsd_slam_viewer/package.xml
+++ b/lsd_slam_viewer/package.xml
@@ -19,6 +19,9 @@
   <build_depend>roslib</build_depend>
   <build_depend>rosbag</build_depend>
   <build_depend>message_generation</build_depend>
+  <build_depend>cmake_modules</build_depend>
+  <build_depend>lsd_slam_msgs</build_depend>
+<!--  <build_depend>libqglviewer-qt4-dev</build_depend>-->
 
   <run_depend>cv_bridge</run_depend>
   <run_depend>dynamic_reconfigure</run_depend>
@@ -26,6 +29,9 @@
   <run_depend>roscpp</run_depend>
   <run_depend>roslib</run_depend>
   <run_depend>rosbag</run_depend>
+  <run_depend>cmake_modules</run_depend>
+  <run_depend>lsd_slam_msgs</run_depend>
+<!--  <run_depend>libqglviewer-qt4</run_depend>-->
 
 </package>
 
diff --git a/lsd_slam_viewer/src/KeyFrameDisplay.cpp b/lsd_slam_viewer/src/KeyFrameDisplay.cpp
index 0ba66a2a..64650e4b 100644
--- a/lsd_slam_viewer/src/KeyFrameDisplay.cpp
+++ b/lsd_slam_viewer/src/KeyFrameDisplay.cpp
@@ -20,6 +20,8 @@
 
 #define GL_GLEXT_PROTOTYPES 1
 
+#include "opencv2/opencv.hpp"
+
 #include "KeyFrameDisplay.h"
 #include <stdio.h>
 #include "settings.h"
@@ -28,8 +30,6 @@
 #include <GL/gl.h>
 #include <GL/glu.h>
 
-#include "opencv2/opencv.hpp"
-
 #include "ros/package.h"
 
 KeyFrameDisplay::KeyFrameDisplay()
@@ -62,7 +62,7 @@ KeyFrameDisplay::~KeyFrameDisplay()
 }
 
 
-void KeyFrameDisplay::setFrom(lsd_slam_viewer::keyframeMsgConstPtr msg)
+void KeyFrameDisplay::setFrom(lsd_slam_msgs::keyframeMsgConstPtr msg)
 {
 	// copy over campose.
 	memcpy(camToWorld.data(), msg->camToWorld.data(), 7*sizeof(float));
diff --git a/lsd_slam_viewer/src/KeyFrameDisplay.h b/lsd_slam_viewer/src/KeyFrameDisplay.h
index af7cd93b..a9261a50 100644
--- a/lsd_slam_viewer/src/KeyFrameDisplay.h
+++ b/lsd_slam_viewer/src/KeyFrameDisplay.h
@@ -24,7 +24,7 @@
 #include <Eigen/Core>
 
 #include "QGLViewer/qglviewer.h"
-#include "lsd_slam_viewer/keyframeMsg.h"
+#include "lsd_slam_msgs/keyframeMsg.h"
 #include "sophus/sim3.hpp"
 
 #include <sstream>
@@ -54,7 +54,7 @@ class KeyFrameDisplay
 	~KeyFrameDisplay();
 
 
-	void setFrom(lsd_slam_viewer::keyframeMsgConstPtr msg);
+	void setFrom(lsd_slam_msgs::keyframeMsgConstPtr msg);
 	void drawCam(float lineWidth = 1, float* color = 0);
 	void drawPC(float pointSize = 1, float alpha = 1);
 	void refreshPC();
diff --git a/lsd_slam_viewer/src/KeyFrameGraphDisplay.cpp b/lsd_slam_viewer/src/KeyFrameGraphDisplay.cpp
index 6fce35f6..fc430f93 100644
--- a/lsd_slam_viewer/src/KeyFrameGraphDisplay.cpp
+++ b/lsd_slam_viewer/src/KeyFrameGraphDisplay.cpp
@@ -139,7 +139,7 @@ void KeyFrameGraphDisplay::draw()
 	dataMutex.unlock();
 }
 
-void KeyFrameGraphDisplay::addMsg(lsd_slam_viewer::keyframeMsgConstPtr msg)
+void KeyFrameGraphDisplay::addMsg(lsd_slam_msgs::keyframeMsgConstPtr msg)
 {
 	dataMutex.lock();
 	if(keyframesByID.count(msg->id) == 0)
@@ -155,7 +155,7 @@ void KeyFrameGraphDisplay::addMsg(lsd_slam_viewer::keyframeMsgConstPtr msg)
 	dataMutex.unlock();
 }
 
-void KeyFrameGraphDisplay::addGraphMsg(lsd_slam_viewer::keyframeGraphMsgConstPtr msg)
+void KeyFrameGraphDisplay::addGraphMsg(lsd_slam_msgs::keyframeGraphMsgConstPtr msg)
 {
 	dataMutex.lock();
 
diff --git a/lsd_slam_viewer/src/KeyFrameGraphDisplay.h b/lsd_slam_viewer/src/KeyFrameGraphDisplay.h
index 7872f528..c7b664d1 100644
--- a/lsd_slam_viewer/src/KeyFrameGraphDisplay.h
+++ b/lsd_slam_viewer/src/KeyFrameGraphDisplay.h
@@ -24,8 +24,8 @@
 #define KEYFRAMEGRAPHDISPLAY_H_
 
 
-#include "lsd_slam_viewer/keyframeGraphMsg.h"
-#include "lsd_slam_viewer/keyframeMsg.h"
+#include "lsd_slam_msgs/keyframeGraphMsg.h"
+#include "lsd_slam_msgs/keyframeMsg.h"
 #include "boost/thread.hpp"
 
 class KeyFrameDisplay;
@@ -60,8 +60,8 @@ class KeyFrameGraphDisplay {
 
 	void draw();
 
-	void addMsg(lsd_slam_viewer::keyframeMsgConstPtr msg);
-	void addGraphMsg(lsd_slam_viewer::keyframeGraphMsgConstPtr msg);
+	void addMsg(lsd_slam_msgs::keyframeMsgConstPtr msg);
+	void addGraphMsg(lsd_slam_msgs::keyframeGraphMsgConstPtr msg);
 
 
 
diff --git a/lsd_slam_viewer/src/PointCloudViewer.cpp b/lsd_slam_viewer/src/PointCloudViewer.cpp
index bfa6ab3b..a6ad2d83 100644
--- a/lsd_slam_viewer/src/PointCloudViewer.cpp
+++ b/lsd_slam_viewer/src/PointCloudViewer.cpp
@@ -97,7 +97,7 @@ void PointCloudViewer::reset()
 
 	resetRequested=false;
 
-	save_folder = ros::package::getPath("lsd_slam_viewer")+"/save/";
+	save_folder = ros::package::getPath("lsd_slam_msgs")+"/save/";
 	localMsBetweenSaves = 1;
 	simMsBetweenSaves = 1;
 	lastCamID = -1;
@@ -118,7 +118,7 @@ void PointCloudViewer::reset()
 	animationPlaybackEnabled = false;
 }
 
-void PointCloudViewer::addFrameMsg(lsd_slam_viewer::keyframeMsgConstPtr msg)
+void PointCloudViewer::addFrameMsg(lsd_slam_msgs::keyframeMsgConstPtr msg)
 {
 	meddleMutex.lock();
 
@@ -139,7 +139,7 @@ void PointCloudViewer::addFrameMsg(lsd_slam_viewer::keyframeMsgConstPtr msg)
 	meddleMutex.unlock();
 }
 
-void PointCloudViewer::addGraphMsg(lsd_slam_viewer::keyframeGraphMsgConstPtr msg)
+void PointCloudViewer::addGraphMsg(lsd_slam_msgs::keyframeGraphMsgConstPtr msg)
 {
 	meddleMutex.lock();
 
@@ -323,7 +323,7 @@ void PointCloudViewer::keyPressEvent(QKeyEvent *e)
     	  meddleMutex.lock();
 
 
-    	  float x,y,z;
+    	  qreal x,y,z;
     	  camera()->frame()->getPosition(x,y,z);
     	  animationList.push_back(AnimationObject(false, lastAnimTime, 2, qglviewer::Frame(qglviewer::Vec(0,0,0), camera()->frame()->orientation())));
     	  animationList.back().frame.setPosition(x,y,z);
diff --git a/lsd_slam_viewer/src/PointCloudViewer.h b/lsd_slam_viewer/src/PointCloudViewer.h
index ddb943f5..ac009b2b 100644
--- a/lsd_slam_viewer/src/PointCloudViewer.h
+++ b/lsd_slam_viewer/src/PointCloudViewer.h
@@ -27,8 +27,8 @@
 #include <vector>
 #include "boost/thread.hpp"
 #include "qevent.h"
-#include "lsd_slam_viewer/keyframeMsg.h"
-#include "lsd_slam_viewer/keyframeGraphMsg.h"
+#include "lsd_slam_msgs/keyframeMsg.h"
+#include "lsd_slam_msgs/keyframeGraphMsg.h"
 
 #include "QGLViewer/keyFrameInterpolator.h"
 
@@ -132,7 +132,7 @@ class AnimationObject
 		int showCurrentCam_i = showCurrentCam;
 		int isFix_i = isFix;
 
-		float x,y,z;
+		qreal x,y,z;
 		frame.getPosition(x,y,z);
 
     	snprintf(buf, 1000, "Animation: %d at %lf (dur %lf) S: %f %f %d %d %d %d %d Frame: %lf %lf %lf %lf %f %f %f %d",
@@ -157,8 +157,8 @@ class PointCloudViewer : public QGLViewer
 
 	void reset();
 
-	void addFrameMsg(lsd_slam_viewer::keyframeMsgConstPtr msg);
-	void addGraphMsg(lsd_slam_viewer::keyframeGraphMsgConstPtr msg);
+	void addFrameMsg(lsd_slam_msgs::keyframeMsgConstPtr msg);
+	void addGraphMsg(lsd_slam_msgs::keyframeGraphMsgConstPtr msg);
 
 
 protected :
diff --git a/lsd_slam_viewer/src/main_viewer.cpp b/lsd_slam_viewer/src/main_viewer.cpp
index 9d0ceff6..65076922 100644
--- a/lsd_slam_viewer/src/main_viewer.cpp
+++ b/lsd_slam_viewer/src/main_viewer.cpp
@@ -29,8 +29,8 @@
 #include <qapplication.h>
 
 
-#include "lsd_slam_viewer/keyframeGraphMsg.h"
-#include "lsd_slam_viewer/keyframeMsg.h"
+#include "lsd_slam_msgs/keyframeGraphMsg.h"
+#include "lsd_slam_msgs/keyframeMsg.h"
 
 
 #include "boost/foreach.hpp"
@@ -66,7 +66,7 @@ void dynConfCb(lsd_slam_viewer::LSDSLAMViewerParamsConfig &config, uint32_t leve
 
 }
 
-void frameCb(lsd_slam_viewer::keyframeMsgConstPtr msg)
+void frameCb(lsd_slam_msgs::keyframeMsgConstPtr msg)
 {
 
 	if(msg->time > lastFrameTime) return;
@@ -74,7 +74,7 @@ void frameCb(lsd_slam_viewer::keyframeMsgConstPtr msg)
 	if(viewer != 0)
 		viewer->addFrameMsg(msg);
 }
-void graphCb(lsd_slam_viewer::keyframeGraphMsgConstPtr msg)
+void graphCb(lsd_slam_msgs::keyframeGraphMsgConstPtr msg)
 {
 	if(viewer != 0)
 		viewer->addGraphMsg(msg);
@@ -132,11 +132,11 @@ void rosFileLoop( int argc, char** argv )
 	 {
 
 		 if(m.getTopic() == "/lsd_slam/liveframes" || m.getTopic() == "/lsd_slam/keyframes")
-			 frameCb(m.instantiate<lsd_slam_viewer::keyframeMsg>());
+			 frameCb(m.instantiate<lsd_slam_msgs::keyframeMsg>());
 
 
 		 if(m.getTopic() == "/lsd_slam/graph")
-			 graphCb(m.instantiate<lsd_slam_viewer::keyframeGraphMsg>());
+			 graphCb(m.instantiate<lsd_slam_msgs::keyframeGraphMsg>());
 	 }
 
 	ros::spin();