diff --git a/CMakeLists.txt b/CMakeLists.txt
index 976709646ff5d2d052630537fe6178a4251c1653..246dffbc6f04c9054d91d8f09fb0daadf5052d98 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -1,4 +1,6 @@
 # Pre-requisites about cmake itself
+#Start WOLF build
+MESSAGE("Starting WOLF CMakeLists ...")
 CMAKE_MINIMUM_REQUIRED(VERSION 2.6)
 
 if(COMMAND cmake_policy)
@@ -13,30 +15,13 @@ SET(CMAKE_MACOSX_RPATH 1)
 PROJECT(core)
 set(PLUGIN_NAME "wolf${PROJECT_NAME}")
 
-#string(COMPARE EQUAL "${CMAKE_BINARY_DIR}" "" result)
-#IF(result)
-#  SET(CMAKE_BINARY_DIR ${CMAKE_CURRENT_SOURCE_DIR})
-#ENDIF()
-#message(STATUS "Binary path : " ${CMAKE_BINARY_DIR})
-#
-
-#SET(EXECUTABLE_OUTPUT_PATH ${CMAKE_BINARY_DIR}/bin)
-#SET(LIBRARY_OUTPUT_PATH ${CMAKE_BINARY_DIR}/lib)
-#
-# We'll set the install prefix only is it's empty
-# which shouldn't be the case ...
-#string(COMPARE EQUAL "${CMAKE_INSTALL_PREFIX}" "" result)
-#IF(result)
-  # This path is actually default on linux
-#  SET(CMAKE_INSTALL_PREFIX /usr/local)
-#ENDIF()
-#message(STATUS "Installation path : " ${CMAKE_INSTALL_PREFIX})
-
+# Paths
 SET(EXECUTABLE_OUTPUT_PATH ${CMAKE_CURRENT_SOURCE_DIR}/bin)
 SET(LIBRARY_OUTPUT_PATH ${CMAKE_CURRENT_SOURCE_DIR}/lib)
 SET(CMAKE_INSTALL_PREFIX /usr/local)
 SET(CMAKE_SKIP_INSTALL_ALL_DEPENDENCY FALSE)
 
+# Build type
 IF (NOT CMAKE_BUILD_TYPE)
   SET(CMAKE_BUILD_TYPE "DEBUG")
 ENDIF (NOT CMAKE_BUILD_TYPE)
@@ -65,12 +50,14 @@ if(UNIX)
   set(CMAKE_CXX_FLAGS
     "${CMAKE_CXX_FLAGS} -Werror=all -Werror=extra -Wno-unknown-pragmas -Wno-sign-compare -Wno-unused-parameter -Wno-missing-field-initializers")
 endif(UNIX)
+
+# Options
 IF(NOT BUILD_TESTS)
   OPTION(BUILD_TESTS "Build Unit tests" ON)
 ENDIF(NOT BUILD_TESTS)
 
 IF(NOT BUILD_DEMOS)
-  OPTION(BUILD_DEMOS "Build Demos" OFF)
+  OPTION(BUILD_DEMOS "Build Demos" ON)
 ENDIF(NOT BUILD_DEMOS)
 
 IF(NOT BUILD_DOC)
@@ -81,10 +68,7 @@ IF(PROFILING)
   add_definitions(-DPROFILING)
   message("Compiling with profiling options...")
 ENDIF(PROFILING)
-#############
-## Testing ##
-#############
-#
+
 if(BUILD_TESTS)
     # Enables testing for this directory and below.
     # Note that ctest expects to find a test file in the build directory root.
@@ -93,18 +77,6 @@ if(BUILD_TESTS)
     enable_testing()
 endif()
 
-#+START_SRC --------------------------------------------------------------------------------------------------------------------------------
-#Start WOLF build
-MESSAGE("Starting WOLF CMakeLists ...")
-CMAKE_MINIMUM_REQUIRED(VERSION 2.8)
-
-#CMAKE modules
-
-SET(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_SOURCE_DIR}/cmake_modules")
-MESSAGE(STATUS ${CMAKE_MODULE_PATH})
-
-# Some wolf compilation options
-
 IF((CMAKE_BUILD_TYPE MATCHES DEBUG) OR (CMAKE_BUILD_TYPE MATCHES debug) OR (CMAKE_BUILD_TYPE MATCHES Debug))
   set(_WOLF_DEBUG true)
 ENDIF()
@@ -119,9 +91,16 @@ IF(BUILD_DEMOS OR BUILD_TESTS)
 ENDIF(BUILD_DEMOS OR BUILD_TESTS)
 
 
-#find dependencies.
+#START_SRC --------------------------------------------------------------------------------------------------------------------------------
+
+#CMAKE modules
+
+SET(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_SOURCE_DIR}/cmake_modules")
+MESSAGE(STATUS "Cmake modules at: " ${CMAKE_MODULE_PATH})
 
 
+#find dependencies.
+
 FIND_PACKAGE(Threads REQUIRED)
 
 FIND_PACKAGE(Ceres REQUIRED) #Ceres is required
@@ -161,8 +140,10 @@ ELSE (SPDLOG_INCLUDE_DIR)
  MESSAGE(FATAL_ERROR "Could not find spdlog")
 ENDIF (SPDLOG_INCLUDE_DIR)
 
-INCLUDE_DIRECTORIES(${EIGEN_INCLUDE_DIRS})
-INCLUDE_DIRECTORIES("include")
+
+# Includes
+INCLUDE_DIRECTORIES("include") # In this same project
+INCLUDE_DIRECTORIES(${EIGEN3_INCLUDE_DIRS})
 INCLUDE_DIRECTORIES(${YAMLCPP_INCLUDE_DIR})
 INCLUDE_DIRECTORIES(${CERES_INCLUDE_DIRS})
 
@@ -469,19 +450,14 @@ IF (Ceres_FOUND)
     TARGET_LINK_LIBRARIES(${PLUGIN_NAME} ${CERES_LIBRARIES})
 ENDIF(Ceres_FOUND)
 
