diff --git a/CMakeLists.txt b/CMakeLists.txt
new file mode 100644
index 0000000..5dd15ac
--- /dev/null
+++ b/CMakeLists.txt
@@ -0,0 +1,58 @@
+# CaroloCup-CameraPlayback - Simple application realized with
+#                            OpenDaVINCI to playback recordings
+#                            captured from our CaroloCup miniature
+#                            cars.
+# Copyright (C) 2015 Hugo Andrade, Christian Berger, Federico Giaimo.
+#
+# This program is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# as published by the Free Software Foundation; either version 2
+# of the License, or (at your option) any later version.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+# GNU General Public License for more details.
+#
+# You should have received a copy of the GNU General Public License
+# along with this program; if not, write to the Free Software
+# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+
+CMAKE_MINIMUM_REQUIRED (VERSION 2.8)
+
+PROJECT (CaroloCup-CameraPlayback)
+
+###########################################################################
+# Set the search path for .cmake files.
+SET (CMAKE_MODULE_PATH "${CMAKE_CURRENT_SOURCE_DIR}/cmake.Modules" ${CMAKE_MODULE_PATH})
+
+###########################################################################
+# Find OpenDaVINCI.
+FIND_PACKAGE (OpenDaVINCI REQUIRED)
+
+###########################################################################
+# Find OpenCV.
+SET(OpenCV_DIR "/usr")
+FIND_PACKAGE (OpenCV REQUIRED)
+
+###########################################################################
+# Set linking libraries to successfully link test suites and binaries.
+SET (LIBRARIES ${OPENDAVINCI_LIBRARIES} ${OpenCV_LIBS})
+
+###########################################################################
+# Set header files from OpenDaVINCI.
+INCLUDE_DIRECTORIES (${OPENDAVINCI_INCLUDE_DIRS}/opendavinci)
+# Set header files from OpenCV.
+INCLUDE_DIRECTORIES (${OpenCV_INCLUDE_DIR})
+# Set include directory.
+INCLUDE_DIRECTORIES(include)
+
+###############################################################################
+# Build this project.
+ADD_EXECUTABLE (CaroloCup-CameraPlayback "CaroloCup-CameraPlayback.cpp")
+TARGET_LINK_LIBRARIES (CaroloCup-CameraPlayback ${LIBRARIES}) 
+
+###############################################################################
+# Install this project.
+INSTALL(TARGETS CaroloCup-CameraPlayback RUNTIME DESTINATION bin COMPONENT software)
+
diff --git a/Dockerfile b/Dockerfile
index ba26966..aae5a92 100644
--- a/Dockerfile
+++ b/Dockerfile
@@ -19,7 +19,7 @@
 # Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
 
 # Author: Christian Berger
-# Date: 2015-06-25
+# Date: 2015-07-30
 
 FROM seresearch/opendavinci-ubuntu-amd64:latest
 MAINTAINER Christian Berger "christian.berger@gu.se"
@@ -38,5 +38,5 @@ RUN apt-get autoremove
 # Set locale (fix the locale warnings)
 RUN localedef -v -c -i en_US -f UTF-8 en_US.UTF-8 || :
 
-ADD CaroloCup-CameraPlayback /CaroloCup-CameraPlayback
+ADD build/CaroloCup-CameraPlayback /CaroloCup-CameraPlayback
 
diff --git a/Makefile b/Makefile
index f7b4b8e..6878630 100644
--- a/Makefile
+++ b/Makefile
@@ -1,41 +1,9 @@
-PREFIX=/usr
-CXX=g++
-RM=rm -f
-INCLUDES=-I$(PREFIX)/include/opendavinci
-CPPFLAGS=-Wall $(INCLUDES)
-LDLIBS=-L$(PREFIX)/lib/ -lopendavinci -lpthread -lopencv_core -lopencv_highgui
-
-BIN=CaroloCup-CameraPlayback
-SRCS=CaroloCup-CameraPlayback.cpp
-OBJS=$(subst .cpp,.o,$(SRCS))
-
 # Make variables to create the Docker image.
 PRODUCT=carolocup-cameraplayback
