diff --git a/CMakeLists.txt b/CMakeLists.txt
index f8d3a5b5..62467787 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -32,8 +32,6 @@ SET(PROJECT_VERSION_PATCH ${RTABMAP_PATCH_VERSION})
 
 SET(PROJECT_SOVERSION "${PROJECT_VERSION_MAJOR}.${PROJECT_VERSION_MINOR}")
 
-SET(RTABMAP_QT_VERSION 4 CACHE STRING "Which QT version to use")
-
 ####### COMPILATION PARAMS #######
 # In case of Makefiles if the user does not setup CMAKE_BUILD_TYPE, assume it's Release:
 IF(${CMAKE_GENERATOR} MATCHES ".*Makefiles")
@@ -143,6 +141,11 @@ FIND_PACKAGE(OpenCV REQUIRED QUIET)
 FIND_PACKAGE(PCL 1.7 REQUIRED QUIET)
 FIND_PACKAGE(ZLIB REQUIRED QUIET)
 
+# fix libproj.so not found on Xenial
+if(NOT "${PCL_LIBRARIES}" STREQUAL "")
+   list(REMOVE_ITEM PCL_LIBRARIES "vtkproj4")
+endif()
+
 # OpenMP ("-fopenmp" should be added for flann included in PCL)
 # the gcc-4.2.1 coming with MacOS X is not compatible with the OpenMP pragmas we use, so disabling OpenMP for it
 if((NOT APPLE) OR (NOT CMAKE_COMPILER_IS_GNUCXX) OR (GCC_VERSION VERSION_GREATER 4.2.1) OR (CMAKE_CXX_COMPILER_ID STREQUAL "Clang"))
@@ -167,12 +170,13 @@ IF(ZLIB_FOUND)
 ENDIF(ZLIB_FOUND)
 
 IF(WITH_QT)
-# If Qt is here, the GUI will be built
-    IF("${RTABMAP_QT_VERSION}" STREQUAL "4")
+    # If Qt is here, the GUI will be built
+    # look for Qt5 before Qt4
+    FIND_PACKAGE(Qt5 COMPONENTS Widgets Core Gui Svg)
+    IF(NOT Qt5_FOUND)
         FIND_PACKAGE(Qt4 COMPONENTS QtCore QtGui QtSvg)
-    ELSE()
-        FIND_PACKAGE(Qt5 COMPONENTS Widgets Core Gui Svg)
-    ENDIF()
+    ENDIF(NOT Qt5_FOUND)
+        
     IF(QT4_FOUND OR Qt5_FOUND)
         FIND_PACKAGE(VTK REQUIRED)
         IF("${VTK_MAJOR_VERSION}" EQUAL 5)