-# IF (GLOG_FOUND)
-#     TARGET_LINK_LIBRARIES(${PLUGIN_NAME} ${GLOG_LIBRARY})
-# ENDIF (GLOG_FOUND)
-#check if this is done correctly
-
 IF(BUILD_TESTS)
-  MESSAGE("Building tests.")
+  MESSAGE(STATUS "Will build tests.")
   add_subdirectory(test)
 ENDIF(BUILD_TESTS)
 
 IF(BUILD_DEMOS)
   #Build demos
-  MESSAGE("Building demos.")
+  MESSAGE(STATUS "Will build demos.")
   ADD_SUBDIRECTORY(demos)
 ENDIF(BUILD_DEMOS)
 
@@ -561,6 +537,7 @@ INSTALL(DIRECTORY ${SPDLOG_INCLUDE_DIRS} DESTINATION "include/iri-algorithms/")
 export(PACKAGE ${PLUGIN_NAME})
 
 #-END_SRC --------------------------------------------------------------------------------------------------------------------------------
+
 FIND_PACKAGE(Doxygen)
 
 FIND_PATH(IRI_DOC_DIR doxygen.conf ${CMAKE_SOURCE_DIR}/doc/iri_doc/)
@@ -603,17 +580,4 @@ ELSE(UNIX)
     )
 ENDIF(UNIX)
 
-IF (UNIX)
-  SET(CPACK_PACKAGE_FILE_NAME "iri-${PLUGIN_NAME}-dev-${CPACK_PACKAGE_VERSION}${CPACK_DEBIAN_PACKAGE_ARCHITECTURE}")
-  SET(CPACK_PACKAGE_NAME "iri-${PLUGIN_NAME}-dev")
-  SET(CPACK_PACKAGE_DESCRIPTION_SUMMARY "...Enter something here...")
-  SET(CPACK_PACKAGING_INSTALL_PREFIX ${CMAKE_INSTALL_PREFIX})
-  SET(CPACK_GENERATOR "DEB")
-  SET(CPACK_DEBIAN_PACKAGE_MAINTAINER "galenya - labrobotica@iri.upc.edu")
-  SET(CPACK_SET_DESTDIR "ON")  # Necessary because of the absolute install paths
-  INCLUDE(CPack)
-ELSE(UNIX)
-  ADD_CUSTOM_COMMAND(
-    COMMENT "packaging only implemented in unix"
-    TARGET  uninstall)
-ENDIF(UNIX)
+
diff --git a/cmake_modules/FindEigen3.cmake b/cmake_modules/FindEigen3.cmake
index d44ea8903d4c5a72af468d603dc1d8e5a6bbf542..9e969786089ca8ea3801be8b084c51a5782f09b5 100644
--- a/cmake_modules/FindEigen3.cmake
+++ b/cmake_modules/FindEigen3.cmake
@@ -1,263 +1,97 @@
-# Ceres Solver - A fast non-linear least squares minimizer
-# Copyright 2015 Google Inc. All rights reserved.
-# http://ceres-solver.org/
+# - Try to find Eigen3 lib
 #
-# Redistribution and use in source and binary forms, with or without
-# modification, are permitted provided that the following conditions are met:
+# This module supports requiring a minimum version, e.g. you can do
+#   find_package(Eigen3 3.1.2)
+# to require version 3.1.2 or newer of Eigen3.
 #
-# * Redistributions of source code must retain the above copyright notice,
-#   this list of conditions and the following disclaimer.
-# * Redistributions in binary form must reproduce the above copyright notice,
-#   this list of conditions and the following disclaimer in the documentation
-#   and/or other materials provided with the distribution.
-# * Neither the name of Google Inc. nor the names of its contributors may be
-#   used to endorse or promote products derived from this software without
-#   specific prior written permission.
+# Once done this will define
 #
-# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
-# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
-# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
-# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
-# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
-# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
-# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
-# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
-# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
-# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-# POSSIBILITY OF SUCH DAMAGE.
+#  EIGEN3_FOUND - system has eigen lib with correct version
+#  EIGEN3_INCLUDE_DIR - the eigen include directory
+#  EIGEN3_VERSION - eigen version
 #
-# Author: alexs.mac@gmail.com (Alex Stewart)
+# This module reads hints about search locations from 
+# the following enviroment variables:
 #