-VERSION=v2
+VERSION=v3
 REPOSITORY=seresearch
 BUILDLOG=build.log
 
-all: $(BIN)
-
-$(BIN): $(OBJS)
-	$(CXX) -o $(BIN) $(OBJS) $(LDLIBS) 
-
-docker: $(BIN)
+docker:
 	docker build -t $(REPOSITORY)/$(PRODUCT):$(VERSION) . | tee $(BUILDLOG) || exit 1
 
-depend: .depend
-
-.depend: $(SRCS)
-	rm -f ./.depend
-	$(CXX) $(CPPFLAGS) -MM $^>>./.depend;
-
-clean:
-	$(RM) $(OBJS)
-
-dist-clean: clean
-	$(RM) $(BUILDLOG)
-	$(RM) $(BIN)
-	$(RM) *~ .depend
-
-include .depend
-
diff --git a/cmake.Modules/FindOpenCV.cmake b/cmake.Modules/FindOpenCV.cmake
new file mode 100644
index 0000000..8d4726e
--- /dev/null
+++ b/cmake.Modules/FindOpenCV.cmake
@@ -0,0 +1,286 @@
+###########################################################
+#                  Find OpenCV Library
+# See http://sourceforge.net/projects/opencvlibrary/
+#----------------------------------------------------------
+#
+## 1: Setup:
+# The following variables are optionally searched for defaults
+#  OpenCV_DIR:            Base directory of OpenCv tree to use.
+#
+## 2: Variable
+# The following are set after configuration is done: 
+#  
+#  OpenCV_FOUND
+#  OpenCV_LIBS
+#  OpenCV_INCLUDE_DIR
+#  OpenCV_VERSION (OpenCV_VERSION_MAJOR, OpenCV_VERSION_MINOR, OpenCV_VERSION_PATCH)
+#
+#
+# Deprecated variable are used to maintain backward compatibility with
+# the script of Jan Woetzel (2006/09): www.mip.informatik.uni-kiel.de/~jw
+#  OpenCV_INCLUDE_DIRS
+#  OpenCV_LIBRARIES
+#  OpenCV_LINK_DIRECTORIES
+# 
+## 3: Version
+#
+# 2010/04/07 Benoit Rat, Correct a bug when OpenCVConfig.cmake is not found.
+# 2010/03/24 Benoit Rat, Add compatibility for when OpenCVConfig.cmake is not found.
+# 2010/03/22 Benoit Rat, Creation of the script.
+#
+#
+# tested with:
+# - OpenCV 2.1:  MinGW, MSVC2008
+# - OpenCV 2.0:  MinGW, MSVC2008, GCC4
+#
+#
+## 4: Licence:
+#
+# LGPL 2.1 : GNU Lesser General Public License Usage
+# Alternatively, this file may be used under the terms of the GNU Lesser
+
+# General Public License version 2.1 as published by the Free Software
+# Foundation and appearing in the file LICENSE.LGPL included in the
+# packaging of this file.  Please review the following information to
+# ensure the GNU Lesser General Public License version 2.1 requirements
+# will be met: http://www.gnu.org/licenses/old-licenses/lgpl-2.1.html.
+# 
+#----------------------------------------------------------
+
+
+find_path(OpenCV_DIR "OpenCVConfig.cmake" DOC "Root directory of OpenCV")
+
+set(OpenCV_configScript "${OpenCV_DIR}/OpenCVConfig.cmake")
+
+if(NOT EXISTS "${OpenCV_DIR}")
+    if(NOT WIN32)
+        include(FindPkgConfig)
+        if(PKG_CONFIG_FOUND)
+            pkg_check_modules(OPENCV_PKGCONF opencv)
+            set(OpenCV_DIR ${OPENCV_PKGCONF_PREFIX})
+            if(EXISTS "${OpenCV_DIR}")
+                set(OpenCV_configScript_DIR "${OpenCV_DIR}/share/opencv")
+                if(EXISTS "${OpenCV_configScript_DIR}")
+                    set(OpenCV_configScript "${OpenCV_configScript_DIR}/OpenCVConfig.cmake")
+                endif(EXISTS "${OpenCV_configScript_DIR}")
+            endif(EXISTS "${OpenCV_DIR}")
+        endif(PKG_CONFIG_FOUND)
+    endif(NOT WIN32)
+endif(NOT EXISTS "${OpenCV_DIR}")
+
+
+##====================================================
+## Find OpenCV libraries
+##----------------------------------------------------
+if(EXISTS "${OpenCV_DIR}")
+    set(CONTINUE_SEARCHING true)
+
+    #When its possible to use the Config script use it.
+    if(EXISTS "${OpenCV_configScript}")
+        ## Include the standard CMake script
+        include("${OpenCV_configScript}")
+        ## Search for a specific version
+        if(WIN32 OR NOT PKG_CONFIG_FOUND)
+              set(CVLIB_SUFFIX "${OpenCV_VERSION_MAJOR}${OpenCV_VERSION_MINOR}${OpenCV_VERSION_PATCH}")
+        endif(WIN32 OR NOT PKG_CONFIG_FOUND)
+        ##Boris:TODO: check for correct OpenCV version in pkg-config case !!!
+        #Otherwise it try to guess it.
+    else(EXISTS "${OpenCV_configScript}")
+        find_path(OpenCV_INCLUDE_DIR "cv.h" PATHS "${OpenCV_DIR}" PATH_SUFFIXES "include" "include/opencv" DOC "")
+        if(EXISTS  ${OpenCV_INCLUDE_DIR})
+            include_directories(${OpenCV_INCLUDE_DIR})
+        endif(EXISTS  ${OpenCV_INCLUDE_DIR})
+
+        if(NOT EXISTS ${VERSION_FILE_DIR}) #may alreay be in cache
+            #Find OpenCV version by looking at cvver.h or core/version.hpp
+            find_path(VERSION_FILE_DIR "version.hpp" PATHS "${OpenCV_DIR}" PATH_SUFFIXES "include" "include/opencv" "include/opencv2" "include/opencv2/core" DOC "")
+            if(NOT EXISTS ${VERSION_FILE_DIR})
+                find_path(VERSION_FILE_DIR "cvver.h" PATHS "${OpenCV_DIR}" PATH_SUFFIXES "include" "include/opencv" DOC "")
+                if(NOT EXISTS ${VERSION_FILE_DIR})
+                    message(STATUS "OpenCV version file not found")
+                    set(CONTINUE_SEARCHING false)
+                    set(OpenCV_FOUND false CACHE BOOL "" FORCE)
+                else(NOT EXISTS ${VERSION_FILE_DIR})
+                    set(VERSION_FILE ${VERSION_FILE_DIR}/cvver.h)  
+                endif(NOT EXISTS ${VERSION_FILE_DIR})
+            else(NOT EXISTS ${VERSION_FILE_DIR})
+                set(VERSION_FILE ${VERSION_FILE_DIR}/version.hpp)
+            endif(NOT EXISTS ${VERSION_FILE_DIR})
+
+            if (CONTINUE_SEARCHING)
+                #file(STRINGS ${OpenCV_INCLUDE_DIR}/cvver.h OpenCV_VERSIONS_TMP REGEX "^#define CV_[A-Z]+_VERSION[ \t]+[0-9]+$")
+                file(STRINGS ${VERSION_FILE} OpenCV_VERSIONS_TMP REGEX "^#define CV_VERSION_[A-Z]+[ \t]+[0-9]+$")
+                if(NOT("${OpenCV_VERSIONS_TMP}" STREQUAL ""))
+                    # Version >= 2.4.0
+                    string(REGEX REPLACE ".*#define CV_VERSION_EPOCH[ \t]+([0-9]+).*" "\\1" OpenCV_VERSION_MAJOR ${OpenCV_VERSIONS_TMP})
+                    string(REGEX REPLACE ".*#define CV_VERSION_MAJOR[ \t]+([0-9]+).*" "\\1" OpenCV_VERSION_MINOR ${OpenCV_VERSIONS_TMP})
+                    string(REGEX REPLACE ".*#define CV_VERSION_MINOR[ \t]+([0-9]+).*" "\\1" OpenCV_VERSION_PATCH ${OpenCV_VERSIONS_TMP})
+                else()
+                    # Version < 2.4.0
+                    file(STRINGS ${VERSION_FILE} OpenCV_VERSIONS_TMP REGEX "^#define CV_[A-Z]+_VERSION[ \t]+[0-9]+$")
+                    string(REGEX REPLACE ".*#define CV_MAJOR_VERSION[ \t]+([0-9]+).*" "\\1" OpenCV_VERSION_MAJOR ${OpenCV_VERSIONS_TMP})
+                    string(REGEX REPLACE ".*#define CV_MINOR_VERSION[ \t]+([0-9]+).*" "\\1" OpenCV_VERSION_MINOR ${OpenCV_VERSIONS_TMP})
+                    string(REGEX REPLACE ".*#define CV_SUBMINOR_VERSION[ \t]+([0-9]+).*" "\\1" OpenCV_VERSION_PATCH ${OpenCV_VERSIONS_TMP})
+                endif()
+
+                set(OpenCV_VERSION ${OpenCV_VERSION_MAJOR}.${OpenCV_VERSION_MINOR}.${OpenCV_VERSION_PATCH} CACHE STRING "" FORCE)
+                if(WIN32 OR NOT PKG_CONFIG_FOUND)
+                    set(CVLIB_SUFFIX "${OpenCV_VERSION_MAJOR}${OpenCV_VERSION_MINOR}${OpenCV_VERSION_PATCH}")
+                endif(WIN32 OR NOT PKG_CONFIG_FOUND)
+
+                ##Boris:TODO: check for correct OpenCV version in pklg-config case !!!
+            endif (CONTINUE_SEARCHING)
+        endif(NOT EXISTS ${VERSION_FILE_DIR})
+
+        if (CONTINUE_SEARCHING)
+            if(${OpenCV_VERSION} VERSION_GREATER 2.1.0)
+                set(OPENCV_LIB_COMPONENTS calib3d contrib core features2d flann gpu highgui imgproc legacy ml objdetect video)
+
+                #Add parent directory of ${OpenCV_INCLUDE_DIR} to ${OpenCV_INCLUDE_DIR} itself
+                #to be able to do both
+                #include <cv.h> and #include <opencv2/core/core.hpp>
+                get_filename_component(PARENT_DIR ${OpenCV_INCLUDE_DIR} PATH)
+                set(OpenCV_INCLUDE_DIR "${OpenCV_INCLUDE_DIR};${PARENT_DIR}")
+            else(${OpenCV_VERSION} VERSION_GREATER 2.1.0)
+                set(OPENCV_LIB_COMPONENTS cxcore cv ml highgui cvaux)
+            endif(${OpenCV_VERSION} VERSION_GREATER 2.1.0)
+        endif (CONTINUE_SEARCHING)
+    endif(EXISTS "${OpenCV_configScript}")
+
+    if (CONTINUE_SEARCHING)
+        if(${OpenCV_VERSION} VERSION_GREATER 2.1.0)
+            set(CVLIB_SUFFIX ".${OpenCV_VERSION}")
+
+            ## Initiate the variable before the loop
+            set(OpenCV_LIBS "")
+
+            ## Loop over each components
+            foreach(__CVLIB ${OPENCV_LIB_COMPONENTS})
+                find_library(OpenCV_${__CVLIB}_LIBRARY_DEBUG
+                        NAMES "${__CVLIB}${CVLIB_SUFFIX}d"
+                              "lib${__CVLIB}${CVLIB_SUFFIX}d"
+                        PATHS ${OpenCV_DIR}/lib/
+                              ${OpenCV_DIR}/lib/x86_64-linux-gnu/
+                        NO_DEFAULT_PATH)
+
+                find_library(OpenCV_${__CVLIB}_LIBRARY_RELEASE
+                        NAMES "${__CVLIB}${CVLIB_SUFFIX}"
+                              "lib${__CVLIB}${CVLIB_SUFFIX}"
+                              "opencv_${__CVLIB}${CVLIB_SUFFIX}"
+                              "opencv_${__CVLIB}" "${__CVLIB}"
+                        PATHS ${OpenCV_DIR}/lib/
+                              ${OpenCV_DIR}/lib/x86_64-linux-gnu/
+                        NO_DEFAULT_PATH)
+
+                #On MacOSX libraries are named:  libopencv_${__CVLIB}${CVLIB_SUFFIX}.dylib
+                #On Linux libraries are named:  libopencv_${__CVLIB}.so${CVLIB_SUFFIX}
+                # but with pkg-config ${OPENCV_LIB_COMPONENTS} are already prefixed with opencv_ ?
+
+                #we add: "opencv_${__CVLIB}${CVLIB_SUFFIX}" for MacOSX
+                #we add: "${__CVLIB}" for linux (but version is not checked !)
+
+                #Remove the cache value
+                set(OpenCV_${__CVLIB}_LIBRARY "" CACHE STRING "" FORCE)
+
+                #both debug/release
+                if(OpenCV_${__CVLIB}_LIBRARY_DEBUG AND OpenCV_${__CVLIB}_LIBRARY_RELEASE)
+                    set(OpenCV_${__CVLIB}_LIBRARY debug ${OpenCV_${__CVLIB}_LIBRARY_DEBUG} optimized ${OpenCV_${__CVLIB}_LIBRARY_RELEASE}  CACHE STRING "" FORCE)
+                #only debug
+                elseif(OpenCV_${__CVLIB}_LIBRARY_DEBUG)
+                    set(OpenCV_${__CVLIB}_LIBRARY ${OpenCV_${__CVLIB}_LIBRARY_DEBUG}  CACHE STRING "" FORCE)
+                #only release
+                elseif(OpenCV_${__CVLIB}_LIBRARY_RELEASE)
+                    set(OpenCV_${__CVLIB}_LIBRARY ${OpenCV_${__CVLIB}_LIBRARY_RELEASE}  CACHE STRING "" FORCE)
+                #no library found
+                else()
+                    set(OpenCV_FOUND_TMP false)
+                endif()
+
+                #Add to the general list
+                if(OpenCV_${__CVLIB}_LIBRARY)
+                    set(OpenCV_LIBS ${OpenCV_LIBS} ${OpenCV_${__CVLIB}_LIBRARY})
+                endif(OpenCV_${__CVLIB}_LIBRARY)
+            endforeach(__CVLIB)
+
+            if("${OpenCV_LIBS}" STREQUAL "")
+                set(OpenCV_FOUND false CACHE BOOL "" FORCE)
+            else()
+                set(OpenCV_FOUND true CACHE BOOL "" FORCE)
+            endif()
+        else(${OpenCV_VERSION} VERSION_GREATER 2.1.0)
+            ## Initiate the variable before the loop
+            set(OpenCV_LIBS "")
+            set(OpenCV_FOUND_TMP true)
+
+            ## Loop over each components
+            foreach(__CVLIB ${OPENCV_LIB_COMPONENTS})
+                find_library(OpenCV_${__CVLIB}_LIBRARY_DEBUG NAMES "${__CVLIB}${CVLIB_SUFFIX}d" "lib${__CVLIB}${CVLIB_SUFFIX}d" PATHS "${OpenCV_DIR}/lib" NO_DEFAULT_PATH)
+                find_library(OpenCV_${__CVLIB}_LIBRARY_RELEASE NAMES "${__CVLIB}${CVLIB_SUFFIX}" "lib${__CVLIB}${CVLIB_SUFFIX}" PATHS "${OpenCV_DIR}/lib" NO_DEFAULT_PATH)
+
+                #Remove the cache value
+                set(OpenCV_${__CVLIB}_LIBRARY "" CACHE STRING "" FORCE)
+
+                #both debug/release
+                if(OpenCV_${__CVLIB}_LIBRARY_DEBUG AND OpenCV_${__CVLIB}_LIBRARY_RELEASE)
+                    set(OpenCV_${__CVLIB}_LIBRARY debug ${OpenCV_${__CVLIB}_LIBRARY_DEBUG} optimized ${OpenCV_${__CVLIB}_LIBRARY_RELEASE}  CACHE STRING "" FORCE)
+                #only debug
+                elseif(OpenCV_${__CVLIB}_LIBRARY_DEBUG)
+                    set(OpenCV_${__CVLIB}_LIBRARY ${OpenCV_${__CVLIB}_LIBRARY_DEBUG}  CACHE STRING "" FORCE)
+                #only release
+                elseif(OpenCV_${__CVLIB}_LIBRARY_RELEASE)
+                    set(OpenCV_${__CVLIB}_LIBRARY ${OpenCV_${__CVLIB}_LIBRARY_RELEASE}  CACHE STRING "" FORCE)
+                #no library found
+                else()
+                    set(OpenCV_FOUND_TMP false)
+                endif()
+
+                #Add to the general list
+                if(OpenCV_${__CVLIB}_LIBRARY)
+                    set(OpenCV_LIBS ${OpenCV_LIBS} ${OpenCV_${__CVLIB}_LIBRARY})
+                endif(OpenCV_${__CVLIB}_LIBRARY)
+            endforeach(__CVLIB)
+
+            if("${OpenCV_LIBS}" STREQUAL "")
+                set(OpenCV_FOUND false CACHE BOOL "" FORCE)
+            else()
+                set(OpenCV_FOUND true CACHE BOOL "" FORCE)
+            endif()
+        endif(${OpenCV_VERSION} VERSION_GREATER 2.1.0)
+    endif(CONTINUE_SEARCHING)
+else(EXISTS "${OpenCV_DIR}")
+      set(ERR_MSG "Please specify OpenCV directory using OpenCV_DIR env. variable")
+endif(EXISTS "${OpenCV_DIR}")
+##====================================================
+
+##====================================================
+## Print message
+##----------------------------------------------------
+if(NOT OpenCV_FOUND)
+    # make FIND_PACKAGE friendly
+    if(NOT OpenCV_FIND_QUIETLY)
+        if(OpenCV_FIND_REQUIRED)
+            message(FATAL_ERROR "OpenCV required but some headers or libs not found. ${ERR_MSG}")
+        else(OpenCV_FIND_REQUIRED)
+            message(STATUS "WARNING: OpenCV was not found. ${ERR_MSG}")
+        endif(OpenCV_FIND_REQUIRED)
+    endif(NOT OpenCV_FIND_QUIETLY)
+endif(NOT OpenCV_FOUND)
+##====================================================
+
+
+##====================================================
+## Backward compatibility
+##----------------------------------------------------
+if(OpenCV_FOUND)
+    option(OpenCV_BACKWARD_COMPA "Add some variable to make this script compatible with the other version of FindOpenCV.cmake" false)
+    if(OpenCV_BACKWARD_COMPA)
+        find_path(OpenCV_INCLUDE_DIRS "cv.h" PATHS "${OpenCV_DIR}" PATH_SUFFIXES "include" "include/opencv" DOC "Include directory") 
+        find_path(OpenCV_INCLUDE_DIR "cv.h" PATHS "${OpenCV_DIR}" PATH_SUFFIXES "include" "include/opencv" DOC "Include directory")
+        set(OpenCV_LIBRARIES "${OpenCV_LIBS}" CACHE STRING "" FORCE)
+    endif(OpenCV_BACKWARD_COMPA)
+
+    MESSAGE(STATUS "Found OpenCV headers:   ${OpenCV_INCLUDE_DIR}")
+    MESSAGE(STATUS "Found OpenCV libraries: ${OpenCV_LIBS}")
+endif(OpenCV_FOUND)
+##====================================================
+