README.md

@@ -74,16 +74,24 @@ libgflags.a will be installed at **/usr/local/lib**
     
 -   Build and install with:
 
-        $ cd glog
-        $ ./configure --with-gflags=/usr/local/
-        $ make
-        $ sudo make install
-    
+        $ ./autogen.sh
+        $ ./configure --with-gflags=/usr/local/
+        $ make
+        $ sudo make install
+
 libglog.so will be installed at **/usr/local/lib**
 
 -   Tourbleshooting:
 
-    If the `make` command fails with the error: `/bin/bash: aclocal-1.14: command not found`, install Glog with the following commands:
+    * ./autogen.sh fails with './autogen.sh: autoreconf: not found'
+
+    In a fresh installation you will probably need to install autoreconf running
+    
+        $ sudo make install dh-autoreconf 
+
+    * `make` command fails with the error: `/bin/bash: aclocal-1.14: command not found`
+    
+    Install Glog with the following commands:
         
         $ cd glog
         $ sudo apt-get install autoconf
@@ -300,6 +308,21 @@ At this point you might need to switch to the `catkin_build` branch of the wolf
 **(6)**  Run tests:
 
     $ catkin run_tests
+    
+Troubleshooting
+---------------
+
+#### Boost
+
+We have made our best to keep being boost-independent.
+However, in case you run into a boost installation issue at execution time, check that you have boost installed. 
+If needed, install it with:
+
+[Boost](http://www.boost.org/). Free peer-reviewed portable C++ source libraries.
+
+    $ sudo apt-get install libboost-all-dev
+
+
 
 Inspiring Links
 ---------------

cmake_modules/FindGlog.cmake

+# Ceres Solver - A fast non-linear least squares minimizer
+# Copyright 2015 Google Inc. All rights reserved.
+# http://ceres-solver.org/
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions are met:
+#
+# * 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.
+#
+# 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.
+#
+# Author: alexs.mac@gmail.com (Alex Stewart)
+#
+
+# FindGlog.cmake - Find Google glog logging library.
+#
+# This module defines the following variables:
+#
+# GLOG_FOUND: TRUE iff glog is found.
+# GLOG_INCLUDE_DIRS: Include directories for glog.
+# GLOG_LIBRARIES: Libraries required to link glog.
+# FOUND_INSTALLED_GLOG_CMAKE_CONFIGURATION: True iff the version of glog found
+#                                           was built & installed / exported
+#                                           as a CMake package.
+#
+# The following variables control the behaviour of this module:
+#
+# GLOG_PREFER_EXPORTED_GLOG_CMAKE_CONFIGURATION: TRUE/FALSE, iff TRUE then
+#                           then prefer using an exported CMake configuration
+#                           generated by glog > 0.3.4 over searching for the
+#                           glog components manually.  Otherwise (FALSE)
+#                           ignore any exported glog 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 either
+#                           GLOG_INCLUDE_DIR_HINTS or GLOG_LIBRARY_DIR_HINTS
+#                           otherwise FALSE.
+# GLOG_INCLUDE_DIR_HINTS: List of additional directories in which to
+#                         search for glog includes, e.g: /timbuktu/include.
+# GLOG_LIBRARY_DIR_HINTS: List of additional directories in which to
+#                         search for glog libraries, e.g: /timbuktu/lib.
+#
+# 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.
+#
+# GLOG_INCLUDE_DIR: Include directory for glog, not including the
+#                   include directory of any dependencies.
+# GLOG_LIBRARY: glog library, not including the libraries of any
+#               dependencies.
+
+# Reset CALLERS_CMAKE_FIND_LIBRARY_PREFIXES to its value when
+# FindGlog was invoked.
+macro(GLOG_RESET_FIND_LIBRARY_PREFIX)
+  if (MSVC AND CALLERS_CMAKE_FIND_LIBRARY_PREFIXES)
+    set(CMAKE_FIND_LIBRARY_PREFIXES "${CALLERS_CMAKE_FIND_LIBRARY_PREFIXES}")
+  endif()
+endmacro(GLOG_RESET_FIND_LIBRARY_PREFIX)
+
+# Called if we failed to find glog 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(GLOG_REPORT_NOT_FOUND REASON_MSG)
+  unset(GLOG_FOUND)
+  unset(GLOG_INCLUDE_DIRS)
+  unset(GLOG_LIBRARIES)
+  # Make results of search visible in the CMake GUI if glog has not
+  # been found so that user does not have to toggle to advanced view.
+  mark_as_advanced(CLEAR GLOG_INCLUDE_DIR
+                         GLOG_LIBRARY)
+
+  glog_reset_find_library_prefix()
+
+  # Note <package>_FIND_[REQUIRED/QUIETLY] variables defined by FindPackage()
+  # use the camelcase library name, not uppercase.
+  if (Glog_FIND_QUIETLY)
+    message(STATUS "Failed to find glog - " ${REASON_MSG} ${ARGN})
+  elseif (Glog_FIND_REQUIRED)
+    message(FATAL_ERROR "Failed to find glog - " ${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 glog - " ${REASON_MSG} ${ARGN})
+  endif ()
+  return()
+endmacro(GLOG_REPORT_NOT_FOUND)
+
+# Protect against any alternative find_package scripts for this library having
+# been called previously (in a client project) which set GLOG_FOUND, but not
+# the other variables we require / set here which could cause the search logic
+# here to fail.
+unset(GLOG_FOUND)
+
+# -----------------------------------------------------------------
+# By default, if the user has expressed no preference for using an exported
+# glog CMake configuration over performing a search for the installed
+# components, and has not specified any hints for the search locations, then
+# prefer a glog exported configuration if available.
+if (NOT DEFINED GLOG_PREFER_EXPORTED_GLOG_CMAKE_CONFIGURATION
+    AND NOT GLOG_INCLUDE_DIR_HINTS
+    AND NOT GLOG_LIBRARY_DIR_HINTS)
+  message(STATUS "No preference for use of exported glog CMake configuration "
+    "set, and no hints for include/library directories provided. "
+    "Defaulting to preferring an installed/exported glog CMake configuration "
+    "if available.")
+  set(GLOG_PREFER_EXPORTED_GLOG_CMAKE_CONFIGURATION TRUE)
+endif()
+
+if (GLOG_PREFER_EXPORTED_GLOG_CMAKE_CONFIGURATION)
+  # Try to find an exported CMake configuration for glog, as generated by
+  # glog versions > 0.3.4
+  #
+  # 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.
+  #
+  # NOTE: We use the NAMES option as glog erroneously uses 'google-glog' as its
+  #       project name when built with CMake, but exports itself as just 'glog'.
+  #       On Linux/OS X this does not break detection as the project name is
+  #       not used as part of the install path for the CMake package files,
+  #       e.g. /usr/local/lib/cmake/glog, where the <glog> suffix is hardcoded
+  #       in glog's CMakeLists.  However, on Windows the project name *is*
+  #       part of the install prefix: C:/Program Files/google-glog/[include,lib].
+  #       However, by default CMake checks:
+  #       C:/Program Files/<FIND_PACKAGE_ARGUMENT_NAME='glog'> which does not
+  #       exist and thus detection fails.  Thus we use the NAMES to force the
+  #       search to use both google-glog & glog.
+  #
+  # [1] http://www.cmake.org/cmake/help/v2.8.11/cmake.html#command:find_package
+  find_package(glog QUIET
+                    NAMES google-glog glog
+                    NO_MODULE
+                    NO_CMAKE_PACKAGE_REGISTRY
+                    NO_CMAKE_BUILDS_PATH)
+  if (glog_FOUND)
+    message(STATUS "Found installed version of glog: ${glog_DIR}")
+  else()
+    # Failed to find an installed version of glog, repeat search allowing
+    # exported build directories.
+    message(STATUS "Failed to find installed glog CMake configuration, "
+      "searching for glog build directories exported with CMake.")
+    # Again pass NO_CMAKE_BUILDS_PATH, as we know that glog is exported and
+    # do not want to treat projects built with the CMake GUI preferentially.
+    find_package(glog QUIET
+                      NAMES google-glog glog
+                      NO_MODULE
+                      NO_CMAKE_BUILDS_PATH)
+    if (glog_FOUND)
+      message(STATUS "Found exported glog build directory: ${glog_DIR}")
+    endif(glog_FOUND)
+  endif(glog_FOUND)
+
+  set(FOUND_INSTALLED_GLOG_CMAKE_CONFIGURATION ${glog_FOUND})
+
+  if (FOUND_INSTALLED_GLOG_CMAKE_CONFIGURATION)
+    message(STATUS "Detected glog version: ${glog_VERSION}")
+    set(GLOG_FOUND ${glog_FOUND})
+    # glog wraps the include directories into the exported glog::glog target.
+    set(GLOG_INCLUDE_DIR "")
+    set(GLOG_LIBRARY glog::glog)
+  else (FOUND_INSTALLED_GLOG_CMAKE_CONFIGURATION)
+    message(STATUS "Failed to find an installed/exported CMake configuration "
+      "for glog, will perform search for installed glog components.")
+  endif (FOUND_INSTALLED_GLOG_CMAKE_CONFIGURATION)
+endif(GLOG_PREFER_EXPORTED_GLOG_CMAKE_CONFIGURATION)
+
+if (NOT GLOG_FOUND)
+  # Either failed to find an exported glog CMake configuration, or user
+  # told us not to use one.  Perform a manual search for all glog components.
+
+  # Handle possible presence of lib prefix for libraries on MSVC, see
+  # also GLOG_RESET_FIND_LIBRARY_PREFIX().
+  if (MSVC)
+    # Preserve the caller's original values for CMAKE_FIND_LIBRARY_PREFIXES
+    # s/t we can set it back before returning.
+    set(CALLERS_CMAKE_FIND_LIBRARY_PREFIXES "${CMAKE_FIND_LIBRARY_PREFIXES}")
+    # The empty string in this list is important, it represents the case when
+    # the libraries have no prefix (shared libraries / DLLs).
+    set(CMAKE_FIND_LIBRARY_PREFIXES "lib" "" "${CMAKE_FIND_LIBRARY_PREFIXES}")
+  endif (MSVC)
+
+  # Search user-installed locations first, so that we prefer user installs
+  # to system installs where both exist.
+  list(APPEND GLOG_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)
+  # Windows (for C:/Program Files prefix).
+  list(APPEND GLOG_CHECK_PATH_SUFFIXES
+    glog/include
+    glog/Include
+    Glog/include
+    Glog/Include
+    google-glog/include # CMake installs with project name prefix.
+    google-glog/Include)
+
+  list(APPEND GLOG_CHECK_LIBRARY_DIRS
+    /usr/local/lib
+    /usr/local/homebrew/lib # Mac OS X.
+    /opt/local/lib
+    /usr/lib)
+  # Windows (for C:/Program Files prefix).
+  list(APPEND GLOG_CHECK_LIBRARY_SUFFIXES
+    glog/lib
+    glog/Lib
+    Glog/lib
+    Glog/Lib
+    google-glog/lib # CMake installs with project name prefix.
+    google-glog/Lib)
+
+  # Search supplied hint directories first if supplied.
+  find_path(GLOG_INCLUDE_DIR
+    NAMES glog/logging.h
+    HINTS ${GLOG_INCLUDE_DIR_HINTS}
+    PATHS ${GLOG_CHECK_INCLUDE_DIRS}
+    PATH_SUFFIXES ${GLOG_CHECK_PATH_SUFFIXES})
+  if (NOT GLOG_INCLUDE_DIR OR
+      NOT EXISTS ${GLOG_INCLUDE_DIR})
+    glog_report_not_found(
+      "Could not find glog include directory, set GLOG_INCLUDE_DIR "
+      "to directory containing glog/logging.h")
+  endif (NOT GLOG_INCLUDE_DIR OR
+    NOT EXISTS ${GLOG_INCLUDE_DIR})
+
+  find_library(GLOG_LIBRARY NAMES glog
+    HINTS ${GLOG_LIBRARY_DIR_HINTS}
+    PATHS ${GLOG_CHECK_LIBRARY_DIRS}
+    PATH_SUFFIXES ${GLOG_CHECK_LIBRARY_SUFFIXES})
+  if (NOT GLOG_LIBRARY OR
+      NOT EXISTS ${GLOG_LIBRARY})
+    glog_report_not_found(
+      "Could not find glog library, set GLOG_LIBRARY "
+      "to full path to libglog.")
+  endif (NOT GLOG_LIBRARY OR
+    NOT EXISTS ${GLOG_LIBRARY})
+
+  # Mark internally as found, then verify. GLOG_REPORT_NOT_FOUND() unsets
+  # if called.
+  set(GLOG_FOUND TRUE)
+
+  # Glog does not seem to provide any record of the version in its
+  # source tree, thus cannot extract version.
+
+  # Catch case when caller has set GLOG_INCLUDE_DIR in the cache / GUI and
+  # thus FIND_[PATH/LIBRARY] are not called, but specified locations are
+  # invalid, otherwise we would report the library as found.
+  if (GLOG_INCLUDE_DIR AND
+      NOT EXISTS ${GLOG_INCLUDE_DIR}/glog/logging.h)
+    glog_report_not_found(
+      "Caller defined GLOG_INCLUDE_DIR:"
+      " ${GLOG_INCLUDE_DIR} does not contain glog/logging.h header.")
+  endif (GLOG_INCLUDE_DIR AND
+    NOT EXISTS ${GLOG_INCLUDE_DIR}/glog/logging.h)
+  # TODO: This regex for glog library is pretty primitive, we use lowercase
+  #       for comparison to handle Windows using CamelCase library names, could
+  #       this check be better?
+  string(TOLOWER "${GLOG_LIBRARY}" LOWERCASE_GLOG_LIBRARY)
+  if (GLOG_LIBRARY AND
+      NOT "${LOWERCASE_GLOG_LIBRARY}" MATCHES ".*glog[^/]*")
+    glog_report_not_found(
+      "Caller defined GLOG_LIBRARY: "
+      "${GLOG_LIBRARY} does not match glog.")
+  endif (GLOG_LIBRARY AND
+    NOT "${LOWERCASE_GLOG_LIBRARY}" MATCHES ".*glog[^/]*")
+
+  glog_reset_find_library_prefix()
+
+endif(NOT GLOG_FOUND)
+
+# Set standard CMake FindPackage variables if found.
+if (GLOG_FOUND)
+  set(GLOG_INCLUDE_DIRS ${GLOG_INCLUDE_DIR})
+  set(GLOG_LIBRARIES ${GLOG_LIBRARY})
+endif (GLOG_FOUND)
+
+# If we are using an exported CMake glog target, the include directories are
+# wrapped into the target itself, and do not have to be (and are not)
+# separately specified.  In which case, we should not add GLOG_INCLUDE_DIRS
+# to the list of required variables in order that glog be reported as found.
+if (FOUND_INSTALLED_GLOG_CMAKE_CONFIGURATION)
+  set(GLOG_REQUIRED_VARIABLES GLOG_LIBRARIES)
+else()
+  set(GLOG_REQUIRED_VARIABLES GLOG_INCLUDE_DIRS GLOG_LIBRARIES)
+endif()
+
+# Handle REQUIRED / QUIET optional arguments.
+include(FindPackageHandleStandardArgs)
+find_package_handle_standard_args(Glog DEFAULT_MSG
+  ${GLOG_REQUIRED_VARIABLES})
+
+# Only mark internal variables as advanced if we found glog, otherwise
+# leave them visible in the standard GUI for the user to set manually.
+if (GLOG_FOUND)
+  mark_as_advanced(FORCE GLOG_INCLUDE_DIR
+                         GLOG_LIBRARY
+                         glog_DIR) # Autogenerated by find_package(glog)
+endif (GLOG_FOUND)

src/CMakeLists.txt

@@ -29,7 +29,7 @@ ENDIF(BUILD_EXAMPLES OR BUILD_TESTS)
 
 #find dependencies.
 
-FIND_PACKAGE(Eigen 3 REQUIRED)
+FIND_PACKAGE(Eigen 3.2.92 REQUIRED)
 
 FIND_PACKAGE(Threads REQUIRED)
 
@@ -50,16 +50,24 @@ IF(laser_scan_utils_FOUND)
     MESSAGE("laser_scan_utils Library FOUND: laser_scan_utils related sources will be built.")
 ENDIF(laser_scan_utils_FOUND)
 
-FIND_PACKAGE(raw_gps_utils ) #raw_gps_utils is not required
+FIND_PACKAGE(raw_gps_utils QUIET) #raw_gps_utils is not required
 IF(raw_gps_utils_FOUND)
     MESSAGE("raw_gps_utils Library FOUND: raw_gps_utils related sources will be built.")
 ENDIF(raw_gps_utils_FOUND)
 
 # OpenCV
-FIND_PACKAGE(OpenCV 2 QUIET)
-IF(OpenCV_FOUND)
-    MESSAGE("OpenCV Library FOUND: OpenCV related sources will be built.")
-ENDIF(OpenCV_FOUND)
+FIND_PACKAGE(OpenCV QUIET)
+if (OpenCV_FOUND) 
+  if (${OpenCV_VERSION_MAJOR} GREATER 2)
+    message("-- Found OpenCV ${OpenCV_VERSION_MAJOR}.${OpenCV_VERSION_MINOR}. Related sources will be built.")
+    ADD_DEFINITIONS(-DHAVE_OPENCV3)
+    SET(USE_CV true)
+  else(${OpenCV_VERSION_MAJOR} GREATER 2)
+    message("[WARN] Found OpenCV ${OpenCV_VERSION_MAJOR}.${OpenCV_VERSION_MINOR}. Notice that WOLF is prepared to work with OpenCV 3.0 and higher versions.")
+  endif(${OpenCV_VERSION_MAJOR} GREATER 2)
+else(OpenCV_FOUND)
+    message("[WARN] OpenCV not found. Related sources will NOT be built.")
+endif(OpenCV_FOUND)
 
 # YAML with yaml-cpp
 INCLUDE (${PROJECT_SOURCE_DIR}/cmake_modules/FindYamlCpp.cmake)
@@ -71,6 +79,16 @@ ELSEIF(YAMLCPP_FOUND)
     MESSAGE("yaml-cpp Library NOT FOUND!")
 ENDIF(YAMLCPP_FOUND)
 
+#GLOG
+INCLUDE (${PROJECT_SOURCE_DIR}/cmake_modules/FindGlog.cmake)
+IF(GLOG_FOUND)
+    MESSAGE("glog Library FOUND: glog related sources will be built.")
+    MESSAGE(STATUS ${GLOG_INCLUDE_DIR})
+    MESSAGE(STATUS ${GLOG_LIBRARY})
+ELSEIF(GLOG_FOUND)
+    MESSAGE("glog Library NOT FOUND!")
+ENDIF(GLOG_FOUND)
+
 # SuiteSparse doesn't have find*.cmake:
 FIND_PATH(
     Suitesparse_INCLUDE_DIRS
@@ -151,12 +169,17 @@ IF(YAMLCPP_FOUND)
     INCLUDE_DIRECTORIES(${YAMLCPP_INCLUDE_DIR})
 ENDIF(YAMLCPP_FOUND)
 
+IF(GLOG_FOUND)
+INCLUDE_DIRECTORIES(${GLOG_INCLUDE_DIR})
+ENDIF(GLOG_FOUND)
+
 #headers
 SET(HDRS_BASE
     capture_base.h
     constraint_analytic.h
+    constraint_autodiff.h
     constraint_base.h
-    constraint_sparse.h
+    factory.h
     feature_base.h
     feature_match.h
     frame_base.h
@@ -173,6 +196,7 @@ SET(HDRS_BASE
     pinholeTools.h
     problem.h
     processor_base.h
+    processor_capture_holder.h
     processor_factory.h
     processor_motion.h
     processor_tracker.h
@@ -198,9 +222,11 @@ SET(HDRS
     capture_void.h
     constraint_container.h
     constraint_corner_2D.h
+    constraint_AHP.h
     constraint_epipolar.h
     constraint_imu.h
     constraint_fix.h
+    constraint_fix_bias.h
     constraint_fix_3D.h
     constraint_gps_2D.h
     constraint_gps_pseudorange_3D.h
@@ -265,6 +291,7 @@ SET(SRCS_BASE
     node_base.cpp
     problem.cpp
     processor_base.cpp
+    processor_capture_holder.cpp
     processor_motion.cpp
     processor_tracker.cpp
     processor_tracker_feature.cpp
@@ -316,20 +343,18 @@ SET(SRCS_DTASSC
 #optional HDRS and SRCS
 IF (Ceres_FOUND)
     SET(HDRS_WRAPPER
-        ceres_wrapper/auto_diff_cost_function_wrapper.h
+        ceres_wrapper/sparse_utils.h
+        ceres_wrapper/solver_manager.h
         ceres_wrapper/ceres_manager.h
+        #ceres_wrapper/qr_manager.h
         ceres_wrapper/cost_function_wrapper.h
-        ceres_wrapper/create_auto_diff_cost_function.h
-        ceres_wrapper/create_auto_diff_cost_function_ceres.h
-        ceres_wrapper/create_auto_diff_cost_function_wrapper.h
         ceres_wrapper/create_numeric_diff_cost_function.h
-        ceres_wrapper/create_numeric_diff_cost_function_ceres.h
         ceres_wrapper/local_parametrization_wrapper.h 
         )
     SET(SRCS_WRAPPER
+        ceres_wrapper/solver_manager.cpp
         ceres_wrapper/ceres_manager.cpp
-        ceres_wrapper/create_auto_diff_cost_function.cpp
-        ceres_wrapper/create_numeric_diff_cost_function.cpp
+        #ceres_wrapper/qr_manager.cpp
         ceres_wrapper/local_parametrization_wrapper.cpp 
         )
 ELSE(Ceres_FOUND)
@@ -402,8 +427,10 @@ IF(YAMLCPP_FOUND)
     # sources
     SET(SRCS ${SRCS}
         yaml/processor_odom_3D_yaml.cpp
+        yaml/processor_imu_yaml.cpp
         yaml/sensor_camera_yaml.cpp
         yaml/sensor_odom_3D_yaml.cpp
+        yaml/sensor_imu_yaml.cpp
         )
     IF(laser_scan_utils_FOUND)
         SET(SRCS ${SRCS}
@@ -412,7 +439,7 @@ IF(YAMLCPP_FOUND)
     ENDIF(laser_scan_utils_FOUND)
     IF(OpenCV_FOUND)
         SET(SRCS ${SRCS}
-            yaml/processor_image_yaml.cpp
+        	yaml/processor_image_yaml.cpp
             )
     ENDIF(OpenCV_FOUND)
 ENDIF(YAMLCPP_FOUND)
@@ -451,6 +478,9 @@ IF (YAMLCPP_FOUND)
     TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${YAMLCPP_LIBRARY})
 ENDIF (YAMLCPP_FOUND)
 
+IF (GLOG_FOUND)
+    TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${GLOG_LIBRARY})
+ENDIF (GLOG_FOUND)
 
 #install library
 #=============================================================
@@ -492,13 +522,14 @@ export(PACKAGE ${PROJECT_NAME})
 #############
 ## Testing ##
 #############
-
-if(BUILD_TESTS)
+IF (GLOG_FOUND)
+    if(BUILD_TESTS)
     
-    MESSAGE("Building tests.")
-    add_subdirectory(test)
+        MESSAGE("Building tests.")
+        add_subdirectory(test)
 
-endif()
+    endif(BUILD_TESTS)
+ENDIF (GLOG_FOUND)
 
 IF(BUILD_EXAMPLES)
   #Build examples & tests

src/capture_buffer.h

+/**
+ * \file capture_buffer.h
+ *
+ *  Created on: Jul 12, 2017
+ *  \author: Jeremie Deray
+ */
+
+#ifndef _WOLF_CAPTURE_BUFFER_H_
+#define _WOLF_CAPTURE_BUFFER_H_
+
+#include "wolf.h"
+#include "time_stamp.h"
+
+#include <list>
+#include <algorithm>
+
+namespace wolf {
+
+//using namespace Eigen;
+
+enum class CaptureBufferPolicy : std::size_t
+{
+  TIME = 0,
+  NUM_CAPTURES,
+  ALL
+};
+
+/** \brief class for capture buffers.
+ *
+ * It consists of a buffer of pre-integrated motions (aka. delta-integrals) that is being filled
+ * by the motion processors (deriving from ProcessorMotion).
+ * Each delta-integral is accompanied by a time stamp, a Jacobian and a covariances matrix.
+ *
+ * This buffer contains the time stamp and delta-integrals:
+ *  - since the last key-Frame
+ *  - until the frame of this capture.
+ *
+ * The buffer can be queried for motion-integrals and delta-integrals corresponding to a provided time stamp,
+ * with the following rules:
+ *   - If the query time stamp is later than the last one in the buffer,
+ *     the last motion-integral or delta-integral is returned.
+ *   - If the query time stamp is earlier than the beginning of the buffer,
+ *     the earliest Motion or Delta is returned.
+ *   - If the query time stamp matches one time stamp in the buffer exactly,
+ *     the returned motion-integral or delta-integral is the one of the queried time stamp.
+ *   - If the query time stamp does not match any time stamp in the buffer,
+ *     the returned motion-integral or delta-integral is the one immediately before the query time stamp.
+ */
+
+//template <CaptureBufferPolicy policy>
+class CaptureBuffer
+{
+public:
+
+  CaptureBuffer(const Scalar _buffer_dt, const int _max_capture = -1);
+  ~CaptureBuffer() = default;
+
+  void push_back(const CaptureBasePtr& capture);
+
+//  std::list<CaptureBasePtr>& get();
+//  const std::list<CaptureBasePtr>& get() const;
+
+  const CaptureBasePtr& getCapture(const TimeStamp& _ts) const;
+  void getCapture(const TimeStamp& _ts, CaptureBasePtr& _motion) const;
+
+  void remove(const CaptureBasePtr& capture);
+
+  void clear();
+//  void print();
+
+  const TimeStamp& earliest() const;
+  const TimeStamp& latest() const;
+
+//private:
+
+  int max_capture_;
+  Scalar buffer_dt_;
+
+  std::list<CaptureBasePtr> container_;
+};
+
+
+CaptureBuffer::CaptureBuffer(const Scalar _buffer_dt, const int _max_capture) :
+  max_capture_(_max_capture), buffer_dt_(_buffer_dt)
+{
+  //
+}
+
+void CaptureBuffer::push_back(const CaptureBasePtr& capture)
+{
+  container_.push_back(capture);
+
+  WOLF_DEBUG("Buffer dt : ", container_.back()->getTimeStamp() -
+             container_.front()->getTimeStamp(), " vs ", buffer_dt_);
+
+  while (container_.back()->getTimeStamp() -
+         container_.front()->getTimeStamp() > buffer_dt_)
+  {
+    container_.pop_front();
+  }
+}
+
+const CaptureBasePtr& CaptureBuffer::getCapture(const TimeStamp& _ts) const
+{
+  //assert((container_.front().ts_ <= _ts) && "Query time stamp out of buffer bounds");
+  auto previous = std::find_if(container_.rbegin(), container_.rend(),
+  [&](const CaptureBasePtr& c)
+  {
+    return c->getTimeStamp() <= _ts;
+  });
+
+  if (previous == container_.rend())
+    // The time stamp is older than the buffer's oldest data.
+    // We could do something here, and throw an error or something, but by now we'll return the first valid data
+    previous--;
+
+  return *previous;
+}
+
+void CaptureBuffer::getCapture(const TimeStamp& _ts, CaptureBasePtr& _capture) const
+{
+//  //assert((container_.front().ts_ <= _ts) && "Query time stamp out of buffer bounds");
+//  auto previous = std::find_if(container_.rbegin(), container_.rend(),
+//  [&](const Motion& m)
+//  {
+//    return c->getTimeStamp() <= _ts;
+//  });
+
+//  if (previous == container_.rend())
+//    // The time stamp is older than the buffer's oldest data.
+//    // We could do something here, but by now we'll return the last valid data
+//    previous--;
+
+//  _capture = *previous;
+
+  _capture = getCapture(_ts);
+}
+
+//inline std::list<CaptureBasePtr>& CaptureBuffer::get()
+//{
+//  return container_;
+//}
+
+//inline const std::list<CaptureBasePtr>& CaptureBuffer::get() const
+//{
+//  return container_;
+//}
+
+inline void CaptureBuffer::remove(const CaptureBasePtr& capture)
+{
+  container_.remove(capture);
+}
+
+inline void CaptureBuffer::clear()
+{
+  return container_.clear();
+}
+
+inline const TimeStamp& CaptureBuffer::earliest() const
+{
+  return (!container_.empty())? container_.front()->getTimeStamp() :
+                                InvalidStamp;
+}
+
+inline const TimeStamp& CaptureBuffer::latest() const
+{
+  return (!container_.empty())? container_.back()->getTimeStamp() :
+                                InvalidStamp;
+}
+
+} // namespace wolf
+
+#endif /* _WOLF_CAPTURE_BUFFER_H_ */

src/capture_imu.cpp

 #include "capture_imu.h"
+#include "sensor_imu.h"
 
 namespace wolf {
 
 
-CaptureIMU::CaptureIMU(const TimeStamp& _init_ts, SensorBasePtr _sensor_ptr,
-                             const Eigen::Vector6s& _acc_gyro_data) :
-        CaptureMotion(_init_ts, _sensor_ptr, _acc_gyro_data, 10, 9 )
+CaptureIMU::CaptureIMU(const TimeStamp& _init_ts, SensorBasePtr _sensor_ptr, const Eigen::Vector6s& _acc_gyro_data, 
+                        FrameBasePtr _origin_frame_ptr) :
+        CaptureMotion(_init_ts, _sensor_ptr, _acc_gyro_data, 6, 10, 9, 6, _origin_frame_ptr)
 {
     setType("IMU");
+
+    SensorIMUPtr imu_ptr = std::static_pointer_cast<SensorIMU>(_sensor_ptr);
+
+    Scalar acc_var  = imu_ptr->getAccelNoise() * imu_ptr->getAccelNoise();
+    Scalar gyro_var = imu_ptr->getGyroNoise()  * imu_ptr->getGyroNoise();
+
+    Vector6s data_vars; data_vars << acc_var, acc_var, acc_var, gyro_var, gyro_var, gyro_var;
+
+    Matrix6s data_cov; data_cov.setZero();
+
+    data_cov.diagonal() = data_vars;
+
+    setDataCovariance(data_cov);
+
 //    std::cout << "constructed    +C-IMU" << id() << std::endl;
 }
 
+CaptureIMU::CaptureIMU(const TimeStamp& _init_ts, SensorBasePtr _sensor_ptr, const Eigen::Vector6s& _acc_gyro_data, 
+                       const Eigen::MatrixXs& _data_cov, FrameBasePtr _origin_frame_ptr) :
+        CaptureMotion(_init_ts, _sensor_ptr, _acc_gyro_data, _data_cov, 6, 10, 9, 6, _origin_frame_ptr)
+{
+    setType("IMU");
+//    std::cout << "constructed    +C-IMU" << id() << std::endl;
+}
+
+
 CaptureIMU::~CaptureIMU()
 {
 //    std::cout << "destructed     -C-IMU" << id() << std::endl;

src/capture_imu.h

@@ -12,7 +12,12 @@ class CaptureIMU : public CaptureMotion
 {
     public:
 
-        CaptureIMU(const TimeStamp& _init_ts, SensorBasePtr _sensor_ptr, const Eigen::Vector6s& _data);
+        CaptureIMU(const TimeStamp& _init_ts, SensorBasePtr _sensor_ptr, const Eigen::Vector6s& _data, 
+                    FrameBasePtr _origin_frame_ptr = nullptr);
+
+        CaptureIMU(const TimeStamp& _init_ts, SensorBasePtr _sensor_ptr, const Eigen::Vector6s& _data, 
+                   const Eigen::MatrixXs& _data_cov, FrameBasePtr _origin_frame_ptr = nullptr);
+
         virtual ~CaptureIMU();
 
 };

src/capture_motion.h

@@ -48,15 +48,12 @@ class CaptureMotion : public CaptureBase
     public:
         CaptureMotion(const TimeStamp& _ts, SensorBasePtr _sensor_ptr,
                       const Eigen::VectorXs& _data,
-                      Size _delta_size, Size _delta_cov_size,
+                      Size _data_size, Size _delta_size, Size _cov_size, Size _calib_size,
                       FrameBasePtr _origin_frame_ptr = nullptr);
 
-//        CaptureMotion(const TimeStamp& _ts, SensorBasePtr _sensor_ptr, const Eigen::VectorXs& _data,
-//                      const Eigen::VectorXs& _data_sigmas, FrameBasePtr _origin_frame_ptr = nullptr);
-
         CaptureMotion(const TimeStamp& _ts, SensorBasePtr _sensor_ptr,
                       const Eigen::VectorXs& _data, const Eigen::MatrixXs& _data_cov,
-                      Size _delta_size, Size _delta_cov_size,
+                      Size _data_size, Size _delta_size, Size _cov_size, Size _calib_size,
                       FrameBasePtr _origin_frame_ptr = nullptr);
 
         virtual ~CaptureMotion();
@@ -81,27 +78,30 @@ class CaptureMotion : public CaptureBase
         FrameBasePtr origin_frame_ptr_; ///< Pointer to the origin frame of the motion
 };
 
-inline CaptureMotion::CaptureMotion(const TimeStamp& _ts, SensorBasePtr _sensor_ptr,
+inline CaptureMotion::CaptureMotion(const TimeStamp& _ts,
+                                    SensorBasePtr _sensor_ptr,
                                     const Eigen::VectorXs& _data,
-                                    Size _delta_size, Size _delta_cov_size,
+                                    Size _data_size, Size _delta_size, Size _delta_cov_size, Size _calib_size,
                                     FrameBasePtr _origin_frame_ptr) :
         CaptureBase("MOTION", _ts, _sensor_ptr),
         data_(_data),
-        data_cov_(Eigen::MatrixXs::Identity(_data.rows(), _data.rows())),
-        buffer_(_delta_size,_delta_cov_size),
+        data_cov_(Eigen::MatrixXs::Zero(_data.rows(), _data.rows())), // Someone should test this zero and do something smart accordingly
+        buffer_(_data_size, _delta_size,_delta_cov_size, _calib_size),
         origin_frame_ptr_(_origin_frame_ptr)
 {
     //
 }
 
-inline CaptureMotion::CaptureMotion(const TimeStamp& _ts, SensorBasePtr _sensor_ptr,
-                                    const Eigen::VectorXs& _data, const Eigen::MatrixXs& _data_cov,
-                                    Size _delta_size, Size _delta_cov_size,
+inline CaptureMotion::CaptureMotion(const TimeStamp& _ts,
+                                    SensorBasePtr _sensor_ptr,
+                                    const Eigen::VectorXs& _data,
+                                    const Eigen::MatrixXs& _data_cov,
+                                    Size _data_size, Size _delta_size, Size _delta_cov_size, Size _calib_size,
                                     FrameBasePtr _origin_frame_ptr) :
         CaptureBase("MOTION", _ts, _sensor_ptr),
         data_(_data),
         data_cov_(_data_cov),
-        buffer_(_delta_size,_delta_cov_size),
+        buffer_(_data_size, _delta_size,_delta_cov_size, _calib_size),
         origin_frame_ptr_(_origin_frame_ptr)
 {
     //

src/ceres_wrapper/ceres_manager.h

@@ -7,84 +7,71 @@
 #include "glog/logging.h"
 
 //wolf includes
+#include "solver_manager.h"
 #include "cost_function_wrapper.h"
 #include "local_parametrization_wrapper.h"
-#include "../wolf.h"
-#include "../state_block.h"
-#include "create_auto_diff_cost_function.h"
 #include "create_numeric_diff_cost_function.h"
 
-namespace wolf {
+namespace ceres {
+    typedef std::shared_ptr<CostFunction>  CostFunctionPtr;
+}
 
+namespace wolf {
 
-
-/** \brief Enumeration of covariance blocks to be computed
- *
- * Enumeration of covariance blocks to be computed
- *
- */
-typedef enum
-{
-    ALL, ///< All blocks and all cross-covariances
-    ALL_MARGINALS, ///< All marginals
-    ROBOT_LANDMARKS ///< marginals of landmarks and current robot pose plus cross covariances of current robot and all landmarks
-} CovarianceBlocksToBeComputed;
+WOLF_PTR_TYPEDEFS(CeresManager);
 
 /** \brief Ceres manager for WOLF
  *
  */
 
-class CeresManager
+class CeresManager : public SolverManager
 {
 	protected:
-		std::map<unsigned int, ceres::ResidualBlockId> id_2_residual_idx_;
-        std::map<unsigned int, ceres::CostFunction*> id_2_costfunction_;
+		std::map<ConstraintBasePtr, ceres::ResidualBlockId> ctr_2_residual_idx_;
+        std::map<ConstraintBasePtr, ceres::CostFunctionPtr> ctr_2_costfunction_;
 		ceres::Problem* ceres_problem_;
 		ceres::Solver::Options ceres_options_;
 		ceres::Covariance* covariance_;
-		ProblemPtr wolf_problem_;
-		bool use_wolf_auto_diff_;
+		ceres::Solver::Summary summary_;
 
 	public:
-        CeresManager(ProblemPtr _wolf_problem, const ceres::Solver::Options& _ceres_options = ceres::Solver::Options(), const bool _use_wolf_auto_diff = true);
+        CeresManager(ProblemPtr _wolf_problem, const ceres::Solver::Options& _ceres_options = ceres::Solver::Options());
 
 		~CeresManager();
 
-		ceres::Solver::Summary solve();
+		virtual std::string solve(const unsigned int& _report_level);
 
-		void computeCovariances(CovarianceBlocksToBeComputed _blocks = ROBOT_LANDMARKS);
+        ceres::Solver::Summary getSummary();
 
-        ceres::Solver::Options& getSolverOptions();
+        virtual void computeCovariances(CovarianceBlocksToBeComputed _blocks = ROBOT_LANDMARKS);
 
-        void setUseWolfAutoDiff(bool _use_wolf_auto_diff);
+        virtual void computeCovariances(const StateBlockList& st_list);
 
-	private:
-
-		void update();
+        ceres::Solver::Options& getSolverOptions();
 
-		void addConstraint(ConstraintBasePtr _corr_ptr, unsigned int _id);
+	private:
 
-		void removeConstraint(const unsigned int& _corr_idx);
+        virtual void addConstraint(ConstraintBasePtr _ctr_ptr);
 
-		void addStateBlock(StateBlockPtr _st_ptr);
+        virtual void removeConstraint(ConstraintBasePtr _ctr_ptr);
 
-		void removeStateBlock(double* _st_ptr);
+        virtual void addStateBlock(StateBlockPtr _st_ptr);
 
-		void removeAllStateBlocks();
+        virtual void removeStateBlock(StateBlockPtr _st_ptr);
 
-		void updateStateBlockStatus(StateBlockPtr _st_ptr);
+		virtual void updateStateBlockStatus(StateBlockPtr _st_ptr);
 
-		ceres::CostFunction* createCostFunction(ConstraintBasePtr _corrPtr);
+		ceres::CostFunctionPtr createCostFunction(ConstraintBasePtr _ctr_ptr);
 };
 
-inline ceres::Solver::Options& CeresManager::getSolverOptions()
+inline ceres::Solver::Summary CeresManager::getSummary()
 {
-    return ceres_options_;
+    return summary_;
 }
 
-inline void CeresManager::setUseWolfAutoDiff(bool _use_wolf_auto_diff)
+inline ceres::Solver::Options& CeresManager::getSolverOptions()
 {
-    use_wolf_auto_diff_ = _use_wolf_auto_diff;
+    return ceres_options_;
 }
 
 } // namespace wolf

src/ceres_wrapper/cost_function_wrapper.h

@@ -13,60 +13,32 @@
 
 namespace wolf {
 
+WOLF_PTR_TYPEDEFS(CostFunctionWrapper);
+
 class CostFunctionWrapper : public ceres::CostFunction
 {
     protected:
-        ConstraintAnalytic* constraint_ptr_;
-        std::vector<unsigned int> state_blocks_sizes_;
+        ConstraintBasePtr constraint_ptr_;
 
     public:
 
-        CostFunctionWrapper(ConstraintAnalytic* _constraint_ptr) :
+        CostFunctionWrapper(ConstraintBasePtr _constraint_ptr) :
             ceres::CostFunction(),
-            constraint_ptr_(_constraint_ptr),
-            state_blocks_sizes_(constraint_ptr_->getStateSizes())
+            constraint_ptr_(_constraint_ptr)
         {
-            for (unsigned int i = 0; i < constraint_ptr_->getStateBlockPtrVector().size(); i++)
-                mutable_parameter_block_sizes()->push_back(state_blocks_sizes_[i]);
+            for (auto st_block_size : constraint_ptr_->getStateSizes())
+                mutable_parameter_block_sizes()->push_back(st_block_size);
 
             set_num_residuals(constraint_ptr_->getSize());
         };
 
         virtual ~CostFunctionWrapper()
         {
-
         };
 
         virtual bool Evaluate(double const* const* parameters, double* residuals, double** jacobians) const
         {
-            // load parameters evaluation value
-            std::vector<Eigen::Map<const Eigen::VectorXs>> state_blocks_map_;
-            for (unsigned int i = 0; i < state_blocks_sizes_.size(); i++)
-                state_blocks_map_.push_back(Eigen::Map<const Eigen::VectorXs>((Scalar*)parameters[i], state_blocks_sizes_[i]));
-
-            // residuals
-            Eigen::Map<Eigen::VectorXs> residuals_map((Scalar*)residuals, constraint_ptr_->getSize());
-            residuals_map = constraint_ptr_->evaluateResiduals(state_blocks_map_);
-
-            // also compute jacobians
-            if (jacobians != nullptr)
-            {
-                std::vector<Eigen::Map<Eigen::MatrixXs>> jacobians_map_;
-                std::vector<bool> compute_jacobians_(state_blocks_sizes_.size());
-
-                for (unsigned int i = 0; i < state_blocks_sizes_.size(); i++)
-                {
-                    compute_jacobians_[i] = (jacobians[i] != nullptr);
-                    if (jacobians[i] != nullptr)
-                        jacobians_map_.push_back(Eigen::Map<Eigen::MatrixXs>((Scalar*)jacobians[i], constraint_ptr_->getSize(), state_blocks_sizes_[i]));
-                    else
-                        jacobians_map_.push_back(Eigen::Map<Eigen::MatrixXs>(nullptr, 0, 0)); //TODO: check if it can be done
-                }
-
-                // evaluate jacobians
-                constraint_ptr_->evaluateJacobians(state_blocks_map_, jacobians_map_, compute_jacobians_);
-            }
-            return true;
+            return constraint_ptr_->evaluate(parameters, residuals, jacobians);
         }
 };
 

src/ceres_wrapper/create_auto_diff_cost_function.cpp

-/*
- * create_auto_diff_cost_function.cpp
- *
- *  Created on: May 18, 2016
- *      Author: jvallve
- */
-
-#include "create_auto_diff_cost_function.h"
-
-// Constraints
-#include "../constraint_sparse.h"
-#include "../constraint_fix.h"
-#include "../constraint_fix_3D.h"
-#include "../constraint_gps_2D.h"
-#include "../constraint_gps_pseudorange_3D.h"
-#include "../constraint_gps_pseudorange_2D.h"
-#include "../constraint_odom_2D.h"
-#include "../constraint_odom_3D.h"
-#include "../constraint_corner_2D.h"
-#include "../constraint_point_2D.h"
-#include "../constraint_point_to_line_2D.h"
-#include "../constraint_container.h"
-
-#ifdef CV_VERSION
-  #include "../constraint_AHP.h"
-#endif
-
-#include "../constraint_imu.h"
-
-// Wolf and ceres auto_diff creators
-#include "create_auto_diff_cost_function_wrapper.h"
-#include "create_auto_diff_cost_function_ceres.h"
-
-namespace wolf {
-
-ceres::CostFunction* createAutoDiffCostFunction(ConstraintBasePtr _ctr_ptr, bool _use_wolf_autodiff)
-{
-    switch (_ctr_ptr->getTypeId())
-    {
-        case CTR_GPS_FIX_2D:
-            if (_use_wolf_autodiff)
-                return createAutoDiffCostFunctionWrapper<ConstraintGPS2D>(_ctr_ptr);
-            else
-                return createAutoDiffCostFunctionCeres<ConstraintGPS2D>(_ctr_ptr);
-
-        case CTR_FIX:
-            if (_use_wolf_autodiff)
-                return createAutoDiffCostFunctionWrapper<ConstraintFix>(_ctr_ptr);
-            else
-                return createAutoDiffCostFunctionCeres<ConstraintFix>(_ctr_ptr);
-
-        case CTR_FIX_3D:
-            if (_use_wolf_autodiff)
-                return createAutoDiffCostFunctionWrapper<ConstraintFix3D>(_ctr_ptr);
-            else
-                return createAutoDiffCostFunctionCeres<ConstraintFix3D>(_ctr_ptr);
-
-        case CTR_ODOM_2D:
-            if (_use_wolf_autodiff)
-                return createAutoDiffCostFunctionWrapper<ConstraintOdom2D>(_ctr_ptr);
-            else
-                return createAutoDiffCostFunctionCeres<ConstraintOdom2D>(_ctr_ptr);
-
-        case CTR_ODOM_3D:
-            if (_use_wolf_autodiff)
-                return createAutoDiffCostFunctionWrapper<ConstraintOdom3D>(_ctr_ptr);
-            else
-                return createAutoDiffCostFunctionCeres<ConstraintOdom3D>(_ctr_ptr);
-
-        case CTR_CORNER_2D:
-            if (_use_wolf_autodiff)
-                return createAutoDiffCostFunctionWrapper<ConstraintCorner2D>(_ctr_ptr);
-            else
-                return createAutoDiffCostFunctionCeres<ConstraintCorner2D>(_ctr_ptr);
-
-        case CTR_CONTAINER:
-            if (_use_wolf_autodiff)
-                return createAutoDiffCostFunctionWrapper<ConstraintContainer>(_ctr_ptr);
-            else
-                return createAutoDiffCostFunctionCeres<ConstraintContainer>(_ctr_ptr);
-
-        case CTR_GPS_PR_3D:
-            if (_use_wolf_autodiff)
-                return createAutoDiffCostFunctionWrapper<ConstraintGPSPseudorange3D>(_ctr_ptr);
-            else
-                return createAutoDiffCostFunctionCeres<ConstraintGPSPseudorange3D>(_ctr_ptr);
-
-        case CTR_GPS_PR_2D:
-            if (_use_wolf_autodiff)
-                return createAutoDiffCostFunctionWrapper<ConstraintGPSPseudorange2D>(_ctr_ptr);
-            else
-                return createAutoDiffCostFunctionCeres<ConstraintGPSPseudorange2D>(_ctr_ptr);
-
-        case CTR_POINT_2D:
-            if (_use_wolf_autodiff)
-            	return createAutoDiffCostFunctionWrapper<ConstraintPoint2D>(_ctr_ptr);
-            else
-                return createAutoDiffCostFunctionCeres<ConstraintPoint2D>(_ctr_ptr);
-
-        case CTR_POINT_TO_LINE_2D:
-            if (_use_wolf_autodiff)
-            	return createAutoDiffCostFunctionWrapper<ConstraintPointToLine2D>(_ctr_ptr);
-            else
-                return createAutoDiffCostFunctionCeres<ConstraintPointToLine2D>(_ctr_ptr);
-#ifdef CV_VERSION
-        case CTR_EPIPOLAR:
-            if (_use_wolf_autodiff)
-                return createAutoDiffCostFunctionWrapper<ConstraintAHP>(_ctr_ptr);
-            else
-                return createAutoDiffCostFunctionCeres<ConstraintAHP>(_ctr_ptr);
-
-        case CTR_AHP:
-            if (_use_wolf_autodiff)
-                return createAutoDiffCostFunctionWrapper<ConstraintAHP>(_ctr_ptr);
-            else
-                return createAutoDiffCostFunctionCeres<ConstraintAHP>(_ctr_ptr);
-#endif
-        case CTR_IMU:
-            if (_use_wolf_autodiff)
-                return createAutoDiffCostFunctionWrapper<ConstraintIMU>(_ctr_ptr);
-            else
-                return createAutoDiffCostFunctionCeres<ConstraintIMU>(_ctr_ptr);
-
-            /* For adding a new constraint, add the #include and a case:
-            case CTR_ENUM:
-                if (_use_wolf_autodiff)
-                    return createAutoDiffCostFunctionWrapper<ConstraintType>(_ctr_ptr);
-                else
-                    return createAutoDiffCostFunctionCeres<ConstraintType>(_ctr_ptr);
-             */
-
-        default:
-            throw std::invalid_argument( "Unknown constraint type! Please add it in the file: ceres_wrapper/create_auto_diff_cost_function.cpp" );
-    }
-}
-
-} // namespace wolf

src/ceres_wrapper/create_auto_diff_cost_function.h

-/*
- * create_auto_diff_cost_function.h
- *
- *  Created on: Apr 4, 2016
- *      Author: jvallve
- */
-
-#ifndef SRC_CERES_WRAPPER_CREATE_AUTO_DIFF_COST_FUNCTION_H_
-#define SRC_CERES_WRAPPER_CREATE_AUTO_DIFF_COST_FUNCTION_H_
-
-#include "../constraint_base.h"
-#include "ceres/cost_function.h"
-
-namespace wolf {
-    ceres::CostFunction* createAutoDiffCostFunction(ConstraintBasePtr _ctr_ptr, bool _use_wolf_autodiff);
-
-}
-
-#endif /* SRC_CERES_WRAPPER_CREATE_AUTO_DIFF_COST_FUNCTION_H_ */

src/ceres_wrapper/create_auto_diff_cost_function_ceres.h

-/*
- * create_auto_diff_cost_function_ceres.h
- *
- *  Created on: Apr 5, 2016
- *      Author: jvallve
- */
-
-#ifndef SRC_CERES_WRAPPER_CREATE_AUTO_DIFF_COST_FUNCTION_CERES_H_
-#define SRC_CERES_WRAPPER_CREATE_AUTO_DIFF_COST_FUNCTION_CERES_H_
-
-#include "ceres/autodiff_cost_function.h"
-
-namespace wolf {
-
-template <class CtrType>
-ceres::AutoDiffCostFunction<CtrType,
-                            CtrType::residualSize,
-                            CtrType::block0Size,CtrType::block1Size,
-                            CtrType::block2Size,CtrType::block3Size,
-                            CtrType::block4Size,CtrType::block5Size,
-                            CtrType::block6Size,CtrType::block7Size,
-                            CtrType::block8Size,CtrType::block9Size>* createAutoDiffCostFunctionCeres(ConstraintBasePtr _constraint_ptr)
-{
-    static_assert(CtrType::residualSize != 0,"Measurement size cannot be null!");
-    static_assert(!(CtrType::block0Size == 0 ||
-                   (CtrType::block1Size > 0 && CtrType::block0Size == 0) ||
-                   (CtrType::block2Size > 0 && (CtrType::block0Size == 0 || CtrType::block1Size == 0)) ||
-                   (CtrType::block3Size > 0 && (CtrType::block0Size == 0 || CtrType::block1Size == 0 || CtrType::block2Size == 0)) ||
-                   (CtrType::block4Size > 0 && (CtrType::block0Size == 0 || CtrType::block1Size == 0 || CtrType::block2Size == 0 || CtrType::block3Size == 0)) ||
-                   (CtrType::block5Size > 0 && (CtrType::block0Size == 0 || CtrType::block1Size == 0 || CtrType::block2Size == 0 || CtrType::block3Size == 0 || CtrType::block4Size == 0)) ||
-                   (CtrType::block6Size > 0 && (CtrType::block0Size == 0 || CtrType::block1Size == 0 || CtrType::block2Size == 0 || CtrType::block3Size == 0 || CtrType::block4Size == 0 || CtrType::block5Size == 0)) ||
-                   (CtrType::block7Size > 0 && (CtrType::block0Size == 0 || CtrType::block1Size == 0 || CtrType::block2Size == 0 || CtrType::block3Size == 0 || CtrType::block4Size == 0 || CtrType::block5Size == 0 || CtrType::block6Size == 0)) ||
-                   (CtrType::block8Size > 0 && (CtrType::block0Size == 0 || CtrType::block1Size == 0 || CtrType::block2Size == 0 || CtrType::block3Size == 0 || CtrType::block4Size == 0 || CtrType::block5Size == 0 || CtrType::block6Size == 0 || CtrType::block7Size == 0)) ||
-                   (CtrType::block9Size > 0 && (CtrType::block0Size == 0 || CtrType::block1Size == 0 || CtrType::block2Size == 0 || CtrType::block3Size == 0 || CtrType::block4Size == 0 || CtrType::block5Size == 0 || CtrType::block6Size == 0 || CtrType::block7Size == 0 || CtrType::block8Size == 0))),
-                  "bad block sizes numbers!");
-
-    return new ceres::AutoDiffCostFunction<CtrType, CtrType::residualSize,
-                                           CtrType::block0Size,CtrType::block1Size,CtrType::block2Size,CtrType::block3Size,CtrType::block4Size,
-                                           CtrType::block5Size,CtrType::block6Size,CtrType::block7Size,CtrType::block8Size,CtrType::block9Size>(std::static_pointer_cast<CtrType>(_constraint_ptr).get()); // TODO revise pointer types
-};
-
-} // namespace wolf
-
-#endif /* SRC_CERES_WRAPPER_CREATE_AUTO_DIFF_COST_FUNCTION_CERES_H_ */

src/ceres_wrapper/create_auto_diff_cost_function_wrapper.h

-/*
- * create_auto_diff_cost_function.h
- *
- *  Created on: Apr 5, 2016
- *      Author: jvallve
- */
-
-#ifndef SRC_CERES_WRAPPER_CREATE_AUTO_DIFF_COST_FUNCTION_WRAPPER_H_
-#define SRC_CERES_WRAPPER_CREATE_AUTO_DIFF_COST_FUNCTION_WRAPPER_H_
-
-#include "auto_diff_cost_function_wrapper.h"
-
-namespace wolf {
-
-template <class CtrType>
-AutoDiffCostFunctionWrapperBase<CtrType,
-                                CtrType::residualSize,
-                                CtrType::block0Size,CtrType::block1Size,
-                                CtrType::block2Size,CtrType::block3Size,
-                                CtrType::block4Size,CtrType::block5Size,
-                                CtrType::block6Size,CtrType::block7Size,
-                                CtrType::block8Size,CtrType::block9Size>* createAutoDiffCostFunctionWrapper(ConstraintBasePtr _constraint_ptr)
-{
-    static_assert(CtrType::residualSize != 0,"Measurement size cannot be null!");
-    static_assert(!(CtrType::block0Size == 0 ||
-                   (CtrType::block1Size > 0 && CtrType::block0Size == 0) ||
-                   (CtrType::block2Size > 0 && (CtrType::block0Size == 0 || CtrType::block1Size == 0)) ||
-                   (CtrType::block3Size > 0 && (CtrType::block0Size == 0 || CtrType::block1Size == 0 || CtrType::block2Size == 0)) ||
-                   (CtrType::block4Size > 0 && (CtrType::block0Size == 0 || CtrType::block1Size == 0 || CtrType::block2Size == 0 || CtrType::block3Size == 0)) ||
-                   (CtrType::block5Size > 0 && (CtrType::block0Size == 0 || CtrType::block1Size == 0 || CtrType::block2Size == 0 || CtrType::block3Size == 0 || CtrType::block4Size == 0)) ||
-                   (CtrType::block6Size > 0 && (CtrType::block0Size == 0 || CtrType::block1Size == 0 || CtrType::block2Size == 0 || CtrType::block3Size == 0 || CtrType::block4Size == 0 || CtrType::block5Size == 0)) ||
-                   (CtrType::block7Size > 0 && (CtrType::block0Size == 0 || CtrType::block1Size == 0 || CtrType::block2Size == 0 || CtrType::block3Size == 0 || CtrType::block4Size == 0 || CtrType::block5Size == 0 || CtrType::block6Size == 0)) ||
-                   (CtrType::block8Size > 0 && (CtrType::block0Size == 0 || CtrType::block1Size == 0 || CtrType::block2Size == 0 || CtrType::block3Size == 0 || CtrType::block4Size == 0 || CtrType::block5Size == 0 || CtrType::block6Size == 0 || CtrType::block7Size == 0)) ||
-                   (CtrType::block9Size > 0 && (CtrType::block0Size == 0 || CtrType::block1Size == 0 || CtrType::block2Size == 0 || CtrType::block3Size == 0 || CtrType::block4Size == 0 || CtrType::block5Size == 0 || CtrType::block6Size == 0 || CtrType::block7Size == 0 || CtrType::block8Size == 0))),
-                  "bad block sizes numbers!");
-
-    return new AutoDiffCostFunctionWrapperBase<CtrType, CtrType::residualSize,
-                                               CtrType::block0Size,CtrType::block1Size,CtrType::block2Size,CtrType::block3Size,CtrType::block4Size,
-                                               CtrType::block5Size,CtrType::block6Size,CtrType::block7Size,CtrType::block8Size,CtrType::block9Size>(std::static_pointer_cast<CtrType>(_constraint_ptr).get()); // TODO revise pointer types
-};
-
-} // namespace wolf
-
-#endif /* SRC_CERES_WRAPPER_CREATE_AUTO_DIFF_COST_FUNCTION_WRAPPER_H_ */

src/ceres_wrapper/create_numeric_diff_cost_function.cpp

-/*
- * create_numerical_diff_cost_function.cpp
- *
- *  Created on: May 18, 2016
- *      Author: jvallve
- */
-
-#include "create_numeric_diff_cost_function.h"
-
-// Constraints
-#include "../constraint_odom_2D.h"
-
-// Wolf and ceres auto_diff creators
-#include "create_numeric_diff_cost_function_ceres.h"
-
-namespace wolf {
-
-ceres::CostFunction* createNumericDiffCostFunction(ConstraintBasePtr _ctr_ptr, bool _use_wolf_numericdiff)
-{
-    if (_use_wolf_numericdiff)
-        throw std::invalid_argument( "Numeric differentiation not implemented in wolf" );
-
-    switch (_ctr_ptr->getTypeId())
-    {
-        // just for testing
-        case CTR_ODOM_2D:
-            return createNumericDiffCostFunctionCeres<ConstraintOdom2D>(_ctr_ptr);
-
-        /* For adding a new constraint, add the #include and a case:
-        case CTR_ENUM:
-            return createNumericDiffCostFunctionCeres<ConstraintType>(_ctr_ptr);
-         */
-
-        default:
-            throw std::invalid_argument( "Unknown constraint type! Please add it in the file: ceres_wrapper/create_Numeric_diff_cost_function.h" );
-    }
-}
-
-} // namespace wolf

src/ceres_wrapper/create_numeric_diff_cost_function.h

 /*
- * create_numerical_diff_cost_function.h
+ * create_numeric_diff_cost_function.h
  *
  *  Created on: Apr 5, 2016
  *      Author: jvallve
@@ -9,11 +9,44 @@
 #define SRC_CERES_WRAPPER_CREATE_NUMERIC_DIFF_COST_FUNCTION_H_
 
 #include "ceres/cost_function.h"
+#include "ceres/numeric_diff_cost_function.h"
+
+// Constraints
+#include "../constraint_odom_2D.h"
 #include "../constraint_base.h"
 
+
 namespace wolf {
 
-ceres::CostFunction* createNumericDiffCostFunction(ConstraintBasePtr _ctr_ptr, bool _use_wolf_numericdiff);
+// Wolf ceres auto_diff creator
+template <class T>
+std::shared_ptr<ceres::NumericDiffCostFunction<T, ceres::CENTRAL, T::residualSize,
+                                               T::block0Size,T::block1Size,T::block2Size,T::block3Size,T::block4Size,
+                                               T::block5Size,T::block6Size,T::block7Size,T::block8Size,T::block9Size> > createNumericDiffCostFunctionCeres(ConstraintBasePtr _constraint_ptr)
+{
+    return std::make_shared<ceres::NumericDiffCostFunction<T, ceres::CENTRAL, T::residualSize,
+                                                           T::block0Size,T::block1Size,T::block2Size,T::block3Size,T::block4Size,
+                                                           T::block5Size,T::block6Size,T::block7Size,T::block8Size,T::block9Size> >(std::static_pointer_cast<T>(_constraint_ptr).get());
+};
+
+
+std::shared_ptr<ceres::CostFunction> createNumericDiffCostFunction(ConstraintBasePtr _ctr_ptr)
+{
+    switch (_ctr_ptr->getTypeId())
+    {
+        // just for testing
+        case CTR_ODOM_2D:
+            return createNumericDiffCostFunctionCeres<ConstraintOdom2D>(_ctr_ptr);
+
+        /* For adding a new constraint, add the #include and a case:
+        case CTR_ENUM:
+            return createNumericDiffCostFunctionCeres<ConstraintType>(_ctr_ptr);
+         */
+
+        default:
+            throw std::invalid_argument( "Unknown constraint type! Please add it in the file: ceres_wrapper/create_Numeric_diff_cost_function.h" );
+    }
+}
 
 } // namespace wolf
 

src/ceres_wrapper/create_numeric_diff_cost_function_ceres.h

-/*
- * create_numric_cost_function_ceres.h
- *
- *  Created on: Apr 5, 2016
- *      Author: jvallve
- */
-
-#ifndef SRC_CERES_WRAPPER_CREATE_NUMERIC_DIFF_COST_FUNCTION_CERES_H_
-#define SRC_CERES_WRAPPER_CREATE_NUMERIC_DIFF_COST_FUNCTION_CERES_H_
-
-#include "ceres/numeric_diff_cost_function.h"
-
-namespace wolf {
-
-template <class CtrType>
-ceres::NumericDiffCostFunction<CtrType, ceres::CENTRAL,
-                                CtrType::residualSize,
-                                CtrType::block0Size,CtrType::block1Size,
-                                CtrType::block2Size,CtrType::block3Size,
-                                CtrType::block4Size,CtrType::block5Size,
-                                CtrType::block6Size,CtrType::block7Size,
-                                CtrType::block8Size,CtrType::block9Size>* createNumericDiffCostFunctionCeres(ConstraintBasePtr _constraint_ptr)
-{
-    static_assert(CtrType::residualSize != 0,"Measurement size cannot be null!");
-    static_assert(!(CtrType::block0Size == 0 ||
-                   (CtrType::block1Size > 0 && CtrType::block0Size == 0) ||
-                   (CtrType::block2Size > 0 && (CtrType::block0Size == 0 || CtrType::block1Size == 0)) ||
-                   (CtrType::block3Size > 0 && (CtrType::block0Size == 0 || CtrType::block1Size == 0 || CtrType::block2Size == 0)) ||
-                   (CtrType::block4Size > 0 && (CtrType::block0Size == 0 || CtrType::block1Size == 0 || CtrType::block2Size == 0 || CtrType::block3Size == 0)) ||
-                   (CtrType::block5Size > 0 && (CtrType::block0Size == 0 || CtrType::block1Size == 0 || CtrType::block2Size == 0 || CtrType::block3Size == 0 || CtrType::block4Size == 0)) ||
-                   (CtrType::block6Size > 0 && (CtrType::block0Size == 0 || CtrType::block1Size == 0 || CtrType::block2Size == 0 || CtrType::block3Size == 0 || CtrType::block4Size == 0 || CtrType::block5Size == 0)) ||
-                   (CtrType::block7Size > 0 && (CtrType::block0Size == 0 || CtrType::block1Size == 0 || CtrType::block2Size == 0 || CtrType::block3Size == 0 || CtrType::block4Size == 0 || CtrType::block5Size == 0 || CtrType::block6Size == 0)) ||
-                   (CtrType::block8Size > 0 && (CtrType::block0Size == 0 || CtrType::block1Size == 0 || CtrType::block2Size == 0 || CtrType::block3Size == 0 || CtrType::block4Size == 0 || CtrType::block5Size == 0 || CtrType::block6Size == 0 || CtrType::block7Size == 0)) ||
-                   (CtrType::block9Size > 0 && (CtrType::block0Size == 0 || CtrType::block1Size == 0 || CtrType::block2Size == 0 || CtrType::block3Size == 0 || CtrType::block4Size == 0 || CtrType::block5Size == 0 || CtrType::block6Size == 0 || CtrType::block7Size == 0 || CtrType::block8Size == 0))),
-                  "bad block sizes numbers!");
-
-    return new ceres::NumericDiffCostFunction<CtrType, ceres::CENTRAL, CtrType::residualSize,
-                                              CtrType::block0Size,CtrType::block1Size,CtrType::block2Size,CtrType::block3Size,CtrType::block4Size,
-                                              CtrType::block5Size,CtrType::block6Size,CtrType::block7Size,CtrType::block8Size,CtrType::block9Size>(std::static_pointer_cast<CtrType>(_constraint_ptr).get()); // TODO revise pointer type
-};
-
-
-
-
-} // namespace wolf
-
-#endif /* SRC_CERES_WRAPPER_CREATE_NUMERIC_DIFF_COST_FUNCTION_CERES_H_ */

src/ceres_wrapper/qr_manager.cpp

+/*
+ * qr_manager.cpp
+ *
+ *  Created on: Jun 7, 2017
+ *      Author: jvallve
+ */
+
+#include "qr_manager.h"
+
+namespace wolf {
+
+QRManager::QRManager(ProblemPtr _wolf_problem, const unsigned int& _N_batch) :
+        SolverManager(_wolf_problem),
+        A_(), // empty matrix
+        b_(),
+        any_state_block_removed_(false),
+        new_state_blocks_(0),
+        N_batch_(_N_batch),
+        pending_changes_(false)
+{
+    //
+}
+
+QRManager::~QRManager()
+{
+    sb_2_col_.clear();
+    ctr_2_row_.clear();
+}
+
+std::string QRManager::solve(const unsigned int& _report_level)
+{
+    // check for update notifications
+    update();
+
+    // Decomposition
+    if (!computeDecomposition())
+        return std::string("decomposition failed\n");
+
+    // Solve
+    Eigen::VectorXs x_incr = solver_.solve(-b_);
+    b_.setZero();
+
+    // update state blocks
+    for (auto sb_pair : sb_2_col_)
+        sb_pair.first->setState(sb_pair.first->getState() + x_incr.segment(sb_pair.second, sb_pair.first->getSize()));
+
+    if (_report_level == 1)
+        return std::string("Success!\n");
+    else if (_report_level == 2)
+        return std::string("Success!\n");
+
+    return std::string();
+}
+
+void QRManager::computeCovariances(CovarianceBlocksToBeComputed _blocks)
+{
+    // TODO
+}
+
+void QRManager::computeCovariances(const StateBlockList& _sb_list)
+{
+    //std::cout << "computing covariances.." << std::endl;
+    update();
+    computeDecomposition();
+
+//    std::cout << "R is " << solver_.matrixR().rows() << "x" << solver_.matrixR().cols() << std::endl;
+//    std::cout << Eigen::MatrixXs(solver_.matrixR()) << std::endl;
+
+    covariance_solver_.compute(solver_.matrixR().topRows(solver_.matrixR().cols()));
+
+    Eigen::SparseMatrix<Scalar, Eigen::ColMajor> I(A_.cols(),A_.cols());
+    I.setIdentity();
+    Eigen::SparseMatrix<Scalar, Eigen::ColMajor> iR = covariance_solver_.solve(I);
+    Eigen::MatrixXs Sigma_full = solver_.colsPermutation() * iR * iR.transpose() * solver_.colsPermutation().transpose();
+
+//    std::cout << "A' A = \n" << Eigen::MatrixXs(A_.transpose() * A_)<< std::endl;
+//    std::cout << "P iR iR' P' = \n" << Eigen::MatrixXs(P * iR * iR.transpose() * P.transpose()) << std::endl;
+//    std::cout << "Sigma * Lambda = \n" << Eigen::MatrixXs(Sigma_full * A_.transpose() * A_) << std::endl;
+//    std::cout << "Permutation: \n" << solver_.colsPermutation() << std::endl;
+//    std::cout << "Sigma = \n" << Sigma_full << std::endl;
+
+    // STORE DESIRED COVARIANCES
+    for (auto sb_row = _sb_list.begin(); sb_row != _sb_list.end(); sb_row++)
+        for (auto sb_col = sb_row; sb_col!=_sb_list.end(); sb_col++)
+        {
+            //std::cout << "cov state block " << sb_col->get() << std::endl;
+            assert(sb_2_col_.find(*sb_col) != sb_2_col_.end() && "state block not found");
+            //std::cout << "block: " << sb_2_col_[*sb_row] << "," << sb_2_col_[*sb_col] << std::endl << Sigma_full.block(sb_2_col_[*sb_row], sb_2_col_[*sb_col], (*sb_row)->getSize(), (*sb_col)->getSize()) << std::endl;
+            wolf_problem_->addCovarianceBlock(*sb_row, *sb_col, Sigma_full.block(sb_2_col_[*sb_row], sb_2_col_[*sb_col], (*sb_row)->getSize(), (*sb_col)->getSize()));
+        }
+}
+
+bool QRManager::computeDecomposition()
+{
+    if (pending_changes_)
+    {
+        // Rebuild problem
+        if (any_state_block_removed_)
+        {
+            // rebuild maps
+            unsigned int state_size = 0;
+            for (auto sb_pair : sb_2_col_)
+            {
+                sb_2_col_[sb_pair.first] = state_size;
+                state_size += sb_pair.first->getSize();
+            }
+
+            unsigned int meas_size = 0;
+            for (auto ctr_pair : ctr_2_row_)
+            {
+                ctr_2_row_[ctr_pair.first] = meas_size;
+                meas_size += ctr_pair.first->getSize();
+            }
+
+            // resize and setZero A, b
+            A_.resize(meas_size,state_size);
+            b_.resize(meas_size);
+        }
+
+        if (any_state_block_removed_ || new_state_blocks_ >= N_batch_)
+        {
+            // relinearize all constraints
+            for (auto ctr_pair : ctr_2_row_)
+                relinearizeConstraint(ctr_pair.first);
+
+            any_state_block_removed_ = false;
+            new_state_blocks_ = 0;
+        }
+
+        // Decomposition
+        solver_.compute(A_);
+        if (solver_.info() != Eigen::Success)
+            return false;
+    }
+
+    pending_changes_ = false;
+
+    return true;
+}
+
+void QRManager::addConstraint(ConstraintBasePtr _ctr_ptr)
+{
+    //std::cout << "add constraint " << _ctr_ptr->id() << std::endl;
+    assert(ctr_2_row_.find(_ctr_ptr) == ctr_2_row_.end() && "adding existing constraint");
+    ctr_2_row_[_ctr_ptr] = A_.rows();
+    A_.conservativeResize(A_.rows() + _ctr_ptr->getSize(), A_.cols());
+    b_.conservativeResize(b_.size() + _ctr_ptr->getSize());
+
+    assert(A_.rows() >= ctr_2_row_[_ctr_ptr] + _ctr_ptr->getSize() - 1 && "bad A number of rows");
+    assert(b_.rows() >= ctr_2_row_[_ctr_ptr] + _ctr_ptr->getSize() - 1 && "bad b number of rows");
+
+    relinearizeConstraint(_ctr_ptr);
+
+    pending_changes_ = true;
+}
+
+
+void QRManager::removeConstraint(ConstraintBasePtr _ctr_ptr)
+{
+    //std::cout << "remove constraint " << _ctr_ptr->id() << std::endl;
+    assert(ctr_2_row_.find(_ctr_ptr) != ctr_2_row_.end() && "removing unknown constraint");
+    eraseBlockRow(A_, ctr_2_row_[_ctr_ptr], _ctr_ptr->getSize());
+    b_.segment(ctr_2_row_[_ctr_ptr], _ctr_ptr->getSize()).setZero();
+    ctr_2_row_.erase(_ctr_ptr);
+    pending_changes_ = true;
+}
+
+void QRManager::addStateBlock(StateBlockPtr _st_ptr)
+{
+    //std::cout << "add state block " << _st_ptr.get() << std::endl;
+    assert(sb_2_col_.find(_st_ptr) == sb_2_col_.end() && "adding existing state block");
+    sb_2_col_[_st_ptr] = A_.cols();
+    A_.conservativeResize(A_.rows(), A_.cols() + _st_ptr->getSize());
+
+    new_state_blocks_++;
+    pending_changes_ = true;
+}
+
+void QRManager::removeStateBlock(StateBlockPtr _st_ptr)
+{
+    //std::cout << "remove state block " << _st_ptr.get() << std::endl;
+    assert(sb_2_col_.find(_st_ptr) != sb_2_col_.end() && "removing unknown state block");
+    eraseBlockCol(A_, sb_2_col_[_st_ptr], _st_ptr->getSize());
+
+    // flag to rebuild problem
+    any_state_block_removed_ = true;
+    // TODO: insert identity while problem is not re-built?
+
+    sb_2_col_.erase(_st_ptr);
+    pending_changes_ = true;
+}
+
+void QRManager::updateStateBlockStatus(StateBlockPtr _st_ptr)
+{
+    //std::cout << "update state block " << _st_ptr.get() << std::endl;
+    if (_st_ptr->isFixed())
+    {
+        if (sb_2_col_.find(_st_ptr) != sb_2_col_.end())
+            removeStateBlock(_st_ptr);
+    }
+    else
+        if (sb_2_col_.find(_st_ptr) == sb_2_col_.end())
+            addStateBlock(_st_ptr);
+}
+
+void QRManager::relinearizeConstraint(ConstraintBasePtr _ctr_ptr)
+{
+    // evaluate constraint
+    std::vector<const Scalar*> ctr_states_ptr;
+    for (auto sb : _ctr_ptr->getStateBlockPtrVector())
+        ctr_states_ptr.push_back(sb->getPtr());
+    Eigen::VectorXs residual(_ctr_ptr->getSize());
+    std::vector<Eigen::MatrixXs> jacobians;
+    _ctr_ptr->evaluate(ctr_states_ptr,residual,jacobians);
+
+    // Fill jacobians
+    Eigen::SparseMatrixs A_block_row(_ctr_ptr->getSize(), A_.cols());
+    for (auto i = 0; i < jacobians.size(); i++)
+        if (!_ctr_ptr->getStateBlockPtrVector()[i]->isFixed())
+        {
+            assert(sb_2_col_.find(_ctr_ptr->getStateBlockPtrVector()[i]) != sb_2_col_.end() && "constraint involving a state block not added");
+            assert(A_.cols() >= sb_2_col_[_ctr_ptr->getStateBlockPtrVector()[i]] + jacobians[i].cols() - 1 && "bad A number of cols");
+            // insert since A_block_row has just been created so it's empty for sure
+            insertSparseBlock(jacobians[i], A_block_row, 0, sb_2_col_[_ctr_ptr->getStateBlockPtrVector()[i]]);
+        }
+    assignBlockRow(A_, A_block_row, ctr_2_row_[_ctr_ptr]);
+
+    // Fill residual
+    b_.segment(ctr_2_row_[_ctr_ptr], _ctr_ptr->getSize()) = residual;
+}
+
+} /* namespace wolf */

src/ceres_wrapper/qr_manager.h

+/*
+ * qr_manager.h
+ *
+ *  Created on: Jun 7, 2017
+ *      Author: jvallve
+ */
+
+#ifndef SRC_CERES_WRAPPER_QR_MANAGER_H_
+#define SRC_CERES_WRAPPER_QR_MANAGER_H_
+
+#include "solver_manager.h"
+#include "sparse_utils.h"
+
+namespace wolf
+{
+
+class QRManager : public SolverManager
+{
+    protected:
+        Eigen::SparseQR<Eigen::SparseMatrixs, Eigen::COLAMDOrdering<int>> solver_;
+        Eigen::SparseQR<Eigen::SparseMatrixs, Eigen::NaturalOrdering<int>> covariance_solver_;
+        Eigen::SparseMatrixs A_;
+        Eigen::VectorXs b_;
+        std::map<StateBlockPtr, unsigned int> sb_2_col_;
+        std::map<ConstraintBasePtr, unsigned int> ctr_2_row_;
+        bool any_state_block_removed_;
+        unsigned int new_state_blocks_;
+        unsigned int N_batch_;
+        bool pending_changes_;
+
+    public:
+
+        QRManager(ProblemPtr _wolf_problem, const unsigned int& _N_batch);
+
+        virtual ~QRManager();
+
+        virtual std::string solve(const unsigned int& _report_level);
+
+        virtual void computeCovariances(CovarianceBlocksToBeComputed _blocks = ROBOT_LANDMARKS);
+
+        virtual void computeCovariances(const StateBlockList& _sb_list);
+
+    private:
+
+        bool computeDecomposition();
+
+        virtual void addConstraint(ConstraintBasePtr _ctr_ptr);
+
+        virtual void removeConstraint(ConstraintBasePtr _ctr_ptr);
+
+        virtual void addStateBlock(StateBlockPtr _st_ptr);
+
+        virtual void removeStateBlock(StateBlockPtr _st_ptr);
+
+        virtual void updateStateBlockStatus(StateBlockPtr _st_ptr);
+
+        void relinearizeConstraint(ConstraintBasePtr _ctr_ptr);
+};
+
+} /* namespace wolf */
+
+#endif /* SRC_CERES_WRAPPER_QR_MANAGER_H_ */