+# EIGEN3_ROOT
+# EIGEN3_ROOT_DIR
+
+# Copyright (c) 2006, 2007 Montel Laurent, <montel@kde.org>
+# Copyright (c) 2008, 2009 Gael Guennebaud, <g.gael@free.fr>
+# Copyright (c) 2009 Benoit Jacob <jacob.benoit.1@gmail.com>
+# Redistribution and use is allowed according to the terms of the 2-clause BSD license.
+
+if(NOT Eigen3_FIND_VERSION)
+  if(NOT Eigen3_FIND_VERSION_MAJOR)
+    set(Eigen3_FIND_VERSION_MAJOR 2)
+  endif(NOT Eigen3_FIND_VERSION_MAJOR)
+  if(NOT Eigen3_FIND_VERSION_MINOR)
+    set(Eigen3_FIND_VERSION_MINOR 91)
+  endif(NOT Eigen3_FIND_VERSION_MINOR)
+  if(NOT Eigen3_FIND_VERSION_PATCH)
+    set(Eigen3_FIND_VERSION_PATCH 0)
+  endif(NOT Eigen3_FIND_VERSION_PATCH)
+
+  set(Eigen3_FIND_VERSION "${Eigen3_FIND_VERSION_MAJOR}.${Eigen3_FIND_VERSION_MINOR}.${Eigen3_FIND_VERSION_PATCH}")
+endif(NOT Eigen3_FIND_VERSION)
+
+macro(_eigen3_check_version)
+  file(READ "${EIGEN3_INCLUDE_DIR}/Eigen/src/Core/util/Macros.h" _eigen3_version_header)
+
+  string(REGEX MATCH "define[ \t]+EIGEN_WORLD_VERSION[ \t]+([0-9]+)" _eigen3_world_version_match "${_eigen3_version_header}")
+  set(EIGEN3_WORLD_VERSION "${CMAKE_MATCH_1}")
+  string(REGEX MATCH "define[ \t]+EIGEN_MAJOR_VERSION[ \t]+([0-9]+)" _eigen3_major_version_match "${_eigen3_version_header}")
+  set(EIGEN3_MAJOR_VERSION "${CMAKE_MATCH_1}")
+  string(REGEX MATCH "define[ \t]+EIGEN_MINOR_VERSION[ \t]+([0-9]+)" _eigen3_minor_version_match "${_eigen3_version_header}")
+  set(EIGEN3_MINOR_VERSION "${CMAKE_MATCH_1}")
+
+  set(EIGEN3_VERSION ${EIGEN3_WORLD_VERSION}.${EIGEN3_MAJOR_VERSION}.${EIGEN3_MINOR_VERSION})
+  if(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION})
+    set(EIGEN3_VERSION_OK FALSE)
+  else(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION})
+    set(EIGEN3_VERSION_OK TRUE)
+  endif(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION})
+
+  if(NOT EIGEN3_VERSION_OK)
+
+    message(STATUS "Eigen3 version ${EIGEN3_VERSION} found in ${EIGEN3_INCLUDE_DIR}, "
+                   "but at least version ${Eigen3_FIND_VERSION} is required")
+  endif(NOT EIGEN3_VERSION_OK)
+endmacro(_eigen3_check_version)
+
+if (EIGEN3_INCLUDE_DIR)
+
+  # in cache already
+  _eigen3_check_version()
+  set(EIGEN3_FOUND ${EIGEN3_VERSION_OK})
+
+else (EIGEN3_INCLUDE_DIR)
+  
+  # search first if an Eigen3Config.cmake is available in the system,
+  # if successful this would set EIGEN3_INCLUDE_DIR and the rest of
+  # the script will work as usual
+  find_package(Eigen3 ${Eigen3_FIND_VERSION} NO_MODULE QUIET)
+
+  if(NOT EIGEN3_INCLUDE_DIR)
+    find_path(EIGEN3_INCLUDE_DIR NAMES signature_of_eigen3_matrix_library
+        HINTS
+        ENV EIGEN3_ROOT 
+        ENV EIGEN3_ROOT_DIR
+        PATHS
+        ${CMAKE_INSTALL_PREFIX}/include
+        ${KDE4_INCLUDE_DIR}
+        PATH_SUFFIXES eigen3 eigen
+      )
+  endif(NOT EIGEN3_INCLUDE_DIR)
+
+  if(EIGEN3_INCLUDE_DIR)
+    _eigen3_check_version()
+  endif(EIGEN3_INCLUDE_DIR)
+
+  include(FindPackageHandleStandardArgs)
+  find_package_handle_standard_args(Eigen3 DEFAULT_MSG EIGEN3_INCLUDE_DIR EIGEN3_VERSION_OK)
+
+  mark_as_advanced(EIGEN3_INCLUDE_DIR)
+
+endif(EIGEN3_INCLUDE_DIR)
 
