diff --git a/CMakeLists.txt b/CMakeLists.txt
index 5957a50..829a268 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -473,30 +473,7 @@ IF(WITH_DC1394)
 ENDIF(WITH_DC1394)
 
 IF(WITH_G2O)
-    FIND_PACKAGE(g2o NO_MODULE)
-    IF(g2o_FOUND)
-       MESSAGE(STATUS "Found g2o (targets)")
-         SET(G2O_FOUND ${g2o_FOUND})
-         get_target_property(G2O_INCLUDES g2o::core INTERFACE_INCLUDE_DIRECTORIES)
-         MESSAGE(STATUS "g2o include dir: ${G2O_INCLUDES}")
-         FIND_FILE(G2O_FACTORY_FILE g2o/core/factory.h 
-         PATHS ${G2O_INCLUDES}
-         NO_DEFAULT_PATH)
-       FILE(READ ${G2O_FACTORY_FILE} TMPTXT)
-       STRING(FIND "${TMPTXT}" "shared_ptr" matchres)
-       IF(${matchres} EQUAL -1)
-           MESSAGE(STATUS "Old g2o factory version detected without shared ptr (factory file: ${G2O_FACTORY_FILE}).")
-           SET(G2O_CPP11 2)
-       ELSE()
-           MESSAGE(STATUS "Latest g2o factory version detected with shared ptr (factory file: ${G2O_FACTORY_FILE}).")
-           SET(G2O_CPP11 1)
-       ENDIF()
-    ELSE()
-        FIND_PACKAGE(G2O QUIET)
-        IF(G2O_FOUND)
-            MESSAGE(STATUS "Found g2o: ${G2O_INCLUDE_DIRS}")
-        ENDIF(G2O_FOUND)
-    ENDIF()
+    find_package(g2o CONFIG REQUIRED)
 ENDIF(WITH_G2O)
 
 IF(WITH_GTSAM)
diff --git a/corelib/src/CMakeLists.txt b/corelib/src/CMakeLists.txt
index c85acc1..d071f79 100644
--- a/corelib/src/CMakeLists.txt
+++ b/corelib/src/CMakeLists.txt
@@ -392,12 +392,10 @@ ENDIF(WITH_TORO)
 IF(G2O_FOUND)
 	IF(g2o_FOUND)
 	  SET(LIBRARIES ${LIBRARIES}
-	     g2o::core
-	     g2o::solver_eigen
-	     g2o::solver_pcg
-	     g2o::types_slam2d
-	     g2o::types_slam3d
-	     g2o::types_sba)
+	     g2o::core 
+         g2o::stuff 
+         g2o::types_icp 
+         g2o::types_sba)
 	  IF(TARGET g2o::solver_csparse)
 	    SET(LIBRARIES ${LIBRARIES}
           g2o::solver_csparse