@@ -592,7 +596,7 @@ MESSAGE(STATUS "  With Qt5                  = YES (License: Open Source or Comme
 ELSEIF(NOT WITH_QT)
 MESSAGE(STATUS "  With Qt                   = NO (WITH_QT=OFF)")
 ELSE()
-MESSAGE(STATUS "  With Qt                   = NO (Qt not found, to use Qt5 you should set -DRTABMAP_QT_VERSION=5)")
+MESSAGE(STATUS "  With Qt                   = NO (Qt not found)")
 ENDIF()
 
 MESSAGE(STATUS "--------------------------------------------")
diff --git a/app/src/CMakeLists.txt b/app/src/CMakeLists.txt
index e921a753..fcdce8f8 100644
--- a/app/src/CMakeLists.txt
+++ b/app/src/CMakeLists.txt
@@ -5,7 +5,7 @@ SET(headers_ui
 )
 
 #This will generate moc_* for Qt
-IF("${RTABMAP_QT_VERSION}" STREQUAL "4")
+IF(QT4_FOUND)
     QT4_WRAP_CPP(moc_srcs ${headers_ui})
 ELSE()
     QT5_WRAP_CPP(moc_srcs ${headers_ui})
@@ -25,9 +25,9 @@ SET(INCLUDE_DIRS
     ${PCL_INCLUDE_DIRS}
 )
 
-IF("${RTABMAP_QT_VERSION}" STREQUAL "4")
+IF(QT4_FOUND)
     INCLUDE(${QT_USE_FILE})
-ENDIF()
+ENDIF(QT4_FOUND)
 
 SET(LIBRARIES
     ${QT_LIBRARIES} 
@@ -73,9 +73,9 @@ ELSE()
   ADD_EXECUTABLE(rtabmap ${SRC_FILES})
 ENDIF()
 TARGET_LINK_LIBRARIES(rtabmap rtabmap_core rtabmap_gui rtabmap_utilite ${LIBRARIES})
-IF("${RTABMAP_QT_VERSION}" STREQUAL "5")
+IF(Qt5_FOUND)
     QT5_USE_MODULES(rtabmap Widgets Core Gui Svg PrintSupport)
-ENDIF()
+ENDIF(Qt5_FOUND)
 
 IF(APPLE AND BUILD_AS_BUNDLE)
   SET_TARGET_PROPERTIES(rtabmap PROPERTIES
@@ -166,7 +166,7 @@ IF((APPLE AND BUILD_AS_BUNDLE) OR WIN32)
   # over.
   # To find dependencies, cmake use "otool" on Apple and "dumpbin" on Windows (make sure you have one of them).
   install(CODE "
-   file(GLOB_RECURSE QTPLUGINS \"\$ENV{DESTDIR}\${CMAKE_INSTALL_PREFIX}/${plugin_dest_dir}/plugins/*${CMAKE_SHARED_LIBRARY_SUFFIX}\")
+   file(GLOB_RECURSE QTPLUGINS \"\$ENV{DESTDIR}\${CMAKE_INSTALL_PREFIX}/${plugin_dest_dir}/plugins/*${CMAKE_SHARED_LIBRARY_SUFFIX}\")
     set(BU_CHMOD_BUNDLE_ITEMS ON)
     include(\"BundleUtilities\")
     fixup_bundle(\"${APPS}\" \"\${QTPLUGINS}\" \"${DIRS}\")
diff --git a/examples/NoEventsExample/CMakeLists.txt b/examples/NoEventsExample/CMakeLists.txt
index 60df1003..9ffbba5f 100644
--- a/examples/NoEventsExample/CMakeLists.txt
+++ b/examples/NoEventsExample/CMakeLists.txt
@@ -16,7 +16,6 @@ if(internal)
 		rtabmap_gui
 		rtabmap_utilite
 	)  
-	SET(MY_QT_VERSION ${RTABMAP_QT_VERSION})
 else()
 	# external build
 	PROJECT( MyProject )
@@ -25,15 +24,11 @@ else()
 	FIND_PACKAGE(OpenCV REQUIRED)
 	FIND_PACKAGE(PCL 1.7 REQUIRED)
 	
-	# Use Qt4 or Qt5
-	SET(MY_QT_VERSION 4 CACHE STRING "Which Qt version to use")
-	IF("${MY_QT_VERSION}" STREQUAL "4")
-		message("4")
-	    FIND_PACKAGE(Qt4 COMPONENTS QtCore QtGui QtSvg)
-	ELSE()
-		message("5")
-	    FIND_PACKAGE(Qt5 COMPONENTS Widgets Core Gui Svg)
-	ENDIF()
+	# Find Qt5 first
+	FIND_PACKAGE(Qt5 COMPONENTS Widgets Core Gui Svg)
+    	IF(NOT Qt5_FOUND)
+       		FIND_PACKAGE(Qt4 COMPONENTS QtCore QtGui QtSvg)
+    	ENDIF(NOT Qt5_FOUND)
 endif()
 
 SET(INCLUDE_DIRS
@@ -42,9 +37,9 @@ SET(INCLUDE_DIRS
     ${PCL_INCLUDE_DIRS}
 )
 
-IF("${MY_QT_VERSION}" STREQUAL "4")
+IF(QT4_FOUND)
     INCLUDE(${QT_USE_FILE})
-ENDIF()
+ENDIF(QT4_FOUND)
 
 SET(LIBRARIES
 	${RTABMap_LIBRARIES}
diff --git a/examples/RGBDMapping/CMakeLists.txt b/examples/RGBDMapping/CMakeLists.txt
index 712c73d6..b13ffcb0 100644
--- a/examples/RGBDMapping/CMakeLists.txt
+++ b/examples/RGBDMapping/CMakeLists.txt
@@ -16,7 +16,6 @@ if(internal)
 		rtabmap_gui
 		rtabmap_utilite
 	)  
-	SET(MY_QT_VERSION ${RTABMAP_QT_VERSION})
 else()
 	# external build
 	PROJECT( MyProject )
@@ -25,15 +24,11 @@ else()
 	FIND_PACKAGE(OpenCV REQUIRED)
 	FIND_PACKAGE(PCL 1.7 REQUIRED)
 	
-	# Use Qt4 or Qt5
-	SET(MY_QT_VERSION 4 CACHE STRING "Which Qt version to use")
-	IF("${MY_QT_VERSION}" STREQUAL "4")
-		message("4")
-	    FIND_PACKAGE(Qt4 COMPONENTS QtCore QtGui QtSvg)
-	ELSE()
-		message("5")
-	    FIND_PACKAGE(Qt5 COMPONENTS Widgets Core Gui Svg)
-	ENDIF()
+	# Find Qt5 first
+	FIND_PACKAGE(Qt5 COMPONENTS Widgets Core Gui Svg)
+    	IF(NOT Qt5_FOUND)
+       		FIND_PACKAGE(Qt4 COMPONENTS QtCore QtGui QtSvg)
+    	ENDIF(NOT Qt5_FOUND)
 endif()
 
 SET(INCLUDE_DIRS
@@ -42,9 +37,9 @@ SET(INCLUDE_DIRS
     ${PCL_INCLUDE_DIRS}
 )
 
-IF("${MY_QT_VERSION}" STREQUAL "4")
+IF(QT4_FOUND)
     INCLUDE(${QT_USE_FILE})
-ENDIF()
+ENDIF(QT4_FOUND)
 
 SET(LIBRARIES
 	${RTABMap_LIBRARIES}
@@ -55,7 +50,7 @@ SET(LIBRARIES
 
 INCLUDE_DIRECTORIES(${INCLUDE_DIRS})
 
-IF("${MY_QT_VERSION}" STREQUAL "4")
+IF(QT4_FOUND)
     QT4_WRAP_CPP(moc_srcs MapBuilder.h)
 ELSE()
     QT5_WRAP_CPP(moc_srcs MapBuilder.h)
diff --git a/examples/WifiMapping/CMakeLists.txt b/examples/WifiMapping/CMakeLists.txt
index b7271056..a5ffabd3 100644
--- a/examples/WifiMapping/CMakeLists.txt
+++ b/examples/WifiMapping/CMakeLists.txt
@@ -16,7 +16,6 @@ if(internal)
 		rtabmap_gui
 		rtabmap_utilite
 	)  
-	SET(MY_QT_VERSION ${RTABMAP_QT_VERSION})
 else()
 	# external build
 	PROJECT( MyProject )
@@ -25,15 +24,11 @@ else()
 	FIND_PACKAGE(OpenCV REQUIRED)
 	FIND_PACKAGE(PCL 1.7 REQUIRED)
 	
-	# Use Qt4 or Qt5
-	SET(MY_QT_VERSION 4 CACHE STRING "Which Qt version to use")
-	IF("${MY_QT_VERSION}" STREQUAL "4")
-		message("4")
-	    FIND_PACKAGE(Qt4 COMPONENTS QtCore QtGui QtSvg)
-	ELSE()
-		message("5")
-	    FIND_PACKAGE(Qt5 COMPONENTS Widgets Core Gui Svg)
-	ENDIF()
+	# Find Qt5 first
+	FIND_PACKAGE(Qt5 COMPONENTS Widgets Core Gui Svg)
+    	IF(NOT Qt5_FOUND)
+       		FIND_PACKAGE(Qt4 COMPONENTS QtCore QtGui QtSvg)
+    	ENDIF(NOT Qt5_FOUND)
 endif()
 
 SET(INCLUDE_DIRS
@@ -42,9 +37,9 @@ SET(INCLUDE_DIRS
     ${PCL_INCLUDE_DIRS}
 )
 
-IF("${MY_QT_VERSION}" STREQUAL "4")
+IF(QT4_FOUND)
     INCLUDE(${QT_USE_FILE})
-ENDIF()
+ENDIF(QT4_FOUND)
 
 SET(LIBRARIES
 	${RTABMap_LIBRARIES}
diff --git a/guilib/src/CMakeLists.txt b/guilib/src/CMakeLists.txt
index e3c98f56..f2d8f159 100644
--- a/guilib/src/CMakeLists.txt
+++ b/guilib/src/CMakeLists.txt
@@ -48,7 +48,7 @@ SET(qrc
     ./GuiLib.qrc
 )
 
-IF("${RTABMAP_QT_VERSION}" STREQUAL "4")
+IF(QT4_FOUND)
     # generate rules for building source files from the resources
     QT4_ADD_RESOURCES(srcs_qrc ${qrc})
 
@@ -106,9 +106,9 @@ SET(INCLUDE_DIRS
 	${PCL_INCLUDE_DIRS}
 )
 
-IF("${RTABMAP_QT_VERSION}" STREQUAL "4")
+IF(QT4_FOUND)
     INCLUDE(${QT_USE_FILE})
-ENDIF()
+ENDIF(QT4_FOUND)
 
 SET(LIBRARIES
     ${QT_LIBRARIES} 
@@ -131,9 +131,9 @@ ADD_LIBRARY(rtabmap_gui ${SRC_FILES})
 # Linking with Qt libraries
 
 TARGET_LINK_LIBRARIES(rtabmap_gui rtabmap_core rtabmap_utilite ${LIBRARIES})
-IF("${RTABMAP_QT_VERSION}" STREQUAL "5")
+IF(Qt5_FOUND)
     QT5_USE_MODULES(rtabmap_gui Widgets Core Gui Svg PrintSupport)
-ENDIF()
+ENDIF(Qt5_FOUND)
 
 SET_TARGET_PROPERTIES(
      rtabmap_gui
diff --git a/package.xml b/package.xml
index 343651cf..1ebc5f33 100644
--- a/package.xml
+++ b/package.xml
@@ -12,19 +12,17 @@
 
   <buildtool_depend>cmake</buildtool_depend>
   
-  <build_depend>libpcl-all-dev</build_depend>
-  <build_depend>libqt4-dev</build_depend>
-  <build_depend>libvtk-qt</build_depend>
+  <build_depend>libpcl-all-dev</build_depend> <!-- include qt and vtk -->
   <build_depend>libsqlite3-dev</build_depend>
   <build_depend>zlib</build_depend>
   <build_depend>libfreenect</build_depend>
   <build_depend>libopenni-dev</build_depend>
   <build_depend>libopenni2-dev</build_depend>
   <build_depend>cv_bridge</build_depend>
+  <!-- libproj-dev needed due to error in vtk6 (kinetic)-->
+  <build_depend>libproj-dev</build_depend>
   
   <run_depend>libpcl-all-dev</run_depend>
-  <run_depend>libqt4-dev</run_depend>
-  <run_depend>libvtk-qt</run_depend>
   <run_depend>libsqlite3-dev</run_depend>
   <run_depend>zlib</run_depend>
   <run_depend>libfreenect</run_depend>
diff --git a/tools/Calibration/CMakeLists.txt b/tools/Calibration/CMakeLists.txt
index 4915e6a8..8c1c87a9 100644
--- a/tools/Calibration/CMakeLists.txt
+++ b/tools/Calibration/CMakeLists.txt
@@ -6,9 +6,9 @@ SET(INCLUDE_DIRS
     ${PCL_INCLUDE_DIRS}
 )
 
-IF("${RTABMAP_QT_VERSION}" STREQUAL "4")
+IF(QT4_FOUND)
     INCLUDE(${QT_USE_FILE})
-ENDIF()
+ENDIF(QT4_FOUND)
 
 SET(LIBRARIES
 	${OpenCV_LIBRARIES} 
diff --git a/tools/DataRecorder/CMakeLists.txt b/tools/DataRecorder/CMakeLists.txt
index d7361db8..d93af8a1 100644
--- a/tools/DataRecorder/CMakeLists.txt
+++ b/tools/DataRecorder/CMakeLists.txt
@@ -12,9 +12,9 @@ SET(INCLUDE_DIRS
     ${PCL_INCLUDE_DIRS}
 )
 
-IF("${RTABMAP_QT_VERSION}" STREQUAL "4")
+IF(QT4_FOUND)
     INCLUDE(${QT_USE_FILE})
-ENDIF()
+ENDIF(QT4_FOUND)
 
 SET(LIBRARIES
     ${OpenCV_LIBRARIES} 
diff --git a/tools/DatabaseViewer/CMakeLists.txt b/tools/DatabaseViewer/CMakeLists.txt
index 69bb750b..b04b2ed2 100644
--- a/tools/DatabaseViewer/CMakeLists.txt
+++ b/tools/DatabaseViewer/CMakeLists.txt
@@ -11,9 +11,9 @@ SET(INCLUDE_DIRS
     ${PCL_INCLUDE_DIRS}
 )
 
-IF("${RTABMAP_QT_VERSION}" STREQUAL "4")
+IF(QT4_FOUND)
     INCLUDE(${QT_USE_FILE})
-ENDIF()
+ENDIF(QT4_FOUND)
 
 SET(LIBRARIES
 	${OpenCV_LIBRARIES}
diff --git a/tools/EpipolarGeometry/CMakeLists.txt b/tools/EpipolarGeometry/CMakeLists.txt
index ab519b2c..6a2ae47a 100644
--- a/tools/EpipolarGeometry/CMakeLists.txt
+++ b/tools/EpipolarGeometry/CMakeLists.txt
@@ -11,9 +11,9 @@ SET(INCLUDE_DIRS
     ${PCL_INCLUDE_DIRS}
 )
 
-IF("${RTABMAP_QT_VERSION}" STREQUAL "4")
+IF(QT4_FOUND)
     INCLUDE(${QT_USE_FILE})
-ENDIF()
+ENDIF(QT4_FOUND)
 
 SET(LIBRARIES
 	${OpenCV_LIBRARIES}
diff --git a/tools/OdometryViewer/CMakeLists.txt b/tools/OdometryViewer/CMakeLists.txt
index 3ec03e4a..05733d36 100644
--- a/tools/OdometryViewer/CMakeLists.txt
+++ b/tools/OdometryViewer/CMakeLists.txt
@@ -8,9 +8,9 @@ SET(INCLUDE_DIRS
     ${PCL_INCLUDE_DIRS}
 )
 
-IF("${RTABMAP_QT_VERSION}" STREQUAL "4")
+IF(QT4_FOUND)
     INCLUDE(${QT_USE_FILE})
-ENDIF()
+ENDIF(QT4_FOUND)
 
 SET(LIBRARIES
     ${OpenCV_LIBRARIES}