-# FindEigen.cmake - Find Eigen library, version >= 3.
-#
-# This module defines the following variables:
-#
-# EIGEN_FOUND: TRUE iff Eigen is found.
-# EIGEN_INCLUDE_DIRS: Include directories for Eigen.
-# EIGEN_VERSION: Extracted from Eigen/src/Core/util/Macros.h
-# EIGEN_WORLD_VERSION: Equal to 3 if EIGEN_VERSION = 3.2.0
-# EIGEN_MAJOR_VERSION: Equal to 2 if EIGEN_VERSION = 3.2.0
-# EIGEN_MINOR_VERSION: Equal to 0 if EIGEN_VERSION = 3.2.0
-# FOUND_INSTALLED_EIGEN_CMAKE_CONFIGURATION: True iff the version of Eigen
-#                                            found was built & installed /
-#                                            exported as a CMake package.
-#
-# The following variables control the behaviour of this module:
-#
-# EIGEN_PREFER_EXPORTED_EIGEN_CMAKE_CONFIGURATION: TRUE/FALSE, iff TRUE then
-#                           then prefer using an exported CMake configuration
-#                           generated by Eigen over searching for the
-#                           Eigen components manually.  Otherwise (FALSE)
-#                           ignore any exported Eigen CMake configurations and
-#                           always perform a manual search for the components.
-#                           Default: TRUE iff user does not define this variable
-#                           before we are called, and does NOT specify
-#                           EIGEN_INCLUDE_DIR_HINTS, otherwise FALSE.
-# EIGEN_INCLUDE_DIR_HINTS: List of additional directories in which to
-#                          search for eigen includes, e.g: /timbuktu/eigen3.
-#
-# The following variables are also defined by this module, but in line with
-# CMake recommended FindPackage() module style should NOT be referenced directly
-# by callers (use the plural variables detailed above instead).  These variables
-# do however affect the behaviour of the module via FIND_[PATH/LIBRARY]() which
-# are NOT re-called (i.e. search for library is not repeated) if these variables
-# are set with valid values _in the CMake cache_. This means that if these
-# variables are set directly in the cache, either by the user in the CMake GUI,
-# or by the user passing -DVAR=VALUE directives to CMake when called (which
-# explicitly defines a cache variable), then they will be used verbatim,
-# bypassing the HINTS variables and other hard-coded search locations.
-#
-# EIGEN_INCLUDE_DIR: Include directory for CXSparse, not including the
-#                    include directory of any dependencies.
-
-# Called if we failed to find Eigen or any of it's required dependencies,
-# unsets all public (designed to be used externally) variables and reports
-# error message at priority depending upon [REQUIRED/QUIET/<NONE>] argument.
-macro(EIGEN_REPORT_NOT_FOUND REASON_MSG)
-  unset(EIGEN_FOUND)
-  unset(EIGEN_INCLUDE_DIRS)
-  unset(FOUND_INSTALLED_EIGEN_CMAKE_CONFIGURATION)
-  # Make results of search visible in the CMake GUI if Eigen has not
-  # been found so that user does not have to toggle to advanced view.
-  mark_as_advanced(CLEAR EIGEN_INCLUDE_DIR)
-  # Note <package>_FIND_[REQUIRED/QUIETLY] variables defined by FindPackage()
-  # use the camelcase library name, not uppercase.
-  if (Eigen_FIND_QUIETLY)
-    message(STATUS "Failed to find Eigen - " ${REASON_MSG} ${ARGN})
-  elseif (Eigen_FIND_REQUIRED)
-    message(FATAL_ERROR "Failed to find Eigen - " ${REASON_MSG} ${ARGN})
-  else()
-    # Neither QUIETLY nor REQUIRED, use no priority which emits a message
-    # but continues configuration and allows generation.
-    message("-- Failed to find Eigen - " ${REASON_MSG} ${ARGN})
-  endif ()
-  return()
-endmacro(EIGEN_REPORT_NOT_FOUND)
-
-# Protect against any alternative find_package scripts for this library having
-# been called previously (in a client project) which set EIGEN_FOUND, but not
-# the other variables we require / set here which could cause the search logic
-# here to fail.
-unset(EIGEN_FOUND)
-
-# -----------------------------------------------------------------
-# By default, if the user has expressed no preference for using an exported
-# Eigen CMake configuration over performing a search for the installed
-# components, and has not specified any hints for the search locations, then
-# prefer an exported configuration if available.
-if (NOT DEFINED EIGEN_PREFER_EXPORTED_EIGEN_CMAKE_CONFIGURATION
-    AND NOT EIGEN_INCLUDE_DIR_HINTS)
-  message(STATUS "No preference for use of exported Eigen CMake configuration "
-    "set, and no hints for include directory provided. "
-    "Defaulting to preferring an installed/exported Eigen CMake configuration "
-    "if available.")
-  set(EIGEN_PREFER_EXPORTED_EIGEN_CMAKE_CONFIGURATION TRUE)
-endif()
-
-if (EIGEN_PREFER_EXPORTED_EIGEN_CMAKE_CONFIGURATION)
-  # Try to find an exported CMake configuration for Eigen.
-  #
-  # We search twice, s/t we can invert the ordering of precedence used by
-  # find_package() for exported package build directories, and installed
-  # packages (found via CMAKE_SYSTEM_PREFIX_PATH), listed as items 6) and 7)
-  # respectively in [1].
-  #
-  # By default, exported build directories are (in theory) detected first, and
-  # this is usually the case on Windows.  However, on OS X & Linux, the install
-  # path (/usr/local) is typically present in the PATH environment variable
-  # which is checked in item 4) in [1] (i.e. before both of the above, unless
-  # NO_SYSTEM_ENVIRONMENT_PATH is passed).  As such on those OSs installed
-  # packages are usually detected in preference to exported package build
-  # directories.
-  #
-  # To ensure a more consistent response across all OSs, and as users usually
-  # want to prefer an installed version of a package over a locally built one
-  # where both exist (esp. as the exported build directory might be removed
-  # after installation), we first search with NO_CMAKE_PACKAGE_REGISTRY which
-  # means any build directories exported by the user are ignored, and thus
-  # installed directories are preferred.  If this fails to find the package
-  # we then research again, but without NO_CMAKE_PACKAGE_REGISTRY, so any
-  # exported build directories will now be detected.
-  #
-  # To prevent confusion on Windows, we also pass NO_CMAKE_BUILDS_PATH (which
-  # is item 5) in [1]), to not preferentially use projects that were built
-  # recently with the CMake GUI to ensure that we always prefer an installed
-  # version if available.
-  #
-  # [1] http://www.cmake.org/cmake/help/v2.8.11/cmake.html#command:find_package
-  find_package(Eigen3 QUIET
-                      NO_MODULE
-                      NO_CMAKE_PACKAGE_REGISTRY
-                      NO_CMAKE_BUILDS_PATH)
-  if (EIGEN3_FOUND)
-    message(STATUS "Found installed version of Eigen: ${Eigen3_DIR}")
-  else()
-    # Failed to find an installed version of Eigen, repeat search allowing
-    # exported build directories.
-    message(STATUS "Failed to find installed Eigen CMake configuration, "
-      "searching for Eigen build directories exported with CMake.")
-    # Again pass NO_CMAKE_BUILDS_PATH, as we know that Eigen is exported and
-    # do not want to treat projects built with the CMake GUI preferentially.
-    find_package(Eigen3 QUIET
-                        NO_MODULE
-                        NO_CMAKE_BUILDS_PATH)
-    if (EIGEN3_FOUND)
-      message(STATUS "Found exported Eigen build directory: ${Eigen3_DIR}")
-    endif()
-  endif()
-  if (EIGEN3_FOUND)
-    set(FOUND_INSTALLED_EIGEN_CMAKE_CONFIGURATION TRUE)
-    set(EIGEN_FOUND ${EIGEN3_FOUND})
-    set(EIGEN_INCLUDE_DIR "${EIGEN3_INCLUDE_DIR}" CACHE STRING
-      "Eigen include directory" FORCE)
-  else()
-    message(STATUS "Failed to find an installed/exported CMake configuration "
-      "for Eigen, will perform search for installed Eigen components.")
-  endif()
-endif()
-
-if (NOT EIGEN_FOUND)
-  # Search user-installed locations first, so that we prefer user installs
-  # to system installs where both exist.
-  list(APPEND EIGEN_CHECK_INCLUDE_DIRS
-    /usr/local/include
-    /usr/local/homebrew/include # Mac OS X
-    /opt/local/var/macports/software # Mac OS X.
-    /opt/local/include
-    /usr/include)
-  # Additional suffixes to try appending to each search path.
-  list(APPEND EIGEN_CHECK_PATH_SUFFIXES
-    eigen3 # Default root directory for Eigen.
-    Eigen/include/eigen3 # Windows (for C:/Program Files prefix) < 3.3
-    Eigen3/include/eigen3 ) # Windows (for C:/Program Files prefix) >= 3.3
-
-  # Search supplied hint directories first if supplied.
-  find_path(EIGEN_INCLUDE_DIR
-    NAMES Eigen/Core
-    HINTS ${EIGEN_INCLUDE_DIR_HINTS}
-    PATHS ${EIGEN_CHECK_INCLUDE_DIRS}
-    PATH_SUFFIXES ${EIGEN_CHECK_PATH_SUFFIXES})
-
-  if (NOT EIGEN_INCLUDE_DIR OR
-      NOT EXISTS ${EIGEN_INCLUDE_DIR})
-    eigen_report_not_found(
-      "Could not find eigen3 include directory, set EIGEN_INCLUDE_DIR to "
-      "path to eigen3 include directory, e.g. /usr/local/include/eigen3.")
-  endif (NOT EIGEN_INCLUDE_DIR OR
-    NOT EXISTS ${EIGEN_INCLUDE_DIR})
-
-  # Mark internally as found, then verify. EIGEN_REPORT_NOT_FOUND() unsets
-  # if called.
-  set(EIGEN_FOUND TRUE)
-endif()
-
-# Extract Eigen version from Eigen/src/Core/util/Macros.h
-if (EIGEN_INCLUDE_DIR)
-  set(EIGEN_VERSION_FILE ${EIGEN_INCLUDE_DIR}/Eigen/src/Core/util/Macros.h)
-  if (NOT EXISTS ${EIGEN_VERSION_FILE})
-    eigen_report_not_found(
-      "Could not find file: ${EIGEN_VERSION_FILE} "
-      "containing version information in Eigen install located at: "
-      "${EIGEN_INCLUDE_DIR}.")
-  else (NOT EXISTS ${EIGEN_VERSION_FILE})
-    file(READ ${EIGEN_VERSION_FILE} EIGEN_VERSION_FILE_CONTENTS)
-
-    string(REGEX MATCH "#define EIGEN_WORLD_VERSION [0-9]+"
-      EIGEN_WORLD_VERSION "${EIGEN_VERSION_FILE_CONTENTS}")
-    string(REGEX REPLACE "#define EIGEN_WORLD_VERSION ([0-9]+)" "\\1"
-      EIGEN_WORLD_VERSION "${EIGEN_WORLD_VERSION}")
-
-    string(REGEX MATCH "#define EIGEN_MAJOR_VERSION [0-9]+"
-      EIGEN_MAJOR_VERSION "${EIGEN_VERSION_FILE_CONTENTS}")
-    string(REGEX REPLACE "#define EIGEN_MAJOR_VERSION ([0-9]+)" "\\1"
-      EIGEN_MAJOR_VERSION "${EIGEN_MAJOR_VERSION}")
-
-    string(REGEX MATCH "#define EIGEN_MINOR_VERSION [0-9]+"
-      EIGEN_MINOR_VERSION "${EIGEN_VERSION_FILE_CONTENTS}")
-    string(REGEX REPLACE "#define EIGEN_MINOR_VERSION ([0-9]+)" "\\1"
-      EIGEN_MINOR_VERSION "${EIGEN_MINOR_VERSION}")
-
-    # This is on a single line s/t CMake does not interpret it as a list of
-    # elements and insert ';' separators which would result in 3.;2.;0 nonsense.
-    set(EIGEN_VERSION "${EIGEN_WORLD_VERSION}.${EIGEN_MAJOR_VERSION}.${EIGEN_MINOR_VERSION}")
-  endif (NOT EXISTS ${EIGEN_VERSION_FILE})
-endif (EIGEN_INCLUDE_DIR)
-
-# Set standard CMake FindPackage variables if found.
-if (EIGEN_FOUND)
-  set(EIGEN_INCLUDE_DIRS ${EIGEN_INCLUDE_DIR})
-endif (EIGEN_FOUND)
-
-# Handle REQUIRED / QUIET optional arguments and version.
-include(FindPackageHandleStandardArgs)
-find_package_handle_standard_args(Eigen
-  REQUIRED_VARS EIGEN_INCLUDE_DIRS
-  VERSION_VAR EIGEN_VERSION)
-
-# Only mark internal variables as advanced if we found Eigen, otherwise
-# leave it visible in the standard GUI for the user to set manually.
-if (EIGEN_FOUND)
-  mark_as_advanced(FORCE EIGEN_INCLUDE_DIR
-    Eigen3_DIR) # Autogenerated by find_package(Eigen3)
-endif (EIGEN_FOUND)
\ No newline at end of file
diff --git a/include/core/ceres_wrapper/solver_ceres.h b/include/core/ceres_wrapper/solver_ceres.h
index 62e2ab1a2da15ecec5ea372150a3c83c8b6b3bea..0673207bc47e1873084e3f4d711628a67b322f3c 100644
--- a/include/core/ceres_wrapper/solver_ceres.h
+++ b/include/core/ceres_wrapper/solver_ceres.h
@@ -106,10 +106,10 @@ class SolverCeres : public SolverManager
 
         std::unique_ptr<ceres::Problem>& getCeresProblem();
 
