Add slam_in_autonomous_driving repo

workspace/slam_in_autonomous_driving/thirdparty/g2o/.gitignore

@@ -0,0 +1,5 @@
+\build*
+\lib
+\bin
+\.vs
+\.vscode

workspace/slam_in_autonomous_driving/thirdparty/g2o/.travis.yml

@@ -0,0 +1,57 @@
+sudo: required
+dist: bionic
+language: cpp
+
+os:
+  - linux
+  - osx
+
+env:
+  global:
+    - MAKEFLAGS="-j 2"
+
+notifications:
+  email:
+    recipients:
+      - rainer.kuemmerle@gmail.com
+    on_success: change # default: change
+    on_failure: always # default: always
+
+compiler:
+  - gcc
+  - clang
+
+before_install:
+  - env | sort
+
+install:
+  - script/install-deps-${TRAVIS_OS_NAME}.sh
+
+before_script:
+  - mkdir build
+  - cd build
+  - if [[ "$CC" == "gcc" && "$TRAVIS_OS_NAME" == "linux" ]]; then
+      export CC=gcc-8 ;
+      export CXX=g++-8 ;
+    fi
+  - cmake --version
+  - cmake -DBUILD_UNITTESTS=ON ..
+  - cat g2o/config.h
+
+script:
+  - make
+  - if [[ "$TRAVIS_OS_NAME" == "linux" ]]; then
+      ctest --extra-verbose ;
+    fi
+
+# right now only build the master branch
+branches:
+  only:
+    - master
+
+matrix:
+  exclude:
+    - os: osx
+      compiler: gcc
+  allow_failures:
+    - os: osx

workspace/slam_in_autonomous_driving/thirdparty/g2o/CMakeLists.txt

@@ -0,0 +1,415 @@
+cmake_minimum_required(VERSION 3.1)
+set(CMAKE_LEGACY_CYGWIN_WIN32 0)
+
+project(g2o)
+
+include(CPack)
+
+# The library prefix
+set(LIB_PREFIX g2o_)
+
+set(g2o_C_FLAGS)
+set(g2o_CXX_FLAGS)
+
+# default built type
+if(NOT CMAKE_BUILD_TYPE)
+  set(CMAKE_BUILD_TYPE Release CACHE STRING
+      "Choose the type of build, options are: None Debug Release RelWithDebInfo MinSizeRel."
+      FORCE)
+endif(NOT CMAKE_BUILD_TYPE)
+
+# postfix, based on type
+set(CMAKE_DEBUG_POSTFIX "_d" CACHE STRING "postfix applied to debug build of libraries")
+set(CMAKE_RELEASE_POSTFIX "" CACHE STRING "postfix applied to release build of libraries")
+set(CMAKE_RELWITHDEBINFO_POSTFIX "_rd" CACHE STRING "postfix applied to release-with-debug-information libraries")
+set(CMAKE_MINSIZEREL_POSTFIX "_s" CACHE STRING "postfix applied to minimium-size-build libraries")
+
+# work out the postfix; required where we use OUTPUT_NAME
+if(CMAKE_BUILD_TYPE MATCHES Release)
+  set(EXE_POSTFIX)
+elseif(CMAKE_BUILD_TYPE MATCHES Debug)
+  set(EXE_POSTFIX ${CMAKE_DEBUG_POSTFIX})
+elseif(CMAKE_BUILD_TYPE MATCHES RelWithDebInfo)
+  set(EXE_POSTFIX ${CMAKE_RELWITHDEBINFO_POSTFIX})
+elseif(CMAKE_BUILD_TYPE MATCHES MinSizeRel)
+  set(EXE_POSTFIX ${CMAKE_MINSIZEREL_POSTFIX})
+endif(CMAKE_BUILD_TYPE MATCHES Release)
+
+# Allow the developer to select if Dynamic or Static libraries are built
+option (BUILD_SHARED_LIBS "Build Shared Libraries (preferred and required for the g2o plugin system)" ON)
+set (G2O_LIB_TYPE STATIC)
+if (BUILD_SHARED_LIBS)
+  set (G2O_LIB_TYPE SHARED)
+endif()
+
+# There seems to be an issue with MSVC8
+# see http://eigen.tuxfamily.org/bz/show_bug.cgi?id=83
+if(MSVC90)
+  add_definitions(-DEIGEN_DONT_ALIGN_STATICALLY=1)
+  message(STATUS "Disabling memory alignment for MSVC8")
+endif(MSVC90)
+
+# On the Mac platform, configure the RPATH as per the INSTALL, to
+# avoid the problem of loading both the built and INSTALLed versions
+# of the shared targets
+if(APPLE)
+  set(CMAKE_BUILD_WITH_INSTALL_RPATH TRUE)
+  set(CMAKE_INSTALL_RPATH "")
+endif(APPLE)
+
+# Set the output directory for the build executables and libraries
+set(g2o_RUNTIME_OUTPUT_DIRECTORY ${g2o_SOURCE_DIR}/bin CACHE PATH "Target for the binaries")
+if(WIN32)
+  set(g2o_LIBRARY_OUTPUT_DIRECTORY ${g2o_SOURCE_DIR}/bin CACHE PATH "Target for the libraries")
+else(WIN32)
+  set(g2o_LIBRARY_OUTPUT_DIRECTORY ${g2o_SOURCE_DIR}/lib CACHE PATH "Target for the libraries")
+endif(WIN32)
+set(CMAKE_ARCHIVE_OUTPUT_DIRECTORY ${g2o_LIBRARY_OUTPUT_DIRECTORY})
+set(CMAKE_LIBRARY_OUTPUT_DIRECTORY ${g2o_LIBRARY_OUTPUT_DIRECTORY})
+set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${g2o_RUNTIME_OUTPUT_DIRECTORY})
+
+# Set standard installation directories
+set(RUNTIME_DESTINATION ${CMAKE_INSTALL_PREFIX}/bin)
+set(LIBRARY_DESTINATION ${CMAKE_INSTALL_PREFIX}/lib)
+set(ARCHIVE_DESTINATION ${CMAKE_INSTALL_PREFIX}/lib)
+set(INCLUDES_DESTINATION ${CMAKE_INSTALL_PREFIX}/include)
+set(INCLUDES_INSTALL_DIR ${INCLUDES_DESTINATION}/g2o)
+
+# Set search directory for looking for our custom CMake scripts to
+# look for SuiteSparse, QGLViewer, and Eigen3.
+list(APPEND CMAKE_MODULE_PATH ${g2o_SOURCE_DIR}/cmake_modules)
+
+# Detect OS and define macros appropriately
+if(WIN32)
+  add_definitions(-DWINDOWS)
+  message(STATUS "Compiling on Windows")
+elseif(CYGWIN)
+  message(STATUS "Compiling on Cygwin")
+  add_definitions(-DCYGWIN)
+elseif(APPLE)
+  add_definitions(-DUNIX)
+  message(STATUS "Compiling on OSX")
+elseif(UNIX)
+  add_definitions(-DUNIX)
+  message(STATUS "Compiling on Unix")
+endif(WIN32)
+
+# detect Android Cross Compiler
+# based on android-cmake which sets the variable ANDROID for us
+if(ANDROID)
+  add_definitions(-DANDROID)
+  message(STATUS "Cross compiling for Android")
+endif()
+
+# For building the CHOLMOD / CSPARSE solvers
+option (G2O_USE_CHOLMOD "Build g2o with CHOLMOD support" ON)
+
+find_package(Cholmod)
+find_package(BLAS)
+find_package(LAPACK)
+
+if(G2O_USE_CHOLMOD AND CHOLMOD_FOUND AND BLAS_FOUND AND LAPACK_FOUND)
+  message(STATUS "Found CHOLMOD and its dependencies")
+  set(CHOLMOD_FOUND TRUE)
+else()
+  set(CHOLMOD_FOUND FALSE)
+endif()
+
+option (G2O_USE_CSPARSE "Build g2o with CSParse support" ON)
+
+find_package(CSparse)
+if (G2O_USE_CSPARSE)
+  if(CSPARSE_FOUND)
+    set(BUILD_CSPARSE OFF CACHE BOOL "Build local CSparse library")
+  else(CSPARSE_FOUND)
+    set(BUILD_CSPARSE ON CACHE BOOL "Build local CSparse library")
+    if(BUILD_CSPARSE)
+      set(CSPARSE_FOUND TRUE)
+    endif()
+  endif(CSPARSE_FOUND)
+else(G2O_USE_CSPARSE)
+  set(BUILD_CSPARSE OFF "Build local CSparse library")
+endif(G2O_USE_CSPARSE)
+
+option(BUILD_LGPL_SHARED_LIBS "Build LGPL Code as Shared Libraries (LGPL Code)" ON)
+set (G2O_LGPL_LIB_TYPE STATIC)
+if (BUILD_LGPL_SHARED_LIBS)
+  set (G2O_LGPL_LIB_TYPE SHARED)
+else()
+  message(STATUS "Building LGPL code as static library (affects license of the binary)")
+endif()
+
+# Eigen library parallelise itself, though, presumably due to performance issues
+# OPENMP is experimental. We experienced some slowdown with it
+set(G2O_USE_OPENMP OFF CACHE BOOL "Build g2o with OpenMP support (EXPERIMENTAL)")
+if(G2O_USE_OPENMP)
+  find_package(OpenMP)
+  if(OPENMP_FOUND)
+    set (G2O_OPENMP 1)
+    set(g2o_C_FLAGS "${g2o_C_FLAGS} ${OpenMP_C_FLAGS}")
+    set(g2o_CXX_FLAGS "${g2o_CXX_FLAGS} -DEIGEN_DONT_PARALLELIZE ${OpenMP_CXX_FLAGS}")
+    message(STATUS "Compiling with OpenMP support")
+  endif(OPENMP_FOUND)
+endif(G2O_USE_OPENMP)
+
+# OpenGL is used in the draw actions for the different types, as well
+# as for creating the GUI itself
+find_package(OpenGL)
+
+# If OpenGL was found, use the import target if available. If not, use old-style includes
+set(G2O_USE_OPENGL ON CACHE BOOL "Build g2o with OpenGL support for visualization")
+if (OPENGL_FOUND AND G2O_USE_OPENGL)
+  if (TARGET OpenGL::GL)
+    set(G2O_OPENGL_TARGET "OpenGL::GL;OpenGL::GLU")
+  else()
+    set(G2O_OPENGL_TARGET "${OPENGL_LIBRARIES}")
+    include_directories(${OPENGL_INCLUDE_DIR})
+  endif()
+  set (G2O_HAVE_OPENGL 1)
+  message(STATUS "Compiling with OpenGL support")
+  #message(WARNING G2O_OPENGL_TARGET=${G2O_OPENGL_TARGET})
+endif()
+
+# For building the GUI
+find_package(QGLViewer)
+
+# shall we build the core apps using the library
+set(G2O_BUILD_APPS ON CACHE BOOL "Build g2o apps")
+if(G2O_BUILD_APPS)
+  message(STATUS "Compiling g2o apps")
+endif(G2O_BUILD_APPS)
+
+include(CMakeDependentOption)
+CMAKE_DEPENDENT_OPTION(G2O_BUILD_LINKED_APPS "Build apps linked with the libraries (no plugin system)" OFF
+  "G2O_BUILD_APPS" OFF)
+
+# shall we build the examples
+set(G2O_BUILD_EXAMPLES ON CACHE BOOL "Build g2o examples")
+if(G2O_BUILD_EXAMPLES)
+  message(STATUS "Compiling g2o examples")
+endif(G2O_BUILD_EXAMPLES)
+
+option(G2O_FAST_MATH "Enable fast math operations" OFF)
+option(G2O_NO_IMPLICIT_OWNERSHIP_OF_OBJECTS "Disables memory management in the graph types, this requires the callers to manager the memory of edges and nodes" OFF)
+
+# Start of SSE* autodetect code
+# (borrowed from MRPT CMake scripts, BSD)
+option(DO_SSE_AUTODETECT "Enable autodetection of SSE* CPU sets and enable their use in optimized code" ON)
+if(NOT EXISTS "/proc/cpuinfo")
+	set(DO_SSE_AUTODETECT OFF)
+endif()
+if (DO_SSE_AUTODETECT)
+  file(READ "/proc/cpuinfo" G2O_CPU_INFO)
+endif()
+
+# Macro for each SSE* var: Invoke with name in uppercase:
+macro(DEFINE_SSE_VAR  _setname)
+	string(TOLOWER ${_setname} _set)
+	if (DO_SSE_AUTODETECT)
+		# Automatic detection:
+		set(CMAKE_G2O_HAS_${_setname} 0)
+		if (${G2O_CPU_INFO} MATCHES ".*${_set}.*")
+			set(CMAKE_G2O_HAS_${_setname} 1)
+		endif()
+	else (DO_SSE_AUTODETECT)
+		# Manual:
+		set("DISABLE_${_setname}" OFF CACHE BOOL "Forces compilation WITHOUT ${_setname} extensions")
+		mark_as_advanced("DISABLE_${_setname}")
+		set(CMAKE_G2O_HAS_${_setname} 0)
+		if (NOT DISABLE_${_setname})
+			set(CMAKE_G2O_HAS_${_setname} 1)
+		endif (NOT DISABLE_${_setname})
+	endif (DO_SSE_AUTODETECT)
+endmacro(DEFINE_SSE_VAR)
+
+# SSE optimizations:
+DEFINE_SSE_VAR(SSE2)
+DEFINE_SSE_VAR(SSE3)
+DEFINE_SSE_VAR(SSE4_1)
+DEFINE_SSE_VAR(SSE4_2)
+DEFINE_SSE_VAR(SSE4_A)
+
+# Add build flags for clang AND GCC
+if (${CMAKE_CXX_COMPILER_ID} MATCHES "Clang" OR CMAKE_COMPILER_IS_GNUCXX)
+  # SSE2?
+  if (CMAKE_G2O_HAS_SSE2)
+    add_compile_options(-msse2)
+  endif()
+  # SSE3?
+  if (CMAKE_G2O_HAS_SSE3)
+    add_compile_options(-msse3 -mssse3)
+  endif()
+  # SSE4*?
+  if (CMAKE_G2O_HAS_SSE4_1)
+    add_compile_options(-msse4.1)
+  endif()
+  if (CMAKE_G2O_HAS_SSE4_2)
+    add_compile_options(-msse4.2)
+  endif()
+  if (CMAKE_G2O_HAS_SSE4_A)
+    add_compile_options(-msse4a)
+  endif()
+endif()
+# End of of SSE* autodetect code -------
+
+# Compiler specific options for gcc
+if(CMAKE_COMPILER_IS_GNUCXX)
+  option (BUILD_WITH_MARCH_NATIVE "Build with \"-march native\"" OFF)
+  message(STATUS "Compiling with GCC")
+
+  # Generic settings for optimisation
+  set(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE} -O3")
+  set(CMAKE_C_FLAGS_RELEASE "${CMAKE_C_FLAGS_RELEASE} -O3")
+
+  if(G2O_FAST_MATH)
+    set(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE} -ffast-math")
+  endif()
+
+  # switch off optimization for debug builds
+  set(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS_DEBUG} -O0")
+  set(CMAKE_C_FLAGS_DEBUG "${CMAKE_C_FLAGS_DEBUG} -O0")
+
+  # OS X
+  #if(${CMAKE_SYSTEM_NAME} MATCHES "Darwin")
+    #set(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE}")
+    #set(CMAKE_C_FLAGS_RELEASE "${CMAKE_C_FLAGS_RELEASE}")
+  #endif(${CMAKE_SYSTEM_NAME} MATCHES "Darwin")
+  # Linux
+  if(BUILD_WITH_MARCH_NATIVE AND NOT "${CMAKE_SYSTEM_PROCESSOR}" MATCHES "arm" AND "${CMAKE_SYSTEM_NAME}" MATCHES "Linux")
+    set(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE} -march=native")
+    set(CMAKE_C_FLAGS_RELEASE "${CMAKE_C_FLAGS_RELEASE} -march=native")
+  endif()
+  # activate warnings !!!
+  set(g2o_C_FLAGS "${g2o_C_FLAGS} -Wall -W")
+  set(g2o_CXX_FLAGS "${g2o_CXX_FLAGS} -Wall -W")
+endif(CMAKE_COMPILER_IS_GNUCXX)
+
+if(CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  message(STATUS "Compiling with Clang")
+
+  # activate all warnings
+  #set(g2o_C_FLAGS "${g2o_C_FLAGS} -Weverything")
+  #set(g2o_CXX_FLAGS "${g2o_CXX_FLAGS} -Weverything")
+  set(g2o_C_FLAGS "${g2o_C_FLAGS} -Wall")
+  set(g2o_CXX_FLAGS "${g2o_CXX_FLAGS} -Wall")
+  #set(g2o_CXX_FLAGS "${g2o_CXX_FLAGS} -Wall -stdlib=libc++")
+endif()
+
+if(MSVC)
+  message(STATUS "Compiling with MSVC")
+
+  if (CMAKE_GENERATOR MATCHES "ARM(64)?$")
+    set(MSVC_ARM ON)
+  endif()
+
+  add_definitions(-DNOMINMAX)
+  add_definitions(-D_USE_MATH_DEFINES)
+
+  # exception handling
+  add_definitions("/EHsc")
+
+  if (G2O_FAST_MATH)
+    set(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE} /fp:fast")
+  endif()
+
+  set(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE} /Ox /Oi")
+  set(CMAKE_C_FLAGS_RELEASE "${CMAKE_C_FLAGS_RELEASE} /Ox /Oi")
+
+  set(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS_DEBUG} -O0")
+  set(CMAKE_C_FLAGS_DEBUG "${CMAKE_C_FLAGS_DEBUG} -O0")
+
+  # SSE2 optimizations
+  # No need to specify if building for x64 (actually, it generates an annoying warning)
+  if (NOT MSVC_ARM)
+    if(NOT CMAKE_SIZEOF_VOID_P EQUAL 8)
+        add_definitions("/arch:SSE2")
+    endif()
+  endif()
+
+  if (BUILD_SHARED_LIBS)
+    # disable warning on missing DLL interfaces
+    add_definitions("/wd4251")
+  endif()
+
+  # Fix issue: https://github.com/RainerKuemmerle/g2o/issues/66
+  #            Link error LNK2005 due to duplicated symbols
+  add_definitions("/Ob2")
+  # Fix other stupid warnings:
+  add_definitions(-D_CRT_SECURE_NO_WARNINGS=1)  # Avoid deprecated fprintf(), etc.
+  add_definitions("/nologo")
+  # TODO not sure this should be a thing
+  add_definitions("/wd4244") # Conversion from number_t -> int
+  add_definitions("/wd4267") # Conversion during return
+  add_definitions("/wd4522") # Duplicated operator=() in Eigen headers
+
+endif(MSVC)
+
+# C++11 support
+set(CMAKE_CXX_STANDARD 11)
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
+
+# specifying compiler flags
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${g2o_CXX_FLAGS}")
+set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${g2o_C_FLAGS}")
+
+# Find Eigen3. If it defines the target, this is used. If not,
+# fall back to the using the module form.
+# See https://eigen.tuxfamily.org/dox/TopicCMakeGuide.html for details
+find_package(Eigen3 REQUIRED)
+if (TARGET Eigen3::Eigen)
+  set(G2O_EIGEN3_EIGEN_TARGET Eigen3::Eigen)
+else()
+  include_directories(${EIGEN3_INCLUDE_DIR})
+endif ()
+
+# Set up the top-level include directories
+include_directories(${g2o_SOURCE_DIR} ${PROJECT_BINARY_DIR})
+
+# Generate config.h
+set(G2O_OPENGL_FOUND ${OPENGL_FOUND})
+set(G2O_HAVE_CHOLMOD ${CHOLMOD_FOUND})
+set(G2O_HAVE_CSPARSE ${CSPARSE_FOUND})
+set(G2O_SHARED_LIBS ${BUILD_SHARED_LIBS})
+set(G2O_LGPL_SHARED_LIBS ${BUILD_LGPL_SHARED_LIBS})
+set(G2O_CXX_COMPILER "${CMAKE_CXX_COMPILER_ID} ${CMAKE_CXX_COMPILER}")
+
+configure_file(config.h.in "${PROJECT_BINARY_DIR}/g2o/config.h")
+install(FILES ${PROJECT_BINARY_DIR}/g2o/config.h DESTINATION ${INCLUDES_DESTINATION}/g2o)
+
+
+# Generate cmake configuration scripts
+set(G2O_GENERATED_DIR "${CMAKE_CURRENT_BINARY_DIR}/generated")
+set(G2O_VERSION_CONFIG "${G2O_GENERATED_DIR}/${PROJECT_NAME}ConfigVersion.cmake")
+set(G2O_PROJECT_CONFIG "${G2O_GENERATED_DIR}/${PROJECT_NAME}Config.cmake")
+set(G2O_TARGETS_EXPORT_NAME "${PROJECT_NAME}Targets")
+set(G2O_CONFIG_INSTALL_DIR "lib/cmake/${PROJECT_NAME}")
+set(G2O_NAMESPACE "${PROJECT_NAME}::")
+set(G2O_VERSION 1.0.0)
+
+include(CMakePackageConfigHelpers)
+WRITE_BASIC_PACKAGE_VERSION_FILE(
+    "${G2O_VERSION_CONFIG}" VERSION ${G2O_VERSION} COMPATIBILITY SameMajorVersion
+)
+
+configure_file("${g2o_SOURCE_DIR}/cmake_modules/Config.cmake.in" "${G2O_PROJECT_CONFIG}" @ONLY)
+
+
+install(
+    FILES "${G2O_PROJECT_CONFIG}" "${G2O_VERSION_CONFIG}"
+    DESTINATION "${G2O_CONFIG_INSTALL_DIR}")
+
+install(
+    EXPORT "${G2O_TARGETS_EXPORT_NAME}"
+    NAMESPACE "${G2O_NAMESPACE}"
+    DESTINATION "${G2O_CONFIG_INSTALL_DIR}")
+
+# building unit test framework and our tests
+option(BUILD_UNITTESTS "build unit test framework and the tests" OFF)
+if(BUILD_UNITTESTS)
+  enable_testing()
+  add_subdirectory(unit_test)
+endif()
+
+# Include the subdirectories
+add_subdirectory(EXTERNAL)
+add_subdirectory(g2o)

workspace/slam_in_autonomous_driving/thirdparty/g2o/EXTERNAL/CMakeLists.txt

@@ -0,0 +1,8 @@
+# Only build CSPARSE if we need to
+if(BUILD_CSPARSE AND G2O_USE_CSPARSE)
+  add_subdirectory(csparse)
+endif()
+
+if (G2O_HAVE_OPENGL)
+  add_subdirectory(freeglut)
+endif()

workspace/slam_in_autonomous_driving/thirdparty/g2o/EXTERNAL/ceres/LICENSE

@@ -0,0 +1,27 @@
+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.

workspace/slam_in_autonomous_driving/thirdparty/g2o/EXTERNAL/ceres/autodiff.h

@@ -0,0 +1,301 @@
+// 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: keir@google.com (Keir Mierle)
+//
+// Computation of the Jacobian matrix for vector-valued functions of multiple
+// variables, using automatic differentiation based on the implementation of
+// dual numbers in jet.h. Before reading the rest of this file, it is adivsable
+// to read jet.h's header comment in detail.
+//
+// The helper wrapper AutoDiff::Differentiate() computes the jacobian of
+// functors with templated operator() taking this form:
+//
+//   struct F {
+//     template<typename T>
+//     bool operator()(const T *x, const T *y, ..., T *z) {
+//       // Compute z[] based on x[], y[], ...
+//       // return true if computation succeeded, false otherwise.
+//     }
+//   };
+//
+// All inputs and outputs may be vector-valued.
+//
+// To understand how jets are used to compute the jacobian, a
+// picture may help. Consider a vector-valued function, F, returning 3
+// dimensions and taking a vector-valued parameter of 4 dimensions:
+//
+//     y            x
+//   [ * ]    F   [ * ]
+//   [ * ]  <---  [ * ]
+//   [ * ]        [ * ]
+//                [ * ]
+//
+// Similar to the 2-parameter example for f described in jet.h, computing the
+// jacobian dy/dx is done by substutiting a suitable jet object for x and all
+// intermediate steps of the computation of F. Since x is has 4 dimensions, use
+// a Jet<double, 4>.
+//
+// Before substituting a jet object for x, the dual components are set
+// appropriately for each dimension of x:
+//
+//          y                       x
+//   [ * | * * * * ]    f   [ * | 1 0 0 0 ]   x0
+//   [ * | * * * * ]  <---  [ * | 0 1 0 0 ]   x1
+//   [ * | * * * * ]        [ * | 0 0 1 0 ]   x2
+//         ---+---          [ * | 0 0 0 1 ]   x3
+//            |                   ^ ^ ^ ^
+//          dy/dx                 | | | +----- infinitesimal for x3
+//                                | | +------- infinitesimal for x2
+//                                | +--------- infinitesimal for x1
+//                                +----------- infinitesimal for x0
+//
+// The reason to set the internal 4x4 submatrix to the identity is that we wish
+// to take the derivative of y separately with respect to each dimension of x.
+// Each column of the 4x4 identity is therefore for a single component of the
+// independent variable x.
+//
+// Then the jacobian of the mapping, dy/dx, is the 3x4 sub-matrix of the
+// extended y vector, indicated in the above diagram.
+//
+// Functors with multiple parameters
+// ---------------------------------
+// In practice, it is often convenient to use a function f of two or more
+// vector-valued parameters, for example, x[3] and z[6]. Unfortunately, the jet
+// framework is designed for a single-parameter vector-valued input. The wrapper
+// in this file addresses this issue adding support for functions with one or
+// more parameter vectors.
+//
+// To support multiple parameters, all the parameter vectors are concatenated
+// into one and treated as a single parameter vector, except that since the
+// functor expects different inputs, we need to construct the jets as if they
+// were part of a single parameter vector. The extended jets are passed
+// separately for each parameter.
+//
+// For example, consider a functor F taking two vector parameters, p[2] and
+// q[3], and producing an output y[4]:
+//
+//   struct F {
+//     template<typename T>
+//     bool operator()(const T *p, const T *q, T *z) {
+//       // ...
+//     }
+//   };
+//
+// In this case, the necessary jet type is Jet<double, 5>. Here is a
+// visualization of the jet objects in this case:
+//
+//          Dual components for p ----+
+//                                    |
+//                                   -+-
+//           y                 [ * | 1 0 | 0 0 0 ]    --- p[0]
+//                             [ * | 0 1 | 0 0 0 ]    --- p[1]
+//   [ * | . . | + + + ]         |
+//   [ * | . . | + + + ]         v
+//   [ * | . . | + + + ]  <--- F(p, q)
+//   [ * | . . | + + + ]            ^
+//         ^^^   ^^^^^              |
+//        dy/dp  dy/dq            [ * | 0 0 | 1 0 0 ] --- q[0]
+//                                [ * | 0 0 | 0 1 0 ] --- q[1]
+//                                [ * | 0 0 | 0 0 1 ] --- q[2]
+//                                            --+--
+//                                              |
+//          Dual components for q --------------+
+//
+// where the 4x2 submatrix (marked with ".") and 4x3 submatrix (marked with "+"
+// of y in the above diagram are the derivatives of y with respect to p and q
+// respectively. This is how autodiff works for functors taking multiple vector
+// valued arguments (up to 6).
+//
+// Jacobian NULL pointers
+// ----------------------
+// In general, the functions below will accept NULL pointers for all or some of
+// the Jacobian parameters, meaning that those Jacobians will not be computed.
+
+#ifndef CERES_PUBLIC_INTERNAL_AUTODIFF_H_
+#define CERES_PUBLIC_INTERNAL_AUTODIFF_H_
+
+#include <stddef.h>
+
+#include "jet.h"
+#include "eigen.h"
+#include "fixed_array.h"
+#include "variadic_evaluate.h"
+
+namespace ceres {
+namespace internal {
+
+// Extends src by a 1st order pertubation for every dimension and puts it in
+// dst. The size of src is N. Since this is also used for perturbations in
+// blocked arrays, offset is used to shift which part of the jet the
+// perturbation occurs. This is used to set up the extended x augmented by an
+// identity matrix. The JetT type should be a Jet type, and T should be a
+// numeric type (e.g. double). For example,
+//
+//             0   1 2   3 4 5   6 7 8
+//   dst[0]  [ * | . . | 1 0 0 | . . . ]
+//   dst[1]  [ * | . . | 0 1 0 | . . . ]
+//   dst[2]  [ * | . . | 0 0 1 | . . . ]
+//
+// is what would get put in dst if N was 3, offset was 3, and the jet type JetT
+// was 8-dimensional.
+template <typename JetT, typename T, int N>
+inline void Make1stOrderPerturbation(int offset, const T* src, JetT* dst) {
+  for (int j = 0; j < N; ++j) {
+    dst[j].a = src[j];
+    dst[j].v.setZero();
+    dst[j].v[offset + j] = T(1.0);
+  }
+}
+
+// Takes the 0th order part of src, assumed to be a Jet type, and puts it in
+// dst. This is used to pick out the "vector" part of the extended y.
+template <typename JetT, typename T>
+inline void Take0thOrderPart(int M, const JetT *src, T dst) {
+  for (int i = 0; i < M; ++i) {
+    dst[i] = src[i].a;
+  }
+}
+
+// Takes N 1st order parts, starting at index N0, and puts them in the M x N
+// matrix 'dst'. This is used to pick out the "matrix" parts of the extended y.
+template <typename JetT, typename T, int N0, int N>
+inline void Take1stOrderPart(const int M, const JetT *src, T *dst) {
+  for (int i = 0; i < M; ++i) {
+    Eigen::Map<Eigen::Matrix<T, N, 1> >(dst + N * i, N) =
+        src[i].v.template segment<N>(N0);
+  }
+}
+
+// This is in a struct because default template parameters on a
+// function are not supported in C++03 (though it is available in
+// C++0x). N0 through N5 are the dimension of the input arguments to
+// the user supplied functor.
+template <typename Functor, typename T,
+          int N0 = 0, int N1 = 0, int N2 = 0, int N3 = 0, int N4 = 0,
+          int N5 = 0, int N6 = 0, int N7 = 0, int N8 = 0, int N9 = 0>
+struct AutoDiff {
+  static bool Differentiate(const Functor& functor,
+                            T const *const *parameters,
+                            int num_outputs,
+                            T *function_value,
+                            T **jacobians) {
+    typedef Jet<T, N0 + N1 + N2 + N3 + N4 + N5 + N6 + N7 + N8 + N9> JetT;
+    FixedArray<JetT, (256 * 7) / sizeof(JetT)> x(
+        N0 + N1 + N2 + N3 + N4 + N5 + N6 + N7 + N8 + N9 + num_outputs);
+
+    // These are the positions of the respective jets in the fixed array x.
+    const int jet0  = 0;
+    const int jet1  = N0;
+    const int jet2  = N0 + N1;
+    const int jet3  = N0 + N1 + N2;
+    const int jet4  = N0 + N1 + N2 + N3;
+    const int jet5  = N0 + N1 + N2 + N3 + N4;
+    const int jet6  = N0 + N1 + N2 + N3 + N4 + N5;
+    const int jet7  = N0 + N1 + N2 + N3 + N4 + N5 + N6;
+    const int jet8  = N0 + N1 + N2 + N3 + N4 + N5 + N6 + N7;
+    const int jet9  = N0 + N1 + N2 + N3 + N4 + N5 + N6 + N7 + N8;
+
+    const JetT *unpacked_parameters[10] = {
+        x.get() + jet0,
+        x.get() + jet1,
+        x.get() + jet2,
+        x.get() + jet3,
+        x.get() + jet4,
+        x.get() + jet5,
+        x.get() + jet6,
+        x.get() + jet7,
+        x.get() + jet8,
+        x.get() + jet9,
+    };
+
+    JetT* output = x.get() + N0 + N1 + N2 + N3 + N4 + N5 + N6 + N7 + N8 + N9;
+
+    // Invalidate the output Jets, so that we can detect if the user
+    // did not assign values to all of them.
+    for (int i = 0; i < num_outputs; ++i) {
+      output[i].a = kImpossibleValue;
+      output[i].v.setConstant(kImpossibleValue);
+    }
+
+#define CERES_MAKE_1ST_ORDER_PERTURBATION(i)                            \
+    if (N ## i) {                                                       \
+      internal::Make1stOrderPerturbation<JetT, T, N ## i>(              \
+          jet ## i,                                                     \
+          parameters[i],                                                \
+          x.get() + jet ## i);                                          \
+    }
+    CERES_MAKE_1ST_ORDER_PERTURBATION(0);
+    CERES_MAKE_1ST_ORDER_PERTURBATION(1);
+    CERES_MAKE_1ST_ORDER_PERTURBATION(2);
+    CERES_MAKE_1ST_ORDER_PERTURBATION(3);
+    CERES_MAKE_1ST_ORDER_PERTURBATION(4);
+    CERES_MAKE_1ST_ORDER_PERTURBATION(5);
+    CERES_MAKE_1ST_ORDER_PERTURBATION(6);
+    CERES_MAKE_1ST_ORDER_PERTURBATION(7);
+    CERES_MAKE_1ST_ORDER_PERTURBATION(8);
+    CERES_MAKE_1ST_ORDER_PERTURBATION(9);
+#undef CERES_MAKE_1ST_ORDER_PERTURBATION
+
+    if (!VariadicEvaluate<Functor, JetT,
+                          N0, N1, N2, N3, N4, N5, N6, N7, N8, N9>::Call(
+        functor, unpacked_parameters, output)) {
+      return false;
+    }
+
+    internal::Take0thOrderPart(num_outputs, output, function_value);
+
+#define CERES_TAKE_1ST_ORDER_PERTURBATION(i) \
+    if (N ## i) { \
+      if (jacobians[i]) { \
+        internal::Take1stOrderPart<JetT, T, \
+                                   jet ## i, \
+                                   N ## i>(num_outputs, \
+                                           output, \
+                                           jacobians[i]); \
+      } \
+    }
+    CERES_TAKE_1ST_ORDER_PERTURBATION(0);
+    CERES_TAKE_1ST_ORDER_PERTURBATION(1);
+    CERES_TAKE_1ST_ORDER_PERTURBATION(2);
+    CERES_TAKE_1ST_ORDER_PERTURBATION(3);
+    CERES_TAKE_1ST_ORDER_PERTURBATION(4);
+    CERES_TAKE_1ST_ORDER_PERTURBATION(5);
+    CERES_TAKE_1ST_ORDER_PERTURBATION(6);
+    CERES_TAKE_1ST_ORDER_PERTURBATION(7);
+    CERES_TAKE_1ST_ORDER_PERTURBATION(8);
+    CERES_TAKE_1ST_ORDER_PERTURBATION(9);
+#undef CERES_TAKE_1ST_ORDER_PERTURBATION
+    return true;
+  }
+};
+
+}  // namespace internal
+}  // namespace ceres
+
+#endif  // CERES_PUBLIC_INTERNAL_AUTODIFF_H_

workspace/slam_in_autonomous_driving/thirdparty/g2o/EXTERNAL/ceres/eigen.h

@@ -0,0 +1,93 @@
+// 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: sameeragarwal@google.com (Sameer Agarwal)
+
+#ifndef CERES_INTERNAL_EIGEN_H_
+#define CERES_INTERNAL_EIGEN_H_
+
+#include "Eigen/Core"
+
+namespace ceres {
+
+typedef Eigen::Matrix<double, Eigen::Dynamic, 1> Vector;
+typedef Eigen::Matrix<double,
+                      Eigen::Dynamic,
+                      Eigen::Dynamic,
+                      Eigen::RowMajor> Matrix;
+typedef Eigen::Map<Vector> VectorRef;
+typedef Eigen::Map<Matrix> MatrixRef;
+typedef Eigen::Map<const Vector> ConstVectorRef;
+typedef Eigen::Map<const Matrix> ConstMatrixRef;
+
+// Column major matrices for DenseSparseMatrix/DenseQRSolver
+typedef Eigen::Matrix<double,
+                      Eigen::Dynamic,
+                      Eigen::Dynamic,
+                      Eigen::ColMajor> ColMajorMatrix;
+
+typedef Eigen::Map<ColMajorMatrix, 0,
+                   Eigen::Stride<Eigen::Dynamic, 1> > ColMajorMatrixRef;
+
+typedef Eigen::Map<const ColMajorMatrix,
+                   0,
+                   Eigen::Stride<Eigen::Dynamic, 1> > ConstColMajorMatrixRef;
+
+
+
+// C++ does not support templated typdefs, thus the need for this
+// struct so that we can support statically sized Matrix and Maps.
+template <int num_rows = Eigen::Dynamic, int num_cols = Eigen::Dynamic>
+struct EigenTypes {
+  typedef Eigen::Matrix <double, num_rows, num_cols, Eigen::RowMajor>
+  Matrix;
+
+  typedef Eigen::Map<
+    Eigen::Matrix<double, num_rows, num_cols, Eigen::RowMajor> >
+  MatrixRef;
+
+  typedef Eigen::Matrix <double, num_rows, 1>
+  Vector;
+
+  typedef Eigen::Map <
+    Eigen::Matrix<double, num_rows, 1> >
+  VectorRef;
+
+
+  typedef Eigen::Map<
+    const Eigen::Matrix<double, num_rows, num_cols, Eigen::RowMajor> >
+  ConstMatrixRef;
+
+  typedef Eigen::Map <
+    const Eigen::Matrix<double, num_rows, 1> >
+  ConstVectorRef;
+};
+
+}  // namespace ceres
+
+#endif  // CERES_INTERNAL_EIGEN_H_

workspace/slam_in_autonomous_driving/thirdparty/g2o/EXTERNAL/ceres/fixed_array.h

@@ -0,0 +1,188 @@
+// 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: rennie@google.com (Jeffrey Rennie)
+// Author: sanjay@google.com (Sanjay Ghemawat) -- renamed to FixedArray
+
+#ifndef CERES_PUBLIC_INTERNAL_FIXED_ARRAY_H_
+#define CERES_PUBLIC_INTERNAL_FIXED_ARRAY_H_
+
+#include <cstddef>
+#include "Eigen/Core"
+#include "macros.h"
+#include "manual_constructor.h"
+
+namespace ceres {
+namespace internal {
+
+// A FixedArray<T> represents a non-resizable array of T where the
+// length of the array does not need to be a compile time constant.
+//
+// FixedArray allocates small arrays inline, and large arrays on
+// the heap.  It is a good replacement for non-standard and deprecated
+// uses of alloca() and variable length arrays (a GCC extension).
+//
+// FixedArray keeps performance fast for small arrays, because it
+// avoids heap operations.  It also helps reduce the chances of
+// accidentally overflowing your stack if large input is passed to
+// your function.
+//
+// Also, FixedArray is useful for writing portable code.  Not all
+// compilers support arrays of dynamic size.
+
+// Most users should not specify an inline_elements argument and let
+// FixedArray<> automatically determine the number of elements
+// to store inline based on sizeof(T).
+//
+// If inline_elements is specified, the FixedArray<> implementation
+// will store arrays of length <= inline_elements inline.
+//
+// Finally note that unlike vector<T> FixedArray<T> will not zero-initialize
+// simple types like int, double, bool, etc.
+//
+// Non-POD types will be default-initialized just like regular vectors or
+// arrays.
+
+#if defined(_WIN64)
+   typedef __int64      ssize_t;
+#elif defined(_WIN32)
+   typedef __int32      ssize_t;
+#endif
+
+template <typename T, ssize_t inline_elements = -1>
+class FixedArray {
+ public:
+  // For playing nicely with stl:
+  typedef T value_type;
+  typedef T* iterator;
+  typedef T const* const_iterator;
+  typedef T& reference;
+  typedef T const& const_reference;
+  typedef T* pointer;
+  typedef std::ptrdiff_t difference_type;
+  typedef size_t size_type;
+
+  // REQUIRES: n >= 0
+  // Creates an array object that can store "n" elements.
+  //
+  // FixedArray<T> will not zero-initialiaze POD (simple) types like int,
+  // double, bool, etc.
+  // Non-POD types will be default-initialized just like regular vectors or
+  // arrays.
+  explicit FixedArray(size_type n);
+
+  // Releases any resources.
+  ~FixedArray();
+
+  // Returns the length of the array.
+  inline size_type size() const { return size_; }
+
+  // Returns the memory size of the array in bytes.
+  inline size_t memsize() const { return size_ * sizeof(T); }
+
+  // Returns a pointer to the underlying element array.
+  inline const T* get() const { return &array_[0].element; }
+  inline T* get() { return &array_[0].element; }
+
+  // REQUIRES: 0 <= i < size()
+  // Returns a reference to the "i"th element.
+  inline T& operator[](size_type i) {
+    return array_[i].element;
+  }
+
+  // REQUIRES: 0 <= i < size()
+  // Returns a reference to the "i"th element.
+  inline const T& operator[](size_type i) const {
+    return array_[i].element;
+  }
+
+  inline iterator begin() { return &array_[0].element; }
+  inline iterator end() { return &array_[size_].element; }
+
+  inline const_iterator begin() const { return &array_[0].element; }
+  inline const_iterator end() const { return &array_[size_].element; }
+
+ private:
+  // Container to hold elements of type T.  This is necessary to handle
+  // the case where T is a a (C-style) array.  The size of InnerContainer
+  // and T must be the same, otherwise callers' assumptions about use
+  // of this code will be broken.
+  struct InnerContainer {
+    T element;
+  };
+
+  // How many elements should we store inline?
+  //   a. If not specified, use a default of 256 bytes (256 bytes
+  //      seems small enough to not cause stack overflow or unnecessary
+  //      stack pollution, while still allowing stack allocation for
+  //      reasonably long character arrays.
+  //   b. Never use 0 length arrays (not ISO C++)
+  static const size_type S1 = ((inline_elements < 0)
+                               ? (256/sizeof(T)) : inline_elements);
+  static const size_type S2 = (S1 <= 0) ? 1 : S1;
+  static const size_type kInlineElements = S2;
+
+  size_type const       size_;
+  InnerContainer* const array_;
+
+  // Allocate some space, not an array of elements of type T, so that we can
+  // skip calling the T constructors and destructors for space we never use.
+  ManualConstructor<InnerContainer> inline_space_[kInlineElements];
+};
+
+// Implementation details follow
+
+template <class T, ssize_t S>
+inline FixedArray<T, S>::FixedArray(typename FixedArray<T, S>::size_type n)
+    : size_(n),
+      array_((n <= kInlineElements
+              ? reinterpret_cast<InnerContainer*>(inline_space_)
+              : new InnerContainer[n])) {
+  // Construct only the elements actually used.
+  if (array_ == reinterpret_cast<InnerContainer*>(inline_space_)) {
+    for (size_t i = 0; i != size_; ++i) {
+      inline_space_[i].Init();
+    }
+  }
+}
+
+template <class T, ssize_t S>
+inline FixedArray<T, S>::~FixedArray() {
+  if (array_ != reinterpret_cast<InnerContainer*>(inline_space_)) {
+    delete[] array_;
+  } else {
+    for (size_t i = 0; i != size_; ++i) {
+      inline_space_[i].Destroy();
+    }
+  }
+}
+
+}  // namespace internal
+}  // namespace ceres
+
+#endif  // CERES_PUBLIC_INTERNAL_FIXED_ARRAY_H_

workspace/slam_in_autonomous_driving/thirdparty/g2o/EXTERNAL/ceres/fpclassify.h

@@ -0,0 +1,81 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2012 Google Inc. All rights reserved.
+// http://code.google.com/p/ceres-solver/
+//
+// 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: keir@google.com (Keir Mierle)
+//
+// Portable floating point classification. The names are picked such that they
+// do not collide with macros. For example, "isnan" in C99 is a macro and hence
+// does not respect namespaces.
+//
+// TODO(keir): Finish porting!
+
+#ifndef CERES_PUBLIC_FPCLASSIFY_H_
+#define CERES_PUBLIC_FPCLASSIFY_H_
+
+#if defined(_MSC_VER)
+#include <float.h>
+#endif
+
+#include <limits>
+
+namespace ceres {
+
+#if defined(_MSC_VER)
+inline bool IsFinite  (double x) { return _finite(x);                }
+inline bool IsInfinite(double x) { return !_finite(x) && !_isnan(x); }
+inline bool IsNaN     (double x) { return _isnan(x);                 }
+inline bool IsNormal  (double x) {
+  int classification = _fpclass(x);
+  return classification == _FPCLASS_NN ||
+         classification == _FPCLASS_PN;
+}
+#elif defined(ANDROID)
+
+// On Android NDK r6, when using STLPort, the isinf and isfinite functions are
+// not available, so reimplement them.
+#  if defined(_STLPORT_VERSION)
+inline bool IsInfinite(double x) {
+  return x ==  std::numeric_limits<double>::infinity() ||
+         x == -std::numeric_limits<double>::infinity();
+}
+inline bool IsFinite(double x) {
+  return !isnan(x) && !IsInfinite(x);
+}
+#  endif  // defined(_STLPORT_VERSION)
+#else
+// These definitions are for the normal Unix suspects.
+// TODO(keir): Test the "else" with more platforms.
+inline bool IsFinite  (double x) { return std::isfinite(x); }
+inline bool IsInfinite(double x) { return std::isinf(x);    }
+inline bool IsNaN     (double x) { return std::isnan(x);    }
+inline bool IsNormal  (double x) { return std::isnormal(x); }
+#endif
+
+}  // namespace ceres
+
+#endif  // CERES_PUBLIC_FPCLASSIFY_H_

workspace/slam_in_autonomous_driving/thirdparty/g2o/EXTERNAL/ceres/jet.h

@@ -0,0 +1,753 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2010, 2011, 2012 Google Inc. All rights reserved.
+// http://code.google.com/p/ceres-solver/
+//
+// 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: keir@google.com (Keir Mierle)
+//
+// A simple implementation of N-dimensional dual numbers, for automatically
+// computing exact derivatives of functions.
+//
+// While a complete treatment of the mechanics of automatic differentation is
+// beyond the scope of this header (see
+// http://en.wikipedia.org/wiki/Automatic_differentiation for details), the
+// basic idea is to extend normal arithmetic with an extra element, "e," often
+// denoted with the greek symbol epsilon, such that e != 0 but e^2 = 0. Dual
+// numbers are extensions of the real numbers analogous to complex numbers:
+// whereas complex numbers augment the reals by introducing an imaginary unit i
+// such that i^2 = -1, dual numbers introduce an "infinitesimal" unit e such
+// that e^2 = 0. Dual numbers have two components: the "real" component and the
+// "infinitesimal" component, generally written as x + y*e. Surprisingly, this
+// leads to a convenient method for computing exact derivatives without needing
+// to manipulate complicated symbolic expressions.
+//
+// For example, consider the function
+//
+//   f(x) = x^2 ,
+//
+// evaluated at 10. Using normal arithmetic, f(10) = 100, and df/dx(10) = 20.
+// Next, augument 10 with an infinitesimal to get:
+//
+//   f(10 + e) = (10 + e)^2
+//             = 100 + 2 * 10 * e + e^2
+//             = 100 + 20 * e       -+-
+//                     --            |
+//                     |             +--- This is zero, since e^2 = 0
+//                     |
+//                     +----------------- This is df/dx!
+//
+// Note that the derivative of f with respect to x is simply the infinitesimal
+// component of the value of f(x + e). So, in order to take the derivative of
+// any function, it is only necessary to replace the numeric "object" used in
+// the function with one extended with infinitesimals. The class Jet, defined in
+// this header, is one such example of this, where substitution is done with
+// templates.
+//
+// To handle derivatives of functions taking multiple arguments, different
+// infinitesimals are used, one for each variable to take the derivative of. For
+// example, consider a scalar function of two scalar parameters x and y:
+//
+//   f(x, y) = x^2 + x * y
+//
+// Following the technique above, to compute the derivatives df/dx and df/dy for
+// f(1, 3) involves doing two evaluations of f, the first time replacing x with
+// x + e, the second time replacing y with y + e.
+//
+// For df/dx:
+//
+//   f(1 + e, y) = (1 + e)^2 + (1 + e) * 3
+//               = 1 + 2 * e + 3 + 3 * e
+//               = 4 + 5 * e
+//
+//               --> df/dx = 5
+//
+// For df/dy:
+//
+//   f(1, 3 + e) = 1^2 + 1 * (3 + e)
+//               = 1 + 3 + e
+//               = 4 + e
+//
+//               --> df/dy = 1
+//
+// To take the gradient of f with the implementation of dual numbers ("jets") in
+// this file, it is necessary to create a single jet type which has components
+// for the derivative in x and y, and passing them to a templated version of f:
+//
+//   template<typename T>
+//   T f(const T &x, const T &y) {
+//     return x * x + x * y;
+//   }
+//
+//   // The "2" means there should be 2 dual number components.
+//   Jet<double, 2> x(0);  // Pick the 0th dual number for x.
+//   Jet<double, 2> y(1);  // Pick the 1st dual number for y.
+//   Jet<double, 2> z = f(x, y);
+//
+//   LG << "df/dx = " << z.a[0]
+//      << "df/dy = " << z.a[1];
+//
+// Most users should not use Jet objects directly; a wrapper around Jet objects,
+// which makes computing the derivative, gradient, or jacobian of templated
+// functors simple, is in autodiff.h. Even autodiff.h should not be used
+// directly; instead autodiff_cost_function.h is typically the file of interest.
+//
+// For the more mathematically inclined, this file implements first-order
+// "jets". A 1st order jet is an element of the ring
+//
+//   T[N] = T[t_1, ..., t_N] / (t_1, ..., t_N)^2
+//
+// which essentially means that each jet consists of a "scalar" value 'a' from T
+// and a 1st order perturbation vector 'v' of length N:
+//
+//   x = a + \sum_i v[i] t_i
+//
+// A shorthand is to write an element as x = a + u, where u is the pertubation.
+// Then, the main point about the arithmetic of jets is that the product of
+// perturbations is zero:
+//
+//   (a + u) * (b + v) = ab + av + bu + uv
+//                     = ab + (av + bu) + 0
+//
+// which is what operator* implements below. Addition is simpler:
+//
+//   (a + u) + (b + v) = (a + b) + (u + v).
+//
+// The only remaining question is how to evaluate the function of a jet, for
+// which we use the chain rule:
+//
+//   f(a + u) = f(a) + f'(a) u
+//
+// where f'(a) is the (scalar) derivative of f at a.
+//
+// By pushing these things through sufficiently and suitably templated
+// functions, we can do automatic differentiation. Just be sure to turn on
+// function inlining and common-subexpression elimination, or it will be very
+// slow!
+//
+// WARNING: Most Ceres users should not directly include this file or know the
+// details of how jets work. Instead the suggested method for automatic
+// derivatives is to use autodiff_cost_function.h, which is a wrapper around
+// both jets.h and autodiff.h to make taking derivatives of cost functions for
+// use in Ceres easier.
+
+#ifndef CERES_PUBLIC_JET_H_
+#define CERES_PUBLIC_JET_H_
+
+#include <cmath>
+#include <iosfwd>
+#include <iostream>  // NOLINT
+#include <string>
+
+#include "Eigen/Core"
+#include "fpclassify.h"
+
+namespace ceres {
+
+template <typename T, int N>
+struct Jet {
+  enum { DIMENSION = N };
+
+  // Default-construct "a" because otherwise this can lead to false errors about
+  // uninitialized uses when other classes relying on default constructed T
+  // (where T is a Jet<T, N>). This usually only happens in opt mode. Note that
+  // the C++ standard mandates that e.g. default constructed doubles are
+  // initialized to 0.0; see sections 8.5 of the C++03 standard.
+  Jet() : a() {
+    v.setZero();
+  }
+
+  // Constructor from scalar: a + 0.
+  explicit Jet(const T& value) {
+    a = value;
+    v.setZero();
+  }
+
+  // Constructor from scalar plus variable: a + t_i.
+  Jet(const T& value, int k) {
+    a = value;
+    v.setZero();
+    v[k] = T(1.0);
+  }
+
+  // Compound operators
+  Jet<T, N>& operator+=(const Jet<T, N> &y) {
+    *this = *this + y;
+    return *this;
+  }
+
+  Jet<T, N>& operator-=(const Jet<T, N> &y) {
+    *this = *this - y;
+    return *this;
+  }
+
+  Jet<T, N>& operator*=(const Jet<T, N> &y) {
+    *this = *this * y;
+    return *this;
+  }
+
+  Jet<T, N>& operator/=(const Jet<T, N> &y) {
+    *this = *this / y;
+    return *this;
+  }
+
+  // The scalar part.
+  T a;
+
+  // The infinitesimal part.
+  //
+  // Note the Eigen::DontAlign bit is needed here because this object
+  // gets allocated on the stack and as part of other arrays and
+  // structs. Forcing the right alignment there is the source of much
+  // pain and suffering. Even if that works, passing Jets around to
+  // functions by value has problems because the C++ ABI does not
+  // guarantee alignment for function arguments.
+  //
+  // Setting the DontAlign bit prevents Eigen from using SSE for the
+  // various operations on Jets. This is a small performance penalty
+  // since the AutoDiff code will still expose much of the code as
+  // statically sized loops to the compiler. But given the subtle
+  // issues that arise due to alignment, especially when dealing with
+  // multiple platforms, it seems to be a trade off worth making.
+  Eigen::Matrix<T, N, 1, Eigen::DontAlign> v;
+};
+
+// Unary +
+template<typename T, int N> inline
+Jet<T, N> const& operator+(const Jet<T, N>& f) {
+  return f;
+}
+
+// TODO(keir): Try adding __attribute__((always_inline)) to these functions to
+// see if it causes a performance increase.
+
+// Unary -
+template<typename T, int N> inline
+Jet<T, N> operator-(const Jet<T, N>&f) {
+  Jet<T, N> g;
+  g.a = -f.a;
+  g.v = -f.v;
+  return g;
+}
+
+// Binary +
+template<typename T, int N> inline
+Jet<T, N> operator+(const Jet<T, N>& f,
+                    const Jet<T, N>& g) {
+  Jet<T, N> h;
+  h.a = f.a + g.a;
+  h.v = f.v + g.v;
+  return h;
+}
+
+// Binary + with a scalar: x + s
+template<typename T, int N> inline
+Jet<T, N> operator+(const Jet<T, N>& f, T s) {
+  Jet<T, N> h;
+  h.a = f.a + s;
+  h.v = f.v;
+  return h;
+}
+
+// Binary + with a scalar: s + x
+template<typename T, int N> inline
+Jet<T, N> operator+(T s, const Jet<T, N>& f) {
+  Jet<T, N> h;
+  h.a = f.a + s;
+  h.v = f.v;
+  return h;
+}
+
+// Binary -
+template<typename T, int N> inline
+Jet<T, N> operator-(const Jet<T, N>& f,
+                    const Jet<T, N>& g) {
+  Jet<T, N> h;
+  h.a = f.a - g.a;
+  h.v = f.v - g.v;
+  return h;
+}
+
+// Binary - with a scalar: x - s
+template<typename T, int N> inline
+Jet<T, N> operator-(const Jet<T, N>& f, T s) {
+  Jet<T, N> h;
+  h.a = f.a - s;
+  h.v = f.v;
+  return h;
+}
+
+// Binary - with a scalar: s - x
+template<typename T, int N> inline
+Jet<T, N> operator-(T s, const Jet<T, N>& f) {
+  Jet<T, N> h;
+  h.a = s - f.a;
+  h.v = -f.v;
+  return h;
+}
+
+// Binary *
+template<typename T, int N> inline
+Jet<T, N> operator*(const Jet<T, N>& f,
+                    const Jet<T, N>& g) {
+  Jet<T, N> h;
+  h.a = f.a * g.a;
+  h.v = f.a * g.v + f.v * g.a;
+  return h;
+}
+
+// Binary * with a scalar: x * s
+template<typename T, int N> inline
+Jet<T, N> operator*(const Jet<T, N>& f, T s) {
+  Jet<T, N> h;
+  h.a = f.a * s;
+  h.v = f.v * s;
+  return h;
+}
+
+// Binary * with a scalar: s * x
+template<typename T, int N> inline
+Jet<T, N> operator*(T s, const Jet<T, N>& f) {
+  Jet<T, N> h;
+  h.a = f.a * s;
+  h.v = f.v * s;
+  return h;
+}
+
+// Binary /
+template<typename T, int N> inline
+Jet<T, N> operator/(const Jet<T, N>& f,
+                    const Jet<T, N>& g) {
+  Jet<T, N> h;
+  // This uses:
+  //
+  //   a + u   (a + u)(b - v)   (a + u)(b - v)
+  //   ----- = -------------- = --------------
+  //   b + v   (b + v)(b - v)        b^2
+  //
+  // which holds because v*v = 0.
+  const T g_a_inverse = T(1.0) / g.a;
+  h.a = f.a * g_a_inverse;
+  const T f_a_by_g_a = f.a * g_a_inverse;
+  for (int i = 0; i < N; ++i) {
+    h.v[i] = (f.v[i] - f_a_by_g_a * g.v[i]) * g_a_inverse;
+  }
+  return h;
+}
+
+// Binary / with a scalar: s / x
+template<typename T, int N> inline
+Jet<T, N> operator/(T s, const Jet<T, N>& g) {
+  Jet<T, N> h;
+  h.a = s / g.a;
+  const T minus_s_g_a_inverse2 = -s / (g.a * g.a);
+  h.v = g.v * minus_s_g_a_inverse2;
+  return h;
+}
+
+// Binary / with a scalar: x / s
+template<typename T, int N> inline
+Jet<T, N> operator/(const Jet<T, N>& f, T s) {
+  Jet<T, N> h;
+  const T s_inverse = 1.0 / s;
+  h.a = f.a * s_inverse;
+  h.v = f.v * s_inverse;
+  return h;
+}
+
+// Binary comparison operators for both scalars and jets.
+#define CERES_DEFINE_JET_COMPARISON_OPERATOR(op) \
+template<typename T, int N> inline \
+bool operator op(const Jet<T, N>& f, const Jet<T, N>& g) { \
+  return f.a op g.a; \
+} \
+template<typename T, int N> inline \
+bool operator op(const T& s, const Jet<T, N>& g) { \
+  return s op g.a; \
+} \
+template<typename T, int N> inline \
+bool operator op(const Jet<T, N>& f, const T& s) { \
+  return f.a op s; \
+}
+CERES_DEFINE_JET_COMPARISON_OPERATOR( <  )  // NOLINT
+CERES_DEFINE_JET_COMPARISON_OPERATOR( <= )  // NOLINT
+CERES_DEFINE_JET_COMPARISON_OPERATOR( >  )  // NOLINT
+CERES_DEFINE_JET_COMPARISON_OPERATOR( >= )  // NOLINT
+CERES_DEFINE_JET_COMPARISON_OPERATOR( == )  // NOLINT
+CERES_DEFINE_JET_COMPARISON_OPERATOR( != )  // NOLINT
+#undef CERES_DEFINE_JET_COMPARISON_OPERATOR
+
+// Pull some functions from namespace std.
+//
+// This is necessary because we want to use the same name (e.g. 'sqrt') for
+// double-valued and Jet-valued functions, but we are not allowed to put
+// Jet-valued functions inside namespace std.
+//
+// TODO(keir): Switch to "using".
+inline double abs     (double x) { return std::abs(x);      }
+inline double log     (double x) { return std::log(x);      }
+inline double exp     (double x) { return std::exp(x);      }
+inline double sqrt    (double x) { return std::sqrt(x);     }
+inline double cos     (double x) { return std::cos(x);      }
+inline double acos    (double x) { return std::acos(x);     }
+inline double sin     (double x) { return std::sin(x);      }
+inline double asin    (double x) { return std::asin(x);     }
+inline double tan     (double x) { return std::tan(x);      }
+inline double atan    (double x) { return std::atan(x);     }
+inline double sinh    (double x) { return std::sinh(x);     }
+inline double cosh    (double x) { return std::cosh(x);     }
+inline double tanh    (double x) { return std::tanh(x);     }
+inline double pow  (double x, double y) { return std::pow(x, y);   }
+inline double atan2(double y, double x) { return std::atan2(y, x); }
+
+// In general, f(a + h) ~= f(a) + f'(a) h, via the chain rule.
+
+// abs(x + h) ~= x + h or -(x + h)
+template <typename T, int N> inline
+Jet<T, N> abs(const Jet<T, N>& f) {
+  return f.a < T(0.0) ? -f : f;
+}
+
+// log(a + h) ~= log(a) + h / a
+template <typename T, int N> inline
+Jet<T, N> log(const Jet<T, N>& f) {
+  Jet<T, N> g;
+  g.a = log(f.a);
+  const T a_inverse = T(1.0) / f.a;
+  g.v = f.v * a_inverse;
+  return g;
+}
+
+// exp(a + h) ~= exp(a) + exp(a) h
+template <typename T, int N> inline
+Jet<T, N> exp(const Jet<T, N>& f) {
+  Jet<T, N> g;
+  g.a = exp(f.a);
+  g.v = g.a * f.v;
+  return g;
+}
+
+// sqrt(a + h) ~= sqrt(a) + h / (2 sqrt(a))
+template <typename T, int N> inline
+Jet<T, N> sqrt(const Jet<T, N>& f) {
+  Jet<T, N> g;
+  g.a = sqrt(f.a);
+  const T two_a_inverse = T(1.0) / (T(2.0) * g.a);
+  g.v = f.v * two_a_inverse;
+  return g;
+}
+
+// cos(a + h) ~= cos(a) - sin(a) h
+template <typename T, int N> inline
+Jet<T, N> cos(const Jet<T, N>& f) {
+  Jet<T, N> g;
+  g.a = cos(f.a);
+  const T sin_a = sin(f.a);
+  g.v = - sin_a * f.v;
+  return g;
+}
+
+// acos(a + h) ~= acos(a) - 1 / sqrt(1 - a^2) h
+template <typename T, int N> inline
+Jet<T, N> acos(const Jet<T, N>& f) {
+  Jet<T, N> g;
+  g.a = acos(f.a);
+  const T tmp = - T(1.0) / sqrt(T(1.0) - f.a * f.a);
+  g.v = tmp * f.v;
+  return g;
+}
+
+// sin(a + h) ~= sin(a) + cos(a) h
+template <typename T, int N> inline
+Jet<T, N> sin(const Jet<T, N>& f) {
+  Jet<T, N> g;
+  g.a = sin(f.a);
+  const T cos_a = cos(f.a);
+  g.v = cos_a * f.v;
+  return g;
+}
+
+// asin(a + h) ~= asin(a) + 1 / sqrt(1 - a^2) h
+template <typename T, int N> inline
+Jet<T, N> asin(const Jet<T, N>& f) {
+  Jet<T, N> g;
+  g.a = asin(f.a);
+  const T tmp = T(1.0) / sqrt(T(1.0) - f.a * f.a);
+  g.v = tmp * f.v;
+  return g;
+}
+
+// tan(a + h) ~= tan(a) + (1 + tan(a)^2) h
+template <typename T, int N> inline
+Jet<T, N> tan(const Jet<T, N>& f) {
+  Jet<T, N> g;
+  g.a = tan(f.a);
+  double tan_a = tan(f.a);
+  const T tmp = T(1.0) + tan_a * tan_a;
+  g.v = tmp * f.v;
+  return g;
+}
+
+// atan(a + h) ~= atan(a) + 1 / (1 + a^2) h
+template <typename T, int N> inline
+Jet<T, N> atan(const Jet<T, N>& f) {
+  Jet<T, N> g;
+  g.a = atan(f.a);
+  const T tmp = T(1.0) / (T(1.0) + f.a * f.a);
+  g.v = tmp * f.v;
+  return g;
+}
+
+// sinh(a + h) ~= sinh(a) + cosh(a) h
+template <typename T, int N> inline
+Jet<T, N> sinh(const Jet<T, N>& f) {
+  Jet<T, N> g;
+  g.a = sinh(f.a);
+  const T cosh_a = cosh(f.a);
+  g.v = cosh_a * f.v;
+  return g;
+}
+
+// cosh(a + h) ~= cosh(a) + sinh(a) h
+template <typename T, int N> inline
+Jet<T, N> cosh(const Jet<T, N>& f) {
+  Jet<T, N> g;
+  g.a = cosh(f.a);
+  const T sinh_a = sinh(f.a);
+  g.v = sinh_a * f.v;
+  return g;
+}
+
+// tanh(a + h) ~= tanh(a) + (1 - tanh(a)^2) h
+template <typename T, int N> inline
+Jet<T, N> tanh(const Jet<T, N>& f) {
+  Jet<T, N> g;
+  g.a = tanh(f.a);
+  double tanh_a = tanh(f.a);
+  const T tmp = T(1.0) - tanh_a * tanh_a;
+  g.v = tmp * f.v;
+  return g;
+}
+
+// Jet Classification. It is not clear what the appropriate semantics are for
+// these classifications. This picks that IsFinite and isnormal are "all"
+// operations, i.e. all elements of the jet must be finite for the jet itself
+// to be finite (or normal). For IsNaN and IsInfinite, the answer is less
+// clear. This takes a "any" approach for IsNaN and IsInfinite such that if any
+// part of a jet is nan or inf, then the entire jet is nan or inf. This leads
+// to strange situations like a jet can be both IsInfinite and IsNaN, but in
+// practice the "any" semantics are the most useful for e.g. checking that
+// derivatives are sane.
+
+// The jet is finite if all parts of the jet are finite.
+template <typename T, int N> inline
+bool IsFinite(const Jet<T, N>& f) {
+  if (!IsFinite(f.a)) {
+    return false;
+  }
+  for (int i = 0; i < N; ++i) {
+    if (!IsFinite(f.v[i])) {
+      return false;
+    }
+  }
+  return true;
+}
+
+// The jet is infinite if any part of the jet is infinite.
+template <typename T, int N> inline
+bool IsInfinite(const Jet<T, N>& f) {
+  if (IsInfinite(f.a)) {
+    return true;
+  }
+  for (int i = 0; i < N; i++) {
+    if (IsInfinite(f.v[i])) {
+      return true;
+    }
+  }
+  return false;
+}
+
+// The jet is NaN if any part of the jet is NaN.
+template <typename T, int N> inline
+bool IsNaN(const Jet<T, N>& f) {
+  if (IsNaN(f.a)) {
+    return true;
+  }
+  for (int i = 0; i < N; ++i) {
+    if (IsNaN(f.v[i])) {
+      return true;
+    }
+  }
+  return false;
+}
+
+// The jet is normal if all parts of the jet are normal.
+template <typename T, int N> inline
+bool IsNormal(const Jet<T, N>& f) {
+  if (!IsNormal(f.a)) {
+    return false;
+  }
+  for (int i = 0; i < N; ++i) {
+    if (!IsNormal(f.v[i])) {
+      return false;
+    }
+  }
+  return true;
+}
+
+// atan2(b + db, a + da) ~= atan2(b, a) + (- b da + a db) / (a^2 + b^2)
+//
+// In words: the rate of change of theta is 1/r times the rate of
+// change of (x, y) in the positive angular direction.
+template <typename T, int N> inline
+Jet<T, N> atan2(const Jet<T, N>& g, const Jet<T, N>& f) {
+  // Note order of arguments:
+  //
+  //   f = a + da
+  //   g = b + db
+
+  Jet<T, N> out;
+
+  out.a = atan2(g.a, f.a);
+
+  T const temp = T(1.0) / (f.a * f.a + g.a * g.a);
+  out.v = temp * (- g.a * f.v + f.a * g.v);
+  return out;
+}
+
+
+// pow -- base is a differentiatble function, exponent is a constant.
+// (a+da)^p ~= a^p + p*a^(p-1) da
+template <typename T, int N> inline
+Jet<T, N> pow(const Jet<T, N>& f, double g) {
+  Jet<T, N> out;
+  out.a = pow(f.a, g);
+  T const temp = g * pow(f.a, g - T(1.0));
+  out.v = temp * f.v;
+  return out;
+}
+
+// pow -- base is a constant, exponent is a differentiable function.
+// (a)^(p+dp) ~= a^p + a^p log(a) dp
+template <typename T, int N> inline
+Jet<T, N> pow(double f, const Jet<T, N>& g) {
+  Jet<T, N> out;
+  out.a = pow(f, g.a);
+  T const temp = log(f) * out.a;
+  out.v = temp * g.v;
+  return out;
+}
+
+
+// pow -- both base and exponent are differentiable functions.
+// (a+da)^(b+db) ~= a^b + b * a^(b-1) da + a^b log(a) * db
+template <typename T, int N> inline
+Jet<T, N> pow(const Jet<T, N>& f, const Jet<T, N>& g) {
+  Jet<T, N> out;
+
+  T const temp1 = pow(f.a, g.a);
+  T const temp2 = g.a * pow(f.a, g.a - T(1.0));
+  T const temp3 = temp1 * log(f.a);
+
+  out.a = temp1;
+  out.v = temp2 * f.v + temp3 * g.v;
+  return out;
+}
+
+// Define the helper functions Eigen needs to embed Jet types.
+//
+// NOTE(keir): machine_epsilon() and precision() are missing, because they don't
+// work with nested template types (e.g. where the scalar is itself templated).
+// Among other things, this means that decompositions of Jet's does not work,
+// for example
+//
+//   Matrix<Jet<T, N> ... > A, x, b;
+//   ...
+//   A.solve(b, &x)
+//
+// does not work and will fail with a strange compiler error.
+//
+// TODO(keir): This is an Eigen 2.0 limitation that is lifted in 3.0. When we
+// switch to 3.0, also add the rest of the specialization functionality.
+template<typename T, int N> inline const Jet<T, N>& ei_conj(const Jet<T, N>& x) { return x;              }  // NOLINT
+template<typename T, int N> inline const Jet<T, N>& ei_real(const Jet<T, N>& x) { return x;              }  // NOLINT
+template<typename T, int N> inline       Jet<T, N>  ei_imag(const Jet<T, N>&  ) { return Jet<T, N>(0.0); }  // NOLINT
+template<typename T, int N> inline       Jet<T, N>  ei_abs (const Jet<T, N>& x) { return fabs(x);        }  // NOLINT
+template<typename T, int N> inline       Jet<T, N>  ei_abs2(const Jet<T, N>& x) { return x * x;          }  // NOLINT
+template<typename T, int N> inline       Jet<T, N>  ei_sqrt(const Jet<T, N>& x) { return sqrt(x);        }  // NOLINT
+template<typename T, int N> inline       Jet<T, N>  ei_exp (const Jet<T, N>& x) { return exp(x);         }  // NOLINT
+template<typename T, int N> inline       Jet<T, N>  ei_log (const Jet<T, N>& x) { return log(x);         }  // NOLINT
+template<typename T, int N> inline       Jet<T, N>  ei_sin (const Jet<T, N>& x) { return sin(x);         }  // NOLINT
+template<typename T, int N> inline       Jet<T, N>  ei_cos (const Jet<T, N>& x) { return cos(x);         }  // NOLINT
+template<typename T, int N> inline       Jet<T, N>  ei_tan (const Jet<T, N>& x) { return tan(x);         }  // NOLINT
+template<typename T, int N> inline       Jet<T, N>  ei_atan(const Jet<T, N>& x) { return atan(x);        }  // NOLINT
+template<typename T, int N> inline       Jet<T, N>  ei_sinh(const Jet<T, N>& x) { return sinh(x);        }  // NOLINT
+template<typename T, int N> inline       Jet<T, N>  ei_cosh(const Jet<T, N>& x) { return cosh(x);        }  // NOLINT
+template<typename T, int N> inline       Jet<T, N>  ei_tanh(const Jet<T, N>& x) { return tanh(x);        }  // NOLINT
+template<typename T, int N> inline       Jet<T, N>  ei_pow (const Jet<T, N>& x, Jet<T, N> y) { return pow(x, y); }  // NOLINT
+
+// Note: This has to be in the ceres namespace for argument dependent lookup to
+// function correctly. Otherwise statements like CHECK_LE(x, 2.0) fail with
+// strange compile errors.
+template <typename T, int N>
+inline std::ostream &operator<<(std::ostream &s, const Jet<T, N>& z) {
+  return s << "[" << z.a << " ; " << z.v.transpose() << "]";
+}
+
+}  // namespace ceres
+
+namespace Eigen {
+
+// Creating a specialization of NumTraits enables placing Jet objects inside
+// Eigen arrays, getting all the goodness of Eigen combined with autodiff.
+template<typename T, int N>
+struct NumTraits<ceres::Jet<T, N> > {
+  typedef ceres::Jet<T, N> Real;
+  typedef ceres::Jet<T, N> NonInteger;
+  typedef ceres::Jet<T, N> Nested;
+  typedef ceres::Jet<T, N> Literal;
+
+  static typename ceres::Jet<T, N> dummy_precision() {
+    return ceres::Jet<T, N>(1e-12);
+  }
+
+  static inline int digits10() { return NumTraits<T>::digits10(); }
+
+  enum {
+    IsComplex = 0,
+    IsInteger = 0,
+    IsSigned,
+    ReadCost = 1,
+    AddCost = 1,
+    // For Jet types, multiplication is more expensive than addition.
+    MulCost = 3,
+    HasFloatingPoint = 1,
+    RequireInitialization = 1
+  };
+};
+
+}  // namespace Eigen
+
+#endif  // CERES_PUBLIC_JET_H_

workspace/slam_in_autonomous_driving/thirdparty/g2o/EXTERNAL/ceres/macros.h

@@ -0,0 +1,170 @@
+// 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.
+//
+//
+// Various Google-specific macros.
+//
+// This code is compiled directly on many platforms, including client
+// platforms like Windows, Mac, and embedded systems.  Before making
+// any changes here, make sure that you're not breaking any platforms.
+
+#ifndef CERES_PUBLIC_INTERNAL_MACROS_H_
+#define CERES_PUBLIC_INTERNAL_MACROS_H_
+
+#include <cstddef>  // For size_t.
+
+// A macro to disallow the copy constructor and operator= functions
+// This should be used in the private: declarations for a class
+//
+// For disallowing only assign or copy, write the code directly, but declare
+// the intend in a comment, for example:
+//
+//   void operator=(const TypeName&);  // _DISALLOW_ASSIGN
+
+// Note, that most uses of CERES_DISALLOW_ASSIGN and CERES_DISALLOW_COPY
+// are broken semantically, one should either use disallow both or
+// neither. Try to avoid these in new code.
+#define CERES_DISALLOW_COPY_AND_ASSIGN(TypeName) \
+  TypeName(const TypeName&);               \
+  void operator=(const TypeName&)
+
+// A macro to disallow all the implicit constructors, namely the
+// default constructor, copy constructor and operator= functions.
+//
+// This should be used in the private: declarations for a class
+// that wants to prevent anyone from instantiating it. This is
+// especially useful for classes containing only static methods.
+#define CERES_DISALLOW_IMPLICIT_CONSTRUCTORS(TypeName) \
+  TypeName();                                    \
+  CERES_DISALLOW_COPY_AND_ASSIGN(TypeName)
+
+// The arraysize(arr) macro returns the # of elements in an array arr.
+// The expression is a compile-time constant, and therefore can be
+// used in defining new arrays, for example.  If you use arraysize on
+// a pointer by mistake, you will get a compile-time error.
+//
+// One caveat is that arraysize() doesn't accept any array of an
+// anonymous type or a type defined inside a function.  In these rare
+// cases, you have to use the unsafe ARRAYSIZE() macro below.  This is
+// due to a limitation in C++'s template system.  The limitation might
+// eventually be removed, but it hasn't happened yet.
+
+// This template function declaration is used in defining arraysize.
+// Note that the function doesn't need an implementation, as we only
+// use its type.
+template <typename T, size_t N>
+char (&ArraySizeHelper(T (&array)[N]))[N];
+
+// That gcc wants both of these prototypes seems mysterious. VC, for
+// its part, can't decide which to use (another mystery). Matching of
+// template overloads: the final frontier.
+#ifndef _WIN32
+template <typename T, size_t N>
+char (&ArraySizeHelper(const T (&array)[N]))[N];
+#endif
+
+#define arraysize(array) (sizeof(ArraySizeHelper(array)))
+
+// ARRAYSIZE performs essentially the same calculation as arraysize,
+// but can be used on anonymous types or types defined inside
+// functions.  It's less safe than arraysize as it accepts some
+// (although not all) pointers.  Therefore, you should use arraysize
+// whenever possible.
+//
+// The expression ARRAYSIZE(a) is a compile-time constant of type
+// size_t.
+//
+// ARRAYSIZE catches a few type errors.  If you see a compiler error
+//
+//   "warning: division by zero in ..."
+//
+// when using ARRAYSIZE, you are (wrongfully) giving it a pointer.
+// You should only use ARRAYSIZE on statically allocated arrays.
+//
+// The following comments are on the implementation details, and can
+// be ignored by the users.
+//
+// ARRAYSIZE(arr) works by inspecting sizeof(arr) (the # of bytes in
+// the array) and sizeof(*(arr)) (the # of bytes in one array
+// element).  If the former is divisible by the latter, perhaps arr is
+// indeed an array, in which case the division result is the # of
+// elements in the array.  Otherwise, arr cannot possibly be an array,
+// and we generate a compiler error to prevent the code from
+// compiling.
+//
+// Since the size of bool is implementation-defined, we need to cast
+// !(sizeof(a) & sizeof(*(a))) to size_t in order to ensure the final
+// result has type size_t.
+//
+// This macro is not perfect as it wrongfully accepts certain
+// pointers, namely where the pointer size is divisible by the pointee
+// size.  Since all our code has to go through a 32-bit compiler,
+// where a pointer is 4 bytes, this means all pointers to a type whose
+// size is 3 or greater than 4 will be (righteously) rejected.
+//
+// Kudos to Jorg Brown for this simple and elegant implementation.
+//
+// - wan 2005-11-16
+//
+// Starting with Visual C++ 2005, WinNT.h includes ARRAYSIZE. However,
+// the definition comes from the over-broad windows.h header that
+// introduces a macro, ERROR, that conflicts with the logging framework
+// that Ceres uses. Instead, rename ARRAYSIZE to CERES_ARRAYSIZE.
+#define CERES_ARRAYSIZE(a)                              \
+  ((sizeof(a) / sizeof(*(a))) /                         \
+   static_cast<size_t>(!(sizeof(a) % sizeof(*(a)))))
+
+// Tell the compiler to warn about unused return values for functions
+// declared with this macro.  The macro should be used on function
+// declarations following the argument list:
+//
+//   Sprocket* AllocateSprocket() MUST_USE_RESULT;
+//
+#if (__GNUC__ > 3 || (__GNUC__ == 3 && __GNUC_MINOR__ >= 4)) \
+  && !defined(COMPILER_ICC)
+#define CERES_MUST_USE_RESULT __attribute__ ((warn_unused_result))
+#else
+#define CERES_MUST_USE_RESULT
+#endif
+
+// Platform independent macros to get aligned memory allocations.
+// For example
+//
+//   MyFoo my_foo CERES_ALIGN_ATTRIBUTE(16);
+//
+// Gives us an instance of MyFoo which is aligned at a 16 byte
+// boundary.
+#if defined(_MSC_VER)
+#define CERES_ALIGN_ATTRIBUTE(n) __declspec(align(n))
+#define CERES_ALIGN_OF(T) __alignof(T)
+#elif defined(__GNUC__)
+#define CERES_ALIGN_ATTRIBUTE(n) __attribute__((aligned(n)))
+#define CERES_ALIGN_OF(T) __alignof(T)
+#endif
+
+#endif  // CERES_PUBLIC_INTERNAL_MACROS_H_

workspace/slam_in_autonomous_driving/thirdparty/g2o/EXTERNAL/ceres/manual_constructor.h

@@ -0,0 +1,208 @@
+// 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: kenton@google.com (Kenton Varda)
+//
+// ManualConstructor statically-allocates space in which to store some
+// object, but does not initialize it.  You can then call the constructor
+// and destructor for the object yourself as you see fit.  This is useful
+// for memory management optimizations, where you want to initialize and
+// destroy an object multiple times but only allocate it once.
+//
+// (When I say ManualConstructor statically allocates space, I mean that
+// the ManualConstructor object itself is forced to be the right size.)
+
+#ifndef CERES_PUBLIC_INTERNAL_MANUAL_CONSTRUCTOR_H_
+#define CERES_PUBLIC_INTERNAL_MANUAL_CONSTRUCTOR_H_
+
+#include <new>
+
+namespace ceres {
+namespace internal {
+
+// ------- Define CERES_ALIGNED_CHAR_ARRAY --------------------------------
+
+#ifndef CERES_ALIGNED_CHAR_ARRAY
+
+// Because MSVC and older GCCs require that the argument to their alignment
+// construct to be a literal constant integer, we use a template instantiated
+// at all the possible powers of two.
+template<int alignment, int size> struct AlignType { };
+template<int size> struct AlignType<0, size> { typedef char result[size]; };
+
+#if !defined(CERES_ALIGN_ATTRIBUTE)
+#define CERES_ALIGNED_CHAR_ARRAY you_must_define_CERES_ALIGNED_CHAR_ARRAY_for_your_compiler
+#else  // !defined(CERES_ALIGN_ATTRIBUTE)
+
+#define CERES_ALIGN_TYPE_TEMPLATE(X) \
+  template<int size> struct AlignType<X, size> { \
+    typedef CERES_ALIGN_ATTRIBUTE(X) char result[size]; \
+  }
+
+CERES_ALIGN_TYPE_TEMPLATE(1);
+CERES_ALIGN_TYPE_TEMPLATE(2);
+CERES_ALIGN_TYPE_TEMPLATE(4);
+CERES_ALIGN_TYPE_TEMPLATE(8);
+CERES_ALIGN_TYPE_TEMPLATE(16);
+CERES_ALIGN_TYPE_TEMPLATE(32);
+CERES_ALIGN_TYPE_TEMPLATE(64);
+CERES_ALIGN_TYPE_TEMPLATE(128);
+CERES_ALIGN_TYPE_TEMPLATE(256);
+CERES_ALIGN_TYPE_TEMPLATE(512);
+CERES_ALIGN_TYPE_TEMPLATE(1024);
+CERES_ALIGN_TYPE_TEMPLATE(2048);
+CERES_ALIGN_TYPE_TEMPLATE(4096);
+CERES_ALIGN_TYPE_TEMPLATE(8192);
+// Any larger and MSVC++ will complain.
+
+#undef CERES_ALIGN_TYPE_TEMPLATE
+
+#define CERES_ALIGNED_CHAR_ARRAY(T, Size) \
+  typename AlignType<CERES_ALIGN_OF(T), sizeof(T) * Size>::result
+
+#endif  // !defined(CERES_ALIGN_ATTRIBUTE)
+
+#endif  // CERES_ALIGNED_CHAR_ARRAY
+
+template <typename Type>
+class ManualConstructor {
+ public:
+  // No constructor or destructor because one of the most useful uses of
+  // this class is as part of a union, and members of a union cannot have
+  // constructors or destructors.  And, anyway, the whole point of this
+  // class is to bypass these.
+
+  inline Type* get() {
+    return reinterpret_cast<Type*>(space_);
+  }
+  inline const Type* get() const  {
+    return reinterpret_cast<const Type*>(space_);
+  }
+
+  inline Type* operator->() { return get(); }
+  inline const Type* operator->() const { return get(); }
+
+  inline Type& operator*() { return *get(); }
+  inline const Type& operator*() const { return *get(); }
+
+  // This is needed to get around the strict aliasing warning GCC generates.
+  inline void* space() {
+    return reinterpret_cast<void*>(space_);
+  }
+
+  // You can pass up to four constructor arguments as arguments of Init().
+  inline void Init() {
+    new(space()) Type;
+  }
+
+  template <typename T1>
+  inline void Init(const T1& p1) {
+    new(space()) Type(p1);
+  }
+
+  template <typename T1, typename T2>
+  inline void Init(const T1& p1, const T2& p2) {
+    new(space()) Type(p1, p2);
+  }
+
+  template <typename T1, typename T2, typename T3>
+  inline void Init(const T1& p1, const T2& p2, const T3& p3) {
+    new(space()) Type(p1, p2, p3);
+  }
+
+  template <typename T1, typename T2, typename T3, typename T4>
+  inline void Init(const T1& p1, const T2& p2, const T3& p3, const T4& p4) {
+    new(space()) Type(p1, p2, p3, p4);
+  }
+
+  template <typename T1, typename T2, typename T3, typename T4, typename T5>
+  inline void Init(const T1& p1, const T2& p2, const T3& p3, const T4& p4,
+                   const T5& p5) {
+    new(space()) Type(p1, p2, p3, p4, p5);
+  }
+
+  template <typename T1, typename T2, typename T3, typename T4, typename T5,
+            typename T6>
+  inline void Init(const T1& p1, const T2& p2, const T3& p3, const T4& p4,
+                   const T5& p5, const T6& p6) {
+    new(space()) Type(p1, p2, p3, p4, p5, p6);
+  }
+
+  template <typename T1, typename T2, typename T3, typename T4, typename T5,
+            typename T6, typename T7>
+  inline void Init(const T1& p1, const T2& p2, const T3& p3, const T4& p4,
+                   const T5& p5, const T6& p6, const T7& p7) {
+    new(space()) Type(p1, p2, p3, p4, p5, p6, p7);
+  }
+
+  template <typename T1, typename T2, typename T3, typename T4, typename T5,
+            typename T6, typename T7, typename T8>
+  inline void Init(const T1& p1, const T2& p2, const T3& p3, const T4& p4,
+                   const T5& p5, const T6& p6, const T7& p7, const T8& p8) {
+    new(space()) Type(p1, p2, p3, p4, p5, p6, p7, p8);
+  }
+
+  template <typename T1, typename T2, typename T3, typename T4, typename T5,
+            typename T6, typename T7, typename T8, typename T9>
+  inline void Init(const T1& p1, const T2& p2, const T3& p3, const T4& p4,
+                   const T5& p5, const T6& p6, const T7& p7, const T8& p8,
+                   const T9& p9) {
+    new(space()) Type(p1, p2, p3, p4, p5, p6, p7, p8, p9);
+  }
+
+  template <typename T1, typename T2, typename T3, typename T4, typename T5,
+            typename T6, typename T7, typename T8, typename T9, typename T10>
+  inline void Init(const T1& p1, const T2& p2, const T3& p3, const T4& p4,
+                   const T5& p5, const T6& p6, const T7& p7, const T8& p8,
+                   const T9& p9, const T10& p10) {
+    new(space()) Type(p1, p2, p3, p4, p5, p6, p7, p8, p9, p10);
+  }
+
+  template <typename T1, typename T2, typename T3, typename T4, typename T5,
+            typename T6, typename T7, typename T8, typename T9, typename T10,
+            typename T11>
+  inline void Init(const T1& p1, const T2& p2, const T3& p3, const T4& p4,
+                   const T5& p5, const T6& p6, const T7& p7, const T8& p8,
+                   const T9& p9, const T10& p10, const T11& p11) {
+    new(space()) Type(p1, p2, p3, p4, p5, p6, p7, p8, p9, p10, p11);
+  }
+
+  inline void Destroy() {
+    get()->~Type();
+  }
+
+ private:
+  CERES_ALIGNED_CHAR_ARRAY(Type, 1) space_;
+};
+
+#undef CERES_ALIGNED_CHAR_ARRAY
+
+}  // namespace internal
+}  // namespace ceres
+
+#endif  // CERES_PUBLIC_INTERNAL_MANUAL_CONSTRUCTOR_H_

workspace/slam_in_autonomous_driving/thirdparty/g2o/EXTERNAL/ceres/variadic_evaluate.h

@@ -0,0 +1,207 @@
+// 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: sameeragarwal@google.com (Sameer Agarwal)
+//         mierle@gmail.com (Keir Mierle)
+
+#ifndef CERES_PUBLIC_INTERNAL_VARIADIC_EVALUATE_H_
+#define CERES_PUBLIC_INTERNAL_VARIADIC_EVALUATE_H_
+
+#include <stddef.h>
+
+#include "jet.h"
+#include "eigen.h"
+#include "fixed_array.h"
+
+namespace ceres {
+
+// It is a near impossibility that user code generates this exact
+// value in normal operation, thus we will use it to fill arrays
+// before passing them to user code. If on return an element of the
+// array still contains this value, we will assume that the user code
+// did not write to that memory location.
+const double kImpossibleValue = 1e302;
+
+// For SizedCostFunction and AutoDiffCostFunction, DYNAMIC can be
+// specified for the number of residuals. If specified, then the
+// number of residuas for that cost function can vary at runtime.
+enum DimensionType {
+  DYNAMIC = -1
+};
+
+namespace internal {
+
+// This block of quasi-repeated code calls the user-supplied functor, which may
+// take a variable number of arguments. This is accomplished by specializing the
+// struct based on the size of the trailing parameters; parameters with 0 size
+// are assumed missing.
+template<typename Functor, typename T, int N0, int N1, int N2, int N3, int N4,
+         int N5, int N6, int N7, int N8, int N9>
+struct VariadicEvaluate {
+  static bool Call(const Functor& functor, T const *const *input, T* output) {
+    return functor(input[0],
+                   input[1],
+                   input[2],
+                   input[3],
+                   input[4],
+                   input[5],
+                   input[6],
+                   input[7],
+                   input[8],
+                   input[9],
+                   output);
+  }
+};
+
+template<typename Functor, typename T, int N0, int N1, int N2, int N3, int N4,
+         int N5, int N6, int N7, int N8>
+struct VariadicEvaluate<Functor, T, N0, N1, N2, N3, N4, N5, N6, N7, N8, 0> {
+  static bool Call(const Functor& functor, T const *const *input, T* output) {
+    return functor(input[0],
+                   input[1],
+                   input[2],
+                   input[3],
+                   input[4],
+                   input[5],
+                   input[6],
+                   input[7],
+                   input[8],
+                   output);
+  }
+};
+
+template<typename Functor, typename T, int N0, int N1, int N2, int N3, int N4,
+         int N5, int N6, int N7>
+struct VariadicEvaluate<Functor, T, N0, N1, N2, N3, N4, N5, N6, N7, 0, 0> {
+  static bool Call(const Functor& functor, T const *const *input, T* output) {
+    return functor(input[0],
+                   input[1],
+                   input[2],
+                   input[3],
+                   input[4],
+                   input[5],
+                   input[6],
+                   input[7],
+                   output);
+  }
+};
+
+template<typename Functor, typename T, int N0, int N1, int N2, int N3, int N4,
+         int N5, int N6>
+struct VariadicEvaluate<Functor, T, N0, N1, N2, N3, N4, N5, N6, 0, 0, 0> {
+  static bool Call(const Functor& functor, T const *const *input, T* output) {
+    return functor(input[0],
+                   input[1],
+                   input[2],
+                   input[3],
+                   input[4],
+                   input[5],
+                   input[6],
+                   output);
+  }
+};
+
+template<typename Functor, typename T, int N0, int N1, int N2, int N3, int N4,
+         int N5>
+struct VariadicEvaluate<Functor, T, N0, N1, N2, N3, N4, N5, 0, 0, 0, 0> {
+  static bool Call(const Functor& functor, T const *const *input, T* output) {
+    return functor(input[0],
+                   input[1],
+                   input[2],
+                   input[3],
+                   input[4],
+                   input[5],
+                   output);
+  }
+};
+
+template<typename Functor, typename T, int N0, int N1, int N2, int N3, int N4>
+struct VariadicEvaluate<Functor, T, N0, N1, N2, N3, N4, 0, 0, 0, 0, 0> {
+  static bool Call(const Functor& functor, T const *const *input, T* output) {
+    return functor(input[0],
+                   input[1],
+                   input[2],
+                   input[3],
+                   input[4],
+                   output);
+  }
+};
+
+template<typename Functor, typename T, int N0, int N1, int N2, int N3>
+struct VariadicEvaluate<Functor, T, N0, N1, N2, N3, 0, 0, 0, 0, 0, 0> {
+  static bool Call(const Functor& functor, T const *const *input, T* output) {
+    return functor(input[0],
+                   input[1],
+                   input[2],
+                   input[3],
+                   output);
+  }
+};
+
+template<typename Functor, typename T, int N0, int N1, int N2>
+struct VariadicEvaluate<Functor, T, N0, N1, N2, 0, 0, 0, 0, 0, 0, 0> {
+  static bool Call(const Functor& functor, T const *const *input, T* output) {
+    return functor(input[0],
+                   input[1],
+                   input[2],
+                   output);
+  }
+};
+
+template<typename Functor, typename T, int N0, int N1>
+struct VariadicEvaluate<Functor, T, N0, N1, 0, 0, 0, 0, 0, 0, 0, 0> {
+  static bool Call(const Functor& functor, T const *const *input, T* output) {
+    return functor(input[0],
+                   input[1],
+                   output);
+  }
+};
+
+template<typename Functor, typename T, int N0>
+struct VariadicEvaluate<Functor, T, N0, 0, 0, 0, 0, 0, 0, 0, 0, 0> {
+  static bool Call(const Functor& functor, T const *const *input, T* output) {
+    return functor(input[0],
+                   output);
+  }
+};
+
+// Template instantiation for dynamically-sized functors.
+template<typename Functor, typename T>
+struct VariadicEvaluate<Functor, T, ceres::DYNAMIC, ceres::DYNAMIC,
+                        ceres::DYNAMIC, ceres::DYNAMIC, ceres::DYNAMIC,
+                        ceres::DYNAMIC, ceres::DYNAMIC, ceres::DYNAMIC,
+                        ceres::DYNAMIC, ceres::DYNAMIC> {
+  static bool Call(const Functor& functor, T const *const *input, T* output) {
+    return functor(input, output);
+  }
+};
+
+}  // namespace internal
+}  // namespace ceres
+
+#endif  // CERES_PUBLIC_INTERNAL_VARIADIC_EVALUATE_H_

workspace/slam_in_autonomous_driving/thirdparty/g2o/EXTERNAL/csparse/CMakeLists.txt

@@ -0,0 +1,84 @@
+cmake_minimum_required(VERSION 2.6)
+
+project(csparse)
+
+#set(CMAKE_C_FLAGS_RELEASE "-O3 -DNDEBUG")
+
+add_library(csparse ${G2O_LGPL_LIB_TYPE}
+  cs_add.c
+  cs_amd.c
+  cs_chol.c
+  cs_cholsol.c
+  cs_compress.c
+  cs_counts.c
+  cs_cumsum.c
+  cs_dfs.c
+  cs_dmperm.c
+  cs_droptol.c
+  cs_dropzeros.c
+  cs_dupl.c
+  cs_entry.c
+  cs_ereach.c
+  cs_etree.c
+  cs_fkeep.c
+  cs_gaxpy.c
+  cs_happly.c
+  cs_house.c
+  cs_ipvec.c
+  cs_leaf.c
+  cs_load.c
+  cs_lsolve.c
+  cs_ltsolve.c
+  cs_lu.c
+  cs_lusol.c
+  cs_malloc.c
+  cs_maxtrans.c
+  cs_multiply.c
+  cs_norm.c
+  cs_permute.c
+  cs_pinv.c
+  cs_post.c
+  cs_print.c
+  cs_pvec.c
+  cs_qr.c
+  cs_qrsol.c
+  cs_randperm.c
+  cs_reach.c
+  cs_scatter.c
+  cs_scc.c
+  cs_schol.c
+  cs_spsolve.c
+  cs_sqr.c
+  cs_symperm.c
+  cs_tdfs.c
+  cs_transpose.c
+  cs_updown.c
+  cs_usolve.c
+  cs_util.c
+  cs_utsolve.c
+  cs_api.h
+)
+
+set_target_properties(csparse PROPERTIES OUTPUT_NAME ${LIB_PREFIX}ext_csparse)
+if (APPLE)
+  set_target_properties(csparse PROPERTIES INSTALL_NAME_DIR "${CMAKE_INSTALL_PREFIX}/lib")
+endif()
+if (UNIX)
+  target_link_libraries(csparse PUBLIC m)
+endif()
+
+install(TARGETS csparse
+  EXPORT ${G2O_TARGETS_EXPORT_NAME}
+  RUNTIME DESTINATION ${RUNTIME_DESTINATION}
+  LIBRARY DESTINATION ${LIBRARY_DESTINATION}
+  ARCHIVE DESTINATION ${ARCHIVE_DESTINATION}
+  INCLUDES DESTINATION ${INCLUDES_DESTINATION}
+)
+
+file(GLOB headers "${CMAKE_CURRENT_SOURCE_DIR}/*.h" "${CMAKE_CURRENT_SOURCE_DIR}/*.hpp")
+install(FILES ${headers} DESTINATION ${INCLUDES_INSTALL_DIR}/EXTERNAL/csparse)
+
+# Set up the variables
+set(CSPARSE_LIBRARY "$<TARGET_FILE:csparse>")
+set(CSPARSE_INCLUDE_DIR ${CMAKE_CURRENT_SOURCE_DIR} CACHE PATH "Include directory for CSparse" FORCE)
+set(CSPARSE_LIBRARY     ${CSPARSE_LIBRARY} CACHE FILEPATH "CSparse library" FORCE)

workspace/slam_in_autonomous_driving/thirdparty/g2o/EXTERNAL/csparse/License.txt

@@ -0,0 +1,19 @@
+CSparse: a Concise Sparse matrix package.
+Copyright (c) 2006, Timothy A. Davis.
+http://www.cise.ufl.edu/research/sparse/CSparse
+
+--------------------------------------------------------------------------------
+
+CSparse is free software; you can redistribute it and/or
+modify it under the terms of the GNU Lesser General Public
+License as published by the Free Software Foundation; either
+version 2.1 of the License, or (at your option) any later version.
+
+CSparse is distributed in the hope that it will be useful,
+but WITHOUT ANY WARRANTY; without even the implied warranty of
+MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+Lesser General Public License for more details.
+
+You should have received a copy of the GNU Lesser General Public
+License along with this Module; if not, write to the Free Software
+Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA

workspace/slam_in_autonomous_driving/thirdparty/g2o/EXTERNAL/csparse/README.txt

@@ -0,0 +1,3 @@
+CSparse/Source directory: primary ANSI C source code files for CSparse.
+All of these files are printed verbatim in the book.  To compile the
+libcsparse.a C-callable library, just type "make" in this directory.

workspace/slam_in_autonomous_driving/thirdparty/g2o/EXTERNAL/csparse/cs.h

@@ -0,0 +1,178 @@
+#ifndef _CS_H
+#define _CS_H
+#include <stdlib.h>
+#include <limits.h>
+#include <math.h>
+#include <stdio.h>
+#include <stddef.h>
+#ifdef MATLAB_MEX_FILE
+#include "mex.h"
+#endif
+#define CS_VER 3                    /* CSparse Version */
+#define CS_SUBVER 1
+#define CS_SUBSUB 0
+#define CS_DATE "Jun 1, 2012"       /* CSparse release date */
+#define CS_COPYRIGHT "Copyright (c) Timothy A. Davis, 2006-2012"
+
+#include "cs_api.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+// rk: We define csi to be int to be backward compatible with older CSparse releases.
+//     At some point we might adapt the internal structures of g2o to use the same type
+//     as the one given is this header file.
+#define csi int
+
+/* -------------------------------------------------------------------------- */
+/* In version 3.0.0 of CSparse, "int" is no longer used.  32-bit MATLAB is
+   becoming more rare, as are 32-bit computers.  CSparse now uses "csi" as its
+   basic integer, which is ptrdiff_t by default (the same as mwSignedIndex in a
+   MATLAB mexFunction).  That makes the basic integer 32-bit on 32-bit
+   computers and 64-bit on 64-bit computers.  It is #define'd below, in case
+   you wish to change it to something else (back to "int" for example).  You
+   can also compile with -Dcsi=int (or whatever) to change csi without editting
+   this file. */
+#ifdef MATLAB_MEX_FILE
+#undef csi
+#define csi mwSignedIndex
+#endif
+#ifndef csi
+#define csi ptrdiff_t
+#endif
+/* -------------------------------------------------------------------------- */
+
+/* --- primary CSparse routines and data structures ------------------------- */
+typedef struct cs_sparse    /* matrix in compressed-column or triplet form */
+{
+    csi nzmax ;     /* maximum number of entries */
+    csi m ;         /* number of rows */
+    csi n ;         /* number of columns */
+    csi *p ;        /* column pointers (size n+1) or col indices (size nzmax) */
+    csi *i ;        /* row indices, size nzmax */
+    double *x ;     /* numerical values, size nzmax */
+    csi nz ;        /* # of entries in triplet matrix, -1 for compressed-col */
+} cs ;
+
+G2O_CSPARSE_API cs *cs_add (const cs *A, const cs *B, double alpha, double beta) ;
+G2O_CSPARSE_API csi cs_cholsol (csi order, const cs *A, double *b) ;
+G2O_CSPARSE_API cs *cs_compress (const cs *T) ;
+G2O_CSPARSE_API csi cs_dupl (cs *A) ;
+G2O_CSPARSE_API csi cs_entry (cs *T, csi i, csi j, double x) ;
+G2O_CSPARSE_API csi cs_gaxpy (const cs *A, const double *x, double *y) ;
+G2O_CSPARSE_API cs *cs_load (FILE *f) ;
+G2O_CSPARSE_API csi cs_lusol (csi order, const cs *A, double *b, double tol) ;
+G2O_CSPARSE_API cs *cs_multiply (const cs *A, const cs *B) ;
+G2O_CSPARSE_API double cs_norm (const cs *A) ;
+G2O_CSPARSE_API csi cs_print (const cs *A, csi brief) ;
+G2O_CSPARSE_API csi cs_qrsol (csi order, const cs *A, double *b) ;
+G2O_CSPARSE_API cs *cs_transpose (const cs *A, csi values) ;
+/* utilities */
+G2O_CSPARSE_API void *cs_calloc (csi n, size_t size) ;
+G2O_CSPARSE_API void *cs_free (void *p) ;
+G2O_CSPARSE_API void *cs_realloc (void *p, csi n, size_t size, csi *ok) ;
+G2O_CSPARSE_API cs *cs_spalloc (csi m, csi n, csi nzmax, csi values, csi triplet) ;
+G2O_CSPARSE_API cs *cs_spfree (cs *A) ;
+G2O_CSPARSE_API csi cs_sprealloc (cs *A, csi nzmax) ;
+G2O_CSPARSE_API void *cs_malloc (csi n, size_t size) ;
+
+/* --- secondary CSparse routines and data structures ----------------------- */
+typedef struct cs_symbolic  /* symbolic Cholesky, LU, or QR analysis */
+{
+    csi *pinv ;     /* inverse row perm. for QR, fill red. perm for Chol */
+    csi *q ;        /* fill-reducing column permutation for LU and QR */
+    csi *parent ;   /* elimination tree for Cholesky and QR */
+    csi *cp ;       /* column pointers for Cholesky, row counts for QR */
+    csi *leftmost ; /* leftmost[i] = min(find(A(i,:))), for QR */
+    csi m2 ;        /* # of rows for QR, after adding fictitious rows */
+    double lnz ;    /* # entries in L for LU or Cholesky; in V for QR */
+    double unz ;    /* # entries in U for LU; in R for QR */
+} css ;
+
+typedef struct cs_numeric   /* numeric Cholesky, LU, or QR factorization */
+{
+    cs *L ;         /* L for LU and Cholesky, V for QR */
+    cs *U ;         /* U for LU, R for QR, not used for Cholesky */
+    csi *pinv ;     /* partial pivoting for LU */
+    double *B ;     /* beta [0..n-1] for QR */
+} csn ;
+
+typedef struct cs_dmperm_results    /* cs_dmperm or cs_scc output */
+{
+    csi *p ;        /* size m, row permutation */
+    csi *q ;        /* size n, column permutation */
+    csi *r ;        /* size nb+1, block k is rows r[k] to r[k+1]-1 in A(p,q) */
+    csi *s ;        /* size nb+1, block k is cols s[k] to s[k+1]-1 in A(p,q) */
+    csi nb ;        /* # of blocks in fine dmperm decomposition */
+    csi rr [5] ;    /* coarse row decomposition */
+    csi cc [5] ;    /* coarse column decomposition */
+} csd ;
+
+G2O_CSPARSE_API csi *cs_amd (csi order, const cs *A) ;
+G2O_CSPARSE_API csn *cs_chol (const cs *A, const css *S) ;
+G2O_CSPARSE_API csd *cs_dmperm (const cs *A, csi seed) ;
+G2O_CSPARSE_API csi cs_droptol (cs *A, double tol) ;
+G2O_CSPARSE_API csi cs_dropzeros (cs *A) ;
+G2O_CSPARSE_API csi cs_happly (const cs *V, csi i, double beta, double *x) ;
+G2O_CSPARSE_API csi cs_ipvec (const csi *p, const double *b, double *x, csi n) ;
+G2O_CSPARSE_API csi cs_lsolve (const cs *L, double *x) ;
+G2O_CSPARSE_API csi cs_ltsolve (const cs *L, double *x) ;
+G2O_CSPARSE_API csn *cs_lu (const cs *A, const css *S, double tol) ;
+G2O_CSPARSE_API cs *cs_permute (const cs *A, const csi *pinv, const csi *q, csi values) ;
+G2O_CSPARSE_API csi *cs_pinv (const csi *p, csi n) ;
+G2O_CSPARSE_API csi cs_pvec (const csi *p, const double *b, double *x, csi n) ;
+G2O_CSPARSE_API csn *cs_qr (const cs *A, const css *S) ;
+G2O_CSPARSE_API css *cs_schol (csi order, const cs *A) ;
+G2O_CSPARSE_API css *cs_sqr (csi order, const cs *A, csi qr) ;
+G2O_CSPARSE_API cs *cs_symperm (const cs *A, const csi *pinv, csi values) ;
+G2O_CSPARSE_API csi cs_updown (cs *L, csi sigma, const cs *C, const csi *parent) ;
+G2O_CSPARSE_API csi cs_usolve (const cs *U, double *x) ;
+G2O_CSPARSE_API csi cs_utsolve (const cs *U, double *x) ;
+/* utilities */
+G2O_CSPARSE_API css *cs_sfree (css *S) ;
+G2O_CSPARSE_API csn *cs_nfree (csn *N) ;
+G2O_CSPARSE_API csd *cs_dfree (csd *D) ;
+
+/* --- tertiary CSparse routines -------------------------------------------- */
+G2O_CSPARSE_API csi *cs_counts (const cs *A, const csi *parent, const csi *post, csi ata) ;
+G2O_CSPARSE_API double cs_cumsum (csi *p, csi *c, csi n) ;
+G2O_CSPARSE_API csi cs_dfs (csi j, cs *G, csi top, csi *xi, csi *pstack, const csi *pinv) ;
+G2O_CSPARSE_API csi cs_ereach (const cs *A, csi k, const csi *parent, csi *s, csi *w) ;
+G2O_CSPARSE_API csi *cs_etree (const cs *A, csi ata) ;
+G2O_CSPARSE_API csi cs_fkeep (cs *A, csi (*fkeep) (csi, csi, double, void *), void *other) ;
+G2O_CSPARSE_API double cs_house (double *x, double *beta, csi n) ;
+G2O_CSPARSE_API csi cs_leaf (csi i, csi j, const csi *first, csi *maxfirst, csi *prevleaf,
+    csi *ancestor, csi *jleaf) ;
+G2O_CSPARSE_API csi *cs_maxtrans (const cs *A, csi seed) ;
+G2O_CSPARSE_API csi *cs_post (const csi *parent, csi n) ;
+G2O_CSPARSE_API csi *cs_randperm (csi n, csi seed) ;
+G2O_CSPARSE_API csi cs_reach (cs *G, const cs *B, csi k, csi *xi, const csi *pinv) ;
+G2O_CSPARSE_API csi cs_scatter (const cs *A, csi j, double beta, csi *w, double *x, csi mark,
+    cs *C, csi nz) ;
+G2O_CSPARSE_API csd *cs_scc (cs *A) ;
+G2O_CSPARSE_API csi cs_spsolve (cs *G, const cs *B, csi k, csi *xi, double *x,
+    const csi *pinv, csi lo) ;
+G2O_CSPARSE_API csi cs_tdfs (csi j, csi k, csi *head, const csi *next, csi *post,
+    csi *stack) ;
+/* utilities */
+G2O_CSPARSE_API csd *cs_dalloc (csi m, csi n) ;
+G2O_CSPARSE_API csd *cs_ddone (csd *D, cs *C, void *w, csi ok) ;
+G2O_CSPARSE_API cs *cs_done (cs *C, void *w, void *x, csi ok) ;
+G2O_CSPARSE_API csi *cs_idone (csi *p, cs *C, void *w, csi ok) ;
+G2O_CSPARSE_API csn *cs_ndone (csn *N, cs *C, void *w, void *x, csi ok) ;
+
+#define CS_MAX(a,b) (((a) > (b)) ? (a) : (b))
+#define CS_MIN(a,b) (((a) < (b)) ? (a) : (b))
+#define CS_FLIP(i) (-(i)-2)
+#define CS_UNFLIP(i) (((i) < 0) ? CS_FLIP(i) : (i))
+#define CS_MARKED(w,j) (w [j] < 0)
+#define CS_MARK(w,j) { w [j] = CS_FLIP (w [j]) ; }
+#define CS_CSC(A) (A && (A->nz == -1))
+#define CS_TRIPLET(A) (A && (A->nz >= 0))
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif

workspace/slam_in_autonomous_driving/thirdparty/g2o/EXTERNAL/csparse/cs_add.c

@@ -0,0 +1,28 @@
+#include "cs.h"
+/* C = alpha*A + beta*B */
+cs *cs_add (const cs *A, const cs *B, double alpha, double beta)
+{
+    csi p, j, nz = 0, anz, *Cp, *Ci, *Bp, m, n, bnz, *w, values ;
+    double *x, *Bx, *Cx ;
+    cs *C ;
+    if (!CS_CSC (A) || !CS_CSC (B)) return (NULL) ;         /* check inputs */
+    if (A->m != B->m || A->n != B->n) return (NULL) ;
+    m = A->m ; anz = A->p [A->n] ;
+    n = B->n ; Bp = B->p ; Bx = B->x ; bnz = Bp [n] ;
+    w = cs_calloc (m, sizeof (csi)) ;                       /* get workspace */
+    values = (A->x != NULL) && (Bx != NULL) ;
+    x = values ? cs_malloc (m, sizeof (double)) : NULL ;    /* get workspace */
+    C = cs_spalloc (m, n, anz + bnz, values, 0) ;           /* allocate result*/
+    if (!C || !w || (values && !x)) return (cs_done (C, w, x, 0)) ;
+    Cp = C->p ; Ci = C->i ; Cx = C->x ;
+    for (j = 0 ; j < n ; j++)
+    {
+        Cp [j] = nz ;                   /* column j of C starts here */
+        nz = cs_scatter (A, j, alpha, w, x, j+1, C, nz) ;   /* alpha*A(:,j)*/
+        nz = cs_scatter (B, j, beta, w, x, j+1, C, nz) ;    /* beta*B(:,j) */
+        if (values) for (p = Cp [j] ; p < nz ; p++) Cx [p] = x [Ci [p]] ;
+    }
+    Cp [n] = nz ;                       /* finalize the last column of C */
+    cs_sprealloc (C, 0) ;               /* remove extra space from C */
+    return (cs_done (C, w, x, 1)) ;     /* success; free workspace, return C */
+}

workspace/slam_in_autonomous_driving/thirdparty/g2o/EXTERNAL/csparse/cs_amd.c

@@ -0,0 +1,364 @@
+#include "cs.h"
+/* clear w */
+static csi cs_wclear (csi mark, csi lemax, csi *w, csi n)
+{
+    csi k ;
+    if (mark < 2 || (mark + lemax < 0))
+    {
+        for (k = 0 ; k < n ; k++) if (w [k] != 0) w [k] = 1 ;
+        mark = 2 ;
+    }
+    return (mark) ;     /* at this point, w [0..n-1] < mark holds */
+}
+
+/* keep off-diagonal entries; drop diagonal entries */
+static csi cs_diag (csi i, csi j, double aij, void *other) { return (i != j) ; }
+
+/* p = amd(A+A') if symmetric is true, or amd(A'A) otherwise */
+csi *cs_amd (csi order, const cs *A)  /* order 0:natural, 1:Chol, 2:LU, 3:QR */
+{
+    cs *C, *A2, *AT ;
+    csi *Cp, *Ci, *last, *W, *len, *nv, *next, *P, *head, *elen, *degree, *w,
+        *hhead, *ATp, *ATi, d, dk, dext, lemax = 0, e, elenk, eln, i, j, k, k1,
+        k2, k3, jlast, ln, dense, nzmax, mindeg = 0, nvi, nvj, nvk, mark, wnvi,
+        ok, cnz, nel = 0, p, p1, p2, p3, p4, pj, pk, pk1, pk2, pn, q, n, m, t ;
+    csi h ;
+    /* --- Construct matrix C ----------------------------------------------- */
+    if (!CS_CSC (A) || order <= 0 || order > 3) return (NULL) ; /* check */
+    AT = cs_transpose (A, 0) ;              /* compute A' */
+    if (!AT) return (NULL) ;
+    m = A->m ; n = A->n ;
+    dense = CS_MAX (16, 10 * sqrt ((double) n)) ;   /* find dense threshold */
+    dense = CS_MIN (n-2, dense) ;
+    if (order == 1 && n == m)
+    {
+        C = cs_add (A, AT, 0, 0) ;          /* C = A+A' */
+    }
+    else if (order == 2)
+    {
+        ATp = AT->p ;                       /* drop dense columns from AT */
+        ATi = AT->i ;
+        for (p2 = 0, j = 0 ; j < m ; j++)
+        {
+            p = ATp [j] ;                   /* column j of AT starts here */
+            ATp [j] = p2 ;                  /* new column j starts here */
+            if (ATp [j+1] - p > dense) continue ;   /* skip dense col j */
+            for ( ; p < ATp [j+1] ; p++) ATi [p2++] = ATi [p] ;
+        }
+        ATp [m] = p2 ;                      /* finalize AT */
+        A2 = cs_transpose (AT, 0) ;         /* A2 = AT' */
+        C = A2 ? cs_multiply (AT, A2) : NULL ;  /* C=A'*A with no dense rows */
+        cs_spfree (A2) ;
+    }
+    else
+    {
+        C = cs_multiply (AT, A) ;           /* C=A'*A */
+    }
+    cs_spfree (AT) ;
+    if (!C) return (NULL) ;
+    cs_fkeep (C, &cs_diag, NULL) ;          /* drop diagonal entries */
+    Cp = C->p ;
+    cnz = Cp [n] ;
+    P = cs_malloc (n+1, sizeof (csi)) ;     /* allocate result */
+    W = cs_malloc (8*(n+1), sizeof (csi)) ; /* get workspace */
+    t = cnz + cnz/5 + 2*n ;                 /* add elbow room to C */
+    if (!P || !W || !cs_sprealloc (C, t)) return (cs_idone (P, C, W, 0)) ;
+    len  = W           ; nv     = W +   (n+1) ; next   = W + 2*(n+1) ;
+    head = W + 3*(n+1) ; elen   = W + 4*(n+1) ; degree = W + 5*(n+1) ;
+    w    = W + 6*(n+1) ; hhead  = W + 7*(n+1) ;
+    last = P ;                              /* use P as workspace for last */
+    /* --- Initialize quotient graph ---------------------------------------- */
+    for (k = 0 ; k < n ; k++) len [k] = Cp [k+1] - Cp [k] ;
+    len [n] = 0 ;
+    nzmax = C->nzmax ;
+    Ci = C->i ;
+    for (i = 0 ; i <= n ; i++)
+    {
+        head [i] = -1 ;                     /* degree list i is empty */
+        last [i] = -1 ;
+        next [i] = -1 ;
+        hhead [i] = -1 ;                    /* hash list i is empty */
+        nv [i] = 1 ;                        /* node i is just one node */
+        w [i] = 1 ;                         /* node i is alive */
+        elen [i] = 0 ;                      /* Ek of node i is empty */
+        degree [i] = len [i] ;              /* degree of node i */
+    }
+    mark = cs_wclear (0, 0, w, n) ;         /* clear w */
+    elen [n] = -2 ;                         /* n is a dead element */
+    Cp [n] = -1 ;                           /* n is a root of assembly tree */
+    w [n] = 0 ;                             /* n is a dead element */
+    /* --- Initialize degree lists ------------------------------------------ */
+    for (i = 0 ; i < n ; i++)
+    {
+        d = degree [i] ;
+        if (d == 0)                         /* node i is empty */
+        {
+            elen [i] = -2 ;                 /* element i is dead */
+            nel++ ;
+            Cp [i] = -1 ;                   /* i is a root of assembly tree */
+            w [i] = 0 ;
+        }
+        else if (d > dense)                 /* node i is dense */
+        {
+            nv [i] = 0 ;                    /* absorb i into element n */
+            elen [i] = -1 ;                 /* node i is dead */
+            nel++ ;
+            Cp [i] = CS_FLIP (n) ;
+            nv [n]++ ;
+        }
+        else
+        {
+            if (head [d] != -1) last [head [d]] = i ;
+            next [i] = head [d] ;           /* put node i in degree list d */
+            head [d] = i ;
+        }
+    }
+    while (nel < n)                         /* while (selecting pivots) do */
+    {
+        /* --- Select node of minimum approximate degree -------------------- */
+        for (k = -1 ; mindeg < n && (k = head [mindeg]) == -1 ; mindeg++) ;
+        if (next [k] != -1) last [next [k]] = -1 ;
+        head [mindeg] = next [k] ;          /* remove k from degree list */
+        elenk = elen [k] ;                  /* elenk = |Ek| */
+        nvk = nv [k] ;                      /* # of nodes k represents */
+        nel += nvk ;                        /* nv[k] nodes of A eliminated */
+        /* --- Garbage collection ------------------------------------------- */
+        if (elenk > 0 && cnz + mindeg >= nzmax)
+        {
+            for (j = 0 ; j < n ; j++)
+            {
+                if ((p = Cp [j]) >= 0)      /* j is a live node or element */
+                {
+                    Cp [j] = Ci [p] ;       /* save first entry of object */
+                    Ci [p] = CS_FLIP (j) ;  /* first entry is now CS_FLIP(j) */
+                }
+            }
+            for (q = 0, p = 0 ; p < cnz ; ) /* scan all of memory */
+            {
+                if ((j = CS_FLIP (Ci [p++])) >= 0)  /* found object j */
+                {
+                    Ci [q] = Cp [j] ;       /* restore first entry of object */
+                    Cp [j] = q++ ;          /* new pointer to object j */
+                    for (k3 = 0 ; k3 < len [j]-1 ; k3++) Ci [q++] = Ci [p++] ;
+                }
+            }
+            cnz = q ;                       /* Ci [cnz...nzmax-1] now free */
+        }
+        /* --- Construct new element ---------------------------------------- */
+        dk = 0 ;
+        nv [k] = -nvk ;                     /* flag k as in Lk */
+        p = Cp [k] ;
+        pk1 = (elenk == 0) ? p : cnz ;      /* do in place if elen[k] == 0 */
+        pk2 = pk1 ;
+        for (k1 = 1 ; k1 <= elenk + 1 ; k1++)
+        {
+            if (k1 > elenk)
+            {
+                e = k ;                     /* search the nodes in k */
+                pj = p ;                    /* list of nodes starts at Ci[pj]*/
+                ln = len [k] - elenk ;      /* length of list of nodes in k */
+            }
+            else
+            {
+                e = Ci [p++] ;              /* search the nodes in e */
+                pj = Cp [e] ;
+                ln = len [e] ;              /* length of list of nodes in e */
+            }
+            for (k2 = 1 ; k2 <= ln ; k2++)
+            {
+                i = Ci [pj++] ;
+                if ((nvi = nv [i]) <= 0) continue ; /* node i dead, or seen */
+                dk += nvi ;                 /* degree[Lk] += size of node i */
+                nv [i] = -nvi ;             /* negate nv[i] to denote i in Lk*/
+                Ci [pk2++] = i ;            /* place i in Lk */
+                if (next [i] != -1) last [next [i]] = last [i] ;
+                if (last [i] != -1)         /* remove i from degree list */
+                {
+                    next [last [i]] = next [i] ;
+                }
+                else
+                {
+                    head [degree [i]] = next [i] ;
+                }
+            }
+            if (e != k)
+            {
+                Cp [e] = CS_FLIP (k) ;      /* absorb e into k */
+                w [e] = 0 ;                 /* e is now a dead element */
+            }
+        }
+        if (elenk != 0) cnz = pk2 ;         /* Ci [cnz...nzmax] is free */
+        degree [k] = dk ;                   /* external degree of k - |Lk\i| */
+        Cp [k] = pk1 ;                      /* element k is in Ci[pk1..pk2-1] */
+        len [k] = pk2 - pk1 ;
+        elen [k] = -2 ;                     /* k is now an element */
+        /* --- Find set differences ----------------------------------------- */
+        mark = cs_wclear (mark, lemax, w, n) ;  /* clear w if necessary */
+        for (pk = pk1 ; pk < pk2 ; pk++)    /* scan 1: find |Le\Lk| */
+        {
+            i = Ci [pk] ;
+            if ((eln = elen [i]) <= 0) continue ;/* skip if elen[i] empty */
+            nvi = -nv [i] ;                      /* nv [i] was negated */
+            wnvi = mark - nvi ;
+            for (p = Cp [i] ; p <= Cp [i] + eln - 1 ; p++)  /* scan Ei */
+            {
+                e = Ci [p] ;
+                if (w [e] >= mark)
+                {
+                    w [e] -= nvi ;          /* decrement |Le\Lk| */
+                }
+                else if (w [e] != 0)        /* ensure e is a live element */
+                {
+                    w [e] = degree [e] + wnvi ; /* 1st time e seen in scan 1 */
+                }
+            }
+        }
+        /* --- Degree update ------------------------------------------------ */
+        for (pk = pk1 ; pk < pk2 ; pk++)    /* scan2: degree update */
+        {
+            i = Ci [pk] ;                   /* consider node i in Lk */
+            p1 = Cp [i] ;
+            p2 = p1 + elen [i] - 1 ;
+            pn = p1 ;
+            for (h = 0, d = 0, p = p1 ; p <= p2 ; p++)    /* scan Ei */
+            {
+                e = Ci [p] ;
+                if (w [e] != 0)             /* e is an unabsorbed element */
+                {
+                    dext = w [e] - mark ;   /* dext = |Le\Lk| */
+                    if (dext > 0)
+                    {
+                        d += dext ;         /* sum up the set differences */
+                        Ci [pn++] = e ;     /* keep e in Ei */
+                        h += e ;            /* compute the hash of node i */
+                    }
+                    else
+                    {
+                        Cp [e] = CS_FLIP (k) ;  /* aggressive absorb. e->k */
+                        w [e] = 0 ;             /* e is a dead element */
+                    }
+                }
+            }
+            elen [i] = pn - p1 + 1 ;        /* elen[i] = |Ei| */
+            p3 = pn ;
+            p4 = p1 + len [i] ;
+            for (p = p2 + 1 ; p < p4 ; p++) /* prune edges in Ai */
+            {
+                j = Ci [p] ;
+                if ((nvj = nv [j]) <= 0) continue ; /* node j dead or in Lk */
+                d += nvj ;                  /* degree(i) += |j| */
+                Ci [pn++] = j ;             /* place j in node list of i */
+                h += j ;                    /* compute hash for node i */
+            }
+            if (d == 0)                     /* check for mass elimination */
+            {
+                Cp [i] = CS_FLIP (k) ;      /* absorb i into k */
+                nvi = -nv [i] ;
+                dk -= nvi ;                 /* |Lk| -= |i| */
+                nvk += nvi ;                /* |k| += nv[i] */
+                nel += nvi ;
+                nv [i] = 0 ;
+                elen [i] = -1 ;             /* node i is dead */
+            }
+            else
+            {
+                degree [i] = CS_MIN (degree [i], d) ;   /* update degree(i) */
+                Ci [pn] = Ci [p3] ;         /* move first node to end */
+                Ci [p3] = Ci [p1] ;         /* move 1st el. to end of Ei */
+                Ci [p1] = k ;               /* add k as 1st element in of Ei */
+                len [i] = pn - p1 + 1 ;     /* new len of adj. list of node i */
+                h = ((h<0) ? (-h):h) % n ;  /* finalize hash of i */
+                next [i] = hhead [h] ;      /* place i in hash bucket */
+                hhead [h] = i ;
+                last [i] = h ;              /* save hash of i in last[i] */
+            }
+        }                                   /* scan2 is done */
+        degree [k] = dk ;                   /* finalize |Lk| */
+        lemax = CS_MAX (lemax, dk) ;
+        mark = cs_wclear (mark+lemax, lemax, w, n) ;    /* clear w */
+        /* --- Supernode detection ------------------------------------------ */
+        for (pk = pk1 ; pk < pk2 ; pk++)
+        {
+            i = Ci [pk] ;
+            if (nv [i] >= 0) continue ;         /* skip if i is dead */
+            h = last [i] ;                      /* scan hash bucket of node i */
+            i = hhead [h] ;
+            hhead [h] = -1 ;                    /* hash bucket will be empty */
+            for ( ; i != -1 && next [i] != -1 ; i = next [i], mark++)
+            {
+                ln = len [i] ;
+                eln = elen [i] ;
+                for (p = Cp [i]+1 ; p <= Cp [i] + ln-1 ; p++) w [Ci [p]] = mark;
+                jlast = i ;
+                for (j = next [i] ; j != -1 ; ) /* compare i with all j */
+                {
+                    ok = (len [j] == ln) && (elen [j] == eln) ;
+                    for (p = Cp [j] + 1 ; ok && p <= Cp [j] + ln - 1 ; p++)
+                    {
+                        if (w [Ci [p]] != mark) ok = 0 ;    /* compare i and j*/
+                    }
+                    if (ok)                     /* i and j are identical */
+                    {
+                        Cp [j] = CS_FLIP (i) ;  /* absorb j into i */
+                        nv [i] += nv [j] ;
+                        nv [j] = 0 ;
+                        elen [j] = -1 ;         /* node j is dead */
+                        j = next [j] ;          /* delete j from hash bucket */
+                        next [jlast] = j ;
+                    }
+                    else
+                    {
+                        jlast = j ;             /* j and i are different */
+                        j = next [j] ;
+                    }
+                }
+            }
+        }
+        /* --- Finalize new element------------------------------------------ */
+        for (p = pk1, pk = pk1 ; pk < pk2 ; pk++)   /* finalize Lk */
+        {
+            i = Ci [pk] ;
+            if ((nvi = -nv [i]) <= 0) continue ;/* skip if i is dead */
+            nv [i] = nvi ;                      /* restore nv[i] */
+            d = degree [i] + dk - nvi ;         /* compute external degree(i) */
+            d = CS_MIN (d, n - nel - nvi) ;
+            if (head [d] != -1) last [head [d]] = i ;
+            next [i] = head [d] ;               /* put i back in degree list */
+            last [i] = -1 ;
+            head [d] = i ;
+            mindeg = CS_MIN (mindeg, d) ;       /* find new minimum degree */
+            degree [i] = d ;
+            Ci [p++] = i ;                      /* place i in Lk */
+        }
+        nv [k] = nvk ;                      /* # nodes absorbed into k */
+        if ((len [k] = p-pk1) == 0)         /* length of adj list of element k*/
+        {
+            Cp [k] = -1 ;                   /* k is a root of the tree */
+            w [k] = 0 ;                     /* k is now a dead element */
+        }
+        if (elenk != 0) cnz = p ;           /* free unused space in Lk */
+    }
+    /* --- Postordering ----------------------------------------------------- */
+    for (i = 0 ; i < n ; i++) Cp [i] = CS_FLIP (Cp [i]) ;/* fix assembly tree */
+    for (j = 0 ; j <= n ; j++) head [j] = -1 ;
+    for (j = n ; j >= 0 ; j--)              /* place unordered nodes in lists */
+    {
+        if (nv [j] > 0) continue ;          /* skip if j is an element */
+        next [j] = head [Cp [j]] ;          /* place j in list of its parent */
+        head [Cp [j]] = j ;
+    }
+    for (e = n ; e >= 0 ; e--)              /* place elements in lists */
+    {
+        if (nv [e] <= 0) continue ;         /* skip unless e is an element */
+        if (Cp [e] != -1)
+        {
+            next [e] = head [Cp [e]] ;      /* place e in list of its parent */
+            head [Cp [e]] = e ;
+        }
+    }
+    for (k = 0, i = 0 ; i <= n ; i++)       /* postorder the assembly tree */
+    {
+        if (Cp [i] == -1) k = cs_tdfs (i, k, head, next, P, w) ;
+    }
+    return (cs_idone (P, C, W, 1)) ;
+}

workspace/slam_in_autonomous_driving/thirdparty/g2o/EXTERNAL/csparse/cs_api.h

@@ -0,0 +1,49 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2012 Mark Pupilli
+// 
+// g2o is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published
+// by the Free Software Foundation, either version 3 of the License, or
+// (at your option) any later version.
+// 
+// g2o is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+// GNU Lesser General Public License for more details.
+// 
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program.  If not, see <http://www.gnu.org/licenses/>.
+
+/***************************************************************************
+ *  Description: import/export macros for creating DLLS with Microsoft
+ *	compiler. Any exported function needs to be declared with the
+ *  appropriate G2O_XXXX_API macro. Also, there must be separate macros
+ *  for each DLL (arrrrrgh!!!)
+ *
+ *  17 Jan 2012
+ *  Email: pupilli@cs.bris.ac.uk
+ ****************************************************************************/
+#ifndef G2O_CSPARSE_API_H
+#define G2O_CSPARSE_API_H
+
+#include "g2o/config.h"
+
+#ifdef _MSC_VER
+ 
+// We are using a Microsoft compiler:
+#ifdef G2O_LGPL_SHARED_LIBS
+# ifdef csparse_EXPORTS
+#  define G2O_CSPARSE_API __declspec(dllexport)
+# else
+#  define G2O_CSPARSE_API __declspec(dllimport)
+# endif
+#else
+#define G2O_CSPARSE_API
+#endif
+
+#else
+// Not Microsoft compiler so set empty definition:
+# define G2O_CSPARSE_API
+#endif
+
+#endif // G2O_CSPARSE_API_H

workspace/slam_in_autonomous_driving/thirdparty/g2o/EXTERNAL/csparse/cs_chol.c

@@ -0,0 +1,58 @@
+#include "cs.h"
+/* L = chol (A, [pinv parent cp]), pinv is optional */
+csn *cs_chol (const cs *A, const css *S)
+{
+    double d, lki, *Lx, *x, *Cx ;
+    csi top, i, p, k, n, *Li, *Lp, *cp, *pinv, *s, *c, *parent, *Cp, *Ci ;
+    cs *L, *C, *E ;
+    csn *N ;
+    if (!CS_CSC (A) || !S || !S->cp || !S->parent) return (NULL) ;
+    n = A->n ;
+    N = cs_calloc (1, sizeof (csn)) ;       /* allocate result */
+    c = cs_malloc (2*n, sizeof (csi)) ;     /* get csi workspace */
+    x = cs_malloc (n, sizeof (double)) ;    /* get double workspace */
+    cp = S->cp ; pinv = S->pinv ; parent = S->parent ;
+    C = pinv ? cs_symperm (A, pinv, 1) : ((cs *) A) ;
+    E = pinv ? C : NULL ;           /* E is alias for A, or a copy E=A(p,p) */
+    if (!N || !c || !x || !C) return (cs_ndone (N, E, c, x, 0)) ;
+    s = c + n ;
+    Cp = C->p ; Ci = C->i ; Cx = C->x ;
+    N->L = L = cs_spalloc (n, n, cp [n], 1, 0) ;    /* allocate result */
+    if (!L) return (cs_ndone (N, E, c, x, 0)) ;
+    Lp = L->p ; Li = L->i ; Lx = L->x ;
+    for (k = 0 ; k < n ; k++) Lp [k] = c [k] = cp [k] ;
+    for (k = 0 ; k < n ; k++)       /* compute L(k,:) for L*L' = C */
+    {
+        /* --- Nonzero pattern of L(k,:) ------------------------------------ */
+        top = cs_ereach (C, k, parent, s, c) ;      /* find pattern of L(k,:) */
+        x [k] = 0 ;                                 /* x (0:k) is now zero */
+        for (p = Cp [k] ; p < Cp [k+1] ; p++)       /* x = full(triu(C(:,k))) */
+        {
+            if (Ci [p] <= k) x [Ci [p]] = Cx [p] ;
+        }
+        d = x [k] ;                     /* d = C(k,k) */
+        x [k] = 0 ;                     /* clear x for k+1st iteration */
+        /* --- Triangular solve --------------------------------------------- */
+        for ( ; top < n ; top++)    /* solve L(0:k-1,0:k-1) * x = C(:,k) */
+        {
+            i = s [top] ;               /* s [top..n-1] is pattern of L(k,:) */
+            lki = x [i] / Lx [Lp [i]] ; /* L(k,i) = x (i) / L(i,i) */
+            x [i] = 0 ;                 /* clear x for k+1st iteration */
+            for (p = Lp [i] + 1 ; p < c [i] ; p++)
+            {
+                x [Li [p]] -= Lx [p] * lki ;
+            }
+            d -= lki * lki ;            /* d = d - L(k,i)*L(k,i) */
+            p = c [i]++ ;
+            Li [p] = k ;                /* store L(k,i) in column i */
+            Lx [p] = lki ;
+        }
+        /* --- Compute L(k,k) ----------------------------------------------- */
+        if (d <= 0) return (cs_ndone (N, E, c, x, 0)) ; /* not pos def */
+        p = c [k]++ ;
+        Li [p] = k ;                /* store L(k,k) = sqrt (d) in column k */
+        Lx [p] = sqrt (d) ;
+    }
+    Lp [n] = cp [n] ;               /* finalize L */
+    return (cs_ndone (N, E, c, x, 1)) ; /* success: free E,s,x; return N */
+}

workspace/slam_in_autonomous_driving/thirdparty/g2o/EXTERNAL/csparse/cs_cholsol.c

@@ -0,0 +1,26 @@
+#include "cs.h"
+/* x=A\b where A is symmetric positive definite; b overwritten with solution */
+csi cs_cholsol (csi order, const cs *A, double *b)
+{
+    double *x ;
+    css *S ;
+    csn *N ;
+    csi n, ok ;
+    if (!CS_CSC (A) || !b) return (0) ;     /* check inputs */
+    n = A->n ;
+    S = cs_schol (order, A) ;               /* ordering and symbolic analysis */
+    N = cs_chol (A, S) ;                    /* numeric Cholesky factorization */
+    x = cs_malloc (n, sizeof (double)) ;    /* get workspace */
+    ok = (S && N && x) ;
+    if (ok)
+    {
+        cs_ipvec (S->pinv, b, x, n) ;   /* x = P*b */
+        cs_lsolve (N->L, x) ;           /* x = L\x */
+        cs_ltsolve (N->L, x) ;          /* x = L'\x */
+        cs_pvec (S->pinv, x, b, n) ;    /* b = P'*x */
+    }
+    cs_free (x) ;
+    cs_sfree (S) ;
+    cs_nfree (N) ;
+    return (ok) ;
+}

workspace/slam_in_autonomous_driving/thirdparty/g2o/EXTERNAL/csparse/cs_compress.c

@@ -0,0 +1,22 @@
+#include "cs.h"
+/* C = compressed-column form of a triplet matrix T */
+cs *cs_compress (const cs *T)
+{
+    csi m, n, nz, p, k, *Cp, *Ci, *w, *Ti, *Tj ;
+    double *Cx, *Tx ;
+    cs *C ;
+    if (!CS_TRIPLET (T)) return (NULL) ;                /* check inputs */
+    m = T->m ; n = T->n ; Ti = T->i ; Tj = T->p ; Tx = T->x ; nz = T->nz ;
+    C = cs_spalloc (m, n, nz, Tx != NULL, 0) ;          /* allocate result */
+    w = cs_calloc (n, sizeof (csi)) ;                   /* get workspace */
+    if (!C || !w) return (cs_done (C, w, NULL, 0)) ;    /* out of memory */
+    Cp = C->p ; Ci = C->i ; Cx = C->x ;
+    for (k = 0 ; k < nz ; k++) w [Tj [k]]++ ;           /* column counts */
+    cs_cumsum (Cp, w, n) ;                              /* column pointers */
+    for (k = 0 ; k < nz ; k++)
+    {
+        Ci [p = w [Tj [k]]++] = Ti [k] ;    /* A(i,j) is the pth entry in C */
+        if (Cx) Cx [p] = Tx [k] ;
+    }
+    return (cs_done (C, w, NULL, 1)) ;      /* success; free w and return C */
+}

workspace/slam_in_autonomous_driving/thirdparty/g2o/EXTERNAL/csparse/cs_counts.c

@@ -0,0 +1,61 @@
+#include "cs.h"
+/* column counts of LL'=A or LL'=A'A, given parent & post ordering */
+#define HEAD(k,j) (ata ? head [k] : j)
+#define NEXT(J)   (ata ? next [J] : -1)
+static void init_ata (cs *AT, const csi *post, csi *w, csi **head, csi **next)
+{
+    csi i, k, p, m = AT->n, n = AT->m, *ATp = AT->p, *ATi = AT->i ;
+    *head = w+4*n, *next = w+5*n+1 ;
+    for (k = 0 ; k < n ; k++) w [post [k]] = k ;    /* invert post */
+    for (i = 0 ; i < m ; i++)
+    {
+        for (k = n, p = ATp[i] ; p < ATp[i+1] ; p++) k = CS_MIN (k, w [ATi[p]]);
+        (*next) [i] = (*head) [k] ;     /* place row i in linked list k */
+        (*head) [k] = i ;
+    }
+}
+csi *cs_counts (const cs *A, const csi *parent, const csi *post, csi ata)
+{
+    csi i, j, k, n, m, J, s, p, q, jleaf, *ATp, *ATi, *maxfirst, *prevleaf,
+        *ancestor, *head = NULL, *next = NULL, *colcount, *w, *first, *delta ;
+    cs *AT ;
+    if (!CS_CSC (A) || !parent || !post) return (NULL) ;    /* check inputs */
+    m = A->m ; n = A->n ;
+    s = 4*n + (ata ? (n+m+1) : 0) ;
+    delta = colcount = cs_malloc (n, sizeof (csi)) ;    /* allocate result */
+    w = cs_malloc (s, sizeof (csi)) ;                   /* get workspace */
+    AT = cs_transpose (A, 0) ;                          /* AT = A' */
+    if (!AT || !colcount || !w) return (cs_idone (colcount, AT, w, 0)) ;
+    ancestor = w ; maxfirst = w+n ; prevleaf = w+2*n ; first = w+3*n ;
+    for (k = 0 ; k < s ; k++) w [k] = -1 ;      /* clear workspace w [0..s-1] */
+    for (k = 0 ; k < n ; k++)                   /* find first [j] */
+    {
+        j = post [k] ;
+        delta [j] = (first [j] == -1) ? 1 : 0 ;  /* delta[j]=1 if j is a leaf */
+        for ( ; j != -1 && first [j] == -1 ; j = parent [j]) first [j] = k ;
+    }
+    ATp = AT->p ; ATi = AT->i ;
+    if (ata) init_ata (AT, post, w, &head, &next) ;
+    for (i = 0 ; i < n ; i++) ancestor [i] = i ; /* each node in its own set */
+    for (k = 0 ; k < n ; k++)
+    {
+        j = post [k] ;          /* j is the kth node in postordered etree */
+        if (parent [j] != -1) delta [parent [j]]-- ;    /* j is not a root */
+        for (J = HEAD (k,j) ; J != -1 ; J = NEXT (J))   /* J=j for LL'=A case */
+        {
+            for (p = ATp [J] ; p < ATp [J+1] ; p++)
+            {
+                i = ATi [p] ;
+                q = cs_leaf (i, j, first, maxfirst, prevleaf, ancestor, &jleaf);
+                if (jleaf >= 1) delta [j]++ ;   /* A(i,j) is in skeleton */
+                if (jleaf == 2) delta [q]-- ;   /* account for overlap in q */
+            }
+        }
+        if (parent [j] != -1) ancestor [j] = parent [j] ;
+    }
+    for (j = 0 ; j < n ; j++)           /* sum up delta's of each child */
+    {
+        if (parent [j] != -1) colcount [parent [j]] += colcount [j] ;
+    }
+    return (cs_idone (colcount, AT, w, 1)) ;    /* success: free workspace */
+} 

workspace/slam_in_autonomous_driving/thirdparty/g2o/EXTERNAL/csparse/cs_cumsum.c

@@ -0,0 +1,17 @@
+#include "cs.h"
+/* p [0..n] = cumulative sum of c [0..n-1], and then copy p [0..n-1] into c */
+double cs_cumsum (csi *p, csi *c, csi n)
+{
+    csi i, nz = 0 ;
+    double nz2 = 0 ;
+    if (!p || !c) return (-1) ;     /* check inputs */
+    for (i = 0 ; i < n ; i++)
+    {
+        p [i] = nz ;
+        nz += c [i] ;
+        nz2 += c [i] ;              /* also in double to avoid csi overflow */
+        c [i] = p [i] ;             /* also copy p[0..n-1] back into c[0..n-1]*/
+    }
+    p [n] = nz ;
+    return (nz2) ;                  /* return sum (c [0..n-1]) */
+}

workspace/slam_in_autonomous_driving/thirdparty/g2o/EXTERNAL/csparse/cs_dfs.c

@@ -0,0 +1,36 @@
+#include "cs.h"
+/* depth-first-search of the graph of a matrix, starting at node j */
+csi cs_dfs (csi j, cs *G, csi top, csi *xi, csi *pstack, const csi *pinv)
+{
+    csi i, p, p2, done, jnew, head = 0, *Gp, *Gi ;
+    if (!CS_CSC (G) || !xi || !pstack) return (-1) ;    /* check inputs */
+    Gp = G->p ; Gi = G->i ;
+    xi [0] = j ;                /* initialize the recursion stack */
+    while (head >= 0)
+    {
+        j = xi [head] ;         /* get j from the top of the recursion stack */
+        jnew = pinv ? (pinv [j]) : j ;
+        if (!CS_MARKED (Gp, j))
+        {
+            CS_MARK (Gp, j) ;       /* mark node j as visited */
+            pstack [head] = (jnew < 0) ? 0 : CS_UNFLIP (Gp [jnew]) ;
+        }
+        done = 1 ;                  /* node j done if no unvisited neighbors */
+        p2 = (jnew < 0) ? 0 : CS_UNFLIP (Gp [jnew+1]) ;
+        for (p = pstack [head] ; p < p2 ; p++)  /* examine all neighbors of j */
+        {
+            i = Gi [p] ;            /* consider neighbor node i */
+            if (CS_MARKED (Gp, i)) continue ;   /* skip visited node i */
+            pstack [head] = p ;     /* pause depth-first search of node j */
+            xi [++head] = i ;       /* start dfs at node i */
+            done = 0 ;              /* node j is not done */
+            break ;                 /* break, to start dfs (i) */
+        }
+        if (done)               /* depth-first search at node j is done */
+        {
+            head-- ;            /* remove j from the recursion stack */
+            xi [--top] = j ;    /* and place in the output stack */
+        }
+    }
+    return (top) ;
+}

workspace/slam_in_autonomous_driving/thirdparty/g2o/EXTERNAL/csparse/cs_dmperm.c

@@ -0,0 +1,144 @@
+#include "cs.h"
+/* breadth-first search for coarse decomposition (C0,C1,R1 or R0,R3,C3) */
+static csi cs_bfs (const cs *A, csi n, csi *wi, csi *wj, csi *queue,
+    const csi *imatch, const csi *jmatch, csi mark)
+{
+    csi *Ap, *Ai, head = 0, tail = 0, j, i, p, j2 ;
+    cs *C ;
+    for (j = 0 ; j < n ; j++)           /* place all unmatched nodes in queue */
+    {
+        if (imatch [j] >= 0) continue ; /* skip j if matched */
+        wj [j] = 0 ;                    /* j in set C0 (R0 if transpose) */
+        queue [tail++] = j ;            /* place unmatched col j in queue */
+    }
+    if (tail == 0) return (1) ;         /* quick return if no unmatched nodes */
+    C = (mark == 1) ? ((cs *) A) : cs_transpose (A, 0) ;
+    if (!C) return (0) ;                /* bfs of C=A' to find R3,C3 from R0 */
+    Ap = C->p ; Ai = C->i ;
+    while (head < tail)                 /* while queue is not empty */
+    {
+        j = queue [head++] ;            /* get the head of the queue */
+        for (p = Ap [j] ; p < Ap [j+1] ; p++)
+        {
+            i = Ai [p] ;
+            if (wi [i] >= 0) continue ; /* skip if i is marked */
+            wi [i] = mark ;             /* i in set R1 (C3 if transpose) */
+            j2 = jmatch [i] ;           /* traverse alternating path to j2 */
+            if (wj [j2] >= 0) continue ;/* skip j2 if it is marked */
+            wj [j2] = mark ;            /* j2 in set C1 (R3 if transpose) */
+            queue [tail++] = j2 ;       /* add j2 to queue */
+        }
+    }
+    if (mark != 1) cs_spfree (C) ;      /* free A' if it was created */
+    return (1) ;
+}
+
+/* collect matched rows and columns into p and q */
+static void cs_matched (csi n, const csi *wj, const csi *imatch, csi *p, csi *q,
+    csi *cc, csi *rr, csi set, csi mark)
+{
+    csi kc = cc [set], j ;
+    csi kr = rr [set-1] ;
+    for (j = 0 ; j < n ; j++)
+    {
+        if (wj [j] != mark) continue ;      /* skip if j is not in C set */
+        p [kr++] = imatch [j] ;
+        q [kc++] = j ;
+    }
+    cc [set+1] = kc ;
+    rr [set] = kr ;
+}
+
+/* collect unmatched rows into the permutation vector p */
+static void cs_unmatched (csi m, const csi *wi, csi *p, csi *rr, csi set)
+{
+    csi i, kr = rr [set] ;
+    for (i = 0 ; i < m ; i++) if (wi [i] == 0) p [kr++] = i ;
+    rr [set+1] = kr ;
+}
+
+/* return 1 if row i is in R2 */
+static csi cs_rprune (csi i, csi j, double aij, void *other)
+{
+    csi *rr = (csi *) other ;
+    return (i >= rr [1] && i < rr [2]) ;
+}
+
+/* Given A, compute coarse and then fine dmperm */
+csd *cs_dmperm (const cs *A, csi seed)
+{
+    csi m, n, i, j, k, cnz, nc, *jmatch, *imatch, *wi, *wj, *pinv, *Cp, *Ci,
+        *ps, *rs, nb1, nb2, *p, *q, *cc, *rr, *r, *s, ok ;
+    cs *C ;
+    csd *D, *scc ;
+    /* --- Maximum matching ------------------------------------------------- */
+    if (!CS_CSC (A)) return (NULL) ;            /* check inputs */
+    m = A->m ; n = A->n ;
+    D = cs_dalloc (m, n) ;                      /* allocate result */
+    if (!D) return (NULL) ;
+    p = D->p ; q = D->q ; r = D->r ; s = D->s ; cc = D->cc ; rr = D->rr ;
+    jmatch = cs_maxtrans (A, seed) ;            /* max transversal */
+    imatch = jmatch + m ;                       /* imatch = inverse of jmatch */
+    if (!jmatch) return (cs_ddone (D, NULL, jmatch, 0)) ;
+    /* --- Coarse decomposition --------------------------------------------- */
+    wi = r ; wj = s ;                           /* use r and s as workspace */
+    for (j = 0 ; j < n ; j++) wj [j] = -1 ;     /* unmark all cols for bfs */
+    for (i = 0 ; i < m ; i++) wi [i] = -1 ;     /* unmark all rows for bfs */
+    cs_bfs (A, n, wi, wj, q, imatch, jmatch, 1) ;       /* find C1, R1 from C0*/
+    ok = cs_bfs (A, m, wj, wi, p, jmatch, imatch, 3) ;  /* find R3, C3 from R0*/
+    if (!ok) return (cs_ddone (D, NULL, jmatch, 0)) ;
+    cs_unmatched (n, wj, q, cc, 0) ;                    /* unmatched set C0 */
+    cs_matched (n, wj, imatch, p, q, cc, rr, 1, 1) ;    /* set R1 and C1 */
+    cs_matched (n, wj, imatch, p, q, cc, rr, 2, -1) ;   /* set R2 and C2 */
+    cs_matched (n, wj, imatch, p, q, cc, rr, 3, 3) ;    /* set R3 and C3 */
+    cs_unmatched (m, wi, p, rr, 3) ;                    /* unmatched set R0 */
+    cs_free (jmatch) ;
+    /* --- Fine decomposition ----------------------------------------------- */
+    pinv = cs_pinv (p, m) ;         /* pinv=p' */
+    if (!pinv) return (cs_ddone (D, NULL, NULL, 0)) ;
+    C = cs_permute (A, pinv, q, 0) ;/* C=A(p,q) (it will hold A(R2,C2)) */
+    cs_free (pinv) ;
+    if (!C) return (cs_ddone (D, NULL, NULL, 0)) ;
+    Cp = C->p ;
+    nc = cc [3] - cc [2] ;          /* delete cols C0, C1, and C3 from C */
+    if (cc [2] > 0) for (j = cc [2] ; j <= cc [3] ; j++) Cp [j-cc[2]] = Cp [j] ;
+    C->n = nc ;
+    if (rr [2] - rr [1] < m)        /* delete rows R0, R1, and R3 from C */
+    {
+        cs_fkeep (C, cs_rprune, rr) ;
+        cnz = Cp [nc] ;
+        Ci = C->i ;
+        if (rr [1] > 0) for (k = 0 ; k < cnz ; k++) Ci [k] -= rr [1] ;
+    }
+    C->m = nc ;
+    scc = cs_scc (C) ;              /* find strongly connected components of C*/
+    if (!scc) return (cs_ddone (D, C, NULL, 0)) ;
+    /* --- Combine coarse and fine decompositions --------------------------- */
+    ps = scc->p ;                   /* C(ps,ps) is the permuted matrix */
+    rs = scc->r ;                   /* kth block is rs[k]..rs[k+1]-1 */
+    nb1 = scc->nb  ;                /* # of blocks of A(R2,C2) */
+    for (k = 0 ; k < nc ; k++) wj [k] = q [ps [k] + cc [2]] ;
+    for (k = 0 ; k < nc ; k++) q [k + cc [2]] = wj [k] ;
+    for (k = 0 ; k < nc ; k++) wi [k] = p [ps [k] + rr [1]] ;
+    for (k = 0 ; k < nc ; k++) p [k + rr [1]] = wi [k] ;
+    nb2 = 0 ;                       /* create the fine block partitions */
+    r [0] = s [0] = 0 ;
+    if (cc [2] > 0) nb2++ ;         /* leading coarse block A (R1, [C0 C1]) */
+    for (k = 0 ; k < nb1 ; k++)     /* coarse block A (R2,C2) */
+    {
+        r [nb2] = rs [k] + rr [1] ; /* A (R2,C2) splits into nb1 fine blocks */
+        s [nb2] = rs [k] + cc [2] ;
+        nb2++ ;
+    }
+    if (rr [2] < m)
+    {
+        r [nb2] = rr [2] ;          /* trailing coarse block A ([R3 R0], C3) */
+        s [nb2] = cc [3] ;
+        nb2++ ;
+    }
+    r [nb2] = m ;
+    s [nb2] = n ;
+    D->nb = nb2 ;
+    cs_dfree (scc) ;
+    return (cs_ddone (D, C, NULL, 1)) ;
+}

workspace/slam_in_autonomous_driving/thirdparty/g2o/EXTERNAL/csparse/cs_droptol.c

@@ -0,0 +1,9 @@
+#include "cs.h"
+static csi cs_tol (csi i, csi j, double aij, void *tol)
+{
+    return (fabs (aij) > *((double *) tol)) ;
+}
+csi cs_droptol (cs *A, double tol)
+{
+    return (cs_fkeep (A, &cs_tol, &tol)) ;    /* keep all large entries */
+}

workspace/slam_in_autonomous_driving/thirdparty/g2o/EXTERNAL/csparse/cs_dropzeros.c

@@ -0,0 +1,9 @@
+#include "cs.h"
+static csi cs_nonzero (csi i, csi j, double aij, void *other)
+{
+    return (aij != 0) ;
+}
+csi cs_dropzeros (cs *A)
+{
+    return (cs_fkeep (A, &cs_nonzero, NULL)) ;  /* keep all nonzero entries */
+} 

workspace/slam_in_autonomous_driving/thirdparty/g2o/EXTERNAL/csparse/cs_dupl.c

@@ -0,0 +1,34 @@
+#include "cs.h"
+/* remove duplicate entries from A */
+csi cs_dupl (cs *A)
+{
+    csi i, j, p, q, nz = 0, n, m, *Ap, *Ai, *w ;
+    double *Ax ;
+    if (!CS_CSC (A)) return (0) ;               /* check inputs */
+    m = A->m ; n = A->n ; Ap = A->p ; Ai = A->i ; Ax = A->x ;
+    w = cs_malloc (m, sizeof (csi)) ;           /* get workspace */
+    if (!w) return (0) ;                        /* out of memory */
+    for (i = 0 ; i < m ; i++) w [i] = -1 ;      /* row i not yet seen */
+    for (j = 0 ; j < n ; j++)
+    {
+        q = nz ;                                /* column j will start at q */
+        for (p = Ap [j] ; p < Ap [j+1] ; p++)
+        {
+            i = Ai [p] ;                        /* A(i,j) is nonzero */
+            if (w [i] >= q)
+            {
+                Ax [w [i]] += Ax [p] ;          /* A(i,j) is a duplicate */
+            }
+            else
+            {
+                w [i] = nz ;                    /* record where row i occurs */
+                Ai [nz] = i ;                   /* keep A(i,j) */
+                Ax [nz++] = Ax [p] ;
+            }
+        }
+        Ap [j] = q ;                            /* record start of column j */
+    }
+    Ap [n] = nz ;                               /* finalize A */
+    cs_free (w) ;                               /* free workspace */
+    return (cs_sprealloc (A, 0)) ;              /* remove extra space from A */
+}

workspace/slam_in_autonomous_driving/thirdparty/g2o/EXTERNAL/csparse/cs_entry.c

@@ -0,0 +1,13 @@
+#include "cs.h"
+/* add an entry to a triplet matrix; return 1 if ok, 0 otherwise */
+csi cs_entry (cs *T, csi i, csi j, double x)
+{
+    if (!CS_TRIPLET (T) || i < 0 || j < 0) return (0) ;     /* check inputs */
+    if (T->nz >= T->nzmax && !cs_sprealloc (T,2*(T->nzmax))) return (0) ;
+    if (T->x) T->x [T->nz] = x ;
+    T->i [T->nz] = i ;
+    T->p [T->nz++] = j ;
+    T->m = CS_MAX (T->m, i+1) ;
+    T->n = CS_MAX (T->n, j+1) ;
+    return (1) ;
+}

workspace/slam_in_autonomous_driving/thirdparty/g2o/EXTERNAL/csparse/cs_ereach.c

@@ -0,0 +1,23 @@
+#include "cs.h"
+/* find nonzero pattern of Cholesky L(k,1:k-1) using etree and triu(A(:,k)) */
+csi cs_ereach (const cs *A, csi k, const csi *parent, csi *s, csi *w)
+{
+    csi i, p, n, len, top, *Ap, *Ai ;
+    if (!CS_CSC (A) || !parent || !s || !w) return (-1) ;   /* check inputs */
+    top = n = A->n ; Ap = A->p ; Ai = A->i ;
+    CS_MARK (w, k) ;                /* mark node k as visited */
+    for (p = Ap [k] ; p < Ap [k+1] ; p++)
+    {
+        i = Ai [p] ;                /* A(i,k) is nonzero */
+        if (i > k) continue ;       /* only use upper triangular part of A */
+        for (len = 0 ; !CS_MARKED (w,i) ; i = parent [i]) /* traverse up etree*/
+        {
+            s [len++] = i ;         /* L(k,i) is nonzero */
+            CS_MARK (w, i) ;        /* mark i as visited */
+        }
+        while (len > 0) s [--top] = s [--len] ; /* push path onto stack */
+    }
+    for (p = top ; p < n ; p++) CS_MARK (w, s [p]) ;    /* unmark all nodes */
+    CS_MARK (w, k) ;                /* unmark node k */
+    return (top) ;                  /* s [top..n-1] contains pattern of L(k,:)*/
+}

workspace/slam_in_autonomous_driving/thirdparty/g2o/EXTERNAL/csparse/cs_etree.c

@@ -0,0 +1,30 @@
+#include "cs.h"
+/* compute the etree of A (using triu(A), or A'A without forming A'A */
+csi *cs_etree (const cs *A, csi ata)
+{
+    csi i, k, p, m, n, inext, *Ap, *Ai, *w, *parent, *ancestor, *prev ;
+    if (!CS_CSC (A)) return (NULL) ;        /* check inputs */
+    m = A->m ; n = A->n ; Ap = A->p ; Ai = A->i ;
+    parent = cs_malloc (n, sizeof (csi)) ;              /* allocate result */
+    w = cs_malloc (n + (ata ? m : 0), sizeof (csi)) ;   /* get workspace */
+    if (!w || !parent) return (cs_idone (parent, NULL, w, 0)) ;
+    ancestor = w ; prev = w + n ;
+    if (ata) for (i = 0 ; i < m ; i++) prev [i] = -1 ;
+    for (k = 0 ; k < n ; k++)
+    {
+        parent [k] = -1 ;                   /* node k has no parent yet */
+        ancestor [k] = -1 ;                 /* nor does k have an ancestor */
+        for (p = Ap [k] ; p < Ap [k+1] ; p++)
+        {
+            i = ata ? (prev [Ai [p]]) : (Ai [p]) ;
+            for ( ; i != -1 && i < k ; i = inext)   /* traverse from i to k */
+            {
+                inext = ancestor [i] ;              /* inext = ancestor of i */
+                ancestor [i] = k ;                  /* path compression */
+                if (inext == -1) parent [i] = k ;   /* no anc., parent is k */
+            }
+            if (ata) prev [Ai [p]] = k ;
+        }
+    }
+    return (cs_idone (parent, NULL, w, 1)) ;
+}

workspace/slam_in_autonomous_driving/thirdparty/g2o/EXTERNAL/csparse/cs_fkeep.c

@@ -0,0 +1,25 @@
+#include "cs.h"
+/* drop entries for which fkeep(A(i,j)) is false; return nz if OK, else -1 */
+csi cs_fkeep (cs *A, csi (*fkeep) (csi, csi, double, void *), void *other)
+{
+    csi j, p, nz = 0, n, *Ap, *Ai ;
+    double *Ax ;
+    if (!CS_CSC (A) || !fkeep) return (-1) ;    /* check inputs */
+    n = A->n ; Ap = A->p ; Ai = A->i ; Ax = A->x ;
+    for (j = 0 ; j < n ; j++)
+    {
+        p = Ap [j] ;                        /* get current location of col j */
+        Ap [j] = nz ;                       /* record new location of col j */
+        for ( ; p < Ap [j+1] ; p++)
+        {
+            if (fkeep (Ai [p], j, Ax ? Ax [p] : 1, other))
+            {
+                if (Ax) Ax [nz] = Ax [p] ;  /* keep A(i,j) */
+                Ai [nz++] = Ai [p] ;
+            }
+        }
+    }
+    Ap [n] = nz ;                           /* finalize A */
+    cs_sprealloc (A, 0) ;                   /* remove extra space from A */
+    return (nz) ;
+}

workspace/slam_in_autonomous_driving/thirdparty/g2o/EXTERNAL/csparse/cs_gaxpy.c

@@ -0,0 +1,17 @@
+#include "cs.h"
+/* y = A*x+y */
+csi cs_gaxpy (const cs *A, const double *x, double *y)
+{
+    csi p, j, n, *Ap, *Ai ;
+    double *Ax ;
+    if (!CS_CSC (A) || !x || !y) return (0) ;       /* check inputs */
+    n = A->n ; Ap = A->p ; Ai = A->i ; Ax = A->x ;
+    for (j = 0 ; j < n ; j++)
+    {
+        for (p = Ap [j] ; p < Ap [j+1] ; p++)
+        {
+            y [Ai [p]] += Ax [p] * x [j] ;
+        }
+    }
+    return (1) ;
+}

workspace/slam_in_autonomous_driving/thirdparty/g2o/EXTERNAL/csparse/cs_happly.c

@@ -0,0 +1,19 @@
+#include "cs.h"
+/* apply the ith Householder vector to x */
+csi cs_happly (const cs *V, csi i, double beta, double *x)
+{
+    csi p, *Vp, *Vi ;
+    double *Vx, tau = 0 ;
+    if (!CS_CSC (V) || !x) return (0) ;     /* check inputs */
+    Vp = V->p ; Vi = V->i ; Vx = V->x ;
+    for (p = Vp [i] ; p < Vp [i+1] ; p++)   /* tau = v'*x */
+    {
+        tau += Vx [p] * x [Vi [p]] ;
+    }
+    tau *= beta ;                           /* tau = beta*(v'*x) */
+    for (p = Vp [i] ; p < Vp [i+1] ; p++)   /* x = x - v*tau */
+    {
+        x [Vi [p]] -= Vx [p] * tau ;
+    }
+    return (1) ;
+}

workspace/slam_in_autonomous_driving/thirdparty/g2o/EXTERNAL/csparse/cs_house.c

@@ -0,0 +1,23 @@
+#include "cs.h"
+/* create a Householder reflection [v,beta,s]=house(x), overwrite x with v,
+ * where (I-beta*v*v')*x = s*e1.  See Algo 5.1.1, Golub & Van Loan, 3rd ed. */
+double cs_house (double *x, double *beta, csi n)
+{
+    double s, sigma = 0 ;
+    csi i ;
+    if (!x || !beta) return (-1) ;          /* check inputs */
+    for (i = 1 ; i < n ; i++) sigma += x [i] * x [i] ;
+    if (sigma == 0)
+    {
+        s = fabs (x [0]) ;                  /* s = |x(0)| */
+        (*beta) = (x [0] <= 0) ? 2 : 0 ;
+        x [0] = 1 ;
+    }
+    else
+    {
+        s = sqrt (x [0] * x [0] + sigma) ;  /* s = norm (x) */
+        x [0] = (x [0] <= 0) ? (x [0] - s) : (-sigma / (x [0] + s)) ;
+        (*beta) = -1. / (s * x [0]) ;
+    }
+    return (s) ;
+}

workspace/slam_in_autonomous_driving/thirdparty/g2o/EXTERNAL/csparse/cs_ipvec.c

@@ -0,0 +1,9 @@
+#include "cs.h"
+/* x(p) = b, for dense vectors x and b; p=NULL denotes identity */
+csi cs_ipvec (const csi *p, const double *b, double *x, csi n)
+{
+    csi k ;
+    if (!x || !b) return (0) ;                              /* check inputs */
+    for (k = 0 ; k < n ; k++) x [p ? p [k] : k] = b [k] ;
+    return (1) ;
+}

workspace/slam_in_autonomous_driving/thirdparty/g2o/EXTERNAL/csparse/cs_leaf.c

@@ -0,0 +1,22 @@
+#include "cs.h"
+/* consider A(i,j), node j in ith row subtree and return lca(jprev,j) */
+csi cs_leaf (csi i, csi j, const csi *first, csi *maxfirst, csi *prevleaf,
+    csi *ancestor, csi *jleaf)
+{
+    csi q, s, sparent, jprev ;
+    if (!first || !maxfirst || !prevleaf || !ancestor || !jleaf) return (-1) ;
+    *jleaf = 0 ;
+    if (i <= j || first [j] <= maxfirst [i]) return (-1) ;  /* j not a leaf */
+    maxfirst [i] = first [j] ;      /* update max first[j] seen so far */
+    jprev = prevleaf [i] ;          /* jprev = previous leaf of ith subtree */
+    prevleaf [i] = j ;
+    *jleaf = (jprev == -1) ? 1: 2 ; /* j is first or subsequent leaf */
+    if (*jleaf == 1) return (i) ;   /* if 1st leaf, q = root of ith subtree */
+    for (q = jprev ; q != ancestor [q] ; q = ancestor [q]) ;
+    for (s = jprev ; s != q ; s = sparent)
+    {
+        sparent = ancestor [s] ;    /* path compression */
+        ancestor [s] = q ;
+    }
+    return (q) ;                    /* q = least common ancester (jprev,j) */
+}

workspace/slam_in_autonomous_driving/thirdparty/g2o/EXTERNAL/csparse/cs_load.c

@@ -0,0 +1,15 @@
+#include "cs.h"
+/* load a triplet matrix from a file */
+cs *cs_load (FILE *f)
+{
+    double i, j ;   /* use double for integers to avoid csi conflicts */
+    double x ;
+    cs *T ;
+    if (!f) return (NULL) ;                             /* check inputs */
+    T = cs_spalloc (0, 0, 1, 1, 1) ;                    /* allocate result */
+    while (fscanf (f, "%lg %lg %lg\n", &i, &j, &x) == 3)
+    {
+        if (!cs_entry (T, (csi) i, (csi) j, x)) return (cs_spfree (T)) ;
+    }
+    return (T) ;
+}

workspace/slam_in_autonomous_driving/thirdparty/g2o/EXTERNAL/csparse/cs_lsolve.c

@@ -0,0 +1,18 @@
+#include "cs.h"
+/* solve Lx=b where x and b are dense.  x=b on input, solution on output. */
+csi cs_lsolve (const cs *L, double *x)
+{
+    csi p, j, n, *Lp, *Li ;
+    double *Lx ;
+    if (!CS_CSC (L) || !x) return (0) ;                     /* check inputs */
+    n = L->n ; Lp = L->p ; Li = L->i ; Lx = L->x ;
+    for (j = 0 ; j < n ; j++)
+    {
+        x [j] /= Lx [Lp [j]] ;
+        for (p = Lp [j]+1 ; p < Lp [j+1] ; p++)
+        {
+            x [Li [p]] -= Lx [p] * x [j] ;
+        }
+    }
+    return (1) ;
+}

workspace/slam_in_autonomous_driving/thirdparty/g2o/EXTERNAL/csparse/cs_ltsolve.c

@@ -0,0 +1,18 @@
+#include "cs.h"
+/* solve L'x=b where x and b are dense.  x=b on input, solution on output. */
+csi cs_ltsolve (const cs *L, double *x)
+{
+    csi p, j, n, *Lp, *Li ;
+    double *Lx ;
+    if (!CS_CSC (L) || !x) return (0) ;                     /* check inputs */
+    n = L->n ; Lp = L->p ; Li = L->i ; Lx = L->x ;
+    for (j = n-1 ; j >= 0 ; j--)
+    {
+        for (p = Lp [j]+1 ; p < Lp [j+1] ; p++)
+        {
+            x [j] -= Lx [p] * x [Li [p]] ;
+        }
+        x [j] /= Lx [Lp [j]] ;
+    }
+    return (1) ;
+}

workspace/slam_in_autonomous_driving/thirdparty/g2o/EXTERNAL/csparse/cs_lu.c

@@ -0,0 +1,86 @@
+#include "cs.h"
+/* [L,U,pinv]=lu(A, [q lnz unz]). lnz and unz can be guess */
+csn *cs_lu (const cs *A, const css *S, double tol)
+{
+    cs *L, *U ;
+    csn *N ;
+    double pivot, *Lx, *Ux, *x,  a, t ;
+    csi *Lp, *Li, *Up, *Ui, *pinv, *xi, *q, n, ipiv, k, top, p, i, col, lnz,unz;
+    if (!CS_CSC (A) || !S) return (NULL) ;          /* check inputs */
+    n = A->n ;
+    q = S->q ; lnz = S->lnz ; unz = S->unz ;
+    x = cs_malloc (n, sizeof (double)) ;            /* get double workspace */
+    xi = cs_malloc (2*n, sizeof (csi)) ;            /* get csi workspace */
+    N = cs_calloc (1, sizeof (csn)) ;               /* allocate result */
+    if (!x || !xi || !N) return (cs_ndone (N, NULL, xi, x, 0)) ;
+    N->L = L = cs_spalloc (n, n, lnz, 1, 0) ;       /* allocate result L */
+    N->U = U = cs_spalloc (n, n, unz, 1, 0) ;       /* allocate result U */
+    N->pinv = pinv = cs_malloc (n, sizeof (csi)) ;  /* allocate result pinv */
+    if (!L || !U || !pinv) return (cs_ndone (N, NULL, xi, x, 0)) ;
+    Lp = L->p ; Up = U->p ;
+    for (i = 0 ; i < n ; i++) x [i] = 0 ;           /* clear workspace */
+    for (i = 0 ; i < n ; i++) pinv [i] = -1 ;       /* no rows pivotal yet */
+    for (k = 0 ; k <= n ; k++) Lp [k] = 0 ;         /* no cols of L yet */
+    lnz = unz = 0 ;
+    for (k = 0 ; k < n ; k++)       /* compute L(:,k) and U(:,k) */
+    {
+        /* --- Triangular solve --------------------------------------------- */
+        Lp [k] = lnz ;              /* L(:,k) starts here */
+        Up [k] = unz ;              /* U(:,k) starts here */
+        if ((lnz + n > L->nzmax && !cs_sprealloc (L, 2*L->nzmax + n)) ||
+            (unz + n > U->nzmax && !cs_sprealloc (U, 2*U->nzmax + n)))
+        {
+            return (cs_ndone (N, NULL, xi, x, 0)) ;
+        }
+        Li = L->i ; Lx = L->x ; Ui = U->i ; Ux = U->x ;
+        col = q ? (q [k]) : k ;
+        top = cs_spsolve (L, A, col, xi, x, pinv, 1) ;  /* x = L\A(:,col) */
+        /* --- Find pivot --------------------------------------------------- */
+        ipiv = -1 ;
+        a = -1 ;
+        for (p = top ; p < n ; p++)
+        {
+            i = xi [p] ;            /* x(i) is nonzero */
+            if (pinv [i] < 0)       /* row i is not yet pivotal */
+            {
+                if ((t = fabs (x [i])) > a)
+                {
+                    a = t ;         /* largest pivot candidate so far */
+                    ipiv = i ;
+                }
+            }
+            else                    /* x(i) is the entry U(pinv[i],k) */
+            {
+                Ui [unz] = pinv [i] ;
+                Ux [unz++] = x [i] ;
+            }
+        }
+        if (ipiv == -1 || a <= 0) return (cs_ndone (N, NULL, xi, x, 0)) ;
+        if (pinv [col] < 0 && fabs (x [col]) >= a*tol) ipiv = col ;
+        /* --- Divide by pivot ---------------------------------------------- */
+        pivot = x [ipiv] ;          /* the chosen pivot */
+        Ui [unz] = k ;              /* last entry in U(:,k) is U(k,k) */
+        Ux [unz++] = pivot ;
+        pinv [ipiv] = k ;           /* ipiv is the kth pivot row */
+        Li [lnz] = ipiv ;           /* first entry in L(:,k) is L(k,k) = 1 */
+        Lx [lnz++] = 1 ;
+        for (p = top ; p < n ; p++) /* L(k+1:n,k) = x / pivot */
+        {
+            i = xi [p] ;
+            if (pinv [i] < 0)       /* x(i) is an entry in L(:,k) */
+            {
+                Li [lnz] = i ;      /* save unpermuted row in L */
+                Lx [lnz++] = x [i] / pivot ;    /* scale pivot column */
+            }
+            x [i] = 0 ;             /* x [0..n-1] = 0 for next k */
+        }
+    }
+    /* --- Finalize L and U ------------------------------------------------- */
+    Lp [n] = lnz ;
+    Up [n] = unz ;
+    Li = L->i ;                     /* fix row indices of L for final pinv */
+    for (p = 0 ; p < lnz ; p++) Li [p] = pinv [Li [p]] ;
+    cs_sprealloc (L, 0) ;           /* remove extra space from L and U */
+    cs_sprealloc (U, 0) ;
+    return (cs_ndone (N, NULL, xi, x, 1)) ;     /* success */
+}

workspace/slam_in_autonomous_driving/thirdparty/g2o/EXTERNAL/csparse/cs_lusol.c

@@ -0,0 +1,26 @@
+#include "cs.h"
+/* x=A\b where A is unsymmetric; b overwritten with solution */
+csi cs_lusol (csi order, const cs *A, double *b, double tol)
+{
+    double *x ;
+    css *S ;
+    csn *N ;
+    csi n, ok ;
+    if (!CS_CSC (A) || !b) return (0) ;     /* check inputs */
+    n = A->n ;
+    S = cs_sqr (order, A, 0) ;              /* ordering and symbolic analysis */
+    N = cs_lu (A, S, tol) ;                 /* numeric LU factorization */
+    x = cs_malloc (n, sizeof (double)) ;    /* get workspace */
+    ok = (S && N && x) ;
+    if (ok)
+    {
+        cs_ipvec (N->pinv, b, x, n) ;       /* x = b(p) */
+        cs_lsolve (N->L, x) ;               /* x = L\x */
+        cs_usolve (N->U, x) ;               /* x = U\x */
+        cs_ipvec (S->q, x, b, n) ;          /* b(q) = x */
+    }
+    cs_free (x) ;
+    cs_sfree (S) ;
+    cs_nfree (N) ;
+    return (ok) ;
+}

workspace/slam_in_autonomous_driving/thirdparty/g2o/EXTERNAL/csparse/cs_malloc.c

@@ -0,0 +1,35 @@
+#include "cs.h"
+#ifdef MATLAB_MEX_FILE
+#define malloc mxMalloc
+#define free mxFree
+#define realloc mxRealloc
+#define calloc mxCalloc
+#endif
+
+/* wrapper for malloc */
+void *cs_malloc (csi n, size_t size)
+{
+    return (malloc (CS_MAX (n,1) * size)) ;
+}
+
+/* wrapper for calloc */
+void *cs_calloc (csi n, size_t size)
+{
+    return (calloc (CS_MAX (n,1), size)) ;
+}
+
+/* wrapper for free */
+void *cs_free (void *p)
+{
+    if (p) free (p) ;       /* free p if it is not already NULL */
+    return (NULL) ;         /* return NULL to simplify the use of cs_free */
+}
+
+/* wrapper for realloc */
+void *cs_realloc (void *p, csi n, size_t size, csi *ok)
+{
+    void *pnew ;
+    pnew = realloc (p, CS_MAX (n,1) * size) ; /* realloc the block */
+    *ok = (pnew != NULL) ;                  /* realloc fails if pnew is NULL */
+    return ((*ok) ? pnew : p) ;             /* return original p if failure */
+}

workspace/slam_in_autonomous_driving/thirdparty/g2o/EXTERNAL/csparse/cs_maxtrans.c

@@ -0,0 +1,92 @@
+#include "cs.h"
+/* find an augmenting path starting at column k and extend the match if found */
+static void cs_augment (csi k, const cs *A, csi *jmatch, csi *cheap, csi *w,
+        csi *js, csi *is, csi *ps)
+{
+    csi found = 0, p, i = -1, *Ap = A->p, *Ai = A->i, head = 0, j ;
+    js [0] = k ;                        /* start with just node k in jstack */
+    while (head >= 0)
+    {
+        /* --- Start (or continue) depth-first-search at node j ------------- */
+        j = js [head] ;                 /* get j from top of jstack */
+        if (w [j] != k)                 /* 1st time j visited for kth path */
+        {
+            w [j] = k ;                 /* mark j as visited for kth path */
+            for (p = cheap [j] ; p < Ap [j+1] && !found ; p++)
+            {
+                i = Ai [p] ;            /* try a cheap assignment (i,j) */
+                found = (jmatch [i] == -1) ;
+            }
+            cheap [j] = p ;             /* start here next time j is traversed*/
+            if (found)
+            {
+                is [head] = i ;         /* column j matched with row i */
+                break ;                 /* end of augmenting path */
+            }
+            ps [head] = Ap [j] ;        /* no cheap match: start dfs for j */
+        }
+        /* --- Depth-first-search of neighbors of j ------------------------- */
+        for (p = ps [head] ; p < Ap [j+1] ; p++)
+        {
+            i = Ai [p] ;                /* consider row i */
+            if (w [jmatch [i]] == k) continue ; /* skip jmatch [i] if marked */
+            ps [head] = p + 1 ;         /* pause dfs of node j */
+            is [head] = i ;             /* i will be matched with j if found */
+            js [++head] = jmatch [i] ;  /* start dfs at column jmatch [i] */
+            break ;
+        }
+        if (p == Ap [j+1]) head-- ;     /* node j is done; pop from stack */
+    }                                   /* augment the match if path found: */
+    if (found) for (p = head ; p >= 0 ; p--) jmatch [is [p]] = js [p] ;
+}
+
+/* find a maximum transveral */
+csi *cs_maxtrans (const cs *A, csi seed)  /*[jmatch [0..m-1]; imatch [0..n-1]]*/
+{
+    csi i, j, k, n, m, p, n2 = 0, m2 = 0, *Ap, *jimatch, *w, *cheap, *js, *is,
+        *ps, *Ai, *Cp, *jmatch, *imatch, *q ;
+    cs *C ;
+    if (!CS_CSC (A)) return (NULL) ;                /* check inputs */
+    n = A->n ; m = A->m ; Ap = A->p ; Ai = A->i ;
+    w = jimatch = cs_calloc (m+n, sizeof (csi)) ;   /* allocate result */
+    if (!jimatch) return (NULL) ;
+    for (k = 0, j = 0 ; j < n ; j++)    /* count nonempty rows and columns */
+    {
+        n2 += (Ap [j] < Ap [j+1]) ;
+        for (p = Ap [j] ; p < Ap [j+1] ; p++)
+        {
+            w [Ai [p]] = 1 ;
+            k += (j == Ai [p]) ;        /* count entries already on diagonal */
+        }
+    }
+    if (k == CS_MIN (m,n))              /* quick return if diagonal zero-free */
+    {
+        jmatch = jimatch ; imatch = jimatch + m ;
+        for (i = 0 ; i < k ; i++) jmatch [i] = i ;
+        for (      ; i < m ; i++) jmatch [i] = -1 ;
+        for (j = 0 ; j < k ; j++) imatch [j] = j ;
+        for (      ; j < n ; j++) imatch [j] = -1 ;
+        return (cs_idone (jimatch, NULL, NULL, 1)) ;
+    }
+    for (i = 0 ; i < m ; i++) m2 += w [i] ;
+    C = (m2 < n2) ? cs_transpose (A,0) : ((cs *) A) ; /* transpose if needed */
+    if (!C) return (cs_idone (jimatch, (m2 < n2) ? C : NULL, NULL, 0)) ;
+    n = C->n ; m = C->m ; Cp = C->p ;
+    jmatch = (m2 < n2) ? jimatch + n : jimatch ;
+    imatch = (m2 < n2) ? jimatch : jimatch + m ;
+    w = cs_malloc (5*n, sizeof (csi)) ;             /* get workspace */
+    if (!w) return (cs_idone (jimatch, (m2 < n2) ? C : NULL, w, 0)) ;
+    cheap = w + n ; js = w + 2*n ; is = w + 3*n ; ps = w + 4*n ;
+    for (j = 0 ; j < n ; j++) cheap [j] = Cp [j] ;  /* for cheap assignment */
+    for (j = 0 ; j < n ; j++) w [j] = -1 ;          /* all columns unflagged */
+    for (i = 0 ; i < m ; i++) jmatch [i] = -1 ;     /* nothing matched yet */
+    q = cs_randperm (n, seed) ;                     /* q = random permutation */
+    for (k = 0 ; k < n ; k++)   /* augment, starting at column q[k] */
+    {
+        cs_augment (q ? q [k]: k, C, jmatch, cheap, w, js, is, ps) ;
+    }
+    cs_free (q) ;
+    for (j = 0 ; j < n ; j++) imatch [j] = -1 ;     /* find row match */
+    for (i = 0 ; i < m ; i++) if (jmatch [i] >= 0) imatch [jmatch [i]] = i ;
+    return (cs_idone (jimatch, (m2 < n2) ? C : NULL, w, 1)) ;
+}

workspace/slam_in_autonomous_driving/thirdparty/g2o/EXTERNAL/csparse/cs_multiply.c

@@ -0,0 +1,35 @@
+#include "cs.h"
+/* C = A*B */
+cs *cs_multiply (const cs *A, const cs *B)
+{
+    csi p, j, nz = 0, anz, *Cp, *Ci, *Bp, m, n, bnz, *w, values, *Bi ;
+    double *x, *Bx, *Cx ;
+    cs *C ;
+    if (!CS_CSC (A) || !CS_CSC (B)) return (NULL) ;      /* check inputs */
+    if (A->n != B->m) return (NULL) ;
+    m = A->m ; anz = A->p [A->n] ;
+    n = B->n ; Bp = B->p ; Bi = B->i ; Bx = B->x ; bnz = Bp [n] ;
+    w = cs_calloc (m, sizeof (csi)) ;                    /* get workspace */
+    values = (A->x != NULL) && (Bx != NULL) ;
+    x = values ? cs_malloc (m, sizeof (double)) : NULL ; /* get workspace */
+    C = cs_spalloc (m, n, anz + bnz, values, 0) ;        /* allocate result */
+    if (!C || !w || (values && !x)) return (cs_done (C, w, x, 0)) ;
+    Cp = C->p ;
+    for (j = 0 ; j < n ; j++)
+    {
+        if (nz + m > C->nzmax && !cs_sprealloc (C, 2*(C->nzmax)+m))
+        {
+            return (cs_done (C, w, x, 0)) ;             /* out of memory */
+        } 
+        Ci = C->i ; Cx = C->x ;         /* C->i and C->x may be reallocated */
+        Cp [j] = nz ;                   /* column j of C starts here */
+        for (p = Bp [j] ; p < Bp [j+1] ; p++)
+        {
+            nz = cs_scatter (A, Bi [p], Bx ? Bx [p] : 1, w, x, j+1, C, nz) ;
+        }
+        if (values) for (p = Cp [j] ; p < nz ; p++) Cx [p] = x [Ci [p]] ;
+    }
+    Cp [n] = nz ;                       /* finalize the last column of C */
+    cs_sprealloc (C, 0) ;               /* remove extra space from C */
+    return (cs_done (C, w, x, 1)) ;     /* success; free workspace, return C */
+}

workspace/slam_in_autonomous_driving/thirdparty/g2o/EXTERNAL/csparse/cs_norm.c

@@ -0,0 +1,15 @@
+#include "cs.h"
+/* 1-norm of a sparse matrix = max (sum (abs (A))), largest column sum */
+double cs_norm (const cs *A)
+{
+    csi p, j, n, *Ap ;
+    double *Ax,  norm = 0, s ;
+    if (!CS_CSC (A) || !A->x) return (-1) ;             /* check inputs */
+    n = A->n ; Ap = A->p ; Ax = A->x ;
+    for (j = 0 ; j < n ; j++)
+    {
+        for (s = 0, p = Ap [j] ; p < Ap [j+1] ; p++) s += fabs (Ax [p]) ;
+        norm = CS_MAX (norm, s) ;
+    }
+    return (norm) ;
+}

workspace/slam_in_autonomous_driving/thirdparty/g2o/EXTERNAL/csparse/cs_permute.c

@@ -0,0 +1,25 @@
+#include "cs.h"
+/* C = A(p,q) where p and q are permutations of 0..m-1 and 0..n-1. */
+cs *cs_permute (const cs *A, const csi *pinv, const csi *q, csi values)
+{
+    csi t, j, k, nz = 0, m, n, *Ap, *Ai, *Cp, *Ci ;
+    double *Cx, *Ax ;
+    cs *C ;
+    if (!CS_CSC (A)) return (NULL) ;    /* check inputs */
+    m = A->m ; n = A->n ; Ap = A->p ; Ai = A->i ; Ax = A->x ;
+    C = cs_spalloc (m, n, Ap [n], values && Ax != NULL, 0) ;  /* alloc result */
+    if (!C) return (cs_done (C, NULL, NULL, 0)) ;   /* out of memory */
+    Cp = C->p ; Ci = C->i ; Cx = C->x ;
+    for (k = 0 ; k < n ; k++)
+    {
+        Cp [k] = nz ;                   /* column k of C is column q[k] of A */
+        j = q ? (q [k]) : k ;
+        for (t = Ap [j] ; t < Ap [j+1] ; t++)
+        {
+            if (Cx) Cx [nz] = Ax [t] ;  /* row i of A is row pinv[i] of C */
+            Ci [nz++] = pinv ? (pinv [Ai [t]]) : Ai [t] ;
+        }
+    }
+    Cp [n] = nz ;                       /* finalize the last column of C */
+    return (cs_done (C, NULL, NULL, 1)) ;
+}

workspace/slam_in_autonomous_driving/thirdparty/g2o/EXTERNAL/csparse/cs_pinv.c

@@ -0,0 +1,11 @@
+#include "cs.h"
+/* pinv = p', or p = pinv' */
+csi *cs_pinv (csi const *p, csi n)
+{
+    csi k, *pinv ;
+    if (!p) return (NULL) ;                     /* p = NULL denotes identity */
+    pinv = cs_malloc (n, sizeof (csi)) ;        /* allocate result */
+    if (!pinv) return (NULL) ;                  /* out of memory */
+    for (k = 0 ; k < n ; k++) pinv [p [k]] = k ;/* invert the permutation */
+    return (pinv) ;                             /* return result */
+}

workspace/slam_in_autonomous_driving/thirdparty/g2o/EXTERNAL/csparse/cs_post.c

@@ -0,0 +1,24 @@
+#include "cs.h"
+/* post order a forest */
+csi *cs_post (const csi *parent, csi n)
+{
+    csi j, k = 0, *post, *w, *head, *next, *stack ;
+    if (!parent) return (NULL) ;                        /* check inputs */
+    post = cs_malloc (n, sizeof (csi)) ;                /* allocate result */
+    w = cs_malloc (3*n, sizeof (csi)) ;                 /* get workspace */
+    if (!w || !post) return (cs_idone (post, NULL, w, 0)) ;
+    head = w ; next = w + n ; stack = w + 2*n ;
+    for (j = 0 ; j < n ; j++) head [j] = -1 ;           /* empty linked lists */
+    for (j = n-1 ; j >= 0 ; j--)            /* traverse nodes in reverse order*/
+    {
+        if (parent [j] == -1) continue ;    /* j is a root */
+        next [j] = head [parent [j]] ;      /* add j to list of its parent */
+        head [parent [j]] = j ;
+    }
+    for (j = 0 ; j < n ; j++)
+    {
+        if (parent [j] != -1) continue ;    /* skip j if it is not a root */
+        k = cs_tdfs (j, k, head, next, post, stack) ;
+    }
+    return (cs_idone (post, NULL, w, 1)) ;  /* success; free w, return post */
+}

workspace/slam_in_autonomous_driving/thirdparty/g2o/EXTERNAL/csparse/cs_print.c

@@ -0,0 +1,39 @@
+#include "cs.h"
+/* print a sparse matrix; use %g for integers to avoid differences with csi */
+csi cs_print (const cs *A, csi brief)
+{
+    csi p, j, m, n, nzmax, nz, *Ap, *Ai ;
+    double *Ax ;
+    if (!A) { printf ("(null)\n") ; return (0) ; }
+    m = A->m ; n = A->n ; Ap = A->p ; Ai = A->i ; Ax = A->x ;
+    nzmax = A->nzmax ; nz = A->nz ;
+    printf ("CSparse Version %d.%d.%d, %s.  %s\n", CS_VER, CS_SUBVER,
+        CS_SUBSUB, CS_DATE, CS_COPYRIGHT) ;
+    if (nz < 0)
+    {
+        printf ("%g-by-%g, nzmax: %g nnz: %g, 1-norm: %g\n", (double) m,
+            (double) n, (double) nzmax, (double) (Ap [n]), cs_norm (A)) ;
+        for (j = 0 ; j < n ; j++)
+        {
+            printf ("    col %g : locations %g to %g\n", (double) j, 
+                (double) (Ap [j]), (double) (Ap [j+1]-1)) ;
+            for (p = Ap [j] ; p < Ap [j+1] ; p++)
+            {
+                printf ("      %g : %g\n", (double) (Ai [p]), Ax ? Ax [p] : 1) ;
+                if (brief && p > 20) { printf ("  ...\n") ; return (1) ; }
+            }
+        }
+    }
+    else
+    {
+        printf ("triplet: %g-by-%g, nzmax: %g nnz: %g\n", (double) m,
+            (double) n, (double) nzmax, (double) nz) ;
+        for (p = 0 ; p < nz ; p++)
+        {
+            printf ("    %g %g : %g\n", (double) (Ai [p]), (double) (Ap [p]),
+                Ax ? Ax [p] : 1) ;
+            if (brief && p > 20) { printf ("  ...\n") ; return (1) ; }
+        }
+    }
+    return (1) ;
+}

workspace/slam_in_autonomous_driving/thirdparty/g2o/EXTERNAL/csparse/cs_pvec.c

@@ -0,0 +1,9 @@
+#include "cs.h"
+/* x = b(p), for dense vectors x and b; p=NULL denotes identity */
+csi cs_pvec (const csi *p, const double *b, double *x, csi n)
+{
+    csi k ;
+    if (!x || !b) return (0) ;                              /* check inputs */
+    for (k = 0 ; k < n ; k++) x [k] = b [p ? p [k] : k] ;
+    return (1) ;
+}

workspace/slam_in_autonomous_driving/thirdparty/g2o/EXTERNAL/csparse/cs_qr.c

@@ -0,0 +1,73 @@
+#include "cs.h"
+/* sparse QR factorization [V,beta,pinv,R] = qr (A) */
+csn *cs_qr (const cs *A, const css *S)
+{
+    double *Rx, *Vx, *Ax, *x,  *Beta ;
+    csi i, k, p, m, n, vnz, p1, top, m2, len, col, rnz, *s, *leftmost, *Ap, *Ai,
+        *parent, *Rp, *Ri, *Vp, *Vi, *w, *pinv, *q ;
+    cs *R, *V ;
+    csn *N ;
+    if (!CS_CSC (A) || !S) return (NULL) ;
+    m = A->m ; n = A->n ; Ap = A->p ; Ai = A->i ; Ax = A->x ;
+    q = S->q ; parent = S->parent ; pinv = S->pinv ; m2 = S->m2 ;
+    vnz = S->lnz ; rnz = S->unz ; leftmost = S->leftmost ;
+    w = cs_malloc (m2+n, sizeof (csi)) ;            /* get csi workspace */
+    x = cs_malloc (m2, sizeof (double)) ;           /* get double workspace */
+    N = cs_calloc (1, sizeof (csn)) ;               /* allocate result */
+    if (!w || !x || !N) return (cs_ndone (N, NULL, w, x, 0)) ;
+    s = w + m2 ;                                    /* s is size n */
+    for (k = 0 ; k < m2 ; k++) x [k] = 0 ;          /* clear workspace x */
+    N->L = V = cs_spalloc (m2, n, vnz, 1, 0) ;      /* allocate result V */
+    N->U = R = cs_spalloc (m2, n, rnz, 1, 0) ;      /* allocate result R */
+    N->B = Beta = cs_malloc (n, sizeof (double)) ;  /* allocate result Beta */
+    if (!R || !V || !Beta) return (cs_ndone (N, NULL, w, x, 0)) ;
+    Rp = R->p ; Ri = R->i ; Rx = R->x ;
+    Vp = V->p ; Vi = V->i ; Vx = V->x ;
+    for (i = 0 ; i < m2 ; i++) w [i] = -1 ; /* clear w, to mark nodes */
+    rnz = 0 ; vnz = 0 ;
+    for (k = 0 ; k < n ; k++)               /* compute V and R */
+    {
+        Rp [k] = rnz ;                      /* R(:,k) starts here */
+        Vp [k] = p1 = vnz ;                 /* V(:,k) starts here */
+        w [k] = k ;                         /* add V(k,k) to pattern of V */
+        Vi [vnz++] = k ;
+        top = n ;
+        col = q ? q [k] : k ;
+        for (p = Ap [col] ; p < Ap [col+1] ; p++)   /* find R(:,k) pattern */
+        {
+            i = leftmost [Ai [p]] ;         /* i = min(find(A(i,q))) */
+            for (len = 0 ; w [i] != k ; i = parent [i]) /* traverse up to k */
+            {
+                s [len++] = i ;
+                w [i] = k ;
+            }
+            while (len > 0) s [--top] = s [--len] ; /* push path on stack */
+            i = pinv [Ai [p]] ;             /* i = permuted row of A(:,col) */
+            x [i] = Ax [p] ;                /* x (i) = A(:,col) */
+            if (i > k && w [i] < k)         /* pattern of V(:,k) = x (k+1:m) */
+            {
+                Vi [vnz++] = i ;            /* add i to pattern of V(:,k) */
+                w [i] = k ;
+            }
+        }
+        for (p = top ; p < n ; p++) /* for each i in pattern of R(:,k) */
+        {
+            i = s [p] ;                     /* R(i,k) is nonzero */
+            cs_happly (V, i, Beta [i], x) ; /* apply (V(i),Beta(i)) to x */
+            Ri [rnz] = i ;                  /* R(i,k) = x(i) */
+            Rx [rnz++] = x [i] ;
+            x [i] = 0 ;
+            if (parent [i] == k) vnz = cs_scatter (V, i, 0, w, NULL, k, V, vnz);
+        }
+        for (p = p1 ; p < vnz ; p++)        /* gather V(:,k) = x */
+        {
+            Vx [p] = x [Vi [p]] ;
+            x [Vi [p]] = 0 ;
+        }
+        Ri [rnz] = k ;                     /* R(k,k) = norm (x) */
+        Rx [rnz++] = cs_house (Vx+p1, Beta+k, vnz-p1) ; /* [v,beta]=house(x) */
+    }
+    Rp [n] = rnz ;                          /* finalize R */
+    Vp [n] = vnz ;                          /* finalize V */
+    return (cs_ndone (N, NULL, w, x, 1)) ;  /* success */
+}

workspace/slam_in_autonomous_driving/thirdparty/g2o/EXTERNAL/csparse/cs_qrsol.c

@@ -0,0 +1,53 @@
+#include "cs.h"
+/* x=A\b where A can be rectangular; b overwritten with solution */
+csi cs_qrsol (csi order, const cs *A, double *b)
+{
+    double *x ;
+    css *S ;
+    csn *N ;
+    cs *AT = NULL ;
+    csi k, m, n, ok ;
+    if (!CS_CSC (A) || !b) return (0) ; /* check inputs */
+    n = A->n ;
+    m = A->m ;
+    if (m >= n)
+    {
+        S = cs_sqr (order, A, 1) ;          /* ordering and symbolic analysis */
+        N = cs_qr (A, S) ;                  /* numeric QR factorization */
+        x = cs_calloc (S ? S->m2 : 1, sizeof (double)) ;    /* get workspace */
+        ok = (S && N && x) ;
+        if (ok)
+        {
+            cs_ipvec (S->pinv, b, x, m) ;   /* x(0:m-1) = b(p(0:m-1) */
+            for (k = 0 ; k < n ; k++)       /* apply Householder refl. to x */
+            {
+                cs_happly (N->L, k, N->B [k], x) ;
+            }
+            cs_usolve (N->U, x) ;           /* x = R\x */
+            cs_ipvec (S->q, x, b, n) ;      /* b(q(0:n-1)) = x(0:n-1) */
+        }
+    }
+    else
+    {
+        AT = cs_transpose (A, 1) ;          /* Ax=b is underdetermined */
+        S = cs_sqr (order, AT, 1) ;         /* ordering and symbolic analysis */
+        N = cs_qr (AT, S) ;                 /* numeric QR factorization of A' */
+        x = cs_calloc (S ? S->m2 : 1, sizeof (double)) ;    /* get workspace */
+        ok = (AT && S && N && x) ;
+        if (ok)
+        {
+            cs_pvec (S->q, b, x, m) ;       /* x(q(0:m-1)) = b(0:m-1) */
+            cs_utsolve (N->U, x) ;          /* x = R'\x */
+            for (k = m-1 ; k >= 0 ; k--)    /* apply Householder refl. to x */
+            {
+                cs_happly (N->L, k, N->B [k], x) ;
+            }
+            cs_pvec (S->pinv, x, b, n) ;    /* b(0:n-1) = x(p(0:n-1)) */
+        }
+    }
+    cs_free (x) ;
+    cs_sfree (S) ;
+    cs_nfree (N) ;
+    cs_spfree (AT) ;
+    return (ok) ;
+}

workspace/slam_in_autonomous_driving/thirdparty/g2o/EXTERNAL/csparse/cs_randperm.c

@@ -0,0 +1,22 @@
+#include "cs.h"
+/* return a random permutation vector, the identity perm, or p = n-1:-1:0.
+ * seed = -1 means p = n-1:-1:0.  seed = 0 means p = identity.  otherwise
+ * p = random permutation.  */
+csi *cs_randperm (csi n, csi seed)
+{
+    csi *p, k, j, t ;
+    if (seed == 0) return (NULL) ;      /* return p = NULL (identity) */
+    p = cs_malloc (n, sizeof (csi)) ;   /* allocate result */
+    if (!p) return (NULL) ;             /* out of memory */
+    for (k = 0 ; k < n ; k++) p [k] = n-k-1 ;
+    if (seed == -1) return (p) ;        /* return reverse permutation */
+    srand (seed) ;                      /* get new random number seed */
+    for (k = 0 ; k < n ; k++)
+    {
+        j = k + (rand ( ) % (n-k)) ;    /* j = rand integer in range k to n-1 */
+        t = p [j] ;                     /* swap p[k] and p[j] */
+        p [j] = p [k] ;
+        p [k] = t ;
+    }
+    return (p) ;
+}

workspace/slam_in_autonomous_driving/thirdparty/g2o/EXTERNAL/csparse/cs_reach.c

@@ -0,0 +1,19 @@
+#include "cs.h"
+/* xi [top...n-1] = nodes reachable from graph of G*P' via nodes in B(:,k).
+ * xi [n...2n-1] used as workspace */
+csi cs_reach (cs *G, const cs *B, csi k, csi *xi, const csi *pinv)
+{
+    csi p, n, top, *Bp, *Bi, *Gp ;
+    if (!CS_CSC (G) || !CS_CSC (B) || !xi) return (-1) ;    /* check inputs */
+    n = G->n ; Bp = B->p ; Bi = B->i ; Gp = G->p ;
+    top = n ;
+    for (p = Bp [k] ; p < Bp [k+1] ; p++)
+    {
+        if (!CS_MARKED (Gp, Bi [p]))    /* start a dfs at unmarked node i */
+        {
+            top = cs_dfs (Bi [p], G, top, xi, xi+n, pinv) ;
+        }
+    }
+    for (p = top ; p < n ; p++) CS_MARK (Gp, xi [p]) ;  /* restore G */
+    return (top) ;
+}

workspace/slam_in_autonomous_driving/thirdparty/g2o/EXTERNAL/csparse/cs_scatter.c

@@ -0,0 +1,22 @@
+#include "cs.h"
+/* x = x + beta * A(:,j), where x is a dense vector and A(:,j) is sparse */
+csi cs_scatter (const cs *A, csi j, double beta, csi *w, double *x, csi mark,
+    cs *C, csi nz)
+{
+    csi i, p, *Ap, *Ai, *Ci ;
+    double *Ax ;
+    if (!CS_CSC (A) || !w || !CS_CSC (C)) return (-1) ;     /* check inputs */
+    Ap = A->p ; Ai = A->i ; Ax = A->x ; Ci = C->i ;
+    for (p = Ap [j] ; p < Ap [j+1] ; p++)
+    {
+        i = Ai [p] ;                            /* A(i,j) is nonzero */
+        if (w [i] < mark)
+        {
+            w [i] = mark ;                      /* i is new entry in column j */
+            Ci [nz++] = i ;                     /* add i to pattern of C(:,j) */
+            if (x) x [i] = beta * Ax [p] ;      /* x(i) = beta*A(i,j) */
+        }
+        else if (x) x [i] += beta * Ax [p] ;    /* i exists in C(:,j) already */
+    }
+    return (nz) ;
+}

workspace/slam_in_autonomous_driving/thirdparty/g2o/EXTERNAL/csparse/cs_scc.c

@@ -0,0 +1,41 @@
+#include "cs.h"
+/* find the strongly connected components of a square matrix */
+csd *cs_scc (cs *A)     /* matrix A temporarily modified, then restored */
+{
+    csi n, i, k, b, nb = 0, top, *xi, *pstack, *p, *r, *Ap, *ATp, *rcopy, *Blk ;
+    cs *AT ;
+    csd *D ;
+    if (!CS_CSC (A)) return (NULL) ;                /* check inputs */
+    n = A->n ; Ap = A->p ;
+    D = cs_dalloc (n, 0) ;                          /* allocate result */
+    AT = cs_transpose (A, 0) ;                      /* AT = A' */
+    xi = cs_malloc (2*n+1, sizeof (csi)) ;          /* get workspace */
+    if (!D || !AT || !xi) return (cs_ddone (D, AT, xi, 0)) ;
+    Blk = xi ; rcopy = pstack = xi + n ;
+    p = D->p ; r = D->r ; ATp = AT->p ;
+    top = n ;
+    for (i = 0 ; i < n ; i++)   /* first dfs(A) to find finish times (xi) */
+    {
+        if (!CS_MARKED (Ap, i)) top = cs_dfs (i, A, top, xi, pstack, NULL) ;
+    }
+    for (i = 0 ; i < n ; i++) CS_MARK (Ap, i) ; /* restore A; unmark all nodes*/
+    top = n ;
+    nb = n ;
+    for (k = 0 ; k < n ; k++)   /* dfs(A') to find strongly connnected comp */
+    {
+        i = xi [k] ;            /* get i in reverse order of finish times */
+        if (CS_MARKED (ATp, i)) continue ;  /* skip node i if already ordered */
+        r [nb--] = top ;        /* node i is the start of a component in p */
+        top = cs_dfs (i, AT, top, p, pstack, NULL) ;
+    }
+    r [nb] = 0 ;                /* first block starts at zero; shift r up */
+    for (k = nb ; k <= n ; k++) r [k-nb] = r [k] ;
+    D->nb = nb = n-nb ;         /* nb = # of strongly connected components */
+    for (b = 0 ; b < nb ; b++)  /* sort each block in natural order */
+    {
+        for (k = r [b] ; k < r [b+1] ; k++) Blk [p [k]] = b ;
+    }
+    for (b = 0 ; b <= nb ; b++) rcopy [b] = r [b] ;
+    for (i = 0 ; i < n ; i++) p [rcopy [Blk [i]]++] = i ;
+    return (cs_ddone (D, AT, xi, 1)) ;
+}

workspace/slam_in_autonomous_driving/thirdparty/g2o/EXTERNAL/csparse/cs_schol.c

@@ -0,0 +1,26 @@
+#include "cs.h"
+/* ordering and symbolic analysis for a Cholesky factorization */
+css *cs_schol (csi order, const cs *A)
+{
+    csi n, *c, *post, *P ;
+    cs *C ;
+    css *S ;
+    if (!CS_CSC (A)) return (NULL) ;        /* check inputs */
+    n = A->n ;
+    S = cs_calloc (1, sizeof (css)) ;       /* allocate result S */
+    if (!S) return (NULL) ;                 /* out of memory */
+    P = cs_amd (order, A) ;                 /* P = amd(A+A'), or natural */
+    S->pinv = cs_pinv (P, n) ;              /* find inverse permutation */
+    cs_free (P) ;
+    if (order && !S->pinv) return (cs_sfree (S)) ;
+    C = cs_symperm (A, S->pinv, 0) ;        /* C = spones(triu(A(P,P))) */
+    S->parent = cs_etree (C, 0) ;           /* find etree of C */
+    post = cs_post (S->parent, n) ;         /* postorder the etree */
+    c = cs_counts (C, S->parent, post, 0) ; /* find column counts of chol(C) */
+    cs_free (post) ;
+    cs_spfree (C) ;
+    S->cp = cs_malloc (n+1, sizeof (csi)) ; /* allocate result S->cp */
+    S->unz = S->lnz = cs_cumsum (S->cp, c, n) ; /* find column pointers for L */
+    cs_free (c) ;
+    return ((S->lnz >= 0) ? S : cs_sfree (S)) ;
+}

workspace/slam_in_autonomous_driving/thirdparty/g2o/EXTERNAL/csparse/cs_spsolve.c

@@ -0,0 +1,28 @@
+#include "cs.h"
+/* solve Gx=b(:,k), where G is either upper (lo=0) or lower (lo=1) triangular */
+csi cs_spsolve (cs *G, const cs *B, csi k, csi *xi, double *x, const csi *pinv,
+    csi lo)
+{
+    csi j, J, p, q, px, top, n, *Gp, *Gi, *Bp, *Bi ;
+    double *Gx, *Bx ;
+    if (!CS_CSC (G) || !CS_CSC (B) || !xi || !x) return (-1) ;
+    Gp = G->p ; Gi = G->i ; Gx = G->x ; n = G->n ;
+    Bp = B->p ; Bi = B->i ; Bx = B->x ;
+    top = cs_reach (G, B, k, xi, pinv) ;        /* xi[top..n-1]=Reach(B(:,k)) */
+    for (p = top ; p < n ; p++) x [xi [p]] = 0 ;    /* clear x */
+    for (p = Bp [k] ; p < Bp [k+1] ; p++) x [Bi [p]] = Bx [p] ; /* scatter B */
+    for (px = top ; px < n ; px++)
+    {
+        j = xi [px] ;                               /* x(j) is nonzero */
+        J = pinv ? (pinv [j]) : j ;                 /* j maps to col J of G */
+        if (J < 0) continue ;                       /* column J is empty */
+        x [j] /= Gx [lo ? (Gp [J]) : (Gp [J+1]-1)] ;/* x(j) /= G(j,j) */
+        p = lo ? (Gp [J]+1) : (Gp [J]) ;            /* lo: L(j,j) 1st entry */
+        q = lo ? (Gp [J+1]) : (Gp [J+1]-1) ;        /* up: U(j,j) last entry */
+        for ( ; p < q ; p++)
+        {
+            x [Gi [p]] -= Gx [p] * x [j] ;          /* x(i) -= G(i,j) * x(j) */
+        }
+    }
+    return (top) ;                                  /* return top of stack */
+}

workspace/slam_in_autonomous_driving/thirdparty/g2o/EXTERNAL/csparse/cs_sqr.c

@@ -0,0 +1,88 @@
+#include "cs.h"
+/* compute nnz(V) = S->lnz, S->pinv, S->leftmost, S->m2 from A and S->parent */
+static csi cs_vcount (const cs *A, css *S)
+{
+    csi i, k, p, pa, n = A->n, m = A->m, *Ap = A->p, *Ai = A->i, *next, *head,
+        *tail, *nque, *pinv, *leftmost, *w, *parent = S->parent ;
+    S->pinv = pinv = cs_malloc (m+n, sizeof (csi)) ;        /* allocate pinv, */
+    S->leftmost = leftmost = cs_malloc (m, sizeof (csi)) ;  /* and leftmost */
+    w = cs_malloc (m+3*n, sizeof (csi)) ;   /* get workspace */
+    if (!pinv || !w || !leftmost)
+    {
+        cs_free (w) ;                       /* pinv and leftmost freed later */
+        return (0) ;                        /* out of memory */
+    }
+    next = w ; head = w + m ; tail = w + m + n ; nque = w + m + 2*n ;
+    for (k = 0 ; k < n ; k++) head [k] = -1 ;   /* queue k is empty */
+    for (k = 0 ; k < n ; k++) tail [k] = -1 ;
+    for (k = 0 ; k < n ; k++) nque [k] = 0 ;
+    for (i = 0 ; i < m ; i++) leftmost [i] = -1 ;
+    for (k = n-1 ; k >= 0 ; k--)
+    {
+        for (p = Ap [k] ; p < Ap [k+1] ; p++)
+        {
+            leftmost [Ai [p]] = k ;         /* leftmost[i] = min(find(A(i,:)))*/
+        }
+    }
+    for (i = m-1 ; i >= 0 ; i--)            /* scan rows in reverse order */
+    {
+        pinv [i] = -1 ;                     /* row i is not yet ordered */
+        k = leftmost [i] ;
+        if (k == -1) continue ;             /* row i is empty */
+        if (nque [k]++ == 0) tail [k] = i ; /* first row in queue k */
+        next [i] = head [k] ;               /* put i at head of queue k */
+        head [k] = i ;
+    }
+    S->lnz = 0 ;
+    S->m2 = m ;
+    for (k = 0 ; k < n ; k++)               /* find row permutation and nnz(V)*/
+    {
+        i = head [k] ;                      /* remove row i from queue k */
+        S->lnz++ ;                          /* count V(k,k) as nonzero */
+        if (i < 0) i = S->m2++ ;            /* add a fictitious row */
+        pinv [i] = k ;                      /* associate row i with V(:,k) */
+        if (--nque [k] <= 0) continue ;     /* skip if V(k+1:m,k) is empty */
+        S->lnz += nque [k] ;                /* nque [k] is nnz (V(k+1:m,k)) */
+        if ((pa = parent [k]) != -1)        /* move all rows to parent of k */
+        {
+            if (nque [pa] == 0) tail [pa] = tail [k] ;
+            next [tail [k]] = head [pa] ;
+            head [pa] = next [i] ;
+            nque [pa] += nque [k] ;
+        }
+    }
+    for (i = 0 ; i < m ; i++) if (pinv [i] < 0) pinv [i] = k++ ;
+    cs_free (w) ;
+    return (1) ;
+}
+
+/* symbolic ordering and analysis for QR or LU */
+css *cs_sqr (csi order, const cs *A, csi qr)
+{
+    csi n, k, ok = 1, *post ;
+    css *S ;
+    if (!CS_CSC (A)) return (NULL) ;        /* check inputs */
+    n = A->n ;
+    S = cs_calloc (1, sizeof (css)) ;       /* allocate result S */
+    if (!S) return (NULL) ;                 /* out of memory */
+    S->q = cs_amd (order, A) ;              /* fill-reducing ordering */
+    if (order && !S->q) return (cs_sfree (S)) ;
+    if (qr)                                 /* QR symbolic analysis */
+    {
+        cs *C = order ? cs_permute (A, NULL, S->q, 0) : ((cs *) A) ;
+        S->parent = cs_etree (C, 1) ;       /* etree of C'*C, where C=A(:,q) */
+        post = cs_post (S->parent, n) ;
+        S->cp = cs_counts (C, S->parent, post, 1) ;  /* col counts chol(C'*C) */
+        cs_free (post) ;
+        ok = C && S->parent && S->cp && cs_vcount (C, S) ;
+        if (ok) for (S->unz = 0, k = 0 ; k < n ; k++) S->unz += S->cp [k] ;
+        ok = ok && S->lnz >= 0 && S->unz >= 0 ;     /* csi overflow guard */
+        if (order) cs_spfree (C) ;
+    }
+    else
+    {
+        S->unz = 4*(A->p [n]) + n ;         /* for LU factorization only, */
+        S->lnz = S->unz ;                   /* guess nnz(L) and nnz(U) */
+    }
+    return (ok ? S : cs_sfree (S)) ;        /* return result S */
+}

workspace/slam_in_autonomous_driving/thirdparty/g2o/EXTERNAL/csparse/cs_symperm.c

@@ -0,0 +1,39 @@
+#include "cs.h"
+/* C = A(p,p) where A and C are symmetric the upper part stored; pinv not p */
+cs *cs_symperm (const cs *A, const csi *pinv, csi values)
+{
+    csi i, j, p, q, i2, j2, n, *Ap, *Ai, *Cp, *Ci, *w ;
+    double *Cx, *Ax ;
+    cs *C ;
+    if (!CS_CSC (A)) return (NULL) ;                    /* check inputs */
+    n = A->n ; Ap = A->p ; Ai = A->i ; Ax = A->x ;
+    C = cs_spalloc (n, n, Ap [n], values && (Ax != NULL), 0) ; /* alloc result*/
+    w = cs_calloc (n, sizeof (csi)) ;                   /* get workspace */
+    if (!C || !w) return (cs_done (C, w, NULL, 0)) ;    /* out of memory */
+    Cp = C->p ; Ci = C->i ; Cx = C->x ;
+    for (j = 0 ; j < n ; j++)           /* count entries in each column of C */
+    {
+        j2 = pinv ? pinv [j] : j ;      /* column j of A is column j2 of C */
+        for (p = Ap [j] ; p < Ap [j+1] ; p++)
+        {
+            i = Ai [p] ;
+            if (i > j) continue ;       /* skip lower triangular part of A */
+            i2 = pinv ? pinv [i] : i ;  /* row i of A is row i2 of C */
+            w [CS_MAX (i2, j2)]++ ;     /* column count of C */
+        }
+    }
+    cs_cumsum (Cp, w, n) ;              /* compute column pointers of C */
+    for (j = 0 ; j < n ; j++)
+    {
+        j2 = pinv ? pinv [j] : j ;      /* column j of A is column j2 of C */
+        for (p = Ap [j] ; p < Ap [j+1] ; p++)
+        {
+            i = Ai [p] ;
+            if (i > j) continue ;       /* skip lower triangular part of A*/
+            i2 = pinv ? pinv [i] : i ;  /* row i of A is row i2 of C */
+            Ci [q = w [CS_MAX (i2, j2)]++] = CS_MIN (i2, j2) ;
+            if (Cx) Cx [q] = Ax [p] ;
+        }
+    }
+    return (cs_done (C, w, NULL, 1)) ;  /* success; free workspace, return C */
+}

workspace/slam_in_autonomous_driving/thirdparty/g2o/EXTERNAL/csparse/cs_tdfs.c

@@ -0,0 +1,24 @@
+#include "cs.h"
+/* depth-first search and postorder of a tree rooted at node j */
+csi cs_tdfs (csi j, csi k, csi *head, const csi *next, csi *post, csi *stack)
+{
+    csi i, p, top = 0 ;
+    if (!head || !next || !post || !stack) return (-1) ;    /* check inputs */
+    stack [0] = j ;                 /* place j on the stack */
+    while (top >= 0)                /* while (stack is not empty) */
+    {
+        p = stack [top] ;           /* p = top of stack */
+        i = head [p] ;              /* i = youngest child of p */
+        if (i == -1)
+        {
+            top-- ;                 /* p has no unordered children left */
+            post [k++] = p ;        /* node p is the kth postordered node */
+        }
+        else
+        {
+            head [p] = next [i] ;   /* remove i from children of p */
+            stack [++top] = i ;     /* start dfs on child node i */
+        }
+    }
+    return (k) ;
+}

workspace/slam_in_autonomous_driving/thirdparty/g2o/EXTERNAL/csparse/cs_transpose.c

@@ -0,0 +1,25 @@
+#include "cs.h"
+/* C = A' */
+cs *cs_transpose (const cs *A, csi values)
+{
+    csi p, q, j, *Cp, *Ci, n, m, *Ap, *Ai, *w ;
+    double *Cx, *Ax ;
+    cs *C ;
+    if (!CS_CSC (A)) return (NULL) ;    /* check inputs */
+    m = A->m ; n = A->n ; Ap = A->p ; Ai = A->i ; Ax = A->x ;
+    C = cs_spalloc (n, m, Ap [n], values && Ax, 0) ;       /* allocate result */
+    w = cs_calloc (m, sizeof (csi)) ;                      /* get workspace */
+    if (!C || !w) return (cs_done (C, w, NULL, 0)) ;       /* out of memory */
+    Cp = C->p ; Ci = C->i ; Cx = C->x ;
+    for (p = 0 ; p < Ap [n] ; p++) w [Ai [p]]++ ;          /* row counts */
+    cs_cumsum (Cp, w, m) ;                                 /* row pointers */
+    for (j = 0 ; j < n ; j++)
+    {
+        for (p = Ap [j] ; p < Ap [j+1] ; p++)
+        {
+            Ci [q = w [Ai [p]]++] = j ; /* place A(i,j) as entry C(j,i) */
+            if (Cx) Cx [q] = Ax [p] ;
+        }
+    }
+    return (cs_done (C, w, NULL, 1)) ;  /* success; free w and return C */
+}

workspace/slam_in_autonomous_driving/thirdparty/g2o/EXTERNAL/csparse/cs_updown.c

@@ -0,0 +1,37 @@
+#include "cs.h"
+/* sparse Cholesky update/downdate, L*L' + sigma*w*w' (sigma = +1 or -1) */
+csi cs_updown (cs *L, csi sigma, const cs *C, const csi *parent)
+{
+    csi n, p, f, j, *Lp, *Li, *Cp, *Ci ;
+    double *Lx, *Cx, alpha, beta = 1, delta, gamma, w1, w2, *w, beta2 = 1 ;
+    if (!CS_CSC (L) || !CS_CSC (C) || !parent) return (0) ;  /* check inputs */
+    Lp = L->p ; Li = L->i ; Lx = L->x ; n = L->n ;
+    Cp = C->p ; Ci = C->i ; Cx = C->x ;
+    if ((p = Cp [0]) >= Cp [1]) return (1) ;        /* return if C empty */
+    w = cs_malloc (n, sizeof (double)) ;            /* get workspace */
+    if (!w) return (0) ;                            /* out of memory */
+    f = Ci [p] ;
+    for ( ; p < Cp [1] ; p++) f = CS_MIN (f, Ci [p]) ;  /* f = min (find (C)) */
+    for (j = f ; j != -1 ; j = parent [j]) w [j] = 0 ;  /* clear workspace w */
+    for (p = Cp [0] ; p < Cp [1] ; p++) w [Ci [p]] = Cx [p] ; /* w = C */
+    for (j = f ; j != -1 ; j = parent [j])          /* walk path f up to root */
+    {
+        p = Lp [j] ;
+        alpha = w [j] / Lx [p] ;                    /* alpha = w(j) / L(j,j) */
+        beta2 = beta*beta + sigma*alpha*alpha ;
+        if (beta2 <= 0) break ;                     /* not positive definite */
+        beta2 = sqrt (beta2) ;
+        delta = (sigma > 0) ? (beta / beta2) : (beta2 / beta) ;
+        gamma = sigma * alpha / (beta2 * beta) ;
+        Lx [p] = delta * Lx [p] + ((sigma > 0) ? (gamma * w [j]) : 0) ;
+        beta = beta2 ;
+        for (p++ ; p < Lp [j+1] ; p++)
+        {
+            w1 = w [Li [p]] ;
+            w [Li [p]] = w2 = w1 - alpha * Lx [p] ;
+            Lx [p] = delta * Lx [p] + gamma * ((sigma > 0) ? w1 : w2) ;
+        }
+    }
+    cs_free (w) ;
+    return (beta2 > 0) ;
+}

workspace/slam_in_autonomous_driving/thirdparty/g2o/EXTERNAL/csparse/cs_usolve.c

@@ -0,0 +1,18 @@
+#include "cs.h"
+/* solve Ux=b where x and b are dense.  x=b on input, solution on output. */
+csi cs_usolve (const cs *U, double *x)
+{
+    csi p, j, n, *Up, *Ui ;
+    double *Ux ;
+    if (!CS_CSC (U) || !x) return (0) ;                     /* check inputs */
+    n = U->n ; Up = U->p ; Ui = U->i ; Ux = U->x ;
+    for (j = n-1 ; j >= 0 ; j--)
+    {
+        x [j] /= Ux [Up [j+1]-1] ;
+        for (p = Up [j] ; p < Up [j+1]-1 ; p++)
+        {
+            x [Ui [p]] -= Ux [p] * x [j] ;
+        }
+    }
+    return (1) ;
+}

workspace/slam_in_autonomous_driving/thirdparty/g2o/EXTERNAL/csparse/cs_util.c

@@ -0,0 +1,119 @@
+#include "cs.h"
+/* allocate a sparse matrix (triplet form or compressed-column form) */
+cs *cs_spalloc (csi m, csi n, csi nzmax, csi values, csi triplet)
+{
+    cs *A = cs_calloc (1, sizeof (cs)) ;    /* allocate the cs struct */
+    if (!A) return (NULL) ;                 /* out of memory */
+    A->m = m ;                              /* define dimensions and nzmax */
+    A->n = n ;
+    A->nzmax = nzmax = CS_MAX (nzmax, 1) ;
+    A->nz = triplet ? 0 : -1 ;              /* allocate triplet or comp.col */
+    A->p = cs_malloc (triplet ? nzmax : n+1, sizeof (csi)) ;
+    A->i = cs_malloc (nzmax, sizeof (csi)) ;
+    A->x = values ? cs_malloc (nzmax, sizeof (double)) : NULL ;
+    return ((!A->p || !A->i || (values && !A->x)) ? cs_spfree (A) : A) ;
+}
+
+/* change the max # of entries sparse matrix */
+csi cs_sprealloc (cs *A, csi nzmax)
+{
+    csi ok, oki, okj = 1, okx = 1 ;
+    if (!A) return (0) ;
+    if (nzmax <= 0) nzmax = (CS_CSC (A)) ? (A->p [A->n]) : A->nz ;
+    A->i = cs_realloc (A->i, nzmax, sizeof (csi), &oki) ;
+    if (CS_TRIPLET (A)) A->p = cs_realloc (A->p, nzmax, sizeof (csi), &okj) ;
+    if (A->x) A->x = cs_realloc (A->x, nzmax, sizeof (double), &okx) ;
+    ok = (oki && okj && okx) ;
+    if (ok) A->nzmax = nzmax ;
+    return (ok) ;
+}
+
+/* free a sparse matrix */
+cs *cs_spfree (cs *A)
+{
+    if (!A) return (NULL) ;     /* do nothing if A already NULL */
+    cs_free (A->p) ;
+    cs_free (A->i) ;
+    cs_free (A->x) ;
+    return ((cs *) cs_free (A)) ;   /* free the cs struct and return NULL */
+}
+
+/* free a numeric factorization */
+csn *cs_nfree (csn *N)
+{
+    if (!N) return (NULL) ;     /* do nothing if N already NULL */
+    cs_spfree (N->L) ;
+    cs_spfree (N->U) ;
+    cs_free (N->pinv) ;
+    cs_free (N->B) ;
+    return ((csn *) cs_free (N)) ;  /* free the csn struct and return NULL */
+}
+
+/* free a symbolic factorization */
+css *cs_sfree (css *S)
+{
+    if (!S) return (NULL) ;     /* do nothing if S already NULL */
+    cs_free (S->pinv) ;
+    cs_free (S->q) ;
+    cs_free (S->parent) ;
+    cs_free (S->cp) ;
+    cs_free (S->leftmost) ;
+    return ((css *) cs_free (S)) ;  /* free the css struct and return NULL */
+}
+
+/* allocate a cs_dmperm or cs_scc result */
+csd *cs_dalloc (csi m, csi n)
+{
+    csd *D ;
+    D = cs_calloc (1, sizeof (csd)) ;
+    if (!D) return (NULL) ;
+    D->p = cs_malloc (m, sizeof (csi)) ;
+    D->r = cs_malloc (m+6, sizeof (csi)) ;
+    D->q = cs_malloc (n, sizeof (csi)) ;
+    D->s = cs_malloc (n+6, sizeof (csi)) ;
+    return ((!D->p || !D->r || !D->q || !D->s) ? cs_dfree (D) : D) ;
+}
+
+/* free a cs_dmperm or cs_scc result */
+csd *cs_dfree (csd *D)
+{
+    if (!D) return (NULL) ;     /* do nothing if D already NULL */
+    cs_free (D->p) ;
+    cs_free (D->q) ;
+    cs_free (D->r) ;
+    cs_free (D->s) ;
+    return ((csd *) cs_free (D)) ;  /* free the csd struct and return NULL */
+}
+
+/* free workspace and return a sparse matrix result */
+cs *cs_done (cs *C, void *w, void *x, csi ok)
+{
+    cs_free (w) ;                       /* free workspace */
+    cs_free (x) ;
+    return (ok ? C : cs_spfree (C)) ;   /* return result if OK, else free it */
+}
+
+/* free workspace and return csi array result */
+csi *cs_idone (csi *p, cs *C, void *w, csi ok)
+{
+    cs_spfree (C) ;                     /* free temporary matrix */
+    cs_free (w) ;                       /* free workspace */
+    return (ok ? p : (csi *) cs_free (p)) ; /* return result, or free it */
+}
+
+/* free workspace and return a numeric factorization (Cholesky, LU, or QR) */
+csn *cs_ndone (csn *N, cs *C, void *w, void *x, csi ok)
+{
+    cs_spfree (C) ;                     /* free temporary matrix */
+    cs_free (w) ;                       /* free workspace */
+    cs_free (x) ;
+    return (ok ? N : cs_nfree (N)) ;    /* return result if OK, else free it */
+}
+
+/* free workspace and return a csd result */
+csd *cs_ddone (csd *D, cs *C, void *w, csi ok)
+{
+    cs_spfree (C) ;                     /* free temporary matrix */
+    cs_free (w) ;                       /* free workspace */
+    return (ok ? D : cs_dfree (D)) ;    /* return result if OK, else free it */
+}

workspace/slam_in_autonomous_driving/thirdparty/g2o/EXTERNAL/csparse/cs_utsolve.c

@@ -0,0 +1,18 @@
+#include "cs.h"
+/* solve U'x=b where x and b are dense.  x=b on input, solution on output. */
+csi cs_utsolve (const cs *U, double *x)
+{
+    csi p, j, n, *Up, *Ui ;
+    double *Ux ;
+    if (!CS_CSC (U) || !x) return (0) ;                     /* check inputs */
+    n = U->n ; Up = U->p ; Ui = U->i ; Ux = U->x ;
+    for (j = 0 ; j < n ; j++)
+    {
+        for (p = Up [j] ; p < Up [j+1]-1 ; p++)
+        {
+            x [j] -= Ux [p] * x [Ui [p]] ;
+        }
+        x [j] /= Ux [Up [j+1]-1] ;
+    }
+    return (1) ;
+}

workspace/slam_in_autonomous_driving/thirdparty/g2o/EXTERNAL/csparse/lesser.txt

@@ -0,0 +1,504 @@
+                  GNU LESSER GENERAL PUBLIC LICENSE
+                       Version 2.1, February 1999
+
+ Copyright (C) 1991, 1999 Free Software Foundation, Inc.
+     51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+ Everyone is permitted to copy and distribute verbatim copies
+ of this license document, but changing it is not allowed.
+
+[This is the first released version of the Lesser GPL.  It also counts
+ as the successor of the GNU Library Public License, version 2, hence
+ the version number 2.1.]
+
+                            Preamble
+
+  The licenses for most software are designed to take away your
+freedom to share and change it.  By contrast, the GNU General Public
+Licenses are intended to guarantee your freedom to share and change
+free software--to make sure the software is free for all its users.
+
+  This license, the Lesser General Public License, applies to some
+specially designated software packages--typically libraries--of the
+Free Software Foundation and other authors who decide to use it.  You
+can use it too, but we suggest you first think carefully about whether
+this license or the ordinary General Public License is the better
+strategy to use in any particular case, based on the explanations below.
+
+  When we speak of free software, we are referring to freedom of use,
+not price.  Our General Public Licenses are designed to make sure that
+you have the freedom to distribute copies of free software (and charge
+for this service if you wish); that you receive source code or can get
+it if you want it; that you can change the software and use pieces of
+it in new free programs; and that you are informed that you can do
+these things.
+
+  To protect your rights, we need to make restrictions that forbid
+distributors to deny you these rights or to ask you to surrender these
+rights.  These restrictions translate to certain responsibilities for
+you if you distribute copies of the library or if you modify it.
+
+  For example, if you distribute copies of the library, whether gratis
+or for a fee, you must give the recipients all the rights that we gave
+you.  You must make sure that they, too, receive or can get the source
+code.  If you link other code with the library, you must provide
+complete object files to the recipients, so that they can relink them
+with the library after making changes to the library and recompiling
+it.  And you must show them these terms so they know their rights.
+
+  We protect your rights with a two-step method: (1) we copyright the
+library, and (2) we offer you this license, which gives you legal
+permission to copy, distribute and/or modify the library.
+
+  To protect each distributor, we want to make it very clear that
+there is no warranty for the free library.  Also, if the library is
+modified by someone else and passed on, the recipients should know
+that what they have is not the original version, so that the original
+author's reputation will not be affected by problems that might be
+introduced by others.
+
+  Finally, software patents pose a constant threat to the existence of
+any free program.  We wish to make sure that a company cannot
+effectively restrict the users of a free program by obtaining a
+restrictive license from a patent holder.  Therefore, we insist that
+any patent license obtained for a version of the library must be
+consistent with the full freedom of use specified in this license.
+
+  Most GNU software, including some libraries, is covered by the
+ordinary GNU General Public License.  This license, the GNU Lesser
+General Public License, applies to certain designated libraries, and
+is quite different from the ordinary General Public License.  We use
+this license for certain libraries in order to permit linking those
+libraries into non-free programs.
+
+  When a program is linked with a library, whether statically or using
+a shared library, the combination of the two is legally speaking a
+combined work, a derivative of the original library.  The ordinary
+General Public License therefore permits such linking only if the
+entire combination fits its criteria of freedom.  The Lesser General
+Public License permits more lax criteria for linking other code with
+the library.
+
+  We call this license the "Lesser" General Public License because it
+does Less to protect the user's freedom than the ordinary General
+Public License.  It also provides other free software developers Less
+of an advantage over competing non-free programs.  These disadvantages
+are the reason we use the ordinary General Public License for many
+libraries.  However, the Lesser license provides advantages in certain
+special circumstances.
+
+  For example, on rare occasions, there may be a special need to
+encourage the widest possible use of a certain library, so that it becomes
+a de-facto standard.  To achieve this, non-free programs must be
+allowed to use the library.  A more frequent case is that a free
+library does the same job as widely used non-free libraries.  In this
+case, there is little to gain by limiting the free library to free
+software only, so we use the Lesser General Public License.
+
+  In other cases, permission to use a particular library in non-free
+programs enables a greater number of people to use a large body of
+free software.  For example, permission to use the GNU C Library in
+non-free programs enables many more people to use the whole GNU
+operating system, as well as its variant, the GNU/Linux operating
+system.
+
+  Although the Lesser General Public License is Less protective of the
+users' freedom, it does ensure that the user of a program that is
+linked with the Library has the freedom and the wherewithal to run
+that program using a modified version of the Library.
+
+  The precise terms and conditions for copying, distribution and
+modification follow.  Pay close attention to the difference between a
+"work based on the library" and a "work that uses the library".  The
+former contains code derived from the library, whereas the latter must
+be combined with the library in order to run.
+
+                  GNU LESSER GENERAL PUBLIC LICENSE
+   TERMS AND CONDITIONS FOR COPYING, DISTRIBUTION AND MODIFICATION
+
+  0. This License Agreement applies to any software library or other
+program which contains a notice placed by the copyright holder or
+other authorized party saying it may be distributed under the terms of
+this Lesser General Public License (also called "this License").
+Each licensee is addressed as "you".
+
+  A "library" means a collection of software functions and/or data
+prepared so as to be conveniently linked with application programs
+(which use some of those functions and data) to form executables.
+
+  The "Library", below, refers to any such software library or work
+which has been distributed under these terms.  A "work based on the
+Library" means either the Library or any derivative work under
+copyright law: that is to say, a work containing the Library or a
+portion of it, either verbatim or with modifications and/or translated
+straightforwardly into another language.  (Hereinafter, translation is
+included without limitation in the term "modification".)
+
+  "Source code" for a work means the preferred form of the work for
+making modifications to it.  For a library, complete source code means
+all the source code for all modules it contains, plus any associated
+interface definition files, plus the scripts used to control compilation
+and installation of the library.
+
+  Activities other than copying, distribution and modification are not
+covered by this License; they are outside its scope.  The act of
+running a program using the Library is not restricted, and output from
+such a program is covered only if its contents constitute a work based
+on the Library (independent of the use of the Library in a tool for
+writing it).  Whether that is true depends on what the Library does
+and what the program that uses the Library does.
+  
+  1. You may copy and distribute verbatim copies of the Library's
+complete source code as you receive it, in any medium, provided that
+you conspicuously and appropriately publish on each copy an
+appropriate copyright notice and disclaimer of warranty; keep intact
+all the notices that refer to this License and to the absence of any
+warranty; and distribute a copy of this License along with the
+Library.
+
+  You may charge a fee for the physical act of transferring a copy,
+and you may at your option offer warranty protection in exchange for a
+fee.
+
+  2. You may modify your copy or copies of the Library or any portion
+of it, thus forming a work based on the Library, and copy and
+distribute such modifications or work under the terms of Section 1
+above, provided that you also meet all of these conditions:
+
+    a) The modified work must itself be a software library.
+
+    b) You must cause the files modified to carry prominent notices
+    stating that you changed the files and the date of any change.
+
+    c) You must cause the whole of the work to be licensed at no
+    charge to all third parties under the terms of this License.
+
+    d) If a facility in the modified Library refers to a function or a
+    table of data to be supplied by an application program that uses
+    the facility, other than as an argument passed when the facility
+    is invoked, then you must make a good faith effort to ensure that,
+    in the event an application does not supply such function or
+    table, the facility still operates, and performs whatever part of
+    its purpose remains meaningful.
+
+    (For example, a function in a library to compute square roots has
+    a purpose that is entirely well-defined independent of the
+    application.  Therefore, Subsection 2d requires that any
+    application-supplied function or table used by this function must
+    be optional: if the application does not supply it, the square
+    root function must still compute square roots.)
+
+These requirements apply to the modified work as a whole.  If
+identifiable sections of that work are not derived from the Library,
+and can be reasonably considered independent and separate works in
+themselves, then this License, and its terms, do not apply to those
+sections when you distribute them as separate works.  But when you
+distribute the same sections as part of a whole which is a work based
+on the Library, the distribution of the whole must be on the terms of
+this License, whose permissions for other licensees extend to the
+entire whole, and thus to each and every part regardless of who wrote
+it.
+
+Thus, it is not the intent of this section to claim rights or contest
+your rights to work written entirely by you; rather, the intent is to
+exercise the right to control the distribution of derivative or
+collective works based on the Library.
+
+In addition, mere aggregation of another work not based on the Library
+with the Library (or with a work based on the Library) on a volume of
+a storage or distribution medium does not bring the other work under
+the scope of this License.
+
+  3. You may opt to apply the terms of the ordinary GNU General Public
+License instead of this License to a given copy of the Library.  To do
+this, you must alter all the notices that refer to this License, so
+that they refer to the ordinary GNU General Public License, version 2,
+instead of to this License.  (If a newer version than version 2 of the
+ordinary GNU General Public License has appeared, then you can specify
+that version instead if you wish.)  Do not make any other change in
+these notices.
+
+  Once this change is made in a given copy, it is irreversible for
+that copy, so the ordinary GNU General Public License applies to all
+subsequent copies and derivative works made from that copy.
+
+  This option is useful when you wish to copy part of the code of
+the Library into a program that is not a library.
+
+  4. You may copy and distribute the Library (or a portion or
+derivative of it, under Section 2) in object code or executable form
+under the terms of Sections 1 and 2 above provided that you accompany
+it with the complete corresponding machine-readable source code, which
+must be distributed under the terms of Sections 1 and 2 above on a
+medium customarily used for software interchange.
+
+  If distribution of object code is made by offering access to copy
+from a designated place, then offering equivalent access to copy the
+source code from the same place satisfies the requirement to
+distribute the source code, even though third parties are not
+compelled to copy the source along with the object code.
+
+  5. A program that contains no derivative of any portion of the
+Library, but is designed to work with the Library by being compiled or
+linked with it, is called a "work that uses the Library".  Such a
+work, in isolation, is not a derivative work of the Library, and
+therefore falls outside the scope of this License.
+
+  However, linking a "work that uses the Library" with the Library
+creates an executable that is a derivative of the Library (because it
+contains portions of the Library), rather than a "work that uses the
+library".  The executable is therefore covered by this License.
+Section 6 states terms for distribution of such executables.
+
+  When a "work that uses the Library" uses material from a header file
+that is part of the Library, the object code for the work may be a
+derivative work of the Library even though the source code is not.
+Whether this is true is especially significant if the work can be
+linked without the Library, or if the work is itself a library.  The
+threshold for this to be true is not precisely defined by law.
+
+  If such an object file uses only numerical parameters, data
+structure layouts and accessors, and small macros and small inline
+functions (ten lines or less in length), then the use of the object
+file is unrestricted, regardless of whether it is legally a derivative
+work.  (Executables containing this object code plus portions of the
+Library will still fall under Section 6.)
+
+  Otherwise, if the work is a derivative of the Library, you may
+distribute the object code for the work under the terms of Section 6.
+Any executables containing that work also fall under Section 6,
+whether or not they are linked directly with the Library itself.
+
+  6. As an exception to the Sections above, you may also combine or
+link a "work that uses the Library" with the Library to produce a
+work containing portions of the Library, and distribute that work
+under terms of your choice, provided that the terms permit
+modification of the work for the customer's own use and reverse
+engineering for debugging such modifications.
+
+  You must give prominent notice with each copy of the work that the
+Library is used in it and that the Library and its use are covered by
+this License.  You must supply a copy of this License.  If the work
+during execution displays copyright notices, you must include the
+copyright notice for the Library among them, as well as a reference
+directing the user to the copy of this License.  Also, you must do one
+of these things:
+
+    a) Accompany the work with the complete corresponding
+    machine-readable source code for the Library including whatever
+    changes were used in the work (which must be distributed under
+    Sections 1 and 2 above); and, if the work is an executable linked
+    with the Library, with the complete machine-readable "work that
+    uses the Library", as object code and/or source code, so that the
+    user can modify the Library and then relink to produce a modified
+    executable containing the modified Library.  (It is understood
+    that the user who changes the contents of definitions files in the
+    Library will not necessarily be able to recompile the application
+    to use the modified definitions.)
+
+    b) Use a suitable shared library mechanism for linking with the
+    Library.  A suitable mechanism is one that (1) uses at run time a
+    copy of the library already present on the user's computer system,
+    rather than copying library functions into the executable, and (2)
+    will operate properly with a modified version of the library, if
+    the user installs one, as long as the modified version is
+    interface-compatible with the version that the work was made with.
+
+    c) Accompany the work with a written offer, valid for at
+    least three years, to give the same user the materials
+    specified in Subsection 6a, above, for a charge no more
+    than the cost of performing this distribution.
+
+    d) If distribution of the work is made by offering access to copy
+    from a designated place, offer equivalent access to copy the above
+    specified materials from the same place.
+
+    e) Verify that the user has already received a copy of these
+    materials or that you have already sent this user a copy.
+
+  For an executable, the required form of the "work that uses the
+Library" must include any data and utility programs needed for
+reproducing the executable from it.  However, as a special exception,
+the materials to be distributed need not include anything that is
+normally distributed (in either source or binary form) with the major
+components (compiler, kernel, and so on) of the operating system on
+which the executable runs, unless that component itself accompanies
+the executable.
+
+  It may happen that this requirement contradicts the license
+restrictions of other proprietary libraries that do not normally
+accompany the operating system.  Such a contradiction means you cannot
+use both them and the Library together in an executable that you
+distribute.
+
+  7. You may place library facilities that are a work based on the
+Library side-by-side in a single library together with other library
+facilities not covered by this License, and distribute such a combined
+library, provided that the separate distribution of the work based on
+the Library and of the other library facilities is otherwise
+permitted, and provided that you do these two things:
+
+    a) Accompany the combined library with a copy of the same work
+    based on the Library, uncombined with any other library
+    facilities.  This must be distributed under the terms of the
+    Sections above.
+
+    b) Give prominent notice with the combined library of the fact
+    that part of it is a work based on the Library, and explaining
+    where to find the accompanying uncombined form of the same work.
+
+  8. You may not copy, modify, sublicense, link with, or distribute
+the Library except as expressly provided under this License.  Any
+attempt otherwise to copy, modify, sublicense, link with, or
+distribute the Library is void, and will automatically terminate your
+rights under this License.  However, parties who have received copies,
+or rights, from you under this License will not have their licenses
+terminated so long as such parties remain in full compliance.
+
+  9. You are not required to accept this License, since you have not
+signed it.  However, nothing else grants you permission to modify or
+distribute the Library or its derivative works.  These actions are
+prohibited by law if you do not accept this License.  Therefore, by
+modifying or distributing the Library (or any work based on the
+Library), you indicate your acceptance of this License to do so, and
+all its terms and conditions for copying, distributing or modifying
+the Library or works based on it.
+
+  10. Each time you redistribute the Library (or any work based on the
+Library), the recipient automatically receives a license from the
+original licensor to copy, distribute, link with or modify the Library
+subject to these terms and conditions.  You may not impose any further
+restrictions on the recipients' exercise of the rights granted herein.
+You are not responsible for enforcing compliance by third parties with
+this License.
+
+  11. If, as a consequence of a court judgment or allegation of patent
+infringement or for any other reason (not limited to patent issues),
+conditions are imposed on you (whether by court order, agreement or
+otherwise) that contradict the conditions of this License, they do not
+excuse you from the conditions of this License.  If you cannot
+distribute so as to satisfy simultaneously your obligations under this
+License and any other pertinent obligations, then as a consequence you
+may not distribute the Library at all.  For example, if a patent
+license would not permit royalty-free redistribution of the Library by
+all those who receive copies directly or indirectly through you, then
+the only way you could satisfy both it and this License would be to
+refrain entirely from distribution of the Library.
+
+If any portion of this section is held invalid or unenforceable under any
+particular circumstance, the balance of the section is intended to apply,
+and the section as a whole is intended to apply in other circumstances.
+
+It is not the purpose of this section to induce you to infringe any
+patents or other property right claims or to contest validity of any
+such claims; this section has the sole purpose of protecting the
+integrity of the free software distribution system which is
+implemented by public license practices.  Many people have made
+generous contributions to the wide range of software distributed
+through that system in reliance on consistent application of that
+system; it is up to the author/donor to decide if he or she is willing
+to distribute software through any other system and a licensee cannot
+impose that choice.
+
+This section is intended to make thoroughly clear what is believed to
+be a consequence of the rest of this License.
+
+  12. If the distribution and/or use of the Library is restricted in
+certain countries either by patents or by copyrighted interfaces, the
+original copyright holder who places the Library under this License may add
+an explicit geographical distribution limitation excluding those countries,
+so that distribution is permitted only in or among countries not thus
+excluded.  In such case, this License incorporates the limitation as if
+written in the body of this License.
+
+  13. The Free Software Foundation may publish revised and/or new
+versions of the Lesser General Public License from time to time.
+Such new versions will be similar in spirit to the present version,
+but may differ in detail to address new problems or concerns.
+
+Each version is given a distinguishing version number.  If the Library
+specifies a version number of this License which applies to it and
+"any later version", you have the option of following the terms and
+conditions either of that version or of any later version published by
+the Free Software Foundation.  If the Library does not specify a
+license version number, you may choose any version ever published by
+the Free Software Foundation.
+
+  14. If you wish to incorporate parts of the Library into other free
+programs whose distribution conditions are incompatible with these,
+write to the author to ask for permission.  For software which is
+copyrighted by the Free Software Foundation, write to the Free
+Software Foundation; we sometimes make exceptions for this.  Our
+decision will be guided by the two goals of preserving the free status
+of all derivatives of our free software and of promoting the sharing
+and reuse of software generally.
+
+                            NO WARRANTY
+
+  15. BECAUSE THE LIBRARY IS LICENSED FREE OF CHARGE, THERE IS NO
+WARRANTY FOR THE LIBRARY, TO THE EXTENT PERMITTED BY APPLICABLE LAW.
+EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT HOLDERS AND/OR
+OTHER PARTIES PROVIDE THE LIBRARY "AS IS" WITHOUT WARRANTY OF ANY
+KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+PURPOSE.  THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE
+LIBRARY IS WITH YOU.  SHOULD THE LIBRARY PROVE DEFECTIVE, YOU ASSUME
+THE COST OF ALL NECESSARY SERVICING, REPAIR OR CORRECTION.
+
+  16. IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN
+WRITING WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MAY MODIFY
+AND/OR REDISTRIBUTE THE LIBRARY AS PERMITTED ABOVE, BE LIABLE TO YOU
+FOR DAMAGES, INCLUDING ANY GENERAL, SPECIAL, INCIDENTAL OR
+CONSEQUENTIAL DAMAGES ARISING OUT OF THE USE OR INABILITY TO USE THE
+LIBRARY (INCLUDING BUT NOT LIMITED TO LOSS OF DATA OR DATA BEING
+RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD PARTIES OR A
+FAILURE OF THE LIBRARY TO OPERATE WITH ANY OTHER SOFTWARE), EVEN IF
+SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF SUCH
+DAMAGES.
+
+                     END OF TERMS AND CONDITIONS
+
+           How to Apply These Terms to Your New Libraries
+
+  If you develop a new library, and you want it to be of the greatest
+possible use to the public, we recommend making it free software that
+everyone can redistribute and change.  You can do so by permitting
+redistribution under these terms (or, alternatively, under the terms of the
+ordinary General Public License).
+
+  To apply these terms, attach the following notices to the library.  It is
+safest to attach them to the start of each source file to most effectively
+convey the exclusion of warranty; and each file should have at least the
+"copyright" line and a pointer to where the full notice is found.
+
+    <one line to give the library's name and a brief idea of what it does.>
+    Copyright (C) <year>  <name of author>
+
+    This library is free software; you can redistribute it and/or
+    modify it under the terms of the GNU Lesser General Public
+    License as published by the Free Software Foundation; either
+    version 2.1 of the License, or (at your option) any later version.
+
+    This library is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+    Lesser General Public License for more details.
+
+    You should have received a copy of the GNU Lesser General Public
+    License along with this library; if not, write to the Free Software
+    Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+
+Also add information on how to contact you by electronic and paper mail.
+
+You should also get your employer (if you work as a programmer) or your
+school, if any, to sign a "copyright disclaimer" for the library, if
+necessary.  Here is a sample; alter the names:
+
+  Yoyodyne, Inc., hereby disclaims all copyright interest in the
+  library `Frob' (a library for tweaking knobs) written by James Random Hacker.
+
+  <signature of Ty Coon>, 1 April 1990
+  Ty Coon, President of Vice
+
+That's all there is to it!
+
+

workspace/slam_in_autonomous_driving/thirdparty/g2o/EXTERNAL/freeglut/CMakeLists.txt

@@ -0,0 +1,23 @@
+add_library(freeglut_minimal ${G2O_LIB_TYPE}
+  freeglut_font.cpp
+  freeglut_stroke_mono_roman.cpp
+  freeglut_stroke_roman.cpp
+)
+
+target_link_libraries(freeglut_minimal PUBLIC ${G2O_OPENGL_TARGET} ${G2O_EIGEN3_EIGEN_TARGET})
+
+set_target_properties(freeglut_minimal PROPERTIES OUTPUT_NAME ${LIB_PREFIX}ext_freeglut_minimal)
+if (APPLE)
+  set_target_properties(freeglut_minimal PROPERTIES INSTALL_NAME_DIR "${CMAKE_INSTALL_PREFIX}/lib")
+endif()
+
+install(TARGETS freeglut_minimal
+  EXPORT ${G2O_TARGETS_EXPORT_NAME}
+  RUNTIME DESTINATION ${RUNTIME_DESTINATION}
+  LIBRARY DESTINATION ${LIBRARY_DESTINATION}
+  ARCHIVE DESTINATION ${ARCHIVE_DESTINATION}
+  INCLUDES DESTINATION ${INCLUDES_DESTINATION}
+)
+
+file(GLOB headers "${CMAKE_CURRENT_SOURCE_DIR}/*.h" "${CMAKE_CURRENT_SOURCE_DIR}/*.hpp")
+install(FILES ${headers} DESTINATION ${INCLUDES_INSTALL_DIR}/freeglut)

workspace/slam_in_autonomous_driving/thirdparty/g2o/EXTERNAL/freeglut/COPYING

@@ -0,0 +1,27 @@
+
+  Freeglut Copyright
+  ------------------
+  
+  Freeglut code without an explicit copyright is covered by the following 
+  copyright:
+  
+  Copyright (c) 1999-2000 Pawel W. Olszta. All Rights Reserved.
+  Permission is hereby granted, free of charge,  to any person obtaining a copy 
+  of this software and associated documentation files (the "Software"), to deal
+  in the Software without restriction,  including without limitation the rights 
+  to use, copy,  modify, merge,  publish, distribute,  sublicense,  and/or sell 
+  copies or substantial portions of the Software.
+  
+  The above  copyright notice  and this permission notice  shall be included in 
+  all copies or substantial portions of the Software.
+  
+  THE SOFTWARE  IS PROVIDED "AS IS",  WITHOUT WARRANTY OF ANY KIND,  EXPRESS OR 
+  IMPLIED,  INCLUDING  BUT  NOT LIMITED  TO THE WARRANTIES  OF MERCHANTABILITY, 
+  FITNESS  FOR  A PARTICULAR PURPOSE  AND NONINFRINGEMENT.  IN  NO EVENT  SHALL 
+  PAWEL W. OLSZTA BE LIABLE FOR ANY CLAIM,  DAMAGES OR OTHER LIABILITY, WHETHER 
+  IN  AN ACTION  OF CONTRACT,  TORT OR OTHERWISE,  ARISING FROM,  OUT OF  OR IN 
+  CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+  
+  Except as contained in this notice,  the name of Pawel W. Olszta shall not be 
+  used  in advertising  or otherwise to promote the sale, use or other dealings 
+  in this Software without prior written authorization from Pawel W. Olszta.

workspace/slam_in_autonomous_driving/thirdparty/g2o/EXTERNAL/freeglut/freeglut_font.cpp

@@ -0,0 +1,156 @@
+/*
+ * freeglut_font.c
+ *
+ * Bitmap and stroke fonts displaying.
+ *
+ * Copyright (c) 1999-2000 Pawel W. Olszta. All Rights Reserved.
+ * Written by Pawel W. Olszta, <olszta@sourceforge.net>
+ * Creation date: Thu Dec 16 1999
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included
+ * in all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
+ * OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * PAWEL W. OLSZTA BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER
+ * IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+ */
+
+#include "freeglut_minimal.h"
+#include <iostream>
+
+#define  freeglut_return_if_fail( expr ) \
+    if( !(expr) )                        \
+        return;
+#define  freeglut_return_val_if_fail( expr, val ) \
+    if( !(expr) )                                 \
+        return val ;
+
+namespace freeglut_minimal {
+
+/*
+ * Matches a font ID with a SFG_StrokeFont structure pointer.
+ * This was changed to match the GLUT header style.
+ */
+static SFG_StrokeFont* fghStrokeByID(FontID font )
+{
+    if( font == GLUT_STROKE_ROMAN      )
+        return (SFG_StrokeFont*)&fgStrokeRoman;
+    if( font == GLUT_STROKE_MONO_ROMAN )
+        return (SFG_StrokeFont*)&fgStrokeMonoRoman;
+
+    std::cerr << "stroke font " << (int)font << " not found" << std::endl;
+    return 0;
+}
+
+void glutStrokeString(FontID fontID, const char *string_)
+{
+    const unsigned char* string = reinterpret_cast<const unsigned char*>(string_);
+    unsigned char c;
+    int i, j;
+    float length = 0.0;
+    SFG_StrokeFont* font;
+    //FREEGLUT_EXIT_IF_NOT_INITIALISED ( "glutStrokeString" );
+    font = fghStrokeByID( fontID );
+    freeglut_return_if_fail( font );
+    if ( !string || ! *string )
+        return;
+
+    /*
+     * Step through the string, drawing each character.
+     * A newline will simply translate the next character's insertion
+     * point back to the start of the line and down one line.
+     */
+    while( ( c = *string++) )
+        if( c < font->Quantity )
+        {
+            if( c == '\n' )
+            {
+                glTranslatef ( -length, -( float )( font->Height ), 0.0 );
+                length = 0.0;
+            }
+            else  /* Not an EOL, draw the bitmap character */
+            {
+                const SFG_StrokeChar *schar = font->Characters[ c ];
+                if( schar )
+                {
+                    const SFG_StrokeStrip *strip = schar->Strips;
+
+                    for( i = 0; i < schar->Number; i++, strip++ )
+                    {
+                        glBegin( GL_LINE_STRIP );
+                        for( j = 0; j < strip->Number; j++ )
+                            glVertex2f( strip->Vertices[ j ].X,
+                                        strip->Vertices[ j ].Y);
+
+                        glEnd( );
+                    }
+
+                    length += schar->Right;
+                    glTranslatef( schar->Right, 0.0f, 0.0f );
+                }
+            }
+        }
+}
+
+/*
+ * Return the width of a string drawn using a stroke font
+ */
+int glutStrokeLength( FontID fontID, const char* string_ )
+{
+    const unsigned char* string = reinterpret_cast<const unsigned char*>(string_);
+    unsigned char c;
+    float length = 0.0;
+    float this_line_length = 0.0;
+    SFG_StrokeFont* font;
+    //FREEGLUT_EXIT_IF_NOT_INITIALISED ( "glutStrokeLength" );
+    font = fghStrokeByID( fontID );
+    freeglut_return_val_if_fail( font, 0 );
+    if ( !string || ! *string )
+        return 0;
+
+    while( ( c = *string++) )
+        if( c < font->Quantity )
+        {
+            if( c == '\n' ) /* EOL; reset the length of this line */
+            {
+                if( length < this_line_length )
+                    length = this_line_length;
+                this_line_length = 0.0;
+            }
+            else  /* Not an EOL, increment the length of this line */
+            {
+                const SFG_StrokeChar *schar = font->Characters[ c ];
+                if( schar )
+                    this_line_length += schar->Right;
+            }
+        }
+    if( length < this_line_length )
+        length = this_line_length;
+    return( int )( length + 0.5 );
+}
+
+/*
+ * Returns the height of a stroke font
+ */
+GLfloat glutStrokeHeight( FontID fontID )
+{
+    SFG_StrokeFont* font;
+    //FREEGLUT_EXIT_IF_NOT_INITIALISED ( "glutStrokeHeight" );
+    font = fghStrokeByID( fontID );
+    freeglut_return_val_if_fail( font, 0.0 );
+    return font->Height;
+}
+
+} // end namespace
+
+/*** END OF FILE ***/

workspace/slam_in_autonomous_driving/thirdparty/g2o/EXTERNAL/freeglut/freeglut_minimal.h

@@ -0,0 +1,107 @@
+#ifndef  __FREEGLUT_EXT_H__
+#define  __FREEGLUT_EXT_H__
+
+/*
+ * freeglut_ext.h
+ *
+ * The non-GLUT-compatible extensions to the freeglut library include file
+ *
+ * Copyright (c) 1999-2000 Pawel W. Olszta. All Rights Reserved.
+ * Written by Pawel W. Olszta, <olszta@sourceforge.net>
+ * Creation date: Thu Dec 2 1999
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included
+ * in all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
+ * OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * PAWEL W. OLSZTA BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER
+ * IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+ */
+
+#include "g2o/stuff/opengl_wrapper.h"
+
+#ifdef _MSC_VER
+// We are using a Microsoft compiler:
+#ifdef G2O_SHARED_LIBS
+#  ifdef freeglut_minimal_EXPORTS
+#    define G2O_FGAPI __declspec(dllexport)
+#  else
+#    define G2O_FGAPI __declspec(dllimport)
+#  endif
+#else
+#  define G2O_FGAPI
+#endif
+#else
+// Not Microsoft compiler so set empty definition:
+#  define G2O_FGAPI
+#endif
+
+namespace freeglut_minimal {
+
+  enum FontID {
+    GLUT_STROKE_ROMAN,
+    GLUT_STROKE_MONO_ROMAN
+  };
+
+  /* The stroke font structures */
+
+  typedef struct tagSFG_StrokeVertex SFG_StrokeVertex;
+  struct tagSFG_StrokeVertex
+  {
+    GLfloat         X, Y;
+  };
+
+  typedef struct tagSFG_StrokeStrip SFG_StrokeStrip;
+  struct tagSFG_StrokeStrip
+  {
+    int             Number;
+    const SFG_StrokeVertex* Vertices;
+  };
+
+  typedef struct tagSFG_StrokeChar SFG_StrokeChar;
+  struct tagSFG_StrokeChar
+  {
+    GLfloat         Right;
+    int             Number;
+    const SFG_StrokeStrip* Strips;
+  };
+
+  typedef struct tagSFG_StrokeFont SFG_StrokeFont;
+  struct tagSFG_StrokeFont
+  {
+    char*           Name;                       /* The source font name      */
+    int             Quantity;                   /* Number of chars in font   */
+    GLfloat         Height;                     /* Height of the characters  */
+    const SFG_StrokeChar** Characters;          /* The characters mapping    */
+  };
+
+  extern const SFG_StrokeFont fgStrokeRoman;
+  extern const SFG_StrokeFont fgStrokeMonoRoman;
+
+  G2O_FGAPI void glutStrokeString(FontID font, const char* string);
+
+  /*
+   * Font stuff, see freeglut_font.c
+   */
+  G2O_FGAPI GLfloat glutStrokeHeight(FontID font);
+
+  /*
+   * Return the width of a string drawn using a stroke font
+   */
+  G2O_FGAPI int glutStrokeLength(FontID fontID, const char* string);
+
+} // end namespace
+
+/*** END OF FILE ***/
+
+#endif /* __FREEGLUT_EXT_H__ */

workspace/slam_in_autonomous_driving/thirdparty/g2o/Makefile

@@ -0,0 +1,23 @@
+SHELL = /bin/bash
+
+ifeq ($(VERBOSE), 1)
+QUIET=
+else
+QUIET=-s --no-print-directory
+endif
+
+all: build/Makefile
+	@ $(MAKE) $(QUIET) -C build
+
+debug: build/Makefile
+	@ $(MAKE) $(QUIET) -C build
+
+clean: build/Makefile
+	@ $(MAKE) $(QUIET) -C build clean
+
+build/Makefile:
+	@ echo "Running cmake to generate Makefile"; \
+	cd build; \
+	cmake ../; \
+	cd -
+

workspace/slam_in_autonomous_driving/thirdparty/g2o/README.md

@@ -0,0 +1,157 @@
+g2o - General Graph Optimization
+================================
+
+Linux: [![Build Status](https://travis-ci.org/RainerKuemmerle/g2o.svg?branch=master)](https://travis-ci.org/RainerKuemmerle/g2o)
+Windows: [![Build status](https://ci.appveyor.com/api/projects/status/9w0cpb9krc6t4nt7/branch/master?svg=true)](https://ci.appveyor.com/project/RainerKuemmerle/g2o/branch/master)
+[![Code Quality: Cpp](https://img.shields.io/lgtm/grade/cpp/g/RainerKuemmerle/g2o.svg?logo=lgtm&logoWidth=18)](https://lgtm.com/projects/g/RainerKuemmerle/g2o/context:cpp)
+[![Total Alerts](https://img.shields.io/lgtm/alerts/g/RainerKuemmerle/g2o.svg?logo=lgtm&logoWidth=18)](https://lgtm.com/projects/g/RainerKuemmerle/g2o/alerts)
+
+g2o is an open-source C++ framework for optimizing graph-based nonlinear error
+functions. g2o has been designed to be easily extensible to a wide range of
+problems and a new problem typically can be specified in a few lines of code.
+The current implementation provides solutions to several variants of SLAM and
+BA.
+
+A wide range of problems in robotics as well as in computer-vision involve the
+minimization of a non-linear error function that can be represented as a graph.
+Typical instances are simultaneous localization and mapping (SLAM) or bundle
+adjustment (BA). The overall goal in these problems is to find the
+configuration of parameters or state variables that maximally explain a set of
+measurements affected by Gaussian noise. g2o is an open-source C++ framework
+for such nonlinear least squares problems. g2o has been designed to be easily
+extensible to a wide range of problems and a new problem typically can be
+specified in a few lines of code. The current implementation provides solutions
+to several variants of SLAM and BA. g2o offers a performance comparable to
+implementations of state-of-the-art approaches for the specific problems
+(02/2011).
+
+### Papers Describing the Approach:
+Rainer Kuemmerle, Giorgio Grisetti, Hauke Strasdat,
+Kurt Konolige, and Wolfram Burgard
+g2o: A General Framework for Graph Optimization
+IEEE International Conference on Robotics and Automation (ICRA), 2011
+http://ais.informatik.uni-freiburg.de/publications/papers/kuemmerle11icra.pdf
+
+### Documentation
+A detailed description of how the library is structured and how to use and extend it can be found in /doc/g2o.pdf
+The API documentation can be generated as described in doc/doxygen/readme.txt
+
+### License
+g2o is licensed under the BSD License. However, some libraries are available
+under different license terms. See below.
+
+The following parts are licensed under LGPL3+:
+- csparse\_extension
+
+The following parts are licensed under GPL3+:
+- g2o\_viewer
+- g2o\_incremental
+- slam2d\_g2o (example for 2D SLAM with a QGLviewer GUI)
+
+Please note that some features of CHOLMOD (which may be used by g2o, see
+libsuitesparse below) are licensed under the GPL. To avoid that your binary has
+to be licensed under the GPL, you may have to re-compile CHOLMOD without
+including its GPL features. The CHOLMOD library distributed with, for example,
+Ubuntu or Debian includes the GPL features. The supernodal factorization is
+considered by g2o, if it is available.
+
+Within the folder EXTERNAL we include software not written by us to
+guarantee easy compilation.
+- csparse: LPGL2.1 (see EXTERNAL/csparse/License.txt)
+  csparse is compiled if it is not provided by the system.
+- ceres: BSD (see EXTERNAL/ceres/LICENSE)
+  Headers to perform Automatic Differentiation
+- freeglut: X Consortium (Copyright (c) 1999-2000 Pawel W. Olszta)
+  We use a stripped down version for drawing text in OpenGL.
+
+See the doc folder for the full text of the licenses.
+
+g2o is distributed in the hope that it will be useful,
+but WITHOUT ANY WARRANTY; without even the implied warranty of
+MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+licenses for more details.
+
+
+### Requirements
+* cmake             http://www.cmake.org/
+* Eigen3            http://eigen.tuxfamily.org
+
+On Ubuntu / Debian these dependencies are resolved by installing the
+following packages.
+  - cmake
+  - libeigen3-dev
+
+#### Optional requirements
+* suitesparse       http://faculty.cse.tamu.edu/davis/suitesparse.html
+* Qt5               http://qt-project.org
+* libQGLViewer      http://www.libqglviewer.com/
+
+On Ubuntu / Debian these dependencies are resolved by installing the
+following packages.
+- libsuitesparse-dev
+- qtdeclarative5-dev
+- qt5-qmake
+- libqglviewer-dev
+
+#### Mac OS X
+If using [Homebrew](http://brew.sh/), then
+
+`brew install brewsci/science/g2o`
+
+will install g2o together with its required dependencies. In this case no manual compilation is necessary.
+
+#### Windows
+
+If using [vcpkg](https://github.com/Microsoft/vcpkg), then
+
+`scripts\install-deps-windows.bat`
+
+will build and install the required dependencies. The location of `vcpkg` and required triplet are determined by the environment variables `VCPKG_ROOT_DIR` and `VCPKG_DEFAULT_TRIPLET`.
+
+### Compilation
+
+Our primary development platform is Linux. Experimental support for
+Mac OS X, Android and Windows (MinGW or MSVC).
+We recommend a so-called out of source build which can be achieved
+by the following command sequence.
+
+- `mkdir build`
+- `cd build`
+- `cmake ../`
+- `make`
+
+The binaries will be placed in bin and the libraries in lib which
+are both located in the top-level folder.
+
+On Windows with `vcpkg` the following two commands will generate build scripts for Visual Studio 2017 MSVC 14 tool set:
+
+- `mkdir build`
+- `cd build`
+- `cmake -G "Visual Studio 14 2017 Win64" -DG2O_BUILD_APPS=ON -DG2O_BUILD_EXAMPLES=ON -DVCPKG_TARGET_TRIPLET="%VCPKG_DEFAULT_TRIPLET%" -DCMAKE_TOOLCHAIN_FILE="%VCPKG_ROOT_DIR%\scripts\buildsystems\vcpkg.cmake" ..`
+
+If you are compiling on Windows and you are for some reasons **not** using `vcpkg` please download Eigen3 and extract it.
+Within cmake-gui set the variable G2O\_EIGEN3\_INCLUDE to that directory.
+
+### Cross-Compiling for Android
+
+- `mkdir build`
+- `cd build`
+- `cmake -DCMAKE_TOOLCHAIN_FILE=../script/android.toolchain.cmake -DANDROID_NDK=<YOUR_PATH_TO_ANDROID_NDK_r10d+> -DCMAKE_BUILD_TYPE=Release -DANDROID_ABI="armeabi-v7a with NEON" -DEIGEN3_INCLUDE_DIR="<YOUR_PATH_TO_EIGEN>" -DEIGEN3_VERSION_OK=ON .. && cmake --build .`
+
+
+### Acknowledgments
+We thank the following contributors for providing patches:
+- Simon J. Julier: patches to achieve compatibility with Mac OS X and others.
+- Michael A. Eriksen for submitting patches to compile with MSVC.
+- Mark Pupilli for submitting patches to compile with MSVC.
+
+### Projects using g2o
+- [g2opy](https://github.com/uoip/g2opy): Python binding
+- [.Net wrapper](https://github.com/fugro/g2o)
+
+### Contact information
+Rainer Kuemmerle <kuemmerl@informatik.uni-freiburg.de>   
+Giorgio Grisetti <grisetti@dis.uniroma1.it>   
+Hauke Strasdat <strasdat@gmail.com>   
+Kurt Konolige <konolige@willowgarage.com>   
+Wolfram Burgard <burgard@informatik.uni-freiburg.de>   

workspace/slam_in_autonomous_driving/thirdparty/g2o/appveyor.yml

@@ -0,0 +1,25 @@
+branches:
+  only:
+    - master
+
+os: Visual Studio 2017
+
+clone_folder: c:\projects\g2o
+
+platform: x64
+configuration: Debug
+
+build:
+  project: c:\projects\g2o\build\g2o.sln 
+
+install:
+  - set QTDIR=C:\Qt\5.10.1\msvc2017_64
+  - set PATH=%PATH%;%QTDIR%\bin
+  - ps: wget http://bitbucket.org/eigen/eigen/get/3.3.5.zip -outfile eigen3.zip
+  - cmd: 7z x eigen3.zip -o"C:\projects" -y > nul
+  
+before_build:
+  - cd c:\projects\g2o
+  - mkdir build
+  - cd build
+  - cmake -G "Visual Studio 15 2017 Win64" -D EIGEN3_INCLUDE_DIR=C:\projects\eigen-eigen-b3f3d4950030 ..

workspace/slam_in_autonomous_driving/thirdparty/g2o/cmake_modules/Config.cmake.in

@@ -0,0 +1,7 @@
+include(CMakeFindDependencyMacro)
+
+find_dependency(Eigen3)
+find_dependency(OpenGL)
+
+include("${CMAKE_CURRENT_LIST_DIR}/@G2O_TARGETS_EXPORT_NAME@.cmake")
+

workspace/slam_in_autonomous_driving/thirdparty/g2o/cmake_modules/FindBLAS.cmake

@@ -0,0 +1,433 @@
+# Find BLAS library
+#
+# This module finds an installed library that implements the BLAS
+# linear-algebra interface (see http://www.netlib.org/blas/).
+# The list of libraries searched for is mainly taken
+# from the autoconf macro file, acx_blas.m4 (distributed at
+# http://ac-archive.sourceforge.net/ac-archive/acx_blas.html).
+#
+# This module sets the following variables:
+#  BLAS_FOUND - set to true if a library implementing the BLAS interface
+#    is found
+#  BLAS_INCLUDE_DIR - Directories containing the BLAS header files
+#  BLAS_DEFINITIONS - Compilation options to use BLAS
+#  BLAS_LINKER_FLAGS - Linker flags to use BLAS (excluding -l
+#    and -L).
+#  BLAS_LIBRARIES_DIR - Directories containing the BLAS libraries.
+#     May be null if BLAS_LIBRARIES contains libraries name using full path.
+#  BLAS_LIBRARIES - List of libraries to link against BLAS interface.
+#     May be null if the compiler supports auto-link (e.g. VC++).
+#  BLAS_USE_FILE - The name of the cmake module to include to compile
+#     applications or libraries using BLAS.
+#
+# This module was modified by CGAL team:
+# - find libraries for a C++ compiler, instead of Fortran
+# - added BLAS_INCLUDE_DIR, BLAS_DEFINITIONS and BLAS_LIBRARIES_DIR
+# - removed BLAS95_LIBRARIES
+
+include(CheckFunctionExists)
+
+
+# This macro checks for the existence of the combination of fortran libraries
+# given by _list.  If the combination is found, this macro checks (using the
+# check_function_exists macro) whether can link against that library
+# combination using the name of a routine given by _name using the linker
+# flags given by _flags.  If the combination of libraries is found and passes
+# the link test, LIBRARIES is set to the list of complete library paths that
+# have been found and DEFINITIONS to the required definitions.
+# Otherwise, LIBRARIES is set to FALSE.
+# N.B. _prefix is the prefix applied to the names of all cached variables that
+# are generated internally and marked advanced by this macro.
+macro(check_fortran_libraries DEFINITIONS LIBRARIES _prefix _name _flags _list _path)
+  #message("DEBUG: check_fortran_libraries(${_list} in ${_path})")
+
+  # Check for the existence of the libraries given by _list
+  set(_libraries_found TRUE)
+  set(_libraries_work FALSE)
+  set(${DEFINITIONS} "")
+  set(${LIBRARIES} "")
+  set(_combined_name)
+  foreach(_library ${_list})
+    set(_combined_name ${_combined_name}_${_library})
+
+    if(_libraries_found)
+      # search first in ${_path}
+      find_library(${_prefix}_${_library}_LIBRARY
+                  NAMES ${_library}
+                  PATHS ${_path} NO_DEFAULT_PATH
+                  )
+      # if not found, search in environment variables and system
+      if ( WIN32 )
+        find_library(${_prefix}_${_library}_LIBRARY
+                    NAMES ${_library}
+                    PATHS ENV LIB
+                    )
+      elseif ( APPLE )
+        find_library(${_prefix}_${_library}_LIBRARY
+                    NAMES ${_library}
+                    PATHS /usr/local/lib /usr/lib /usr/local/lib64 /usr/lib64 ENV DYLD_LIBRARY_PATH
+                    )
+      else ()
+        find_library(${_prefix}_${_library}_LIBRARY
+                    NAMES ${_library}
+                    PATHS /usr/local/lib /usr/lib /usr/local/lib64 /usr/lib64 ENV LD_LIBRARY_PATH
+                    )
+      endif()
+      mark_as_advanced(${_prefix}_${_library}_LIBRARY)
+      set(${LIBRARIES} ${${LIBRARIES}} ${${_prefix}_${_library}_LIBRARY})
+      set(_libraries_found ${${_prefix}_${_library}_LIBRARY})
+    endif(_libraries_found)
+  endforeach(_library ${_list})
+  if(_libraries_found)
+    set(_libraries_found ${${LIBRARIES}})
+  endif()
+
+  # Test this combination of libraries with the Fortran/f2c interface.
+  # We test the Fortran interface first as it is well standardized.
+  if(_libraries_found AND NOT _libraries_work)
+    set(${DEFINITIONS}  "-D${_prefix}_USE_F2C")
+    set(${LIBRARIES}    ${_libraries_found})
+    # Some C++ linkers require the f2c library to link with Fortran libraries.
+    # I do not know which ones, thus I just add the f2c library if it is available.
+    find_package( F2C QUIET )
+    if ( F2C_FOUND )
+      set(${DEFINITIONS}  ${${DEFINITIONS}} ${F2C_DEFINITIONS})
+      set(${LIBRARIES}    ${${LIBRARIES}} ${F2C_LIBRARIES})
+    endif()
+    set(CMAKE_REQUIRED_DEFINITIONS  ${${DEFINITIONS}})
+    set(CMAKE_REQUIRED_LIBRARIES    ${_flags} ${${LIBRARIES}})
+    #message("DEBUG: CMAKE_REQUIRED_DEFINITIONS = ${CMAKE_REQUIRED_DEFINITIONS}")
+    #message("DEBUG: CMAKE_REQUIRED_LIBRARIES = ${CMAKE_REQUIRED_LIBRARIES}")
+    # Check if function exists with f2c calling convention (ie a trailing underscore)
+    check_function_exists(${_name}_ ${_prefix}_${_name}_${_combined_name}_f2c_WORKS)
+    set(CMAKE_REQUIRED_DEFINITIONS} "")
+    set(CMAKE_REQUIRED_LIBRARIES    "")
+    mark_as_advanced(${_prefix}_${_name}_${_combined_name}_f2c_WORKS)
+    set(_libraries_work ${${_prefix}_${_name}_${_combined_name}_f2c_WORKS})
+  endif(_libraries_found AND NOT _libraries_work)
+
+  # If not found, test this combination of libraries with a C interface.
+  # A few implementations (ie ACML) provide a C interface. Unfortunately, there is no standard.
+  if(_libraries_found AND NOT _libraries_work)
+    set(${DEFINITIONS} "")
+    set(${LIBRARIES}   ${_libraries_found})
+    set(CMAKE_REQUIRED_DEFINITIONS "")
+    set(CMAKE_REQUIRED_LIBRARIES   ${_flags} ${${LIBRARIES}})
+    #message("DEBUG: CMAKE_REQUIRED_LIBRARIES = ${CMAKE_REQUIRED_LIBRARIES}")
+    check_function_exists(${_name} ${_prefix}_${_name}${_combined_name}_WORKS)
+    set(CMAKE_REQUIRED_LIBRARIES "")
+    mark_as_advanced(${_prefix}_${_name}${_combined_name}_WORKS)
+    set(_libraries_work ${${_prefix}_${_name}${_combined_name}_WORKS})
+  endif(_libraries_found AND NOT _libraries_work)
+
+  # on failure
+  if(NOT _libraries_work)
+    set(${DEFINITIONS} "")
+    set(${LIBRARIES}   FALSE)
+  endif()
+  #message("DEBUG: ${DEFINITIONS} = ${${DEFINITIONS}}")
+  #message("DEBUG: ${LIBRARIES} = ${${LIBRARIES}}")
+endmacro(check_fortran_libraries)
+
+
+#
+# main
+#
+
+# Is it already configured?
+if (BLAS_LIBRARIES_DIR OR BLAS_LIBRARIES)
+
+  set(BLAS_FOUND TRUE)
+
+else()
+
+  # reset variables
+  set( BLAS_INCLUDE_DIR "" )
+  set( BLAS_DEFINITIONS "" )
+  set( BLAS_LINKER_FLAGS "" )
+  set( BLAS_LIBRARIES "" )
+  set( BLAS_LIBRARIES_DIR "" )
+
+    #
+    # If Unix, search for BLAS function in possible libraries
+    #
+
+    # BLAS in OpenBLAS? (http://www.openblas.net)
+    if(NOT BLAS_LIBRARIES)
+        check_fortran_libraries(
+            BLAS_DEFINITIONS
+            BLAS_LIBRARIES
+            BLAS
+            sgemm
+            ""
+            "openblas"
+            ""
+            ""
+        )
+    endif()
+
+    # BLAS in ATLAS library? (http://math-atlas.sourceforge.net/)
+    if(NOT BLAS_LIBRARIES)
+      check_fortran_libraries(
+      BLAS_DEFINITIONS
+      BLAS_LIBRARIES
+      BLAS
+      sgemm
+      ""
+      "cblas;f77blas;atlas"
+      "${CGAL_TAUCS_LIBRARIES_DIR} ENV BLAS_LIB_DIR"
+      )
+    endif()
+
+    # BLAS in PhiPACK libraries? (requires generic BLAS lib, too)
+    if(NOT BLAS_LIBRARIES)
+      check_fortran_libraries(
+      BLAS_DEFINITIONS
+      BLAS_LIBRARIES
+      BLAS
+      sgemm
+      ""
+      "sgemm;dgemm;blas"
+      "${CGAL_TAUCS_LIBRARIES_DIR} ENV BLAS_LIB_DIR"
+      )
+    endif()
+
+    # BLAS in Alpha CXML library?
+    if(NOT BLAS_LIBRARIES)
+      check_fortran_libraries(
+      BLAS_DEFINITIONS
+      BLAS_LIBRARIES
+      BLAS
+      sgemm
+      ""
+      "cxml"
+      "${CGAL_TAUCS_LIBRARIES_DIR} ENV BLAS_LIB_DIR"
+      )
+    endif()
+
+    # BLAS in Alpha DXML library? (now called CXML, see above)
+    if(NOT BLAS_LIBRARIES)
+      check_fortran_libraries(
+      BLAS_DEFINITIONS
+      BLAS_LIBRARIES
+      BLAS
+      sgemm
+      ""
+      "dxml"
+      "${CGAL_TAUCS_LIBRARIES_DIR} ENV BLAS_LIB_DIR"
+      )
+    endif()
+
+    # BLAS in Sun Performance library?
+    if(NOT BLAS_LIBRARIES)
+      check_fortran_libraries(
+      BLAS_DEFINITIONS
+      BLAS_LIBRARIES
+      BLAS
+      sgemm
+      "-xlic_lib=sunperf"
+      "sunperf;sunmath"
+      "${CGAL_TAUCS_LIBRARIES_DIR} ENV BLAS_LIB_DIR"
+      )
+      if(BLAS_LIBRARIES)
+        # Extra linker flag
+        set(BLAS_LINKER_FLAGS "-xlic_lib=sunperf")
+      endif()
+    endif()
+
+    # BLAS in SCSL library?  (SGI/Cray Scientific Library)
+    if(NOT BLAS_LIBRARIES)
+      check_fortran_libraries(
+      BLAS_DEFINITIONS
+      BLAS_LIBRARIES
+      BLAS
+      sgemm
+      ""
+      "scsl"
+      "${CGAL_TAUCS_LIBRARIES_DIR} ENV BLAS_LIB_DIR"
+      )
+    endif()
+
+    # BLAS in SGIMATH library?
+    if(NOT BLAS_LIBRARIES)
+      check_fortran_libraries(
+      BLAS_DEFINITIONS
+      BLAS_LIBRARIES
+      BLAS
+      sgemm
+      ""
+      "complib.sgimath"
+      "${CGAL_TAUCS_LIBRARIES_DIR} ENV BLAS_LIB_DIR"
+      )
+    endif()
+
+    # BLAS in IBM ESSL library? (requires generic BLAS lib, too)
+    if(NOT BLAS_LIBRARIES)
+      check_fortran_libraries(
+      BLAS_DEFINITIONS
+      BLAS_LIBRARIES
+      BLAS
+      sgemm
+      ""
+      "essl;blas"
+      "${CGAL_TAUCS_LIBRARIES_DIR} ENV BLAS_LIB_DIR"
+      )
+    endif()
+
+    #BLAS in intel mkl 10 library? (em64t 64bit)
+    if(NOT BLAS_LIBRARIES)
+      check_fortran_libraries(
+      BLAS_DEFINITIONS
+      BLAS_LIBRARIES
+      BLAS
+      sgemm
+      ""
+      "mkl_intel_lp64;mkl_intel_thread;mkl_core;guide;pthread"
+      "${CGAL_TAUCS_LIBRARIES_DIR} ENV BLAS_LIB_DIR"
+      )
+    endif()
+
+    ### windows version of intel mkl 10?
+    if(NOT BLAS_LIBRARIES)
+      check_fortran_libraries(
+      BLAS_DEFINITIONS
+      BLAS_LIBRARIES
+      BLAS
+      SGEMM
+      ""
+      "mkl_c_dll;mkl_intel_thread_dll;mkl_core_dll;libguide40"
+      "${CGAL_TAUCS_LIBRARIES_DIR} ENV BLAS_LIB_DIR"
+      )
+    endif()
+
+    #older versions of intel mkl libs
+
+    # BLAS in intel mkl library? (shared)
+    if(NOT BLAS_LIBRARIES)
+      check_fortran_libraries(
+      BLAS_DEFINITIONS
+      BLAS_LIBRARIES
+      BLAS
+      sgemm
+      ""
+      "mkl;guide;pthread"
+      "${CGAL_TAUCS_LIBRARIES_DIR} ENV BLAS_LIB_DIR"
+      )
+    endif()
+
+    #BLAS in intel mkl library? (static, 32bit)
+    if(NOT BLAS_LIBRARIES)
+      check_fortran_libraries(
+      BLAS_DEFINITIONS
+      BLAS_LIBRARIES
+      BLAS
+      sgemm
+      ""
+      "mkl_ia32;guide;pthread"
+      "${CGAL_TAUCS_LIBRARIES_DIR} ENV BLAS_LIB_DIR"
+      )
+    endif()
+
+    #BLAS in intel mkl library? (static, em64t 64bit)
+    if(NOT BLAS_LIBRARIES)
+      check_fortran_libraries(
+      BLAS_DEFINITIONS
+      BLAS_LIBRARIES
+      BLAS
+      sgemm
+      ""
+      "mkl_em64t;guide;pthread"
+      "${CGAL_TAUCS_LIBRARIES_DIR} ENV BLAS_LIB_DIR"
+      )
+    endif()
+
+    #BLAS in acml library?
+    if(NOT BLAS_LIBRARIES)
+      check_fortran_libraries(
+      BLAS_DEFINITIONS
+      BLAS_LIBRARIES
+      BLAS
+      sgemm
+      ""
+      "acml"
+      "${CGAL_TAUCS_LIBRARIES_DIR} ENV BLAS_LIB_DIR"
+      )
+    endif()
+
+    # Apple BLAS library?
+    if(NOT BLAS_LIBRARIES)
+      check_fortran_libraries(
+      BLAS_DEFINITIONS
+      BLAS_LIBRARIES
+      BLAS
+      sgemm
+      ""
+      "Accelerate"
+      "${CGAL_TAUCS_LIBRARIES_DIR} ENV BLAS_LIB_DIR"
+      )
+    endif()
+
+    if ( NOT BLAS_LIBRARIES )
+      check_fortran_libraries(
+      BLAS_DEFINITIONS
+      BLAS_LIBRARIES
+      BLAS
+      sgemm
+      ""
+      "vecLib"
+      "${CGAL_TAUCS_LIBRARIES_DIR} ENV BLAS_LIB_DIR"
+      )
+    endif ( NOT BLAS_LIBRARIES )
+
+    # Generic BLAS library?
+    # This configuration *must* be the last try as this library is notably slow.
+    if ( NOT BLAS_LIBRARIES )
+      check_fortran_libraries(
+      BLAS_DEFINITIONS
+      BLAS_LIBRARIES
+      BLAS
+      sgemm
+      ""
+      "blas"
+      "${CGAL_TAUCS_LIBRARIES_DIR} ENV BLAS_LIB_DIR"
+      )
+    endif()
+
+  if(BLAS_LIBRARIES_DIR OR BLAS_LIBRARIES)
+    set(BLAS_FOUND TRUE)
+  else()
+    set(BLAS_FOUND FALSE)
+  endif()
+
+  if(NOT BLAS_FIND_QUIETLY)
+    if(BLAS_FOUND)
+      message(STATUS "A library with BLAS API found.")
+    else(BLAS_FOUND)
+      if(BLAS_FIND_REQUIRED)
+        message(FATAL_ERROR "A required library with BLAS API not found. Please specify library location.")
+      else()
+        message(STATUS "A library with BLAS API not found. Please specify library location.")
+      endif()
+    endif(BLAS_FOUND)
+  endif(NOT BLAS_FIND_QUIETLY)
+
+  # Add variables to cache
+  set( BLAS_INCLUDE_DIR   "${BLAS_INCLUDE_DIR}"
+                          CACHE PATH "Directories containing the BLAS header files" FORCE )
+  set( BLAS_DEFINITIONS   "${BLAS_DEFINITIONS}"
+                          CACHE STRING "Compilation options to use BLAS" FORCE )
+  set( BLAS_LINKER_FLAGS  "${BLAS_LINKER_FLAGS}"
+                          CACHE STRING "Linker flags to use BLAS" FORCE )
+  set( BLAS_LIBRARIES     "${BLAS_LIBRARIES}"
+                          CACHE FILEPATH "BLAS libraries name" FORCE )
+  set( BLAS_LIBRARIES_DIR "${BLAS_LIBRARIES_DIR}"
+                          CACHE PATH "Directories containing the BLAS libraries" FORCE )
+
+  #message("DEBUG: BLAS_INCLUDE_DIR = ${BLAS_INCLUDE_DIR}")
+  #message("DEBUG: BLAS_DEFINITIONS = ${BLAS_DEFINITIONS}")
+  #message("DEBUG: BLAS_LINKER_FLAGS = ${BLAS_LINKER_FLAGS}")
+  #message("DEBUG: BLAS_LIBRARIES = ${BLAS_LIBRARIES}")
+  #message("DEBUG: BLAS_LIBRARIES_DIR = ${BLAS_LIBRARIES_DIR}")
+  #message("DEBUG: BLAS_FOUND = ${BLAS_FOUND}")
+
+endif(BLAS_LIBRARIES_DIR OR BLAS_LIBRARIES)

workspace/slam_in_autonomous_driving/thirdparty/g2o/cmake_modules/FindCSparse.cmake

@@ -0,0 +1,27 @@
+# Look for csparse; note the difference in the directory specifications!
+find_path(CSPARSE_INCLUDE_DIR NAMES cs.h
+  PATHS
+  /usr/include/suitesparse
+  /usr/include
+  /opt/local/include
+  /usr/local/include
+  /sw/include
+  /usr/include/ufsparse
+  /opt/local/include/ufsparse
+  /usr/local/include/ufsparse
+  /sw/include/ufsparse
+  PATH_SUFFIXES
+  suitesparse
+  )
+
+find_library(CSPARSE_LIBRARY NAMES cxsparse libcxsparse
+  PATHS
+  /usr/lib
+  /usr/local/lib
+  /opt/local/lib
+  /sw/lib
+  )
+
+include(FindPackageHandleStandardArgs)
+find_package_handle_standard_args(CSPARSE DEFAULT_MSG
+  CSPARSE_INCLUDE_DIR CSPARSE_LIBRARY)

workspace/slam_in_autonomous_driving/thirdparty/g2o/cmake_modules/FindCholmod.cmake

@@ -0,0 +1,94 @@
+# Cholmod lib usually requires linking to a blas and lapack library.
+# It is up to the user of this module to find a BLAS and link to it.
+
+if (CHOLMOD_INCLUDE_DIR AND CHOLMOD_LIBRARIES)
+  set(CHOLMOD_FIND_QUIETLY TRUE)
+endif (CHOLMOD_INCLUDE_DIR AND CHOLMOD_LIBRARIES)
+
+find_path(CHOLMOD_INCLUDE_DIR
+  NAMES
+  cholmod.h
+  PATHS
+  $ENV{CHOLMODDIR}
+  ${INCLUDE_INSTALL_DIR}
+  PATH_SUFFIXES
+  suitesparse
+  ufsparse
+)
+
+find_library(CHOLMOD_LIBRARY 
+    NAMES cholmod libcholmod 
+    PATHS $ENV{CHOLMODDIR} ${LIB_INSTALL_DIR})
+
+set(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARY})
+
+if(CHOLMOD_LIBRARIES)
+
+  get_filename_component(CHOLMOD_LIBDIR ${CHOLMOD_LIBRARIES} PATH)
+
+  find_library(AMD_LIBRARY 
+    NAMES amd libamd 
+    PATHS ${CHOLMOD_LIBDIR} $ENV{CHOLMODDIR} ${LIB_INSTALL_DIR})
+  if (AMD_LIBRARY)
+    set(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARIES} ${AMD_LIBRARY})
+  else ()
+    set(CHOLMOD_LIBRARIES FALSE)
+  endif ()
+
+endif(CHOLMOD_LIBRARIES)
+
+if(CHOLMOD_LIBRARIES)
+
+  find_library(COLAMD_LIBRARY NAMES colamd libcolamd PATHS ${CHOLMOD_LIBDIR} $ENV{CHOLMODDIR} ${LIB_INSTALL_DIR})
+  if (COLAMD_LIBRARY)
+    set(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARIES} ${COLAMD_LIBRARY})
+  else ()
+    set(CHOLMOD_LIBRARIES FALSE)
+  endif ()
+
+endif(CHOLMOD_LIBRARIES)
+
+if(CHOLMOD_LIBRARIES)
+
+  find_library(CAMD_LIBRARY NAMES camd libcamd PATHS ${CHOLMOD_LIBDIR} $ENV{CHOLMODDIR} ${LIB_INSTALL_DIR})
+  if (CAMD_LIBRARY)
+    set(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARIES} ${CAMD_LIBRARY})
+  else ()
+    set(CHOLMOD_LIBRARIES FALSE)
+  endif ()
+
+endif(CHOLMOD_LIBRARIES)
+
+if(CHOLMOD_LIBRARIES)
+
+  find_library(CCOLAMD_LIBRARY NAMES ccolamd libccolamd PATHS ${CHOLMOD_LIBDIR} $ENV{CHOLMODDIR} ${LIB_INSTALL_DIR})
+  if (CCOLAMD_LIBRARY)
+    set(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARIES} ${CCOLAMD_LIBRARY})
+  else ()
+    set(CHOLMOD_LIBRARIES FALSE)
+  endif ()
+
+endif(CHOLMOD_LIBRARIES)
+
+if(CHOLMOD_LIBRARIES)
+
+  find_library(CHOLMOD_METIS_LIBRARY NAMES metis libmetis PATHS ${CHOLMOD_LIBDIR} $ENV{CHOLMODDIR} ${LIB_INSTALL_DIR})
+  if (CHOLMOD_METIS_LIBRARY)
+    set(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARIES} ${CHOLMOD_METIS_LIBRARY})
+  endif ()
+
+endif(CHOLMOD_LIBRARIES)
+
+if(CHOLMOD_LIBRARIES)
+  find_library(CHOLMOD_SUITESPARSECONFIG_LIBRARY NAMES suitesparseconfig
+    PATHS ${CHOLMOD_LIBDIR} $ENV{CHOLMODDIR} ${LIB_INSTALL_DIR})
+  if (CHOLMOD_SUITESPARSECONFIG_LIBRARY)
+    set(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARIES} ${CHOLMOD_SUITESPARSECONFIG_LIBRARY})
+  endif ()
+endif(CHOLMOD_LIBRARIES)
+
+include(FindPackageHandleStandardArgs)
+find_package_handle_standard_args(CHOLMOD DEFAULT_MSG
+                                  CHOLMOD_INCLUDE_DIR CHOLMOD_LIBRARIES)
+
+mark_as_advanced(CHOLMOD_LIBRARIES)

workspace/slam_in_autonomous_driving/thirdparty/g2o/cmake_modules/FindEigen3.cmake

@@ -0,0 +1,87 @@
+# - Try to find Eigen3 lib
+#
+# 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.
+#
+# Once done this will define
+#
+#  EIGEN3_FOUND - system has eigen lib with correct version
+#  EIGEN3_INCLUDE_DIR - the eigen include directory
+#  EIGEN3_VERSION - eigen version
+
+# 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)
+
+  # specific additional paths for some OS
+  if (WIN32)
+    set(EIGEN_ADDITIONAL_SEARCH_PATHS ${EIGEN_ADDITIONAL_SEARCH_PATHS} "C:/Program Files/Eigen/include" "C:/Program Files (x86)/Eigen/include")
+  endif(WIN32)
+
+  find_path(EIGEN3_INCLUDE_DIR NAMES signature_of_eigen3_matrix_library
+      PATHS
+      include
+      ${EIGEN_ADDITIONAL_SEARCH_PATHS}
+      ${KDE4_INCLUDE_DIR}
+      PATH_SUFFIXES eigen3 eigen
+    )
+
+  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)
+

workspace/slam_in_autonomous_driving/thirdparty/g2o/cmake_modules/FindG2O.cmake

@@ -0,0 +1,113 @@
+# Find the header files
+
+find_path(G2O_INCLUDE_DIR g2o/core/base_vertex.h
+  ${G2O_ROOT}/include
+  $ENV{G2O_ROOT}/include
+  $ENV{G2O_ROOT}
+  /usr/local/include
+  /usr/include
+  /opt/local/include
+  /sw/local/include
+  /sw/include
+  NO_DEFAULT_PATH
+  )
+
+# Macro to unify finding both the debug and release versions of the
+# libraries; this is adapted from the OpenSceneGraph FIND_LIBRARY
+# macro.
+
+macro(FIND_G2O_LIBRARY MYLIBRARY MYLIBRARYNAME)
+
+  find_library("${MYLIBRARY}_DEBUG"
+    NAMES "g2o_${MYLIBRARYNAME}_d"
+    PATHS
+    ${G2O_ROOT}/lib/Debug
+    ${G2O_ROOT}/lib
+    $ENV{G2O_ROOT}/lib/Debug
+    $ENV{G2O_ROOT}/lib
+    NO_DEFAULT_PATH
+    )
+
+  find_library("${MYLIBRARY}_DEBUG"
+    NAMES "g2o_${MYLIBRARYNAME}_d"
+    PATHS
+    ~/Library/Frameworks
+    /Library/Frameworks
+    /usr/local/lib
+    /usr/local/lib64
+    /usr/lib
+    /usr/lib64
+    /opt/local/lib
+    /sw/local/lib
+    /sw/lib
+    )
+  
+  find_library(${MYLIBRARY}
+    NAMES "g2o_${MYLIBRARYNAME}"
+    PATHS
+    ${G2O_ROOT}/lib/Release
+    ${G2O_ROOT}/lib
+    $ENV{G2O_ROOT}/lib/Release
+    $ENV{G2O_ROOT}/lib
+    NO_DEFAULT_PATH
+    )
+
+  find_library(${MYLIBRARY}
+    NAMES "g2o_${MYLIBRARYNAME}"
+    PATHS
+    ~/Library/Frameworks
+    /Library/Frameworks
+    /usr/local/lib
+    /usr/local/lib64
+    /usr/lib
+    /usr/lib64
+    /opt/local/lib
+    /sw/local/lib
+    /sw/lib
+    )
+  
+  if(NOT ${MYLIBRARY}_DEBUG)
+    if(MYLIBRARY)
+      set(${MYLIBRARY}_DEBUG ${MYLIBRARY})
+    endif(MYLIBRARY)
+  endif( NOT ${MYLIBRARY}_DEBUG)
+  
+endmacro(FIND_G2O_LIBRARY LIBRARY LIBRARYNAME)
+
+# Find the core elements
+FIND_G2O_LIBRARY(G2O_STUFF_LIBRARY stuff)
+FIND_G2O_LIBRARY(G2O_CORE_LIBRARY core)
+
+# Find the CLI library
+FIND_G2O_LIBRARY(G2O_CLI_LIBRARY cli)
+
+# Find the pluggable solvers
+FIND_G2O_LIBRARY(G2O_SOLVER_CHOLMOD solver_cholmod)
+FIND_G2O_LIBRARY(G2O_SOLVER_CSPARSE solver_csparse)
+FIND_G2O_LIBRARY(G2O_SOLVER_CSPARSE_EXTENSION csparse_extension)
+FIND_G2O_LIBRARY(G2O_SOLVER_DENSE solver_dense)
+FIND_G2O_LIBRARY(G2O_SOLVER_PCG solver_pcg)
+FIND_G2O_LIBRARY(G2O_SOLVER_SLAM2D_LINEAR solver_slam2d_linear)
+FIND_G2O_LIBRARY(G2O_SOLVER_STRUCTURE_ONLY solver_structure_only)
+FIND_G2O_LIBRARY(G2O_SOLVER_EIGEN solver_eigen)
+
+# Find the predefined types
+FIND_G2O_LIBRARY(G2O_TYPES_DATA types_data)
+FIND_G2O_LIBRARY(G2O_TYPES_ICP types_icp)
+FIND_G2O_LIBRARY(G2O_TYPES_SBA types_sba)
+FIND_G2O_LIBRARY(G2O_TYPES_SCLAM2D types_sclam2d)
+FIND_G2O_LIBRARY(G2O_TYPES_SIM3 types_sim3)
+FIND_G2O_LIBRARY(G2O_TYPES_SLAM2D types_slam2d)
+FIND_G2O_LIBRARY(G2O_TYPES_SLAM3D types_slam3d)
+
+# G2O solvers declared found if we found at least one solver
+set(G2O_SOLVERS_FOUND "NO")
+if(G2O_SOLVER_CHOLMOD OR G2O_SOLVER_CSPARSE OR G2O_SOLVER_DENSE OR G2O_SOLVER_PCG OR G2O_SOLVER_SLAM2D_LINEAR OR G2O_SOLVER_STRUCTURE_ONLY OR G2O_SOLVER_EIGEN)
+  set(G2O_SOLVERS_FOUND "YES")
+endif(G2O_SOLVER_CHOLMOD OR G2O_SOLVER_CSPARSE OR G2O_SOLVER_DENSE OR G2O_SOLVER_PCG OR G2O_SOLVER_SLAM2D_LINEAR OR G2O_SOLVER_STRUCTURE_ONLY OR G2O_SOLVER_EIGEN)
+
+# G2O itself declared found if we found the core libraries and at least one solver
+set(G2O_FOUND "NO")
+if(G2O_STUFF_LIBRARY AND G2O_CORE_LIBRARY AND G2O_INCLUDE_DIR AND G2O_SOLVERS_FOUND)
+  set(G2O_FOUND "YES")
+endif(G2O_STUFF_LIBRARY AND G2O_CORE_LIBRARY AND G2O_INCLUDE_DIR AND G2O_SOLVERS_FOUND)

workspace/slam_in_autonomous_driving/thirdparty/g2o/cmake_modules/FindLAPACK.cmake

@@ -0,0 +1,273 @@
+# Find LAPACK library
+#
+# This module finds an installed library that implements the LAPACK
+# linear-algebra interface (see http://www.netlib.org/lapack/).
+# The approach follows mostly that taken for the autoconf macro file, acx_lapack.m4
+# (distributed at http://ac-archive.sourceforge.net/ac-archive/acx_lapack.html).
+#
+# This module sets the following variables:
+#  LAPACK_FOUND - set to true if a library implementing the LAPACK interface
+#    is found
+#  LAPACK_INCLUDE_DIR - Directories containing the LAPACK header files
+#  LAPACK_DEFINITIONS - Compilation options to use LAPACK
+#  LAPACK_LINKER_FLAGS - Linker flags to use LAPACK (excluding -l
+#    and -L).
+#  LAPACK_LIBRARIES_DIR - Directories containing the LAPACK libraries.
+#     May be null if LAPACK_LIBRARIES contains libraries name using full path.
+#  LAPACK_LIBRARIES - List of libraries to link against LAPACK interface.
+#     May be null if the compiler supports auto-link (e.g. VC++).
+#  LAPACK_USE_FILE - The name of the cmake module to include to compile
+#     applications or libraries using LAPACK.
+#
+# This module was modified by CGAL team:
+# - find libraries for a C++ compiler, instead of Fortran
+# - added LAPACK_INCLUDE_DIR, LAPACK_DEFINITIONS and LAPACK_LIBRARIES_DIR
+# - removed LAPACK95_LIBRARIES
+
+
+include(CheckFunctionExists)
+
+# This macro checks for the existence of the combination of fortran libraries
+# given by _list.  If the combination is found, this macro checks (using the
+# check_function_exists macro) whether can link against that library
+# combination using the name of a routine given by _name using the linker
+# flags given by _flags.  If the combination of libraries is found and passes
+# the link test, LIBRARIES is set to the list of complete library paths that
+# have been found and DEFINITIONS to the required definitions.
+# Otherwise, LIBRARIES is set to FALSE.
+# N.B. _prefix is the prefix applied to the names of all cached variables that
+# are generated internally and marked advanced by this macro.
+macro(check_lapack_libraries DEFINITIONS LIBRARIES _prefix _name _flags _list _blas _path)
+  #message("DEBUG: check_lapack_libraries(${_list} in ${_path} with ${_blas})")
+
+  # Check for the existence of the libraries given by _list
+  set(_libraries_found TRUE)
+  set(_libraries_work FALSE)
+  set(${DEFINITIONS} "")
+  set(${LIBRARIES} "")
+  set(_combined_name)
+  foreach(_library ${_list})
+    set(_combined_name ${_combined_name}_${_library})
+
+    if(_libraries_found)
+      # search first in ${_path}
+      find_library(${_prefix}_${_library}_LIBRARY
+                  NAMES ${_library}
+                  PATHS ${_path} NO_DEFAULT_PATH
+                  )
+      # if not found, search in environment variables and system
+      if ( WIN32 )
+        find_library(${_prefix}_${_library}_LIBRARY
+                    NAMES ${_library}
+                    PATHS ENV LIB
+                    )
+      elseif ( APPLE )
+        find_library(${_prefix}_${_library}_LIBRARY
+                    NAMES ${_library}
+                    PATHS /usr/local/lib /usr/lib /usr/local/lib64 /usr/lib64 ENV DYLD_LIBRARY_PATH
+                    )
+      else ()
+        find_library(${_prefix}_${_library}_LIBRARY
+                    NAMES ${_library}
+                    PATHS /usr/local/lib /usr/lib /usr/local/lib64 /usr/lib64 ENV LD_LIBRARY_PATH
+                    )
+      endif()
+      mark_as_advanced(${_prefix}_${_library}_LIBRARY)
+      set(${LIBRARIES} ${${LIBRARIES}} ${${_prefix}_${_library}_LIBRARY})
+      set(_libraries_found ${${_prefix}_${_library}_LIBRARY})
+    endif(_libraries_found)
+  endforeach(_library ${_list})
+  if(_libraries_found)
+    set(_libraries_found ${${LIBRARIES}})
+  endif()
+
+  # Test this combination of libraries with the Fortran/f2c interface.
+  # We test the Fortran interface first as it is well standardized.
+  if(_libraries_found AND NOT _libraries_work)
+    set(${DEFINITIONS}  "-D${_prefix}_USE_F2C")
+    set(${LIBRARIES}    ${_libraries_found})
+    # Some C++ linkers require the f2c library to link with Fortran libraries.
+    # I do not know which ones, thus I just add the f2c library if it is available.
+    find_package( F2C QUIET )
+    if ( F2C_FOUND )
+      set(${DEFINITIONS}  ${${DEFINITIONS}} ${F2C_DEFINITIONS})
+      set(${LIBRARIES}    ${${LIBRARIES}} ${F2C_LIBRARIES})
+    endif()
+    set(CMAKE_REQUIRED_DEFINITIONS  ${${DEFINITIONS}})
+    set(CMAKE_REQUIRED_LIBRARIES    ${_flags} ${${LIBRARIES}} ${_blas})
+    #message("DEBUG: CMAKE_REQUIRED_DEFINITIONS = ${CMAKE_REQUIRED_DEFINITIONS}")
+    #message("DEBUG: CMAKE_REQUIRED_LIBRARIES = ${CMAKE_REQUIRED_LIBRARIES}")
+    # Check if function exists with f2c calling convention (ie a trailing underscore)
+    check_function_exists(${_name}_ ${_prefix}_${_name}_${_combined_name}_f2c_WORKS)
+    set(CMAKE_REQUIRED_DEFINITIONS} "")
+    set(CMAKE_REQUIRED_LIBRARIES    "")
+    mark_as_advanced(${_prefix}_${_name}_${_combined_name}_f2c_WORKS)
+    set(_libraries_work ${${_prefix}_${_name}_${_combined_name}_f2c_WORKS})
+  endif(_libraries_found AND NOT _libraries_work)
+
+  # If not found, test this combination of libraries with a C interface.
+  # A few implementations (ie ACML) provide a C interface. Unfortunately, there is no standard.
+  if(_libraries_found AND NOT _libraries_work)
+    set(${DEFINITIONS} "")
+    set(${LIBRARIES}   ${_libraries_found})
+    set(CMAKE_REQUIRED_DEFINITIONS "")
+    set(CMAKE_REQUIRED_LIBRARIES   ${_flags} ${${LIBRARIES}} ${_blas})
+    #message("DEBUG: CMAKE_REQUIRED_LIBRARIES = ${CMAKE_REQUIRED_LIBRARIES}")
+    check_function_exists(${_name} ${_prefix}_${_name}${_combined_name}_WORKS)
+    set(CMAKE_REQUIRED_LIBRARIES "")
+    mark_as_advanced(${_prefix}_${_name}${_combined_name}_WORKS)
+    set(_libraries_work ${${_prefix}_${_name}${_combined_name}_WORKS})
+  endif(_libraries_found AND NOT _libraries_work)
+
+  # on failure
+  if(NOT _libraries_work)
+    set(${DEFINITIONS} "")
+    set(${LIBRARIES}   FALSE)
+  endif()
+  #message("DEBUG: ${DEFINITIONS} = ${${DEFINITIONS}}")
+  #message("DEBUG: ${LIBRARIES} = ${${LIBRARIES}}")
+endmacro(check_lapack_libraries)
+
+
+#
+# main
+#
+
+# LAPACK requires BLAS
+if(LAPACK_FIND_QUIETLY OR NOT LAPACK_FIND_REQUIRED)
+  find_package(BLAS)
+else()
+  find_package(BLAS REQUIRED)
+endif()
+
+if (NOT BLAS_FOUND)
+
+  message(STATUS "LAPACK requires BLAS.")
+  set(LAPACK_FOUND FALSE)
+
+# Is it already configured?
+elseif (LAPACK_LIBRARIES_DIR OR LAPACK_LIBRARIES)
+
+  set(LAPACK_FOUND TRUE)
+
+else()
+
+  # reset variables
+  set( LAPACK_INCLUDE_DIR "" )
+  set( LAPACK_DEFINITIONS "" )
+  set( LAPACK_LINKER_FLAGS "" ) # unused (yet)
+  set( LAPACK_LIBRARIES "" )
+  set( LAPACK_LIBRARIES_DIR "" )
+
+    #
+    # If Unix, search for LAPACK function in possible libraries
+    #
+
+    #intel mkl lapack?
+    if(NOT LAPACK_LIBRARIES)
+      check_lapack_libraries(
+      LAPACK_DEFINITIONS
+      LAPACK_LIBRARIES
+      LAPACK
+      cheev
+      ""
+      "mkl_lapack"
+      "${BLAS_LIBRARIES}"
+      "${CGAL_TAUCS_LIBRARIES_DIR} ENV LAPACK_LIB_DIR"
+      )
+    endif()
+
+    #acml lapack?
+    if(NOT LAPACK_LIBRARIES)
+      check_lapack_libraries(
+      LAPACK_DEFINITIONS
+      LAPACK_LIBRARIES
+      LAPACK
+      cheev
+      ""
+      "acml"
+      "${BLAS_LIBRARIES}"
+      "${CGAL_TAUCS_LIBRARIES_DIR} ENV LAPACK_LIB_DIR"
+      )
+    endif()
+
+    # Apple LAPACK library?
+    if(NOT LAPACK_LIBRARIES)
+      check_lapack_libraries(
+      LAPACK_DEFINITIONS
+      LAPACK_LIBRARIES
+      LAPACK
+      cheev
+      ""
+      "Accelerate"
+      "${BLAS_LIBRARIES}"
+      "${CGAL_TAUCS_LIBRARIES_DIR} ENV LAPACK_LIB_DIR"
+      )
+    endif()
+
+    if ( NOT LAPACK_LIBRARIES )
+      check_lapack_libraries(
+      LAPACK_DEFINITIONS
+      LAPACK_LIBRARIES
+      LAPACK
+      cheev
+      ""
+      "vecLib"
+      "${BLAS_LIBRARIES}"
+      "${CGAL_TAUCS_LIBRARIES_DIR} ENV LAPACK_LIB_DIR"
+      )
+    endif ( NOT LAPACK_LIBRARIES )
+
+    # Generic LAPACK library?
+    # This configuration *must* be the last try as this library is notably slow.
+    if ( NOT LAPACK_LIBRARIES )
+      check_lapack_libraries(
+      LAPACK_DEFINITIONS
+      LAPACK_LIBRARIES
+      LAPACK
+      cheev
+      ""
+      "lapack"
+      "${BLAS_LIBRARIES}"
+      "${CGAL_TAUCS_LIBRARIES_DIR} ENV LAPACK_LIB_DIR"
+      )
+    endif()
+
+  if(LAPACK_LIBRARIES_DIR OR LAPACK_LIBRARIES)
+    set(LAPACK_FOUND TRUE)
+  else()
+    set(LAPACK_FOUND FALSE)
+  endif()
+
+  if(NOT LAPACK_FIND_QUIETLY)
+    if(LAPACK_FOUND)
+      message(STATUS "A library with LAPACK API found.")
+    else(LAPACK_FOUND)
+      if(LAPACK_FIND_REQUIRED)
+        message(FATAL_ERROR "A required library with LAPACK API not found. Please specify library location.")
+      else()
+        message(STATUS "A library with LAPACK API not found. Please specify library location.")
+      endif()
+    endif(LAPACK_FOUND)
+  endif(NOT LAPACK_FIND_QUIETLY)
+
+  # Add variables to cache
+  set( LAPACK_INCLUDE_DIR   "${LAPACK_INCLUDE_DIR}"
+                            CACHE PATH "Directories containing the LAPACK header files" FORCE )
+  set( LAPACK_DEFINITIONS   "${LAPACK_DEFINITIONS}"
+                            CACHE STRING "Compilation options to use LAPACK" FORCE )
+  set( LAPACK_LINKER_FLAGS  "${LAPACK_LINKER_FLAGS}"
+                            CACHE STRING "Linker flags to use LAPACK" FORCE )
+  set( LAPACK_LIBRARIES     "${LAPACK_LIBRARIES}"
+                            CACHE FILEPATH "LAPACK libraries name" FORCE )
+  set( LAPACK_LIBRARIES_DIR "${LAPACK_LIBRARIES_DIR}"
+                            CACHE PATH "Directories containing the LAPACK libraries" FORCE )
+
+  #message("DEBUG: LAPACK_INCLUDE_DIR = ${LAPACK_INCLUDE_DIR}")
+  #message("DEBUG: LAPACK_DEFINITIONS = ${LAPACK_DEFINITIONS}")
+  #message("DEBUG: LAPACK_LINKER_FLAGS = ${LAPACK_LINKER_FLAGS}")
+  #message("DEBUG: LAPACK_LIBRARIES = ${LAPACK_LIBRARIES}")
+  #message("DEBUG: LAPACK_LIBRARIES_DIR = ${LAPACK_LIBRARIES_DIR}")
+  #message("DEBUG: LAPACK_FOUND = ${LAPACK_FOUND}")
+
+endif(NOT BLAS_FOUND)

workspace/slam_in_autonomous_driving/thirdparty/g2o/cmake_modules/FindQGLViewer.cmake

@@ -0,0 +1,50 @@
+find_package(Qt5 COMPONENTS Core Xml OpenGL Gui Widgets)
+if(NOT Qt5_FOUND)
+  message("Qt5 not found. Install it and set Qt5_DIR accordingly")
+  if (WIN32)
+    message("  In Windows, Qt5_DIR should be something like C:/Qt/5.4/msvc2013_64_opengl/lib/cmake/Qt5")
+  endif()
+endif()
+
+find_path(QGLVIEWER_INCLUDE_DIR qglviewer.h
+    /usr/include/QGLViewer
+    /opt/local/include/QGLViewer
+    /usr/local/include/QGLViewer
+    /sw/include/QGLViewer
+    ENV QGLVIEWERROOT
+  )
+
+find_library(QGLVIEWER_LIBRARY_RELEASE
+  NAMES qglviewer QGLViewer qglviewer-qt5 QGLViewer-qt5
+  PATHS /usr/lib
+        /usr/local/lib
+        /opt/local/lib
+        /sw/lib
+        ENV QGLVIEWERROOT
+        ENV LD_LIBRARY_PATH
+        ENV LIBRARY_PATH
+  PATH_SUFFIXES QGLViewer QGLViewer/release
+)
+find_library(QGLVIEWER_LIBRARY_DEBUG
+  NAMES dqglviewer dQGLViewer dqglviewer-qt5 dQGLViewer-qt5 QGLViewerd2
+  PATHS /usr/lib
+        /usr/local/lib
+        /opt/local/lib
+        /sw/lib
+        ENV QGLVIEWERROOT
+        ENV LD_LIBRARY_PATH
+        ENV LIBRARY_PATH
+  PATH_SUFFIXES QGLViewer QGLViewer/debug
+)
+
+if(QGLVIEWER_LIBRARY_RELEASE)
+  if(QGLVIEWER_LIBRARY_DEBUG)
+    set(QGLVIEWER_LIBRARY optimized ${QGLVIEWER_LIBRARY_RELEASE} debug ${QGLVIEWER_LIBRARY_DEBUG})
+  else()
+    set(QGLVIEWER_LIBRARY ${QGLVIEWER_LIBRARY_RELEASE})
+  endif()
+endif()
+
+include(FindPackageHandleStandardArgs)
+find_package_handle_standard_args(QGLVIEWER DEFAULT_MSG
+  QGLVIEWER_INCLUDE_DIR QGLVIEWER_LIBRARY)

workspace/slam_in_autonomous_driving/thirdparty/g2o/cmake_modules/FindSuiteSparse.cmake

@@ -0,0 +1,127 @@
+find_path(CHOLMOD_INCLUDE_DIR NAMES cholmod.h amd.h camd.h
+    PATHS
+    ${SUITE_SPARSE_ROOT}/include
+    /usr/include/suitesparse
+    /usr/include/ufsparse
+    /opt/local/include/ufsparse
+    /usr/local/include/ufsparse
+    /sw/include/ufsparse
+  )
+
+find_library(CHOLMOD_LIBRARY NAMES cholmod
+     PATHS
+     ${SUITE_SPARSE_ROOT}/lib
+     /usr/lib
+     /usr/local/lib
+     /opt/local/lib
+     /sw/lib
+   )
+
+find_library(AMD_LIBRARY NAMES SHARED NAMES amd
+  PATHS
+  ${SUITE_SPARSE_ROOT}/lib
+  /usr/lib
+  /usr/local/lib
+  /opt/local/lib
+  /sw/lib
+  )
+
+find_library(CAMD_LIBRARY NAMES camd
+  PATHS
+  ${SUITE_SPARSE_ROOT}/lib
+  /usr/lib
+  /usr/local/lib
+  /opt/local/lib
+  /sw/lib
+  )
+
+find_library(SUITESPARSECONFIG_LIBRARY NAMES suitesparseconfig
+  PATHS
+  ${SUITE_SPARSE_ROOT}/lib
+  /usr/lib
+  /usr/local/lib
+  /opt/local/lib
+  /sw/lib
+  )
+
+
+# Different platforms seemingly require linking against different sets of libraries
+if(CYGWIN)
+  find_package(PkgConfig)
+  find_library(COLAMD_LIBRARY NAMES colamd
+    PATHS
+    /usr/lib
+    /usr/local/lib
+    /opt/local/lib
+    /sw/lib
+    )
+  PKG_CHECK_MODULES(LAPACK lapack)
+
+  set(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARY} ${AMD_LIBRARY} ${CAMD_LIBRARY} ${COLAMD_LIBRARY} ${CCOLAMD_LIBRARY} ${LAPACK_LIBRARIES})
+
+# MacPorts build of the SparseSuite requires linking against extra libraries
+
+elseif(APPLE)
+
+  find_library(COLAMD_LIBRARY NAMES colamd
+    PATHS
+    /usr/lib
+    /usr/local/lib
+    /opt/local/lib
+    /sw/lib
+    )
+
+  find_library(CCOLAMD_LIBRARY NAMES ccolamd
+    PATHS
+    /usr/lib
+    /usr/local/lib
+    /opt/local/lib
+    /sw/lib
+    )
+
+  find_library(METIS_LIBRARY NAMES metis
+    PATHS
+    /usr/lib
+    /usr/local/lib
+    /opt/local/lib
+    /sw/lib
+    )
+
+  set(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARY} ${AMD_LIBRARY} ${CAMD_LIBRARY} ${COLAMD_LIBRARY} ${CCOLAMD_LIBRARY} ${METIS_LIBRARY} "-framework Accelerate")
+else(APPLE)
+  set(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARY} ${AMD_LIBRARY})
+endif(CYGWIN)
+
+if(CHOLMOD_INCLUDE_DIR AND CHOLMOD_LIBRARIES)
+  set(CHOLMOD_FOUND TRUE)
+else(CHOLMOD_INCLUDE_DIR AND CHOLMOD_LIBRARIES)
+  set(CHOLMOD_FOUND FALSE)
+endif(CHOLMOD_INCLUDE_DIR AND CHOLMOD_LIBRARIES)
+
+# Look for csparse; note the difference in the directory specifications!
+find_path(CSPARSE_INCLUDE_DIR NAMES cs.h
+  PATHS
+  /usr/include/suitesparse
+  /usr/include
+  /opt/local/include
+  /usr/local/include
+  /sw/include
+  /usr/include/ufsparse
+  /opt/local/include/ufsparse
+  /usr/local/include/ufsparse
+  /sw/include/ufsparse
+  )
+
+find_library(CSPARSE_LIBRARY NAMES cxsparse
+  PATHS
+  /usr/lib
+  /usr/local/lib
+  /opt/local/lib
+  /sw/lib
+  )
+
+if(CSPARSE_INCLUDE_DIR AND CSPARSE_LIBRARY)
+  set(CSPARSE_FOUND TRUE)
+else(CSPARSE_INCLUDE_DIR AND CSPARSE_LIBRARY)
+  set(CSPARSE_FOUND FALSE)
+endif(CSPARSE_INCLUDE_DIR AND CSPARSE_LIBRARY)

workspace/slam_in_autonomous_driving/thirdparty/g2o/config.h.in

@@ -0,0 +1,48 @@
+#ifndef G2O_CONFIG_H
+#define G2O_CONFIG_H
+
+
+#cmakedefine G2O_HAVE_OPENGL 1
+#cmakedefine G2O_OPENGL_FOUND 1
+#cmakedefine G2O_OPENMP 1
+#cmakedefine G2O_SHARED_LIBS 1
+#cmakedefine G2O_LGPL_SHARED_LIBS 1
+
+// available sparse matrix libraries
+#cmakedefine G2O_HAVE_CHOLMOD 1
+#cmakedefine G2O_HAVE_CSPARSE 1
+
+#cmakedefine G2O_NO_IMPLICIT_OWNERSHIP_OF_OBJECTS
+
+#ifdef G2O_NO_IMPLICIT_OWNERSHIP_OF_OBJECTS
+#define G2O_DELETE_IMPLICITLY_OWNED_OBJECTS 0
+#else
+#define G2O_DELETE_IMPLICITLY_OWNED_OBJECTS 1
+#endif
+
+#cmakedefine G2O_SINGLE_PRECISION_MATH
+#ifdef G2O_SINGLE_PRECISION_MATH
+    #define G2O_NUMBER_FORMAT_STR "%g"
+
+    #ifdef __cplusplus
+        using number_t = float;
+    #else
+        typedef float number_t;
+    #endif
+#else
+    #define G2O_NUMBER_FORMAT_STR "%lg"
+
+    #ifdef __cplusplus
+        using number_t = double;
+    #else
+        typedef double number_t;
+    #endif
+#endif
+
+#cmakedefine G2O_CXX_COMPILER "@G2O_CXX_COMPILER@"
+
+#ifdef __cplusplus
+#include <g2o/core/eigen_types.h>
+#endif
+
+#endif

workspace/slam_in_autonomous_driving/thirdparty/g2o/doc/.gitignore

@@ -0,0 +1,7 @@
+*.aux
+*.bbl
+*.blg
+*.dvi
+*.log
+doxygen/html
+doxygen/YES

workspace/slam_in_autonomous_driving/thirdparty/g2o/doc/Makefile

@@ -0,0 +1,10 @@
+all: g2o.pdf
+
+%.pdf:	%.dvi
+	dvipdf $<
+
+g2o.dvi: g2o.tex
+	latex g2o.tex
+	bibtex g2o
+	latex g2o.tex
+	latex g2o.tex

workspace/slam_in_autonomous_driving/thirdparty/g2o/doc/README_IF_IT_WAS_WORKING_AND_IT_DOES_NOT.txt

@@ -0,0 +1,217 @@
+There is some relevant change in the devel release of g2o.
+
+Here is the list
+
+*CHANGE IN THE ACCESSORS*
+The following non const accessors have been removed
+
+Vertex class:
+EstType& Vertex::estimate(); // use Vertex::setEstimate(const EstType& est);
+
+Edge class:
+MeasType& Edge::measurement(); // use Edge::setMeasurement(const MeasType& m);
+MeasType& Edge::inverseMeasurement(); // the inverse measurement should be computed directly by setMeasurement. not possible to set it by hand to avoid inconsistencies
+
+SE3Quat class:
+Vector3d& SE3Quat::translation(); // use SE3Quat::setTranslation(...) instead;
+Quarerniond& SE3Quat::rotation(); // use SE3Quat::setRotation(...) instead; This also ensures that the quaternion is normalized and w>0.
+double& opertor[];                // we disabled the write access to se3 elements to avoid inconsistencied in the representation.
+				  // people like to introduce inconsistencies in the representation.
+
+SE2 class:
+Vector2d& SE2::translation();     // use SE2::setTranslation(...) instead. 
+Rotation2Dd& SE2::rotation();     // use SE2::setRotation(...) instead. This ensures that the orientation is normalized
+double& opertor[];                // removed, same as in the list before.
+
+
+*PARAMETERS*
+It is possible to add parameters blocks in a graph.
+Parameters are be quantities that are fixed during the optimization,
+like the offset of a sensor or the intrinsics of a camera.
+
+Parameters are identified by a unique int id.
+See the Parameters class in optimizable_graph.
+
+
+If you want to create a parameter block for a graph you have to
+
+1) extend the class Parameters
+
+class MyParams: public Parameters {
+      ...
+      ...
+      virtual void read(istream& os) {
+      	      //implement here your read function;
+      }
+
+      virtual void write(ostream& os) {
+      	      //implement here your write function;
+      }
+
+}
+
+2) register the parameters in the metatype system
+
+factory->registerType("MY_PARAMS", new HyperGraphElementCreator<MyParams>);
+The parameters are always saved at the beginning of a graph file
+
+
+3) to insert parameters in a graph you have to
+ 
+// create the parameters
+MyParams* p=new MyParams();
+
+// set an id
+p->setId(0);
+
+// add them to the graph
+if (opt->addParameters(p)){
+   cerr << "success" << endl;
+} else {
+   cerr << "fail" << endl;
+}
+
+4) to access the parameters by id you can use the 
+OptimizableGraph::parameters(int id) function;
+
+MyParams* p = dynamic_cast<MyParams*>opt->(parameters(id));
+
+
+*CACHE*
+
+   A cache is some structure that contains intermediate 
+   calculations that depend on the estimate stored in a vertex.
+   Cache stores some intermediate result that would be computer
+   over and over again, during the computeError() of edges that involve the same vertex.
+   g2o now supports the caches, however it is good practice to use them
+   only when an initial system is running well, to get faster computation.
+
+   For instance a cache of an SE3 can contain a rotation matrix, 
+   its opposite, a useful quantity to compute the Jacobian and so on.
+
+   Each vertex can have zero or more cache blocks, 
+   whose pointers are stored in a vector within the vertex 
+   (in _cacheVector).
+   Thus each cache block is indexed within a vertex by a unique 
+   id   (VertexCache::_id), that corresponds to its position in the
+   _cacheVector.
+
+   Cache blocks are dynamic, in the sense that they are created 
+   only by those edges that require them, when they are inserted 
+   in a graph (addEdge.) 
+   To construct a cache, an edge should:
+   -  tell which cache_id is associated to each of the 
+      connected vertices.  These ids are stored in a vector 
+      stored inside the edge (_cacheIds).
+   - implement a function that "creates" a cache for a certain vertex
+     VertexCache* createCache(int vertexNum, OptimizableGraph* g).
+
+
+   So in short, if you want to use a cache you should:
+   1) Extend the VertexCache class to do the right things
+      In this class you have to implement the update() method,
+      that will be called whenever the estimate of the 
+      corresponding vertex changes.
+
+   2) In an edge where you want to use a cache, you should
+      define a createCache(int vertexNum) function that creates a 
+      new instance of cache for the vertex at position 
+      vertexNum withing Edge::_vertices.
+
+   3) Before inserting an edge that uses a cache in a graph,
+      you should tell the system which cache id is associated to 
+      each vertex of the edge. This is done by filling the _cacheIds
+      vector with the ids. An id of -1 means no cache.
+   
+   4) Each method that changes the estimate of a vertex 
+      (e.g. oplus) should update the cache accordingly.
+
+
+As an example:
+
+class MyCache: public VertexCache{
+	MyCache(Vertex*v, int id): VertexCache(v, id);	
+	virtual void update(){
+	// update here the cache 
+	//contents based on the vertex estimate
+	}
+};
+
+class EdgeThatUsesACache: public EdgeThatDoesNotUseACache{
+	EdgeThatUsesACache(): EdgeThatDoesNotUseACache{
+		// resize the cahce vector, all empty caches
+		_cacheIds.resize(vertices().size(), -1);
+	}
+	
+	
+	virtual VertexCache* createCache(int vertexNum) {
+		OptimizableGraph::Vertex*v 
+		= (OptimizableGraph::Vertex*)vertices()[vertexNum];
+		
+		if (vertexNum==0){	
+			return new MyCache(v, _cacheIds[0]);
+		}
+		if (vertexNum==1){
+			return new AnotherCache(v, _cacheIds[1]);
+		}
+		....
+	}
+
+	void computeError() {
+		// get the cache of the first vertex
+		OptimizableGraph::Vertex*v = 	
+		  (OptimizableGraph::Vertex*v) 
+		     vertices[0];
+		MyCache*  c1= 
+		  static_cast<MyCache*>  
+		  (v->getCache(_cacheIds[0]));
+
+		// do things with the cache....
+
+	}
+};
+
+
+When inserting an edge that uses the cache in a graph, you should do the following actions *in sequence*:
+1) set the vertices in the edge
+
+EdgeThatUsesACache e = new EdgeThatUsesACache();
+e->vertices[0]=v0;
+e->vertices[1]=v1;
+...
+e->vertices[n]=vn;
+
+
+2) assign a cache ID to each vertex in the edge that has a cache that is used for that edge. *The cacheId should be unique for each type of cache, and as small as possible*.
+
+e->setCacheId(0,first_cache_id);
+e->setCacheId(1,second_cache_id);
+e->setCacheId(2,third_cache_id);
+
+3) Add the edge in the graph. This will take care of all necessary bookeeping.
+
+g.addEdge(e);
+
+*meaning of the cache_id*
+The cache id is the position of a cache in the _vertexCache vector.
+If you have multiple edges that leave from a vertex and use the same cache id,
+only one cache block will be allocated for that id, and it will be shared among
+all edges. 
+
+
+CACHE VERY IMPORTANT
+Any vertex type that uses a cache should call the updateCache() function in each method that can change the estimate.
+These include, for instance (oplus(...), setEstimate(...)
+
+For instance
+
+class MyVertexThatUsesACache {
+      //
+      void oplus(double* u){
+      	   // do the oplus things here
+	   
+	   updateCache(); // update the cache vector when the estimate is changes
+      }
+
+      ...
+};

workspace/slam_in_autonomous_driving/thirdparty/g2o/doc/doxygen/readme.txt

@@ -0,0 +1,5 @@
+run the following in the current folder
+
+doxygen doxy.config
+
+assumes that dot tool is installed and available from path

workspace/slam_in_autonomous_driving/thirdparty/g2o/doc/license-bsd.txt

@@ -0,0 +1,26 @@
+g2o - General Graph Optimization
+Copyright (C) 2011 Rainer Kuemmerle, Giorgio Grisetti, Hauke Strasdat,
+Kurt Konolige, and Wolfram Burgard
+All rights reserved.
+
+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.
+
+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
+HOLDER 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.

workspace/slam_in_autonomous_driving/thirdparty/g2o/doc/license-gpl.txt

@@ -0,0 +1,674 @@
+                    GNU GENERAL PUBLIC LICENSE
+                       Version 3, 29 June 2007
+
+ Copyright (C) 2007 Free Software Foundation, Inc. <http://fsf.org/>
+ Everyone is permitted to copy and distribute verbatim copies
+ of this license document, but changing it is not allowed.
+
+                            Preamble
+
+  The GNU General Public License is a free, copyleft license for
+software and other kinds of works.
+
+  The licenses for most software and other practical works are designed
+to take away your freedom to share and change the works.  By contrast,
+the GNU General Public License is intended to guarantee your freedom to
+share and change all versions of a program--to make sure it remains free
+software for all its users.  We, the Free Software Foundation, use the
+GNU General Public License for most of our software; it applies also to
+any other work released this way by its authors.  You can apply it to
+your programs, too.
+
+  When we speak of free software, we are referring to freedom, not
+price.  Our General Public Licenses are designed to make sure that you
+have the freedom to distribute copies of free software (and charge for
+them if you wish), that you receive source code or can get it if you
+want it, that you can change the software or use pieces of it in new
+free programs, and that you know you can do these things.
+
+  To protect your rights, we need to prevent others from denying you
+these rights or asking you to surrender the rights.  Therefore, you have
+certain responsibilities if you distribute copies of the software, or if
+you modify it: responsibilities to respect the freedom of others.
+
+  For example, if you distribute copies of such a program, whether
+gratis or for a fee, you must pass on to the recipients the same
+freedoms that you received.  You must make sure that they, too, receive
+or can get the source code.  And you must show them these terms so they
+know their rights.
+
+  Developers that use the GNU GPL protect your rights with two steps:
+(1) assert copyright on the software, and (2) offer you this License
+giving you legal permission to copy, distribute and/or modify it.
+
+  For the developers' and authors' protection, the GPL clearly explains
+that there is no warranty for this free software.  For both users' and
+authors' sake, the GPL requires that modified versions be marked as
+changed, so that their problems will not be attributed erroneously to
+authors of previous versions.
+
+  Some devices are designed to deny users access to install or run
+modified versions of the software inside them, although the manufacturer
+can do so.  This is fundamentally incompatible with the aim of
+protecting users' freedom to change the software.  The systematic
+pattern of such abuse occurs in the area of products for individuals to
+use, which is precisely where it is most unacceptable.  Therefore, we
+have designed this version of the GPL to prohibit the practice for those
+products.  If such problems arise substantially in other domains, we
+stand ready to extend this provision to those domains in future versions
+of the GPL, as needed to protect the freedom of users.
+
+  Finally, every program is threatened constantly by software patents.
+States should not allow patents to restrict development and use of
+software on general-purpose computers, but in those that do, we wish to
+avoid the special danger that patents applied to a free program could
+make it effectively proprietary.  To prevent this, the GPL assures that
+patents cannot be used to render the program non-free.
+
+  The precise terms and conditions for copying, distribution and
+modification follow.
+
+                       TERMS AND CONDITIONS
+
+  0. Definitions.
+
+  "This License" refers to version 3 of the GNU General Public License.
+
+  "Copyright" also means copyright-like laws that apply to other kinds of
+works, such as semiconductor masks.
+
+  "The Program" refers to any copyrightable work licensed under this
+License.  Each licensee is addressed as "you".  "Licensees" and
+"recipients" may be individuals or organizations.
+
+  To "modify" a work means to copy from or adapt all or part of the work
+in a fashion requiring copyright permission, other than the making of an
+exact copy.  The resulting work is called a "modified version" of the
+earlier work or a work "based on" the earlier work.
+
+  A "covered work" means either the unmodified Program or a work based
+on the Program.
+
+  To "propagate" a work means to do anything with it that, without
+permission, would make you directly or secondarily liable for
+infringement under applicable copyright law, except executing it on a
+computer or modifying a private copy.  Propagation includes copying,
+distribution (with or without modification), making available to the
+public, and in some countries other activities as well.
+
+  To "convey" a work means any kind of propagation that enables other
+parties to make or receive copies.  Mere interaction with a user through
+a computer network, with no transfer of a copy, is not conveying.
+
+  An interactive user interface displays "Appropriate Legal Notices"
+to the extent that it includes a convenient and prominently visible
+feature that (1) displays an appropriate copyright notice, and (2)
+tells the user that there is no warranty for the work (except to the
+extent that warranties are provided), that licensees may convey the
+work under this License, and how to view a copy of this License.  If
+the interface presents a list of user commands or options, such as a
+menu, a prominent item in the list meets this criterion.
+
+  1. Source Code.
+
+  The "source code" for a work means the preferred form of the work
+for making modifications to it.  "Object code" means any non-source
+form of a work.
+
+  A "Standard Interface" means an interface that either is an official
+standard defined by a recognized standards body, or, in the case of
+interfaces specified for a particular programming language, one that
+is widely used among developers working in that language.
+
+  The "System Libraries" of an executable work include anything, other
+than the work as a whole, that (a) is included in the normal form of
+packaging a Major Component, but which is not part of that Major
+Component, and (b) serves only to enable use of the work with that
+Major Component, or to implement a Standard Interface for which an
+implementation is available to the public in source code form.  A
+"Major Component", in this context, means a major essential component
+(kernel, window system, and so on) of the specific operating system
+(if any) on which the executable work runs, or a compiler used to
+produce the work, or an object code interpreter used to run it.
+
+  The "Corresponding Source" for a work in object code form means all
+the source code needed to generate, install, and (for an executable
+work) run the object code and to modify the work, including scripts to
+control those activities.  However, it does not include the work's
+System Libraries, or general-purpose tools or generally available free
+programs which are used unmodified in performing those activities but
+which are not part of the work.  For example, Corresponding Source
+includes interface definition files associated with source files for
+the work, and the source code for shared libraries and dynamically
+linked subprograms that the work is specifically designed to require,
+such as by intimate data communication or control flow between those
+subprograms and other parts of the work.
+
+  The Corresponding Source need not include anything that users
+can regenerate automatically from other parts of the Corresponding
+Source.
+
+  The Corresponding Source for a work in source code form is that
+same work.
+
+  2. Basic Permissions.
+
+  All rights granted under this License are granted for the term of
+copyright on the Program, and are irrevocable provided the stated
+conditions are met.  This License explicitly affirms your unlimited
+permission to run the unmodified Program.  The output from running a
+covered work is covered by this License only if the output, given its
+content, constitutes a covered work.  This License acknowledges your
+rights of fair use or other equivalent, as provided by copyright law.
+
+  You may make, run and propagate covered works that you do not
+convey, without conditions so long as your license otherwise remains
+in force.  You may convey covered works to others for the sole purpose
+of having them make modifications exclusively for you, or provide you
+with facilities for running those works, provided that you comply with
+the terms of this License in conveying all material for which you do
+not control copyright.  Those thus making or running the covered works
+for you must do so exclusively on your behalf, under your direction
+and control, on terms that prohibit them from making any copies of
+your copyrighted material outside their relationship with you.
+
+  Conveying under any other circumstances is permitted solely under
+the conditions stated below.  Sublicensing is not allowed; section 10
+makes it unnecessary.
+
+  3. Protecting Users' Legal Rights From Anti-Circumvention Law.
+
+  No covered work shall be deemed part of an effective technological
+measure under any applicable law fulfilling obligations under article
+11 of the WIPO copyright treaty adopted on 20 December 1996, or
+similar laws prohibiting or restricting circumvention of such
+measures.
+
+  When you convey a covered work, you waive any legal power to forbid
+circumvention of technological measures to the extent such circumvention
+is effected by exercising rights under this License with respect to
+the covered work, and you disclaim any intention to limit operation or
+modification of the work as a means of enforcing, against the work's
+users, your or third parties' legal rights to forbid circumvention of
+technological measures.
+
+  4. Conveying Verbatim Copies.
+
+  You may convey verbatim copies of the Program's source code as you
+receive it, in any medium, provided that you conspicuously and
+appropriately publish on each copy an appropriate copyright notice;
+keep intact all notices stating that this License and any
+non-permissive terms added in accord with section 7 apply to the code;
+keep intact all notices of the absence of any warranty; and give all
+recipients a copy of this License along with the Program.
+
+  You may charge any price or no price for each copy that you convey,
+and you may offer support or warranty protection for a fee.
+
+  5. Conveying Modified Source Versions.
+
+  You may convey a work based on the Program, or the modifications to
+produce it from the Program, in the form of source code under the
+terms of section 4, provided that you also meet all of these conditions:
+
+    a) The work must carry prominent notices stating that you modified
+    it, and giving a relevant date.
+
+    b) The work must carry prominent notices stating that it is
+    released under this License and any conditions added under section
+    7.  This requirement modifies the requirement in section 4 to
+    "keep intact all notices".
+
+    c) You must license the entire work, as a whole, under this
+    License to anyone who comes into possession of a copy.  This
+    License will therefore apply, along with any applicable section 7
+    additional terms, to the whole of the work, and all its parts,
+    regardless of how they are packaged.  This License gives no
+    permission to license the work in any other way, but it does not
+    invalidate such permission if you have separately received it.
+
+    d) If the work has interactive user interfaces, each must display
+    Appropriate Legal Notices; however, if the Program has interactive
+    interfaces that do not display Appropriate Legal Notices, your
+    work need not make them do so.
+
+  A compilation of a covered work with other separate and independent
+works, which are not by their nature extensions of the covered work,
+and which are not combined with it such as to form a larger program,
+in or on a volume of a storage or distribution medium, is called an
+"aggregate" if the compilation and its resulting copyright are not
+used to limit the access or legal rights of the compilation's users
+beyond what the individual works permit.  Inclusion of a covered work
+in an aggregate does not cause this License to apply to the other
+parts of the aggregate.
+
+  6. Conveying Non-Source Forms.
+
+  You may convey a covered work in object code form under the terms
+of sections 4 and 5, provided that you also convey the
+machine-readable Corresponding Source under the terms of this License,
+in one of these ways:
+
+    a) Convey the object code in, or embodied in, a physical product
+    (including a physical distribution medium), accompanied by the
+    Corresponding Source fixed on a durable physical medium
+    customarily used for software interchange.
+
+    b) Convey the object code in, or embodied in, a physical product
+    (including a physical distribution medium), accompanied by a
+    written offer, valid for at least three years and valid for as
+    long as you offer spare parts or customer support for that product
+    model, to give anyone who possesses the object code either (1) a
+    copy of the Corresponding Source for all the software in the
+    product that is covered by this License, on a durable physical
+    medium customarily used for software interchange, for a price no
+    more than your reasonable cost of physically performing this
+    conveying of source, or (2) access to copy the
+    Corresponding Source from a network server at no charge.
+
+    c) Convey individual copies of the object code with a copy of the
+    written offer to provide the Corresponding Source.  This
+    alternative is allowed only occasionally and noncommercially, and
+    only if you received the object code with such an offer, in accord
+    with subsection 6b.
+
+    d) Convey the object code by offering access from a designated
+    place (gratis or for a charge), and offer equivalent access to the
+    Corresponding Source in the same way through the same place at no
+    further charge.  You need not require recipients to copy the
+    Corresponding Source along with the object code.  If the place to
+    copy the object code is a network server, the Corresponding Source
+    may be on a different server (operated by you or a third party)
+    that supports equivalent copying facilities, provided you maintain
+    clear directions next to the object code saying where to find the
+    Corresponding Source.  Regardless of what server hosts the
+    Corresponding Source, you remain obligated to ensure that it is
+    available for as long as needed to satisfy these requirements.
+
+    e) Convey the object code using peer-to-peer transmission, provided
+    you inform other peers where the object code and Corresponding
+    Source of the work are being offered to the general public at no
+    charge under subsection 6d.
+
+  A separable portion of the object code, whose source code is excluded
+from the Corresponding Source as a System Library, need not be
+included in conveying the object code work.
+
+  A "User Product" is either (1) a "consumer product", which means any
+tangible personal property which is normally used for personal, family,
+or household purposes, or (2) anything designed or sold for incorporation
+into a dwelling.  In determining whether a product is a consumer product,
+doubtful cases shall be resolved in favor of coverage.  For a particular
+product received by a particular user, "normally used" refers to a
+typical or common use of that class of product, regardless of the status
+of the particular user or of the way in which the particular user
+actually uses, or expects or is expected to use, the product.  A product
+is a consumer product regardless of whether the product has substantial
+commercial, industrial or non-consumer uses, unless such uses represent
+the only significant mode of use of the product.
+
+  "Installation Information" for a User Product means any methods,
+procedures, authorization keys, or other information required to install
+and execute modified versions of a covered work in that User Product from
+a modified version of its Corresponding Source.  The information must
+suffice to ensure that the continued functioning of the modified object
+code is in no case prevented or interfered with solely because
+modification has been made.
+
+  If you convey an object code work under this section in, or with, or
+specifically for use in, a User Product, and the conveying occurs as
+part of a transaction in which the right of possession and use of the
+User Product is transferred to the recipient in perpetuity or for a
+fixed term (regardless of how the transaction is characterized), the
+Corresponding Source conveyed under this section must be accompanied
+by the Installation Information.  But this requirement does not apply
+if neither you nor any third party retains the ability to install
+modified object code on the User Product (for example, the work has
+been installed in ROM).
+
+  The requirement to provide Installation Information does not include a
+requirement to continue to provide support service, warranty, or updates
+for a work that has been modified or installed by the recipient, or for
+the User Product in which it has been modified or installed.  Access to a
+network may be denied when the modification itself materially and
+adversely affects the operation of the network or violates the rules and
+protocols for communication across the network.
+
+  Corresponding Source conveyed, and Installation Information provided,
+in accord with this section must be in a format that is publicly
+documented (and with an implementation available to the public in
+source code form), and must require no special password or key for
+unpacking, reading or copying.
+
+  7. Additional Terms.
+
+  "Additional permissions" are terms that supplement the terms of this
+License by making exceptions from one or more of its conditions.
+Additional permissions that are applicable to the entire Program shall
+be treated as though they were included in this License, to the extent
+that they are valid under applicable law.  If additional permissions
+apply only to part of the Program, that part may be used separately
+under those permissions, but the entire Program remains governed by
+this License without regard to the additional permissions.
+
+  When you convey a copy of a covered work, you may at your option
+remove any additional permissions from that copy, or from any part of
+it.  (Additional permissions may be written to require their own
+removal in certain cases when you modify the work.)  You may place
+additional permissions on material, added by you to a covered work,
+for which you have or can give appropriate copyright permission.
+
+  Notwithstanding any other provision of this License, for material you
+add to a covered work, you may (if authorized by the copyright holders of
+that material) supplement the terms of this License with terms:
+
+    a) Disclaiming warranty or limiting liability differently from the
+    terms of sections 15 and 16 of this License; or
+
+    b) Requiring preservation of specified reasonable legal notices or
+    author attributions in that material or in the Appropriate Legal
+    Notices displayed by works containing it; or
+
+    c) Prohibiting misrepresentation of the origin of that material, or
+    requiring that modified versions of such material be marked in
+    reasonable ways as different from the original version; or
+
+    d) Limiting the use for publicity purposes of names of licensors or
+    authors of the material; or
+
+    e) Declining to grant rights under trademark law for use of some
+    trade names, trademarks, or service marks; or
+
+    f) Requiring indemnification of licensors and authors of that
+    material by anyone who conveys the material (or modified versions of
+    it) with contractual assumptions of liability to the recipient, for
+    any liability that these contractual assumptions directly impose on
+    those licensors and authors.
+
+  All other non-permissive additional terms are considered "further
+restrictions" within the meaning of section 10.  If the Program as you
+received it, or any part of it, contains a notice stating that it is
+governed by this License along with a term that is a further
+restriction, you may remove that term.  If a license document contains
+a further restriction but permits relicensing or conveying under this
+License, you may add to a covered work material governed by the terms
+of that license document, provided that the further restriction does
+not survive such relicensing or conveying.
+
+  If you add terms to a covered work in accord with this section, you
+must place, in the relevant source files, a statement of the
+additional terms that apply to those files, or a notice indicating
+where to find the applicable terms.
+
+  Additional terms, permissive or non-permissive, may be stated in the
+form of a separately written license, or stated as exceptions;
+the above requirements apply either way.
+
+  8. Termination.
+
+  You may not propagate or modify a covered work except as expressly
+provided under this License.  Any attempt otherwise to propagate or
+modify it is void, and will automatically terminate your rights under
+this License (including any patent licenses granted under the third
+paragraph of section 11).
+
+  However, if you cease all violation of this License, then your
+license from a particular copyright holder is reinstated (a)
+provisionally, unless and until the copyright holder explicitly and
+finally terminates your license, and (b) permanently, if the copyright
+holder fails to notify you of the violation by some reasonable means
+prior to 60 days after the cessation.
+
+  Moreover, your license from a particular copyright holder is
+reinstated permanently if the copyright holder notifies you of the
+violation by some reasonable means, this is the first time you have
+received notice of violation of this License (for any work) from that
+copyright holder, and you cure the violation prior to 30 days after
+your receipt of the notice.
+
+  Termination of your rights under this section does not terminate the
+licenses of parties who have received copies or rights from you under
+this License.  If your rights have been terminated and not permanently
+reinstated, you do not qualify to receive new licenses for the same
+material under section 10.
+
+  9. Acceptance Not Required for Having Copies.
+
+  You are not required to accept this License in order to receive or
+run a copy of the Program.  Ancillary propagation of a covered work
+occurring solely as a consequence of using peer-to-peer transmission
+to receive a copy likewise does not require acceptance.  However,
+nothing other than this License grants you permission to propagate or
+modify any covered work.  These actions infringe copyright if you do
+not accept this License.  Therefore, by modifying or propagating a
+covered work, you indicate your acceptance of this License to do so.
+
+  10. Automatic Licensing of Downstream Recipients.
+
+  Each time you convey a covered work, the recipient automatically
+receives a license from the original licensors, to run, modify and
+propagate that work, subject to this License.  You are not responsible
+for enforcing compliance by third parties with this License.
+
+  An "entity transaction" is a transaction transferring control of an
+organization, or substantially all assets of one, or subdividing an
+organization, or merging organizations.  If propagation of a covered
+work results from an entity transaction, each party to that
+transaction who receives a copy of the work also receives whatever
+licenses to the work the party's predecessor in interest had or could
+give under the previous paragraph, plus a right to possession of the
+Corresponding Source of the work from the predecessor in interest, if
+the predecessor has it or can get it with reasonable efforts.
+
+  You may not impose any further restrictions on the exercise of the
+rights granted or affirmed under this License.  For example, you may
+not impose a license fee, royalty, or other charge for exercise of
+rights granted under this License, and you may not initiate litigation
+(including a cross-claim or counterclaim in a lawsuit) alleging that
+any patent claim is infringed by making, using, selling, offering for
+sale, or importing the Program or any portion of it.
+
+  11. Patents.
+
+  A "contributor" is a copyright holder who authorizes use under this
+License of the Program or a work on which the Program is based.  The
+work thus licensed is called the contributor's "contributor version".
+
+  A contributor's "essential patent claims" are all patent claims
+owned or controlled by the contributor, whether already acquired or
+hereafter acquired, that would be infringed by some manner, permitted
+by this License, of making, using, or selling its contributor version,
+but do not include claims that would be infringed only as a
+consequence of further modification of the contributor version.  For
+purposes of this definition, "control" includes the right to grant
+patent sublicenses in a manner consistent with the requirements of
+this License.
+
+  Each contributor grants you a non-exclusive, worldwide, royalty-free
+patent license under the contributor's essential patent claims, to
+make, use, sell, offer for sale, import and otherwise run, modify and
+propagate the contents of its contributor version.
+
+  In the following three paragraphs, a "patent license" is any express
+agreement or commitment, however denominated, not to enforce a patent
+(such as an express permission to practice a patent or covenant not to
+sue for patent infringement).  To "grant" such a patent license to a
+party means to make such an agreement or commitment not to enforce a
+patent against the party.
+
+  If you convey a covered work, knowingly relying on a patent license,
+and the Corresponding Source of the work is not available for anyone
+to copy, free of charge and under the terms of this License, through a
+publicly available network server or other readily accessible means,
+then you must either (1) cause the Corresponding Source to be so
+available, or (2) arrange to deprive yourself of the benefit of the
+patent license for this particular work, or (3) arrange, in a manner
+consistent with the requirements of this License, to extend the patent
+license to downstream recipients.  "Knowingly relying" means you have
+actual knowledge that, but for the patent license, your conveying the
+covered work in a country, or your recipient's use of the covered work
+in a country, would infringe one or more identifiable patents in that
+country that you have reason to believe are valid.
+
+  If, pursuant to or in connection with a single transaction or
+arrangement, you convey, or propagate by procuring conveyance of, a
+covered work, and grant a patent license to some of the parties
+receiving the covered work authorizing them to use, propagate, modify
+or convey a specific copy of the covered work, then the patent license
+you grant is automatically extended to all recipients of the covered
+work and works based on it.
+
+  A patent license is "discriminatory" if it does not include within
+the scope of its coverage, prohibits the exercise of, or is
+conditioned on the non-exercise of one or more of the rights that are
+specifically granted under this License.  You may not convey a covered
+work if you are a party to an arrangement with a third party that is
+in the business of distributing software, under which you make payment
+to the third party based on the extent of your activity of conveying
+the work, and under which the third party grants, to any of the
+parties who would receive the covered work from you, a discriminatory
+patent license (a) in connection with copies of the covered work
+conveyed by you (or copies made from those copies), or (b) primarily
+for and in connection with specific products or compilations that
+contain the covered work, unless you entered into that arrangement,
+or that patent license was granted, prior to 28 March 2007.
+
+  Nothing in this License shall be construed as excluding or limiting
+any implied license or other defenses to infringement that may
+otherwise be available to you under applicable patent law.
+
+  12. No Surrender of Others' Freedom.
+
+  If conditions are imposed on you (whether by court order, agreement or
+otherwise) that contradict the conditions of this License, they do not
+excuse you from the conditions of this License.  If you cannot convey a
+covered work so as to satisfy simultaneously your obligations under this
+License and any other pertinent obligations, then as a consequence you may
+not convey it at all.  For example, if you agree to terms that obligate you
+to collect a royalty for further conveying from those to whom you convey
+the Program, the only way you could satisfy both those terms and this
+License would be to refrain entirely from conveying the Program.
+
+  13. Use with the GNU Affero General Public License.
+
+  Notwithstanding any other provision of this License, you have
+permission to link or combine any covered work with a work licensed
+under version 3 of the GNU Affero General Public License into a single
+combined work, and to convey the resulting work.  The terms of this
+License will continue to apply to the part which is the covered work,
+but the special requirements of the GNU Affero General Public License,
+section 13, concerning interaction through a network will apply to the
+combination as such.
+
+  14. Revised Versions of this License.
+
+  The Free Software Foundation may publish revised and/or new versions of
+the GNU General Public License from time to time.  Such new versions will
+be similar in spirit to the present version, but may differ in detail to
+address new problems or concerns.
+
+  Each version is given a distinguishing version number.  If the
+Program specifies that a certain numbered version of the GNU General
+Public License "or any later version" applies to it, you have the
+option of following the terms and conditions either of that numbered
+version or of any later version published by the Free Software
+Foundation.  If the Program does not specify a version number of the
+GNU General Public License, you may choose any version ever published
+by the Free Software Foundation.
+
+  If the Program specifies that a proxy can decide which future
+versions of the GNU General Public License can be used, that proxy's
+public statement of acceptance of a version permanently authorizes you
+to choose that version for the Program.
+
+  Later license versions may give you additional or different
+permissions.  However, no additional obligations are imposed on any
+author or copyright holder as a result of your choosing to follow a
+later version.
+
+  15. Disclaimer of Warranty.
+
+  THERE IS NO WARRANTY FOR THE PROGRAM, TO THE EXTENT PERMITTED BY
+APPLICABLE LAW.  EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT
+HOLDERS AND/OR OTHER PARTIES PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY
+OF ANY KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO,
+THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+PURPOSE.  THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE PROGRAM
+IS WITH YOU.  SHOULD THE PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF
+ALL NECESSARY SERVICING, REPAIR OR CORRECTION.
+
+  16. Limitation of Liability.
+
+  IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING
+WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MODIFIES AND/OR CONVEYS
+THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, INCLUDING ANY
+GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING OUT OF THE
+USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED TO LOSS OF
+DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD
+PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER PROGRAMS),
+EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF
+SUCH DAMAGES.
+
+  17. Interpretation of Sections 15 and 16.
+
+  If the disclaimer of warranty and limitation of liability provided
+above cannot be given local legal effect according to their terms,
+reviewing courts shall apply local law that most closely approximates
+an absolute waiver of all civil liability in connection with the
+Program, unless a warranty or assumption of liability accompanies a
+copy of the Program in return for a fee.
+
+                     END OF TERMS AND CONDITIONS
+
+            How to Apply These Terms to Your New Programs
+
+  If you develop a new program, and you want it to be of the greatest
+possible use to the public, the best way to achieve this is to make it
+free software which everyone can redistribute and change under these terms.
+
+  To do so, attach the following notices to the program.  It is safest
+to attach them to the start of each source file to most effectively
+state the exclusion of warranty; and each file should have at least
+the "copyright" line and a pointer to where the full notice is found.
+
+    <one line to give the program's name and a brief idea of what it does.>
+    Copyright (C) <year>  <name of author>
+
+    This program is free software: you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation, either version 3 of the License, or
+    (at your option) any later version.
+
+    This program is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with this program.  If not, see <http://www.gnu.org/licenses/>.
+
+Also add information on how to contact you by electronic and paper mail.
+
+  If the program does terminal interaction, make it output a short
+notice like this when it starts in an interactive mode:
+
+    <program>  Copyright (C) <year>  <name of author>
+    This program comes with ABSOLUTELY NO WARRANTY; for details type `show w'.
+    This is free software, and you are welcome to redistribute it
+    under certain conditions; type `show c' for details.
+
+The hypothetical commands `show w' and `show c' should show the appropriate
+parts of the General Public License.  Of course, your program's commands
+might be different; for a GUI interface, you would use an "about box".
+
+  You should also get your employer (if you work as a programmer) or school,
+if any, to sign a "copyright disclaimer" for the program, if necessary.
+For more information on this, and how to apply and follow the GNU GPL, see
+<http://www.gnu.org/licenses/>.
+
+  The GNU General Public License does not permit incorporating your program
+into proprietary programs.  If your program is a subroutine library, you
+may consider it more useful to permit linking proprietary applications with
+the library.  If this is what you want to do, use the GNU Lesser General
+Public License instead of this License.  But first, please read
+<http://www.gnu.org/philosophy/why-not-lgpl.html>.

workspace/slam_in_autonomous_driving/thirdparty/g2o/doc/license-lgpl.txt

@@ -0,0 +1,165 @@
+		   GNU LESSER GENERAL PUBLIC LICENSE
+                       Version 3, 29 June 2007
+
+ Copyright (C) 2007 Free Software Foundation, Inc. <http://fsf.org/>
+ Everyone is permitted to copy and distribute verbatim copies
+ of this license document, but changing it is not allowed.
+
+
+  This version of the GNU Lesser General Public License incorporates
+the terms and conditions of version 3 of the GNU General Public
+License, supplemented by the additional permissions listed below.
+
+  0. Additional Definitions.
+
+  As used herein, "this License" refers to version 3 of the GNU Lesser
+General Public License, and the "GNU GPL" refers to version 3 of the GNU
+General Public License.
+
+  "The Library" refers to a covered work governed by this License,
+other than an Application or a Combined Work as defined below.
+
+  An "Application" is any work that makes use of an interface provided
+by the Library, but which is not otherwise based on the Library.
+Defining a subclass of a class defined by the Library is deemed a mode
+of using an interface provided by the Library.
+
+  A "Combined Work" is a work produced by combining or linking an
+Application with the Library.  The particular version of the Library
+with which the Combined Work was made is also called the "Linked
+Version".
+
+  The "Minimal Corresponding Source" for a Combined Work means the
+Corresponding Source for the Combined Work, excluding any source code
+for portions of the Combined Work that, considered in isolation, are
+based on the Application, and not on the Linked Version.
+
+  The "Corresponding Application Code" for a Combined Work means the
+object code and/or source code for the Application, including any data
+and utility programs needed for reproducing the Combined Work from the
+Application, but excluding the System Libraries of the Combined Work.
+
+  1. Exception to Section 3 of the GNU GPL.
+
+  You may convey a covered work under sections 3 and 4 of this License
+without being bound by section 3 of the GNU GPL.
+
+  2. Conveying Modified Versions.
+
+  If you modify a copy of the Library, and, in your modifications, a
+facility refers to a function or data to be supplied by an Application
+that uses the facility (other than as an argument passed when the
+facility is invoked), then you may convey a copy of the modified
+version:
+
+   a) under this License, provided that you make a good faith effort to
+   ensure that, in the event an Application does not supply the
+   function or data, the facility still operates, and performs
+   whatever part of its purpose remains meaningful, or
+
+   b) under the GNU GPL, with none of the additional permissions of
+   this License applicable to that copy.
+
+  3. Object Code Incorporating Material from Library Header Files.
+
+  The object code form of an Application may incorporate material from
+a header file that is part of the Library.  You may convey such object
+code under terms of your choice, provided that, if the incorporated
+material is not limited to numerical parameters, data structure
+layouts and accessors, or small macros, inline functions and templates
+(ten or fewer lines in length), you do both of the following:
+
+   a) Give prominent notice with each copy of the object code that the
+   Library is used in it and that the Library and its use are
+   covered by this License.
+
+   b) Accompany the object code with a copy of the GNU GPL and this license
+   document.
+
+  4. Combined Works.
+
+  You may convey a Combined Work under terms of your choice that,
+taken together, effectively do not restrict modification of the
+portions of the Library contained in the Combined Work and reverse
+engineering for debugging such modifications, if you also do each of
+the following:
+
+   a) Give prominent notice with each copy of the Combined Work that
+   the Library is used in it and that the Library and its use are
+   covered by this License.
+
+   b) Accompany the Combined Work with a copy of the GNU GPL and this license
+   document.
+
+   c) For a Combined Work that displays copyright notices during
+   execution, include the copyright notice for the Library among
+   these notices, as well as a reference directing the user to the
+   copies of the GNU GPL and this license document.
+
+   d) Do one of the following:
+
+       0) Convey the Minimal Corresponding Source under the terms of this
+       License, and the Corresponding Application Code in a form
+       suitable for, and under terms that permit, the user to
+       recombine or relink the Application with a modified version of
+       the Linked Version to produce a modified Combined Work, in the
+       manner specified by section 6 of the GNU GPL for conveying
+       Corresponding Source.
+
+       1) Use a suitable shared library mechanism for linking with the
+       Library.  A suitable mechanism is one that (a) uses at run time
+       a copy of the Library already present on the user's computer
+       system, and (b) will operate properly with a modified version
+       of the Library that is interface-compatible with the Linked
+       Version.
+
+   e) Provide Installation Information, but only if you would otherwise
+   be required to provide such information under section 6 of the
+   GNU GPL, and only to the extent that such information is
+   necessary to install and execute a modified version of the
+   Combined Work produced by recombining or relinking the
+   Application with a modified version of the Linked Version. (If
+   you use option 4d0, the Installation Information must accompany
+   the Minimal Corresponding Source and Corresponding Application
+   Code. If you use option 4d1, you must provide the Installation
+   Information in the manner specified by section 6 of the GNU GPL
+   for conveying Corresponding Source.)
+
+  5. Combined Libraries.
+
+  You may place library facilities that are a work based on the
+Library side by side in a single library together with other library
+facilities that are not Applications and are not covered by this
+License, and convey such a combined library under terms of your
+choice, if you do both of the following:
+
+   a) Accompany the combined library with a copy of the same work based
+   on the Library, uncombined with any other library facilities,
+   conveyed under the terms of this License.
+
+   b) Give prominent notice with the combined library that part of it
+   is a work based on the Library, and explaining where to find the
+   accompanying uncombined form of the same work.
+
+  6. Revised Versions of the GNU Lesser General Public License.
+
+  The Free Software Foundation may publish revised and/or new versions
+of the GNU Lesser General Public License from time to time. Such new
+versions will be similar in spirit to the present version, but may
+differ in detail to address new problems or concerns.
+
+  Each version is given a distinguishing version number. If the
+Library as you received it specifies that a certain numbered version
+of the GNU Lesser General Public License "or any later version"
+applies to it, you have the option of following the terms and
+conditions either of that published version or of any later version
+published by the Free Software Foundation. If the Library as you
+received it does not specify a version number of the GNU Lesser
+General Public License, you may choose any version of the GNU Lesser
+General Public License ever published by the Free Software Foundation.
+
+  If the Library as you received it specifies that a proxy can decide
+whether future versions of the GNU Lesser General Public License shall
+apply, that proxy's public statement of acceptance of any version is
+permanent authorization for you to choose that version for the
+Library.