diff --git a/README.md b/README.md
index bcb58469..12cc7331 100644
--- a/README.md
+++ b/README.md
@@ -89,8 +89,18 @@ Compile the two package by typing:
     rosmake lsd_slam
 
 
+Build  using **Catkin** tool from **catkin** branch:
 
+	sudo apt-get install ros-indigo-libg2o ros-indigo-cv-bridge liblapack-dev libblas-dev freeglut3-dev libqglviewer-dev libsuitesparse-dev ros-indigo-cmake-modules
+	mkdir -p ~/catkin_ws/src/
+	cd ~/catkin_ws/src/
+	git clone -b catkin https://github.com/tum-vision/lsd_slam.git
+	cd ~/catkin_ws
 
+Build order a bit strange as core package depends on one header from viewer package:
+
+	catkin_make --pkg lsd_slam_viewer
+	catkin_make --pkg lsd_slam_core
 
 
 ## 2.3 openFabMap for large loop-closure detection [optional]
diff --git a/lsd_slam_core/CMakeLists.txt b/lsd_slam_core/CMakeLists.txt
index 7b4f8f22..a225b0ac 100644
--- a/lsd_slam_core/CMakeLists.txt
+++ b/lsd_slam_core/CMakeLists.txt
@@ -10,6 +10,8 @@ project(lsd_slam_core)
 set(CMAKE_BUILD_TYPE Release)
 
 find_package(catkin REQUIRED COMPONENTS
+  cmake_modules
+  #lsd_slam_viewer
   cv_bridge
   dynamic_reconfigure
   sensor_msgs
@@ -18,21 +20,46 @@ find_package(catkin REQUIRED COMPONENTS
   rosbag
 )
 
+find_package(cmake_modules REQUIRED)
 find_package(Eigen REQUIRED)
 find_package(X11 REQUIRED)
 include(cmake/FindG2O.cmake)
 include(cmake/FindSuiteParse.cmake)
 
+#find csparse
+#FIND_PACKAGE(SuiteSparse)
+IF(CSPARSE_FOUND)
+MESSAGE(STATUS "CSPARSE FOUND")
+ELSE(CSPARSE_FOUND)
+MESSAGE(STATUS "CSPARSE NOT FOUND")
+ENDIF(CSPARSE_FOUND)
+
+#########################################################
+# FIND X11
+#########################################################
+find_package(X11)
+if(NOT X11_FOUND)
+	message(FATAL_ERROR "Failed to find X11")
+endif(NOT X11_FOUND)
+      
 message("-- CHOLMOD_INCLUDE_DIR : " ${CHOLMOD_INCLUDE_DIR})
 message("-- CSPARSE_INCLUDE_DIR : " ${CSPARSE_INCLUDE_DIR})
 message("-- G2O_INCLUDE_DIR : " ${G2O_INCLUDE_DIR})
+message("-- EIGEN3_INCLUDE_DIR : " ${EIGEN3_INCLUDE_DIR})
+message("-- EIGEN_INCLUDE_DIR : " ${EIGEN_INCLUDE_DIR})
+message("-- X11_LIBRARIES : " ${X11_LIBRARIES})
 
 # FabMap
 # uncomment this part to enable fabmap
-#add_subdirectory(${PROJECT_SOURCE_DIR}/thirdparty/openFabMap)
-#include_directories(${PROJECT_SOURCE_DIR}/thirdparty/openFabMap/include)
-#add_definitions("-DHAVE_FABMAP")
-#set(FABMAP_LIB openFABMAP )
+if(WITH_FABMAP)
+  message("WITH_FABMAP: " ${WITH_FABMAP})
+  add_subdirectory(${PROJECT_SOURCE_DIR}/thirdparty/openFabMap)
+  include_directories(${PROJECT_SOURCE_DIR}/thirdparty/openFabMap/include)
+  add_definitions("-DHAVE_FABMAP")
+  set(FABMAP_LIB openFABMAP )
+else()
+  message("WITH_FABMAP: " ${WITH_FABMAP})
+endif(WITH_FABMAP)
 
 generate_dynamic_reconfigure_options(
   cfg/LSDDebugParams.cfg
@@ -42,7 +69,7 @@ generate_dynamic_reconfigure_options(
 catkin_package(
   LIBRARIES lsdslam
   DEPENDS Eigen SuiteSparse
-  CATKIN_DEPENDS libg2o 
+  CATKIN_DEPENDS libg2o #lsd_slam_viewer
 )
 
 # SSE flags
@@ -51,8 +78,8 @@ add_definitions("-DENABLE_SSE")
 
 # Also add some useful compiler flag
 set(CMAKE_CXX_FLAGS
-   "${CMAKE_CXX_FLAGS} -march=native -Wall -std=c++0x"
-) 
+  "${CMAKE_CXX_FLAGS} -march=native -Wall -std=c++0x"
+)
 
 # Set source files
 set(lsd_SOURCE_FILES
@@ -87,7 +114,9 @@ set(SOURCE_FILES
 
 include_directories(
   include
+  ${EIGEN_INCLUDE_DIR}
   ${EIGEN3_INCLUDE_DIR}
+  ${X11_INCLUDE_DIR}
   ${PROJECT_SOURCE_DIR}/src
   ${PROJECT_SOURCE_DIR}/thirdparty/Sophus
   ${CSPARSE_INCLUDE_DIR} #Has been set by SuiteParse
@@ -97,7 +126,8 @@ include_directories(
 
 # build shared library.
 add_library(lsdslam SHARED ${SOURCE_FILES})
-target_link_libraries(lsdslam ${FABMAP_LIB} ${G2O_LIBRARIES} ${catkin_LIBRARIES} csparse cxsparse )
+target_link_libraries(lsdslam ${FABMAP_LIB} ${G2O_LIBRARIES} ${catkin_LIBRARIES} ${CSPARSE_LIBRARY} ${X11_LIBRARIES}) 
+#csparse cxsparse )
 #rosbuild_link_boost(lsdslam thread)
 
 
diff --git a/lsd_slam_core/package.xml b/lsd_slam_core/package.xml
index 4bc245b4..01b13e8f 100644
--- a/lsd_slam_core/package.xml
+++ b/lsd_slam_core/package.xml
@@ -12,6 +12,7 @@
   <url>http://vision.in.tum.de/lsdslam</url>
   
   <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>cmake_modules</build_depend>
   <build_depend>cv_bridge</build_depend>
   <build_depend>dynamic_reconfigure</build_depend>
   <build_depend>sensor_msgs</build_depend>
@@ -22,6 +23,7 @@
   <build_depend>suitesparse</build_depend>
   <build_depend>libg2o</build_depend>
 
+  <run_depend>cmake_modules</run_depend>
   <run_depend>cv_bridge</run_depend>
   <run_depend>dynamic_reconfigure</run_depend>
   <run_depend>sensor_msgs</run_depend>
diff --git a/lsd_slam_core/src/DataStructures/Frame.h b/lsd_slam_core/src/DataStructures/Frame.h
index 1ff2f530..7dcc294b 100644
--- a/lsd_slam_core/src/DataStructures/Frame.h
+++ b/lsd_slam_core/src/DataStructures/Frame.h
@@ -29,6 +29,11 @@
 #include "util/settings.h"
 
 
+#ifndef isnanf
+	#include <cmath>
+	#define isnanf (float)std::isnan
+#endif
+
 namespace lsd_slam
 {
 
@@ -161,7 +166,7 @@ class Frame
 
 	/** Multi-Map indicating for which other keyframes with which initialization tracking failed.*/
 	std::unordered_multimap< Frame*, Sim3, std::hash<Frame*>, std::equal_to<Frame*>,
-		Eigen::aligned_allocator< std::pair<const Frame*,Sim3> > > trackingFailed;
+		Eigen::aligned_allocator< std::pair<Frame* const,Sim3 > > > trackingFailed;
 
 
 	// flag set when depth is updated.
diff --git a/lsd_slam_core/src/DepthEstimation/DepthMap.h b/lsd_slam_core/src/DepthEstimation/DepthMap.h
index 10f2daba..90afc3e4 100644
--- a/lsd_slam_core/src/DepthEstimation/DepthMap.h
+++ b/lsd_slam_core/src/DepthEstimation/DepthMap.h
@@ -25,7 +25,10 @@
 #include "util/IndexThreadReduce.h"
 #include "util/SophusUtil.h"
 
-
+#ifndef isnanf
+	#include <cmath>
+	#define isnanf (float)std::isnan
+#endif
 
 namespace lsd_slam
 {
diff --git a/lsd_slam_core/src/GlobalMapping/KeyFrameGraph.h b/lsd_slam_core/src/GlobalMapping/KeyFrameGraph.h
index 15d1ea21..f55cf79c 100644
--- a/lsd_slam_core/src/GlobalMapping/KeyFrameGraph.h
+++ b/lsd_slam_core/src/GlobalMapping/KeyFrameGraph.h
@@ -180,7 +180,7 @@ friend class IntegrationTest;
 	g2o::SparseOptimizer graph;
 	
 	std::vector< Frame*, Eigen::aligned_allocator<Frame*> > newKeyframesBuffer;
-	std::vector< KFConstraintStruct*, Eigen::aligned_allocator<FramePoseStruct*> > newEdgeBuffer;
+	std::vector< KFConstraintStruct* > newEdgeBuffer;
 
 
 	int nextEdgeId;
diff --git a/lsd_slam_core/src/IOWrapper/Timestamp.cpp b/lsd_slam_core/src/IOWrapper/Timestamp.cpp
index 4812dd45..18dd5ca7 100644
--- a/lsd_slam_core/src/IOWrapper/Timestamp.cpp
+++ b/lsd_slam_core/src/IOWrapper/Timestamp.cpp
@@ -29,7 +29,7 @@ namespace lsd_slam
 {
 
 
-const std::chrono::monotonic_clock::time_point Timestamp::startupTimePoint = std::chrono::monotonic_clock::now();
+const std::chrono::steady_clock::time_point Timestamp::startupTimePoint = std::chrono::steady_clock::now();
 boost::mutex Timestamp::localtimeMutex;
 
 Timestamp::Timestamp()
@@ -71,7 +71,7 @@ double Timestamp::secondsUntil(const Timestamp& other) const
 Timestamp Timestamp::now()
 {
 	Timestamp result;
-	result.timePoint = std::chrono::monotonic_clock::now();
+	result.timePoint = std::chrono::steady_clock::now();
 	result.systemTimePoint = std::chrono::system_clock::now();
 	return result;
 }
diff --git a/lsd_slam_core/src/IOWrapper/Timestamp.h b/lsd_slam_core/src/IOWrapper/Timestamp.h
index bc59d0fc..6437b950 100644
--- a/lsd_slam_core/src/IOWrapper/Timestamp.h
+++ b/lsd_slam_core/src/IOWrapper/Timestamp.h
@@ -75,10 +75,10 @@ class Timestamp
 	static Timestamp now();
 	
 private:
-	std::chrono::monotonic_clock::time_point timePoint;
+	std::chrono::steady_clock::time_point timePoint;
 	std::chrono::system_clock::time_point systemTimePoint;
 	
-	static const std::chrono::monotonic_clock::time_point startupTimePoint;
+	static const std::chrono::steady_clock::time_point startupTimePoint;
 	static boost::mutex localtimeMutex;
 
 	double externalStamp;
diff --git a/lsd_slam_viewer/CMakeLists.txt b/lsd_slam_viewer/CMakeLists.txt
index f0c2512b..334a5ff3 100644
--- a/lsd_slam_viewer/CMakeLists.txt
+++ b/lsd_slam_viewer/CMakeLists.txt
@@ -9,11 +9,12 @@ project(lsd_slam_viewer)
 #  MinSizeRel : w/o debug symbols, w/ optimization, stripped binaries
 set(CMAKE_BUILD_TYPE Release)
 
-ADD_SUBDIRECTORY(${PROJECT_SOURCE_DIR}/thirdparty/Sophus)
+#ADD_SUBDIRECTORY(${PROJECT_SOURCE_DIR}/thirdparty/Sophus)
 
 set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} ${PROJECT_SOURCE_DIR}/cmake)
 
 find_package(catkin REQUIRED COMPONENTS
+  cmake_modules
   cv_bridge
   dynamic_reconfigure
   sensor_msgs
@@ -23,6 +24,7 @@ find_package(catkin REQUIRED COMPONENTS
   roslib
 )
 
+find_package(cmake_modules REQUIRED)
 find_package(OpenGL REQUIRED)
 set(QT_USE_QTOPENGL TRUE)
 set(QT_USE_QTXML TRUE)
@@ -31,16 +33,64 @@ find_package(Eigen REQUIRED)
 find_package(OpenCV REQUIRED)
 find_package(Boost REQUIRED COMPONENTS thread)
 
+#########################################################
+# FIND GLUT
+#########################################################
+find_package(GLUT REQUIRED)
+include_directories(${GLUT_INCLUDE_DIRS})
+link_directories(${GLUT_LIBRARY_DIRS})
+add_definitions(${GLUT_DEFINITIONS})
+if(NOT GLUT_FOUND)
+    message(ERROR " GLUT not found!")
+endif(NOT GLUT_FOUND)
+
+#########################################################
+# FIND OPENGL
+#########################################################
+find_package(OpenGL REQUIRED)
+include_directories(${OpenGL_INCLUDE_DIRS})
+link_directories(${OpenGL_LIBRARY_DIRS})
+add_definitions(${OpenGL_DEFINITIONS})
+if(NOT OPENGL_FOUND)
+    message(ERROR " OPENGL not found!")
+endif(NOT OPENGL_FOUND)
+
+#########################################################
+# FIND X11
+#########################################################
+find_package(X11)
+if(NOT X11_FOUND)
+	message(FATAL_ERROR "Failed to find X11")
+endif(NOT X11_FOUND)
+
+
+if (${CMAKE_SYSTEM_NAME} MATCHES "Darwin")
+	include_directories(/usr/X11R6/include/)
+	link_directories(/usr/X11R6/lib)
+	SET(EXTRA_LIBS GL X11 GLU glut)
+	#target_link_libraries(main ${EXTRA_LIBS})
+#else()
+#	SET(EXTRA_LIBS GL glut GLU)
+endif()
+
+
+message("-- EIGEN3_INCLUDE_DIR : " ${EIGEN3_INCLUDE_DIR})
+message("-- EIGEN_INCLUDE_DIR : " ${EIGEN_INCLUDE_DIR})
+message("-- X11_LIBRARIES : " ${X11_LIBRARIES})
+
 include_directories(${QGLVIEWER_INCLUDE_DIR}
 		    ${catkin_INCLUDE_DIRS} 
 		    ${EIGEN_INCLUDE_DIR}
 		    ${QT_INCLUDES} )
 
+
+
 # SSE flags
 set(CMAKE_CXX_FLAGS
-   "${CMAKE_CXX_FLAGS} -march=native -Wall -std=c++0x"
+  "${CMAKE_CXX_FLAGS} -march=native -Wall -std=c++0x"
 )
 
+
 add_message_files(DIRECTORY msg FILES keyframeMsg.msg keyframeGraphMsg.msg)
 generate_messages(DEPENDENCIES)
 
@@ -48,6 +98,26 @@ generate_dynamic_reconfigure_options(
   cfg/LSDSLAMViewerParams.cfg
 )
 
+catkin_package(
+#INCLUDE_DIRS include
+LIBRARIES ${PROJECT_NAME}
+CATKIN_DEPENDS
+  cmake_modules
+  cv_bridge
+  dynamic_reconfigure
+  sensor_msgs
+  roscpp
+  rosbag
+  roslib
+DEPENDS
+  cmake_modules
+  cv_bridge
+  dynamic_reconfigure
+  sensor_msgs
+  roscpp
+  rosbag
+  roslib
+)
 
 # Sources files
 set(SOURCE_FILES         
@@ -64,6 +134,7 @@ set(HEADER_FILES
   src/settings.h
 )
 
+
 include_directories(
   ${PROJECT_SOURCE_DIR}/thirdparty/Sophus
 )  
@@ -74,8 +145,12 @@ target_link_libraries(viewer ${QGLViewer_LIBRARIES}
 			     ${catkin_LIBRARIES}
 			     ${Boost_LIBRARIES}
 			     ${QT_LIBRARIES}
-			     GL glut GLU
-		     )
+			     ${X11_LIBRARIES}
+			     ${EXTRA_LIBS}
+			     ${OPENGL_LIBRARIES}
+			     ${GLUT_LIBRARY}
+)
+add_dependencies(viewer lsd_slam_viewer_gencpp)
 
 #add_executable(videoStitch src/main_stitchVideos.cpp)
 #target_link_libraries(viewer ${QGLViewer_LIBRARIES}
diff --git a/lsd_slam_viewer/package.xml b/lsd_slam_viewer/package.xml
index b411459c..f86c79c3 100644
--- a/lsd_slam_viewer/package.xml
+++ b/lsd_slam_viewer/package.xml
@@ -12,6 +12,7 @@
   <url>http://vision.in.tum.de/lsdslam</url>
 
   <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>cmake_modules</build_depend>
   <build_depend>cv_bridge</build_depend>
   <build_depend>dynamic_reconfigure</build_depend>
   <build_depend>sensor_msgs</build_depend>
@@ -20,6 +21,7 @@
   <build_depend>rosbag</build_depend>
   <build_depend>message_generation</build_depend>
 
+  <run_depend>cmake_modules</run_depend>
   <run_depend>cv_bridge</run_depend>
   <run_depend>dynamic_reconfigure</run_depend>
   <run_depend>sensor_msgs</run_depend>
diff --git a/lsd_slam_viewer/src/KeyFrameDisplay.cpp b/lsd_slam_viewer/src/KeyFrameDisplay.cpp
index 0ba66a2a..6111b500 100644
--- a/lsd_slam_viewer/src/KeyFrameDisplay.cpp
+++ b/lsd_slam_viewer/src/KeyFrameDisplay.cpp
@@ -24,9 +24,15 @@
 #include <stdio.h>
 #include "settings.h"
 
-#include <GL/glx.h>
-#include <GL/gl.h>
-#include <GL/glu.h>
+#ifdef __MACH__
+	//#include <GL/glx.h>
+	#include <GL/gl.h>
+	#include <GL/glu.h>
+#else
+	#include <GL/glx.h>
+	#include <GL/gl.h>
+	#include <GL/glu.h>
+#endif
 
 #include "opencv2/opencv.hpp"
 
diff --git a/lsd_slam_viewer/src/PointCloudViewer.cpp b/lsd_slam_viewer/src/PointCloudViewer.cpp
index bfa6ab3b..6840bbc2 100644
--- a/lsd_slam_viewer/src/PointCloudViewer.cpp
+++ b/lsd_slam_viewer/src/PointCloudViewer.cpp
@@ -29,10 +29,15 @@
 #include <zlib.h>
 #include <iostream>
 
-
-#include <GL/glx.h>
-#include <GL/gl.h>
-#include <GL/glu.h>
+#ifdef __MACH__
+	//#include <GL/glx.h>
+	#include <GL/gl.h>
+	#include <GL/glu.h>
+#else
+	#include <GL/glx.h>
+	#include <GL/gl.h>
+	#include <GL/glu.h>
+#endif
 
 #include "QGLViewer/manipulatedCameraFrame.h"
 
diff --git a/lsd_slam_viewer/src/main_viewer.cpp b/lsd_slam_viewer/src/main_viewer.cpp
index 9d0ceff6..627e2013 100644
--- a/lsd_slam_viewer/src/main_viewer.cpp
+++ b/lsd_slam_viewer/src/main_viewer.cpp
@@ -55,9 +55,14 @@ void dynConfCb(lsd_slam_viewer::LSDSLAMViewerParamsConfig &config, uint32_t leve
 	showCurrentCamera = config.showCurrentCamera;
 	showCurrentPointcloud = config.showCurrentPointcloud;
 
-
+#ifdef __MACH__
+	scaledDepthVarTH = __exp10( config.scaledDepthVarTH );
+	absDepthVarTH = __exp10( config.absDepthVarTH );
+#else
 	scaledDepthVarTH = exp10( config.scaledDepthVarTH );
-	absDepthVarTH = exp10( config.absDepthVarTH );
+	absDepthVarTH = exp10( config.absDepthVarTH );	
+#endif
+	
 	minNearSupport = config.minNearSupport;
 	sparsifyFactor = config.sparsifyFactor;
 	cutFirstNKf = config.cutFirstNKf;