-        bool computeCovariances(CovarianceBlocksToBeComputed _blocks
+        bool computeCovariancesDerived(CovarianceBlocksToBeComputed _blocks
                                         = CovarianceBlocksToBeComputed::ROBOT_LANDMARKS) override;
 
-        bool computeCovariances(const std::vector<StateBlockPtr>& st_list) override;
+        bool computeCovariancesDerived(const std::vector<StateBlockPtr>& st_list) override;
 
         bool hasConverged() override;
 
diff --git a/include/core/processor/processor_base.h b/include/core/processor/processor_base.h
index c8945c8036ff54dc2e0fe2243b3ec0919c66eba4..e1d222ae405de823e48a8317f9b38ef1e754837a 100644
--- a/include/core/processor/processor_base.h
+++ b/include/core/processor/processor_base.h
@@ -243,6 +243,7 @@ struct ParamsProcessorBase : public ParamsBase
     std::string print() const override
     {
         return    "voting_active: "         + std::to_string(voting_active)         + "\n"
+                + "voting_aux_active: "     + std::to_string(voting_aux_active)     + "\n"
                 + "time_tolerance: "        + std::to_string(time_tolerance)        + "\n"
                 + "apply_loss_function: "   + std::to_string(apply_loss_function)   + "\n";
     }
diff --git a/include/core/processor/processor_diff_drive.h b/include/core/processor/processor_diff_drive.h
index b1917f7080c5ae06e4491ed3626aea7ca80a6601..427e74dc7c259d3abd3147fe5cf13608740d3472 100644
--- a/include/core/processor/processor_diff_drive.h
+++ b/include/core/processor/processor_diff_drive.h
@@ -22,7 +22,7 @@ struct ParamsProcessorDiffDrive : public ParamsProcessorOdom2d
             ParamsProcessorOdom2d(_unique_name, _server)
         {
         }
-        std::string print() const
+        std::string print() const override
         {
             return ParamsProcessorOdom2d::print();
         }
diff --git a/include/core/processor/processor_motion.h b/include/core/processor/processor_motion.h
index 7804114d0d1d96ad8652eb44d5eb4a010dfd8add..c621f61c3ac0c09943a7f58789763c4f14b8e115 100644
--- a/include/core/processor/processor_motion.h
+++ b/include/core/processor/processor_motion.h
@@ -41,7 +41,7 @@ struct ParamsProcessorMotion : public ParamsProcessorBase
           angle_turned    = _server.getParam<double>(prefix + _unique_name       + "/keyframe_vote/angle_turned");
           unmeasured_perturbation_std = _server.getParam<double>(prefix + _unique_name + "/unmeasured_perturbation_std");
         }
