diff --git a/CMakeLists.txt b/CMakeLists.txt
index e18dc716..c52b5a3d 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -23,6 +23,13 @@ ENDIF(NOT DEFINED CMAKE_INSTALL_LIBDIR)
 
 INCLUDE(GNUInstallDirs)
 
+####### local cmake modules #######
+SET(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake_modules")
+
+if(POLICY CMP0043)
+    cmake_policy(SET CMP0043 OLD)
+endif()
+
 #######################
 # VERSION
 #######################
@@ -48,6 +55,8 @@ IF(NOT Qt5_FOUND)
 ENDIF(NOT Qt5_FOUND)
 ADD_DEFINITIONS(-DQT_NO_KEYWORDS) # To avoid conflicts with boost signals used in ROS
 
+FIND_PACKAGE(Tcmalloc QUIET)
+
 SET(NONFREE 0)
 IF(OPENCV_NONFREE_FOUND OR OPENCV_XFEATURES2D_FOUND)
    SET(NONFREE 1)
@@ -253,8 +262,14 @@ IF(NOT CATKIN_BUILD)
       MESSAGE(STATUS "  With Qt = NO (Qt not found)")
    ENDIF()
 
+   IF(Tcmalloc_FOUND)
+      MESSAGE(STATUS "  With tcmalloc = YES")
+   ELSE()
+      MESSAGE(STATUS "  With tcmalloc = NO (tcmalloc not found)")
+   ENDIF(Tcmalloc_FOUND)
+
    IF(APPLE)
-   MESSAGE(STATUS "  BUILD_AS_BUNDLE = ${BUILD_AS_BUNDLE}")
+      MESSAGE(STATUS "  BUILD_AS_BUNDLE = ${BUILD_AS_BUNDLE}")
    ENDIF(APPLE)
    MESSAGE(STATUS "--------------------------------------------")
 
diff --git a/app/CMakeLists.txt b/app/CMakeLists.txt
index c75bcf8a..08f61dc4 100644
--- a/app/CMakeLists.txt
+++ b/app/CMakeLists.txt
@@ -24,6 +24,14 @@ SET(LIBRARIES
     ${OpenCV_LIBS} 
 )
 
+if(Tcmalloc_FOUND)
+    SET(LIBRARIES
+        ${LIBARIES} 
+        ${Tcmalloc_LIBRARIES}
+    )
+endif(Tcmalloc_FOUND)
+
+
 #include files
 INCLUDE_DIRECTORIES(${INCLUDE_DIRS})
 
@@ -156,4 +164,4 @@ ENDIF((APPLE AND BUILD_AS_BUNDLE) OR WIN32)
 
 if(UNIX AND NOT APPLE)
     set (CMAKE_CXX_FLAGS  "-ltcmalloc")
-ENDIF()
\ No newline at end of file
+ENDIF()
diff --git a/cmake_modules/FindTcmalloc.cmake b/cmake_modules/FindTcmalloc.cmake
new file mode 100644
index 00000000..fbfe24fb
--- /dev/null
+++ b/cmake_modules/FindTcmalloc.cmake
@@ -0,0 +1,34 @@
+# - Find Tcmalloc
+# Find the native Tcmalloc library
+#
+#  Tcmalloc_LIBRARIES   - List of libraries when using Tcmalloc.
+#  Tcmalloc_FOUND       - True if Tcmalloc found.
+
+if (USE_TCMALLOC)
+  set(Tcmalloc_NAMES tcmalloc)
+else ()
+  set(Tcmalloc_NAMES tcmalloc_minimal tcmalloc)
+endif ()
+
+find_library(Tcmalloc_LIBRARY NAMES ${Tcmalloc_NAMES})
+
+if (Tcmalloc_LIBRARY)
+  set(Tcmalloc_FOUND TRUE)
+  set( Tcmalloc_LIBRARIES ${Tcmalloc_LIBRARY} )
+else ()
+  set(Tcmalloc_FOUND FALSE)
+  set( Tcmalloc_LIBRARIES )
+endif ()
+
+if (Tcmalloc_FOUND)
+  message(STATUS "Found Tcmalloc: ${Tcmalloc_LIBRARY}")
+else ()
+  message(STATUS "Not Found Tcmalloc: ${Tcmalloc_LIBRARY}")
+  if (Tcmalloc_FIND_REQUIRED)
+    message(STATUS "Looked for Tcmalloc libraries named ${Tcmalloc_NAMES}.")
+    message(FATAL_ERROR "Could NOT find Tcmalloc library")
+  endif ()
+endif ()
+
+mark_as_advanced(Tcmalloc_LIBRARY)
+