-        std::string print() const
+        std::string print() const override
         {
           return ParamsProcessorBase::print() + "\n"
             + "max_time_span: "     + std::to_string(max_time_span)     + "\n"
diff --git a/include/core/processor/processor_odom_2d.h b/include/core/processor/processor_odom_2d.h
index b61b4ba6ea35f9b1719c7658eb376ce3818bf8fb..2ad5b846b59e3c40d98351a0f8b30f75b3f71635 100644
--- a/include/core/processor/processor_odom_2d.h
+++ b/include/core/processor/processor_odom_2d.h
@@ -31,7 +31,7 @@ struct ParamsProcessorOdom2d : public ParamsProcessorMotion
             cov_det = _server.getParam<double>(prefix + _unique_name + "/keyframe_vote/cov_det");
         }
 
-        std::string print() const
+        std::string print() const override
         {
             return ParamsProcessorMotion::print()    + "\n"
             + "cov_det: "   + std::to_string(cov_det)       + "\n";
diff --git a/include/core/processor/processor_odom_3d.h b/include/core/processor/processor_odom_3d.h
index c6ca8dbaf266775b80df30fd9135a0b0bc5754f4..676cb6cd05d51240cad7a5008ca301ccb5af67e3 100644
--- a/include/core/processor/processor_odom_3d.h
+++ b/include/core/processor/processor_odom_3d.h
@@ -27,7 +27,7 @@ struct ParamsProcessorOdom3d : public ParamsProcessorMotion
         {
             //
         }
-        std::string print() const
+        std::string print() const override
         {
             return ParamsProcessorMotion::print();
         }
diff --git a/include/core/processor/processor_tracker.h b/include/core/processor/processor_tracker.h
index 1a05397319a431fc04a8b33642d90ad09edb524f..4d0970d4b4002fa78e1c5566cdeb0165e08ca1f4 100644
--- a/include/core/processor/processor_tracker.h
+++ b/include/core/processor/processor_tracker.h
@@ -27,7 +27,7 @@ struct ParamsProcessorTracker : public ParamsProcessorBase
         min_features_for_keyframe   = _server.getParam<unsigned int>(prefix + _unique_name   + "/min_features_for_keyframe");
         max_new_features            = _server.getParam<int>(prefix + _unique_name            + "/max_new_features");
     }
-    std::string print() const
+    std::string print() const override
     {
         return ParamsProcessorBase::print()                                                 + "\n"
                 + "min_features_for_keyframe: " + std::to_string(min_features_for_keyframe) + "\n"
diff --git a/include/core/solver/solver_manager.h b/include/core/solver/solver_manager.h
index 53fc181cb072d8eaf3e4ca4977dcef25c9ac6198..fb38a8be279f9ed6572c729fdebe1551bb99f4ae 100644
--- a/include/core/solver/solver_manager.h
+++ b/include/core/solver/solver_manager.h
@@ -71,11 +71,19 @@ class SolverManager
         };
 
         // PROFILING
-        unsigned int n_solve_;
-        std::chrono::microseconds acc_duration_manager_;
+        unsigned int n_solve_, n_cov_;
+        std::chrono::microseconds acc_duration_total_;
         std::chrono::microseconds acc_duration_solver_;
-        std::chrono::microseconds max_duration_manager_;
+        std::chrono::microseconds acc_duration_update_;
+        std::chrono::microseconds acc_duration_state_;
+        std::chrono::microseconds acc_duration_cov_;
+
+        std::chrono::microseconds max_duration_total_;
         std::chrono::microseconds max_duration_solver_;
+        std::chrono::microseconds max_duration_update_;
+        std::chrono::microseconds max_duration_state_;
+        std::chrono::microseconds max_duration_cov_;
+
         void printProfiling(std::ostream& stream = std::cout) const;
 
     protected:
@@ -105,9 +113,13 @@ class SolverManager
          */
         std::string solve(const ReportVerbosity report_level);
 
-        virtual bool computeCovariances(const CovarianceBlocksToBeComputed blocks) = 0;
+        virtual bool computeCovariances(const CovarianceBlocksToBeComputed blocks) final;
+
+        virtual bool computeCovariances(const std::vector<StateBlockPtr>& st_list) final;
+
+        virtual bool computeCovariancesDerived(const CovarianceBlocksToBeComputed blocks) = 0;
 
-        virtual bool computeCovariances(const std::vector<StateBlockPtr>& st_list) = 0;
+        virtual bool computeCovariancesDerived(const std::vector<StateBlockPtr>& st_list) = 0;
 
         virtual bool hasConverged() = 0;
 
diff --git a/src/capture/capture_base.cpp b/src/capture/capture_base.cpp
index 48a01e0035a732b181ebbcbb2f13e0dd3892793f..e1e961ea4712973a5cd7eaa18ff6c401fc355fcd 100644
--- a/src/capture/capture_base.cpp
+++ b/src/capture/capture_base.cpp
@@ -226,9 +226,9 @@ void CaptureBase::setProblem(ProblemPtr _problem)
 
     // update SensorBase::last_capture_
     if (getSensor() and
-        getSensor()->getLastCapture() and
-        getSensor()->getLastCapture()->getTimeStamp() < time_stamp_)
-        getSensor()->setLastCapture(shared_from_this());
+        (not getSensor()->getLastCapture() or
+         getSensor()->getLastCapture()->getTimeStamp() < time_stamp_))
+         getSensor()->setLastCapture(shared_from_this());
 
     for (auto ft : feature_list_)
         ft->setProblem(_problem);
diff --git a/src/ceres_wrapper/solver_ceres.cpp b/src/ceres_wrapper/solver_ceres.cpp
index 56be64659f659b754b9b9c98df7e22bc44e37e54..67b009f243ace9d81664709c375a2008351cb73f 100644
--- a/src/ceres_wrapper/solver_ceres.cpp
+++ b/src/ceres_wrapper/solver_ceres.cpp
@@ -82,7 +82,7 @@ std::string SolverCeres::solveDerived(const ReportVerbosity report_level)
     return report;
 }
 
-bool SolverCeres::computeCovariances(const CovarianceBlocksToBeComputed _blocks)
+bool SolverCeres::computeCovariancesDerived(const CovarianceBlocksToBeComputed _blocks)
 {   
     // update problem
     update();
@@ -289,7 +289,7 @@ bool SolverCeres::computeCovariances(const CovarianceBlocksToBeComputed _blocks)
     return false;
 }
 
-bool SolverCeres::computeCovariances(const std::vector<StateBlockPtr>& st_list)
+bool SolverCeres::computeCovariancesDerived(const std::vector<StateBlockPtr>& st_list)
 {
     //std::cout << "SolverCeres: computing covariances..." << std::endl;
 
diff --git a/src/sensor/sensor_base.cpp b/src/sensor/sensor_base.cpp
index 9e7adfd0b99009c41345826e6a3b8ad866e70957..0e2a89c12e888750073376f992353b5dd8abce96 100644
--- a/src/sensor/sensor_base.cpp
+++ b/src/sensor/sensor_base.cpp
@@ -555,8 +555,16 @@ CheckLog SensorBase::localCheck(bool _verbose, SensorBasePtr _sen_ptr, std::ostr
         // check last_capture_
         if (getProblem()->getTimeStamp().ok())
         {
-            inconsistency_explanation << "SensorBase::last_capture_ is not the actual last capture\n";
-            log.assertTrue((last_capture_ != findLastCaptureBefore(getProblem()->getTimeStamp())), inconsistency_explanation);
+            auto last_capture_found = findLastCaptureBefore(getProblem()->getTimeStamp());
+            inconsistency_explanation << "SensorBase::last_capture_: "
+                                      << (last_capture_ ? std::to_string(last_capture_->id()) : "-")
+                                      << " @ " << last_capture_
+                                      << " is not the actual last capture: "
+                                      << (last_capture_found ?
+                                          std::to_string(last_capture_found->id()) :
+                                          "-")
+                                      << " @ " << last_capture_found << std::endl;
+            log.assertTrue(last_capture_ == last_capture_found, inconsistency_explanation);
         }
         return log;
 }
diff --git a/src/solver/solver_manager.cpp b/src/solver/solver_manager.cpp
index ba07f02ef4dd7d1e3def7a3f819cd0fb2a512514..99dfb8729c9bc5cb05f00758494571de9d52fc23 100644
--- a/src/solver/solver_manager.cpp
+++ b/src/solver/solver_manager.cpp
@@ -13,10 +13,17 @@ SolverManager::SolverManager(const ProblemPtr& _problem) :
 SolverManager::SolverManager(const ProblemPtr& _problem,
                              const ParamsSolverPtr& _params) :
           n_solve_(0),
-          acc_duration_manager_(0),
+          n_cov_(0),
+          acc_duration_total_(0),
           acc_duration_solver_(0),
-          max_duration_manager_(0),
+          acc_duration_update_(0),
+          acc_duration_state_(0),
+          acc_duration_cov_(0),
+          max_duration_total_(0),
           max_duration_solver_(0),
+          max_duration_update_(0),
+          max_duration_state_(0),
+          max_duration_cov_(0),
           wolf_problem_(_problem),
           params_(_params)
 {
@@ -137,11 +144,15 @@ std::string SolverManager::solve()
 
 std::string SolverManager::solve(const ReportVerbosity report_level)
 {
-    auto start_manager = std::chrono::high_resolution_clock::now();
+    auto start_total = std::chrono::high_resolution_clock::now();
     n_solve_++;
 
     // update problem
+    auto start_update = std::chrono::high_resolution_clock::now();
     update();
+    auto duration_update = std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::high_resolution_clock::now() - start_update);
+    acc_duration_update_ += duration_update;
+    max_duration_update_ = std::max(max_duration_update_,duration_update);
 
     // call derived solver
     auto start_solver = std::chrono::high_resolution_clock::now();
@@ -151,6 +162,7 @@ std::string SolverManager::solve(const ReportVerbosity report_level)
     max_duration_solver_ = std::max(max_duration_solver_,duration_solver);
 
     // update StateBlocks with optimized state value.
+    auto start_state = std::chrono::high_resolution_clock::now();
     /// @todo whatif someone has changed the state notification during opti ??
     /// JV: I do not see a problem here, the solver provides the optimal state given the factors, if someone changed the state during optimization, it will be overwritten by the optimal one.
 
@@ -162,14 +174,46 @@ std::string SolverManager::solve(const ReportVerbosity report_level)
         if (!stateblock_statevector.first->isFixed())
             stateblock_statevector.first->setState(stateblock_statevector.second, false); // false = do not raise the flag state_updated_
     }
+    auto duration_state = std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::high_resolution_clock::now() - start_state);
+    acc_duration_state_ += duration_state;
+    max_duration_state_ = std::max(max_duration_state_,duration_state);
 
-    auto duration_manager = std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::high_resolution_clock::now() - start_manager);
-    acc_duration_manager_ += duration_manager;
-    max_duration_manager_ = std::max(max_duration_manager_,duration_manager);
+    auto duration_total = std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::high_resolution_clock::now() - start_total);
+    acc_duration_total_ += duration_total;
+    max_duration_total_ = std::max(max_duration_total_,duration_total);
 
     return report;
 }
 
+
+bool SolverManager::computeCovariances(const CovarianceBlocksToBeComputed blocks)
+{
+    auto start_cov = std::chrono::high_resolution_clock::now();
+    n_cov_++;
+
+    auto ret = computeCovariancesDerived(blocks);
+
+    auto duration_cov = std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::high_resolution_clock::now() - start_cov);
+    acc_duration_cov_ += duration_cov;
+    max_duration_cov_ = std::max(max_duration_cov_,duration_cov);
+
+    return ret;
+}
+
+bool SolverManager::computeCovariances(const std::vector<StateBlockPtr>& st_list)
+{
+    auto start_cov = std::chrono::high_resolution_clock::now();
+    n_cov_++;
+
+    auto ret = computeCovariancesDerived(st_list);
+
+    auto duration_cov = std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::high_resolution_clock::now() - start_cov);
+    acc_duration_cov_ += duration_cov;
+    max_duration_cov_ = std::max(max_duration_cov_,duration_cov);
+
+    return ret;
+}
+
 void SolverManager::addFactor(const FactorBasePtr& fac_ptr)
 {
     // Warning if adding an already added
@@ -564,15 +608,24 @@ bool SolverManager::check(std::string prefix) const
 void SolverManager::printProfiling(std::ostream& _stream) const
 {
     _stream <<"\nSolverManager:"
-            << "\n\ttotal time:           " << 1e-6*(acc_duration_manager_ + acc_duration_solver_).count() << " s"
-            << "\n\tmanager time:         " << 1e-6*acc_duration_manager_.count() << " s"
-            << "\n\tsolver time:          " << 1e-6*acc_duration_solver_.count() << " s"
-            << "\n\texecutions:           " << n_solve_
-            << "\n\taverage time:         " << 1e-3*(acc_duration_manager_ + acc_duration_solver_).count() / n_solve_ << " ms"
-            << "\n\taverage manager time: " << 1e-3*acc_duration_manager_.count() / n_solve_ << " ms"
-            << "\n\taverage solver time:  " << 1e-3*acc_duration_solver_.count() / n_solve_ << " ms"
-            << "\n\tmax manager time:     " << 1e-3*max_duration_manager_.count() << " ms"
-            << "\n\tmax solver time:      " << 1e-3*max_duration_solver_.count() << " ms" << std::endl;
+            <<"\nTotal values:"
+            << "\n\tSolving state:          " << 1e-6*acc_duration_total_.count() << " s - executions: " << n_solve_
+            << "\n\t\tUpdate problem:   " << 1e-6*acc_duration_update_.count() << " s" << " (" << 100.0 * acc_duration_update_.count() / acc_duration_total_.count() << " %)"
+            << "\n\t\tSolver:           " << 1e-6*acc_duration_solver_.count() << " s" << " (" << 100.0 * acc_duration_solver_.count() / acc_duration_total_.count() << " %)"
+            << "\n\t\tUpdate state:     " << 1e-6*acc_duration_state_.count() << " s" << " (" << 100.0 * acc_duration_state_.count() / acc_duration_total_.count() << " %)"
+            << "\n\tSolving covariance:     " << 1e-6*acc_duration_cov_.count() << " s - executions: " << n_cov_
+            <<"\nAverage:"
+            << "\n\tSolving state:          " << 1e-3*acc_duration_total_.count() / n_solve_ << " ms"
+            << "\n\t\tUpdate problem:   " << 1e-3*acc_duration_update_.count() / n_solve_ << " ms"
+            << "\n\t\tSolver:           " << 1e-3*acc_duration_solver_.count() / n_solve_ << " ms"
+            << "\n\t\tUpdate state:     " << 1e-3*acc_duration_state_.count() / n_solve_ << " ms"
+            << "\n\tSolving covariance:     " << 1e-3*acc_duration_cov_.count() / n_cov_ << " ms"
+            <<"\nMax values:"
+            << "\n\tSolving state:          " << 1e-3*max_duration_total_.count() << " ms"
+            << "\n\t\tUpdate problem:   " << 1e-3*max_duration_update_.count() << " ms"
+            << "\n\t\tSolver:           " << 1e-3*max_duration_solver_.count() << " ms"
+            << "\n\t\tUpdate state:     " << 1e-3*max_duration_state_.count() << " ms"
+            << "\n\tSolving covariance:     " << 1e-3*max_duration_cov_.count() << " ms" << std::endl;
 
 }
 
diff --git a/test/dummy/solver_manager_dummy.h b/test/dummy/solver_manager_dummy.h
index 885f955d217b5a6afcd8084ab5d951af26e5b374..aab56cd1ab532a6a7d3e4b876f2de6dea0ea1b7b 100644
--- a/test/dummy/solver_manager_dummy.h
+++ b/test/dummy/solver_manager_dummy.h
@@ -51,8 +51,8 @@ class SolverManagerDummy : public SolverManager
             return factors_derived_.size();
         };
 
-        bool computeCovariances(const CovarianceBlocksToBeComputed blocks) override {return false;};
-        bool computeCovariances(const std::vector<StateBlockPtr>& st_list) override {return false;};
+        bool computeCovariancesDerived(const CovarianceBlocksToBeComputed blocks) override {return false;};
+        bool computeCovariancesDerived(const std::vector<StateBlockPtr>& st_list) override {return false;};
 
 
         // The following are dummy implementations