added initial working code

This commit is contained in:
yitoh 2016-10-07 13:31:30 +09:00
parent 03c98c1b7e
commit e5dd9fcd30
445 changed files with 111655 additions and 0 deletions

99
cmake/FindEigen.cmake Normal file
View File

@ -0,0 +1,99 @@
# - Try to find Eigen lib
#
# This module supports requiring a minimum version, e.g. you can do
# find_package(Eigen 3.1.2)
# to require version 3.1.2 or newer of Eigen.
#
# Once done this will define
#
# EIGEN_FOUND - system has eigen lib with correct version
# EIGEN_INCLUDE_DIR - the eigen include directory
# EIGEN_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>
# Copyright (c) 2014 Lech Swirski <leszek@swirski.co.uk>
# Redistribution and use is allowed according to the terms of the 2-clause BSD license.
if(NOT Eigen_FIND_VERSION)
if(NOT Eigen_FIND_VERSION_MAJOR)
set(Eigen_FIND_VERSION_MAJOR 3)
endif(NOT Eigen_FIND_VERSION_MAJOR)
if(NOT Eigen_FIND_VERSION_MINOR)
set(Eigen_FIND_VERSION_MINOR 0)
endif(NOT Eigen_FIND_VERSION_MINOR)
if(NOT Eigen_FIND_VERSION_PATCH)
set(Eigen_FIND_VERSION_PATCH 0)
endif(NOT Eigen_FIND_VERSION_PATCH)
set(Eigen_FIND_VERSION "${Eigen_FIND_VERSION_MAJOR}.${Eigen_FIND_VERSION_MINOR}.${Eigen_FIND_VERSION_PATCH}")
endif(NOT Eigen_FIND_VERSION)
macro(_eigen_check_version)
find_file(_macros_header "Macros.h"
HINTS ${EIGEN_INCLUDE_DIR}
PATH_SUFFIXES "Eigen/src/Core/util" "src/Core/util")
message(STATUS "Found Eigen header: ${_macros_header}")
file(READ ${_macros_header} _eigen_version_header)
string(REGEX MATCH "define[ \t]+EIGEN_WORLD_VERSION[ \t]+([0-9]+)" _eigen_world_version_match "${_eigen_version_header}")
set(EIGEN_WORLD_VERSION "${CMAKE_MATCH_1}")
string(REGEX MATCH "define[ \t]+EIGEN_MAJOR_VERSION[ \t]+([0-9]+)" _eigen_major_version_match "${_eigen_version_header}")
set(EIGEN_MAJOR_VERSION "${CMAKE_MATCH_1}")
string(REGEX MATCH "define[ \t]+EIGEN_MINOR_VERSION[ \t]+([0-9]+)" _eigen_minor_version_match "${_eigen_version_header}")
set(EIGEN_MINOR_VERSION "${CMAKE_MATCH_1}")
set(EIGEN_VERSION ${EIGEN_WORLD_VERSION}.${EIGEN_MAJOR_VERSION}.${EIGEN_MINOR_VERSION})
if(${EIGEN_VERSION} VERSION_LESS ${Eigen_FIND_VERSION})
set(EIGEN_VERSION_OK FALSE)
else(${EIGEN_VERSION} VERSION_LESS ${Eigen_FIND_VERSION})
set(EIGEN_VERSION_OK TRUE)
endif(${EIGEN_VERSION} VERSION_LESS ${Eigen_FIND_VERSION})
if(NOT EIGEN_VERSION_OK)
message(STATUS "Eigen version ${EIGEN_VERSION} found in ${EIGEN_INCLUDE_DIR}, "
"but at least version ${Eigen_FIND_VERSION} is required")
endif(NOT EIGEN_VERSION_OK)
endmacro(_eigen_check_version)
if (EIGEN_INCLUDE_DIR)
# in cache already
_eigen_check_version()
set(EIGEN_FOUND ${EIGEN_VERSION_OK})
else (EIGEN_INCLUDE_DIR)
find_path(EIGEN_INCLUDE_DIR NAMES signature_of_eigen3_matrix_library
PATHS
${CMAKE_INSTALL_PREFIX}/include
${KDE4_INCLUDE_DIR}
/usr/local/include
/usr/local/homebrew/include # Mac OS X
/opt/local/var/macports/software # Mac OS X.
/opt/local/include
/usr/include
$ENV{PROGRAMFILES}
$ENV{PROGRAMFILES}/Eigen/include
$ENV{PROGRAMW6432}
$ENV{PROGRAMW6432}/Eigen/include
PATH_SUFFIXES eigen eigen3
)
if(EIGEN_INCLUDE_DIR)
_eigen_check_version()
endif(EIGEN_INCLUDE_DIR)
include(FindPackageHandleStandardArgs)
find_package_handle_standard_args(Eigen DEFAULT_MSG EIGEN_INCLUDE_DIR EIGEN_VERSION_OK)
mark_as_advanced(EIGEN_INCLUDE_DIR)
endif(EIGEN_INCLUDE_DIR)

80
cmake/FindEigen3.cmake Normal file
View File

@ -0,0 +1,80 @@
# - 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)
find_path(EIGEN3_INCLUDE_DIR NAMES signature_of_eigen3_matrix_library
PATHS
${CMAKE_INSTALL_PREFIX}/include
${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)

203
cmake/FindTBB.cmake Normal file
View File

@ -0,0 +1,203 @@
# Module for locating Intel's Threading Building Blocks (TBB).
#
# Customizable variables:
# TBB_ROOT_DIR
# Specifies TBB's root directory.
#
# Read-only variables:
# TBB_FOUND
# Indicates whether the library has been found.
#
# TBB_INCLUDE_DIRS
# Specifies TBB's include directory.
#
# TBB_LIBRARIES
# Specifies TBB libraries that should be passed to target_link_libararies.
#
# TBB_<COMPONENT>_LIBRARIES
# Specifies the libraries of a specific <COMPONENT>.
#
# TBB_<COMPONENT>_FOUND
# Indicates whether the specified <COMPONENT> was found.
#
#
# Copyright (c) 2012 Sergiu Dotenco
# Copyright (c) 2014 Patrick M. Niedzielski <PatrickNiedzielski@gmail.com>
#
# 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 PARTTBBLAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
# AUTHORS OR COPYRIGHT HOLDERS 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.
# 2014-02-17: modified by Patrick M. Niedzielski to add support for MSVC12.
INCLUDE (FindPackageHandleStandardArgs)
IF (CMAKE_VERSION VERSION_GREATER 2.8.7)
SET (_TBB_CHECK_COMPONENTS FALSE)
ELSE (CMAKE_VERSION VERSION_GREATER 2.8.7)
SET (_TBB_CHECK_COMPONENTS TRUE)
ENDIF (CMAKE_VERSION VERSION_GREATER 2.8.7)
FIND_PATH (TBB_ROOT_DIR
NAMES include/tbb/tbb.h
PATHS ENV TBBROOT
ENV TBB40_INSTALL_DIR
ENV TBB30_INSTALL_DIR
ENV TBB22_INSTALL_DIR
ENV TBB21_INSTALL_DIR
DOC "TBB root directory")
FIND_PATH (TBB_INCLUDE_DIR
NAMES tbb/tbb.h
HINTS ${TBB_ROOT_DIR}
PATH_SUFFIXES include
DOC "TBB include directory")
IF (MSVC14)
SET (_TBB_COMPILER vc14)
ELSEIF (MSVC12)
SET (_TBB_COMPILER vc12)
ELSEIF (MSVC11)
SET (_TBB_COMPILER vc11)
ELSEIF (MSVC10)
SET (_TBB_COMPILER vc10)
ELSEIF (MSVC90)
SET (_TBB_COMPILER vc9)
ELSEIF (MSVC80)
SET (_TBB_COMPILER vc8)
ELSEIF (WIN32)
SET (_TBB_COMPILER vc_mt)
ENDIF (MSVC14)
IF (CMAKE_SIZEOF_VOID_P EQUAL 8)
SET (_TBB_POSSIBLE_LIB_SUFFIXES lib/intel64/${_TBB_COMPILER})
ELSE (CMAKE_SIZEOF_VOID_P EQUAL 8)
SET (_TBB_POSSIBLE_LIB_SUFFIXES lib/ia32/${_TBB_COMPILER})
ENDIF (CMAKE_SIZEOF_VOID_P EQUAL 8)
LIST (APPEND _TBB_POSSIBLE_LIB_SUFFIXES lib/$ENV{TBB_ARCH_PLATFORM})
FIND_LIBRARY (TBB_LIBRARY_RELEASE
NAMES tbb
HINTS ${TBB_ROOT_DIR}
PATH_SUFFIXES ${_TBB_POSSIBLE_LIB_SUFFIXES}
DOC "TBB release library")
FIND_LIBRARY (TBB_LIBRARY_DEBUG
NAMES tbb_debug
HINTS ${TBB_ROOT_DIR}
PATH_SUFFIXES ${_TBB_POSSIBLE_LIB_SUFFIXES}
DOC "TBB debug library")
IF (TBB_LIBRARY_RELEASE AND TBB_LIBRARY_DEBUG)
SET (TBB_LIBRARY optimized ${TBB_LIBRARY_RELEASE} debug ${TBB_LIBRARY_DEBUG}
CACHE DOC "TBB library")
ELSEIF (TBB_LIBRARY_RELEASE)
SET (TBB_LIBRARY ${TBB_LIBRARY_RELEASE} CACHE DOC "TBB library")
ENDIF (TBB_LIBRARY_RELEASE AND TBB_LIBRARY_DEBUG)
FOREACH (_TBB_COMPONENT ${TBB_FIND_COMPONENTS})
STRING (TOUPPER ${_TBB_COMPONENT} _TBB_COMPONENT_UPPER)
SET (_TBB_LIBRARY_BASE TBB_${_TBB_COMPONENT_UPPER}_LIBRARY)
IF (${_TBB_COMPONENT} STREQUAL preview)
SET (_TBB_LIBRARY_NAME tbb_${_TBB_COMPONENT})
ELSE (${_TBB_COMPONENT} STREQUAL preview)
SET (_TBB_LIBRARY_NAME tbb${_TBB_COMPONENT})
ENDIF (${_TBB_COMPONENT} STREQUAL preview)
FIND_LIBRARY (${_TBB_LIBRARY_BASE}_RELEASE
NAMES ${_TBB_LIBRARY_NAME}
HINTS ${TBB_ROOT_DIR}
PATH_SUFFIXES ${_TBB_POSSIBLE_LIB_SUFFIXES}
DOC "TBB ${_TBB_COMPONENT} release library")
FIND_LIBRARY (${_TBB_LIBRARY_BASE}_DEBUG
NAMES ${_TBB_LIBRARY_NAME}_debug
HINTS ${TBB_ROOT_DIR}
PATH_SUFFIXES ${_TBB_POSSIBLE_LIB_SUFFIXES}
DOC "TBB ${_TBB_COMPONENT} debug library")
MARK_AS_ADVANCED (${_TBB_LIBRARY_BASE} ${_TBB_LIBRARY_BASE}_DEBUG)
SET (TBB_${_TBB_COMPONENT_UPPER}_FOUND TRUE)
IF (${_TBB_LIBRARY_BASE}_DEBUG AND ${_TBB_LIBRARY_BASE}_RELEASE)
SET (${_TBB_LIBRARY_BASE}
debug ${${_TBB_LIBRARY_BASE}_DEBUG}
optimized ${${_TBB_LIBRARY_BASE}_RELEASE} CACHE DOC
"TBB ${_TBB_COMPONENT} library")
ELSEIF (${_TBB_LIBRARY_BASE}_DEBUG)
SET (${_TBB_LIBRARY_BASE} ${${_TBB_LIBRARY_BASE}_DEBUG})
ELSEIF (${_TBB_LIBRARY_BASE}_RELEASE)
SET (${_TBB_LIBRARY_BASE} ${${_TBB_LIBRARY_BASE}_RELEASE}
CACHE DOC "TBB ${_TBB_COMPONENT} library")
ELSE (${_TBB_LIBRARY_BASE}_DEBUG AND ${_TBB_LIBRARY_BASE}_RELEASE)
# Component missing: record it for a later report
LIST (APPEND _TBB_MISSING_COMPONENTS ${_TBB_COMPONENT})
SET (TBB_${_TBB_COMPONENT_UPPER}_FOUND FALSE)
ENDIF (${_TBB_LIBRARY_BASE}_DEBUG AND ${_TBB_LIBRARY_BASE}_RELEASE)
SET (TBB_${_TBB_COMPONENT}_FOUND ${TBB_${_TBB_COMPONENT_UPPER}_FOUND})
IF (${_TBB_LIBRARY_BASE})
# setup the TBB_<COMPONENT>_LIBRARIES variable
SET (TBB_${_TBB_COMPONENT_UPPER}_LIBRARIES ${${_TBB_LIBRARY_BASE}})
ELSE (${_TBB_LIBRARY_BASE})
LIST (APPEND _TBB_MISSING_LIBRARIES TBB_${_TBB_COMPONENT_UPPER}_LIBRARIES)
ENDIF (${_TBB_LIBRARY_BASE})
ENDFOREACH (_TBB_COMPONENT ${TBB_FIND_COMPONENTS})
SET (TBB_LIBRARIES ${TBB_LIBRARY})
SET (TBB_INCLUDE_DIRS ${TBB_INCLUDE_DIR})
IF (DEFINED _TBB_MISSING_COMPONENTS AND _TBB_CHECK_COMPONENTS)
IF (NOT TBB_FIND_QUIETLY)
MESSAGE (STATUS "One or more TBB components were not found:")
# Display missing components indented, each on a separate line
FOREACH (_TBB_MISSING_COMPONENT ${_TBB_MISSING_COMPONENTS})
MESSAGE (STATUS " " ${_TBB_MISSING_COMPONENT})
ENDFOREACH (_TBB_MISSING_COMPONENT ${_TBB_MISSING_COMPONENTS})
ENDIF (NOT TBB_FIND_QUIETLY)
ENDIF (DEFINED _TBB_MISSING_COMPONENTS AND _TBB_CHECK_COMPONENTS)
# Determine library's version
SET (_TBB_VERSION_HEADER ${TBB_INCLUDE_DIR}/tbb/tbb_stddef.h)
IF (EXISTS ${_TBB_VERSION_HEADER})
FILE (READ ${_TBB_VERSION_HEADER} _TBB_VERSION_CONTENTS)
STRING (REGEX REPLACE ".*#define TBB_VERSION_MAJOR[ \t]+([0-9]+).*" "\\1"
TBB_VERSION_MAJOR "${_TBB_VERSION_CONTENTS}")
STRING (REGEX REPLACE ".*#define TBB_VERSION_MINOR[ \t]+([0-9]+).*" "\\1"
TBB_VERSION_MINOR "${_TBB_VERSION_CONTENTS}")
SET (TBB_VERSION ${TBB_VERSION_MAJOR}.${TBB_VERSION_MINOR})
SET (TBB_VERSION_COMPONENTS 2)
ENDIF (EXISTS ${_TBB_VERSION_HEADER})
MARK_AS_ADVANCED (TBB_INCLUDE_DIR TBB_LIBRARY TBB_LIBRARY_RELEASE
TBB_LIBRARY_DEBUG)
IF (NOT _TBB_CHECK_COMPONENTS)
SET (_TBB_FPHSA_ADDITIONAL_ARGS HANDLE_COMPONENTS)
ENDIF (NOT _TBB_CHECK_COMPONENTS)
FIND_PACKAGE_HANDLE_STANDARD_ARGS (TBB REQUIRED_VARS TBB_ROOT_DIR
TBB_INCLUDE_DIR TBB_LIBRARY ${_TBB_MISSING_LIBRARIES} VERSION_VAR TBB_VERSION
${_TBB_FPHSA_ADDITIONAL_ARGS})

View File

@ -0,0 +1,304 @@
# 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.
#
# Authors: pablo.speciale@gmail.com (Pablo Speciale)
# alexs.mac@gmail.com (Alex Stewart)
#
# Config file for Ceres Solver - Find Ceres & dependencies.
#
# This file is used by CMake when find_package(Ceres) is invoked and either
# the directory containing this file either is present in CMAKE_MODULE_PATH
# (if Ceres was installed), or exists in the local CMake package registry if
# the Ceres build directory was exported.
#
# This module defines the following variables:
#
# Ceres_FOUND / CERES_FOUND: True if Ceres has been successfully
# found. Both variables are set as although
# FindPackage() only references Ceres_FOUND
# in Config mode, given the conventions for
# <package>_FOUND when FindPackage() is
# called in Module mode, users could
# reasonably expect to use CERES_FOUND
# instead.
#
# CERES_VERSION: Version of Ceres found.
#
# CERES_INCLUDE_DIRS: Include directories for Ceres and the
# dependencies which appear in the Ceres public
# API and are thus required to use Ceres.
#
# CERES_LIBRARIES: Libraries for Ceres and all
# dependencies against which Ceres was
# compiled. This will not include any optional
# dependencies that were disabled when Ceres was
# compiled.
#
# The following variables are also defined for legacy compatibility
# only. Any new code should not use them as they do not conform to
# the standard CMake FindPackage naming conventions.
#
# CERES_INCLUDES = ${CERES_INCLUDE_DIRS}.
# Called if we failed to find Ceres or any of its required dependencies,
# unsets all public (designed to be used externally) variables and reports
# error message at priority depending upon [REQUIRED/QUIET/<NONE>] argument.
macro(CERES_REPORT_NOT_FOUND REASON_MSG)
# FindPackage() only references Ceres_FOUND, and requires it to be
# explicitly set FALSE to denote not found (not merely undefined).
set(Ceres_FOUND FALSE)
set(CERES_FOUND FALSE)
unset(CERES_INCLUDE_DIRS)
unset(CERES_LIBRARIES)
# Reset the CMake module path to its state when this script was called.
set(CMAKE_MODULE_PATH ${CALLERS_CMAKE_MODULE_PATH})
# Note <package>_FIND_[REQUIRED/QUIETLY] variables defined by
# FindPackage() use the camelcase library name, not uppercase.
if (Ceres_FIND_QUIETLY)
message(STATUS "Failed to find Ceres - " ${REASON_MSG} ${ARGN})
else (Ceres_FIND_REQUIRED)
message(FATAL_ERROR "Failed to find Ceres - " ${REASON_MSG} ${ARGN})
else()
# Neither QUIETLY nor REQUIRED, use SEND_ERROR which emits an error
# that prevents generation, but continues configuration.
message(SEND_ERROR "Failed to find Ceres - " ${REASON_MSG} ${ARGN})
endif ()
return()
endmacro(CERES_REPORT_NOT_FOUND)
# If Ceres was not installed, then by definition it was exported
# from a build directory.
set(CERES_WAS_INSTALLED TRUE)
# Record the state of the CMake module path when this script was
# called so that we can ensure that we leave it in the same state on
# exit as it was on entry, but modify it locally.
set(CALLERS_CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH})
# Get the (current, i.e. installed) directory containing this file.
get_filename_component(CERES_CURRENT_CONFIG_DIR
"${CMAKE_CURRENT_LIST_FILE}" PATH)
if (CERES_WAS_INSTALLED)
# Reset CMake module path to the installation directory of this
# script, thus we will use the FindPackage() scripts shipped with
# Ceres to find Ceres' dependencies, even if the user has equivalently
# named FindPackage() scripts in their project.
set(CMAKE_MODULE_PATH ${CERES_CURRENT_CONFIG_DIR})
# Build the absolute root install directory as a relative path
# (determined when Ceres was configured & built) from the current
# install directory for this this file. This allows for the install
# tree to be relocated, after Ceres was built, outside of CMake.
get_filename_component(CURRENT_ROOT_INSTALL_DIR
${CERES_CURRENT_CONFIG_DIR}/../
ABSOLUTE)
if (NOT EXISTS ${CURRENT_ROOT_INSTALL_DIR})
ceres_report_not_found(
"Ceres install root: ${CURRENT_ROOT_INSTALL_DIR}, "
"determined from relative path from CeresConfig.cmake install location: "
"${CERES_CURRENT_CONFIG_DIR}, does not exist. Either the install "
"directory was deleted, or the install tree was only partially relocated "
"outside of CMake after Ceres was built.")
endif (NOT EXISTS ${CURRENT_ROOT_INSTALL_DIR})
# Set the include directories for Ceres (itself).
set(CERES_INCLUDE_DIR "${CURRENT_ROOT_INSTALL_DIR}/include")
if (NOT EXISTS ${CERES_INCLUDE_DIR}/ceres/ceres.h)
ceres_report_not_found(
"Ceres install root: ${CURRENT_ROOT_INSTALL_DIR}, "
"determined from relative path from CeresConfig.cmake install location: "
"${CERES_CURRENT_CONFIG_DIR}, does not contain Ceres headers. "
"Either the install directory was deleted, or the install tree was only "
"partially relocated outside of CMake after Ceres was built.")
endif (NOT EXISTS ${CERES_INCLUDE_DIR}/ceres/ceres.h)
list(APPEND CERES_INCLUDE_DIRS ${CERES_INCLUDE_DIR})
else(CERES_WAS_INSTALLED)
# Ceres was exported from the build tree.
set(CERES_EXPORTED_BUILD_DIR ${CERES_CURRENT_CONFIG_DIR})
get_filename_component(CERES_EXPORTED_SOURCE_DIR
${CERES_EXPORTED_BUILD_DIR}/../
ABSOLUTE)
if (NOT EXISTS ${CERES_EXPORTED_SOURCE_DIR})
ceres_report_not_found(
"Ceres exported source directory: ${CERES_EXPORTED_SOURCE_DIR}, "
"determined from relative path from CeresConfig.cmake exported build "
"directory: ${CERES_EXPORTED_BUILD_DIR} does not exist.")
endif()
# Reset CMake module path to the cmake directory in the Ceres source
# tree which was exported, thus we will use the FindPackage() scripts shipped
# with Ceres to find Ceres' dependencies, even if the user has equivalently
# named FindPackage() scripts in their project.
set(CMAKE_MODULE_PATH ${CERES_EXPORTED_SOURCE_DIR}/cmake)
# Set the include directories for Ceres (itself).
set(CERES_INCLUDE_DIR "${CERES_EXPORTED_SOURCE_DIR}/include")
if (NOT EXISTS ${CERES_INCLUDE_DIR}/ceres/ceres.h)
ceres_report_not_found(
"Ceres exported source directory: ${CERES_EXPORTED_SOURCE_DIR}, "
"determined from relative path from CeresConfig.cmake exported build "
"directory: ${CERES_EXPORTED_BUILD_DIR}, does not contain Ceres headers.")
endif (NOT EXISTS ${CERES_INCLUDE_DIR}/ceres/ceres.h)
list(APPEND CERES_INCLUDE_DIRS ${CERES_INCLUDE_DIR})
# Append the path to the configured config.h in the exported build directory
# to the Ceres include directories.
set(CERES_CONFIG_FILE
${CERES_EXPORTED_BUILD_DIR}/config/ceres/internal/config.h)
if (NOT EXISTS ${CERES_CONFIG_FILE})
ceres_report_not_found(
"Ceres exported build directory: ${CERES_EXPORTED_BUILD_DIR}, "
"does not contain required configured Ceres config.h, it is not here: "
"${CERES_CONFIG_FILE}.")
endif (NOT EXISTS ${CERES_CONFIG_FILE})
list(APPEND CERES_INCLUDE_DIRS ${CERES_EXPORTED_BUILD_DIR}/config)
endif(CERES_WAS_INSTALLED)
# Set the version.
set(CERES_VERSION 1.11.0 )
# Eigen.
# Flag set during configuration and build of Ceres.
set(CERES_EIGEN_VERSION 3.2.7)
#set(CERES_EIGEN_VERSION 3.2.94)
# Append the locations of Eigen when Ceres was built to the search path hints.
list(APPEND EIGEN_INCLUDE_DIR_HINTS C:/SDK/Eigen_3.3-rc1)
# Search quietly to control the timing of the error message if not found. The
# search should be for an exact match, but for usability reasons do a soft
# match and reject with an explanation below.
find_package(Eigen ${CERES_EIGEN_VERSION} QUIET)
if (EIGEN_FOUND)
if (NOT EIGEN_VERSION VERSION_EQUAL CERES_EIGEN_VERSION)
# CMake's VERSION check in FIND_PACKAGE() will accept any version >= the
# specified version. However, only version = is supported. Improve
# usability by explaining why we don't accept non-exact version matching.
ceres_report_not_found("Found Eigen dependency, but the version of Eigen "
"found (${EIGEN_VERSION}) does not exactly match the version of Eigen "
"Ceres was compiled with (${CERES_EIGEN_VERSION}). This can cause subtle "
"bugs by triggering violations of the One Definition Rule. See the "
"Wikipedia article http://en.wikipedia.org/wiki/One_Definition_Rule "
"for more details")
endif ()
message(STATUS "Found required Ceres dependency: "
"Eigen version ${CERES_EIGEN_VERSION} in ${EIGEN_INCLUDE_DIRS}")
else (EIGEN_FOUND)
ceres_report_not_found("Missing required Ceres "
"dependency: Eigen version ${CERES_EIGEN_VERSION}, please set "
"EIGEN_INCLUDE_DIR.")
endif (EIGEN_FOUND)
list(APPEND CERES_INCLUDE_DIRS ${EIGEN_INCLUDE_DIRS})
# Glog.
# Flag set during configuration and build of Ceres.
set(CERES_USES_MINIGLOG ON)
if (CERES_USES_MINIGLOG)
set(MINIGLOG_INCLUDE_DIR ${CERES_INCLUDE_DIR}/ceres/internal/miniglog)
if (NOT EXISTS ${MINIGLOG_INCLUDE_DIR})
ceres_report_not_found(
"Ceres include directory: "
"${CERES_INCLUDE_DIR} does not include miniglog, but Ceres was "
"compiled with MINIGLOG enabled (in place of Glog).")
endif (NOT EXISTS ${MINIGLOG_INCLUDE_DIR})
list(APPEND CERES_INCLUDE_DIRS ${MINIGLOG_INCLUDE_DIR})
# Output message at standard log level (not the lower STATUS) so that
# the message is output in GUI during configuration to warn user.
message("-- Found Ceres compiled with miniglog substitute "
"for glog, beware this will likely cause problems if glog is later linked.")
else (CERES_USES_MINIGLOG)
# Append the locations of glog when Ceres was built to the search path hints.
list(APPEND GLOG_INCLUDE_DIR_HINTS C:/SDK/glog/src)
get_filename_component(CERES_BUILD_GLOG_LIBRARY_DIR GLOG_LIBRARY-NOTFOUND PATH)
list(APPEND GLOG_LIBRARY_DIR_HINTS ${CERES_BUILD_GLOG_LIBRARY_DIR})
# Search quietly s/t we control the timing of the error message if not found.
find_package(Glog QUIET)
if (GLOG_FOUND)
message(STATUS "Found required Ceres dependency: "
"Glog in ${GLOG_INCLUDE_DIRS}")
else (GLOG_FOUND)
ceres_report_not_found("Missing required Ceres "
"dependency: Glog, please set GLOG_INCLUDE_DIR.")
endif (GLOG_FOUND)
list(APPEND CERES_INCLUDE_DIRS ${GLOG_INCLUDE_DIRS})
endif (CERES_USES_MINIGLOG)
# Import exported Ceres targets, if they have not already been imported.
if (NOT TARGET ceres AND NOT Ceres_BINARY_DIR)
include(${CERES_CURRENT_CONFIG_DIR}/CeresTargets.cmake)
endif (NOT TARGET ceres AND NOT Ceres_BINARY_DIR)
# Set the expected XX_LIBRARIES variable for FindPackage().
set(CERES_LIBRARIES ceres)
# Make user aware of any compile flags that will be added to their targets
# which use Ceres (i.e. flags exported in the Ceres target). Only CMake
# versions >= 2.8.12 support target_compile_options().
if (TARGET ${CERES_LIBRARIES} AND
NOT CMAKE_VERSION VERSION_LESS "2.8.12")
get_target_property(CERES_INTERFACE_COMPILE_OPTIONS
${CERES_LIBRARIES} INTERFACE_COMPILE_OPTIONS)
if (CERES_WAS_INSTALLED)
set(CERES_LOCATION "${CURRENT_ROOT_INSTALL_DIR}")
else()
set(CERES_LOCATION "${CERES_EXPORTED_BUILD_DIR}")
endif()
# Check for -std=c++11 flags.
if (CERES_INTERFACE_COMPILE_OPTIONS MATCHES ".*std=c\\+\\+11.*")
message(STATUS "Ceres version ${CERES_VERSION} detected here: "
"${CERES_LOCATION} was built with C++11. Ceres target will add "
"C++11 flags to compile options for targets using it.")
endif()
endif()
# Set legacy include directories variable for backwards compatibility.
set(CERES_INCLUDES ${CERES_INCLUDE_DIRS})
# Reset CMake module path to its state when this script was called.
set(CMAKE_MODULE_PATH ${CALLERS_CMAKE_MODULE_PATH})
# As we use CERES_REPORT_NOT_FOUND() to abort, if we reach this point we have
# found Ceres and all required dependencies.
if (CERES_WAS_INSTALLED)
message(STATUS "Found Ceres version: ${CERES_VERSION} "
"installed in: ${CURRENT_ROOT_INSTALL_DIR}")
else (CERES_WAS_INSTALLED)
message(STATUS "Found Ceres version: ${CERES_VERSION} "
"exported from build directory: ${CERES_EXPORTED_BUILD_DIR}")
endif()
# Set CERES_FOUND to be equivalent to Ceres_FOUND, which is set to
# TRUE by FindPackage() if this file is found and run, and after which
# Ceres_FOUND is not (explicitly, i.e. undefined does not count) set
# to FALSE.
set(CERES_FOUND TRUE)

View File

@ -0,0 +1,50 @@
# 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: pablo.speciale@gmail.com (Pablo Speciale)
#
# FIND_PACKAGE() searches for a <package>Config.cmake file and an associated
# <package>Version.cmake file, which it loads to check the version number.
#
# This file can be used with CONFIGURE_FILE() to generate such a file for a
# project with very basic logic.
#
# It sets PACKAGE_VERSION_EXACT if the current version string and the requested
# version string are exactly the same and it sets PACKAGE_VERSION_COMPATIBLE
# if the current version is >= requested version.
set(PACKAGE_VERSION 1.11.0)
if ("${PACKAGE_VERSION}" VERSION_LESS "${PACKAGE_FIND_VERSION}")
set(PACKAGE_VERSION_COMPATIBLE FALSE)
else ("${PACKAGE_VERSION}" VERSION_LESS "${PACKAGE_FIND_VERSION}")
set(PACKAGE_VERSION_COMPATIBLE TRUE)
if ("${PACKAGE_FIND_VERSION}" STREQUAL "${PACKAGE_VERSION}")
set(PACKAGE_VERSION_EXACT TRUE)
endif ("${PACKAGE_FIND_VERSION}" STREQUAL "${PACKAGE_VERSION}")
endif ("${PACKAGE_VERSION}" VERSION_LESS "${PACKAGE_FIND_VERSION}")

View File

@ -0,0 +1,19 @@
#----------------------------------------------------------------
# Generated CMake target import file for configuration "Debug".
#----------------------------------------------------------------
# Commands may need to know the format version.
set(CMAKE_IMPORT_FILE_VERSION 1)
# Import target "ceres" for configuration "Debug"
set_property(TARGET ceres APPEND PROPERTY IMPORTED_CONFIGURATIONS DEBUG)
set_target_properties(ceres PROPERTIES
IMPORTED_LINK_INTERFACE_LANGUAGES_DEBUG "CXX"
IMPORTED_LOCATION_DEBUG "${_IMPORT_PREFIX}/lib/ceres-debug.lib"
)
list(APPEND _IMPORT_CHECK_TARGETS ceres )
list(APPEND _IMPORT_CHECK_FILES_FOR_ceres "${_IMPORT_PREFIX}/lib/ceres-debug.lib" )
# Commands beyond this point should not need to know the version.
set(CMAKE_IMPORT_FILE_VERSION)

View File

@ -0,0 +1,19 @@
#----------------------------------------------------------------
# Generated CMake target import file for configuration "Release".
#----------------------------------------------------------------
# Commands may need to know the format version.
set(CMAKE_IMPORT_FILE_VERSION 1)
# Import target "ceres" for configuration "Release"
set_property(TARGET ceres APPEND PROPERTY IMPORTED_CONFIGURATIONS RELEASE)
set_target_properties(ceres PROPERTIES
IMPORTED_LINK_INTERFACE_LANGUAGES_RELEASE "CXX"
IMPORTED_LOCATION_RELEASE "${_IMPORT_PREFIX}/lib/ceres.lib"
)
list(APPEND _IMPORT_CHECK_TARGETS ceres )
list(APPEND _IMPORT_CHECK_FILES_FOR_ceres "${_IMPORT_PREFIX}/lib/ceres.lib" )
# Commands beyond this point should not need to know the version.
set(CMAKE_IMPORT_FILE_VERSION)

View File

@ -0,0 +1,82 @@
# Generated by CMake 3.5.1
if("${CMAKE_MAJOR_VERSION}.${CMAKE_MINOR_VERSION}" LESS 2.5)
message(FATAL_ERROR "CMake >= 2.6.0 required")
endif()
cmake_policy(PUSH)
cmake_policy(VERSION 2.6)
#----------------------------------------------------------------
# Generated CMake target import file.
#----------------------------------------------------------------
# Commands may need to know the format version.
set(CMAKE_IMPORT_FILE_VERSION 1)
# Protect against multiple inclusion, which would fail when already imported targets are added once more.
set(_targetsDefined)
set(_targetsNotDefined)
set(_expectedTargets)
foreach(_expectedTarget ceres)
list(APPEND _expectedTargets ${_expectedTarget})
if(NOT TARGET ${_expectedTarget})
list(APPEND _targetsNotDefined ${_expectedTarget})
endif()
if(TARGET ${_expectedTarget})
list(APPEND _targetsDefined ${_expectedTarget})
endif()
endforeach()
if("${_targetsDefined}" STREQUAL "${_expectedTargets}")
set(CMAKE_IMPORT_FILE_VERSION)
cmake_policy(POP)
return()
endif()
if(NOT "${_targetsDefined}" STREQUAL "")
message(FATAL_ERROR "Some (but not all) targets in this export set were already defined.\nTargets Defined: ${_targetsDefined}\nTargets not yet defined: ${_targetsNotDefined}\n")
endif()
unset(_targetsDefined)
unset(_targetsNotDefined)
unset(_expectedTargets)
# Compute the installation prefix relative to this file.
get_filename_component(_IMPORT_PREFIX "${CMAKE_CURRENT_LIST_FILE}" PATH)
get_filename_component(_IMPORT_PREFIX "${_IMPORT_PREFIX}" PATH)
# Create imported target ceres
add_library(ceres STATIC IMPORTED)
# Load information for each installed configuration.
get_filename_component(_DIR "${CMAKE_CURRENT_LIST_FILE}" PATH)
file(GLOB CONFIG_FILES "${_DIR}/CeresTargets-*.cmake")
foreach(f ${CONFIG_FILES})
include(${f})
endforeach()
# Cleanup temporary variables.
set(_IMPORT_PREFIX)
# Loop over all imported files and verify that they actually exist
foreach(target ${_IMPORT_CHECK_TARGETS} )
foreach(file ${_IMPORT_CHECK_FILES_FOR_${target}} )
if(NOT EXISTS "${file}" )
message(FATAL_ERROR "The imported target \"${target}\" references the file
\"${file}\"
but this file does not exist. Possible reasons include:
* The file was deleted, renamed, or moved to another location.
* An install or uninstall procedure did not complete successfully.
* The installation package was faulty and contained
\"${CMAKE_CURRENT_LIST_FILE}\"
but not all the files it references.
")
endif()
endforeach()
unset(_IMPORT_CHECK_FILES_FOR_${target})
endforeach()
unset(_IMPORT_CHECK_TARGETS)
# This file does not depend on other imported targets which have
# been exported from the same project but in a separate export set.
# Commands beyond this point should not need to know the version.
set(CMAKE_IMPORT_FILE_VERSION)
cmake_policy(POP)

View File

@ -0,0 +1,166 @@
# Ceres Solver - A fast non-linear least squares minimizer
# Copyright 2015 Google Inc. All rights reserved.
# http://ceres-solver.org/
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions are met:
#
# * Redistributions of source code must retain the above copyright notice,
# this list of conditions and the following disclaimer.
# * Redistributions in binary form must reproduce the above copyright notice,
# this list of conditions and the following disclaimer in the documentation
# and/or other materials provided with the distribution.
# * Neither the name of Google Inc. nor the names of its contributors may be
# used to endorse or promote products derived from this software without
# specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
# Author: alexs.mac@gmail.com (Alex Stewart)
#
# FindEigen.cmake - Find Eigen library, version >= 3.
#
# This module defines the following variables:
#
# EIGEN_FOUND: TRUE iff Eigen is found.
# EIGEN_INCLUDE_DIRS: Include directories for Eigen.
#
# EIGEN_VERSION: Extracted from Eigen/src/Core/util/Macros.h
# EIGEN_WORLD_VERSION: Equal to 3 if EIGEN_VERSION = 3.2.0
# EIGEN_MAJOR_VERSION: Equal to 2 if EIGEN_VERSION = 3.2.0
# EIGEN_MINOR_VERSION: Equal to 0 if EIGEN_VERSION = 3.2.0
#
# The following variables control the behaviour of this module:
#
# EIGEN_INCLUDE_DIR_HINTS: List of additional directories in which to
# search for eigen includes, e.g: /timbuktu/eigen3.
#
# The following variables are also defined by this module, but in line with
# CMake recommended FindPackage() module style should NOT be referenced directly
# by callers (use the plural variables detailed above instead). These variables
# do however affect the behaviour of the module via FIND_[PATH/LIBRARY]() which
# are NOT re-called (i.e. search for library is not repeated) if these variables
# are set with valid values _in the CMake cache_. This means that if these
# variables are set directly in the cache, either by the user in the CMake GUI,
# or by the user passing -DVAR=VALUE directives to CMake when called (which
# explicitly defines a cache variable), then they will be used verbatim,
# bypassing the HINTS variables and other hard-coded search locations.
#
# EIGEN_INCLUDE_DIR: Include directory for CXSparse, not including the
# include directory of any dependencies.
# Called if we failed to find Eigen or any of it's required dependencies,
# unsets all public (designed to be used externally) variables and reports
# error message at priority depending upon [REQUIRED/QUIET/<NONE>] argument.
macro(EIGEN_REPORT_NOT_FOUND REASON_MSG)
unset(EIGEN_FOUND)
unset(EIGEN_INCLUDE_DIRS)
# Make results of search visible in the CMake GUI if Eigen has not
# been found so that user does not have to toggle to advanced view.
mark_as_advanced(CLEAR EIGEN_INCLUDE_DIR)
# Note <package>_FIND_[REQUIRED/QUIETLY] variables defined by FindPackage()
# use the camelcase library name, not uppercase.
if (Eigen_FIND_QUIETLY)
message(STATUS "Failed to find Eigen - " ${REASON_MSG} ${ARGN})
elseif (Eigen_FIND_REQUIRED)
message(FATAL_ERROR "Failed to find Eigen - " ${REASON_MSG} ${ARGN})
else()
# Neither QUIETLY nor REQUIRED, use no priority which emits a message
# but continues configuration and allows generation.
message("-- Failed to find Eigen - " ${REASON_MSG} ${ARGN})
endif ()
endmacro(EIGEN_REPORT_NOT_FOUND)
# Search user-installed locations first, so that we prefer user installs
# to system installs where both exist.
#
# TODO: Add standard Windows search locations for Eigen.
list(APPEND EIGEN_CHECK_INCLUDE_DIRS
/usr/local/include
/usr/local/homebrew/include # Mac OS X
/opt/local/var/macports/software # Mac OS X.
/opt/local/include
/usr/include)
# Additional suffixes to try appending to each search path.
list(APPEND EIGEN_CHECK_PATH_SUFFIXES
eigen3 # Default root directory for Eigen.
Eigen/include/eigen3 ) # Windows (for C:/Program Files prefix).
# Search supplied hint directories first if supplied.
find_path(EIGEN_INCLUDE_DIR
NAMES Eigen/Core
PATHS ${EIGEN_INCLUDE_DIR_HINTS}
${EIGEN_CHECK_INCLUDE_DIRS}
PATH_SUFFIXES ${EIGEN_CHECK_PATH_SUFFIXES})
if (NOT EIGEN_INCLUDE_DIR OR
NOT EXISTS ${EIGEN_INCLUDE_DIR})
eigen_report_not_found(
"Could not find eigen3 include directory, set EIGEN_INCLUDE_DIR to "
"path to eigen3 include directory, e.g. /usr/local/include/eigen3.")
endif (NOT EIGEN_INCLUDE_DIR OR
NOT EXISTS ${EIGEN_INCLUDE_DIR})
# Mark internally as found, then verify. EIGEN_REPORT_NOT_FOUND() unsets
# if called.
set(EIGEN_FOUND TRUE)
# Extract Eigen version from Eigen/src/Core/util/Macros.h
if (EIGEN_INCLUDE_DIR)
set(EIGEN_VERSION_FILE ${EIGEN_INCLUDE_DIR}/Eigen/src/Core/util/Macros.h)
if (NOT EXISTS ${EIGEN_VERSION_FILE})
eigen_report_not_found(
"Could not find file: ${EIGEN_VERSION_FILE} "
"containing version information in Eigen install located at: "
"${EIGEN_INCLUDE_DIR}.")
else (NOT EXISTS ${EIGEN_VERSION_FILE})
file(READ ${EIGEN_VERSION_FILE} EIGEN_VERSION_FILE_CONTENTS)
string(REGEX MATCH "#define EIGEN_WORLD_VERSION [0-9]+"
EIGEN_WORLD_VERSION "${EIGEN_VERSION_FILE_CONTENTS}")
string(REGEX REPLACE "#define EIGEN_WORLD_VERSION ([0-9]+)" "\\1"
EIGEN_WORLD_VERSION "${EIGEN_WORLD_VERSION}")
string(REGEX MATCH "#define EIGEN_MAJOR_VERSION [0-9]+"
EIGEN_MAJOR_VERSION "${EIGEN_VERSION_FILE_CONTENTS}")
string(REGEX REPLACE "#define EIGEN_MAJOR_VERSION ([0-9]+)" "\\1"
EIGEN_MAJOR_VERSION "${EIGEN_MAJOR_VERSION}")
string(REGEX MATCH "#define EIGEN_MINOR_VERSION [0-9]+"
EIGEN_MINOR_VERSION "${EIGEN_VERSION_FILE_CONTENTS}")
string(REGEX REPLACE "#define EIGEN_MINOR_VERSION ([0-9]+)" "\\1"
EIGEN_MINOR_VERSION "${EIGEN_MINOR_VERSION}")
# This is on a single line s/t CMake does not interpret it as a list of
# elements and insert ';' separators which would result in 3.;2.;0 nonsense.
set(EIGEN_VERSION "${EIGEN_WORLD_VERSION}.${EIGEN_MAJOR_VERSION}.${EIGEN_MINOR_VERSION}")
endif (NOT EXISTS ${EIGEN_VERSION_FILE})
endif (EIGEN_INCLUDE_DIR)
# Set standard CMake FindPackage variables if found.
if (EIGEN_FOUND)
set(EIGEN_INCLUDE_DIRS ${EIGEN_INCLUDE_DIR})
endif (EIGEN_FOUND)
# Handle REQUIRED / QUIET optional arguments and version.
include(FindPackageHandleStandardArgs)
find_package_handle_standard_args(Eigen
REQUIRED_VARS EIGEN_INCLUDE_DIRS
VERSION_VAR EIGEN_VERSION)
# Only mark internal variables as advanced if we found Eigen, otherwise
# leave it visible in the standard GUI for the user to set manually.
if (EIGEN_FOUND)
mark_as_advanced(FORCE EIGEN_INCLUDE_DIR)
endif (EIGEN_FOUND)

209
external/Ceres-1.11/CMake/FindGlog.cmake vendored Normal file
View File

@ -0,0 +1,209 @@
# Ceres Solver - A fast non-linear least squares minimizer
# Copyright 2015 Google Inc. All rights reserved.
# http://ceres-solver.org/
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions are met:
#
# * Redistributions of source code must retain the above copyright notice,
# this list of conditions and the following disclaimer.
# * Redistributions in binary form must reproduce the above copyright notice,
# this list of conditions and the following disclaimer in the documentation
# and/or other materials provided with the distribution.
# * Neither the name of Google Inc. nor the names of its contributors may be
# used to endorse or promote products derived from this software without
# specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
# Author: alexs.mac@gmail.com (Alex Stewart)
#
# FindGlog.cmake - Find Google glog logging library.
#
# This module defines the following variables:
#
# GLOG_FOUND: TRUE iff glog is found.
# GLOG_INCLUDE_DIRS: Include directories for glog.
# GLOG_LIBRARIES: Libraries required to link glog.
#
# The following variables control the behaviour of this module:
#
# GLOG_INCLUDE_DIR_HINTS: List of additional directories in which to
# search for glog includes, e.g: /timbuktu/include.
# GLOG_LIBRARY_DIR_HINTS: List of additional directories in which to
# search for glog libraries, e.g: /timbuktu/lib.
#
# The following variables are also defined by this module, but in line with
# CMake recommended FindPackage() module style should NOT be referenced directly
# by callers (use the plural variables detailed above instead). These variables
# do however affect the behaviour of the module via FIND_[PATH/LIBRARY]() which
# are NOT re-called (i.e. search for library is not repeated) if these variables
# are set with valid values _in the CMake cache_. This means that if these
# variables are set directly in the cache, either by the user in the CMake GUI,
# or by the user passing -DVAR=VALUE directives to CMake when called (which
# explicitly defines a cache variable), then they will be used verbatim,
# bypassing the HINTS variables and other hard-coded search locations.
#
# GLOG_INCLUDE_DIR: Include directory for glog, not including the
# include directory of any dependencies.
# GLOG_LIBRARY: glog library, not including the libraries of any
# dependencies.
# Reset CALLERS_CMAKE_FIND_LIBRARY_PREFIXES to its value when
# FindGlog was invoked.
macro(GLOG_RESET_FIND_LIBRARY_PREFIX)
if (MSVC)
set(CMAKE_FIND_LIBRARY_PREFIXES "${CALLERS_CMAKE_FIND_LIBRARY_PREFIXES}")
endif (MSVC)
endmacro(GLOG_RESET_FIND_LIBRARY_PREFIX)
# Called if we failed to find glog or any of it's required dependencies,
# unsets all public (designed to be used externally) variables and reports
# error message at priority depending upon [REQUIRED/QUIET/<NONE>] argument.
macro(GLOG_REPORT_NOT_FOUND REASON_MSG)
unset(GLOG_FOUND)
unset(GLOG_INCLUDE_DIRS)
unset(GLOG_LIBRARIES)
# Make results of search visible in the CMake GUI if glog has not
# been found so that user does not have to toggle to advanced view.
mark_as_advanced(CLEAR GLOG_INCLUDE_DIR
GLOG_LIBRARY)
glog_reset_find_library_prefix()
# Note <package>_FIND_[REQUIRED/QUIETLY] variables defined by FindPackage()
# use the camelcase library name, not uppercase.
if (Glog_FIND_QUIETLY)
message(STATUS "Failed to find glog - " ${REASON_MSG} ${ARGN})
elseif (Glog_FIND_REQUIRED)
message(FATAL_ERROR "Failed to find glog - " ${REASON_MSG} ${ARGN})
else()
# Neither QUIETLY nor REQUIRED, use no priority which emits a message
# but continues configuration and allows generation.
message("-- Failed to find glog - " ${REASON_MSG} ${ARGN})
endif ()
endmacro(GLOG_REPORT_NOT_FOUND)
# Handle possible presence of lib prefix for libraries on MSVC, see
# also GLOG_RESET_FIND_LIBRARY_PREFIX().
if (MSVC)
# Preserve the caller's original values for CMAKE_FIND_LIBRARY_PREFIXES
# s/t we can set it back before returning.
set(CALLERS_CMAKE_FIND_LIBRARY_PREFIXES "${CMAKE_FIND_LIBRARY_PREFIXES}")
# The empty string in this list is important, it represents the case when
# the libraries have no prefix (shared libraries / DLLs).
set(CMAKE_FIND_LIBRARY_PREFIXES "lib" "" "${CMAKE_FIND_LIBRARY_PREFIXES}")
endif (MSVC)
# Search user-installed locations first, so that we prefer user installs
# to system installs where both exist.
list(APPEND GLOG_CHECK_INCLUDE_DIRS
/usr/local/include
/usr/local/homebrew/include # Mac OS X
/opt/local/var/macports/software # Mac OS X.
/opt/local/include
/usr/include)
# Windows (for C:/Program Files prefix).
list(APPEND GLOG_CHECK_PATH_SUFFIXES
glog/include
glog/Include
Glog/include
Glog/Include)
list(APPEND GLOG_CHECK_LIBRARY_DIRS
/usr/local/lib
/usr/local/homebrew/lib # Mac OS X.
/opt/local/lib
/usr/lib)
# Windows (for C:/Program Files prefix).
list(APPEND GLOG_CHECK_LIBRARY_SUFFIXES
glog/lib
glog/Lib
Glog/lib
Glog/Lib)
# Search supplied hint directories first if supplied.
find_path(GLOG_INCLUDE_DIR
NAMES glog/logging.h
PATHS ${GLOG_INCLUDE_DIR_HINTS}
${GLOG_CHECK_INCLUDE_DIRS}
PATH_SUFFIXES ${GLOG_CHECK_PATH_SUFFIXES})
if (NOT GLOG_INCLUDE_DIR OR
NOT EXISTS ${GLOG_INCLUDE_DIR})
glog_report_not_found(
"Could not find glog include directory, set GLOG_INCLUDE_DIR "
"to directory containing glog/logging.h")
endif (NOT GLOG_INCLUDE_DIR OR
NOT EXISTS ${GLOG_INCLUDE_DIR})
find_library(GLOG_LIBRARY NAMES glog
PATHS ${GLOG_LIBRARY_DIR_HINTS}
${GLOG_CHECK_LIBRARY_DIRS}
PATH_SUFFIXES ${GLOG_CHECK_LIBRARY_SUFFIXES})
if (NOT GLOG_LIBRARY OR
NOT EXISTS ${GLOG_LIBRARY})
glog_report_not_found(
"Could not find glog library, set GLOG_LIBRARY "
"to full path to libglog.")
endif (NOT GLOG_LIBRARY OR
NOT EXISTS ${GLOG_LIBRARY})
# Mark internally as found, then verify. GLOG_REPORT_NOT_FOUND() unsets
# if called.
set(GLOG_FOUND TRUE)
# Glog does not seem to provide any record of the version in its
# source tree, thus cannot extract version.
# Catch case when caller has set GLOG_INCLUDE_DIR in the cache / GUI and
# thus FIND_[PATH/LIBRARY] are not called, but specified locations are
# invalid, otherwise we would report the library as found.
if (GLOG_INCLUDE_DIR AND
NOT EXISTS ${GLOG_INCLUDE_DIR}/glog/logging.h)
glog_report_not_found(
"Caller defined GLOG_INCLUDE_DIR:"
" ${GLOG_INCLUDE_DIR} does not contain glog/logging.h header.")
endif (GLOG_INCLUDE_DIR AND
NOT EXISTS ${GLOG_INCLUDE_DIR}/glog/logging.h)
# TODO: This regex for glog library is pretty primitive, we use lowercase
# for comparison to handle Windows using CamelCase library names, could
# this check be better?
string(TOLOWER "${GLOG_LIBRARY}" LOWERCASE_GLOG_LIBRARY)
if (GLOG_LIBRARY AND
NOT "${LOWERCASE_GLOG_LIBRARY}" MATCHES ".*glog[^/]*")
glog_report_not_found(
"Caller defined GLOG_LIBRARY: "
"${GLOG_LIBRARY} does not match glog.")
endif (GLOG_LIBRARY AND
NOT "${LOWERCASE_GLOG_LIBRARY}" MATCHES ".*glog[^/]*")
# Set standard CMake FindPackage variables if found.
if (GLOG_FOUND)
set(GLOG_INCLUDE_DIRS ${GLOG_INCLUDE_DIR})
set(GLOG_LIBRARIES ${GLOG_LIBRARY})
endif (GLOG_FOUND)
glog_reset_find_library_prefix()
# Handle REQUIRED / QUIET optional arguments.
include(FindPackageHandleStandardArgs)
find_package_handle_standard_args(Glog DEFAULT_MSG
GLOG_INCLUDE_DIRS GLOG_LIBRARIES)
# Only mark internal variables as advanced if we found glog, otherwise
# leave them visible in the standard GUI for the user to set manually.
if (GLOG_FOUND)
mark_as_advanced(FORCE GLOG_INCLUDE_DIR
GLOG_LIBRARY)
endif (GLOG_FOUND)

View File

@ -0,0 +1,227 @@
// 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)
//
// Create CostFunctions as needed by the least squares framework, with
// Jacobians computed via automatic differentiation. For more
// information on automatic differentation, see the wikipedia article
// at http://en.wikipedia.org/wiki/Automatic_differentiation
//
// To get an auto differentiated cost function, you must define a class with a
// templated operator() (a functor) that computes the cost function in terms of
// the template parameter T. The autodiff framework substitutes appropriate
// "jet" objects for T in order to compute the derivative when necessary, but
// this is hidden, and you should write the function as if T were a scalar type
// (e.g. a double-precision floating point number).
//
// The function must write the computed value in the last argument
// (the only non-const one) and return true to indicate
// success. Please see cost_function.h for details on how the return
// value maybe used to impose simple constraints on the parameter
// block.
//
// For example, consider a scalar error e = k - x'y, where both x and y are
// two-dimensional column vector parameters, the prime sign indicates
// transposition, and k is a constant. The form of this error, which is the
// difference between a constant and an expression, is a common pattern in least
// squares problems. For example, the value x'y might be the model expectation
// for a series of measurements, where there is an instance of the cost function
// for each measurement k.
//
// The actual cost added to the total problem is e^2, or (k - x'k)^2; however,
// the squaring is implicitly done by the optimization framework.
//
// To write an auto-differentiable cost function for the above model, first
// define the object
//
// class MyScalarCostFunctor {
// MyScalarCostFunctor(double k): k_(k) {}
//
// template <typename T>
// bool operator()(const T* const x , const T* const y, T* e) const {
// e[0] = T(k_) - x[0] * y[0] + x[1] * y[1];
// return true;
// }
//
// private:
// double k_;
// };
//
// Note that in the declaration of operator() the input parameters x and y come
// first, and are passed as const pointers to arrays of T. If there were three
// input parameters, then the third input parameter would come after y. The
// output is always the last parameter, and is also a pointer to an array. In
// the example above, e is a scalar, so only e[0] is set.
//
// Then given this class definition, the auto differentiated cost function for
// it can be constructed as follows.
//
// CostFunction* cost_function
// = new AutoDiffCostFunction<MyScalarCostFunctor, 1, 2, 2>(
// new MyScalarCostFunctor(1.0)); ^ ^ ^
// | | |
// Dimension of residual -----+ | |
// Dimension of x ---------------+ |
// Dimension of y ------------------+
//
// In this example, there is usually an instance for each measumerent of k.
//
// In the instantiation above, the template parameters following
// "MyScalarCostFunctor", "1, 2, 2", describe the functor as computing a
// 1-dimensional output from two arguments, both 2-dimensional.
//
// AutoDiffCostFunction also supports cost functions with a
// runtime-determined number of residuals. For example:
//
// CostFunction* cost_function
// = new AutoDiffCostFunction<MyScalarCostFunctor, DYNAMIC, 2, 2>(
// new CostFunctorWithDynamicNumResiduals(1.0), ^ ^ ^
// runtime_number_of_residuals); <----+ | | |
// | | | |
// | | | |
// Actual number of residuals ------+ | | |
// Indicate dynamic number of residuals --------+ | |
// Dimension of x ------------------------------------+ |
// Dimension of y ---------------------------------------+
//
// The framework can currently accommodate cost functions of up to 10
// independent variables, and there is no limit on the dimensionality
// of each of them.
//
// WARNING #1: Since the functor will get instantiated with different types for
// T, you must to convert from other numeric types to T before mixing
// computations with other variables of type T. In the example above, this is
// seen where instead of using k_ directly, k_ is wrapped with T(k_).
//
// WARNING #2: A common beginner's error when first using autodiff cost
// functions is to get the sizing wrong. In particular, there is a tendency to
// set the template parameters to (dimension of residual, number of parameters)
// instead of passing a dimension parameter for *every parameter*. In the
// example above, that would be <MyScalarCostFunctor, 1, 2>, which is missing
// the last '2' argument. Please be careful when setting the size parameters.
#ifndef CERES_PUBLIC_AUTODIFF_COST_FUNCTION_H_
#define CERES_PUBLIC_AUTODIFF_COST_FUNCTION_H_
#include "ceres/internal/autodiff.h"
#include "ceres/internal/scoped_ptr.h"
#include "ceres/sized_cost_function.h"
#include "ceres/types.h"
#include "glog/logging.h"
namespace ceres {
// A cost function which computes the derivative of the cost with respect to
// the parameters (a.k.a. the jacobian) using an autodifferentiation framework.
// The first template argument is the functor object, described in the header
// comment. The second argument is the dimension of the residual (or
// ceres::DYNAMIC to indicate it will be set at runtime), and subsequent
// arguments describe the size of the Nth parameter, one per parameter.
//
// The constructors take ownership of the cost functor.
//
// If the number of residuals (argument kNumResiduals below) is
// ceres::DYNAMIC, then the two-argument constructor must be used. The
// second constructor takes a number of residuals (in addition to the
// templated number of residuals). This allows for varying the number
// of residuals for a single autodiff cost function at runtime.
template <typename CostFunctor,
int kNumResiduals, // Number of residuals, or ceres::DYNAMIC.
int N0, // Number of parameters in block 0.
int N1 = 0, // Number of parameters in block 1.
int N2 = 0, // Number of parameters in block 2.
int N3 = 0, // Number of parameters in block 3.
int N4 = 0, // Number of parameters in block 4.
int N5 = 0, // Number of parameters in block 5.
int N6 = 0, // Number of parameters in block 6.
int N7 = 0, // Number of parameters in block 7.
int N8 = 0, // Number of parameters in block 8.
int N9 = 0> // Number of parameters in block 9.
class AutoDiffCostFunction : public SizedCostFunction<kNumResiduals,
N0, N1, N2, N3, N4,
N5, N6, N7, N8, N9> {
public:
// Takes ownership of functor. Uses the template-provided value for the
// number of residuals ("kNumResiduals").
explicit AutoDiffCostFunction(CostFunctor* functor)
: functor_(functor) {
CHECK_NE(kNumResiduals, DYNAMIC)
<< "Can't run the fixed-size constructor if the "
<< "number of residuals is set to ceres::DYNAMIC.";
}
// Takes ownership of functor. Ignores the template-provided
// kNumResiduals in favor of the "num_residuals" argument provided.
//
// This allows for having autodiff cost functions which return varying
// numbers of residuals at runtime.
AutoDiffCostFunction(CostFunctor* functor, int num_residuals)
: functor_(functor) {
CHECK_EQ(kNumResiduals, DYNAMIC)
<< "Can't run the dynamic-size constructor if the "
<< "number of residuals is not ceres::DYNAMIC.";
SizedCostFunction<kNumResiduals,
N0, N1, N2, N3, N4,
N5, N6, N7, N8, N9>
::set_num_residuals(num_residuals);
}
virtual ~AutoDiffCostFunction() {}
// Implementation details follow; clients of the autodiff cost function should
// not have to examine below here.
//
// To handle varardic cost functions, some template magic is needed. It's
// mostly hidden inside autodiff.h.
virtual bool Evaluate(double const* const* parameters,
double* residuals,
double** jacobians) const {
if (!jacobians) {
return internal::VariadicEvaluate<
CostFunctor, double, N0, N1, N2, N3, N4, N5, N6, N7, N8, N9>
::Call(*functor_, parameters, residuals);
}
return internal::AutoDiff<CostFunctor, double,
N0, N1, N2, N3, N4, N5, N6, N7, N8, N9>::Differentiate(
*functor_,
parameters,
SizedCostFunction<kNumResiduals,
N0, N1, N2, N3, N4,
N5, N6, N7, N8, N9>::num_residuals(),
residuals,
jacobians);
}
private:
internal::scoped_ptr<CostFunctor> functor_;
};
} // namespace ceres
#endif // CERES_PUBLIC_AUTODIFF_COST_FUNCTION_H_

View File

@ -0,0 +1,154 @@
// 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: sergey.vfx@gmail.com (Sergey Sharybin)
// mierle@gmail.com (Keir Mierle)
// sameeragarwal@google.com (Sameer Agarwal)
#ifndef CERES_PUBLIC_AUTODIFF_LOCAL_PARAMETERIZATION_H_
#define CERES_PUBLIC_AUTODIFF_LOCAL_PARAMETERIZATION_H_
#include "ceres/local_parameterization.h"
#include "ceres/internal/autodiff.h"
#include "ceres/internal/scoped_ptr.h"
namespace ceres {
// Create local parameterization with Jacobians computed via automatic
// differentiation. For more information on local parameterizations,
// see include/ceres/local_parameterization.h
//
// To get an auto differentiated local parameterization, you must define
// a class with a templated operator() (a functor) that computes
//
// x_plus_delta = Plus(x, delta);
//
// the template parameter T. The autodiff framework substitutes appropriate
// "Jet" objects for T in order to compute the derivative when necessary, but
// this is hidden, and you should write the function as if T were a scalar type
// (e.g. a double-precision floating point number).
//
// The function must write the computed value in the last argument (the only
// non-const one) and return true to indicate success.
//
// For example, Quaternions have a three dimensional local
// parameterization. It's plus operation can be implemented as (taken
// from internal/ceres/auto_diff_local_parameterization_test.cc)
//
// struct QuaternionPlus {
// template<typename T>
// bool operator()(const T* x, const T* delta, T* x_plus_delta) const {
// const T squared_norm_delta =
// delta[0] * delta[0] + delta[1] * delta[1] + delta[2] * delta[2];
//
// T q_delta[4];
// if (squared_norm_delta > T(0.0)) {
// T norm_delta = sqrt(squared_norm_delta);
// const T sin_delta_by_delta = sin(norm_delta) / norm_delta;
// q_delta[0] = cos(norm_delta);
// q_delta[1] = sin_delta_by_delta * delta[0];
// q_delta[2] = sin_delta_by_delta * delta[1];
// q_delta[3] = sin_delta_by_delta * delta[2];
// } else {
// // We do not just use q_delta = [1,0,0,0] here because that is a
// // constant and when used for automatic differentiation will
// // lead to a zero derivative. Instead we take a first order
// // approximation and evaluate it at zero.
// q_delta[0] = T(1.0);
// q_delta[1] = delta[0];
// q_delta[2] = delta[1];
// q_delta[3] = delta[2];
// }
//
// QuaternionProduct(q_delta, x, x_plus_delta);
// return true;
// }
// };
//
// Then given this struct, the auto differentiated local
// parameterization can now be constructed as
//
// LocalParameterization* local_parameterization =
// new AutoDiffLocalParameterization<QuaternionPlus, 4, 3>;
// | |
// Global Size ---------------+ |
// Local Size -------------------+
//
// WARNING: Since the functor will get instantiated with different types for
// T, you must to convert from other numeric types to T before mixing
// computations with other variables of type T. In the example above, this is
// seen where instead of using k_ directly, k_ is wrapped with T(k_).
template <typename Functor, int kGlobalSize, int kLocalSize>
class AutoDiffLocalParameterization : public LocalParameterization {
public:
AutoDiffLocalParameterization() :
functor_(new Functor()) {}
// Takes ownership of functor.
explicit AutoDiffLocalParameterization(Functor* functor) :
functor_(functor) {}
virtual ~AutoDiffLocalParameterization() {}
virtual bool Plus(const double* x,
const double* delta,
double* x_plus_delta) const {
return (*functor_)(x, delta, x_plus_delta);
}
virtual bool ComputeJacobian(const double* x, double* jacobian) const {
double zero_delta[kLocalSize];
for (int i = 0; i < kLocalSize; ++i) {
zero_delta[i] = 0.0;
}
double x_plus_delta[kGlobalSize];
for (int i = 0; i < kGlobalSize; ++i) {
x_plus_delta[i] = 0.0;
}
const double* parameter_ptrs[2] = {x, zero_delta};
double* jacobian_ptrs[2] = { NULL, jacobian };
return internal::AutoDiff<Functor, double, kGlobalSize, kLocalSize>
::Differentiate(*functor_,
parameter_ptrs,
kGlobalSize,
x_plus_delta,
jacobian_ptrs);
}
virtual int GlobalSize() const { return kGlobalSize; }
virtual int LocalSize() const { return kLocalSize; }
private:
internal::scoped_ptr<Functor> functor_;
};
} // namespace ceres
#endif // CERES_PUBLIC_AUTODIFF_LOCAL_PARAMETERIZATION_H_

View File

@ -0,0 +1,146 @@
/* 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: mierle@gmail.com (Keir Mierle)
*
* A minimal C API for Ceres. Not all functionality is included. This API is
* not intended for clients of Ceres, but is instead intended for easing the
* process of binding Ceres to other languages.
*
* Currently this is a work in progress.
*/
#ifndef CERES_PUBLIC_C_API_H_
#define CERES_PUBLIC_C_API_H_
#include "ceres/internal/port.h"
#include "ceres/internal/disable_warnings.h"
#ifdef __cplusplus
extern "C" {
#endif
/* Init the Ceres private data. Must be called before anything else. */
CERES_EXPORT void ceres_init();
/* Equivalent to CostFunction::Evaluate() in the C++ API.
*
* The user may keep private information inside the opaque user_data object.
* The pointer here is the same one passed in the ceres_add_residual_block().
*/
typedef int (*ceres_cost_function_t)(void* user_data,
double** parameters,
double* residuals,
double** jacobians);
/* Equivalent to LossFunction::Evaluate() from the C++ API. */
typedef void (*ceres_loss_function_t)(void* user_data,
double squared_norm,
double out[3]);
/* Create callback data for Ceres' stock loss functions.
*
* Ceres has several loss functions available by default, and these functions
* expose those to the C API. To use the stock loss functions, call
* ceres_create_*_loss_data(), which internally creates an instance of one of
* the stock loss functions (for example ceres::CauchyLoss), and pass the
* returned "loss_function_data" along with the ceres_stock_loss_function to
* ceres_add_residual_block().
*
* For example:
*
* void* cauchy_loss_function_data =
* ceres_create_cauchy_loss_function_data(1.2, 0.0);
* ceres_problem_add_residual_block(
* problem,
* my_cost_function,
* my_cost_function_data,
* ceres_stock_loss_function,
* cauchy_loss_function_data,
* 1,
* 2,
* parameter_sizes,
* parameter_pointers);
* ...
* ceres_free_stock_loss_function_data(cauchy_loss_function_data);
*
* See loss_function.h for the details of each loss function.
*/
CERES_EXPORT void* ceres_create_huber_loss_function_data(double a);
CERES_EXPORT void* ceres_create_softl1_loss_function_data(double a);
CERES_EXPORT void* ceres_create_cauchy_loss_function_data(double a);
CERES_EXPORT void* ceres_create_arctan_loss_function_data(double a);
CERES_EXPORT void* ceres_create_tolerant_loss_function_data(double a, double b);
/* Free the given stock loss function data. */
CERES_EXPORT void ceres_free_stock_loss_function_data(void* loss_function_data);
/* This is an implementation of ceres_loss_function_t contained within Ceres
* itself, intended as a way to access the various stock Ceres loss functions
* from the C API. This should be passed to ceres_add_residual() below, in
* combination with a user_data pointer generated by
* ceres_create_stock_loss_function() above. */
CERES_EXPORT void ceres_stock_loss_function(void* user_data,
double squared_norm,
double out[3]);
/* Equivalent to Problem from the C++ API. */
struct ceres_problem_s;
typedef struct ceres_problem_s ceres_problem_t;
struct ceres_residual_block_id_s;
typedef struct ceres_residual_block_id_s ceres_residual_block_id_t;
/* Create and destroy a problem */
/* TODO(keir): Add options for the problem. */
CERES_EXPORT ceres_problem_t* ceres_create_problem();
CERES_EXPORT void ceres_free_problem(ceres_problem_t* problem);
/* Add a residual block. */
CERES_EXPORT ceres_residual_block_id_t* ceres_problem_add_residual_block(
ceres_problem_t* problem,
ceres_cost_function_t cost_function,
void* cost_function_data,
ceres_loss_function_t loss_function,
void* loss_function_data,
int num_residuals,
int num_parameter_blocks,
int* parameter_block_sizes,
double** parameters);
CERES_EXPORT void ceres_solve(ceres_problem_t* problem);
/* TODO(keir): Figure out a way to pass a config in. */
#ifdef __cplusplus
}
#endif
#include "ceres/internal/reenable_warnings.h"
#endif /* CERES_PUBLIC_C_API_H_ */

View File

@ -0,0 +1,59 @@
// 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)
//
// This is a forwarding header containing the public symbols exported from
// Ceres. Anything in the "ceres" namespace is available for use.
#ifndef CERES_PUBLIC_CERES_H_
#define CERES_PUBLIC_CERES_H_
#include "ceres/autodiff_cost_function.h"
#include "ceres/autodiff_local_parameterization.h"
#include "ceres/cost_function.h"
#include "ceres/cost_function_to_functor.h"
#include "ceres/covariance.h"
#include "ceres/crs_matrix.h"
#include "ceres/dynamic_autodiff_cost_function.h"
#include "ceres/dynamic_numeric_diff_cost_function.h"
#include "ceres/gradient_problem.h"
#include "ceres/gradient_problem_solver.h"
#include "ceres/iteration_callback.h"
#include "ceres/jet.h"
#include "ceres/local_parameterization.h"
#include "ceres/loss_function.h"
#include "ceres/numeric_diff_cost_function.h"
#include "ceres/ordered_groups.h"
#include "ceres/problem.h"
#include "ceres/sized_cost_function.h"
#include "ceres/solver.h"
#include "ceres/types.h"
#include "ceres/version.h"
#endif // CERES_PUBLIC_CERES_H_

View File

@ -0,0 +1,99 @@
// 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: wjr@google.com (William Rucklidge)
//
// This file contains a cost function that can apply a transformation to
// each residual value before they are square-summed.
#ifndef CERES_PUBLIC_CONDITIONED_COST_FUNCTION_H_
#define CERES_PUBLIC_CONDITIONED_COST_FUNCTION_H_
#include <vector>
#include "ceres/cost_function.h"
#include "ceres/internal/scoped_ptr.h"
#include "ceres/types.h"
#include "ceres/internal/disable_warnings.h"
namespace ceres {
// This class allows you to apply different conditioning to the residual
// values of a wrapped cost function. An example where this is useful is
// where you have an existing cost function that produces N values, but you
// want the total cost to be something other than just the sum of these
// squared values - maybe you want to apply a different scaling to some
// values, to change their contribution to the cost.
//
// Usage:
//
// // my_cost_function produces N residuals
// CostFunction* my_cost_function = ...
// CHECK_EQ(N, my_cost_function->num_residuals());
// vector<CostFunction*> conditioners;
//
// // Make N 1x1 cost functions (1 parameter, 1 residual)
// CostFunction* f_1 = ...
// conditioners.push_back(f_1);
// ...
// CostFunction* f_N = ...
// conditioners.push_back(f_N);
// ConditionedCostFunction* ccf =
// new ConditionedCostFunction(my_cost_function, conditioners);
//
// Now ccf's residual i (i=0..N-1) will be passed though the i'th conditioner.
//
// ccf_residual[i] = f_i(my_cost_function_residual[i])
//
// and the Jacobian will be affected appropriately.
class CERES_EXPORT ConditionedCostFunction : public CostFunction {
public:
// Builds a cost function based on a wrapped cost function, and a
// per-residual conditioner. Takes ownership of all of the wrapped cost
// functions, or not, depending on the ownership parameter. Conditioners
// may be NULL, in which case the corresponding residual is not modified.
ConditionedCostFunction(CostFunction* wrapped_cost_function,
const std::vector<CostFunction*>& conditioners,
Ownership ownership);
virtual ~ConditionedCostFunction();
virtual bool Evaluate(double const* const* parameters,
double* residuals,
double** jacobians) const;
private:
internal::scoped_ptr<CostFunction> wrapped_cost_function_;
std::vector<CostFunction*> conditioners_;
Ownership ownership_;
};
} // namespace ceres
#include "ceres/internal/reenable_warnings.h"
#endif // CERES_PUBLIC_CONDITIONED_COST_FUNCTION_H_

View File

@ -0,0 +1,147 @@
// 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)
// keir@google.m (Keir Mierle)
//
// This is the interface through which the least squares solver accesses the
// residual and Jacobian of the least squares problem. Users are expected to
// subclass CostFunction to define their own terms in the least squares problem.
//
// It is recommended that users define templated residual functors for use as
// arguments for AutoDiffCostFunction (see autodiff_cost_function.h), instead of
// directly implementing the CostFunction interface. This often results in both
// shorter code and faster execution than hand-coded derivatives. However,
// specialized cases may demand direct implementation of the lower-level
// CostFunction interface; for example, this is true when calling legacy code
// which is not templated on numeric types.
#ifndef CERES_PUBLIC_COST_FUNCTION_H_
#define CERES_PUBLIC_COST_FUNCTION_H_
#include <vector>
#include "ceres/internal/macros.h"
#include "ceres/internal/port.h"
#include "ceres/types.h"
#include "ceres/internal/disable_warnings.h"
namespace ceres {
// This class implements the computation of the cost (a.k.a. residual) terms as
// a function of the input (control) variables, and is the interface for users
// to describe their least squares problem to Ceres. In other words, this is the
// modelling layer between users and the Ceres optimizer. The signature of the
// function (number and sizes of input parameter blocks and number of outputs)
// is stored in parameter_block_sizes_ and num_residuals_ respectively. User
// code inheriting from this class is expected to set these two members with the
// corresponding accessors. This information will be verified by the Problem
// when added with AddResidualBlock().
class CERES_EXPORT CostFunction {
public:
CostFunction() : num_residuals_(0) {}
virtual ~CostFunction() {}
// Inputs:
//
// parameters is an array of pointers to arrays containing the
// various parameter blocks. parameters has the same number of
// elements as parameter_block_sizes_. Parameter blocks are in the
// same order as parameter_block_sizes_.i.e.,
//
// parameters_[i] = double[parameter_block_sizes_[i]]
//
// Outputs:
//
// residuals is an array of size num_residuals_.
//
// jacobians is an array of size parameter_block_sizes_ containing
// pointers to storage for jacobian blocks corresponding to each
// parameter block. Jacobian blocks are in the same order as
// parameter_block_sizes, i.e. jacobians[i], is an
// array that contains num_residuals_* parameter_block_sizes_[i]
// elements. Each jacobian block is stored in row-major order, i.e.,
//
// jacobians[i][r*parameter_block_size_[i] + c] =
// d residual[r] / d parameters[i][c]
//
// If jacobians is NULL, then no derivatives are returned; this is
// the case when computing cost only. If jacobians[i] is NULL, then
// the jacobian block corresponding to the i'th parameter block must
// not to be returned.
//
// The return value indicates whether the computation of the
// residuals and/or jacobians was successful or not.
//
// This can be used to communicate numerical failures in jacobian
// computations for instance.
//
// A more interesting and common use is to impose constraints on the
// parameters. If the initial values of the parameter blocks satisfy
// the constraints, then returning false whenever the constraints
// are not satisfied will prevent the solver from moving into the
// infeasible region. This is not a very sophisticated mechanism for
// enforcing constraints, but is often good enough.
//
// Note that it is important that the initial values of the
// parameter block must be feasible, otherwise the solver will
// declare a numerical problem at iteration 0.
virtual bool Evaluate(double const* const* parameters,
double* residuals,
double** jacobians) const = 0;
const std::vector<int32>& parameter_block_sizes() const {
return parameter_block_sizes_;
}
int num_residuals() const {
return num_residuals_;
}
protected:
std::vector<int32>* mutable_parameter_block_sizes() {
return &parameter_block_sizes_;
}
void set_num_residuals(int num_residuals) {
num_residuals_ = num_residuals;
}
private:
// Cost function signature metadata: number of inputs & their sizes,
// number of outputs (residuals).
std::vector<int32> parameter_block_sizes_;
int num_residuals_;
CERES_DISALLOW_COPY_AND_ASSIGN(CostFunction);
};
} // namespace ceres
#include "ceres/internal/reenable_warnings.h"
#endif // CERES_PUBLIC_COST_FUNCTION_H_

View File

@ -0,0 +1,682 @@
// 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)
//
// CostFunctionToFunctor is an adapter class that allows users to use
// SizedCostFunction objects in templated functors which are to be used for
// automatic differentiation. This allows the user to seamlessly mix
// analytic, numeric and automatic differentiation.
//
// For example, let us assume that
//
// class IntrinsicProjection : public SizedCostFunction<2, 5, 3> {
// public:
// IntrinsicProjection(const double* observation);
// virtual bool Evaluate(double const* const* parameters,
// double* residuals,
// double** jacobians) const;
// };
//
// is a cost function that implements the projection of a point in its
// local coordinate system onto its image plane and subtracts it from
// the observed point projection. It can compute its residual and
// either via analytic or numerical differentiation can compute its
// jacobians.
//
// Now we would like to compose the action of this CostFunction with
// the action of camera extrinsics, i.e., rotation and
// translation. Say we have a templated function
//
// template<typename T>
// void RotateAndTranslatePoint(const T* rotation,
// const T* translation,
// const T* point,
// T* result);
//
// Then we can now do the following,
//
// struct CameraProjection {
// CameraProjection(const double* observation)
// : intrinsic_projection_(new IntrinsicProjection(observation)) {
// }
// template <typename T>
// bool operator()(const T* rotation,
// const T* translation,
// const T* intrinsics,
// const T* point,
// T* residual) const {
// T transformed_point[3];
// RotateAndTranslatePoint(rotation, translation, point, transformed_point);
//
// // Note that we call intrinsic_projection_, just like it was
// // any other templated functor.
//
// return intrinsic_projection_(intrinsics, transformed_point, residual);
// }
//
// private:
// CostFunctionToFunctor<2,5,3> intrinsic_projection_;
// };
#ifndef CERES_PUBLIC_COST_FUNCTION_TO_FUNCTOR_H_
#define CERES_PUBLIC_COST_FUNCTION_TO_FUNCTOR_H_
#include <numeric>
#include <vector>
#include "ceres/cost_function.h"
#include "ceres/dynamic_cost_function_to_functor.h"
#include "ceres/internal/fixed_array.h"
#include "ceres/internal/port.h"
#include "ceres/internal/scoped_ptr.h"
namespace ceres {
template <int kNumResiduals,
int N0, 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>
class CostFunctionToFunctor {
public:
// Takes ownership of cost_function.
explicit CostFunctionToFunctor(CostFunction* cost_function)
: cost_functor_(cost_function) {
CHECK_NOTNULL(cost_function);
CHECK(kNumResiduals > 0 || kNumResiduals == DYNAMIC);
// This block breaks the 80 column rule to keep it somewhat readable.
CHECK((!N1 && !N2 && !N3 && !N4 && !N5 && !N6 && !N7 && !N8 && !N9) ||
((N1 > 0) && !N2 && !N3 && !N4 && !N5 && !N6 && !N7 && !N8 && !N9) ||
((N1 > 0) && (N2 > 0) && !N3 && !N4 && !N5 && !N6 && !N7 && !N8 && !N9) || // NOLINT
((N1 > 0) && (N2 > 0) && (N3 > 0) && !N4 && !N5 && !N6 && !N7 && !N8 && !N9) || // NOLINT
((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && !N5 && !N6 && !N7 && !N8 && !N9) || // NOLINT
((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && (N5 > 0) && !N6 && !N7 && !N8 && !N9) || // NOLINT
((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && (N5 > 0) && (N6 > 0) && !N7 && !N8 && !N9) || // NOLINT
((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && (N5 > 0) && (N6 > 0) && (N7 > 0) && !N8 && !N9) || // NOLINT
((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && (N5 > 0) && (N6 > 0) && (N7 > 0) && (N8 > 0) && !N9) || // NOLINT
((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && (N5 > 0) && (N6 > 0) && (N7 > 0) && (N8 > 0) && (N9 > 0))) // NOLINT
<< "Zero block cannot precede a non-zero block. Block sizes are "
<< "(ignore trailing 0s): " << N0 << ", " << N1 << ", " << N2 << ", "
<< N3 << ", " << N4 << ", " << N5 << ", " << N6 << ", " << N7 << ", "
<< N8 << ", " << N9;
const std::vector<int32>& parameter_block_sizes =
cost_function->parameter_block_sizes();
const int num_parameter_blocks =
(N0 > 0) + (N1 > 0) + (N2 > 0) + (N3 > 0) + (N4 > 0) +
(N5 > 0) + (N6 > 0) + (N7 > 0) + (N8 > 0) + (N9 > 0);
CHECK_EQ(parameter_block_sizes.size(), num_parameter_blocks);
CHECK_EQ(N0, parameter_block_sizes[0]);
if (parameter_block_sizes.size() > 1) CHECK_EQ(N1, parameter_block_sizes[1]); // NOLINT
if (parameter_block_sizes.size() > 2) CHECK_EQ(N2, parameter_block_sizes[2]); // NOLINT
if (parameter_block_sizes.size() > 3) CHECK_EQ(N3, parameter_block_sizes[3]); // NOLINT
if (parameter_block_sizes.size() > 4) CHECK_EQ(N4, parameter_block_sizes[4]); // NOLINT
if (parameter_block_sizes.size() > 5) CHECK_EQ(N5, parameter_block_sizes[5]); // NOLINT
if (parameter_block_sizes.size() > 6) CHECK_EQ(N6, parameter_block_sizes[6]); // NOLINT
if (parameter_block_sizes.size() > 7) CHECK_EQ(N7, parameter_block_sizes[7]); // NOLINT
if (parameter_block_sizes.size() > 8) CHECK_EQ(N8, parameter_block_sizes[8]); // NOLINT
if (parameter_block_sizes.size() > 9) CHECK_EQ(N9, parameter_block_sizes[9]); // NOLINT
CHECK_EQ(accumulate(parameter_block_sizes.begin(),
parameter_block_sizes.end(), 0),
N0 + N1 + N2 + N3 + N4 + N5 + N6 + N7 + N8 + N9);
}
bool operator()(const double* x0, double* residuals) const {
CHECK_NE(N0, 0);
CHECK_EQ(N1, 0);
CHECK_EQ(N2, 0);
CHECK_EQ(N3, 0);
CHECK_EQ(N4, 0);
CHECK_EQ(N5, 0);
CHECK_EQ(N6, 0);
CHECK_EQ(N7, 0);
CHECK_EQ(N8, 0);
CHECK_EQ(N9, 0);
return cost_functor_(&x0, residuals);
}
bool operator()(const double* x0,
const double* x1,
double* residuals) const {
CHECK_NE(N0, 0);
CHECK_NE(N1, 0);
CHECK_EQ(N2, 0);
CHECK_EQ(N3, 0);
CHECK_EQ(N4, 0);
CHECK_EQ(N5, 0);
CHECK_EQ(N6, 0);
CHECK_EQ(N7, 0);
CHECK_EQ(N8, 0);
CHECK_EQ(N9, 0);
internal::FixedArray<const double*> parameter_blocks(2);
parameter_blocks[0] = x0;
parameter_blocks[1] = x1;
return cost_functor_(parameter_blocks.get(), residuals);
}
bool operator()(const double* x0,
const double* x1,
const double* x2,
double* residuals) const {
CHECK_NE(N0, 0);
CHECK_NE(N1, 0);
CHECK_NE(N2, 0);
CHECK_EQ(N3, 0);
CHECK_EQ(N4, 0);
CHECK_EQ(N5, 0);
CHECK_EQ(N6, 0);
CHECK_EQ(N7, 0);
CHECK_EQ(N8, 0);
CHECK_EQ(N9, 0);
internal::FixedArray<const double*> parameter_blocks(3);
parameter_blocks[0] = x0;
parameter_blocks[1] = x1;
parameter_blocks[2] = x2;
return cost_functor_(parameter_blocks.get(), residuals);
}
bool operator()(const double* x0,
const double* x1,
const double* x2,
const double* x3,
double* residuals) const {
CHECK_NE(N0, 0);
CHECK_NE(N1, 0);
CHECK_NE(N2, 0);
CHECK_NE(N3, 0);
CHECK_EQ(N4, 0);
CHECK_EQ(N5, 0);
CHECK_EQ(N6, 0);
CHECK_EQ(N7, 0);
CHECK_EQ(N8, 0);
CHECK_EQ(N9, 0);
internal::FixedArray<const double*> parameter_blocks(4);
parameter_blocks[0] = x0;
parameter_blocks[1] = x1;
parameter_blocks[2] = x2;
parameter_blocks[3] = x3;
return cost_functor_(parameter_blocks.get(), residuals);
}
bool operator()(const double* x0,
const double* x1,
const double* x2,
const double* x3,
const double* x4,
double* residuals) const {
CHECK_NE(N0, 0);
CHECK_NE(N1, 0);
CHECK_NE(N2, 0);
CHECK_NE(N3, 0);
CHECK_NE(N4, 0);
CHECK_EQ(N5, 0);
CHECK_EQ(N6, 0);
CHECK_EQ(N7, 0);
CHECK_EQ(N8, 0);
CHECK_EQ(N9, 0);
internal::FixedArray<const double*> parameter_blocks(5);
parameter_blocks[0] = x0;
parameter_blocks[1] = x1;
parameter_blocks[2] = x2;
parameter_blocks[3] = x3;
parameter_blocks[4] = x4;
return cost_functor_(parameter_blocks.get(), residuals);
}
bool operator()(const double* x0,
const double* x1,
const double* x2,
const double* x3,
const double* x4,
const double* x5,
double* residuals) const {
CHECK_NE(N0, 0);
CHECK_NE(N1, 0);
CHECK_NE(N2, 0);
CHECK_NE(N3, 0);
CHECK_NE(N4, 0);
CHECK_NE(N5, 0);
CHECK_EQ(N6, 0);
CHECK_EQ(N7, 0);
CHECK_EQ(N8, 0);
CHECK_EQ(N9, 0);
internal::FixedArray<const double*> parameter_blocks(6);
parameter_blocks[0] = x0;
parameter_blocks[1] = x1;
parameter_blocks[2] = x2;
parameter_blocks[3] = x3;
parameter_blocks[4] = x4;
parameter_blocks[5] = x5;
return cost_functor_(parameter_blocks.get(), residuals);
}
bool operator()(const double* x0,
const double* x1,
const double* x2,
const double* x3,
const double* x4,
const double* x5,
const double* x6,
double* residuals) const {
CHECK_NE(N0, 0);
CHECK_NE(N1, 0);
CHECK_NE(N2, 0);
CHECK_NE(N3, 0);
CHECK_NE(N4, 0);
CHECK_NE(N5, 0);
CHECK_NE(N6, 0);
CHECK_EQ(N7, 0);
CHECK_EQ(N8, 0);
CHECK_EQ(N9, 0);
internal::FixedArray<const double*> parameter_blocks(7);
parameter_blocks[0] = x0;
parameter_blocks[1] = x1;
parameter_blocks[2] = x2;
parameter_blocks[3] = x3;
parameter_blocks[4] = x4;
parameter_blocks[5] = x5;
parameter_blocks[6] = x6;
return cost_functor_(parameter_blocks.get(), residuals);
}
bool operator()(const double* x0,
const double* x1,
const double* x2,
const double* x3,
const double* x4,
const double* x5,
const double* x6,
const double* x7,
double* residuals) const {
CHECK_NE(N0, 0);
CHECK_NE(N1, 0);
CHECK_NE(N2, 0);
CHECK_NE(N3, 0);
CHECK_NE(N4, 0);
CHECK_NE(N5, 0);
CHECK_NE(N6, 0);
CHECK_NE(N7, 0);
CHECK_EQ(N8, 0);
CHECK_EQ(N9, 0);
internal::FixedArray<const double*> parameter_blocks(8);
parameter_blocks[0] = x0;
parameter_blocks[1] = x1;
parameter_blocks[2] = x2;
parameter_blocks[3] = x3;
parameter_blocks[4] = x4;
parameter_blocks[5] = x5;
parameter_blocks[6] = x6;
parameter_blocks[7] = x7;
return cost_functor_(parameter_blocks.get(), residuals);
}
bool operator()(const double* x0,
const double* x1,
const double* x2,
const double* x3,
const double* x4,
const double* x5,
const double* x6,
const double* x7,
const double* x8,
double* residuals) const {
CHECK_NE(N0, 0);
CHECK_NE(N1, 0);
CHECK_NE(N2, 0);
CHECK_NE(N3, 0);
CHECK_NE(N4, 0);
CHECK_NE(N5, 0);
CHECK_NE(N6, 0);
CHECK_NE(N7, 0);
CHECK_NE(N8, 0);
CHECK_EQ(N9, 0);
internal::FixedArray<const double*> parameter_blocks(9);
parameter_blocks[0] = x0;
parameter_blocks[1] = x1;
parameter_blocks[2] = x2;
parameter_blocks[3] = x3;
parameter_blocks[4] = x4;
parameter_blocks[5] = x5;
parameter_blocks[6] = x6;
parameter_blocks[7] = x7;
parameter_blocks[8] = x8;
return cost_functor_(parameter_blocks.get(), residuals);
}
bool operator()(const double* x0,
const double* x1,
const double* x2,
const double* x3,
const double* x4,
const double* x5,
const double* x6,
const double* x7,
const double* x8,
const double* x9,
double* residuals) const {
CHECK_NE(N0, 0);
CHECK_NE(N1, 0);
CHECK_NE(N2, 0);
CHECK_NE(N3, 0);
CHECK_NE(N4, 0);
CHECK_NE(N5, 0);
CHECK_NE(N6, 0);
CHECK_NE(N7, 0);
CHECK_NE(N8, 0);
CHECK_NE(N9, 0);
internal::FixedArray<const double*> parameter_blocks(10);
parameter_blocks[0] = x0;
parameter_blocks[1] = x1;
parameter_blocks[2] = x2;
parameter_blocks[3] = x3;
parameter_blocks[4] = x4;
parameter_blocks[5] = x5;
parameter_blocks[6] = x6;
parameter_blocks[7] = x7;
parameter_blocks[8] = x8;
parameter_blocks[9] = x9;
return cost_functor_(parameter_blocks.get(), residuals);
}
template <typename JetT>
bool operator()(const JetT* x0, JetT* residuals) const {
CHECK_NE(N0, 0);
CHECK_EQ(N1, 0);
CHECK_EQ(N2, 0);
CHECK_EQ(N3, 0);
CHECK_EQ(N4, 0);
CHECK_EQ(N5, 0);
CHECK_EQ(N6, 0);
CHECK_EQ(N7, 0);
CHECK_EQ(N8, 0);
CHECK_EQ(N9, 0);
return cost_functor_(&x0, residuals);
}
template <typename JetT>
bool operator()(const JetT* x0,
const JetT* x1,
JetT* residuals) const {
CHECK_NE(N0, 0);
CHECK_NE(N1, 0);
CHECK_EQ(N2, 0);
CHECK_EQ(N3, 0);
CHECK_EQ(N4, 0);
CHECK_EQ(N5, 0);
CHECK_EQ(N6, 0);
CHECK_EQ(N7, 0);
CHECK_EQ(N8, 0);
CHECK_EQ(N9, 0);
internal::FixedArray<const JetT*> jets(2);
jets[0] = x0;
jets[1] = x1;
return cost_functor_(jets.get(), residuals);
}
template <typename JetT>
bool operator()(const JetT* x0,
const JetT* x1,
const JetT* x2,
JetT* residuals) const {
CHECK_NE(N0, 0);
CHECK_NE(N1, 0);
CHECK_NE(N2, 0);
CHECK_EQ(N3, 0);
CHECK_EQ(N4, 0);
CHECK_EQ(N5, 0);
CHECK_EQ(N6, 0);
CHECK_EQ(N7, 0);
CHECK_EQ(N8, 0);
CHECK_EQ(N9, 0);
internal::FixedArray<const JetT*> jets(3);
jets[0] = x0;
jets[1] = x1;
jets[2] = x2;
return cost_functor_(jets.get(), residuals);
}
template <typename JetT>
bool operator()(const JetT* x0,
const JetT* x1,
const JetT* x2,
const JetT* x3,
JetT* residuals) const {
CHECK_NE(N0, 0);
CHECK_NE(N1, 0);
CHECK_NE(N2, 0);
CHECK_NE(N3, 0);
CHECK_EQ(N4, 0);
CHECK_EQ(N5, 0);
CHECK_EQ(N6, 0);
CHECK_EQ(N7, 0);
CHECK_EQ(N8, 0);
CHECK_EQ(N9, 0);
internal::FixedArray<const JetT*> jets(4);
jets[0] = x0;
jets[1] = x1;
jets[2] = x2;
jets[3] = x3;
return cost_functor_(jets.get(), residuals);
}
template <typename JetT>
bool operator()(const JetT* x0,
const JetT* x1,
const JetT* x2,
const JetT* x3,
const JetT* x4,
JetT* residuals) const {
CHECK_NE(N0, 0);
CHECK_NE(N1, 0);
CHECK_NE(N2, 0);
CHECK_NE(N3, 0);
CHECK_NE(N4, 0);
CHECK_EQ(N5, 0);
CHECK_EQ(N6, 0);
CHECK_EQ(N7, 0);
CHECK_EQ(N8, 0);
CHECK_EQ(N9, 0);
internal::FixedArray<const JetT*> jets(5);
jets[0] = x0;
jets[1] = x1;
jets[2] = x2;
jets[3] = x3;
jets[4] = x4;
return cost_functor_(jets.get(), residuals);
}
template <typename JetT>
bool operator()(const JetT* x0,
const JetT* x1,
const JetT* x2,
const JetT* x3,
const JetT* x4,
const JetT* x5,
JetT* residuals) const {
CHECK_NE(N0, 0);
CHECK_NE(N1, 0);
CHECK_NE(N2, 0);
CHECK_NE(N3, 0);
CHECK_NE(N4, 0);
CHECK_NE(N5, 0);
CHECK_EQ(N6, 0);
CHECK_EQ(N7, 0);
CHECK_EQ(N8, 0);
CHECK_EQ(N9, 0);
internal::FixedArray<const JetT*> jets(6);
jets[0] = x0;
jets[1] = x1;
jets[2] = x2;
jets[3] = x3;
jets[4] = x4;
jets[5] = x5;
return cost_functor_(jets.get(), residuals);
}
template <typename JetT>
bool operator()(const JetT* x0,
const JetT* x1,
const JetT* x2,
const JetT* x3,
const JetT* x4,
const JetT* x5,
const JetT* x6,
JetT* residuals) const {
CHECK_NE(N0, 0);
CHECK_NE(N1, 0);
CHECK_NE(N2, 0);
CHECK_NE(N3, 0);
CHECK_NE(N4, 0);
CHECK_NE(N5, 0);
CHECK_NE(N6, 0);
CHECK_EQ(N7, 0);
CHECK_EQ(N8, 0);
CHECK_EQ(N9, 0);
internal::FixedArray<const JetT*> jets(7);
jets[0] = x0;
jets[1] = x1;
jets[2] = x2;
jets[3] = x3;
jets[4] = x4;
jets[5] = x5;
jets[6] = x6;
return cost_functor_(jets.get(), residuals);
}
template <typename JetT>
bool operator()(const JetT* x0,
const JetT* x1,
const JetT* x2,
const JetT* x3,
const JetT* x4,
const JetT* x5,
const JetT* x6,
const JetT* x7,
JetT* residuals) const {
CHECK_NE(N0, 0);
CHECK_NE(N1, 0);
CHECK_NE(N2, 0);
CHECK_NE(N3, 0);
CHECK_NE(N4, 0);
CHECK_NE(N5, 0);
CHECK_NE(N6, 0);
CHECK_NE(N7, 0);
CHECK_EQ(N8, 0);
CHECK_EQ(N9, 0);
internal::FixedArray<const JetT*> jets(8);
jets[0] = x0;
jets[1] = x1;
jets[2] = x2;
jets[3] = x3;
jets[4] = x4;
jets[5] = x5;
jets[6] = x6;
jets[7] = x7;
return cost_functor_(jets.get(), residuals);
}
template <typename JetT>
bool operator()(const JetT* x0,
const JetT* x1,
const JetT* x2,
const JetT* x3,
const JetT* x4,
const JetT* x5,
const JetT* x6,
const JetT* x7,
const JetT* x8,
JetT* residuals) const {
CHECK_NE(N0, 0);
CHECK_NE(N1, 0);
CHECK_NE(N2, 0);
CHECK_NE(N3, 0);
CHECK_NE(N4, 0);
CHECK_NE(N5, 0);
CHECK_NE(N6, 0);
CHECK_NE(N7, 0);
CHECK_NE(N8, 0);
CHECK_EQ(N9, 0);
internal::FixedArray<const JetT*> jets(9);
jets[0] = x0;
jets[1] = x1;
jets[2] = x2;
jets[3] = x3;
jets[4] = x4;
jets[5] = x5;
jets[6] = x6;
jets[7] = x7;
jets[8] = x8;
return cost_functor_(jets.get(), residuals);
}
template <typename JetT>
bool operator()(const JetT* x0,
const JetT* x1,
const JetT* x2,
const JetT* x3,
const JetT* x4,
const JetT* x5,
const JetT* x6,
const JetT* x7,
const JetT* x8,
const JetT* x9,
JetT* residuals) const {
CHECK_NE(N0, 0);
CHECK_NE(N1, 0);
CHECK_NE(N2, 0);
CHECK_NE(N3, 0);
CHECK_NE(N4, 0);
CHECK_NE(N5, 0);
CHECK_NE(N6, 0);
CHECK_NE(N7, 0);
CHECK_NE(N8, 0);
CHECK_NE(N9, 0);
internal::FixedArray<const JetT*> jets(10);
jets[0] = x0;
jets[1] = x1;
jets[2] = x2;
jets[3] = x3;
jets[4] = x4;
jets[5] = x5;
jets[6] = x6;
jets[7] = x7;
jets[8] = x8;
jets[9] = x9;
return cost_functor_(jets.get(), residuals);
}
private:
DynamicCostFunctionToFunctor cost_functor_;
};
} // namespace ceres
#endif // CERES_PUBLIC_COST_FUNCTION_TO_FUNCTOR_H_

View File

@ -0,0 +1,405 @@
// 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_PUBLIC_COVARIANCE_H_
#define CERES_PUBLIC_COVARIANCE_H_
#include <utility>
#include <vector>
#include "ceres/internal/port.h"
#include "ceres/internal/scoped_ptr.h"
#include "ceres/types.h"
#include "ceres/internal/disable_warnings.h"
namespace ceres {
class Problem;
namespace internal {
class CovarianceImpl;
} // namespace internal
// WARNING
// =======
// It is very easy to use this class incorrectly without understanding
// the underlying mathematics. Please read and understand the
// documentation completely before attempting to use this class.
//
//
// This class allows the user to evaluate the covariance for a
// non-linear least squares problem and provides random access to its
// blocks
//
// Background
// ==========
// One way to assess the quality of the solution returned by a
// non-linear least squares solve is to analyze the covariance of the
// solution.
//
// Let us consider the non-linear regression problem
//
// y = f(x) + N(0, I)
//
// i.e., the observation y is a random non-linear function of the
// independent variable x with mean f(x) and identity covariance. Then
// the maximum likelihood estimate of x given observations y is the
// solution to the non-linear least squares problem:
//
// x* = arg min_x |f(x)|^2
//
// And the covariance of x* is given by
//
// C(x*) = inverse[J'(x*)J(x*)]
//
// Here J(x*) is the Jacobian of f at x*. The above formula assumes
// that J(x*) has full column rank.
//
// If J(x*) is rank deficient, then the covariance matrix C(x*) is
// also rank deficient and is given by
//
// C(x*) = pseudoinverse[J'(x*)J(x*)]
//
// Note that in the above, we assumed that the covariance
// matrix for y was identity. This is an important assumption. If this
// is not the case and we have
//
// y = f(x) + N(0, S)
//
// Where S is a positive semi-definite matrix denoting the covariance
// of y, then the maximum likelihood problem to be solved is
//
// x* = arg min_x f'(x) inverse[S] f(x)
//
// and the corresponding covariance estimate of x* is given by
//
// C(x*) = inverse[J'(x*) inverse[S] J(x*)]
//
// So, if it is the case that the observations being fitted to have a
// covariance matrix not equal to identity, then it is the user's
// responsibility that the corresponding cost functions are correctly
// scaled, e.g. in the above case the cost function for this problem
// should evaluate S^{-1/2} f(x) instead of just f(x), where S^{-1/2}
// is the inverse square root of the covariance matrix S.
//
// This class allows the user to evaluate the covariance for a
// non-linear least squares problem and provides random access to its
// blocks. The computation assumes that the CostFunctions compute
// residuals such that their covariance is identity.
//
// Since the computation of the covariance matrix requires computing
// the inverse of a potentially large matrix, this can involve a
// rather large amount of time and memory. However, it is usually the
// case that the user is only interested in a small part of the
// covariance matrix. Quite often just the block diagonal. This class
// allows the user to specify the parts of the covariance matrix that
// she is interested in and then uses this information to only compute
// and store those parts of the covariance matrix.
//
// Rank of the Jacobian
// --------------------
// As we noted above, if the jacobian is rank deficient, then the
// inverse of J'J is not defined and instead a pseudo inverse needs to
// be computed.
//
// The rank deficiency in J can be structural -- columns which are
// always known to be zero or numerical -- depending on the exact
// values in the Jacobian.
//
// Structural rank deficiency occurs when the problem contains
// parameter blocks that are constant. This class correctly handles
// structural rank deficiency like that.
//
// Numerical rank deficiency, where the rank of the matrix cannot be
// predicted by its sparsity structure and requires looking at its
// numerical values is more complicated. Here again there are two
// cases.
//
// a. The rank deficiency arises from overparameterization. e.g., a
// four dimensional quaternion used to parameterize SO(3), which is
// a three dimensional manifold. In cases like this, the user should
// use an appropriate LocalParameterization. Not only will this lead
// to better numerical behaviour of the Solver, it will also expose
// the rank deficiency to the Covariance object so that it can
// handle it correctly.
//
// b. More general numerical rank deficiency in the Jacobian
// requires the computation of the so called Singular Value
// Decomposition (SVD) of J'J. We do not know how to do this for
// large sparse matrices efficiently. For small and moderate sized
// problems this is done using dense linear algebra.
//
// Gauge Invariance
// ----------------
// In structure from motion (3D reconstruction) problems, the
// reconstruction is ambiguous upto a similarity transform. This is
// known as a Gauge Ambiguity. Handling Gauges correctly requires the
// use of SVD or custom inversion algorithms. For small problems the
// user can use the dense algorithm. For more details see
//
// Ken-ichi Kanatani, Daniel D. Morris: Gauges and gauge
// transformations for uncertainty description of geometric structure
// with indeterminacy. IEEE Transactions on Information Theory 47(5):
// 2017-2028 (2001)
//
// Example Usage
// =============
//
// double x[3];
// double y[2];
//
// Problem problem;
// problem.AddParameterBlock(x, 3);
// problem.AddParameterBlock(y, 2);
// <Build Problem>
// <Solve Problem>
//
// Covariance::Options options;
// Covariance covariance(options);
//
// std::vector<std::pair<const double*, const double*> > covariance_blocks;
// covariance_blocks.push_back(make_pair(x, x));
// covariance_blocks.push_back(make_pair(y, y));
// covariance_blocks.push_back(make_pair(x, y));
//
// CHECK(covariance.Compute(covariance_blocks, &problem));
//
// double covariance_xx[3 * 3];
// double covariance_yy[2 * 2];
// double covariance_xy[3 * 2];
// covariance.GetCovarianceBlock(x, x, covariance_xx)
// covariance.GetCovarianceBlock(y, y, covariance_yy)
// covariance.GetCovarianceBlock(x, y, covariance_xy)
//
class CERES_EXPORT Covariance {
public:
struct CERES_EXPORT Options {
Options()
#ifndef CERES_NO_SUITESPARSE
: algorithm_type(SUITE_SPARSE_QR),
#else
: algorithm_type(EIGEN_SPARSE_QR),
#endif
min_reciprocal_condition_number(1e-14),
null_space_rank(0),
num_threads(1),
apply_loss_function(true) {
}
// Ceres supports three different algorithms for covariance
// estimation, which represent different tradeoffs in speed,
// accuracy and reliability.
//
// 1. DENSE_SVD uses Eigen's JacobiSVD to perform the
// computations. It computes the singular value decomposition
//
// U * S * V' = J
//
// and then uses it to compute the pseudo inverse of J'J as
//
// pseudoinverse[J'J]^ = V * pseudoinverse[S] * V'
//
// It is an accurate but slow method and should only be used
// for small to moderate sized problems. It can handle
// full-rank as well as rank deficient Jacobians.
//
// 2. EIGEN_SPARSE_QR uses the sparse QR factorization algorithm
// in Eigen to compute the decomposition
//
// Q * R = J
//
// [J'J]^-1 = [R*R']^-1
//
// It is a moderately fast algorithm for sparse matrices.
//
// 3. SUITE_SPARSE_QR uses the SuiteSparseQR sparse QR
// factorization algorithm. It uses dense linear algebra and is
// multi threaded, so for large sparse sparse matrices it is
// significantly faster than EIGEN_SPARSE_QR.
//
// Neither EIGEN_SPARSE_QR not SUITE_SPARSE_QR are capable of
// computing the covariance if the Jacobian is rank deficient.
CovarianceAlgorithmType algorithm_type;
// If the Jacobian matrix is near singular, then inverting J'J
// will result in unreliable results, e.g, if
//
// J = [1.0 1.0 ]
// [1.0 1.0000001 ]
//
// which is essentially a rank deficient matrix, we have
//
// inv(J'J) = [ 2.0471e+14 -2.0471e+14]
// [-2.0471e+14 2.0471e+14]
//
// This is not a useful result. Therefore, by default
// Covariance::Compute will return false if a rank deficient
// Jacobian is encountered. How rank deficiency is detected
// depends on the algorithm being used.
//
// 1. DENSE_SVD
//
// min_sigma / max_sigma < sqrt(min_reciprocal_condition_number)
//
// where min_sigma and max_sigma are the minimum and maxiumum
// singular values of J respectively.
//
// 2. SUITE_SPARSE_QR and EIGEN_SPARSE_QR
//
// rank(J) < num_col(J)
//
// Here rank(J) is the estimate of the rank of J returned by the
// sparse QR factorization algorithm. It is a fairly reliable
// indication of rank deficiency.
//
double min_reciprocal_condition_number;
// When using DENSE_SVD, the user has more control in dealing with
// singular and near singular covariance matrices.
//
// As mentioned above, when the covariance matrix is near
// singular, instead of computing the inverse of J'J, the
// Moore-Penrose pseudoinverse of J'J should be computed.
//
// If J'J has the eigen decomposition (lambda_i, e_i), where
// lambda_i is the i^th eigenvalue and e_i is the corresponding
// eigenvector, then the inverse of J'J is
//
// inverse[J'J] = sum_i e_i e_i' / lambda_i
//
// and computing the pseudo inverse involves dropping terms from
// this sum that correspond to small eigenvalues.
//
// How terms are dropped is controlled by
// min_reciprocal_condition_number and null_space_rank.
//
// If null_space_rank is non-negative, then the smallest
// null_space_rank eigenvalue/eigenvectors are dropped
// irrespective of the magnitude of lambda_i. If the ratio of the
// smallest non-zero eigenvalue to the largest eigenvalue in the
// truncated matrix is still below
// min_reciprocal_condition_number, then the Covariance::Compute()
// will fail and return false.
//
// Setting null_space_rank = -1 drops all terms for which
//
// lambda_i / lambda_max < min_reciprocal_condition_number.
//
// This option has no effect on the SUITE_SPARSE_QR and
// EIGEN_SPARSE_QR algorithms.
int null_space_rank;
int num_threads;
// Even though the residual blocks in the problem may contain loss
// functions, setting apply_loss_function to false will turn off
// the application of the loss function to the output of the cost
// function and in turn its effect on the covariance.
//
// TODO(sameergaarwal): Expand this based on Jim's experiments.
bool apply_loss_function;
};
explicit Covariance(const Options& options);
~Covariance();
// Compute a part of the covariance matrix.
//
// The vector covariance_blocks, indexes into the covariance matrix
// block-wise using pairs of parameter blocks. This allows the
// covariance estimation algorithm to only compute and store these
// blocks.
//
// Since the covariance matrix is symmetric, if the user passes
// (block1, block2), then GetCovarianceBlock can be called with
// block1, block2 as well as block2, block1.
//
// covariance_blocks cannot contain duplicates. Bad things will
// happen if they do.
//
// Note that the list of covariance_blocks is only used to determine
// what parts of the covariance matrix are computed. The full
// Jacobian is used to do the computation, i.e. they do not have an
// impact on what part of the Jacobian is used for computation.
//
// The return value indicates the success or failure of the
// covariance computation. Please see the documentation for
// Covariance::Options for more on the conditions under which this
// function returns false.
bool Compute(
const std::vector<std::pair<const double*,
const double*> >& covariance_blocks,
Problem* problem);
// Return the block of the cross-covariance matrix corresponding to
// parameter_block1 and parameter_block2.
//
// Compute must be called before the first call to
// GetCovarianceBlock and the pair <parameter_block1,
// parameter_block2> OR the pair <parameter_block2,
// parameter_block1> must have been present in the vector
// covariance_blocks when Compute was called. Otherwise
// GetCovarianceBlock will return false.
//
// covariance_block must point to a memory location that can store a
// parameter_block1_size x parameter_block2_size matrix. The
// returned covariance will be a row-major matrix.
bool GetCovarianceBlock(const double* parameter_block1,
const double* parameter_block2,
double* covariance_block) const;
// Return the block of the cross-covariance matrix corresponding to
// parameter_block1 and parameter_block2.
// Returns cross-covariance in the tangent space if a local
// parameterization is associated with either parameter block;
// else returns cross-covariance in the ambient space.
//
// Compute must be called before the first call to
// GetCovarianceBlock and the pair <parameter_block1,
// parameter_block2> OR the pair <parameter_block2,
// parameter_block1> must have been present in the vector
// covariance_blocks when Compute was called. Otherwise
// GetCovarianceBlock will return false.
//
// covariance_block must point to a memory location that can store a
// parameter_block1_local_size x parameter_block2_local_size matrix. The
// returned covariance will be a row-major matrix.
bool GetCovarianceBlockInTangentSpace(const double* parameter_block1,
const double* parameter_block2,
double* covariance_block) const;
private:
internal::scoped_ptr<internal::CovarianceImpl> impl_;
};
} // namespace ceres
#include "ceres/internal/reenable_warnings.h"
#endif // CERES_PUBLIC_COVARIANCE_H_

View File

@ -0,0 +1,86 @@
// 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_PUBLIC_CRS_MATRIX_H_
#define CERES_PUBLIC_CRS_MATRIX_H_
#include <vector>
#include "ceres/internal/port.h"
#include "ceres/internal/disable_warnings.h"
namespace ceres {
// A compressed row sparse matrix used primarily for communicating the
// Jacobian matrix to the user.
struct CERES_EXPORT CRSMatrix {
CRSMatrix() : num_rows(0), num_cols(0) {}
int num_rows;
int num_cols;
// A compressed row matrix stores its contents in three arrays,
// rows, cols and values.
//
// rows is a num_rows + 1 sized array that points into the cols and
// values array. For each row i:
//
// cols[rows[i]] ... cols[rows[i + 1] - 1] are the indices of the
// non-zero columns of row i.
//
// values[rows[i]] .. values[rows[i + 1] - 1] are the values of the
// corresponding entries.
//
// cols and values contain as many entries as there are non-zeros in
// the matrix.
//
// e.g, consider the 3x4 sparse matrix
//
// [ 0 10 0 4 ]
// [ 0 2 -3 2 ]
// [ 1 2 0 0 ]
//
// The three arrays will be:
//
//
// -row0- ---row1--- -row2-
// rows = [ 0, 2, 5, 7]
// cols = [ 1, 3, 1, 2, 3, 0, 1]
// values = [10, 4, 2, -3, 2, 1, 2]
std::vector<int> cols;
std::vector<int> rows;
std::vector<double> values;
};
} // namespace ceres
#include "ceres/internal/reenable_warnings.h"
#endif // CERES_PUBLIC_CRS_MATRIX_H_

View File

@ -0,0 +1,441 @@
// 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_PUBLIC_CUBIC_INTERPOLATION_H_
#define CERES_PUBLIC_CUBIC_INTERPOLATION_H_
#include "ceres/internal/port.h"
#include "Eigen/Core"
#include "glog/logging.h"
namespace ceres {
// Given samples from a function sampled at four equally spaced points,
//
// p0 = f(-1)
// p1 = f(0)
// p2 = f(1)
// p3 = f(2)
//
// Evaluate the cubic Hermite spline (also known as the Catmull-Rom
// spline) at a point x that lies in the interval [0, 1].
//
// This is also the interpolation kernel (for the case of a = 0.5) as
// proposed by R. Keys, in:
//
// "Cubic convolution interpolation for digital image processing".
// IEEE Transactions on Acoustics, Speech, and Signal Processing
// 29 (6): 11531160.
//
// For more details see
//
// http://en.wikipedia.org/wiki/Cubic_Hermite_spline
// http://en.wikipedia.org/wiki/Bicubic_interpolation
//
// f if not NULL will contain the interpolated function values.
// dfdx if not NULL will contain the interpolated derivative values.
template <int kDataDimension>
void CubicHermiteSpline(const Eigen::Matrix<double, kDataDimension, 1>& p0,
const Eigen::Matrix<double, kDataDimension, 1>& p1,
const Eigen::Matrix<double, kDataDimension, 1>& p2,
const Eigen::Matrix<double, kDataDimension, 1>& p3,
const double x,
double* f,
double* dfdx) {
DCHECK_GE(x, 0.0);
DCHECK_LE(x, 1.0);
typedef Eigen::Matrix<double, kDataDimension, 1> VType;
const VType a = 0.5 * (-p0 + 3.0 * p1 - 3.0 * p2 + p3);
const VType b = 0.5 * (2.0 * p0 - 5.0 * p1 + 4.0 * p2 - p3);
const VType c = 0.5 * (-p0 + p2);
const VType d = p1;
// Use Horner's rule to evaluate the function value and its
// derivative.
// f = ax^3 + bx^2 + cx + d
if (f != NULL) {
Eigen::Map<VType>(f, kDataDimension) = d + x * (c + x * (b + x * a));
}
// dfdx = 3ax^2 + 2bx + c
if (dfdx != NULL) {
Eigen::Map<VType>(dfdx, kDataDimension) = c + x * (2.0 * b + 3.0 * a * x);
}
}
// Given as input an infinite one dimensional grid, which provides the
// following interface.
//
// class Grid {
// public:
// enum { DATA_DIMENSION = 2; };
// void GetValue(int n, double* f) const;
// };
//
// Here, GetValue gives the value of a function f (possibly vector
// valued) for any integer n.
//
// The enum DATA_DIMENSION indicates the dimensionality of the
// function being interpolated. For example if you are interpolating
// rotations in axis-angle format over time, then DATA_DIMENSION = 3.
//
// CubicInterpolator uses cubic Hermite splines to produce a smooth
// approximation to it that can be used to evaluate the f(x) and f'(x)
// at any point on the real number line.
//
// For more details on cubic interpolation see
//
// http://en.wikipedia.org/wiki/Cubic_Hermite_spline
//
// Example usage:
//
// const double data[] = {1.0, 2.0, 5.0, 6.0};
// Grid1D<double, 1> grid(x, 0, 4);
// CubicInterpolator<Grid1D<double, 1> > interpolator(grid);
// double f, dfdx;
// interpolator.Evaluator(1.5, &f, &dfdx);
template<typename Grid>
class CERES_EXPORT CubicInterpolator {
public:
explicit CubicInterpolator(const Grid& grid)
: grid_(grid) {
// The + casts the enum into an int before doing the
// comparison. It is needed to prevent
// "-Wunnamed-type-template-args" related errors.
CHECK_GE(+Grid::DATA_DIMENSION, 1);
}
void Evaluate(double x, double* f, double* dfdx) const {
const int n = std::floor(x);
Eigen::Matrix<double, Grid::DATA_DIMENSION, 1> p0, p1, p2, p3;
grid_.GetValue(n - 1, p0.data());
grid_.GetValue(n, p1.data());
grid_.GetValue(n + 1, p2.data());
grid_.GetValue(n + 2, p3.data());
CubicHermiteSpline<Grid::DATA_DIMENSION>(p0, p1, p2, p3, x - n, f, dfdx);
}
// The following two Evaluate overloads are needed for interfacing
// with automatic differentiation. The first is for when a scalar
// evaluation is done, and the second one is for when Jets are used.
void Evaluate(const double& x, double* f) const {
Evaluate(x, f, NULL);
}
template<typename JetT> void Evaluate(const JetT& x, JetT* f) const {
double fx[Grid::DATA_DIMENSION], dfdx[Grid::DATA_DIMENSION];
Evaluate(x.a, fx, dfdx);
for (int i = 0; i < Grid::DATA_DIMENSION; ++i) {
f[i].a = fx[i];
f[i].v = dfdx[i] * x.v;
}
}
private:
const Grid& grid_;
};
// An object that implements an infinite one dimensional grid needed
// by the CubicInterpolator where the source of the function values is
// an array of type T on the interval
//
// [begin, ..., end - 1]
//
// Since the input array is finite and the grid is infinite, values
// outside this interval needs to be computed. Grid1D uses the value
// from the nearest edge.
//
// The function being provided can be vector valued, in which case
// kDataDimension > 1. The dimensional slices of the function maybe
// interleaved, or they maybe stacked, i.e, if the function has
// kDataDimension = 2, if kInterleaved = true, then it is stored as
//
// f01, f02, f11, f12 ....
//
// and if kInterleaved = false, then it is stored as
//
// f01, f11, .. fn1, f02, f12, .. , fn2
//
template <typename T,
int kDataDimension = 1,
bool kInterleaved = true>
struct Grid1D {
public:
enum { DATA_DIMENSION = kDataDimension };
Grid1D(const T* data, const int begin, const int end)
: data_(data), begin_(begin), end_(end), num_values_(end - begin) {
CHECK_LT(begin, end);
}
EIGEN_STRONG_INLINE void GetValue(const int n, double* f) const {
const int idx = std::min(std::max(begin_, n), end_ - 1) - begin_;
if (kInterleaved) {
for (int i = 0; i < kDataDimension; ++i) {
f[i] = static_cast<double>(data_[kDataDimension * idx + i]);
}
} else {
for (int i = 0; i < kDataDimension; ++i) {
f[i] = static_cast<double>(data_[i * num_values_ + idx]);
}
}
}
private:
const T* data_;
const int begin_;
const int end_;
const int num_values_;
};
// Given as input an infinite two dimensional grid like object, which
// provides the following interface:
//
// struct Grid {
// enum { DATA_DIMENSION = 1 };
// void GetValue(int row, int col, double* f) const;
// };
//
// Where, GetValue gives us the value of a function f (possibly vector
// valued) for any pairs of integers (row, col), and the enum
// DATA_DIMENSION indicates the dimensionality of the function being
// interpolated. For example if you are interpolating a color image
// with three channels (Red, Green & Blue), then DATA_DIMENSION = 3.
//
// BiCubicInterpolator uses the cubic convolution interpolation
// algorithm of R. Keys, to produce a smooth approximation to it that
// can be used to evaluate the f(r,c), df(r, c)/dr and df(r,c)/dc at
// any point in the real plane.
//
// For more details on the algorithm used here see:
//
// "Cubic convolution interpolation for digital image processing".
// Robert G. Keys, IEEE Trans. on Acoustics, Speech, and Signal
// Processing 29 (6): 11531160, 1981.
//
// http://en.wikipedia.org/wiki/Cubic_Hermite_spline
// http://en.wikipedia.org/wiki/Bicubic_interpolation
//
// Example usage:
//
// const double data[] = {1.0, 3.0, -1.0, 4.0,
// 3.6, 2.1, 4.2, 2.0,
// 2.0, 1.0, 3.1, 5.2};
// Grid2D<double, 1> grid(data, 3, 4);
// BiCubicInterpolator<Grid2D<double, 1> > interpolator(grid);
// double f, dfdr, dfdc;
// interpolator.Evaluate(1.2, 2.5, &f, &dfdr, &dfdc);
template<typename Grid>
class CERES_EXPORT BiCubicInterpolator {
public:
explicit BiCubicInterpolator(const Grid& grid)
: grid_(grid) {
// The + casts the enum into an int before doing the
// comparison. It is needed to prevent
// "-Wunnamed-type-template-args" related errors.
CHECK_GE(+Grid::DATA_DIMENSION, 1);
}
// Evaluate the interpolated function value and/or its
// derivative. Returns false if r or c is out of bounds.
void Evaluate(double r, double c,
double* f, double* dfdr, double* dfdc) const {
// BiCubic interpolation requires 16 values around the point being
// evaluated. We will use pij, to indicate the elements of the
// 4x4 grid of values.
//
// col
// p00 p01 p02 p03
// row p10 p11 p12 p13
// p20 p21 p22 p23
// p30 p31 p32 p33
//
// The point (r,c) being evaluated is assumed to lie in the square
// defined by p11, p12, p22 and p21.
const int row = std::floor(r);
const int col = std::floor(c);
Eigen::Matrix<double, Grid::DATA_DIMENSION, 1> p0, p1, p2, p3;
// Interpolate along each of the four rows, evaluating the function
// value and the horizontal derivative in each row.
Eigen::Matrix<double, Grid::DATA_DIMENSION, 1> f0, f1, f2, f3;
Eigen::Matrix<double, Grid::DATA_DIMENSION, 1> df0dc, df1dc, df2dc, df3dc;
grid_.GetValue(row - 1, col - 1, p0.data());
grid_.GetValue(row - 1, col , p1.data());
grid_.GetValue(row - 1, col + 1, p2.data());
grid_.GetValue(row - 1, col + 2, p3.data());
CubicHermiteSpline<Grid::DATA_DIMENSION>(p0, p1, p2, p3, c - col,
f0.data(), df0dc.data());
grid_.GetValue(row, col - 1, p0.data());
grid_.GetValue(row, col , p1.data());
grid_.GetValue(row, col + 1, p2.data());
grid_.GetValue(row, col + 2, p3.data());
CubicHermiteSpline<Grid::DATA_DIMENSION>(p0, p1, p2, p3, c - col,
f1.data(), df1dc.data());
grid_.GetValue(row + 1, col - 1, p0.data());
grid_.GetValue(row + 1, col , p1.data());
grid_.GetValue(row + 1, col + 1, p2.data());
grid_.GetValue(row + 1, col + 2, p3.data());
CubicHermiteSpline<Grid::DATA_DIMENSION>(p0, p1, p2, p3, c - col,
f2.data(), df2dc.data());
grid_.GetValue(row + 2, col - 1, p0.data());
grid_.GetValue(row + 2, col , p1.data());
grid_.GetValue(row + 2, col + 1, p2.data());
grid_.GetValue(row + 2, col + 2, p3.data());
CubicHermiteSpline<Grid::DATA_DIMENSION>(p0, p1, p2, p3, c - col,
f3.data(), df3dc.data());
// Interpolate vertically the interpolated value from each row and
// compute the derivative along the columns.
CubicHermiteSpline<Grid::DATA_DIMENSION>(f0, f1, f2, f3, r - row, f, dfdr);
if (dfdc != NULL) {
// Interpolate vertically the derivative along the columns.
CubicHermiteSpline<Grid::DATA_DIMENSION>(df0dc, df1dc, df2dc, df3dc,
r - row, dfdc, NULL);
}
}
// The following two Evaluate overloads are needed for interfacing
// with automatic differentiation. The first is for when a scalar
// evaluation is done, and the second one is for when Jets are used.
void Evaluate(const double& r, const double& c, double* f) const {
Evaluate(r, c, f, NULL, NULL);
}
template<typename JetT> void Evaluate(const JetT& r,
const JetT& c,
JetT* f) const {
double frc[Grid::DATA_DIMENSION];
double dfdr[Grid::DATA_DIMENSION];
double dfdc[Grid::DATA_DIMENSION];
Evaluate(r.a, c.a, frc, dfdr, dfdc);
for (int i = 0; i < Grid::DATA_DIMENSION; ++i) {
f[i].a = frc[i];
f[i].v = dfdr[i] * r.v + dfdc[i] * c.v;
}
}
private:
const Grid& grid_;
};
// An object that implements an infinite two dimensional grid needed
// by the BiCubicInterpolator where the source of the function values
// is an grid of type T on the grid
//
// [(row_start, col_start), ..., (row_start, col_end - 1)]
// [ ... ]
// [(row_end - 1, col_start), ..., (row_end - 1, col_end - 1)]
//
// Since the input grid is finite and the grid is infinite, values
// outside this interval needs to be computed. Grid2D uses the value
// from the nearest edge.
//
// The function being provided can be vector valued, in which case
// kDataDimension > 1. The data maybe stored in row or column major
// format and the various dimensional slices of the function maybe
// interleaved, or they maybe stacked, i.e, if the function has
// kDataDimension = 2, is stored in row-major format and if
// kInterleaved = true, then it is stored as
//
// f001, f002, f011, f012, ...
//
// A commonly occuring example are color images (RGB) where the three
// channels are stored interleaved.
//
// If kInterleaved = false, then it is stored as
//
// f001, f011, ..., fnm1, f002, f012, ...
template <typename T,
int kDataDimension = 1,
bool kRowMajor = true,
bool kInterleaved = true>
struct Grid2D {
public:
enum { DATA_DIMENSION = kDataDimension };
Grid2D(const T* data,
const int row_begin, const int row_end,
const int col_begin, const int col_end)
: data_(data),
row_begin_(row_begin), row_end_(row_end),
col_begin_(col_begin), col_end_(col_end),
num_rows_(row_end - row_begin), num_cols_(col_end - col_begin),
num_values_(num_rows_ * num_cols_) {
CHECK_GE(kDataDimension, 1);
CHECK_LT(row_begin, row_end);
CHECK_LT(col_begin, col_end);
}
EIGEN_STRONG_INLINE void GetValue(const int r, const int c, double* f) const {
const int row_idx =
std::min(std::max(row_begin_, r), row_end_ - 1) - row_begin_;
const int col_idx =
std::min(std::max(col_begin_, c), col_end_ - 1) - col_begin_;
const int n =
(kRowMajor)
? num_cols_ * row_idx + col_idx
: num_rows_ * col_idx + row_idx;
if (kInterleaved) {
for (int i = 0; i < kDataDimension; ++i) {
f[i] = static_cast<double>(data_[kDataDimension * n + i]);
}
} else {
for (int i = 0; i < kDataDimension; ++i) {
f[i] = static_cast<double>(data_[i * num_values_ + n]);
}
}
}
private:
const T* data_;
const int row_begin_;
const int row_end_;
const int col_begin_;
const int col_end_;
const int num_rows_;
const int num_cols_;
const int num_values_;
};
} // namespace ceres
#endif // CERES_PUBLIC_CUBIC_INTERPOLATOR_H_

View File

@ -0,0 +1,260 @@
// 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)
//
// This autodiff implementation differs from the one found in
// autodiff_cost_function.h by supporting autodiff on cost functions
// with variable numbers of parameters with variable sizes. With the
// other implementation, all the sizes (both the number of parameter
// blocks and the size of each block) must be fixed at compile time.
//
// The functor API differs slightly from the API for fixed size
// autodiff; the expected interface for the cost functors is:
//
// struct MyCostFunctor {
// template<typename T>
// bool operator()(T const* const* parameters, T* residuals) const {
// // Use parameters[i] to access the i'th parameter block.
// }
// }
//
// Since the sizing of the parameters is done at runtime, you must
// also specify the sizes after creating the dynamic autodiff cost
// function. For example:
//
// DynamicAutoDiffCostFunction<MyCostFunctor, 3> cost_function(
// new MyCostFunctor());
// cost_function.AddParameterBlock(5);
// cost_function.AddParameterBlock(10);
// cost_function.SetNumResiduals(21);
//
// Under the hood, the implementation evaluates the cost function
// multiple times, computing a small set of the derivatives (four by
// default, controlled by the Stride template parameter) with each
// pass. There is a tradeoff with the size of the passes; you may want
// to experiment with the stride.
#ifndef CERES_PUBLIC_DYNAMIC_AUTODIFF_COST_FUNCTION_H_
#define CERES_PUBLIC_DYNAMIC_AUTODIFF_COST_FUNCTION_H_
#include <cmath>
#include <numeric>
#include <vector>
#include "ceres/cost_function.h"
#include "ceres/internal/scoped_ptr.h"
#include "ceres/jet.h"
#include "glog/logging.h"
namespace ceres {
template <typename CostFunctor, int Stride = 4>
class DynamicAutoDiffCostFunction : public CostFunction {
public:
explicit DynamicAutoDiffCostFunction(CostFunctor* functor)
: functor_(functor) {}
virtual ~DynamicAutoDiffCostFunction() {}
void AddParameterBlock(int size) {
mutable_parameter_block_sizes()->push_back(size);
}
void SetNumResiduals(int num_residuals) {
set_num_residuals(num_residuals);
}
virtual bool Evaluate(double const* const* parameters,
double* residuals,
double** jacobians) const {
CHECK_GT(num_residuals(), 0)
<< "You must call DynamicAutoDiffCostFunction::SetNumResiduals() "
<< "before DynamicAutoDiffCostFunction::Evaluate().";
if (jacobians == NULL) {
return (*functor_)(parameters, residuals);
}
// The difficulty with Jets, as implemented in Ceres, is that they were
// originally designed for strictly compile-sized use. At this point, there
// is a large body of code that assumes inside a cost functor it is
// acceptable to do e.g. T(1.5) and get an appropriately sized jet back.
//
// Unfortunately, it is impossible to communicate the expected size of a
// dynamically sized jet to the static instantiations that existing code
// depends on.
//
// To work around this issue, the solution here is to evaluate the
// jacobians in a series of passes, each one computing Stripe *
// num_residuals() derivatives. This is done with small, fixed-size jets.
const int num_parameter_blocks = parameter_block_sizes().size();
const int num_parameters = std::accumulate(parameter_block_sizes().begin(),
parameter_block_sizes().end(),
0);
// Allocate scratch space for the strided evaluation.
std::vector<Jet<double, Stride> > input_jets(num_parameters);
std::vector<Jet<double, Stride> > output_jets(num_residuals());
// Make the parameter pack that is sent to the functor (reused).
std::vector<Jet<double, Stride>* > jet_parameters(num_parameter_blocks,
static_cast<Jet<double, Stride>* >(NULL));
int num_active_parameters = 0;
// To handle constant parameters between non-constant parameter blocks, the
// start position --- a raw parameter index --- of each contiguous block of
// non-constant parameters is recorded in start_derivative_section.
std::vector<int> start_derivative_section;
bool in_derivative_section = false;
int parameter_cursor = 0;
// Discover the derivative sections and set the parameter values.
for (int i = 0; i < num_parameter_blocks; ++i) {
jet_parameters[i] = &input_jets[parameter_cursor];
const int parameter_block_size = parameter_block_sizes()[i];
if (jacobians[i] != NULL) {
if (!in_derivative_section) {
start_derivative_section.push_back(parameter_cursor);
in_derivative_section = true;
}
num_active_parameters += parameter_block_size;
} else {
in_derivative_section = false;
}
for (int j = 0; j < parameter_block_size; ++j, parameter_cursor++) {
input_jets[parameter_cursor].a = parameters[i][j];
}
}
// When `num_active_parameters % Stride != 0` then it can be the case
// that `active_parameter_count < Stride` while parameter_cursor is less
// than the total number of parameters and with no remaining non-constant
// parameter blocks. Pushing parameter_cursor (the total number of
// parameters) as a final entry to start_derivative_section is required
// because if a constant parameter block is encountered after the
// last non-constant block then current_derivative_section is incremented
// and would otherwise index an invalid position in
// start_derivative_section. Setting the final element to the total number
// of parameters means that this can only happen at most once in the loop
// below.
start_derivative_section.push_back(parameter_cursor);
// Evaluate all of the strides. Each stride is a chunk of the derivative to
// evaluate, typically some size proportional to the size of the SIMD
// registers of the CPU.
int num_strides = static_cast<int>(ceil(num_active_parameters /
static_cast<float>(Stride)));
int current_derivative_section = 0;
int current_derivative_section_cursor = 0;
for (int pass = 0; pass < num_strides; ++pass) {
// Set most of the jet components to zero, except for
// non-constant #Stride parameters.
const int initial_derivative_section = current_derivative_section;
const int initial_derivative_section_cursor =
current_derivative_section_cursor;
int active_parameter_count = 0;
parameter_cursor = 0;
for (int i = 0; i < num_parameter_blocks; ++i) {
for (int j = 0; j < parameter_block_sizes()[i];
++j, parameter_cursor++) {
input_jets[parameter_cursor].v.setZero();
if (active_parameter_count < Stride &&
parameter_cursor >= (
start_derivative_section[current_derivative_section] +
current_derivative_section_cursor)) {
if (jacobians[i] != NULL) {
input_jets[parameter_cursor].v[active_parameter_count] = 1.0;
++active_parameter_count;
++current_derivative_section_cursor;
} else {
++current_derivative_section;
current_derivative_section_cursor = 0;
}
}
}
}
if (!(*functor_)(&jet_parameters[0], &output_jets[0])) {
return false;
}
// Copy the pieces of the jacobians into their final place.
active_parameter_count = 0;
current_derivative_section = initial_derivative_section;
current_derivative_section_cursor = initial_derivative_section_cursor;
for (int i = 0, parameter_cursor = 0; i < num_parameter_blocks; ++i) {
for (int j = 0; j < parameter_block_sizes()[i];
++j, parameter_cursor++) {
if (active_parameter_count < Stride &&
parameter_cursor >= (
start_derivative_section[current_derivative_section] +
current_derivative_section_cursor)) {
if (jacobians[i] != NULL) {
for (int k = 0; k < num_residuals(); ++k) {
jacobians[i][k * parameter_block_sizes()[i] + j] =
output_jets[k].v[active_parameter_count];
}
++active_parameter_count;
++current_derivative_section_cursor;
} else {
++current_derivative_section;
current_derivative_section_cursor = 0;
}
}
}
}
// Only copy the residuals over once (even though we compute them on
// every loop).
if (pass == num_strides - 1) {
for (int k = 0; k < num_residuals(); ++k) {
residuals[k] = output_jets[k].a;
}
}
}
return true;
}
private:
internal::scoped_ptr<CostFunctor> functor_;
};
} // namespace ceres
#endif // CERES_PUBLIC_DYNAMIC_AUTODIFF_COST_FUNCTION_H_

View File

@ -0,0 +1,190 @@
// 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)
// dgossow@google.com (David Gossow)
//
// DynamicCostFunctionToFunctor allows users to use CostFunction
// objects in templated functors which are to be used for automatic
// differentiation. It works similar to CostFunctionToFunctor, with the
// difference that it allows you to wrap a cost function with dynamic numbers
// of parameters and residuals.
//
// For example, let us assume that
//
// class IntrinsicProjection : public CostFunction {
// public:
// IntrinsicProjection(const double* observation);
// virtual bool Evaluate(double const* const* parameters,
// double* residuals,
// double** jacobians) const;
// };
//
// is a cost function that implements the projection of a point in its
// local coordinate system onto its image plane and subtracts it from
// the observed point projection. It can compute its residual and
// either via analytic or numerical differentiation can compute its
// jacobians. The intrinsics are passed in as parameters[0] and the point as
// parameters[1].
//
// Now we would like to compose the action of this CostFunction with
// the action of camera extrinsics, i.e., rotation and
// translation. Say we have a templated function
//
// template<typename T>
// void RotateAndTranslatePoint(double const* const* parameters,
// double* residuals);
//
// Then we can now do the following,
//
// struct CameraProjection {
// CameraProjection(const double* observation)
// : intrinsic_projection_.(new IntrinsicProjection(observation)) {
// }
// template <typename T>
// bool operator()(T const* const* parameters,
// T* residual) const {
// const T* rotation = parameters[0];
// const T* translation = parameters[1];
// const T* intrinsics = parameters[2];
// const T* point = parameters[3];
// T transformed_point[3];
// RotateAndTranslatePoint(rotation, translation, point, transformed_point);
//
// // Note that we call intrinsic_projection_, just like it was
// // any other templated functor.
// const T* projection_parameters[2];
// projection_parameters[0] = intrinsics;
// projection_parameters[1] = transformed_point;
// return intrinsic_projection_(projection_parameters, residual);
// }
//
// private:
// DynamicCostFunctionToFunctor intrinsic_projection_;
// };
#ifndef CERES_PUBLIC_DYNAMIC_COST_FUNCTION_TO_FUNCTOR_H_
#define CERES_PUBLIC_DYNAMIC_COST_FUNCTION_TO_FUNCTOR_H_
#include <numeric>
#include <vector>
#include "ceres/cost_function.h"
#include "ceres/internal/fixed_array.h"
#include "ceres/internal/port.h"
#include "ceres/internal/scoped_ptr.h"
namespace ceres {
class DynamicCostFunctionToFunctor {
public:
// Takes ownership of cost_function.
explicit DynamicCostFunctionToFunctor(CostFunction* cost_function)
: cost_function_(cost_function) {
CHECK_NOTNULL(cost_function);
}
bool operator()(double const* const* parameters, double* residuals) const {
return cost_function_->Evaluate(parameters, residuals, NULL);
}
template <typename JetT>
bool operator()(JetT const* const* inputs, JetT* output) const {
const std::vector<int32>& parameter_block_sizes =
cost_function_->parameter_block_sizes();
const int num_parameter_blocks = parameter_block_sizes.size();
const int num_residuals = cost_function_->num_residuals();
const int num_parameters = std::accumulate(parameter_block_sizes.begin(),
parameter_block_sizes.end(), 0);
internal::FixedArray<double> parameters(num_parameters);
internal::FixedArray<double*> parameter_blocks(num_parameter_blocks);
internal::FixedArray<double> jacobians(num_residuals * num_parameters);
internal::FixedArray<double*> jacobian_blocks(num_parameter_blocks);
internal::FixedArray<double> residuals(num_residuals);
// Build a set of arrays to get the residuals and jacobians from
// the CostFunction wrapped by this functor.
double* parameter_ptr = parameters.get();
double* jacobian_ptr = jacobians.get();
for (int i = 0; i < num_parameter_blocks; ++i) {
parameter_blocks[i] = parameter_ptr;
jacobian_blocks[i] = jacobian_ptr;
for (int j = 0; j < parameter_block_sizes[i]; ++j) {
*parameter_ptr++ = inputs[i][j].a;
}
jacobian_ptr += num_residuals * parameter_block_sizes[i];
}
if (!cost_function_->Evaluate(parameter_blocks.get(),
residuals.get(),
jacobian_blocks.get())) {
return false;
}
// Now that we have the incoming Jets, which are carrying the
// partial derivatives of each of the inputs w.r.t to some other
// underlying parameters. The derivative of the outputs of the
// cost function w.r.t to the same underlying parameters can now
// be computed by applying the chain rule.
//
// d output[i] d output[i] d input[j]
// -------------- = sum_j ----------- * ------------
// d parameter[k] d input[j] d parameter[k]
//
// d input[j]
// -------------- = inputs[j], so
// d parameter[k]
//
// outputJet[i] = sum_k jacobian[i][k] * inputJet[k]
//
// The following loop, iterates over the residuals, computing one
// output jet at a time.
for (int i = 0; i < num_residuals; ++i) {
output[i].a = residuals[i];
output[i].v.setZero();
for (int j = 0; j < num_parameter_blocks; ++j) {
const int32 block_size = parameter_block_sizes[j];
for (int k = 0; k < parameter_block_sizes[j]; ++k) {
output[i].v +=
jacobian_blocks[j][i * block_size + k] * inputs[j][k].v;
}
}
}
return true;
}
private:
internal::scoped_ptr<CostFunction> cost_function_;
};
} // namespace ceres
#endif // CERES_PUBLIC_DYNAMIC_COST_FUNCTION_TO_FUNCTOR_H_

View File

@ -0,0 +1,205 @@
// 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: mierle@gmail.com (Keir Mierle)
// sameeragarwal@google.com (Sameer Agarwal)
// thadh@gmail.com (Thad Hughes)
// tbennun@gmail.com (Tal Ben-Nun)
//
// This numeric diff implementation differs from the one found in
// numeric_diff_cost_function.h by supporting numericdiff on cost
// functions with variable numbers of parameters with variable
// sizes. With the other implementation, all the sizes (both the
// number of parameter blocks and the size of each block) must be
// fixed at compile time.
//
// The functor API differs slightly from the API for fixed size
// numeric diff; the expected interface for the cost functors is:
//
// struct MyCostFunctor {
// bool operator()(double const* const* parameters, double* residuals) const {
// // Use parameters[i] to access the i'th parameter block.
// }
// }
//
// Since the sizing of the parameters is done at runtime, you must
// also specify the sizes after creating the
// DynamicNumericDiffCostFunction. For example:
//
// DynamicAutoDiffCostFunction<MyCostFunctor, CENTRAL> cost_function(
// new MyCostFunctor());
// cost_function.AddParameterBlock(5);
// cost_function.AddParameterBlock(10);
// cost_function.SetNumResiduals(21);
#ifndef CERES_PUBLIC_DYNAMIC_NUMERIC_DIFF_COST_FUNCTION_H_
#define CERES_PUBLIC_DYNAMIC_NUMERIC_DIFF_COST_FUNCTION_H_
#include <cmath>
#include <numeric>
#include <vector>
#include "ceres/cost_function.h"
#include "ceres/internal/scoped_ptr.h"
#include "ceres/internal/eigen.h"
#include "ceres/internal/numeric_diff.h"
#include "ceres/numeric_diff_options.h"
#include "glog/logging.h"
namespace ceres {
template <typename CostFunctor, NumericDiffMethodType method = CENTRAL>
class DynamicNumericDiffCostFunction : public CostFunction {
public:
explicit DynamicNumericDiffCostFunction(
const CostFunctor* functor,
Ownership ownership = TAKE_OWNERSHIP,
const NumericDiffOptions& options = NumericDiffOptions())
: functor_(functor),
ownership_(ownership),
options_(options) {
}
// Deprecated. New users should avoid using this constructor. Instead, use the
// constructor with NumericDiffOptions.
DynamicNumericDiffCostFunction(
const CostFunctor* functor,
Ownership ownership,
double relative_step_size)
: functor_(functor),
ownership_(ownership),
options_() {
LOG(WARNING) << "This constructor is deprecated and will be removed in "
"a future version. Please use the NumericDiffOptions "
"constructor instead.";
options_.relative_step_size = relative_step_size;
}
virtual ~DynamicNumericDiffCostFunction() {
if (ownership_ != TAKE_OWNERSHIP) {
functor_.release();
}
}
void AddParameterBlock(int size) {
mutable_parameter_block_sizes()->push_back(size);
}
void SetNumResiduals(int num_residuals) {
set_num_residuals(num_residuals);
}
virtual bool Evaluate(double const* const* parameters,
double* residuals,
double** jacobians) const {
using internal::NumericDiff;
CHECK_GT(num_residuals(), 0)
<< "You must call DynamicNumericDiffCostFunction::SetNumResiduals() "
<< "before DynamicNumericDiffCostFunction::Evaluate().";
const std::vector<int32>& block_sizes = parameter_block_sizes();
CHECK(!block_sizes.empty())
<< "You must call DynamicNumericDiffCostFunction::AddParameterBlock() "
<< "before DynamicNumericDiffCostFunction::Evaluate().";
const bool status = EvaluateCostFunctor(parameters, residuals);
if (jacobians == NULL || !status) {
return status;
}
// Create local space for a copy of the parameters which will get mutated.
int parameters_size = accumulate(block_sizes.begin(), block_sizes.end(), 0);
std::vector<double> parameters_copy(parameters_size);
std::vector<double*> parameters_references_copy(block_sizes.size());
parameters_references_copy[0] = &parameters_copy[0];
for (int block = 1; block < block_sizes.size(); ++block) {
parameters_references_copy[block] = parameters_references_copy[block - 1]
+ block_sizes[block - 1];
}
// Copy the parameters into the local temp space.
for (int block = 0; block < block_sizes.size(); ++block) {
memcpy(parameters_references_copy[block],
parameters[block],
block_sizes[block] * sizeof(*parameters[block]));
}
for (int block = 0; block < block_sizes.size(); ++block) {
if (jacobians[block] != NULL &&
!NumericDiff<CostFunctor, method, DYNAMIC,
DYNAMIC, DYNAMIC, DYNAMIC, DYNAMIC, DYNAMIC,
DYNAMIC, DYNAMIC, DYNAMIC, DYNAMIC, DYNAMIC,
DYNAMIC, DYNAMIC>::EvaluateJacobianForParameterBlock(
functor_.get(),
residuals,
options_,
this->num_residuals(),
block,
block_sizes[block],
&parameters_references_copy[0],
jacobians[block])) {
return false;
}
}
return true;
}
private:
bool EvaluateCostFunctor(double const* const* parameters,
double* residuals) const {
return EvaluateCostFunctorImpl(functor_.get(),
parameters,
residuals,
functor_.get());
}
// Helper templates to allow evaluation of a functor or a
// CostFunction.
bool EvaluateCostFunctorImpl(const CostFunctor* functor,
double const* const* parameters,
double* residuals,
const void* /* NOT USED */) const {
return (*functor)(parameters, residuals);
}
bool EvaluateCostFunctorImpl(const CostFunctor* functor,
double const* const* parameters,
double* residuals,
const CostFunction* /* NOT USED */) const {
return functor->Evaluate(parameters, residuals, NULL);
}
internal::scoped_ptr<const CostFunctor> functor_;
Ownership ownership_;
NumericDiffOptions options_;
};
} // namespace ceres
#endif // CERES_PUBLIC_DYNAMIC_AUTODIFF_COST_FUNCTION_H_

View File

@ -0,0 +1,70 @@
// 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)
//
// 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) != 0; }
inline bool IsInfinite(double x) { return _finite(x) == 0 && _isnan(x) == 0; }
inline bool IsNaN (double x) { return _isnan(x) != 0; }
inline bool IsNormal (double x) { // NOLINT
const int classification = _fpclass(x);
return (classification == _FPCLASS_NN || classification == _FPCLASS_PN);
}
# else
// These definitions are for the normal Unix suspects.
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_

View File

@ -0,0 +1,222 @@
// 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.
// Copyright 2007 Google Inc. All Rights Reserved.
//
// Author: wjr@google.com (William Rucklidge)
//
// This file contains a class that exercises a cost function, to make sure
// that it is computing reasonable derivatives. It compares the Jacobians
// computed by the cost function with those obtained by finite
// differences.
#ifndef CERES_PUBLIC_GRADIENT_CHECKER_H_
#define CERES_PUBLIC_GRADIENT_CHECKER_H_
#include <cstddef>
#include <algorithm>
#include <vector>
#include "ceres/internal/eigen.h"
#include "ceres/internal/fixed_array.h"
#include "ceres/internal/macros.h"
#include "ceres/internal/scoped_ptr.h"
#include "ceres/numeric_diff_cost_function.h"
#include "glog/logging.h"
namespace ceres {
// An object that exercises a cost function, to compare the answers that it
// gives with derivatives estimated using finite differencing.
//
// The only likely usage of this is for testing.
//
// How to use: Fill in an array of pointers to parameter blocks for your
// CostFunction, and then call Probe(). Check that the return value is
// 'true'. See prober_test.cc for an example.
//
// This is templated similarly to NumericDiffCostFunction, as it internally
// uses that.
template <typename CostFunctionToProbe,
int M = 0, int N0 = 0, int N1 = 0, int N2 = 0, int N3 = 0, int N4 = 0>
class GradientChecker {
public:
// Here we stash some results from the probe, for later
// inspection.
struct GradientCheckResults {
// Computed cost.
Vector cost;
// The sizes of these matrices are dictated by the cost function's
// parameter and residual block sizes. Each vector's length will
// term->parameter_block_sizes().size(), and each matrix is the
// Jacobian of the residual with respect to the corresponding parameter
// block.
// Derivatives as computed by the cost function.
std::vector<Matrix> term_jacobians;
// Derivatives as computed by finite differencing.
std::vector<Matrix> finite_difference_jacobians;
// Infinity-norm of term_jacobians - finite_difference_jacobians.
double error_jacobians;
};
// Checks the Jacobian computed by a cost function.
//
// probe_point: The parameter values at which to probe.
// error_tolerance: A threshold for the infinity-norm difference
// between the Jacobians. If the Jacobians differ by more than
// this amount, then the probe fails.
//
// term: The cost function to test. Not retained after this call returns.
//
// results: On return, the two Jacobians (and other information)
// will be stored here. May be NULL.
//
// Returns true if no problems are detected and the difference between the
// Jacobians is less than error_tolerance.
static bool Probe(double const* const* probe_point,
double error_tolerance,
CostFunctionToProbe *term,
GradientCheckResults* results) {
CHECK_NOTNULL(probe_point);
CHECK_NOTNULL(term);
LOG(INFO) << "-------------------- Starting Probe() --------------------";
// We need a GradientCheckeresults, whether or not they supplied one.
internal::scoped_ptr<GradientCheckResults> owned_results;
if (results == NULL) {
owned_results.reset(new GradientCheckResults);
results = owned_results.get();
}
// Do a consistency check between the term and the template parameters.
CHECK_EQ(M, term->num_residuals());
const int num_residuals = M;
const std::vector<int32>& block_sizes = term->parameter_block_sizes();
const int num_blocks = block_sizes.size();
CHECK_LE(num_blocks, 5) << "Unable to test functions that take more "
<< "than 5 parameter blocks";
if (N0) {
CHECK_EQ(N0, block_sizes[0]);
CHECK_GE(num_blocks, 1);
} else {
CHECK_LT(num_blocks, 1);
}
if (N1) {
CHECK_EQ(N1, block_sizes[1]);
CHECK_GE(num_blocks, 2);
} else {
CHECK_LT(num_blocks, 2);
}
if (N2) {
CHECK_EQ(N2, block_sizes[2]);
CHECK_GE(num_blocks, 3);
} else {
CHECK_LT(num_blocks, 3);
}
if (N3) {
CHECK_EQ(N3, block_sizes[3]);
CHECK_GE(num_blocks, 4);
} else {
CHECK_LT(num_blocks, 4);
}
if (N4) {
CHECK_EQ(N4, block_sizes[4]);
CHECK_GE(num_blocks, 5);
} else {
CHECK_LT(num_blocks, 5);
}
results->term_jacobians.clear();
results->term_jacobians.resize(num_blocks);
results->finite_difference_jacobians.clear();
results->finite_difference_jacobians.resize(num_blocks);
internal::FixedArray<double*> term_jacobian_pointers(num_blocks);
internal::FixedArray<double*>
finite_difference_jacobian_pointers(num_blocks);
for (int i = 0; i < num_blocks; i++) {
results->term_jacobians[i].resize(num_residuals, block_sizes[i]);
term_jacobian_pointers[i] = results->term_jacobians[i].data();
results->finite_difference_jacobians[i].resize(
num_residuals, block_sizes[i]);
finite_difference_jacobian_pointers[i] =
results->finite_difference_jacobians[i].data();
}
results->cost.resize(num_residuals, 1);
CHECK(term->Evaluate(probe_point, results->cost.data(),
term_jacobian_pointers.get()));
NumericDiffCostFunction<CostFunctionToProbe, CENTRAL, M, N0, N1, N2, N3, N4>
numeric_term(term, DO_NOT_TAKE_OWNERSHIP);
CHECK(numeric_term.Evaluate(probe_point, results->cost.data(),
finite_difference_jacobian_pointers.get()));
results->error_jacobians = 0;
for (int i = 0; i < num_blocks; i++) {
Matrix jacobian_difference = results->term_jacobians[i] -
results->finite_difference_jacobians[i];
results->error_jacobians =
std::max(results->error_jacobians,
jacobian_difference.lpNorm<Eigen::Infinity>());
}
LOG(INFO) << "========== term-computed derivatives ==========";
for (int i = 0; i < num_blocks; i++) {
LOG(INFO) << "term_computed block " << i;
LOG(INFO) << "\n" << results->term_jacobians[i];
}
LOG(INFO) << "========== finite-difference derivatives ==========";
for (int i = 0; i < num_blocks; i++) {
LOG(INFO) << "finite_difference block " << i;
LOG(INFO) << "\n" << results->finite_difference_jacobians[i];
}
LOG(INFO) << "========== difference ==========";
for (int i = 0; i < num_blocks; i++) {
LOG(INFO) << "difference block " << i;
LOG(INFO) << (results->term_jacobians[i] -
results->finite_difference_jacobians[i]);
}
LOG(INFO) << "||difference|| = " << results->error_jacobians;
return results->error_jacobians < error_tolerance;
}
private:
CERES_DISALLOW_IMPLICIT_CONSTRUCTORS(GradientChecker);
};
} // namespace ceres
#endif // CERES_PUBLIC_GRADIENT_CHECKER_H_

View File

@ -0,0 +1,127 @@
// 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_PUBLIC_GRADIENT_PROBLEM_H_
#define CERES_PUBLIC_GRADIENT_PROBLEM_H_
#include "ceres/internal/macros.h"
#include "ceres/internal/port.h"
#include "ceres/internal/scoped_ptr.h"
#include "ceres/local_parameterization.h"
namespace ceres {
class FirstOrderFunction;
// Instances of GradientProblem represent general non-linear
// optimization problems that must be solved using just the value of
// the objective function and its gradient. Unlike the Problem class,
// which can only be used to model non-linear least squares problems,
// instances of GradientProblem not restricted in the form of the
// objective function.
//
// Structurally GradientProblem is a composition of a
// FirstOrderFunction and optionally a LocalParameterization.
//
// The FirstOrderFunction is responsible for evaluating the cost and
// gradient of the objective function.
//
// The LocalParameterization is responsible for going back and forth
// between the ambient space and the local tangent space. (See
// local_parameterization.h for more details). When a
// LocalParameterization is not provided, then the tangent space is
// assumed to coincide with the ambient Euclidean space that the
// gradient vector lives in.
//
// Example usage:
//
// The following demonstrate the problem construction for Rosenbrock's function
//
// f(x,y) = (1-x)^2 + 100(y - x^2)^2;
//
// class Rosenbrock : public ceres::FirstOrderFunction {
// public:
// virtual ~Rosenbrock() {}
//
// virtual bool Evaluate(const double* parameters,
// double* cost,
// double* gradient) const {
// const double x = parameters[0];
// const double y = parameters[1];
//
// cost[0] = (1.0 - x) * (1.0 - x) + 100.0 * (y - x * x) * (y - x * x);
// if (gradient != NULL) {
// gradient[0] = -2.0 * (1.0 - x) - 200.0 * (y - x * x) * 2.0 * x;
// gradient[1] = 200.0 * (y - x * x);
// }
// return true;
// };
//
// virtual int NumParameters() const { return 2; };
// };
//
// ceres::GradientProblem problem(new Rosenbrock());
class CERES_EXPORT GradientProblem {
public:
// Takes ownership of the function.
explicit GradientProblem(FirstOrderFunction* function);
// Takes ownership of the function and the parameterization.
GradientProblem(FirstOrderFunction* function,
LocalParameterization* parameterization);
int NumParameters() const;
int NumLocalParameters() const;
// This call is not thread safe.
bool Evaluate(const double* parameters, double* cost, double* gradient) const;
bool Plus(const double* x, const double* delta, double* x_plus_delta) const;
private:
internal::scoped_ptr<FirstOrderFunction> function_;
internal::scoped_ptr<LocalParameterization> parameterization_;
internal::scoped_array<double> scratch_;
};
// A FirstOrderFunction object implements the evaluation of a function
// and its gradient.
class CERES_EXPORT FirstOrderFunction {
public:
virtual ~FirstOrderFunction() {}
// cost is never NULL. gradient may be null.
virtual bool Evaluate(const double* const parameters,
double* cost,
double* gradient) const = 0;
virtual int NumParameters() const = 0;
};
} // namespace ceres
#endif // CERES_PUBLIC_GRADIENT_PROBLEM_H_

View File

@ -0,0 +1,357 @@
// 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_PUBLIC_GRADIENT_PROBLEM_SOLVER_H_
#define CERES_PUBLIC_GRADIENT_PROBLEM_SOLVER_H_
#include <cmath>
#include <string>
#include <vector>
#include "ceres/internal/macros.h"
#include "ceres/internal/port.h"
#include "ceres/iteration_callback.h"
#include "ceres/types.h"
#include "ceres/internal/disable_warnings.h"
namespace ceres {
class GradientProblem;
class CERES_EXPORT GradientProblemSolver {
public:
virtual ~GradientProblemSolver();
// The options structure contains, not surprisingly, options that control how
// the solver operates. The defaults should be suitable for a wide range of
// problems; however, better performance is often obtainable with tweaking.
//
// The constants are defined inside types.h
struct CERES_EXPORT Options {
// Default constructor that sets up a generic sparse problem.
Options() {
line_search_direction_type = LBFGS;
line_search_type = WOLFE;
nonlinear_conjugate_gradient_type = FLETCHER_REEVES;
max_lbfgs_rank = 20;
use_approximate_eigenvalue_bfgs_scaling = false;
line_search_interpolation_type = CUBIC;
min_line_search_step_size = 1e-9;
line_search_sufficient_function_decrease = 1e-4;
max_line_search_step_contraction = 1e-3;
min_line_search_step_contraction = 0.6;
max_num_line_search_step_size_iterations = 20;
max_num_line_search_direction_restarts = 5;
line_search_sufficient_curvature_decrease = 0.9;
max_line_search_step_expansion = 10.0;
max_num_iterations = 50;
max_solver_time_in_seconds = 1e9;
function_tolerance = 1e-6;
gradient_tolerance = 1e-10;
logging_type = PER_MINIMIZER_ITERATION;
minimizer_progress_to_stdout = false;
}
// Returns true if the options struct has a valid
// configuration. Returns false otherwise, and fills in *error
// with a message describing the problem.
bool IsValid(std::string* error) const;
// Minimizer options ----------------------------------------
LineSearchDirectionType line_search_direction_type;
LineSearchType line_search_type;
NonlinearConjugateGradientType nonlinear_conjugate_gradient_type;
// The LBFGS hessian approximation is a low rank approximation to
// the inverse of the Hessian matrix. The rank of the
// approximation determines (linearly) the space and time
// complexity of using the approximation. Higher the rank, the
// better is the quality of the approximation. The increase in
// quality is however is bounded for a number of reasons.
//
// 1. The method only uses secant information and not actual
// derivatives.
//
// 2. The Hessian approximation is constrained to be positive
// definite.
//
// So increasing this rank to a large number will cost time and
// space complexity without the corresponding increase in solution
// quality. There are no hard and fast rules for choosing the
// maximum rank. The best choice usually requires some problem
// specific experimentation.
//
// For more theoretical and implementation details of the LBFGS
// method, please see:
//
// Nocedal, J. (1980). "Updating Quasi-Newton Matrices with
// Limited Storage". Mathematics of Computation 35 (151): 773782.
int max_lbfgs_rank;
// As part of the (L)BFGS update step (BFGS) / right-multiply step (L-BFGS),
// the initial inverse Hessian approximation is taken to be the Identity.
// However, Oren showed that using instead I * \gamma, where \gamma is
// chosen to approximate an eigenvalue of the true inverse Hessian can
// result in improved convergence in a wide variety of cases. Setting
// use_approximate_eigenvalue_bfgs_scaling to true enables this scaling.
//
// It is important to note that approximate eigenvalue scaling does not
// always improve convergence, and that it can in fact significantly degrade
// performance for certain classes of problem, which is why it is disabled
// by default. In particular it can degrade performance when the
// sensitivity of the problem to different parameters varies significantly,
// as in this case a single scalar factor fails to capture this variation
// and detrimentally downscales parts of the jacobian approximation which
// correspond to low-sensitivity parameters. It can also reduce the
// robustness of the solution to errors in the jacobians.
//
// Oren S.S., Self-scaling variable metric (SSVM) algorithms
// Part II: Implementation and experiments, Management Science,
// 20(5), 863-874, 1974.
bool use_approximate_eigenvalue_bfgs_scaling;
// Degree of the polynomial used to approximate the objective
// function. Valid values are BISECTION, QUADRATIC and CUBIC.
//
// BISECTION corresponds to pure backtracking search with no
// interpolation.
LineSearchInterpolationType line_search_interpolation_type;
// If during the line search, the step_size falls below this
// value, it is truncated to zero.
double min_line_search_step_size;
// Line search parameters.
// Solving the line search problem exactly is computationally
// prohibitive. Fortunately, line search based optimization
// algorithms can still guarantee convergence if instead of an
// exact solution, the line search algorithm returns a solution
// which decreases the value of the objective function
// sufficiently. More precisely, we are looking for a step_size
// s.t.
//
// f(step_size) <= f(0) + sufficient_decrease * f'(0) * step_size
//
double line_search_sufficient_function_decrease;
// In each iteration of the line search,
//
// new_step_size >= max_line_search_step_contraction * step_size
//
// Note that by definition, for contraction:
//
// 0 < max_step_contraction < min_step_contraction < 1
//
double max_line_search_step_contraction;
// In each iteration of the line search,
//
// new_step_size <= min_line_search_step_contraction * step_size
//
// Note that by definition, for contraction:
//
// 0 < max_step_contraction < min_step_contraction < 1
//
double min_line_search_step_contraction;
// Maximum number of trial step size iterations during each line search,
// if a step size satisfying the search conditions cannot be found within
// this number of trials, the line search will terminate.
int max_num_line_search_step_size_iterations;
// Maximum number of restarts of the line search direction algorithm before
// terminating the optimization. Restarts of the line search direction
// algorithm occur when the current algorithm fails to produce a new descent
// direction. This typically indicates a numerical failure, or a breakdown
// in the validity of the approximations used.
int max_num_line_search_direction_restarts;
// The strong Wolfe conditions consist of the Armijo sufficient
// decrease condition, and an additional requirement that the
// step-size be chosen s.t. the _magnitude_ ('strong' Wolfe
// conditions) of the gradient along the search direction
// decreases sufficiently. Precisely, this second condition
// is that we seek a step_size s.t.
//
// |f'(step_size)| <= sufficient_curvature_decrease * |f'(0)|
//
// Where f() is the line search objective and f'() is the derivative
// of f w.r.t step_size (d f / d step_size).
double line_search_sufficient_curvature_decrease;
// During the bracketing phase of the Wolfe search, the step size is
// increased until either a point satisfying the Wolfe conditions is
// found, or an upper bound for a bracket containing a point satisfying
// the conditions is found. Precisely, at each iteration of the
// expansion:
//
// new_step_size <= max_step_expansion * step_size.
//
// By definition for expansion, max_step_expansion > 1.0.
double max_line_search_step_expansion;
// Maximum number of iterations for the minimizer to run for.
int max_num_iterations;
// Maximum time for which the minimizer should run for.
double max_solver_time_in_seconds;
// Minimizer terminates when
//
// (new_cost - old_cost) < function_tolerance * old_cost;
//
double function_tolerance;
// Minimizer terminates when
//
// max_i |x - Project(Plus(x, -g(x))| < gradient_tolerance
//
// This value should typically be 1e-4 * function_tolerance.
double gradient_tolerance;
// Logging options ---------------------------------------------------------
LoggingType logging_type;
// By default the Minimizer progress is logged to VLOG(1), which
// is sent to STDERR depending on the vlog level. If this flag is
// set to true, and logging_type is not SILENT, the logging output
// is sent to STDOUT.
bool minimizer_progress_to_stdout;
// Callbacks that are executed at the end of each iteration of the
// Minimizer. An iteration may terminate midway, either due to
// numerical failures or because one of the convergence tests has
// been satisfied. In this case none of the callbacks are
// executed.
// Callbacks are executed in the order that they are specified in
// this vector. By default, parameter blocks are updated only at
// the end of the optimization, i.e when the Minimizer
// terminates. This behaviour is controlled by
// update_state_every_variable. If the user wishes to have access
// to the update parameter blocks when his/her callbacks are
// executed, then set update_state_every_iteration to true.
//
// The solver does NOT take ownership of these pointers.
std::vector<IterationCallback*> callbacks;
};
struct CERES_EXPORT Summary {
Summary();
// A brief one line description of the state of the solver after
// termination.
std::string BriefReport() const;
// A full multiline description of the state of the solver after
// termination.
std::string FullReport() const;
bool IsSolutionUsable() const;
// Minimizer summary -------------------------------------------------
TerminationType termination_type;
// Reason why the solver terminated.
std::string message;
// Cost of the problem (value of the objective function) before
// the optimization.
double initial_cost;
// Cost of the problem (value of the objective function) after the
// optimization.
double final_cost;
// IterationSummary for each minimizer iteration in order.
std::vector<IterationSummary> iterations;
// Sum total of all time spent inside Ceres when Solve is called.
double total_time_in_seconds;
// Time (in seconds) spent evaluating the cost.
double cost_evaluation_time_in_seconds;
// Time (in seconds) spent evaluating the gradient.
double gradient_evaluation_time_in_seconds;
// Time (in seconds) spent minimizing the interpolating polynomial
// to compute the next candidate step size as part of a line search.
double line_search_polynomial_minimization_time_in_seconds;
// Number of parameters in the probem.
int num_parameters;
// Dimension of the tangent space of the problem.
int num_local_parameters;
// Type of line search direction used.
LineSearchDirectionType line_search_direction_type;
// Type of the line search algorithm used.
LineSearchType line_search_type;
// When performing line search, the degree of the polynomial used
// to approximate the objective function.
LineSearchInterpolationType line_search_interpolation_type;
// If the line search direction is NONLINEAR_CONJUGATE_GRADIENT,
// then this indicates the particular variant of non-linear
// conjugate gradient used.
NonlinearConjugateGradientType nonlinear_conjugate_gradient_type;
// If the type of the line search direction is LBFGS, then this
// indicates the rank of the Hessian approximation.
int max_lbfgs_rank;
};
// Once a least squares problem has been built, this function takes
// the problem and optimizes it based on the values of the options
// parameters. Upon return, a detailed summary of the work performed
// by the preprocessor, the non-linear minmizer and the linear
// solver are reported in the summary object.
virtual void Solve(const GradientProblemSolver::Options& options,
const GradientProblem& problem,
double* parameters,
GradientProblemSolver::Summary* summary);
};
// Helper function which avoids going through the interface.
CERES_EXPORT void Solve(const GradientProblemSolver::Options& options,
const GradientProblem& problem,
double* parameters,
GradientProblemSolver::Summary* summary);
} // namespace ceres
#include "ceres/internal/reenable_warnings.h"
#endif // CERES_PUBLIC_GRADIENT_PROBLEM_SOLVER_H_

View File

@ -0,0 +1,317 @@
// 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 "ceres/jet.h"
#include "ceres/internal/eigen.h"
#include "ceres/internal/fixed_array.h"
#include "ceres/internal/variadic_evaluate.h"
#include "glog/logging.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) {
DCHECK(src);
DCHECK(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) {
DCHECK(src);
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) {
DCHECK(src);
DCHECK(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) {
// This block breaks the 80 column rule to keep it somewhat readable.
DCHECK_GT(num_outputs, 0);
DCHECK((!N1 && !N2 && !N3 && !N4 && !N5 && !N6 && !N7 && !N8 && !N9) ||
((N1 > 0) && !N2 && !N3 && !N4 && !N5 && !N6 && !N7 && !N8 && !N9) ||
((N1 > 0) && (N2 > 0) && !N3 && !N4 && !N5 && !N6 && !N7 && !N8 && !N9) || // NOLINT
((N1 > 0) && (N2 > 0) && (N3 > 0) && !N4 && !N5 && !N6 && !N7 && !N8 && !N9) || // NOLINT
((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && !N5 && !N6 && !N7 && !N8 && !N9) || // NOLINT
((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && (N5 > 0) && !N6 && !N7 && !N8 && !N9) || // NOLINT
((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && (N5 > 0) && (N6 > 0) && !N7 && !N8 && !N9) || // NOLINT
((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && (N5 > 0) && (N6 > 0) && (N7 > 0) && !N8 && !N9) || // NOLINT
((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && (N5 > 0) && (N6 > 0) && (N7 > 0) && (N8 > 0) && !N9) || // NOLINT
((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && (N5 > 0) && (N6 > 0) && (N7 > 0) && (N8 > 0) && (N9 > 0))) // NOLINT
<< "Zero block cannot precede a non-zero block. Block sizes are "
<< "(ignore trailing 0s): " << N0 << ", " << N1 << ", " << N2 << ", "
<< N3 << ", " << N4 << ", " << N5 << ", " << N6 << ", " << N7 << ", "
<< N8 << ", " << N9;
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;
#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_

View File

@ -0,0 +1,91 @@
// Ceres Solver - A fast non-linear least squares minimizer
// Copyright 2015 Google Inc. All rights reserved.
// http://ceres-solver.org/
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//
// * Redistributions of source code must retain the above copyright notice,
// this list of conditions and the following disclaimer.
// * Redistributions in binary form must reproduce the above copyright notice,
// this list of conditions and the following disclaimer in the documentation
// and/or other materials provided with the distribution.
// * Neither the name of Google Inc. nor the names of its contributors may be
// used to endorse or promote products derived from this software without
// specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//
// Author: alexs.mac@gmail.com (Alex Stewart)
// Configuration options for Ceres.
//
// Do not edit this file, it was automatically configured by CMake when
// Ceres was compiled with the relevant configuration for the machine
// on which Ceres was compiled.
//
// Ceres Developers: All options should have the same name as their mapped
// CMake options, in the preconfigured version of this file
// all options should be enclosed in '@'.
#ifndef CERES_PUBLIC_INTERNAL_CONFIG_H_
#define CERES_PUBLIC_INTERNAL_CONFIG_H_
// If defined, use the LGPL code in Eigen.
// #define CERES_USE_EIGEN_SPARSE
// If defined, Ceres was compiled without LAPACK.
#define CERES_NO_LAPACK
// If defined, Ceres was compiled without SuiteSparse.
#define CERES_NO_SUITESPARSE
// If defined, Ceres was compiled without CXSparse.
#define CERES_NO_CXSPARSE
// If defined, Ceres was compiled without Schur specializations.
// #define CERES_RESTRICT_SCHUR_SPECIALIZATION
// If defined, Ceres was compiled to use Eigen instead of hardcoded BLAS
// routines.
// #define CERES_NO_CUSTOM_BLAS
// If defined, Ceres was compiled with C++11.
// #define CERES_USE_CXX11
// If defined, Ceres was compiled without multithreading support.
// #define CERES_NO_THREADS
// If defined Ceres was compiled with OpenMP multithreading support.
#define CERES_USE_OPENMP
// Additionally defined on *nix if Ceres was compiled with OpenMP support,
// as in this case pthreads is also required.
// #define CERES_HAVE_PTHREAD
// #define CERES_HAVE_RWLOCK
// Which version of unordered map was used when Ceres was compiled. Exactly
// one of these will be defined for any given build.
#define CERES_STD_UNORDERED_MAP
// #define CERES_STD_UNORDERED_MAP_IN_TR1_NAMESPACE
// #define CERES_TR1_UNORDERED_MAP
// #define CERES_NO_UNORDERED_MAP
// If defined, the memory header is in <tr1/memory>, otherwise <memory>.
// #define CERES_TR1_MEMORY_HEADER
// If defined shared_ptr is in std::tr1 namespace, otherwise std.
// #define CERES_TR1_SHARED_PTR
// If defined, Ceres was built as a shared library.
// #define CERES_USING_SHARED_LIBRARY
#endif // CERES_PUBLIC_INTERNAL_CONFIG_H_

View File

@ -0,0 +1,44 @@
// 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.
//
// This file has the sole purpose to silence warnings when including Ceres.
// This is not your usual header guard. The macro CERES_WARNINGS_DISABLED
// shows up again in reenable_warnings.h.
#ifndef CERES_WARNINGS_DISABLED
#define CERES_WARNINGS_DISABLED
#ifdef _MSC_VER
#pragma warning( push )
// Disable the warning C4251 which is trigerred by stl classes in
// Ceres' public interface. To quote MSDN: "C4251 can be ignored "
// "if you are deriving from a type in the Standard C++ Library"
#pragma warning( disable : 4251 )
#endif
#endif // CERES_WARNINGS_DISABLED

View File

@ -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_

View File

@ -0,0 +1,191 @@
// 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 "ceres/internal/macros.h"
#include "ceres/internal/manual_constructor.h"
#include "glog/logging.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) {
DCHECK_LT(i, size_);
return array_[i].element;
}
// REQUIRES: 0 <= i < size()
// Returns a reference to the "i"th element.
inline const T& operator[](size_type i) const {
DCHECK_LT(i, size_);
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_

View File

@ -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_

View File

@ -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_

View File

@ -0,0 +1,426 @@
// 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: settinger@google.com (Scott Ettinger)
// mierle@gmail.com (Keir Mierle)
//
// Simplified Glog style logging with Android support. Supported macros in
// decreasing severity level per line:
//
// VLOG(2), VLOG(N)
// VLOG(1),
// LOG(INFO), VLOG(0), LG
// LOG(WARNING),
// LOG(ERROR),
// LOG(FATAL),
//
// With VLOG(n), the output is directed to one of the 5 Android log levels:
//
// 2 - Verbose
// 1 - Debug
// 0 - Info
// -1 - Warning
// -2 - Error
// -3 - Fatal
//
// Any logging of level 2 and above is directed to the Verbose level. All
// Android log output is tagged with the string "native".
//
// If the symbol ANDROID is not defined, all output goes to std::cerr.
// This allows code to be built on a different system for debug.
//
// Portions of this code are taken from the GLOG package. This code is only a
// small subset of the GLOG functionality. Notable differences from GLOG
// behavior include lack of support for displaying unprintable characters and
// lack of stack trace information upon failure of the CHECK macros. On
// non-Android systems, log output goes to std::cerr and is not written to a
// file.
//
// CHECK macros are defined to test for conditions within code. Any CHECK that
// fails will log the failure and terminate the application.
// e.g. CHECK_GE(3, 2) will pass while CHECK_GE(3, 4) will fail after logging
// "Check failed 3 >= 4".
//
// The following CHECK macros are defined:
//
// CHECK(condition) - fails if condition is false and logs condition.
// CHECK_NOTNULL(variable) - fails if the variable is NULL.
//
// The following binary check macros are also defined :
//
// Macro Operator equivalent
// -------------------- -------------------
// CHECK_EQ(val1, val2) val1 == val2
// CHECK_NE(val1, val2) val1 != val2
// CHECK_GT(val1, val2) val1 > val2
// CHECK_GE(val1, val2) val1 >= val2
// CHECK_LT(val1, val2) val1 < val2
// CHECK_LE(val1, val2) val1 <= val2
//
// Debug only versions of all of the check macros are also defined. These
// macros generate no code in a release build, but avoid unused variable
// warnings / errors.
//
// To use the debug only versions, prepend a D to the normal check macros, e.g.
// DCHECK_EQ(a, b).
#ifndef CERCES_INTERNAL_MINIGLOG_GLOG_LOGGING_H_
#define CERCES_INTERNAL_MINIGLOG_GLOG_LOGGING_H_
#ifdef ANDROID
# include <android/log.h>
#endif // ANDROID
#include <algorithm>
#include <ctime>
#include <fstream>
#include <iostream>
#include <set>
#include <sstream>
#include <string>
#include <vector>
// For appropriate definition of CERES_EXPORT macro.
#include "ceres/internal/port.h"
#include "ceres/internal/disable_warnings.h"
// Log severity level constants.
const int FATAL = -3;
const int ERROR = -2;
const int WARNING = -1;
const int INFO = 0;
// ------------------------- Glog compatibility ------------------------------
namespace google {
typedef int LogSeverity;
const int INFO = ::INFO;
const int WARNING = ::WARNING;
const int ERROR = ::ERROR;
const int FATAL = ::FATAL;
// Sink class used for integration with mock and test functions. If sinks are
// added, all log output is also sent to each sink through the send function.
// In this implementation, WaitTillSent() is called immediately after the send.
// This implementation is not thread safe.
class CERES_EXPORT LogSink {
public:
virtual ~LogSink() {}
virtual void send(LogSeverity severity,
const char* full_filename,
const char* base_filename,
int line,
const struct tm* tm_time,
const char* message,
size_t message_len) = 0;
virtual void WaitTillSent() = 0;
};
// Global set of log sinks. The actual object is defined in logging.cc.
extern CERES_EXPORT std::set<LogSink *> log_sinks_global;
inline void InitGoogleLogging(char *argv) {
// Do nothing; this is ignored.
}
// Note: the Log sink functions are not thread safe.
inline void AddLogSink(LogSink *sink) {
// TODO(settinger): Add locks for thread safety.
log_sinks_global.insert(sink);
}
inline void RemoveLogSink(LogSink *sink) {
log_sinks_global.erase(sink);
}
} // namespace google
// ---------------------------- Logger Class --------------------------------
// Class created for each use of the logging macros.
// The logger acts as a stream and routes the final stream contents to the
// Android logcat output at the proper filter level. If ANDROID is not
// defined, output is directed to std::cerr. This class should not
// be directly instantiated in code, rather it should be invoked through the
// use of the log macros LG, LOG, or VLOG.
class CERES_EXPORT MessageLogger {
public:
MessageLogger(const char *file, int line, const char *tag, int severity)
: file_(file), line_(line), tag_(tag), severity_(severity) {
// Pre-pend the stream with the file and line number.
StripBasename(std::string(file), &filename_only_);
stream_ << filename_only_ << ":" << line << " ";
}
// Output the contents of the stream to the proper channel on destruction.
~MessageLogger() {
stream_ << "\n";
#ifdef ANDROID
static const int android_log_levels[] = {
ANDROID_LOG_FATAL, // LOG(FATAL)
ANDROID_LOG_ERROR, // LOG(ERROR)
ANDROID_LOG_WARN, // LOG(WARNING)
ANDROID_LOG_INFO, // LOG(INFO), LG, VLOG(0)
ANDROID_LOG_DEBUG, // VLOG(1)
ANDROID_LOG_VERBOSE, // VLOG(2) .. VLOG(N)
};
// Bound the logging level.
const int kMaxVerboseLevel = 2;
int android_level_index = std::min(std::max(FATAL, severity_),
kMaxVerboseLevel) - FATAL;
int android_log_level = android_log_levels[android_level_index];
// Output the log string the Android log at the appropriate level.
__android_log_write(android_log_level, tag_.c_str(), stream_.str().c_str());
// Indicate termination if needed.
if (severity_ == FATAL) {
__android_log_write(ANDROID_LOG_FATAL,
tag_.c_str(),
"terminating.\n");
}
#else
// If not building on Android, log all output to std::cerr.
std::cerr << stream_.str();
#endif // ANDROID
LogToSinks(severity_);
WaitForSinks();
// Android logging at level FATAL does not terminate execution, so abort()
// is still required to stop the program.
if (severity_ == FATAL) {
abort();
}
}
// Return the stream associated with the logger object.
std::stringstream &stream() { return stream_; }
private:
void LogToSinks(int severity) {
time_t rawtime;
time (&rawtime);
struct tm* timeinfo;
#if defined(WIN32) || defined(_WIN32) || defined(__WIN32__)
// On Windows, use secure localtime_s not localtime.
struct tm windows_timeinfo;
timeinfo = &windows_timeinfo;
localtime_s(timeinfo, &rawtime);
#else
timeinfo = localtime(&rawtime);
#endif
std::set<google::LogSink*>::iterator iter;
// Send the log message to all sinks.
for (iter = google::log_sinks_global.begin();
iter != google::log_sinks_global.end(); ++iter) {
(*iter)->send(severity, file_.c_str(), filename_only_.c_str(), line_,
timeinfo, stream_.str().c_str(), stream_.str().size());
}
}
void WaitForSinks() {
// TODO(settinger): Add locks for thread safety.
std::set<google::LogSink *>::iterator iter;
// Call WaitTillSent() for all sinks.
for (iter = google::log_sinks_global.begin();
iter != google::log_sinks_global.end(); ++iter) {
(*iter)->WaitTillSent();
}
}
void StripBasename(const std::string &full_path, std::string *filename) {
// TODO(settinger): Add support for OSs with different path separators.
const char kSeparator = '/';
size_t pos = full_path.rfind(kSeparator);
if (pos != std::string::npos) {
*filename = full_path.substr(pos + 1, std::string::npos);
} else {
*filename = full_path;
}
}
std::string file_;
std::string filename_only_;
int line_;
std::string tag_;
std::stringstream stream_;
int severity_;
};
// ---------------------- Logging Macro definitions --------------------------
// This class is used to explicitly ignore values in the conditional
// logging macros. This avoids compiler warnings like "value computed
// is not used" and "statement has no effect".
class CERES_EXPORT LoggerVoidify {
public:
LoggerVoidify() { }
// This has to be an operator with a precedence lower than << but
// higher than ?:
void operator&(const std::ostream &s) { }
};
// Log only if condition is met. Otherwise evaluates to void.
#define LOG_IF(severity, condition) \
!(condition) ? (void) 0 : LoggerVoidify() & \
MessageLogger((char *)__FILE__, __LINE__, "native", severity).stream()
// Log only if condition is NOT met. Otherwise evaluates to void.
#define LOG_IF_FALSE(severity, condition) LOG_IF(severity, !(condition))
// LG is a convenient shortcut for LOG(INFO). Its use is in new
// google3 code is discouraged and the following shortcut exists for
// backward compatibility with existing code.
#ifdef MAX_LOG_LEVEL
# define LOG(n) LOG_IF(n, n <= MAX_LOG_LEVEL)
# define VLOG(n) LOG_IF(n, n <= MAX_LOG_LEVEL)
# define LG LOG_IF(INFO, INFO <= MAX_LOG_LEVEL)
# define VLOG_IF(n, condition) LOG_IF(n, (n <= MAX_LOG_LEVEL) && condition)
#else
# define LOG(n) MessageLogger((char *)__FILE__, __LINE__, "native", n).stream() // NOLINT
# define VLOG(n) MessageLogger((char *)__FILE__, __LINE__, "native", n).stream() // NOLINT
# define LG MessageLogger((char *)__FILE__, __LINE__, "native", INFO).stream() // NOLINT
# define VLOG_IF(n, condition) LOG_IF(n, condition)
#endif
// Currently, VLOG is always on for levels below MAX_LOG_LEVEL.
#ifndef MAX_LOG_LEVEL
# define VLOG_IS_ON(x) (1)
#else
# define VLOG_IS_ON(x) (x <= MAX_LOG_LEVEL)
#endif
#ifndef NDEBUG
# define DLOG LOG
#else
# define DLOG(severity) true ? (void) 0 : LoggerVoidify() & \
MessageLogger((char *)__FILE__, __LINE__, "native", severity).stream()
#endif
// Log a message and terminate.
template<class T>
void LogMessageFatal(const char *file, int line, const T &message) {
MessageLogger((char *)__FILE__, __LINE__, "native", FATAL).stream()
<< message;
}
// ---------------------------- CHECK macros ---------------------------------
// Check for a given boolean condition.
#define CHECK(condition) LOG_IF_FALSE(FATAL, condition) \
<< "Check failed: " #condition " "
#ifndef NDEBUG
// Debug only version of CHECK
# define DCHECK(condition) LOG_IF_FALSE(FATAL, condition) \
<< "Check failed: " #condition " "
#else
// Optimized version - generates no code.
# define DCHECK(condition) if (false) LOG_IF_FALSE(FATAL, condition) \
<< "Check failed: " #condition " "
#endif // NDEBUG
// ------------------------- CHECK_OP macros ---------------------------------
// Generic binary operator check macro. This should not be directly invoked,
// instead use the binary comparison macros defined below.
#define CHECK_OP(val1, val2, op) LOG_IF_FALSE(FATAL, ((val1) op (val2))) \
<< "Check failed: " #val1 " " #op " " #val2 " "
// Check_op macro definitions
#define CHECK_EQ(val1, val2) CHECK_OP(val1, val2, ==)
#define CHECK_NE(val1, val2) CHECK_OP(val1, val2, !=)
#define CHECK_LE(val1, val2) CHECK_OP(val1, val2, <=)
#define CHECK_LT(val1, val2) CHECK_OP(val1, val2, <)
#define CHECK_GE(val1, val2) CHECK_OP(val1, val2, >=)
#define CHECK_GT(val1, val2) CHECK_OP(val1, val2, >)
#ifndef NDEBUG
// Debug only versions of CHECK_OP macros.
# define DCHECK_EQ(val1, val2) CHECK_OP(val1, val2, ==)
# define DCHECK_NE(val1, val2) CHECK_OP(val1, val2, !=)
# define DCHECK_LE(val1, val2) CHECK_OP(val1, val2, <=)
# define DCHECK_LT(val1, val2) CHECK_OP(val1, val2, <)
# define DCHECK_GE(val1, val2) CHECK_OP(val1, val2, >=)
# define DCHECK_GT(val1, val2) CHECK_OP(val1, val2, >)
#else
// These versions generate no code in optimized mode.
# define DCHECK_EQ(val1, val2) if (false) CHECK_OP(val1, val2, ==)
# define DCHECK_NE(val1, val2) if (false) CHECK_OP(val1, val2, !=)
# define DCHECK_LE(val1, val2) if (false) CHECK_OP(val1, val2, <=)
# define DCHECK_LT(val1, val2) if (false) CHECK_OP(val1, val2, <)
# define DCHECK_GE(val1, val2) if (false) CHECK_OP(val1, val2, >=)
# define DCHECK_GT(val1, val2) if (false) CHECK_OP(val1, val2, >)
#endif // NDEBUG
// ---------------------------CHECK_NOTNULL macros ---------------------------
// Helpers for CHECK_NOTNULL(). Two are necessary to support both raw pointers
// and smart pointers.
template <typename T>
T& CheckNotNullCommon(const char *file, int line, const char *names, T& t) {
if (t == NULL) {
LogMessageFatal(file, line, std::string(names));
}
return t;
}
template <typename T>
T* CheckNotNull(const char *file, int line, const char *names, T* t) {
return CheckNotNullCommon(file, line, names, t);
}
template <typename T>
T& CheckNotNull(const char *file, int line, const char *names, T& t) {
return CheckNotNullCommon(file, line, names, t);
}
// Check that a pointer is not null.
#define CHECK_NOTNULL(val) \
CheckNotNull(__FILE__, __LINE__, "'" #val "' Must be non NULL", (val))
#ifndef NDEBUG
// Debug only version of CHECK_NOTNULL
#define DCHECK_NOTNULL(val) \
CheckNotNull(__FILE__, __LINE__, "'" #val "' Must be non NULL", (val))
#else
// Optimized version - generates no code.
#define DCHECK_NOTNULL(val) if (false)\
CheckNotNull(__FILE__, __LINE__, "'" #val "' Must be non NULL", (val))
#endif // NDEBUG
#include "ceres/internal/reenable_warnings.h"
#endif // CERCES_INTERNAL_MINIGLOG_GLOG_LOGGING_H_

View File

@ -0,0 +1,446 @@
// 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)
// tbennun@gmail.com (Tal Ben-Nun)
//
// Finite differencing routines used by NumericDiffCostFunction.
#ifndef CERES_PUBLIC_INTERNAL_NUMERIC_DIFF_H_
#define CERES_PUBLIC_INTERNAL_NUMERIC_DIFF_H_
#include <cstring>
#include "Eigen/Dense"
#include "Eigen/StdVector"
#include "ceres/cost_function.h"
#include "ceres/internal/fixed_array.h"
#include "ceres/internal/scoped_ptr.h"
#include "ceres/internal/variadic_evaluate.h"
#include "ceres/numeric_diff_options.h"
#include "ceres/types.h"
#include "glog/logging.h"
namespace ceres {
namespace internal {
// Helper templates that allow evaluation of a variadic functor or a
// CostFunction object.
template <typename CostFunctor,
int N0, int N1, int N2, int N3, int N4,
int N5, int N6, int N7, int N8, int N9 >
bool EvaluateImpl(const CostFunctor* functor,
double const* const* parameters,
double* residuals,
const void* /* NOT USED */) {
return VariadicEvaluate<CostFunctor,
double,
N0, N1, N2, N3, N4, N5, N6, N7, N8, N9>::Call(
*functor,
parameters,
residuals);
}
template <typename CostFunctor,
int N0, int N1, int N2, int N3, int N4,
int N5, int N6, int N7, int N8, int N9 >
bool EvaluateImpl(const CostFunctor* functor,
double const* const* parameters,
double* residuals,
const CostFunction* /* NOT USED */) {
return functor->Evaluate(parameters, residuals, NULL);
}
// This is split from the main class because C++ doesn't allow partial template
// specializations for member functions. The alternative is to repeat the main
// class for differing numbers of parameters, which is also unfortunate.
template <typename CostFunctor,
NumericDiffMethodType kMethod,
int kNumResiduals,
int N0, int N1, int N2, int N3, int N4,
int N5, int N6, int N7, int N8, int N9,
int kParameterBlock,
int kParameterBlockSize>
struct NumericDiff {
// Mutates parameters but must restore them before return.
static bool EvaluateJacobianForParameterBlock(
const CostFunctor* functor,
const double* residuals_at_eval_point,
const NumericDiffOptions& options,
int num_residuals,
int parameter_block_index,
int parameter_block_size,
double **parameters,
double *jacobian) {
using Eigen::Map;
using Eigen::Matrix;
using Eigen::RowMajor;
using Eigen::ColMajor;
const int num_residuals_internal =
(kNumResiduals != ceres::DYNAMIC ? kNumResiduals : num_residuals);
const int parameter_block_index_internal =
(kParameterBlock != ceres::DYNAMIC ? kParameterBlock :
parameter_block_index);
const int parameter_block_size_internal =
(kParameterBlockSize != ceres::DYNAMIC ? kParameterBlockSize :
parameter_block_size);
typedef Matrix<double, kNumResiduals, 1> ResidualVector;
typedef Matrix<double, kParameterBlockSize, 1> ParameterVector;
// The convoluted reasoning for choosing the Row/Column major
// ordering of the matrix is an artifact of the restrictions in
// Eigen that prevent it from creating RowMajor matrices with a
// single column. In these cases, we ask for a ColMajor matrix.
typedef Matrix<double,
kNumResiduals,
kParameterBlockSize,
(kParameterBlockSize == 1) ? ColMajor : RowMajor>
JacobianMatrix;
Map<JacobianMatrix> parameter_jacobian(jacobian,
num_residuals_internal,
parameter_block_size_internal);
Map<ParameterVector> x_plus_delta(
parameters[parameter_block_index_internal],
parameter_block_size_internal);
ParameterVector x(x_plus_delta);
ParameterVector step_size = x.array().abs() *
((kMethod == RIDDERS) ? options.ridders_relative_initial_step_size :
options.relative_step_size);
// It is not a good idea to make the step size arbitrarily
// small. This will lead to problems with round off and numerical
// instability when dividing by the step size. The general
// recommendation is to not go down below sqrt(epsilon).
double min_step_size = std::sqrt(std::numeric_limits<double>::epsilon());
// For Ridders' method, the initial step size is required to be large,
// thus ridders_relative_initial_step_size is used.
if (kMethod == RIDDERS) {
min_step_size = std::max(min_step_size,
options.ridders_relative_initial_step_size);
}
// For each parameter in the parameter block, use finite differences to
// compute the derivative for that parameter.
FixedArray<double> temp_residual_array(num_residuals_internal);
FixedArray<double> residual_array(num_residuals_internal);
Map<ResidualVector> residuals(residual_array.get(),
num_residuals_internal);
for (int j = 0; j < parameter_block_size_internal; ++j) {
const double delta = std::max(min_step_size, step_size(j));
if (kMethod == RIDDERS) {
if (!EvaluateRiddersJacobianColumn(functor, j, delta,
options,
num_residuals_internal,
parameter_block_size_internal,
x.data(),
residuals_at_eval_point,
parameters,
x_plus_delta.data(),
temp_residual_array.get(),
residual_array.get())) {
return false;
}
} else {
if (!EvaluateJacobianColumn(functor, j, delta,
num_residuals_internal,
parameter_block_size_internal,
x.data(),
residuals_at_eval_point,
parameters,
x_plus_delta.data(),
temp_residual_array.get(),
residual_array.get())) {
return false;
}
}
parameter_jacobian.col(j).matrix() = residuals;
}
return true;
}
static bool EvaluateJacobianColumn(const CostFunctor* functor,
int parameter_index,
double delta,
int num_residuals,
int parameter_block_size,
const double* x_ptr,
const double* residuals_at_eval_point,
double** parameters,
double* x_plus_delta_ptr,
double* temp_residuals_ptr,
double* residuals_ptr) {
using Eigen::Map;
using Eigen::Matrix;
typedef Matrix<double, kNumResiduals, 1> ResidualVector;
typedef Matrix<double, kParameterBlockSize, 1> ParameterVector;
Map<const ParameterVector> x(x_ptr, parameter_block_size);
Map<ParameterVector> x_plus_delta(x_plus_delta_ptr,
parameter_block_size);
Map<ResidualVector> residuals(residuals_ptr, num_residuals);
Map<ResidualVector> temp_residuals(temp_residuals_ptr, num_residuals);
// Mutate 1 element at a time and then restore.
x_plus_delta(parameter_index) = x(parameter_index) + delta;
if (!EvaluateImpl<CostFunctor, N0, N1, N2, N3, N4, N5, N6, N7, N8, N9>(
functor, parameters, residuals.data(), functor)) {
return false;
}
// Compute this column of the jacobian in 3 steps:
// 1. Store residuals for the forward part.
// 2. Subtract residuals for the backward (or 0) part.
// 3. Divide out the run.
double one_over_delta = 1.0 / delta;
if (kMethod == CENTRAL || kMethod == RIDDERS) {
// Compute the function on the other side of x(parameter_index).
x_plus_delta(parameter_index) = x(parameter_index) - delta;
if (!EvaluateImpl<CostFunctor, N0, N1, N2, N3, N4, N5, N6, N7, N8, N9>(
functor, parameters, temp_residuals.data(), functor)) {
return false;
}
residuals -= temp_residuals;
one_over_delta /= 2;
} else {
// Forward difference only; reuse existing residuals evaluation.
residuals -=
Map<const ResidualVector>(residuals_at_eval_point,
num_residuals);
}
// Restore x_plus_delta.
x_plus_delta(parameter_index) = x(parameter_index);
// Divide out the run to get slope.
residuals *= one_over_delta;
return true;
}
// This numeric difference implementation uses adaptive differentiation
// on the parameters to obtain the Jacobian matrix. The adaptive algorithm
// is based on Ridders' method for adaptive differentiation, which creates
// a Romberg tableau from varying step sizes and extrapolates the
// intermediate results to obtain the current computational error.
//
// References:
// C.J.F. Ridders, Accurate computation of F'(x) and F'(x) F"(x), Advances
// in Engineering Software (1978), Volume 4, Issue 2, April 1982,
// Pages 75-76, ISSN 0141-1195,
// http://dx.doi.org/10.1016/S0141-1195(82)80057-0.
static bool EvaluateRiddersJacobianColumn(
const CostFunctor* functor,
int parameter_index,
double delta,
const NumericDiffOptions& options,
int num_residuals,
int parameter_block_size,
const double* x_ptr,
const double* residuals_at_eval_point,
double** parameters,
double* x_plus_delta_ptr,
double* temp_residuals_ptr,
double* residuals_ptr) {
using Eigen::Map;
using Eigen::Matrix;
using Eigen::aligned_allocator;
typedef Matrix<double, kNumResiduals, 1> ResidualVector;
typedef Matrix<double, kNumResiduals, Eigen::Dynamic> ResidualCandidateMatrix;
typedef Matrix<double, kParameterBlockSize, 1> ParameterVector;
Map<const ParameterVector> x(x_ptr, parameter_block_size);
Map<ParameterVector> x_plus_delta(x_plus_delta_ptr,
parameter_block_size);
Map<ResidualVector> residuals(residuals_ptr, num_residuals);
Map<ResidualVector> temp_residuals(temp_residuals_ptr, num_residuals);
// In order for the algorithm to converge, the step size should be
// initialized to a value that is large enough to produce a significant
// change in the function.
// As the derivative is estimated, the step size decreases.
// By default, the step sizes are chosen so that the middle column
// of the Romberg tableau uses the input delta.
double current_step_size = delta *
pow(options.ridders_step_shrink_factor,
options.max_num_ridders_extrapolations / 2);
// Double-buffering temporary differential candidate vectors
// from previous step size.
ResidualCandidateMatrix stepsize_candidates_a(
num_residuals,
options.max_num_ridders_extrapolations);
ResidualCandidateMatrix stepsize_candidates_b(
num_residuals,
options.max_num_ridders_extrapolations);
ResidualCandidateMatrix* current_candidates = &stepsize_candidates_a;
ResidualCandidateMatrix* previous_candidates = &stepsize_candidates_b;
// Represents the computational error of the derivative. This variable is
// initially set to a large value, and is set to the difference between
// current and previous finite difference extrapolations.
// norm_error is supposed to decrease as the finite difference tableau
// generation progresses, serving both as an estimate for differentiation
// error and as a measure of differentiation numerical stability.
double norm_error = std::numeric_limits<double>::max();
// Loop over decreasing step sizes until:
// 1. Error is smaller than a given value (ridders_epsilon),
// 2. Maximal order of extrapolation reached, or
// 3. Extrapolation becomes numerically unstable.
for (int i = 0; i < options.max_num_ridders_extrapolations; ++i) {
// Compute the numerical derivative at this step size.
if (!EvaluateJacobianColumn(functor, parameter_index, current_step_size,
num_residuals,
parameter_block_size,
x.data(),
residuals_at_eval_point,
parameters,
x_plus_delta.data(),
temp_residuals.data(),
current_candidates->col(0).data())) {
// Something went wrong; bail.
return false;
}
// Store initial results.
if (i == 0) {
residuals = current_candidates->col(0);
}
// Shrink differentiation step size.
current_step_size /= options.ridders_step_shrink_factor;
// Extrapolation factor for Richardson acceleration method (see below).
double richardson_factor = options.ridders_step_shrink_factor *
options.ridders_step_shrink_factor;
for (int k = 1; k <= i; ++k) {
// Extrapolate the various orders of finite differences using
// the Richardson acceleration method.
current_candidates->col(k) =
(richardson_factor * current_candidates->col(k - 1) -
previous_candidates->col(k - 1)) / (richardson_factor - 1.0);
richardson_factor *= options.ridders_step_shrink_factor *
options.ridders_step_shrink_factor;
// Compute the difference between the previous value and the current.
double candidate_error = std::max(
(current_candidates->col(k) -
current_candidates->col(k - 1)).norm(),
(current_candidates->col(k) -
previous_candidates->col(k - 1)).norm());
// If the error has decreased, update results.
if (candidate_error <= norm_error) {
norm_error = candidate_error;
residuals = current_candidates->col(k);
// If the error is small enough, stop.
if (norm_error < options.ridders_epsilon) {
break;
}
}
}
// After breaking out of the inner loop, declare convergence.
if (norm_error < options.ridders_epsilon) {
break;
}
// Check to see if the current gradient estimate is numerically unstable.
// If so, bail out and return the last stable result.
if (i > 0) {
double tableau_error = (current_candidates->col(i) -
previous_candidates->col(i - 1)).norm();
// Compare current error to the chosen candidate's error.
if (tableau_error >= 2 * norm_error) {
break;
}
}
std::swap(current_candidates, previous_candidates);
}
return true;
}
};
template <typename CostFunctor,
NumericDiffMethodType kMethod,
int kNumResiduals,
int N0, int N1, int N2, int N3, int N4,
int N5, int N6, int N7, int N8, int N9,
int kParameterBlock>
struct NumericDiff<CostFunctor, kMethod, kNumResiduals,
N0, N1, N2, N3, N4, N5, N6, N7, N8, N9,
kParameterBlock, 0> {
// Mutates parameters but must restore them before return.
static bool EvaluateJacobianForParameterBlock(
const CostFunctor* functor,
const double* residuals_at_eval_point,
const NumericDiffOptions& options,
const int num_residuals,
const int parameter_block_index,
const int parameter_block_size,
double **parameters,
double *jacobian) {
// Silence unused parameter compiler warnings.
(void)functor;
(void)residuals_at_eval_point;
(void)options;
(void)num_residuals;
(void)parameter_block_index;
(void)parameter_block_size;
(void)parameters;
(void)jacobian;
LOG(FATAL) << "Control should never reach here.";
return true;
}
};
} // namespace internal
} // namespace ceres
#endif // CERES_PUBLIC_INTERNAL_NUMERIC_DIFF_H_

View File

@ -0,0 +1,76 @@
// 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)
#ifndef CERES_PUBLIC_INTERNAL_PORT_H_
#define CERES_PUBLIC_INTERNAL_PORT_H_
// This file needs to compile as c code.
#ifdef __cplusplus
#include "ceres/internal/config.h"
#if defined(CERES_TR1_MEMORY_HEADER)
#include <tr1/memory>
#else
#include <memory>
#endif
namespace ceres {
#if defined(CERES_TR1_SHARED_PTR)
using std::tr1::shared_ptr;
#else
using std::shared_ptr;
#endif
} // namespace ceres
#endif // __cplusplus
// A macro to signal which functions and classes are exported when
// building a DLL with MSVC.
//
// Note that the ordering here is important, CERES_BUILDING_SHARED_LIBRARY
// is only defined locally when Ceres is compiled, it is never exported to
// users. However, in order that we do not have to configure config.h
// separately for building vs installing, if we are using MSVC and building
// a shared library, then both CERES_BUILDING_SHARED_LIBRARY and
// CERES_USING_SHARED_LIBRARY will be defined when Ceres is compiled.
// Hence it is important that the check for CERES_BUILDING_SHARED_LIBRARY
// happens first.
#if defined(_MSC_VER) && defined(CERES_BUILDING_SHARED_LIBRARY)
# define CERES_EXPORT __declspec(dllexport)
#elif defined(_MSC_VER) && defined(CERES_USING_SHARED_LIBRARY)
# define CERES_EXPORT __declspec(dllimport)
#else
# define CERES_EXPORT
#endif
#endif // CERES_PUBLIC_INTERNAL_PORT_H_

View File

@ -0,0 +1,38 @@
// 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.
//
// This is not your usual header guard. See disable_warnings.h
#ifdef CERES_WARNINGS_DISABLED
#undef CERES_WARNINGS_DISABLED
#ifdef _MSC_VER
#pragma warning( pop )
#endif
#endif // CERES_WARNINGS_DISABLED

View File

@ -0,0 +1,310 @@
// 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: jorg@google.com (Jorg Brown)
//
// This is an implementation designed to match the anticipated future TR2
// implementation of the scoped_ptr class, and its closely-related brethren,
// scoped_array, scoped_ptr_malloc, and make_scoped_ptr.
#ifndef CERES_PUBLIC_INTERNAL_SCOPED_PTR_H_
#define CERES_PUBLIC_INTERNAL_SCOPED_PTR_H_
#include <assert.h>
#include <stdlib.h>
#include <cstddef>
#include <algorithm>
namespace ceres {
namespace internal {
template <class C> class scoped_ptr;
template <class C, class Free> class scoped_ptr_malloc;
template <class C> class scoped_array;
template <class C>
scoped_ptr<C> make_scoped_ptr(C *);
// A scoped_ptr<T> is like a T*, except that the destructor of
// scoped_ptr<T> automatically deletes the pointer it holds (if
// any). That is, scoped_ptr<T> owns the T object that it points
// to. Like a T*, a scoped_ptr<T> may hold either NULL or a pointer to
// a T object. Also like T*, scoped_ptr<T> is thread-compatible, and
// once you dereference it, you get the threadsafety guarantees of T.
//
// The size of a scoped_ptr is small: sizeof(scoped_ptr<C>) == sizeof(C*)
template <class C>
class scoped_ptr {
public:
// The element type
typedef C element_type;
// Constructor. Defaults to intializing with NULL.
// There is no way to create an uninitialized scoped_ptr.
// The input parameter must be allocated with new.
explicit scoped_ptr(C* p = NULL) : ptr_(p) { }
// Destructor. If there is a C object, delete it.
// We don't need to test ptr_ == NULL because C++ does that for us.
~scoped_ptr() {
enum { type_must_be_complete = sizeof(C) };
delete ptr_;
}
// Reset. Deletes the current owned object, if any.
// Then takes ownership of a new object, if given.
// this->reset(this->get()) works.
void reset(C* p = NULL) {
if (p != ptr_) {
enum { type_must_be_complete = sizeof(C) };
delete ptr_;
ptr_ = p;
}
}
// Accessors to get the owned object.
// operator* and operator-> will assert() if there is no current object.
C& operator*() const {
assert(ptr_ != NULL);
return *ptr_;
}
C* operator->() const {
assert(ptr_ != NULL);
return ptr_;
}
C* get() const { return ptr_; }
// Comparison operators.
// These return whether a scoped_ptr and a raw pointer refer to
// the same object, not just to two different but equal objects.
bool operator==(const C* p) const { return ptr_ == p; }
bool operator!=(const C* p) const { return ptr_ != p; }
// Swap two scoped pointers.
void swap(scoped_ptr& p2) {
C* tmp = ptr_;
ptr_ = p2.ptr_;
p2.ptr_ = tmp;
}
// Release a pointer.
// The return value is the current pointer held by this object.
// If this object holds a NULL pointer, the return value is NULL.
// After this operation, this object will hold a NULL pointer,
// and will not own the object any more.
C* release() {
C* retVal = ptr_;
ptr_ = NULL;
return retVal;
}
private:
C* ptr_;
// google3 friend class that can access copy ctor (although if it actually
// calls a copy ctor, there will be a problem) see below
friend scoped_ptr<C> make_scoped_ptr<C>(C *p);
// Forbid comparison of scoped_ptr types. If C2 != C, it totally doesn't
// make sense, and if C2 == C, it still doesn't make sense because you should
// never have the same object owned by two different scoped_ptrs.
template <class C2> bool operator==(scoped_ptr<C2> const& p2) const;
template <class C2> bool operator!=(scoped_ptr<C2> const& p2) const;
// Disallow evil constructors
scoped_ptr(const scoped_ptr&);
void operator=(const scoped_ptr&);
};
// Free functions
template <class C>
inline void swap(scoped_ptr<C>& p1, scoped_ptr<C>& p2) {
p1.swap(p2);
}
template <class C>
inline bool operator==(const C* p1, const scoped_ptr<C>& p2) {
return p1 == p2.get();
}
template <class C>
inline bool operator==(const C* p1, const scoped_ptr<const C>& p2) {
return p1 == p2.get();
}
template <class C>
inline bool operator!=(const C* p1, const scoped_ptr<C>& p2) {
return p1 != p2.get();
}
template <class C>
inline bool operator!=(const C* p1, const scoped_ptr<const C>& p2) {
return p1 != p2.get();
}
template <class C>
scoped_ptr<C> make_scoped_ptr(C *p) {
// This does nothing but to return a scoped_ptr of the type that the passed
// pointer is of. (This eliminates the need to specify the name of T when
// making a scoped_ptr that is used anonymously/temporarily.) From an
// access control point of view, we construct an unnamed scoped_ptr here
// which we return and thus copy-construct. Hence, we need to have access
// to scoped_ptr::scoped_ptr(scoped_ptr const &). However, it is guaranteed
// that we never actually call the copy constructor, which is a good thing
// as we would call the temporary's object destructor (and thus delete p)
// if we actually did copy some object, here.
return scoped_ptr<C>(p);
}
// scoped_array<C> is like scoped_ptr<C>, except that the caller must allocate
// with new [] and the destructor deletes objects with delete [].
//
// As with scoped_ptr<C>, a scoped_array<C> either points to an object
// or is NULL. A scoped_array<C> owns the object that it points to.
// scoped_array<T> is thread-compatible, and once you index into it,
// the returned objects have only the threadsafety guarantees of T.
//
// Size: sizeof(scoped_array<C>) == sizeof(C*)
template <class C>
class scoped_array {
public:
// The element type
typedef C element_type;
// Constructor. Defaults to intializing with NULL.
// There is no way to create an uninitialized scoped_array.
// The input parameter must be allocated with new [].
explicit scoped_array(C* p = NULL) : array_(p) { }
// Destructor. If there is a C object, delete it.
// We don't need to test ptr_ == NULL because C++ does that for us.
~scoped_array() {
enum { type_must_be_complete = sizeof(C) };
delete[] array_;
}
// Reset. Deletes the current owned object, if any.
// Then takes ownership of a new object, if given.
// this->reset(this->get()) works.
void reset(C* p = NULL) {
if (p != array_) {
enum { type_must_be_complete = sizeof(C) };
delete[] array_;
array_ = p;
}
}
// Get one element of the current object.
// Will assert() if there is no current object, or index i is negative.
C& operator[](std::ptrdiff_t i) const {
assert(i >= 0);
assert(array_ != NULL);
return array_[i];
}
// Get a pointer to the zeroth element of the current object.
// If there is no current object, return NULL.
C* get() const {
return array_;
}
// Comparison operators.
// These return whether a scoped_array and a raw pointer refer to
// the same array, not just to two different but equal arrays.
bool operator==(const C* p) const { return array_ == p; }
bool operator!=(const C* p) const { return array_ != p; }
// Swap two scoped arrays.
void swap(scoped_array& p2) {
C* tmp = array_;
array_ = p2.array_;
p2.array_ = tmp;
}
// Release an array.
// The return value is the current pointer held by this object.
// If this object holds a NULL pointer, the return value is NULL.
// After this operation, this object will hold a NULL pointer,
// and will not own the object any more.
C* release() {
C* retVal = array_;
array_ = NULL;
return retVal;
}
private:
C* array_;
// Forbid comparison of different scoped_array types.
template <class C2> bool operator==(scoped_array<C2> const& p2) const;
template <class C2> bool operator!=(scoped_array<C2> const& p2) const;
// Disallow evil constructors
scoped_array(const scoped_array&);
void operator=(const scoped_array&);
};
// Free functions
template <class C>
inline void swap(scoped_array<C>& p1, scoped_array<C>& p2) {
p1.swap(p2);
}
template <class C>
inline bool operator==(const C* p1, const scoped_array<C>& p2) {
return p1 == p2.get();
}
template <class C>
inline bool operator==(const C* p1, const scoped_array<const C>& p2) {
return p1 == p2.get();
}
template <class C>
inline bool operator!=(const C* p1, const scoped_array<C>& p2) {
return p1 != p2.get();
}
template <class C>
inline bool operator!=(const C* p1, const scoped_array<const C>& p2) {
return p1 != p2.get();
}
// This class wraps the c library function free() in a class that can be
// passed as a template argument to scoped_ptr_malloc below.
class ScopedPtrMallocFree {
public:
inline void operator()(void* x) const {
free(x);
}
};
} // namespace internal
} // namespace ceres
#endif // CERES_PUBLIC_INTERNAL_SCOPED_PTR_H_

View File

@ -0,0 +1,194 @@
// 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 "ceres/jet.h"
#include "ceres/types.h"
#include "ceres/internal/eigen.h"
#include "ceres/internal/fixed_array.h"
#include "glog/logging.h"
namespace ceres {
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_

View File

@ -0,0 +1,225 @@
// 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)
//
// When an iteration callback is specified, Ceres calls the callback
// after each minimizer step (if the minimizer has not converged) and
// passes it an IterationSummary object, defined below.
#ifndef CERES_PUBLIC_ITERATION_CALLBACK_H_
#define CERES_PUBLIC_ITERATION_CALLBACK_H_
#include "ceres/types.h"
#include "ceres/internal/disable_warnings.h"
namespace ceres {
// This struct describes the state of the optimizer after each
// iteration of the minimization.
struct CERES_EXPORT IterationSummary {
IterationSummary()
: iteration(0),
step_is_valid(false),
step_is_nonmonotonic(false),
step_is_successful(false),
cost(0.0),
cost_change(0.0),
gradient_max_norm(0.0),
gradient_norm(0.0),
step_norm(0.0),
eta(0.0),
step_size(0.0),
line_search_function_evaluations(0),
line_search_gradient_evaluations(0),
line_search_iterations(0),
linear_solver_iterations(0),
iteration_time_in_seconds(0.0),
step_solver_time_in_seconds(0.0),
cumulative_time_in_seconds(0.0) {}
// Current iteration number.
int32 iteration;
// Step was numerically valid, i.e., all values are finite and the
// step reduces the value of the linearized model.
//
// Note: step_is_valid is false when iteration = 0.
bool step_is_valid;
// Step did not reduce the value of the objective function
// sufficiently, but it was accepted because of the relaxed
// acceptance criterion used by the non-monotonic trust region
// algorithm.
//
// Note: step_is_nonmonotonic is false when iteration = 0;
bool step_is_nonmonotonic;
// Whether or not the minimizer accepted this step or not. If the
// ordinary trust region algorithm is used, this means that the
// relative reduction in the objective function value was greater
// than Solver::Options::min_relative_decrease. However, if the
// non-monotonic trust region algorithm is used
// (Solver::Options:use_nonmonotonic_steps = true), then even if the
// relative decrease is not sufficient, the algorithm may accept the
// step and the step is declared successful.
//
// Note: step_is_successful is false when iteration = 0.
bool step_is_successful;
// Value of the objective function.
double cost;
// Change in the value of the objective function in this
// iteration. This can be positive or negative.
double cost_change;
// Infinity norm of the gradient vector.
double gradient_max_norm;
// 2-norm of the gradient vector.
double gradient_norm;
// 2-norm of the size of the step computed by the optimization
// algorithm.
double step_norm;
// For trust region algorithms, the ratio of the actual change in
// cost and the change in the cost of the linearized approximation.
double relative_decrease;
// Size of the trust region at the end of the current iteration. For
// the Levenberg-Marquardt algorithm, the regularization parameter
// mu = 1.0 / trust_region_radius.
double trust_region_radius;
// For the inexact step Levenberg-Marquardt algorithm, this is the
// relative accuracy with which the Newton(LM) step is solved. This
// number affects only the iterative solvers capable of solving
// linear systems inexactly. Factorization-based exact solvers
// ignore it.
double eta;
// Step sized computed by the line search algorithm.
double step_size;
// Number of function value evaluations used by the line search algorithm.
int line_search_function_evaluations;
// Number of function gradient evaluations used by the line search algorithm.
int line_search_gradient_evaluations;
// Number of iterations taken by the line search algorithm.
int line_search_iterations;
// Number of iterations taken by the linear solver to solve for the
// Newton step.
int linear_solver_iterations;
// All times reported below are wall times.
// Time (in seconds) spent inside the minimizer loop in the current
// iteration.
double iteration_time_in_seconds;
// Time (in seconds) spent inside the trust region step solver.
double step_solver_time_in_seconds;
// Time (in seconds) since the user called Solve().
double cumulative_time_in_seconds;
};
// Interface for specifying callbacks that are executed at the end of
// each iteration of the Minimizer. The solver uses the return value
// of operator() to decide whether to continue solving or to
// terminate. The user can return three values.
//
// SOLVER_ABORT indicates that the callback detected an abnormal
// situation. The solver returns without updating the parameter blocks
// (unless Solver::Options::update_state_every_iteration is set
// true). Solver returns with Solver::Summary::termination_type set to
// USER_ABORT.
//
// SOLVER_TERMINATE_SUCCESSFULLY indicates that there is no need to
// optimize anymore (some user specified termination criterion has
// been met). Solver returns with Solver::Summary::termination_type
// set to USER_SUCCESS.
//
// SOLVER_CONTINUE indicates that the solver should continue
// optimizing.
//
// For example, the following Callback is used internally by Ceres to
// log the progress of the optimization.
//
// Callback for logging the state of the minimizer to STDERR or STDOUT
// depending on the user's preferences and logging level.
//
// class LoggingCallback : public IterationCallback {
// public:
// explicit LoggingCallback(bool log_to_stdout)
// : log_to_stdout_(log_to_stdout) {}
//
// ~LoggingCallback() {}
//
// CallbackReturnType operator()(const IterationSummary& summary) {
// const char* kReportRowFormat =
// "% 4d: f:% 8e d:% 3.2e g:% 3.2e h:% 3.2e "
// "rho:% 3.2e mu:% 3.2e eta:% 3.2e li:% 3d";
// string output = StringPrintf(kReportRowFormat,
// summary.iteration,
// summary.cost,
// summary.cost_change,
// summary.gradient_max_norm,
// summary.step_norm,
// summary.relative_decrease,
// summary.trust_region_radius,
// summary.eta,
// summary.linear_solver_iterations);
// if (log_to_stdout_) {
// cout << output << endl;
// } else {
// VLOG(1) << output;
// }
// return SOLVER_CONTINUE;
// }
//
// private:
// const bool log_to_stdout_;
// };
//
class CERES_EXPORT IterationCallback {
public:
virtual ~IterationCallback() {}
virtual CallbackReturnType operator()(const IterationSummary& summary) = 0;
};
} // namespace ceres
#include "ceres/internal/reenable_warnings.h"
#endif // CERES_PUBLIC_ITERATION_CALLBACK_H_

749
external/Ceres-1.11/include/ceres/jet.h vendored Normal file
View File

@ -0,0 +1,749 @@
// 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)
//
// 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);
//
// LOG(INFO) << "df/dx = " << z.v[0]
// << "df/dy = " << z.v[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 <limits>
#include <string>
#include "Eigen/Core"
#include "ceres/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);
}
// Constructor from scalar and vector part
// The use of Eigen::DenseBase allows Eigen expressions
// to be passed in without being fully evaluated until
// they are assigned to v
template<typename Derived>
EIGEN_STRONG_INLINE Jet(const T& a, const Eigen::DenseBase<Derived> &v)
: a(a), v(v) {
}
// 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) {
return Jet<T, N>(-f.a, -f.v);
}
// Binary +
template<typename T, int N> inline
Jet<T, N> operator+(const Jet<T, N>& f,
const Jet<T, N>& g) {
return Jet<T, N>(f.a + g.a, f.v + g.v);
}
// Binary + with a scalar: x + s
template<typename T, int N> inline
Jet<T, N> operator+(const Jet<T, N>& f, T s) {
return Jet<T, N>(f.a + s, f.v);
}
// Binary + with a scalar: s + x
template<typename T, int N> inline
Jet<T, N> operator+(T s, const Jet<T, N>& f) {
return Jet<T, N>(f.a + s, f.v);
}
// Binary -
template<typename T, int N> inline
Jet<T, N> operator-(const Jet<T, N>& f,
const Jet<T, N>& g) {
return Jet<T, N>(f.a - g.a, f.v - g.v);
}
// Binary - with a scalar: x - s
template<typename T, int N> inline
Jet<T, N> operator-(const Jet<T, N>& f, T s) {
return Jet<T, N>(f.a - s, f.v);
}
// Binary - with a scalar: s - x
template<typename T, int N> inline
Jet<T, N> operator-(T s, const Jet<T, N>& f) {
return Jet<T, N>(s - f.a, -f.v);
}
// Binary *
template<typename T, int N> inline
Jet<T, N> operator*(const Jet<T, N>& f,
const Jet<T, N>& g) {
return Jet<T, N>(f.a * g.a, f.a * g.v + f.v * g.a);
}
// Binary * with a scalar: x * s
template<typename T, int N> inline
Jet<T, N> operator*(const Jet<T, N>& f, T s) {
return Jet<T, N>(f.a * s, f.v * s);
}
// Binary * with a scalar: s * x
template<typename T, int N> inline
Jet<T, N> operator*(T s, const Jet<T, N>& f) {
return Jet<T, N>(f.a * s, f.v * s);
}
// Binary /
template<typename T, int N> inline
Jet<T, N> operator/(const Jet<T, N>& f,
const Jet<T, N>& g) {
// 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;
const T f_a_by_g_a = f.a * g_a_inverse;
return Jet<T, N>(f.a * g_a_inverse, (f.v - f_a_by_g_a * g.v) * g_a_inverse);
}
// Binary / with a scalar: s / x
template<typename T, int N> inline
Jet<T, N> operator/(T s, const Jet<T, N>& g) {
const T minus_s_g_a_inverse2 = -s / (g.a * g.a);
return Jet<T, N>(s / g.a, g.v * minus_s_g_a_inverse2);
}
// Binary / with a scalar: x / s
template<typename T, int N> inline
Jet<T, N> operator/(const Jet<T, N>& f, T s) {
const T s_inverse = 1.0 / s;
return Jet<T, N>(f.a * s_inverse, f.v * s_inverse);
}
// 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) {
const T a_inverse = T(1.0) / f.a;
return Jet<T, N>(log(f.a), f.v * a_inverse);
}
// exp(a + h) ~= exp(a) + exp(a) h
template <typename T, int N> inline
Jet<T, N> exp(const Jet<T, N>& f) {
const T tmp = exp(f.a);
return Jet<T, N>(tmp, tmp * f.v);
}
// sqrt(a + h) ~= sqrt(a) + h / (2 sqrt(a))
template <typename T, int N> inline
Jet<T, N> sqrt(const Jet<T, N>& f) {
const T tmp = sqrt(f.a);
const T two_a_inverse = T(1.0) / (T(2.0) * tmp);
return Jet<T, N>(tmp, f.v * two_a_inverse);
}
// cos(a + h) ~= cos(a) - sin(a) h
template <typename T, int N> inline
Jet<T, N> cos(const Jet<T, N>& f) {
return Jet<T, N>(cos(f.a), - sin(f.a) * f.v);
}
// 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) {
const T tmp = - T(1.0) / sqrt(T(1.0) - f.a * f.a);
return Jet<T, N>(acos(f.a), tmp * f.v);
}
// sin(a + h) ~= sin(a) + cos(a) h
template <typename T, int N> inline
Jet<T, N> sin(const Jet<T, N>& f) {
return Jet<T, N>(sin(f.a), cos(f.a) * f.v);
}
// 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) {
const T tmp = T(1.0) / sqrt(T(1.0) - f.a * f.a);
return Jet<T, N>(asin(f.a), tmp * f.v);
}
// 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) {
const T tan_a = tan(f.a);
const T tmp = T(1.0) + tan_a * tan_a;
return Jet<T, N>(tan_a, tmp * f.v);
}
// 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) {
const T tmp = T(1.0) / (T(1.0) + f.a * f.a);
return Jet<T, N>(atan(f.a), tmp * f.v);
}
// sinh(a + h) ~= sinh(a) + cosh(a) h
template <typename T, int N> inline
Jet<T, N> sinh(const Jet<T, N>& f) {
return Jet<T, N>(sinh(f.a), cosh(f.a) * f.v);
}
// cosh(a + h) ~= cosh(a) + sinh(a) h
template <typename T, int N> inline
Jet<T, N> cosh(const Jet<T, N>& f) {
return Jet<T, N>(cosh(f.a), sinh(f.a) * f.v);
}
// 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) {
const T tanh_a = tanh(f.a);
const T tmp = T(1.0) - tanh_a * tanh_a;
return Jet<T, N>(tanh_a, tmp * f.v);
}
// 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
T const tmp = T(1.0) / (f.a * f.a + g.a * g.a);
return Jet<T, N>(atan2(g.a, f.a), tmp * (- g.a * f.v + f.a * g.v));
}
// pow -- base is a differentiable 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) {
T const tmp = g * pow(f.a, g - T(1.0));
return Jet<T, N>(pow(f.a, g), tmp * f.v);
}
// pow -- base is a constant, exponent is a differentiable function.
// We have various special cases, see the comment for pow(Jet, Jet) for
// analysis:
//
// 1. For f > 0 we have: (f)^(g + dg) ~= f^g + f^g log(f) dg
//
// 2. For f == 0 and g > 0 we have: (f)^(g + dg) ~= f^g
//
// 3. For f < 0 and integer g we have: (f)^(g + dg) ~= f^g but if dg
// != 0, the derivatives are not defined and we return NaN.
template <typename T, int N> inline
Jet<T, N> pow(double f, const Jet<T, N>& g) {
if (f == 0 && g.a > 0) {
// Handle case 2.
return Jet<T, N>(T(0.0));
}
if (f < 0 && g.a == floor(g.a)) {
// Handle case 3.
Jet<T, N> ret(pow(f, g.a));
for (int i = 0; i < N; i++) {
if (g.v[i] != T(0.0)) {
// Return a NaN when g.v != 0.
ret.v[i] = std::numeric_limits<T>::quiet_NaN();
}
}
return ret;
}
// Handle case 1.
T const tmp = pow(f, g.a);
return Jet<T, N>(tmp, log(f) * tmp * g.v);
}
// pow -- both base and exponent are differentiable functions. This has a
// variety of special cases that require careful handling.
//
// 1. For f > 0:
// (f + df)^(g + dg) ~= f^g + f^(g - 1) * (g * df + f * log(f) * dg)
// The numerical evaluation of f * log(f) for f > 0 is well behaved, even for
// extremely small values (e.g. 1e-99).
//
// 2. For f == 0 and g > 1: (f + df)^(g + dg) ~= 0
// This cases is needed because log(0) can not be evaluated in the f > 0
// expression. However the function f*log(f) is well behaved around f == 0
// and its limit as f-->0 is zero.
//
// 3. For f == 0 and g == 1: (f + df)^(g + dg) ~= 0 + df
//
// 4. For f == 0 and 0 < g < 1: The value is finite but the derivatives are not.
//
// 5. For f == 0 and g < 0: The value and derivatives of f^g are not finite.
//
// 6. For f == 0 and g == 0: The C standard incorrectly defines 0^0 to be 1
// "because there are applications that can exploit this definition". We
// (arbitrarily) decree that derivatives here will be nonfinite, since that
// is consistent with the behavior for f == 0, g < 0 and 0 < g < 1.
// Practically any definition could have been justified because mathematical
// consistency has been lost at this point.
//
// 7. For f < 0, g integer, dg == 0: (f + df)^(g + dg) ~= f^g + g * f^(g - 1) df
// This is equivalent to the case where f is a differentiable function and g
// is a constant (to first order).
//
// 8. For f < 0, g integer, dg != 0: The value is finite but the derivatives are
// not, because any change in the value of g moves us away from the point
// with a real-valued answer into the region with complex-valued answers.
//
// 9. For f < 0, g noninteger: The value and derivatives of f^g are not finite.
template <typename T, int N> inline
Jet<T, N> pow(const Jet<T, N>& f, const Jet<T, N>& g) {
if (f.a == 0 && g.a >= 1) {
// Handle cases 2 and 3.
if (g.a > 1) {
return Jet<T, N>(T(0.0));
}
return f;
}
if (f.a < 0 && g.a == floor(g.a)) {
// Handle cases 7 and 8.
T const tmp = g.a * pow(f.a, g.a - T(1.0));
Jet<T, N> ret(pow(f.a, g.a), tmp * f.v);
for (int i = 0; i < N; i++) {
if (g.v[i] != T(0.0)) {
// Return a NaN when g.v != 0.
ret.v[i] = std::numeric_limits<T>::quiet_NaN();
}
}
return ret;
}
// Handle the remaining cases. For cases 4,5,6,9 we allow the log() function
// to generate -HUGE_VAL or NaN, since those cases result in a nonfinite
// derivative.
T const tmp1 = pow(f.a, g.a);
T const tmp2 = g.a * pow(f.a, g.a - T(1.0));
T const tmp3 = tmp1 * log(f.a);
return Jet<T, N>(tmp1, tmp2 * f.v + tmp3 * g.v);
}
// 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;
static typename ceres::Jet<T, N> dummy_precision() {
return ceres::Jet<T, N>(1e-12);
}
static inline Real epsilon() {
return Real(std::numeric_limits<T>::epsilon());
}
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_

View File

@ -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)
// sameeragarwal@google.com (Sameer Agarwal)
#ifndef CERES_PUBLIC_LOCAL_PARAMETERIZATION_H_
#define CERES_PUBLIC_LOCAL_PARAMETERIZATION_H_
#include <vector>
#include "ceres/internal/port.h"
#include "ceres/internal/scoped_ptr.h"
#include "ceres/internal/disable_warnings.h"
namespace ceres {
// Purpose: Sometimes parameter blocks x can overparameterize a problem
//
// min f(x)
// x
//
// In that case it is desirable to choose a parameterization for the
// block itself to remove the null directions of the cost. More
// generally, if x lies on a manifold of a smaller dimension than the
// ambient space that it is embedded in, then it is numerically and
// computationally more effective to optimize it using a
// parameterization that lives in the tangent space of that manifold
// at each point.
//
// For example, a sphere in three dimensions is a 2 dimensional
// manifold, embedded in a three dimensional space. At each point on
// the sphere, the plane tangent to it defines a two dimensional
// tangent space. For a cost function defined on this sphere, given a
// point x, moving in the direction normal to the sphere at that point
// is not useful. Thus a better way to do a local optimization is to
// optimize over two dimensional vector delta in the tangent space at
// that point and then "move" to the point x + delta, where the move
// operation involves projecting back onto the sphere. Doing so
// removes a redundent dimension from the optimization, making it
// numerically more robust and efficient.
//
// More generally we can define a function
//
// x_plus_delta = Plus(x, delta),
//
// where x_plus_delta has the same size as x, and delta is of size
// less than or equal to x. The function Plus, generalizes the
// definition of vector addition. Thus it satisfies the identify
//
// Plus(x, 0) = x, for all x.
//
// A trivial version of Plus is when delta is of the same size as x
// and
//
// Plus(x, delta) = x + delta
//
// A more interesting case if x is two dimensional vector, and the
// user wishes to hold the first coordinate constant. Then, delta is a
// scalar and Plus is defined as
//
// Plus(x, delta) = x + [0] * delta
// [1]
//
// An example that occurs commonly in Structure from Motion problems
// is when camera rotations are parameterized using Quaternion. There,
// it is useful only make updates orthogonal to that 4-vector defining
// the quaternion. One way to do this is to let delta be a 3
// dimensional vector and define Plus to be
//
// Plus(x, delta) = [cos(|delta|), sin(|delta|) delta / |delta|] * x
//
// The multiplication between the two 4-vectors on the RHS is the
// standard quaternion product.
//
// Given g and a point x, optimizing f can now be restated as
//
// min f(Plus(x, delta))
// delta
//
// Given a solution delta to this problem, the optimal value is then
// given by
//
// x* = Plus(x, delta)
//
// The class LocalParameterization defines the function Plus and its
// Jacobian which is needed to compute the Jacobian of f w.r.t delta.
class CERES_EXPORT LocalParameterization {
public:
virtual ~LocalParameterization();
// Generalization of the addition operation,
//
// x_plus_delta = Plus(x, delta)
//
// with the condition that Plus(x, 0) = x.
virtual bool Plus(const double* x,
const double* delta,
double* x_plus_delta) const = 0;
// The jacobian of Plus(x, delta) w.r.t delta at delta = 0.
//
// jacobian is a row-major GlobalSize() x LocalSize() matrix.
virtual bool ComputeJacobian(const double* x, double* jacobian) const = 0;
// local_matrix = global_matrix * jacobian
//
// global_matrix is a num_rows x GlobalSize row major matrix.
// local_matrix is a num_rows x LocalSize row major matrix.
// jacobian(x) is the matrix returned by ComputeJacobian at x.
//
// This is only used by GradientProblem. For most normal uses, it is
// okay to use the default implementation.
virtual bool MultiplyByJacobian(const double* x,
const int num_rows,
const double* global_matrix,
double* local_matrix) const;
// Size of x.
virtual int GlobalSize() const = 0;
// Size of delta.
virtual int LocalSize() const = 0;
};
// Some basic parameterizations
// Identity Parameterization: Plus(x, delta) = x + delta
class CERES_EXPORT IdentityParameterization : public LocalParameterization {
public:
explicit IdentityParameterization(int size);
virtual ~IdentityParameterization() {}
virtual bool Plus(const double* x,
const double* delta,
double* x_plus_delta) const;
virtual bool ComputeJacobian(const double* x,
double* jacobian) const;
virtual bool MultiplyByJacobian(const double* x,
const int num_cols,
const double* global_matrix,
double* local_matrix) const;
virtual int GlobalSize() const { return size_; }
virtual int LocalSize() const { return size_; }
private:
const int size_;
};
// Hold a subset of the parameters inside a parameter block constant.
class CERES_EXPORT SubsetParameterization : public LocalParameterization {
public:
explicit SubsetParameterization(int size,
const std::vector<int>& constant_parameters);
virtual ~SubsetParameterization() {}
virtual bool Plus(const double* x,
const double* delta,
double* x_plus_delta) const;
virtual bool ComputeJacobian(const double* x,
double* jacobian) const;
virtual bool MultiplyByJacobian(const double* x,
const int num_cols,
const double* global_matrix,
double* local_matrix) const;
virtual int GlobalSize() const {
return static_cast<int>(constancy_mask_.size());
}
virtual int LocalSize() const { return local_size_; }
private:
const int local_size_;
std::vector<char> constancy_mask_;
};
// Plus(x, delta) = [cos(|delta|), sin(|delta|) delta / |delta|] * x
// with * being the quaternion multiplication operator. Here we assume
// that the first element of the quaternion vector is the real (cos
// theta) part.
class CERES_EXPORT QuaternionParameterization : public LocalParameterization {
public:
virtual ~QuaternionParameterization() {}
virtual bool Plus(const double* x,
const double* delta,
double* x_plus_delta) const;
virtual bool ComputeJacobian(const double* x,
double* jacobian) const;
virtual int GlobalSize() const { return 4; }
virtual int LocalSize() const { return 3; }
};
// This provides a parameterization for homogeneous vectors which are commonly
// used in Structure for Motion problems. One example where they are used is
// in representing points whose triangulation is ill-conditioned. Here
// it is advantageous to use an over-parameterization since homogeneous vectors
// can represent points at infinity.
//
// The plus operator is defined as
// Plus(x, delta) =
// [sin(0.5 * |delta|) * delta / |delta|, cos(0.5 * |delta|)] * x
// with * defined as an operator which applies the update orthogonal to x to
// remain on the sphere. We assume that the last element of x is the scalar
// component. The size of the homogeneous vector is required to be greater than
// 1.
class CERES_EXPORT HomogeneousVectorParameterization :
public LocalParameterization {
public:
explicit HomogeneousVectorParameterization(int size);
virtual ~HomogeneousVectorParameterization() {}
virtual bool Plus(const double* x,
const double* delta,
double* x_plus_delta) const;
virtual bool ComputeJacobian(const double* x,
double* jacobian) const;
virtual int GlobalSize() const { return size_; }
virtual int LocalSize() const { return size_ - 1; }
private:
const int size_;
};
// Construct a local parameterization by taking the Cartesian product
// of a number of other local parameterizations. This is useful, when
// a parameter block is the cartesian product of two or more
// manifolds. For example the parameters of a camera consist of a
// rotation and a translation, i.e., SO(3) x R^3.
//
// Currently this class supports taking the cartesian product of up to
// four local parameterizations.
//
// Example usage:
//
// ProductParameterization product_param(new QuaterionionParameterization(),
// new IdentityParameterization(3));
//
// is the local parameterization for a rigid transformation, where the
// rotation is represented using a quaternion.
class CERES_EXPORT ProductParameterization : public LocalParameterization {
public:
//
// NOTE: All the constructors take ownership of the input local
// parameterizations.
//
ProductParameterization(LocalParameterization* local_param1,
LocalParameterization* local_param2);
ProductParameterization(LocalParameterization* local_param1,
LocalParameterization* local_param2,
LocalParameterization* local_param3);
ProductParameterization(LocalParameterization* local_param1,
LocalParameterization* local_param2,
LocalParameterization* local_param3,
LocalParameterization* local_param4);
virtual ~ProductParameterization();
virtual bool Plus(const double* x,
const double* delta,
double* x_plus_delta) const;
virtual bool ComputeJacobian(const double* x,
double* jacobian) const;
virtual int GlobalSize() const { return global_size_; }
virtual int LocalSize() const { return local_size_; }
private:
void Init();
std::vector<LocalParameterization*> local_params_;
int local_size_;
int global_size_;
int buffer_size_;
};
} // namespace ceres
#include "ceres/internal/reenable_warnings.h"
#endif // CERES_PUBLIC_LOCAL_PARAMETERIZATION_H_

View File

@ -0,0 +1,428 @@
// 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)
//
// The LossFunction interface is the way users describe how residuals
// are converted to cost terms for the overall problem cost function.
// For the exact manner in which loss functions are converted to the
// overall cost for a problem, see problem.h.
//
// For least squares problem where there are no outliers and standard
// squared loss is expected, it is not necessary to create a loss
// function; instead passing a NULL to the problem when adding
// residuals implies a standard squared loss.
//
// For least squares problems where the minimization may encounter
// input terms that contain outliers, that is, completely bogus
// measurements, it is important to use a loss function that reduces
// their associated penalty.
//
// Consider a structure from motion problem. The unknowns are 3D
// points and camera parameters, and the measurements are image
// coordinates describing the expected reprojected position for a
// point in a camera. For example, we want to model the geometry of a
// street scene with fire hydrants and cars, observed by a moving
// camera with unknown parameters, and the only 3D points we care
// about are the pointy tippy-tops of the fire hydrants. Our magic
// image processing algorithm, which is responsible for producing the
// measurements that are input to Ceres, has found and matched all
// such tippy-tops in all image frames, except that in one of the
// frame it mistook a car's headlight for a hydrant. If we didn't do
// anything special (i.e. if we used a basic quadratic loss), the
// residual for the erroneous measurement will result in extreme error
// due to the quadratic nature of squared loss. This results in the
// entire solution getting pulled away from the optimimum to reduce
// the large error that would otherwise be attributed to the wrong
// measurement.
//
// Using a robust loss function, the cost for large residuals is
// reduced. In the example above, this leads to outlier terms getting
// downweighted so they do not overly influence the final solution.
//
// What cost function is best?
//
// In general, there isn't a principled way to select a robust loss
// function. The authors suggest starting with a non-robust cost, then
// only experimenting with robust loss functions if standard squared
// loss doesn't work.
#ifndef CERES_PUBLIC_LOSS_FUNCTION_H_
#define CERES_PUBLIC_LOSS_FUNCTION_H_
#include "glog/logging.h"
#include "ceres/internal/macros.h"
#include "ceres/internal/scoped_ptr.h"
#include "ceres/types.h"
#include "ceres/internal/disable_warnings.h"
namespace ceres {
class CERES_EXPORT LossFunction {
public:
virtual ~LossFunction() {}
// For a residual vector with squared 2-norm 'sq_norm', this method
// is required to fill in the value and derivatives of the loss
// function (rho in this example):
//
// out[0] = rho(sq_norm),
// out[1] = rho'(sq_norm),
// out[2] = rho''(sq_norm),
//
// Here the convention is that the contribution of a term to the
// cost function is given by 1/2 rho(s), where
//
// s = ||residuals||^2.
//
// Calling the method with a negative value of 's' is an error and
// the implementations are not required to handle that case.
//
// Most sane choices of rho() satisfy:
//
// rho(0) = 0,
// rho'(0) = 1,
// rho'(s) < 1 in outlier region,
// rho''(s) < 0 in outlier region,
//
// so that they mimic the least squares cost for small residuals.
virtual void Evaluate(double sq_norm, double out[3]) const = 0;
};
// Some common implementations follow below.
//
// Note: in the region of interest (i.e. s < 3) we have:
// TrivialLoss >= HuberLoss >= SoftLOneLoss >= CauchyLoss
// This corresponds to no robustification.
//
// rho(s) = s
//
// At s = 0: rho = [0, 1, 0].
//
// It is not normally necessary to use this, as passing NULL for the
// loss function when building the problem accomplishes the same
// thing.
class CERES_EXPORT TrivialLoss : public LossFunction {
public:
virtual void Evaluate(double, double*) const;
};
// Scaling
// -------
// Given one robustifier
// s -> rho(s)
// one can change the length scale at which robustification takes
// place, by adding a scale factor 'a' as follows:
//
// s -> a^2 rho(s / a^2).
//
// The first and second derivatives are:
//
// s -> rho'(s / a^2),
// s -> (1 / a^2) rho''(s / a^2),
//
// but the behaviour near s = 0 is the same as the original function,
// i.e.
//
// rho(s) = s + higher order terms,
// a^2 rho(s / a^2) = s + higher order terms.
//
// The scalar 'a' should be positive.
//
// The reason for the appearance of squaring is that 'a' is in the
// units of the residual vector norm whereas 's' is a squared
// norm. For applications it is more convenient to specify 'a' than
// its square. The commonly used robustifiers below are described in
// un-scaled format (a = 1) but their implementations work for any
// non-zero value of 'a'.
// Huber.
//
// rho(s) = s for s <= 1,
// rho(s) = 2 sqrt(s) - 1 for s >= 1.
//
// At s = 0: rho = [0, 1, 0].
//
// The scaling parameter 'a' corresponds to 'delta' on this page:
// http://en.wikipedia.org/wiki/Huber_Loss_Function
class CERES_EXPORT HuberLoss : public LossFunction {
public:
explicit HuberLoss(double a) : a_(a), b_(a * a) { }
virtual void Evaluate(double, double*) const;
private:
const double a_;
// b = a^2.
const double b_;
};
// Soft L1, similar to Huber but smooth.
//
// rho(s) = 2 (sqrt(1 + s) - 1).
//
// At s = 0: rho = [0, 1, -1/2].
class CERES_EXPORT SoftLOneLoss : public LossFunction {
public:
explicit SoftLOneLoss(double a) : b_(a * a), c_(1 / b_) { }
virtual void Evaluate(double, double*) const;
private:
// b = a^2.
const double b_;
// c = 1 / a^2.
const double c_;
};
// Inspired by the Cauchy distribution
//
// rho(s) = log(1 + s).
//
// At s = 0: rho = [0, 1, -1].
class CERES_EXPORT CauchyLoss : public LossFunction {
public:
explicit CauchyLoss(double a) : b_(a * a), c_(1 / b_) { }
virtual void Evaluate(double, double*) const;
private:
// b = a^2.
const double b_;
// c = 1 / a^2.
const double c_;
};
// Loss that is capped beyond a certain level using the arc-tangent function.
// The scaling parameter 'a' determines the level where falloff occurs.
// For costs much smaller than 'a', the loss function is linear and behaves like
// TrivialLoss, and for values much larger than 'a' the value asymptotically
// approaches the constant value of a * PI / 2.
//
// rho(s) = a atan(s / a).
//
// At s = 0: rho = [0, 1, 0].
class CERES_EXPORT ArctanLoss : public LossFunction {
public:
explicit ArctanLoss(double a) : a_(a), b_(1 / (a * a)) { }
virtual void Evaluate(double, double*) const;
private:
const double a_;
// b = 1 / a^2.
const double b_;
};
// Loss function that maps to approximately zero cost in a range around the
// origin, and reverts to linear in error (quadratic in cost) beyond this range.
// The tolerance parameter 'a' sets the nominal point at which the
// transition occurs, and the transition size parameter 'b' sets the nominal
// distance over which most of the transition occurs. Both a and b must be
// greater than zero, and typically b will be set to a fraction of a.
// The slope rho'[s] varies smoothly from about 0 at s <= a - b to
// about 1 at s >= a + b.
//
// The term is computed as:
//
// rho(s) = b log(1 + exp((s - a) / b)) - c0.
//
// where c0 is chosen so that rho(0) == 0
//
// c0 = b log(1 + exp(-a / b)
//
// This has the following useful properties:
//
// rho(s) == 0 for s = 0
// rho'(s) ~= 0 for s << a - b
// rho'(s) ~= 1 for s >> a + b
// rho''(s) > 0 for all s
//
// In addition, all derivatives are continuous, and the curvature is
// concentrated in the range a - b to a + b.
//
// At s = 0: rho = [0, ~0, ~0].
class CERES_EXPORT TolerantLoss : public LossFunction {
public:
explicit TolerantLoss(double a, double b);
virtual void Evaluate(double, double*) const;
private:
const double a_, b_, c_;
};
// This is the Tukey biweight loss function which aggressively
// attempts to suppress large errors.
//
// The term is computed as:
//
// rho(s) = a^2 / 6 * (1 - (1 - s / a^2)^3 ) for s <= a^2,
// rho(s) = a^2 / 6 for s > a^2.
//
// At s = 0: rho = [0, 0.5, -1 / a^2]
class CERES_EXPORT TukeyLoss : public ceres::LossFunction {
public:
explicit TukeyLoss(double a) : a_squared_(a * a) { }
virtual void Evaluate(double, double*) const;
private:
const double a_squared_;
};
// Composition of two loss functions. The error is the result of first
// evaluating g followed by f to yield the composition f(g(s)).
// The loss functions must not be NULL.
class CERES_EXPORT ComposedLoss : public LossFunction {
public:
explicit ComposedLoss(const LossFunction* f, Ownership ownership_f,
const LossFunction* g, Ownership ownership_g);
virtual ~ComposedLoss();
virtual void Evaluate(double, double*) const;
private:
internal::scoped_ptr<const LossFunction> f_, g_;
const Ownership ownership_f_, ownership_g_;
};
// The discussion above has to do with length scaling: it affects the space
// in which s is measured. Sometimes you want to simply scale the output
// value of the robustifier. For example, you might want to weight
// different error terms differently (e.g., weight pixel reprojection
// errors differently from terrain errors).
//
// If rho is the wrapped robustifier, then this simply outputs
// s -> a * rho(s)
//
// The first and second derivatives are, not surprisingly
// s -> a * rho'(s)
// s -> a * rho''(s)
//
// Since we treat the a NULL Loss function as the Identity loss
// function, rho = NULL is a valid input and will result in the input
// being scaled by a. This provides a simple way of implementing a
// scaled ResidualBlock.
class CERES_EXPORT ScaledLoss : public LossFunction {
public:
// Constructs a ScaledLoss wrapping another loss function. Takes
// ownership of the wrapped loss function or not depending on the
// ownership parameter.
ScaledLoss(const LossFunction* rho, double a, Ownership ownership) :
rho_(rho), a_(a), ownership_(ownership) { }
virtual ~ScaledLoss() {
if (ownership_ == DO_NOT_TAKE_OWNERSHIP) {
rho_.release();
}
}
virtual void Evaluate(double, double*) const;
private:
internal::scoped_ptr<const LossFunction> rho_;
const double a_;
const Ownership ownership_;
CERES_DISALLOW_COPY_AND_ASSIGN(ScaledLoss);
};
// Sometimes after the optimization problem has been constructed, we
// wish to mutate the scale of the loss function. For example, when
// performing estimation from data which has substantial outliers,
// convergence can be improved by starting out with a large scale,
// optimizing the problem and then reducing the scale. This can have
// better convergence behaviour than just using a loss function with a
// small scale.
//
// This templated class allows the user to implement a loss function
// whose scale can be mutated after an optimization problem has been
// constructed.
//
// Since we treat the a NULL Loss function as the Identity loss
// function, rho = NULL is a valid input.
//
// Example usage
//
// Problem problem;
//
// // Add parameter blocks
//
// CostFunction* cost_function =
// new AutoDiffCostFunction < UW_Camera_Mapper, 2, 9, 3>(
// new UW_Camera_Mapper(feature_x, feature_y));
//
// LossFunctionWrapper* loss_function(new HuberLoss(1.0), TAKE_OWNERSHIP);
//
// problem.AddResidualBlock(cost_function, loss_function, parameters);
//
// Solver::Options options;
// Solger::Summary summary;
//
// Solve(options, &problem, &summary)
//
// loss_function->Reset(new HuberLoss(1.0), TAKE_OWNERSHIP);
//
// Solve(options, &problem, &summary)
//
class CERES_EXPORT LossFunctionWrapper : public LossFunction {
public:
LossFunctionWrapper(LossFunction* rho, Ownership ownership)
: rho_(rho), ownership_(ownership) {
}
virtual ~LossFunctionWrapper() {
if (ownership_ == DO_NOT_TAKE_OWNERSHIP) {
rho_.release();
}
}
virtual void Evaluate(double sq_norm, double out[3]) const {
if (rho_.get() == NULL) {
out[0] = sq_norm;
out[1] = 1.0;
out[2] = 0.0;
}
else {
rho_->Evaluate(sq_norm, out);
}
}
void Reset(LossFunction* rho, Ownership ownership) {
if (ownership_ == DO_NOT_TAKE_OWNERSHIP) {
rho_.release();
}
rho_.reset(rho);
ownership_ = ownership;
}
private:
internal::scoped_ptr<const LossFunction> rho_;
Ownership ownership_;
CERES_DISALLOW_COPY_AND_ASSIGN(LossFunctionWrapper);
};
} // namespace ceres
#include "ceres/internal/reenable_warnings.h"
#endif // CERES_PUBLIC_LOSS_FUNCTION_H_

View File

@ -0,0 +1,78 @@
// 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)
//
// Cost term that implements a prior on a parameter block using a
// normal distribution.
#ifndef CERES_PUBLIC_NORMAL_PRIOR_H_
#define CERES_PUBLIC_NORMAL_PRIOR_H_
#include "ceres/cost_function.h"
#include "ceres/internal/eigen.h"
#include "ceres/internal/disable_warnings.h"
namespace ceres {
// Implements a cost function of the form
//
// cost(x) = ||A(x - b)||^2
//
// where, the matrix A and the vector b are fixed and x is the
// variable. In case the user is interested in implementing a cost
// function of the form
//
// cost(x) = (x - mu)^T S^{-1} (x - mu)
//
// where, mu is a vector and S is a covariance matrix, then, A =
// S^{-1/2}, i.e the matrix A is the square root of the inverse of the
// covariance, also known as the stiffness matrix. There are however
// no restrictions on the shape of A. It is free to be rectangular,
// which would be the case if the covariance matrix S is rank
// deficient.
class CERES_EXPORT NormalPrior: public CostFunction {
public:
// Check that the number of rows in the vector b are the same as the
// number of columns in the matrix A, crash otherwise.
NormalPrior(const Matrix& A, const Vector& b);
virtual bool Evaluate(double const* const* parameters,
double* residuals,
double** jacobians) const;
private:
Matrix A_;
Vector b_;
};
} // namespace ceres
#include "ceres/internal/reenable_warnings.h"
#endif // CERES_PUBLIC_NORMAL_PRIOR_H_

View File

@ -0,0 +1,342 @@
// 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)
// sameeragarwal@google.com (Sameer Agarwal)
//
// Create CostFunctions as needed by the least squares framework with jacobians
// computed via numeric (a.k.a. finite) differentiation. For more details see
// http://en.wikipedia.org/wiki/Numerical_differentiation.
//
// To get an numerically differentiated cost function, you must define
// a class with a operator() (a functor) that computes the residuals.
//
// The function must write the computed value in the last argument
// (the only non-const one) and return true to indicate success.
// Please see cost_function.h for details on how the return value
// maybe used to impose simple constraints on the parameter block.
//
// For example, consider a scalar error e = k - x'y, where both x and y are
// two-dimensional column vector parameters, the prime sign indicates
// transposition, and k is a constant. The form of this error, which is the
// difference between a constant and an expression, is a common pattern in least
// squares problems. For example, the value x'y might be the model expectation
// for a series of measurements, where there is an instance of the cost function
// for each measurement k.
//
// The actual cost added to the total problem is e^2, or (k - x'k)^2; however,
// the squaring is implicitly done by the optimization framework.
//
// To write an numerically-differentiable cost function for the above model, first
// define the object
//
// class MyScalarCostFunctor {
// MyScalarCostFunctor(double k): k_(k) {}
//
// bool operator()(const double* const x,
// const double* const y,
// double* residuals) const {
// residuals[0] = k_ - x[0] * y[0] + x[1] * y[1];
// return true;
// }
//
// private:
// double k_;
// };
//
// Note that in the declaration of operator() the input parameters x
// and y come first, and are passed as const pointers to arrays of
// doubles. If there were three input parameters, then the third input
// parameter would come after y. The output is always the last
// parameter, and is also a pointer to an array. In the example above,
// the residual is a scalar, so only residuals[0] is set.
//
// Then given this class definition, the numerically differentiated
// cost function with central differences used for computing the
// derivative can be constructed as follows.
//
// CostFunction* cost_function
// = new NumericDiffCostFunction<MyScalarCostFunctor, CENTRAL, 1, 2, 2>(
// new MyScalarCostFunctor(1.0)); ^ ^ ^ ^
// | | | |
// Finite Differencing Scheme -+ | | |
// Dimension of residual ------------+ | |
// Dimension of x ----------------------+ |
// Dimension of y -------------------------+
//
// In this example, there is usually an instance for each measurement of k.
//
// In the instantiation above, the template parameters following
// "MyScalarCostFunctor", "1, 2, 2", describe the functor as computing
// a 1-dimensional output from two arguments, both 2-dimensional.
//
// NumericDiffCostFunction also supports cost functions with a
// runtime-determined number of residuals. For example:
//
// CostFunction* cost_function
// = new NumericDiffCostFunction<MyScalarCostFunctor, CENTRAL, DYNAMIC, 2, 2>(
// new CostFunctorWithDynamicNumResiduals(1.0), ^ ^ ^
// TAKE_OWNERSHIP, | | |
// runtime_number_of_residuals); <----+ | | |
// | | | |
// | | | |
// Actual number of residuals ------+ | | |
// Indicate dynamic number of residuals --------------------+ | |
// Dimension of x ------------------------------------------------+ |
// Dimension of y ---------------------------------------------------+
//
// The framework can currently accommodate cost functions of up to 10
// independent variables, and there is no limit on the dimensionality
// of each of them.
//
// The central difference method is considerably more accurate at the cost of
// twice as many function evaluations than forward difference. Consider using
// central differences begin with, and only after that works, trying forward
// difference to improve performance.
//
// WARNING #1: A common beginner's error when first using
// NumericDiffCostFunction is to get the sizing wrong. In particular,
// there is a tendency to set the template parameters to (dimension of
// residual, number of parameters) instead of passing a dimension
// parameter for *every parameter*. In the example above, that would
// be <MyScalarCostFunctor, 1, 2>, which is missing the last '2'
// argument. Please be careful when setting the size parameters.
//
////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////
//
// ALTERNATE INTERFACE
//
// For a variety of reasons, including compatibility with legacy code,
// NumericDiffCostFunction can also take CostFunction objects as
// input. The following describes how.
//
// To get a numerically differentiated cost function, define a
// subclass of CostFunction such that the Evaluate() function ignores
// the jacobian parameter. The numeric differentiation wrapper will
// fill in the jacobian parameter if necessary by repeatedly calling
// the Evaluate() function with small changes to the appropriate
// parameters, and computing the slope. For performance, the numeric
// differentiation wrapper class is templated on the concrete cost
// function, even though it could be implemented only in terms of the
// virtual CostFunction interface.
//
// The numerically differentiated version of a cost function for a cost function
// can be constructed as follows:
//
// CostFunction* cost_function
// = new NumericDiffCostFunction<MyCostFunction, CENTRAL, 1, 4, 8>(
// new MyCostFunction(...), TAKE_OWNERSHIP);
//
// where MyCostFunction has 1 residual and 2 parameter blocks with sizes 4 and 8
// respectively. Look at the tests for a more detailed example.
//
// TODO(keir): Characterize accuracy; mention pitfalls; provide alternatives.
#ifndef CERES_PUBLIC_NUMERIC_DIFF_COST_FUNCTION_H_
#define CERES_PUBLIC_NUMERIC_DIFF_COST_FUNCTION_H_
#include "Eigen/Dense"
#include "ceres/cost_function.h"
#include "ceres/internal/numeric_diff.h"
#include "ceres/internal/scoped_ptr.h"
#include "ceres/numeric_diff_options.h"
#include "ceres/sized_cost_function.h"
#include "ceres/types.h"
#include "glog/logging.h"
namespace ceres {
template <typename CostFunctor,
NumericDiffMethodType method = CENTRAL,
int kNumResiduals = 0, // Number of residuals, or ceres::DYNAMIC
int N0 = 0, // Number of parameters in block 0.
int N1 = 0, // Number of parameters in block 1.
int N2 = 0, // Number of parameters in block 2.
int N3 = 0, // Number of parameters in block 3.
int N4 = 0, // Number of parameters in block 4.
int N5 = 0, // Number of parameters in block 5.
int N6 = 0, // Number of parameters in block 6.
int N7 = 0, // Number of parameters in block 7.
int N8 = 0, // Number of parameters in block 8.
int N9 = 0> // Number of parameters in block 9.
class NumericDiffCostFunction
: public SizedCostFunction<kNumResiduals,
N0, N1, N2, N3, N4,
N5, N6, N7, N8, N9> {
public:
NumericDiffCostFunction(
CostFunctor* functor,
Ownership ownership = TAKE_OWNERSHIP,
int num_residuals = kNumResiduals,
const NumericDiffOptions& options = NumericDiffOptions())
: functor_(functor),
ownership_(ownership),
options_(options) {
if (kNumResiduals == DYNAMIC) {
SizedCostFunction<kNumResiduals,
N0, N1, N2, N3, N4,
N5, N6, N7, N8, N9>
::set_num_residuals(num_residuals);
}
}
// Deprecated. New users should avoid using this constructor. Instead, use the
// constructor with NumericDiffOptions.
NumericDiffCostFunction(CostFunctor* functor,
Ownership ownership,
int num_residuals,
const double relative_step_size)
:functor_(functor),
ownership_(ownership),
options_() {
LOG(WARNING) << "This constructor is deprecated and will be removed in "
"a future version. Please use the NumericDiffOptions "
"constructor instead.";
if (kNumResiduals == DYNAMIC) {
SizedCostFunction<kNumResiduals,
N0, N1, N2, N3, N4,
N5, N6, N7, N8, N9>
::set_num_residuals(num_residuals);
}
options_.relative_step_size = relative_step_size;
}
~NumericDiffCostFunction() {
if (ownership_ != TAKE_OWNERSHIP) {
functor_.release();
}
}
virtual bool Evaluate(double const* const* parameters,
double* residuals,
double** jacobians) const {
using internal::FixedArray;
using internal::NumericDiff;
const int kNumParameters = N0 + N1 + N2 + N3 + N4 + N5 + N6 + N7 + N8 + N9;
const int kNumParameterBlocks =
(N0 > 0) + (N1 > 0) + (N2 > 0) + (N3 > 0) + (N4 > 0) +
(N5 > 0) + (N6 > 0) + (N7 > 0) + (N8 > 0) + (N9 > 0);
// Get the function value (residuals) at the the point to evaluate.
if (!internal::EvaluateImpl<CostFunctor,
N0, N1, N2, N3, N4, N5, N6, N7, N8, N9>(
functor_.get(),
parameters,
residuals,
functor_.get())) {
return false;
}
if (jacobians == NULL) {
return true;
}
// Create a copy of the parameters which will get mutated.
FixedArray<double> parameters_copy(kNumParameters);
FixedArray<double*> parameters_reference_copy(kNumParameterBlocks);
parameters_reference_copy[0] = parameters_copy.get();
if (N1) parameters_reference_copy[1] = parameters_reference_copy[0] + N0;
if (N2) parameters_reference_copy[2] = parameters_reference_copy[1] + N1;
if (N3) parameters_reference_copy[3] = parameters_reference_copy[2] + N2;
if (N4) parameters_reference_copy[4] = parameters_reference_copy[3] + N3;
if (N5) parameters_reference_copy[5] = parameters_reference_copy[4] + N4;
if (N6) parameters_reference_copy[6] = parameters_reference_copy[5] + N5;
if (N7) parameters_reference_copy[7] = parameters_reference_copy[6] + N6;
if (N8) parameters_reference_copy[8] = parameters_reference_copy[7] + N7;
if (N9) parameters_reference_copy[9] = parameters_reference_copy[8] + N8;
#define CERES_COPY_PARAMETER_BLOCK(block) \
if (N ## block) memcpy(parameters_reference_copy[block], \
parameters[block], \
sizeof(double) * N ## block); // NOLINT
CERES_COPY_PARAMETER_BLOCK(0);
CERES_COPY_PARAMETER_BLOCK(1);
CERES_COPY_PARAMETER_BLOCK(2);
CERES_COPY_PARAMETER_BLOCK(3);
CERES_COPY_PARAMETER_BLOCK(4);
CERES_COPY_PARAMETER_BLOCK(5);
CERES_COPY_PARAMETER_BLOCK(6);
CERES_COPY_PARAMETER_BLOCK(7);
CERES_COPY_PARAMETER_BLOCK(8);
CERES_COPY_PARAMETER_BLOCK(9);
#undef CERES_COPY_PARAMETER_BLOCK
#define CERES_EVALUATE_JACOBIAN_FOR_BLOCK(block) \
if (N ## block && jacobians[block] != NULL) { \
if (!NumericDiff<CostFunctor, \
method, \
kNumResiduals, \
N0, N1, N2, N3, N4, N5, N6, N7, N8, N9, \
block, \
N ## block >::EvaluateJacobianForParameterBlock( \
functor_.get(), \
residuals, \
options_, \
SizedCostFunction<kNumResiduals, \
N0, N1, N2, N3, N4, \
N5, N6, N7, N8, N9>::num_residuals(), \
block, \
N ## block, \
parameters_reference_copy.get(), \
jacobians[block])) { \
return false; \
} \
}
CERES_EVALUATE_JACOBIAN_FOR_BLOCK(0);
CERES_EVALUATE_JACOBIAN_FOR_BLOCK(1);
CERES_EVALUATE_JACOBIAN_FOR_BLOCK(2);
CERES_EVALUATE_JACOBIAN_FOR_BLOCK(3);
CERES_EVALUATE_JACOBIAN_FOR_BLOCK(4);
CERES_EVALUATE_JACOBIAN_FOR_BLOCK(5);
CERES_EVALUATE_JACOBIAN_FOR_BLOCK(6);
CERES_EVALUATE_JACOBIAN_FOR_BLOCK(7);
CERES_EVALUATE_JACOBIAN_FOR_BLOCK(8);
CERES_EVALUATE_JACOBIAN_FOR_BLOCK(9);
#undef CERES_EVALUATE_JACOBIAN_FOR_BLOCK
return true;
}
private:
internal::scoped_ptr<CostFunctor> functor_;
Ownership ownership_;
NumericDiffOptions options_;
};
} // namespace ceres
#endif // CERES_PUBLIC_NUMERIC_DIFF_COST_FUNCTION_H_

View File

@ -0,0 +1,79 @@
// 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: tbennun@gmail.com (Tal Ben-Nun)
//
#ifndef CERES_PUBLIC_NUMERIC_DIFF_OPTIONS_H_
#define CERES_PUBLIC_NUMERIC_DIFF_OPTIONS_H_
namespace ceres {
// Options pertaining to numeric differentiation (e.g., convergence criteria,
// step sizes).
struct CERES_EXPORT NumericDiffOptions {
NumericDiffOptions() {
relative_step_size = 1e-6;
ridders_relative_initial_step_size = 1e-2;
max_num_ridders_extrapolations = 10;
ridders_epsilon = 1e-12;
ridders_step_shrink_factor = 2.0;
}
// Numeric differentiation step size (multiplied by parameter block's
// order of magnitude). If parameters are close to zero, the step size
// is set to sqrt(machine_epsilon).
double relative_step_size;
// Initial step size for Ridders adaptive numeric differentiation (multiplied
// by parameter block's order of magnitude).
// If parameters are close to zero, Ridders' method sets the step size
// directly to this value. This parameter is separate from
// "relative_step_size" in order to set a different default value.
//
// Note: For Ridders' method to converge, the step size should be initialized
// to a value that is large enough to produce a significant change in the
// function. As the derivative is estimated, the step size decreases.
double ridders_relative_initial_step_size;
// Maximal number of adaptive extrapolations (sampling) in Ridders' method.
int max_num_ridders_extrapolations;
// Convergence criterion on extrapolation error for Ridders adaptive
// differentiation. The available error estimation methods are defined in
// NumericDiffErrorType and set in the "ridders_error_method" field.
double ridders_epsilon;
// The factor in which to shrink the step size with each extrapolation in
// Ridders' method.
double ridders_step_shrink_factor;
};
} // namespace ceres
#endif // CERES_PUBLIC_NUMERIC_DIFF_OPTIONS_H_

View File

@ -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: sameeragarwal@google.com (Sameer Agarwal)
#ifndef CERES_PUBLIC_ORDERED_GROUPS_H_
#define CERES_PUBLIC_ORDERED_GROUPS_H_
#include <map>
#include <set>
#include <vector>
#include "ceres/internal/port.h"
#include "glog/logging.h"
namespace ceres {
// A class for storing and manipulating an ordered collection of
// groups/sets with the following semantics:
//
// Group ids are non-negative integer values. Elements are any type
// that can serve as a key in a map or an element of a set.
//
// An element can only belong to one group at a time. A group may
// contain an arbitrary number of elements.
//
// Groups are ordered by their group id.
template <typename T>
class OrderedGroups {
public:
// Add an element to a group. If a group with this id does not
// exist, one is created. This method can be called any number of
// times for the same element. Group ids should be non-negative
// numbers.
//
// Return value indicates if adding the element was a success.
bool AddElementToGroup(const T element, const int group) {
if (group < 0) {
return false;
}
typename std::map<T, int>::const_iterator it =
element_to_group_.find(element);
if (it != element_to_group_.end()) {
if (it->second == group) {
// Element is already in the right group, nothing to do.
return true;
}
group_to_elements_[it->second].erase(element);
if (group_to_elements_[it->second].size() == 0) {
group_to_elements_.erase(it->second);
}
}
element_to_group_[element] = group;
group_to_elements_[group].insert(element);
return true;
}
void Clear() {
group_to_elements_.clear();
element_to_group_.clear();
}
// Remove the element, no matter what group it is in. Return value
// indicates if the element was actually removed.
bool Remove(const T element) {
const int current_group = GroupId(element);
if (current_group < 0) {
return false;
}
group_to_elements_[current_group].erase(element);
if (group_to_elements_[current_group].size() == 0) {
// If the group is empty, then get rid of it.
group_to_elements_.erase(current_group);
}
element_to_group_.erase(element);
return true;
}
// Bulk remove elements. The return value indicates the number of
// elements successfully removed.
int Remove(const std::vector<T>& elements) {
if (NumElements() == 0 || elements.size() == 0) {
return 0;
}
int num_removed = 0;
for (int i = 0; i < elements.size(); ++i) {
num_removed += Remove(elements[i]);
}
return num_removed;
}
// Reverse the order of the groups in place.
void Reverse() {
if (NumGroups() == 0) {
return;
}
typename std::map<int, std::set<T> >::reverse_iterator it =
group_to_elements_.rbegin();
std::map<int, std::set<T> > new_group_to_elements;
new_group_to_elements[it->first] = it->second;
int new_group_id = it->first + 1;
for (++it; it != group_to_elements_.rend(); ++it) {
for (typename std::set<T>::const_iterator element_it = it->second.begin();
element_it != it->second.end();
++element_it) {
element_to_group_[*element_it] = new_group_id;
}
new_group_to_elements[new_group_id] = it->second;
new_group_id++;
}
group_to_elements_.swap(new_group_to_elements);
}
// Return the group id for the element. If the element is not a
// member of any group, return -1.
int GroupId(const T element) const {
typename std::map<T, int>::const_iterator it =
element_to_group_.find(element);
if (it == element_to_group_.end()) {
return -1;
}
return it->second;
}
bool IsMember(const T element) const {
typename std::map<T, int>::const_iterator it =
element_to_group_.find(element);
return (it != element_to_group_.end());
}
// This function always succeeds, i.e., implicitly there exists a
// group for every integer.
int GroupSize(const int group) const {
typename std::map<int, std::set<T> >::const_iterator it =
group_to_elements_.find(group);
return (it == group_to_elements_.end()) ? 0 : it->second.size();
}
int NumElements() const {
return element_to_group_.size();
}
// Number of groups with one or more elements.
int NumGroups() const {
return group_to_elements_.size();
}
// The first group with one or more elements. Calling this when
// there are no groups with non-zero elements will result in a
// crash.
int MinNonZeroGroup() const {
CHECK_NE(NumGroups(), 0);
return group_to_elements_.begin()->first;
}
const std::map<int, std::set<T> >& group_to_elements() const {
return group_to_elements_;
}
const std::map<T, int>& element_to_group() const {
return element_to_group_;
}
private:
std::map<int, std::set<T> > group_to_elements_;
std::map<T, int> element_to_group_;
};
// Typedef for the most commonly used version of OrderedGroups.
typedef OrderedGroups<double*> ParameterBlockOrdering;
} // namespace ceres
#endif // CERES_PUBLIC_ORDERED_GROUP_H_

View File

@ -0,0 +1,481 @@
// 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)
// keir@google.com (Keir Mierle)
//
// The Problem object is used to build and hold least squares problems.
#ifndef CERES_PUBLIC_PROBLEM_H_
#define CERES_PUBLIC_PROBLEM_H_
#include <cstddef>
#include <map>
#include <set>
#include <vector>
#include "glog/logging.h"
#include "ceres/internal/macros.h"
#include "ceres/internal/port.h"
#include "ceres/internal/scoped_ptr.h"
#include "ceres/types.h"
#include "ceres/internal/disable_warnings.h"
namespace ceres {
class CostFunction;
class LossFunction;
class LocalParameterization;
class Solver;
struct CRSMatrix;
namespace internal {
class Preprocessor;
class ProblemImpl;
class ParameterBlock;
class ResidualBlock;
} // namespace internal
// A ResidualBlockId is an opaque handle clients can use to remove residual
// blocks from a Problem after adding them.
typedef internal::ResidualBlock* ResidualBlockId;
// A class to represent non-linear least squares problems. Such
// problems have a cost function that is a sum of error terms (known
// as "residuals"), where each residual is a function of some subset
// of the parameters. The cost function takes the form
//
// N 1
// SUM --- loss( || r_i1, r_i2,..., r_ik ||^2 ),
// i=1 2
//
// where
//
// r_ij is residual number i, component j; the residual is a
// function of some subset of the parameters x1...xk. For
// example, in a structure from motion problem a residual
// might be the difference between a measured point in an
// image and the reprojected position for the matching
// camera, point pair. The residual would have two
// components, error in x and error in y.
//
// loss(y) is the loss function; for example, squared error or
// Huber L1 loss. If loss(y) = y, then the cost function is
// non-robustified least squares.
//
// This class is specifically designed to address the important subset
// of "sparse" least squares problems, where each component of the
// residual depends only on a small number number of parameters, even
// though the total number of residuals and parameters may be very
// large. This property affords tremendous gains in scale, allowing
// efficient solving of large problems that are otherwise
// inaccessible.
//
// The canonical example of a sparse least squares problem is
// "structure-from-motion" (SFM), where the parameters are points and
// cameras, and residuals are reprojection errors. Typically a single
// residual will depend only on 9 parameters (3 for the point, 6 for
// the camera).
//
// To create a least squares problem, use the AddResidualBlock() and
// AddParameterBlock() methods, documented below. Here is an example least
// squares problem containing 3 parameter blocks of sizes 3, 4 and 5
// respectively and two residual terms of size 2 and 6:
//
// double x1[] = { 1.0, 2.0, 3.0 };
// double x2[] = { 1.0, 2.0, 3.0, 5.0 };
// double x3[] = { 1.0, 2.0, 3.0, 6.0, 7.0 };
//
// Problem problem;
//
// problem.AddResidualBlock(new MyUnaryCostFunction(...), x1);
// problem.AddResidualBlock(new MyBinaryCostFunction(...), x2, x3);
//
// Please see cost_function.h for details of the CostFunction object.
class CERES_EXPORT Problem {
public:
struct CERES_EXPORT Options {
Options()
: cost_function_ownership(TAKE_OWNERSHIP),
loss_function_ownership(TAKE_OWNERSHIP),
local_parameterization_ownership(TAKE_OWNERSHIP),
enable_fast_removal(false),
disable_all_safety_checks(false) {}
// These flags control whether the Problem object owns the cost
// functions, loss functions, and parameterizations passed into
// the Problem. If set to TAKE_OWNERSHIP, then the problem object
// will delete the corresponding cost or loss functions on
// destruction. The destructor is careful to delete the pointers
// only once, since sharing cost/loss/parameterizations is
// allowed.
Ownership cost_function_ownership;
Ownership loss_function_ownership;
Ownership local_parameterization_ownership;
// If true, trades memory for faster RemoveResidualBlock() and
// RemoveParameterBlock() operations.
//
// By default, RemoveParameterBlock() and RemoveResidualBlock() take time
// proportional to the size of the entire problem. If you only ever remove
// parameters or residuals from the problem occassionally, this might be
// acceptable. However, if you have memory to spare, enable this option to
// make RemoveParameterBlock() take time proportional to the number of
// residual blocks that depend on it, and RemoveResidualBlock() take (on
// average) constant time.
//
// The increase in memory usage is twofold: an additonal hash set per
// parameter block containing all the residuals that depend on the parameter
// block; and a hash set in the problem containing all residuals.
bool enable_fast_removal;
// By default, Ceres performs a variety of safety checks when constructing
// the problem. There is a small but measurable performance penalty to
// these checks, typically around 5% of construction time. If you are sure
// your problem construction is correct, and 5% of the problem construction
// time is truly an overhead you want to avoid, then you can set
// disable_all_safety_checks to true.
//
// WARNING: Do not set this to true, unless you are absolutely sure of what
// you are doing.
bool disable_all_safety_checks;
};
// The default constructor is equivalent to the
// invocation Problem(Problem::Options()).
Problem();
explicit Problem(const Options& options);
~Problem();
// Add a residual block to the overall cost function. The cost
// function carries with it information about the sizes of the
// parameter blocks it expects. The function checks that these match
// the sizes of the parameter blocks listed in parameter_blocks. The
// program aborts if a mismatch is detected. loss_function can be
// NULL, in which case the cost of the term is just the squared norm
// of the residuals.
//
// The user has the option of explicitly adding the parameter blocks
// using AddParameterBlock. This causes additional correctness
// checking; however, AddResidualBlock implicitly adds the parameter
// blocks if they are not present, so calling AddParameterBlock
// explicitly is not required.
//
// The Problem object by default takes ownership of the
// cost_function and loss_function pointers. These objects remain
// live for the life of the Problem object. If the user wishes to
// keep control over the destruction of these objects, then they can
// do this by setting the corresponding enums in the Options struct.
//
// Note: Even though the Problem takes ownership of cost_function
// and loss_function, it does not preclude the user from re-using
// them in another residual block. The destructor takes care to call
// delete on each cost_function or loss_function pointer only once,
// regardless of how many residual blocks refer to them.
//
// Example usage:
//
// double x1[] = {1.0, 2.0, 3.0};
// double x2[] = {1.0, 2.0, 5.0, 6.0};
// double x3[] = {3.0, 6.0, 2.0, 5.0, 1.0};
//
// Problem problem;
//
// problem.AddResidualBlock(new MyUnaryCostFunction(...), NULL, x1);
// problem.AddResidualBlock(new MyBinaryCostFunction(...), NULL, x2, x1);
//
ResidualBlockId AddResidualBlock(
CostFunction* cost_function,
LossFunction* loss_function,
const std::vector<double*>& parameter_blocks);
// Convenience methods for adding residuals with a small number of
// parameters. This is the common case. Instead of specifying the
// parameter block arguments as a vector, list them as pointers.
ResidualBlockId AddResidualBlock(CostFunction* cost_function,
LossFunction* loss_function,
double* x0);
ResidualBlockId AddResidualBlock(CostFunction* cost_function,
LossFunction* loss_function,
double* x0, double* x1);
ResidualBlockId AddResidualBlock(CostFunction* cost_function,
LossFunction* loss_function,
double* x0, double* x1, double* x2);
ResidualBlockId AddResidualBlock(CostFunction* cost_function,
LossFunction* loss_function,
double* x0, double* x1, double* x2,
double* x3);
ResidualBlockId AddResidualBlock(CostFunction* cost_function,
LossFunction* loss_function,
double* x0, double* x1, double* x2,
double* x3, double* x4);
ResidualBlockId AddResidualBlock(CostFunction* cost_function,
LossFunction* loss_function,
double* x0, double* x1, double* x2,
double* x3, double* x4, double* x5);
ResidualBlockId AddResidualBlock(CostFunction* cost_function,
LossFunction* loss_function,
double* x0, double* x1, double* x2,
double* x3, double* x4, double* x5,
double* x6);
ResidualBlockId AddResidualBlock(CostFunction* cost_function,
LossFunction* loss_function,
double* x0, double* x1, double* x2,
double* x3, double* x4, double* x5,
double* x6, double* x7);
ResidualBlockId AddResidualBlock(CostFunction* cost_function,
LossFunction* loss_function,
double* x0, double* x1, double* x2,
double* x3, double* x4, double* x5,
double* x6, double* x7, double* x8);
ResidualBlockId AddResidualBlock(CostFunction* cost_function,
LossFunction* loss_function,
double* x0, double* x1, double* x2,
double* x3, double* x4, double* x5,
double* x6, double* x7, double* x8,
double* x9);
// Add a parameter block with appropriate size to the problem.
// Repeated calls with the same arguments are ignored. Repeated
// calls with the same double pointer but a different size results
// in undefined behaviour.
void AddParameterBlock(double* values, int size);
// Add a parameter block with appropriate size and parameterization
// to the problem. Repeated calls with the same arguments are
// ignored. Repeated calls with the same double pointer but a
// different size results in undefined behaviour.
void AddParameterBlock(double* values,
int size,
LocalParameterization* local_parameterization);
// Remove a parameter block from the problem. The parameterization of the
// parameter block, if it exists, will persist until the deletion of the
// problem (similar to cost/loss functions in residual block removal). Any
// residual blocks that depend on the parameter are also removed, as
// described above in RemoveResidualBlock().
//
// If Problem::Options::enable_fast_removal is true, then the
// removal is fast (almost constant time). Otherwise, removing a parameter
// block will incur a scan of the entire Problem object.
//
// WARNING: Removing a residual or parameter block will destroy the implicit
// ordering, rendering the jacobian or residuals returned from the solver
// uninterpretable. If you depend on the evaluated jacobian, do not use
// remove! This may change in a future release.
void RemoveParameterBlock(double* values);
// Remove a residual block from the problem. Any parameters that the residual
// block depends on are not removed. The cost and loss functions for the
// residual block will not get deleted immediately; won't happen until the
// problem itself is deleted.
//
// WARNING: Removing a residual or parameter block will destroy the implicit
// ordering, rendering the jacobian or residuals returned from the solver
// uninterpretable. If you depend on the evaluated jacobian, do not use
// remove! This may change in a future release.
void RemoveResidualBlock(ResidualBlockId residual_block);
// Hold the indicated parameter block constant during optimization.
void SetParameterBlockConstant(double* values);
// Allow the indicated parameter block to vary during optimization.
void SetParameterBlockVariable(double* values);
// Set the local parameterization for one of the parameter blocks.
// The local_parameterization is owned by the Problem by default. It
// is acceptable to set the same parameterization for multiple
// parameters; the destructor is careful to delete local
// parameterizations only once. The local parameterization can only
// be set once per parameter, and cannot be changed once set.
void SetParameterization(double* values,
LocalParameterization* local_parameterization);
// Get the local parameterization object associated with this
// parameter block. If there is no parameterization object
// associated then NULL is returned.
const LocalParameterization* GetParameterization(double* values) const;
// Set the lower/upper bound for the parameter with position "index".
void SetParameterLowerBound(double* values, int index, double lower_bound);
void SetParameterUpperBound(double* values, int index, double upper_bound);
// Number of parameter blocks in the problem. Always equals
// parameter_blocks().size() and parameter_block_sizes().size().
int NumParameterBlocks() const;
// The size of the parameter vector obtained by summing over the
// sizes of all the parameter blocks.
int NumParameters() const;
// Number of residual blocks in the problem. Always equals
// residual_blocks().size().
int NumResidualBlocks() const;
// The size of the residual vector obtained by summing over the
// sizes of all of the residual blocks.
int NumResiduals() const;
// The size of the parameter block.
int ParameterBlockSize(const double* values) const;
// The size of local parameterization for the parameter block. If
// there is no local parameterization associated with this parameter
// block, then ParameterBlockLocalSize = ParameterBlockSize.
int ParameterBlockLocalSize(const double* values) const;
// Is the given parameter block present in this problem or not?
bool HasParameterBlock(const double* values) const;
// Fills the passed parameter_blocks vector with pointers to the
// parameter blocks currently in the problem. After this call,
// parameter_block.size() == NumParameterBlocks.
void GetParameterBlocks(std::vector<double*>* parameter_blocks) const;
// Fills the passed residual_blocks vector with pointers to the
// residual blocks currently in the problem. After this call,
// residual_blocks.size() == NumResidualBlocks.
void GetResidualBlocks(std::vector<ResidualBlockId>* residual_blocks) const;
// Get all the parameter blocks that depend on the given residual block.
void GetParameterBlocksForResidualBlock(
const ResidualBlockId residual_block,
std::vector<double*>* parameter_blocks) const;
// Get the CostFunction for the given residual block.
const CostFunction* GetCostFunctionForResidualBlock(
const ResidualBlockId residual_block) const;
// Get the LossFunction for the given residual block. Returns NULL
// if no loss function is associated with this residual block.
const LossFunction* GetLossFunctionForResidualBlock(
const ResidualBlockId residual_block) const;
// Get all the residual blocks that depend on the given parameter block.
//
// If Problem::Options::enable_fast_removal is true, then
// getting the residual blocks is fast and depends only on the number of
// residual blocks. Otherwise, getting the residual blocks for a parameter
// block will incur a scan of the entire Problem object.
void GetResidualBlocksForParameterBlock(
const double* values,
std::vector<ResidualBlockId>* residual_blocks) const;
// Options struct to control Problem::Evaluate.
struct EvaluateOptions {
EvaluateOptions()
: apply_loss_function(true),
num_threads(1) {
}
// The set of parameter blocks for which evaluation should be
// performed. This vector determines the order that parameter
// blocks occur in the gradient vector and in the columns of the
// jacobian matrix. If parameter_blocks is empty, then it is
// assumed to be equal to vector containing ALL the parameter
// blocks. Generally speaking the parameter blocks will occur in
// the order in which they were added to the problem. But, this
// may change if the user removes any parameter blocks from the
// problem.
//
// NOTE: This vector should contain the same pointers as the ones
// used to add parameter blocks to the Problem. These parameter
// block should NOT point to new memory locations. Bad things will
// happen otherwise.
std::vector<double*> parameter_blocks;
// The set of residual blocks to evaluate. This vector determines
// the order in which the residuals occur, and how the rows of the
// jacobian are ordered. If residual_blocks is empty, then it is
// assumed to be equal to the vector containing ALL the residual
// blocks. Generally speaking the residual blocks will occur in
// the order in which they were added to the problem. But, this
// may change if the user removes any residual blocks from the
// problem.
std::vector<ResidualBlockId> residual_blocks;
// Even though the residual blocks in the problem may contain loss
// functions, setting apply_loss_function to false will turn off
// the application of the loss function to the output of the cost
// function. This is of use for example if the user wishes to
// analyse the solution quality by studying the distribution of
// residuals before and after the solve.
bool apply_loss_function;
int num_threads;
};
// Evaluate Problem. Any of the output pointers can be NULL. Which
// residual blocks and parameter blocks are used is controlled by
// the EvaluateOptions struct above.
//
// Note 1: The evaluation will use the values stored in the memory
// locations pointed to by the parameter block pointers used at the
// time of the construction of the problem. i.e.,
//
// Problem problem;
// double x = 1;
// problem.AddResidualBlock(new MyCostFunction, NULL, &x);
//
// double cost = 0.0;
// problem.Evaluate(Problem::EvaluateOptions(), &cost, NULL, NULL, NULL);
//
// The cost is evaluated at x = 1. If you wish to evaluate the
// problem at x = 2, then
//
// x = 2;
// problem.Evaluate(Problem::EvaluateOptions(), &cost, NULL, NULL, NULL);
//
// is the way to do so.
//
// Note 2: If no local parameterizations are used, then the size of
// the gradient vector (and the number of columns in the jacobian)
// is the sum of the sizes of all the parameter blocks. If a
// parameter block has a local parameterization, then it contributes
// "LocalSize" entries to the gradient vector (and the number of
// columns in the jacobian).
bool Evaluate(const EvaluateOptions& options,
double* cost,
std::vector<double>* residuals,
std::vector<double>* gradient,
CRSMatrix* jacobian);
private:
friend class Solver;
friend class Covariance;
internal::scoped_ptr<internal::ProblemImpl> problem_impl_;
CERES_DISALLOW_COPY_AND_ASSIGN(Problem);
};
} // namespace ceres
#include "ceres/internal/reenable_warnings.h"
#endif // CERES_PUBLIC_PROBLEM_H_

View File

@ -0,0 +1,629 @@
// 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)
// sameeragarwal@google.com (Sameer Agarwal)
//
// Templated functions for manipulating rotations. The templated
// functions are useful when implementing functors for automatic
// differentiation.
//
// In the following, the Quaternions are laid out as 4-vectors, thus:
//
// q[0] scalar part.
// q[1] coefficient of i.
// q[2] coefficient of j.
// q[3] coefficient of k.
//
// where: i*i = j*j = k*k = -1 and i*j = k, j*k = i, k*i = j.
#ifndef CERES_PUBLIC_ROTATION_H_
#define CERES_PUBLIC_ROTATION_H_
#include <algorithm>
#include <cmath>
#include <limits>
#include "glog/logging.h"
namespace ceres {
// Trivial wrapper to index linear arrays as matrices, given a fixed
// column and row stride. When an array "T* array" is wrapped by a
//
// (const) MatrixAdapter<T, row_stride, col_stride> M"
//
// the expression M(i, j) is equivalent to
//
// arrary[i * row_stride + j * col_stride]
//
// Conversion functions to and from rotation matrices accept
// MatrixAdapters to permit using row-major and column-major layouts,
// and rotation matrices embedded in larger matrices (such as a 3x4
// projection matrix).
template <typename T, int row_stride, int col_stride>
struct MatrixAdapter;
// Convenience functions to create a MatrixAdapter that treats the
// array pointed to by "pointer" as a 3x3 (contiguous) column-major or
// row-major matrix.
template <typename T>
MatrixAdapter<T, 1, 3> ColumnMajorAdapter3x3(T* pointer);
template <typename T>
MatrixAdapter<T, 3, 1> RowMajorAdapter3x3(T* pointer);
// Convert a value in combined axis-angle representation to a quaternion.
// The value angle_axis is a triple whose norm is an angle in radians,
// and whose direction is aligned with the axis of rotation,
// and quaternion is a 4-tuple that will contain the resulting quaternion.
// The implementation may be used with auto-differentiation up to the first
// derivative, higher derivatives may have unexpected results near the origin.
template<typename T>
void AngleAxisToQuaternion(const T* angle_axis, T* quaternion);
// Convert a quaternion to the equivalent combined axis-angle representation.
// The value quaternion must be a unit quaternion - it is not normalized first,
// and angle_axis will be filled with a value whose norm is the angle of
// rotation in radians, and whose direction is the axis of rotation.
// The implemention may be used with auto-differentiation up to the first
// derivative, higher derivatives may have unexpected results near the origin.
template<typename T>
void QuaternionToAngleAxis(const T* quaternion, T* angle_axis);
// Conversions between 3x3 rotation matrix (in column major order) and
// quaternion rotation representations. Templated for use with
// autodifferentiation.
template <typename T>
void RotationMatrixToQuaternion(const T* R, T* quaternion);
template <typename T, int row_stride, int col_stride>
void RotationMatrixToQuaternion(
const MatrixAdapter<const T, row_stride, col_stride>& R,
T* quaternion);
// Conversions between 3x3 rotation matrix (in column major order) and
// axis-angle rotation representations. Templated for use with
// autodifferentiation.
template <typename T>
void RotationMatrixToAngleAxis(const T* R, T* angle_axis);
template <typename T, int row_stride, int col_stride>
void RotationMatrixToAngleAxis(
const MatrixAdapter<const T, row_stride, col_stride>& R,
T* angle_axis);
template <typename T>
void AngleAxisToRotationMatrix(const T* angle_axis, T* R);
template <typename T, int row_stride, int col_stride>
void AngleAxisToRotationMatrix(
const T* angle_axis,
const MatrixAdapter<T, row_stride, col_stride>& R);
// Conversions between 3x3 rotation matrix (in row major order) and
// Euler angle (in degrees) rotation representations.
//
// The {pitch,roll,yaw} Euler angles are rotations around the {x,y,z}
// axes, respectively. They are applied in that same order, so the
// total rotation R is Rz * Ry * Rx.
template <typename T>
void EulerAnglesToRotationMatrix(const T* euler, int row_stride, T* R);
template <typename T, int row_stride, int col_stride>
void EulerAnglesToRotationMatrix(
const T* euler,
const MatrixAdapter<T, row_stride, col_stride>& R);
// Convert a 4-vector to a 3x3 scaled rotation matrix.
//
// The choice of rotation is such that the quaternion [1 0 0 0] goes to an
// identity matrix and for small a, b, c the quaternion [1 a b c] goes to
// the matrix
//
// [ 0 -c b ]
// I + 2 [ c 0 -a ] + higher order terms
// [ -b a 0 ]
//
// which corresponds to a Rodrigues approximation, the last matrix being
// the cross-product matrix of [a b c]. Together with the property that
// R(q1 * q2) = R(q1) * R(q2) this uniquely defines the mapping from q to R.
//
// No normalization of the quaternion is performed, i.e.
// R = ||q||^2 * Q, where Q is an orthonormal matrix
// such that det(Q) = 1 and Q*Q' = I
//
// WARNING: The rotation matrix is ROW MAJOR
template <typename T> inline
void QuaternionToScaledRotation(const T q[4], T R[3 * 3]);
template <typename T, int row_stride, int col_stride> inline
void QuaternionToScaledRotation(
const T q[4],
const MatrixAdapter<T, row_stride, col_stride>& R);
// Same as above except that the rotation matrix is normalized by the
// Frobenius norm, so that R * R' = I (and det(R) = 1).
//
// WARNING: The rotation matrix is ROW MAJOR
template <typename T> inline
void QuaternionToRotation(const T q[4], T R[3 * 3]);
template <typename T, int row_stride, int col_stride> inline
void QuaternionToRotation(
const T q[4],
const MatrixAdapter<T, row_stride, col_stride>& R);
// Rotates a point pt by a quaternion q:
//
// result = R(q) * pt
//
// Assumes the quaternion is unit norm. This assumption allows us to
// write the transform as (something)*pt + pt, as is clear from the
// formula below. If you pass in a quaternion with |q|^2 = 2 then you
// WILL NOT get back 2 times the result you get for a unit quaternion.
template <typename T> inline
void UnitQuaternionRotatePoint(const T q[4], const T pt[3], T result[3]);
// With this function you do not need to assume that q has unit norm.
// It does assume that the norm is non-zero.
template <typename T> inline
void QuaternionRotatePoint(const T q[4], const T pt[3], T result[3]);
// zw = z * w, where * is the Quaternion product between 4 vectors.
template<typename T> inline
void QuaternionProduct(const T z[4], const T w[4], T zw[4]);
// xy = x cross y;
template<typename T> inline
void CrossProduct(const T x[3], const T y[3], T x_cross_y[3]);
template<typename T> inline
T DotProduct(const T x[3], const T y[3]);
// y = R(angle_axis) * x;
template<typename T> inline
void AngleAxisRotatePoint(const T angle_axis[3], const T pt[3], T result[3]);
// --- IMPLEMENTATION
template<typename T, int row_stride, int col_stride>
struct MatrixAdapter {
T* pointer_;
explicit MatrixAdapter(T* pointer)
: pointer_(pointer)
{}
T& operator()(int r, int c) const {
return pointer_[r * row_stride + c * col_stride];
}
};
template <typename T>
MatrixAdapter<T, 1, 3> ColumnMajorAdapter3x3(T* pointer) {
return MatrixAdapter<T, 1, 3>(pointer);
}
template <typename T>
MatrixAdapter<T, 3, 1> RowMajorAdapter3x3(T* pointer) {
return MatrixAdapter<T, 3, 1>(pointer);
}
template<typename T>
inline void AngleAxisToQuaternion(const T* angle_axis, T* quaternion) {
const T& a0 = angle_axis[0];
const T& a1 = angle_axis[1];
const T& a2 = angle_axis[2];
const T theta_squared = a0 * a0 + a1 * a1 + a2 * a2;
// For points not at the origin, the full conversion is numerically stable.
if (theta_squared > T(0.0)) {
const T theta = sqrt(theta_squared);
const T half_theta = theta * T(0.5);
const T k = sin(half_theta) / theta;
quaternion[0] = cos(half_theta);
quaternion[1] = a0 * k;
quaternion[2] = a1 * k;
quaternion[3] = a2 * k;
} else {
// At the origin, sqrt() will produce NaN in the derivative since
// the argument is zero. By approximating with a Taylor series,
// and truncating at one term, the value and first derivatives will be
// computed correctly when Jets are used.
const T k(0.5);
quaternion[0] = T(1.0);
quaternion[1] = a0 * k;
quaternion[2] = a1 * k;
quaternion[3] = a2 * k;
}
}
template<typename T>
inline void QuaternionToAngleAxis(const T* quaternion, T* angle_axis) {
const T& q1 = quaternion[1];
const T& q2 = quaternion[2];
const T& q3 = quaternion[3];
const T sin_squared_theta = q1 * q1 + q2 * q2 + q3 * q3;
// For quaternions representing non-zero rotation, the conversion
// is numerically stable.
if (sin_squared_theta > T(0.0)) {
const T sin_theta = sqrt(sin_squared_theta);
const T& cos_theta = quaternion[0];
// If cos_theta is negative, theta is greater than pi/2, which
// means that angle for the angle_axis vector which is 2 * theta
// would be greater than pi.
//
// While this will result in the correct rotation, it does not
// result in a normalized angle-axis vector.
//
// In that case we observe that 2 * theta ~ 2 * theta - 2 * pi,
// which is equivalent saying
//
// theta - pi = atan(sin(theta - pi), cos(theta - pi))
// = atan(-sin(theta), -cos(theta))
//
const T two_theta =
T(2.0) * ((cos_theta < 0.0)
? atan2(-sin_theta, -cos_theta)
: atan2(sin_theta, cos_theta));
const T k = two_theta / sin_theta;
angle_axis[0] = q1 * k;
angle_axis[1] = q2 * k;
angle_axis[2] = q3 * k;
} else {
// For zero rotation, sqrt() will produce NaN in the derivative since
// the argument is zero. By approximating with a Taylor series,
// and truncating at one term, the value and first derivatives will be
// computed correctly when Jets are used.
const T k(2.0);
angle_axis[0] = q1 * k;
angle_axis[1] = q2 * k;
angle_axis[2] = q3 * k;
}
}
template <typename T>
void RotationMatrixToQuaternion(const T* R, T* angle_axis) {
RotationMatrixToQuaternion(ColumnMajorAdapter3x3(R), angle_axis);
}
// This algorithm comes from "Quaternion Calculus and Fast Animation",
// Ken Shoemake, 1987 SIGGRAPH course notes
template <typename T, int row_stride, int col_stride>
void RotationMatrixToQuaternion(
const MatrixAdapter<const T, row_stride, col_stride>& R,
T* quaternion) {
const T trace = R(0, 0) + R(1, 1) + R(2, 2);
if (trace >= 0.0) {
T t = sqrt(trace + T(1.0));
quaternion[0] = T(0.5) * t;
t = T(0.5) / t;
quaternion[1] = (R(2, 1) - R(1, 2)) * t;
quaternion[2] = (R(0, 2) - R(2, 0)) * t;
quaternion[3] = (R(1, 0) - R(0, 1)) * t;
} else {
int i = 0;
if (R(1, 1) > R(0, 0)) {
i = 1;
}
if (R(2, 2) > R(i, i)) {
i = 2;
}
const int j = (i + 1) % 3;
const int k = (j + 1) % 3;
T t = sqrt(R(i, i) - R(j, j) - R(k, k) + T(1.0));
quaternion[i + 1] = T(0.5) * t;
t = T(0.5) / t;
quaternion[0] = (R(k, j) - R(j, k)) * t;
quaternion[j + 1] = (R(j, i) + R(i, j)) * t;
quaternion[k + 1] = (R(k, i) + R(i, k)) * t;
}
}
// The conversion of a rotation matrix to the angle-axis form is
// numerically problematic when then rotation angle is close to zero
// or to Pi. The following implementation detects when these two cases
// occurs and deals with them by taking code paths that are guaranteed
// to not perform division by a small number.
template <typename T>
inline void RotationMatrixToAngleAxis(const T* R, T* angle_axis) {
RotationMatrixToAngleAxis(ColumnMajorAdapter3x3(R), angle_axis);
}
template <typename T, int row_stride, int col_stride>
void RotationMatrixToAngleAxis(
const MatrixAdapter<const T, row_stride, col_stride>& R,
T* angle_axis) {
T quaternion[4];
RotationMatrixToQuaternion(R, quaternion);
QuaternionToAngleAxis(quaternion, angle_axis);
return;
}
template <typename T>
inline void AngleAxisToRotationMatrix(const T* angle_axis, T* R) {
AngleAxisToRotationMatrix(angle_axis, ColumnMajorAdapter3x3(R));
}
template <typename T, int row_stride, int col_stride>
void AngleAxisToRotationMatrix(
const T* angle_axis,
const MatrixAdapter<T, row_stride, col_stride>& R) {
static const T kOne = T(1.0);
const T theta2 = DotProduct(angle_axis, angle_axis);
if (theta2 > T(std::numeric_limits<double>::epsilon())) {
// We want to be careful to only evaluate the square root if the
// norm of the angle_axis vector is greater than zero. Otherwise
// we get a division by zero.
const T theta = sqrt(theta2);
const T wx = angle_axis[0] / theta;
const T wy = angle_axis[1] / theta;
const T wz = angle_axis[2] / theta;
const T costheta = cos(theta);
const T sintheta = sin(theta);
R(0, 0) = costheta + wx*wx*(kOne - costheta);
R(1, 0) = wz*sintheta + wx*wy*(kOne - costheta);
R(2, 0) = -wy*sintheta + wx*wz*(kOne - costheta);
R(0, 1) = wx*wy*(kOne - costheta) - wz*sintheta;
R(1, 1) = costheta + wy*wy*(kOne - costheta);
R(2, 1) = wx*sintheta + wy*wz*(kOne - costheta);
R(0, 2) = wy*sintheta + wx*wz*(kOne - costheta);
R(1, 2) = -wx*sintheta + wy*wz*(kOne - costheta);
R(2, 2) = costheta + wz*wz*(kOne - costheta);
} else {
// Near zero, we switch to using the first order Taylor expansion.
R(0, 0) = kOne;
R(1, 0) = angle_axis[2];
R(2, 0) = -angle_axis[1];
R(0, 1) = -angle_axis[2];
R(1, 1) = kOne;
R(2, 1) = angle_axis[0];
R(0, 2) = angle_axis[1];
R(1, 2) = -angle_axis[0];
R(2, 2) = kOne;
}
}
template <typename T>
inline void EulerAnglesToRotationMatrix(const T* euler,
const int row_stride_parameter,
T* R) {
CHECK_EQ(row_stride_parameter, 3);
EulerAnglesToRotationMatrix(euler, RowMajorAdapter3x3(R));
}
template <typename T, int row_stride, int col_stride>
void EulerAnglesToRotationMatrix(
const T* euler,
const MatrixAdapter<T, row_stride, col_stride>& R) {
const double kPi = 3.14159265358979323846;
const T degrees_to_radians(kPi / 180.0);
const T pitch(euler[0] * degrees_to_radians);
const T roll(euler[1] * degrees_to_radians);
const T yaw(euler[2] * degrees_to_radians);
const T c1 = cos(yaw);
const T s1 = sin(yaw);
const T c2 = cos(roll);
const T s2 = sin(roll);
const T c3 = cos(pitch);
const T s3 = sin(pitch);
R(0, 0) = c1*c2;
R(0, 1) = -s1*c3 + c1*s2*s3;
R(0, 2) = s1*s3 + c1*s2*c3;
R(1, 0) = s1*c2;
R(1, 1) = c1*c3 + s1*s2*s3;
R(1, 2) = -c1*s3 + s1*s2*c3;
R(2, 0) = -s2;
R(2, 1) = c2*s3;
R(2, 2) = c2*c3;
}
template <typename T> inline
void QuaternionToScaledRotation(const T q[4], T R[3 * 3]) {
QuaternionToScaledRotation(q, RowMajorAdapter3x3(R));
}
template <typename T, int row_stride, int col_stride> inline
void QuaternionToScaledRotation(
const T q[4],
const MatrixAdapter<T, row_stride, col_stride>& R) {
// Make convenient names for elements of q.
T a = q[0];
T b = q[1];
T c = q[2];
T d = q[3];
// This is not to eliminate common sub-expression, but to
// make the lines shorter so that they fit in 80 columns!
T aa = a * a;
T ab = a * b;
T ac = a * c;
T ad = a * d;
T bb = b * b;
T bc = b * c;
T bd = b * d;
T cc = c * c;
T cd = c * d;
T dd = d * d;
R(0, 0) = aa + bb - cc - dd; R(0, 1) = T(2) * (bc - ad); R(0, 2) = T(2) * (ac + bd); // NOLINT
R(1, 0) = T(2) * (ad + bc); R(1, 1) = aa - bb + cc - dd; R(1, 2) = T(2) * (cd - ab); // NOLINT
R(2, 0) = T(2) * (bd - ac); R(2, 1) = T(2) * (ab + cd); R(2, 2) = aa - bb - cc + dd; // NOLINT
}
template <typename T> inline
void QuaternionToRotation(const T q[4], T R[3 * 3]) {
QuaternionToRotation(q, RowMajorAdapter3x3(R));
}
template <typename T, int row_stride, int col_stride> inline
void QuaternionToRotation(const T q[4],
const MatrixAdapter<T, row_stride, col_stride>& R) {
QuaternionToScaledRotation(q, R);
T normalizer = q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3];
CHECK_NE(normalizer, T(0));
normalizer = T(1) / normalizer;
for (int i = 0; i < 3; ++i) {
for (int j = 0; j < 3; ++j) {
R(i, j) *= normalizer;
}
}
}
template <typename T> inline
void UnitQuaternionRotatePoint(const T q[4], const T pt[3], T result[3]) {
const T t2 = q[0] * q[1];
const T t3 = q[0] * q[2];
const T t4 = q[0] * q[3];
const T t5 = -q[1] * q[1];
const T t6 = q[1] * q[2];
const T t7 = q[1] * q[3];
const T t8 = -q[2] * q[2];
const T t9 = q[2] * q[3];
const T t1 = -q[3] * q[3];
result[0] = T(2) * ((t8 + t1) * pt[0] + (t6 - t4) * pt[1] + (t3 + t7) * pt[2]) + pt[0]; // NOLINT
result[1] = T(2) * ((t4 + t6) * pt[0] + (t5 + t1) * pt[1] + (t9 - t2) * pt[2]) + pt[1]; // NOLINT
result[2] = T(2) * ((t7 - t3) * pt[0] + (t2 + t9) * pt[1] + (t5 + t8) * pt[2]) + pt[2]; // NOLINT
}
template <typename T> inline
void QuaternionRotatePoint(const T q[4], const T pt[3], T result[3]) {
// 'scale' is 1 / norm(q).
const T scale = T(1) / sqrt(q[0] * q[0] +
q[1] * q[1] +
q[2] * q[2] +
q[3] * q[3]);
// Make unit-norm version of q.
const T unit[4] = {
scale * q[0],
scale * q[1],
scale * q[2],
scale * q[3],
};
UnitQuaternionRotatePoint(unit, pt, result);
}
template<typename T> inline
void QuaternionProduct(const T z[4], const T w[4], T zw[4]) {
zw[0] = z[0] * w[0] - z[1] * w[1] - z[2] * w[2] - z[3] * w[3];
zw[1] = z[0] * w[1] + z[1] * w[0] + z[2] * w[3] - z[3] * w[2];
zw[2] = z[0] * w[2] - z[1] * w[3] + z[2] * w[0] + z[3] * w[1];
zw[3] = z[0] * w[3] + z[1] * w[2] - z[2] * w[1] + z[3] * w[0];
}
// xy = x cross y;
template<typename T> inline
void CrossProduct(const T x[3], const T y[3], T x_cross_y[3]) {
x_cross_y[0] = x[1] * y[2] - x[2] * y[1];
x_cross_y[1] = x[2] * y[0] - x[0] * y[2];
x_cross_y[2] = x[0] * y[1] - x[1] * y[0];
}
template<typename T> inline
T DotProduct(const T x[3], const T y[3]) {
return (x[0] * y[0] + x[1] * y[1] + x[2] * y[2]);
}
template<typename T> inline
void AngleAxisRotatePoint(const T angle_axis[3], const T pt[3], T result[3]) {
const T theta2 = DotProduct(angle_axis, angle_axis);
if (theta2 > T(std::numeric_limits<double>::epsilon())) {
// Away from zero, use the rodriguez formula
//
// result = pt costheta +
// (w x pt) * sintheta +
// w (w . pt) (1 - costheta)
//
// We want to be careful to only evaluate the square root if the
// norm of the angle_axis vector is greater than zero. Otherwise
// we get a division by zero.
//
const T theta = sqrt(theta2);
const T costheta = cos(theta);
const T sintheta = sin(theta);
const T theta_inverse = 1.0 / theta;
const T w[3] = { angle_axis[0] * theta_inverse,
angle_axis[1] * theta_inverse,
angle_axis[2] * theta_inverse };
// Explicitly inlined evaluation of the cross product for
// performance reasons.
const T w_cross_pt[3] = { w[1] * pt[2] - w[2] * pt[1],
w[2] * pt[0] - w[0] * pt[2],
w[0] * pt[1] - w[1] * pt[0] };
const T tmp =
(w[0] * pt[0] + w[1] * pt[1] + w[2] * pt[2]) * (T(1.0) - costheta);
result[0] = pt[0] * costheta + w_cross_pt[0] * sintheta + w[0] * tmp;
result[1] = pt[1] * costheta + w_cross_pt[1] * sintheta + w[1] * tmp;
result[2] = pt[2] * costheta + w_cross_pt[2] * sintheta + w[2] * tmp;
} else {
// Near zero, the first order Taylor approximation of the rotation
// matrix R corresponding to a vector w and angle w is
//
// R = I + hat(w) * sin(theta)
//
// But sintheta ~ theta and theta * w = angle_axis, which gives us
//
// R = I + hat(w)
//
// and actually performing multiplication with the point pt, gives us
// R * pt = pt + w x pt.
//
// Switching to the Taylor expansion near zero provides meaningful
// derivatives when evaluated using Jets.
//
// Explicitly inlined evaluation of the cross product for
// performance reasons.
const T w_cross_pt[3] = { angle_axis[1] * pt[2] - angle_axis[2] * pt[1],
angle_axis[2] * pt[0] - angle_axis[0] * pt[2],
angle_axis[0] * pt[1] - angle_axis[1] * pt[0] };
result[0] = pt[0] + w_cross_pt[0];
result[1] = pt[1] + w_cross_pt[1];
result[2] = pt[2] + w_cross_pt[2];
}
}
} // namespace ceres
#endif // CERES_PUBLIC_ROTATION_H_

View File

@ -0,0 +1,96 @@
// 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)
//
// A convenience class for cost functions which are statically sized.
// Compared to the dynamically-sized base class, this reduces boilerplate.
//
// The kNumResiduals template parameter can be a constant such as 2 or 5, or it
// can be ceres::DYNAMIC. If kNumResiduals is ceres::DYNAMIC, then subclasses
// are responsible for calling set_num_residuals() at runtime.
#ifndef CERES_PUBLIC_SIZED_COST_FUNCTION_H_
#define CERES_PUBLIC_SIZED_COST_FUNCTION_H_
#include "ceres/types.h"
#include "ceres/cost_function.h"
#include "glog/logging.h"
namespace ceres {
template<int kNumResiduals,
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>
class SizedCostFunction : public CostFunction {
public:
SizedCostFunction() {
CHECK(kNumResiduals > 0 || kNumResiduals == DYNAMIC)
<< "Cost functions must have at least one residual block.";
// This block breaks the 80 column rule to keep it somewhat readable.
CHECK((!N1 && !N2 && !N3 && !N4 && !N5 && !N6 && !N7 && !N8 && !N9) ||
((N1 > 0) && !N2 && !N3 && !N4 && !N5 && !N6 && !N7 && !N8 && !N9) ||
((N1 > 0) && (N2 > 0) && !N3 && !N4 && !N5 && !N6 && !N7 && !N8 && !N9) || // NOLINT
((N1 > 0) && (N2 > 0) && (N3 > 0) && !N4 && !N5 && !N6 && !N7 && !N8 && !N9) || // NOLINT
((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && !N5 && !N6 && !N7 && !N8 && !N9) || // NOLINT
((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && (N5 > 0) && !N6 && !N7 && !N8 && !N9) || // NOLINT
((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && (N5 > 0) && (N6 > 0) && !N7 && !N8 && !N9) || // NOLINT
((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && (N5 > 0) && (N6 > 0) && (N7 > 0) && !N8 && !N9) || // NOLINT
((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && (N5 > 0) && (N6 > 0) && (N7 > 0) && (N8 > 0) && !N9) || // NOLINT
((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && (N5 > 0) && (N6 > 0) && (N7 > 0) && (N8 > 0) && (N9 > 0))) // NOLINT
<< "Zero block cannot precede a non-zero block. Block sizes are "
<< "(ignore trailing 0s): " << N0 << ", " << N1 << ", " << N2 << ", "
<< N3 << ", " << N4 << ", " << N5 << ", " << N6 << ", " << N7 << ", "
<< N8 << ", " << N9;
set_num_residuals(kNumResiduals);
#define CERES_ADD_PARAMETER_BLOCK(N) \
if (N) mutable_parameter_block_sizes()->push_back(N);
CERES_ADD_PARAMETER_BLOCK(N0);
CERES_ADD_PARAMETER_BLOCK(N1);
CERES_ADD_PARAMETER_BLOCK(N2);
CERES_ADD_PARAMETER_BLOCK(N3);
CERES_ADD_PARAMETER_BLOCK(N4);
CERES_ADD_PARAMETER_BLOCK(N5);
CERES_ADD_PARAMETER_BLOCK(N6);
CERES_ADD_PARAMETER_BLOCK(N7);
CERES_ADD_PARAMETER_BLOCK(N8);
CERES_ADD_PARAMETER_BLOCK(N9);
#undef CERES_ADD_PARAMETER_BLOCK
}
virtual ~SizedCostFunction() { }
// Subclasses must implement Evaluate().
};
} // namespace ceres
#endif // CERES_PUBLIC_SIZED_COST_FUNCTION_H_

1028
external/Ceres-1.11/include/ceres/solver.h vendored Normal file

File diff suppressed because it is too large Load Diff

View File

@ -0,0 +1,508 @@
// 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)
//
// Enums and other top level class definitions.
//
// Note: internal/types.cc defines stringification routines for some
// of these enums. Please update those routines if you extend or
// remove enums from here.
#ifndef CERES_PUBLIC_TYPES_H_
#define CERES_PUBLIC_TYPES_H_
#include <string>
#include "ceres/internal/port.h"
#include "ceres/internal/disable_warnings.h"
namespace ceres {
// Basic integer types. These typedefs are in the Ceres namespace to avoid
// conflicts with other packages having similar typedefs.
typedef int int32;
// Argument type used in interfaces that can optionally take ownership
// of a passed in argument. If TAKE_OWNERSHIP is passed, the called
// object takes ownership of the pointer argument, and will call
// delete on it upon completion.
enum Ownership {
DO_NOT_TAKE_OWNERSHIP,
TAKE_OWNERSHIP
};
// TODO(keir): Considerably expand the explanations of each solver type.
enum LinearSolverType {
// These solvers are for general rectangular systems formed from the
// normal equations A'A x = A'b. They are direct solvers and do not
// assume any special problem structure.
// Solve the normal equations using a dense Cholesky solver; based
// on Eigen.
DENSE_NORMAL_CHOLESKY,
// Solve the normal equations using a dense QR solver; based on
// Eigen.
DENSE_QR,
// Solve the normal equations using a sparse cholesky solver; requires
// SuiteSparse or CXSparse.
SPARSE_NORMAL_CHOLESKY,
// Specialized solvers, specific to problems with a generalized
// bi-partitite structure.
// Solves the reduced linear system using a dense Cholesky solver;
// based on Eigen.
DENSE_SCHUR,
// Solves the reduced linear system using a sparse Cholesky solver;
// based on CHOLMOD.
SPARSE_SCHUR,
// Solves the reduced linear system using Conjugate Gradients, based
// on a new Ceres implementation. Suitable for large scale
// problems.
ITERATIVE_SCHUR,
// Conjugate gradients on the normal equations.
CGNR
};
enum PreconditionerType {
// Trivial preconditioner - the identity matrix.
IDENTITY,
// Block diagonal of the Gauss-Newton Hessian.
JACOBI,
// Note: The following three preconditioners can only be used with
// the ITERATIVE_SCHUR solver. They are well suited for Structure
// from Motion problems.
// Block diagonal of the Schur complement. This preconditioner may
// only be used with the ITERATIVE_SCHUR solver.
SCHUR_JACOBI,
// Visibility clustering based preconditioners.
//
// The following two preconditioners use the visibility structure of
// the scene to determine the sparsity structure of the
// preconditioner. This is done using a clustering algorithm. The
// available visibility clustering algorithms are described below.
//
// Note: Requires SuiteSparse.
CLUSTER_JACOBI,
CLUSTER_TRIDIAGONAL
};
enum VisibilityClusteringType {
// Canonical views algorithm as described in
//
// "Scene Summarization for Online Image Collections", Ian Simon, Noah
// Snavely, Steven M. Seitz, ICCV 2007.
//
// This clustering algorithm can be quite slow, but gives high
// quality clusters. The original visibility based clustering paper
// used this algorithm.
CANONICAL_VIEWS,
// The classic single linkage algorithm. It is extremely fast as
// compared to CANONICAL_VIEWS, but can give slightly poorer
// results. For problems with large number of cameras though, this
// is generally a pretty good option.
//
// If you are using SCHUR_JACOBI preconditioner and have SuiteSparse
// available, CLUSTER_JACOBI and CLUSTER_TRIDIAGONAL in combination
// with the SINGLE_LINKAGE algorithm will generally give better
// results.
SINGLE_LINKAGE
};
enum SparseLinearAlgebraLibraryType {
// High performance sparse Cholesky factorization and approximate
// minimum degree ordering.
SUITE_SPARSE,
// A lightweight replacment for SuiteSparse, which does not require
// a LAPACK/BLAS implementation. Consequently, its performance is
// also a bit lower than SuiteSparse.
CX_SPARSE,
// Eigen's sparse linear algebra routines. In particular Ceres uses
// the Simplicial LDLT routines.
EIGEN_SPARSE,
// No sparse linear solver should be used. This does not necessarily
// imply that Ceres was built without any sparse library, although that
// is the likely use case, merely that one should not be used.
NO_SPARSE
};
enum DenseLinearAlgebraLibraryType {
EIGEN,
LAPACK
};
// Logging options
// The options get progressively noisier.
enum LoggingType {
SILENT,
PER_MINIMIZER_ITERATION
};
enum MinimizerType {
LINE_SEARCH,
TRUST_REGION
};
enum LineSearchDirectionType {
// Negative of the gradient.
STEEPEST_DESCENT,
// A generalization of the Conjugate Gradient method to non-linear
// functions. The generalization can be performed in a number of
// different ways, resulting in a variety of search directions. The
// precise choice of the non-linear conjugate gradient algorithm
// used is determined by NonlinerConjuateGradientType.
NONLINEAR_CONJUGATE_GRADIENT,
// BFGS, and it's limited memory approximation L-BFGS, are quasi-Newton
// algorithms that approximate the Hessian matrix by iteratively refining
// an initial estimate with rank-one updates using the gradient at each
// iteration. They are a generalisation of the Secant method and satisfy
// the Secant equation. The Secant equation has an infinium of solutions
// in multiple dimensions, as there are N*(N+1)/2 degrees of freedom in a
// symmetric matrix but only N conditions are specified by the Secant
// equation. The requirement that the Hessian approximation be positive
// definite imposes another N additional constraints, but that still leaves
// remaining degrees-of-freedom. (L)BFGS methods uniquely deteremine the
// approximate Hessian by imposing the additional constraints that the
// approximation at the next iteration must be the 'closest' to the current
// approximation (the nature of how this proximity is measured is actually
// the defining difference between a family of quasi-Newton methods including
// (L)BFGS & DFP). (L)BFGS is currently regarded as being the best known
// general quasi-Newton method.
//
// The principal difference between BFGS and L-BFGS is that whilst BFGS
// maintains a full, dense approximation to the (inverse) Hessian, L-BFGS
// maintains only a window of the last M observations of the parameters and
// gradients. Using this observation history, the calculation of the next
// search direction can be computed without requiring the construction of the
// full dense inverse Hessian approximation. This is particularly important
// for problems with a large number of parameters, where storage of an N-by-N
// matrix in memory would be prohibitive.
//
// For more details on BFGS see:
//
// Broyden, C.G., "The Convergence of a Class of Double-rank Minimization
// Algorithms,"; J. Inst. Maths. Applics., Vol. 6, pp 7690, 1970.
//
// Fletcher, R., "A New Approach to Variable Metric Algorithms,"
// Computer Journal, Vol. 13, pp 317322, 1970.
//
// Goldfarb, D., "A Family of Variable Metric Updates Derived by Variational
// Means," Mathematics of Computing, Vol. 24, pp 2326, 1970.
//
// Shanno, D.F., "Conditioning of Quasi-Newton Methods for Function
// Minimization," Mathematics of Computing, Vol. 24, pp 647656, 1970.
//
// For more details on L-BFGS see:
//
// Nocedal, J. (1980). "Updating Quasi-Newton Matrices with Limited
// Storage". Mathematics of Computation 35 (151): 773782.
//
// Byrd, R. H.; Nocedal, J.; Schnabel, R. B. (1994).
// "Representations of Quasi-Newton Matrices and their use in
// Limited Memory Methods". Mathematical Programming 63 (4):
// 129156.
//
// A general reference for both methods:
//
// Nocedal J., Wright S., Numerical Optimization, 2nd Ed. Springer, 1999.
LBFGS,
BFGS,
};
// Nonliner conjugate gradient methods are a generalization of the
// method of Conjugate Gradients for linear systems. The
// generalization can be carried out in a number of different ways
// leading to number of different rules for computing the search
// direction. Ceres provides a number of different variants. For more
// details see Numerical Optimization by Nocedal & Wright.
enum NonlinearConjugateGradientType {
FLETCHER_REEVES,
POLAK_RIBIERE,
HESTENES_STIEFEL,
};
enum LineSearchType {
// Backtracking line search with polynomial interpolation or
// bisection.
ARMIJO,
WOLFE,
};
// Ceres supports different strategies for computing the trust region
// step.
enum TrustRegionStrategyType {
// The default trust region strategy is to use the step computation
// used in the Levenberg-Marquardt algorithm. For more details see
// levenberg_marquardt_strategy.h
LEVENBERG_MARQUARDT,
// Powell's dogleg algorithm interpolates between the Cauchy point
// and the Gauss-Newton step. It is particularly useful if the
// LEVENBERG_MARQUARDT algorithm is making a large number of
// unsuccessful steps. For more details see dogleg_strategy.h.
//
// NOTES:
//
// 1. This strategy has not been experimented with or tested as
// extensively as LEVENBERG_MARQUARDT, and therefore it should be
// considered EXPERIMENTAL for now.
//
// 2. For now this strategy should only be used with exact
// factorization based linear solvers, i.e., SPARSE_SCHUR,
// DENSE_SCHUR, DENSE_QR and SPARSE_NORMAL_CHOLESKY.
DOGLEG
};
// Ceres supports two different dogleg strategies.
// The "traditional" dogleg method by Powell and the
// "subspace" method described in
// R. H. Byrd, R. B. Schnabel, and G. A. Shultz,
// "Approximate solution of the trust region problem by minimization
// over two-dimensional subspaces", Mathematical Programming,
// 40 (1988), pp. 247--263
enum DoglegType {
// The traditional approach constructs a dogleg path
// consisting of two line segments and finds the furthest
// point on that path that is still inside the trust region.
TRADITIONAL_DOGLEG,
// The subspace approach finds the exact minimum of the model
// constrained to the subspace spanned by the dogleg path.
SUBSPACE_DOGLEG
};
enum TerminationType {
// Minimizer terminated because one of the convergence criterion set
// by the user was satisfied.
//
// 1. (new_cost - old_cost) < function_tolerance * old_cost;
// 2. max_i |gradient_i| < gradient_tolerance
// 3. |step|_2 <= parameter_tolerance * ( |x|_2 + parameter_tolerance)
//
// The user's parameter blocks will be updated with the solution.
CONVERGENCE,
// The solver ran for maximum number of iterations or maximum amount
// of time specified by the user, but none of the convergence
// criterion specified by the user were met. The user's parameter
// blocks will be updated with the solution found so far.
NO_CONVERGENCE,
// The minimizer terminated because of an error. The user's
// parameter blocks will not be updated.
FAILURE,
// Using an IterationCallback object, user code can control the
// minimizer. The following enums indicate that the user code was
// responsible for termination.
//
// Minimizer terminated successfully because a user
// IterationCallback returned SOLVER_TERMINATE_SUCCESSFULLY.
//
// The user's parameter blocks will be updated with the solution.
USER_SUCCESS,
// Minimizer terminated because because a user IterationCallback
// returned SOLVER_ABORT.
//
// The user's parameter blocks will not be updated.
USER_FAILURE
};
// Enums used by the IterationCallback instances to indicate to the
// solver whether it should continue solving, the user detected an
// error or the solution is good enough and the solver should
// terminate.
enum CallbackReturnType {
// Continue solving to next iteration.
SOLVER_CONTINUE,
// Terminate solver, and do not update the parameter blocks upon
// return. Unless the user has set
// Solver:Options:::update_state_every_iteration, in which case the
// state would have been updated every iteration
// anyways. Solver::Summary::termination_type is set to USER_ABORT.
SOLVER_ABORT,
// Terminate solver, update state and
// return. Solver::Summary::termination_type is set to USER_SUCCESS.
SOLVER_TERMINATE_SUCCESSFULLY
};
// The format in which linear least squares problems should be logged
// when Solver::Options::lsqp_iterations_to_dump is non-empty.
enum DumpFormatType {
// Print the linear least squares problem in a human readable format
// to stderr. The Jacobian is printed as a dense matrix. The vectors
// D, x and f are printed as dense vectors. This should only be used
// for small problems.
CONSOLE,
// Write out the linear least squares problem to the directory
// pointed to by Solver::Options::lsqp_dump_directory as text files
// which can be read into MATLAB/Octave. The Jacobian is dumped as a
// text file containing (i,j,s) triplets, the vectors D, x and f are
// dumped as text files containing a list of their values.
//
// A MATLAB/octave script called lm_iteration_???.m is also output,
// which can be used to parse and load the problem into memory.
TEXTFILE
};
// 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
};
// The differentiation method used to compute numerical derivatives in
// NumericDiffCostFunction and DynamicNumericDiffCostFunction.
enum NumericDiffMethodType {
// Compute central finite difference: f'(x) ~ (f(x+h) - f(x-h)) / 2h.
CENTRAL,
// Compute forward finite difference: f'(x) ~ (f(x+h) - f(x)) / h.
FORWARD,
// Adaptive numerical differentiation using Ridders' method. Provides more
// accurate and robust derivatives at the expense of additional cost
// function evaluations.
RIDDERS
};
enum LineSearchInterpolationType {
BISECTION,
QUADRATIC,
CUBIC
};
enum CovarianceAlgorithmType {
DENSE_SVD,
SUITE_SPARSE_QR,
EIGEN_SPARSE_QR
};
CERES_EXPORT const char* LinearSolverTypeToString(
LinearSolverType type);
CERES_EXPORT bool StringToLinearSolverType(std::string value,
LinearSolverType* type);
CERES_EXPORT const char* PreconditionerTypeToString(PreconditionerType type);
CERES_EXPORT bool StringToPreconditionerType(std::string value,
PreconditionerType* type);
CERES_EXPORT const char* VisibilityClusteringTypeToString(
VisibilityClusteringType type);
CERES_EXPORT bool StringToVisibilityClusteringType(std::string value,
VisibilityClusteringType* type);
CERES_EXPORT const char* SparseLinearAlgebraLibraryTypeToString(
SparseLinearAlgebraLibraryType type);
CERES_EXPORT bool StringToSparseLinearAlgebraLibraryType(
std::string value,
SparseLinearAlgebraLibraryType* type);
CERES_EXPORT const char* DenseLinearAlgebraLibraryTypeToString(
DenseLinearAlgebraLibraryType type);
CERES_EXPORT bool StringToDenseLinearAlgebraLibraryType(
std::string value,
DenseLinearAlgebraLibraryType* type);
CERES_EXPORT const char* TrustRegionStrategyTypeToString(
TrustRegionStrategyType type);
CERES_EXPORT bool StringToTrustRegionStrategyType(std::string value,
TrustRegionStrategyType* type);
CERES_EXPORT const char* DoglegTypeToString(DoglegType type);
CERES_EXPORT bool StringToDoglegType(std::string value, DoglegType* type);
CERES_EXPORT const char* MinimizerTypeToString(MinimizerType type);
CERES_EXPORT bool StringToMinimizerType(std::string value, MinimizerType* type);
CERES_EXPORT const char* LineSearchDirectionTypeToString(
LineSearchDirectionType type);
CERES_EXPORT bool StringToLineSearchDirectionType(std::string value,
LineSearchDirectionType* type);
CERES_EXPORT const char* LineSearchTypeToString(LineSearchType type);
CERES_EXPORT bool StringToLineSearchType(std::string value, LineSearchType* type);
CERES_EXPORT const char* NonlinearConjugateGradientTypeToString(
NonlinearConjugateGradientType type);
CERES_EXPORT bool StringToNonlinearConjugateGradientType(
std::string value,
NonlinearConjugateGradientType* type);
CERES_EXPORT const char* LineSearchInterpolationTypeToString(
LineSearchInterpolationType type);
CERES_EXPORT bool StringToLineSearchInterpolationType(
std::string value,
LineSearchInterpolationType* type);
CERES_EXPORT const char* CovarianceAlgorithmTypeToString(
CovarianceAlgorithmType type);
CERES_EXPORT bool StringToCovarianceAlgorithmType(
std::string value,
CovarianceAlgorithmType* type);
CERES_EXPORT const char* NumericDiffMethodTypeToString(
NumericDiffMethodType type);
CERES_EXPORT bool StringToNumericDiffMethodType(
std::string value,
NumericDiffMethodType* type);
CERES_EXPORT const char* TerminationTypeToString(TerminationType type);
CERES_EXPORT bool IsSchurType(LinearSolverType type);
CERES_EXPORT bool IsSparseLinearAlgebraLibraryTypeAvailable(
SparseLinearAlgebraLibraryType type);
CERES_EXPORT bool IsDenseLinearAlgebraLibraryTypeAvailable(
DenseLinearAlgebraLibraryType type);
} // namespace ceres
#include "ceres/internal/reenable_warnings.h"
#endif // CERES_PUBLIC_TYPES_H_

View File

@ -0,0 +1,48 @@
// 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: mierle@gmail.com (Keir Mierle)
#ifndef CERES_PUBLIC_VERSION_H_
#define CERES_PUBLIC_VERSION_H_
#define CERES_VERSION_MAJOR 1
#define CERES_VERSION_MINOR 11
#define CERES_VERSION_REVISION 0
// Classic CPP stringifcation; the extra level of indirection allows the
// preprocessor to expand the macro before being converted to a string.
#define CERES_TO_STRING_HELPER(x) #x
#define CERES_TO_STRING(x) CERES_TO_STRING_HELPER(x)
// The Ceres version as a string; for example "1.9.0".
#define CERES_VERSION_STRING CERES_TO_STRING(CERES_VERSION_MAJOR) "." \
CERES_TO_STRING(CERES_VERSION_MINOR) "." \
CERES_TO_STRING(CERES_VERSION_REVISION)
#endif // CERES_PUBLIC_VERSION_H_

BIN
external/Ceres-1.11/lib/ceres-debug.lib vendored Normal file

Binary file not shown.

BIN
external/Ceres-1.11/lib/ceres.lib vendored Normal file

Binary file not shown.

View File

@ -0,0 +1,59 @@
/** dirt-cheap replacement for CComPtr, which is not included in the free windows compilers */
template< class I >
class AutoComPtr
{
public:
AutoComPtr()
: p( 0 )
{}
AutoComPtr( const AutoComPtr< I >& o )
: p( o.p )
{
if ( p )
p->AddRef();
}
AutoComPtr( I* _p )
: p( _p )
{}
~AutoComPtr()
{
if ( p )
p->Release();
}
void Release()
{
if ( p )
p->Release();
p = 0;
}
I* operator->()
{ return p; }
operator I*()
{ return p; }
AutoComPtr< I >& operator=( const AutoComPtr< I >& o )
{
Release();
p = o.p;
if ( p )
p->AddRef();
return *this;
}
template< class O >
HRESULT QueryInterface( AutoComPtr< O >& o )
{ return p->QueryInterface( __uuidof( O ), reinterpret_cast< void** >( &o.p ) ); }
HRESULT CoCreateInstance( REFCLSID rclsid, LPUNKNOWN pUnkOuter = 0, DWORD dwClsContext = CLSCTX_INPROC_SERVER )
{ return ::CoCreateInstance( rclsid, pUnkOuter, dwClsContext, __uuidof( I ), reinterpret_cast< void** >( &p ) ); }
I* p;
};

View File

@ -0,0 +1,647 @@
/*
* Ubitrack - Library for Ubiquitous Tracking
* Copyright 2006, Technische Universitaet Muenchen, and individual
* contributors as indicated by the @authors tag. See the
* copyright.txt in the distribution for a full listing of individual
* contributors.
*
* This 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 software 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 software; if not, write to the Free
* Software Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA
* 02110-1301 USA, or see the FSF site: http://www.fsf.org.
*/
/**
* @ingroup vision_components
* @file
* Reads camera images using DirectShow
*
* @author Daniel Pustka <daniel.pustka@in.tum.de>
*/
#pragma comment(lib,"Strmiids.lib")
#include "DirectShowFrameGrabber.h"
#include <opencv2/highgui.hpp>
namespace Ubitrack { namespace Drivers {
//DirectShowFrameGrabber::DirectShowFrameGrabber( const std::string& sName, boost::shared_ptr< Graph::UTQLSubgraph > subgraph )
DirectShowFrameGrabber::DirectShowFrameGrabber( const std::string& sName )
: //Dataflow::Component( sName )
m_timeOffset( 0 )
, m_divisor( 1 )
, m_desiredWidth( 640 )
, m_desiredHeight( 480 )
, m_desiredName( sName )
, m_cameraExposure( 0 )
, m_cameraExposureAuto( true )
, m_cameraBrightness( 0 )
, m_cameraContrast( 11 )
, m_cameraSaturation( 4 )
, m_cameraSharpness( 3 )
, m_cameraGamma( 150 )
, m_cameraWhitebalance( 4500 )
, m_cameraWhitebalanceAuto( true )
, m_cameraBacklightComp( false )
, m_cameraGain( 34 )
, m_nFrames( 0 )
, m_lastTime( -1e10 )
, m_running(false)
, is_image_arrived_(false)
//, m_syncer( 1.0 )
////, m_undistorter( *subgraph )
//, m_outPort( "Output", *this )
//, m_colorOutPort( "ColorOutput", *this )
//, m_intrinsicsPort( "Intrinsics", *this, boost::bind( &DirectShowFrameGrabber::getIntrinsic, this, _1 ) )
//, m_outPortRAW("OutputRAW", *this)
{
HRESULT hRes = CoInitializeEx( NULL, COINIT_MULTITHREADED );
if ( hRes == RPC_E_CHANGED_MODE )
{
LOG4CPP_WARN( logger, "CoInitializeEx failed with RPC_E_CHANGED_MODE, continuing..." );
}
else if ( FAILED( hRes ) )
{
std::ostringstream os;
os << "Error in CoInitializeEx:" << std::hex << hRes;
UBITRACK_THROW( os.str() );
}
#if 0
subgraph->m_DataflowAttributes.getAttributeData( "timeOffset", m_timeOffset );
subgraph->m_DataflowAttributes.getAttributeData( "divisor", m_divisor );
subgraph->m_DataflowAttributes.getAttributeData( "imageWidth", m_desiredWidth );
subgraph->m_DataflowAttributes.getAttributeData( "imageHeight", m_desiredHeight );
m_desiredDevicePath = subgraph->m_DataflowAttributes.getAttributeString( "devicePath" );
m_desiredName = subgraph->m_DataflowAttributes.getAttributeString( "cameraName" );
if (subgraph->m_DataflowAttributes.hasAttribute( "cameraExposure" ))
subgraph->m_DataflowAttributes.getAttributeData( "cameraExposure", m_cameraExposure );
if (subgraph->m_DataflowAttributes.hasAttribute( "cameraExposureAuto" ))
m_cameraExposureAuto = subgraph->m_DataflowAttributes.getAttributeString( "cameraExposureAuto" ) == "true";
if (subgraph->m_DataflowAttributes.hasAttribute( "cameraBrightness" ))
subgraph->m_DataflowAttributes.getAttributeData( "cameraBrightness", m_cameraBrightness );
if (subgraph->m_DataflowAttributes.hasAttribute( "cameraContrast" ))
subgraph->m_DataflowAttributes.getAttributeData( "cameraContrast", m_cameraContrast );
if (subgraph->m_DataflowAttributes.hasAttribute( "cameraSaturation" ))
subgraph->m_DataflowAttributes.getAttributeData( "cameraSaturation", m_cameraSaturation );
if (subgraph->m_DataflowAttributes.hasAttribute( "cameraSharpness" ))
subgraph->m_DataflowAttributes.getAttributeData( "cameraSharpness", m_cameraSharpness );
if (subgraph->m_DataflowAttributes.hasAttribute( "cameraGamma" ))
subgraph->m_DataflowAttributes.getAttributeData( "cameraGamma", m_cameraGamma );
if (subgraph->m_DataflowAttributes.hasAttribute( "cameraWhitebalance" ))
subgraph->m_DataflowAttributes.getAttributeData( "cameraWhitebalance", m_cameraWhitebalance );
if (subgraph->m_DataflowAttributes.hasAttribute( "cameraWhitebalanceAuto" ))
m_cameraWhitebalanceAuto = subgraph->m_DataflowAttributes.getAttributeString( "cameraWhitebalanceAuto" ) == "true";
if (subgraph->m_DataflowAttributes.hasAttribute( "cameraBacklightComp" ))
m_cameraBacklightComp = subgraph->m_DataflowAttributes.getAttributeString( "cameraBacklightComp" ) == "true";
if (subgraph->m_DataflowAttributes.hasAttribute( "cameraGain" ))
subgraph->m_DataflowAttributes.getAttributeData( "cameraGain", m_cameraGain );
if (subgraph->m_DataflowAttributes.hasAttribute("cameraModelFile")){
std::string cameraModelFile = subgraph->m_DataflowAttributes.getAttributeString("cameraModelFile");
m_undistorter.reset(new Vision::Undistortion(cameraModelFile));
}
else {
std::string intrinsicFile = subgraph->m_DataflowAttributes.getAttributeString("intrinsicMatrixFile");
std::string distortionFile = subgraph->m_DataflowAttributes.getAttributeString("distortionFile");
m_undistorter.reset(new Vision::Undistortion(intrinsicFile, distortionFile));
}
// dynamically generate input ports
for (Graph::UTQLSubgraph::EdgeMap::iterator it = subgraph->m_Edges.begin(); it != subgraph->m_Edges.end(); it++)
{
if (it->second->isInput())
{
if (0 == it->first.compare(0, 15, "InputIntrinsics")) {
m_intrinsicInPort.reset(new Dataflow::PushConsumer< Measurement::CameraIntrinsics >(it->first, *this,
boost::bind(&DirectShowFrameGrabber::newIntrinsicsPush, this, _1)));
}
}
}
#endif
initGraph();
}
//void DirectShowFrameGrabber::newIntrinsicsPush(Measurement::CameraIntrinsics intrinsics) {
// m_undistorter.reset(new Vision::Undistortion(*intrinsics));
//}
DirectShowFrameGrabber::~DirectShowFrameGrabber()
{
if ( m_pMediaControl )
m_pMediaControl->Stop();
CoUninitialize();
m_running = false;
}
void DirectShowFrameGrabber::start()
{
if ( !m_running && m_pMediaControl )
m_pMediaControl->Run();
// Component::start();
m_running = true;
}
void DirectShowFrameGrabber::stop()
{
if ( m_running && m_pMediaControl )
m_pMediaControl->Pause();
// Component::stop();
// stop();
}
void DirectShowFrameGrabber::initGraph()
{
// Create the System Device Enumerator.
AutoComPtr< ICreateDevEnum > pDevEnum;
AutoComPtr< IEnumMoniker > pEnum;
HRESULT hr = pDevEnum.CoCreateInstance( CLSID_SystemDeviceEnum, NULL, CLSCTX_INPROC_SERVER );
if ( SUCCEEDED( hr ) )
// Create an enumerator for the video capture category.
hr = pDevEnum->CreateClassEnumerator( CLSID_VideoInputDeviceCategory, &pEnum.p, 0 );
AutoComPtr< IMoniker > pMoniker;
AutoComPtr< IMoniker > pSelectedMoniker;
std::string sSelectedCamera;
while ( pEnum->Next( 1, &pMoniker.p, NULL ) == S_OK )
{
if ( !pSelectedMoniker )
pSelectedMoniker = pMoniker;
AutoComPtr< IPropertyBag > pPropBag;
hr = pMoniker->BindToStorage( 0, 0, IID_IPropertyBag, (void**)(&pPropBag.p) );
if ( FAILED( hr ) )
{
pMoniker.Release();
continue; // Skip this one, maybe the next one will work.
}
// Find the device of the camera.
VARIANT varDevicePath;
VariantInit( &varDevicePath );
hr = pPropBag->Read( L"DevicePath", &varDevicePath, 0 );
char sDevicePath[ 128 ];
if ( SUCCEEDED( hr ) )
// could be optimized somehow ..
WideCharToMultiByte( CP_ACP, 0, varDevicePath.bstrVal, -1, sDevicePath, 128, 0, 0 );
// Find the description or friendly name.
VARIANT varName;
VariantInit( &varName );
hr = pPropBag->Read( L"Description", &varName, 0 );
if ( FAILED( hr ) )
hr = pPropBag->Read( L"FriendlyName", &varName, 0 );
if ( SUCCEEDED( hr ) )
{
char sName[ 128 ];
WideCharToMultiByte( CP_ACP, 0, varName.bstrVal, -1, sName, 128, 0, 0 );
LOG4CPP_INFO( logger, "Possible capture device: " << &sName[0] << " device path: " << sDevicePath );
// select device based on name
if ( !m_desiredName.empty() && strstr( sName, m_desiredName.c_str() ) )
{
if( m_desiredDevicePath.empty() )
{
sSelectedCamera = sName;
pSelectedMoniker = pMoniker;
break;
}
else if ( strstr( sDevicePath, m_desiredDevicePath.c_str() ) )
{
sSelectedCamera = sName;
pSelectedMoniker = pMoniker;
LOG4CPP_INFO( logger, "Found device with path-identifier: " << m_desiredDevicePath );
break;
}
}
VariantClear( &varName );
}
pMoniker.Release();
}
// check if a capture device was found
if ( !pSelectedMoniker )
UBITRACK_THROW( "No video capture device found" );
LOG4CPP_INFO( logger, "Using camera: " << sSelectedCamera );
// create capture graph
AutoComPtr< IGraphBuilder > pGraph;
AutoComPtr< ICaptureGraphBuilder2 > pBuild;
// Create the Capture Graph Builder.
hr = pBuild.CoCreateInstance( CLSID_CaptureGraphBuilder2, NULL, CLSCTX_INPROC_SERVER );
if ( SUCCEEDED( hr ) )
{
// Create the Filter Graph Manager.
hr = pGraph.CoCreateInstance( CLSID_FilterGraph, 0, CLSCTX_INPROC_SERVER );
if ( SUCCEEDED( hr ) )
// Initialize the Capture Graph Builder.
pBuild->SetFiltergraph( pGraph );
else
UBITRACK_THROW( "Error creating filter graph manager" );
}
else
UBITRACK_THROW( "Error creating capture graph builder" );
// create capture device filter
AutoComPtr< IBaseFilter > pCaptureFilter;
if ( FAILED( pSelectedMoniker->BindToObject( 0, 0, IID_IBaseFilter, (void**)&pCaptureFilter.p ) ) )
UBITRACK_THROW( "Unable to create capture filter" );
if ( FAILED( pGraph->AddFilter( pCaptureFilter, L"Capture" ) ) )
UBITRACK_THROW( "Unable to add capture filter" );
// find output pin for configuration
AutoComPtr< IPin > pPin;
if ( FAILED( pBuild->FindPin( pCaptureFilter, PINDIR_OUTPUT, &PIN_CATEGORY_CAPTURE, &MEDIATYPE_Video, FALSE, 0, &pPin.p ) ) )
UBITRACK_THROW( "Unable to find pin" );
// enumerate media types
AutoComPtr< IAMStreamConfig > pStreamConfig;
if ( FAILED( pPin.QueryInterface< IAMStreamConfig >( pStreamConfig ) ) )
{ LOG4CPP_WARN( logger, "Unable to get IAMStreamConfig interface" ); }
else
{
int iCount, iSize;
pStreamConfig->GetNumberOfCapabilities( &iCount, &iSize );
boost::scoped_array< BYTE > buf( new BYTE[ iSize ] );
bool bSet = false;
for ( int iCap = 0; iCap < iCount; iCap++ )
{
AM_MEDIA_TYPE *pMediaType;
pStreamConfig->GetStreamCaps( iCap, &pMediaType, buf.get() );
if ( pMediaType->majortype != MEDIATYPE_Video || pMediaType->formattype != FORMAT_VideoInfo )
continue;
VIDEOINFOHEADER* pInfo = (VIDEOINFOHEADER*)pMediaType->pbFormat;
LOG4CPP_INFO( logger, "Media type: fps=" << 1e7 / pInfo->AvgTimePerFrame <<
", width=" << pInfo->bmiHeader.biWidth << ", height=" << pInfo->bmiHeader.biHeight <<
", type=" << ( pMediaType->subtype == MEDIASUBTYPE_RGB24 ? "RGB24" : "?" ) );
// set first format with correct size, but prefer RGB24
if ( ( m_desiredWidth <= 0 || pInfo->bmiHeader.biWidth == m_desiredWidth ) &&
( m_desiredHeight <= 0 || pInfo->bmiHeader.biHeight == m_desiredHeight ) &&
( !bSet || pMediaType->subtype == MEDIASUBTYPE_RGB24 ) )
{
pStreamConfig->SetFormat( pMediaType );
if ( bSet )
break;
bSet = true;
}
// TODO: DeleteMediaType
}
}
// create sample grabber filter
AutoComPtr< IBaseFilter > pSampleGrabberFilter;
if ( FAILED( pSampleGrabberFilter.CoCreateInstance( CLSID_SampleGrabber, NULL, CLSCTX_INPROC_SERVER ) ) )
UBITRACK_THROW( "Unable to create sample grabber filter" );
if ( FAILED( pGraph->AddFilter( pSampleGrabberFilter, L"SampleGrab" ) ) )
UBITRACK_THROW( "Unable to add sample grabber filter" );
// configure sample grabber
AutoComPtr< ISampleGrabber > pSampleGrabber;
pSampleGrabberFilter.QueryInterface( pSampleGrabber );
pSampleGrabber->SetOneShot( FALSE );
pSampleGrabber->SetBufferSamples( FALSE );
pSampleGrabber->SetCallback( this, 0 ); // 0 = Use the SampleCB callback method.
// make it picky on media types
AM_MEDIA_TYPE mediaType;
memset( &mediaType, 0, sizeof( mediaType ) );
mediaType.majortype = MEDIATYPE_Video;
mediaType.subtype = MEDIASUBTYPE_RGB24;
pSampleGrabber->SetMediaType( &mediaType );
// null renderer
AutoComPtr< IBaseFilter > pNullRenderer;
if ( FAILED( pNullRenderer.CoCreateInstance( CLSID_NullRenderer, NULL, CLSCTX_INPROC_SERVER ) ) )
UBITRACK_THROW( "Unable to create null renderer filter" );
if ( FAILED( pGraph->AddFilter( pNullRenderer, L"NullRender" ) ) )
UBITRACK_THROW( "Unable to add null renderer filter" );
// connect all filters
hr = pBuild->RenderStream(
&PIN_CATEGORY_CAPTURE, // Connect this pin ...
&MEDIATYPE_Video, // with this media type ...
pCaptureFilter, // on this filter ...
pSampleGrabberFilter, // to the Sample Grabber ...
pNullRenderer ); // ... and finally to the Null Renderer.
if ( FAILED( hr ) )
UBITRACK_THROW( "Unable to render stream" );
// get media type
pSampleGrabber->GetConnectedMediaType( &mediaType );
if ( mediaType.majortype != MEDIATYPE_Video || mediaType.subtype != MEDIASUBTYPE_RGB24 ||
mediaType.formattype != FORMAT_VideoInfo )
UBITRACK_THROW( "Unsupported MEDIATYPE" );
VIDEOINFOHEADER* pVidInfo = (VIDEOINFOHEADER*)mediaType.pbFormat;
m_sampleWidth = pVidInfo->bmiHeader.biWidth;
m_sampleHeight = pVidInfo->bmiHeader.biHeight;
double fps = (1.0 / pVidInfo->AvgTimePerFrame) * 10000000.0;
LOG4CPP_INFO( logger, "Image dimensions: " << m_sampleWidth << "x" << m_sampleHeight << " FPS: " << fps );
// TODO: FreeMediaType( &mediaType );
#ifdef HAVE_DIRECTSHOW
/* additionally control camera parameters infos at:
* http://msdn.microsoft.com/en-us/library/dd318253(v=vs.85).aspx
*/
LOG4CPP_INFO( logger, "Setting additional direct show parameter ");
IAMCameraControl *pCameraControl;
hr = pCaptureFilter->QueryInterface(IID_IAMCameraControl, (void **)&pCameraControl);
if ( SUCCEEDED(hr) ) {
// could check if provided value is within range
//hr = pCameraControl->GetRange(CameraControl_Exposure,
// NULL, // min
// NULL, // max
// NULL, // minstep
// &defaultExposureValue, // default
// NULL); // capflags
int expFlag = CameraControl_Flags_Manual;
if (m_cameraExposureAuto)
expFlag = CameraControl_Flags_Auto;
hr = pCameraControl->Set(CameraControl_Exposure, // property
m_cameraExposure, // value
expFlag);
if (FAILED(hr))
LOG4CPP_ERROR( logger, "Error setting camera exposure property to " << m_cameraExposure);
}
IAMVideoProcAmp *pAMVideoProcAmp;
hr = pCaptureFilter->QueryInterface(IID_IAMVideoProcAmp, (void**)&pAMVideoProcAmp);
if (SUCCEEDED(hr)) {
long Min, Max, Step, Default, Flags, Val;
pAMVideoProcAmp->GetRange(VideoProcAmp_Brightness, &Min, &Max, &Step, &Default, &Flags);
LOG4CPP_INFO(logger, "Possible Settings for VideoProcAmp_Brightness: min=" <<Min << " max="<<Max << " Step=" << Step << " Default="<<Default << " Flags="<<Flags );
pAMVideoProcAmp->Get(VideoProcAmp_Brightness, &Default, &Flags);
LOG4CPP_INFO(logger, "Current Settings for VideoProcAmp_Brightness: Default=" << Default << " Flags=" << Flags);
pAMVideoProcAmp->GetRange(VideoProcAmp_Contrast, &Min, &Max, &Step, &Default, &Flags);
LOG4CPP_INFO(logger, "Possible Settings for VideoProcAmp_Contrast: min=" << Min << " max=" << Max << " Step=" << Step << " Default=" << Default << " Flags=" << Flags);
pAMVideoProcAmp->Get(VideoProcAmp_Contrast, &Default, &Flags);
LOG4CPP_INFO(logger, "Current Settings for VideoProcAmp_Contrast: Default=" << Default << " Flags=" << Flags);
pAMVideoProcAmp->GetRange(VideoProcAmp_Saturation, &Min, &Max, &Step, &Default, &Flags);
LOG4CPP_INFO(logger, "Possible Settings for VideoProcAmp_Saturation: min=" << Min << " max=" << Max << " Step=" << Step << " Default=" << Default << " Flags=" << Flags);
pAMVideoProcAmp->Get(VideoProcAmp_Saturation, &Default, &Flags);
LOG4CPP_INFO(logger, "Current Settings for VideoProcAmp_Saturation: Default=" << Default << " Flags=" << Flags);
pAMVideoProcAmp->GetRange(VideoProcAmp_Sharpness, &Min, &Max, &Step, &Default, &Flags);
LOG4CPP_INFO(logger, "Possible Settings for VideoProcAmp_Sharpness: min=" << Min << " max=" << Max << " Step=" << Step << " Default=" << Default << " Flags=" << Flags);
pAMVideoProcAmp->Get(VideoProcAmp_Sharpness, &Default, &Flags);
LOG4CPP_INFO(logger, "Current Settings for VideoProcAmp_Sharpness: Default=" << Default << " Flags=" << Flags);
pAMVideoProcAmp->GetRange(VideoProcAmp_Gamma, &Min, &Max, &Step, &Default, &Flags);
LOG4CPP_INFO(logger, "Possible Settings for VideoProcAmp_Gamma: min=" << Min << " max=" << Max << " Step=" << Step << " Default=" << Default << " Flags=" << Flags);
pAMVideoProcAmp->Get(VideoProcAmp_Gamma, &Default, &Flags);
LOG4CPP_INFO(logger, "Current Settings for VideoProcAmp_Gamma: Default=" << Default << " Flags=" << Flags);
pAMVideoProcAmp->GetRange(VideoProcAmp_WhiteBalance, &Min, &Max, &Step, &Default, &Flags);
LOG4CPP_INFO(logger, "Possible Settings for VideoProcAmp_WhiteBalance: min=" << Min << " max=" << Max << " Step=" << Step << " Default=" << Default << " Flags=" << Flags);
pAMVideoProcAmp->Get(VideoProcAmp_WhiteBalance, &Default, &Flags);
LOG4CPP_INFO(logger, "Current Settings for VideoProcAmp_WhiteBalance: Default=" << Default << " Flags=" << Flags);
pAMVideoProcAmp->GetRange(VideoProcAmp_BacklightCompensation, &Min, &Max, &Step, &Default, &Flags);
LOG4CPP_INFO(logger, "Possible Settings for VideoProcAmp_BacklightCompensation: min=" << Min << " max=" << Max << " Step=" << Step << " Default=" << Default << " Flags=" << Flags);
pAMVideoProcAmp->Get(VideoProcAmp_BacklightCompensation, &Default, &Flags);
LOG4CPP_INFO(logger, "Current Settings for VideoProcAmp_BacklightCompensation: Default=" << Default << " Flags=" << Flags);
pAMVideoProcAmp->GetRange(VideoProcAmp_Gain, &Min, &Max, &Step, &Default, &Flags);
LOG4CPP_INFO(logger, "Possible Settings for VideoProcAmp_Gain: min=" << Min << " max=" << Max << " Step=" << Step << " Default=" << Default << " Flags=" << Flags);
pAMVideoProcAmp->Get(VideoProcAmp_Gain, &Default, &Flags);
LOG4CPP_INFO(logger, "Current Settings for VideoProcAmp_Gain: Default=" << Default << " Flags=" << Flags);
hr = pAMVideoProcAmp->Set(VideoProcAmp_Brightness, m_cameraBrightness, VideoProcAmp_Flags_Manual);
if (FAILED(hr))
LOG4CPP_ERROR( logger, "Error setting camera exposure brightness to " << m_cameraBrightness);
hr = pAMVideoProcAmp->Set(VideoProcAmp_Contrast, m_cameraContrast, VideoProcAmp_Flags_Manual);
if (FAILED(hr))
LOG4CPP_ERROR( logger, "Error setting camera contrast property to " << m_cameraContrast);
hr = pAMVideoProcAmp->Set(VideoProcAmp_Saturation, m_cameraSaturation, VideoProcAmp_Flags_Manual);
if (FAILED(hr))
LOG4CPP_ERROR( logger, "Error setting camera saturation property to " << m_cameraSaturation);
hr = pAMVideoProcAmp->Set(VideoProcAmp_Sharpness, m_cameraSharpness, VideoProcAmp_Flags_Manual);
if (FAILED(hr))
LOG4CPP_ERROR( logger, "Error setting camera sharpness property to " << m_cameraSharpness);
hr = pAMVideoProcAmp->Set(VideoProcAmp_Gamma, m_cameraGamma, VideoProcAmp_Flags_Manual);
if (FAILED(hr))
LOG4CPP_ERROR( logger, "Error setting camera gamma property to " << m_cameraGamma);
int wbFlags = VideoProcAmp_Flags_Manual;
if (m_cameraWhitebalanceAuto)
wbFlags = VideoProcAmp_Flags_Auto;
hr = pAMVideoProcAmp->Set(VideoProcAmp_WhiteBalance, m_cameraWhitebalance, wbFlags);
if (FAILED(hr))
LOG4CPP_ERROR( logger, "Error setting camera whitebalance property to " << m_cameraWhitebalance);
int backlightComp = m_cameraBacklightComp ? 1 : 0;
hr = pAMVideoProcAmp->Set(VideoProcAmp_BacklightCompensation, backlightComp, VideoProcAmp_Flags_Manual);
if (FAILED(hr))
LOG4CPP_ERROR( logger, "Error setting camera backlight compensation property to " << backlightComp);
hr = pAMVideoProcAmp->Set(VideoProcAmp_Gain, m_cameraGain, VideoProcAmp_Flags_Manual);
if (FAILED(hr))
LOG4CPP_ERROR( logger, "Error setting camera gain property to " << m_cameraGain);
}
#endif
// start stream
pGraph.QueryInterface< IMediaControl >( m_pMediaControl );
m_pMediaControl->Pause();
}
void DirectShowFrameGrabber::getFrame(cv::Mat &img){
{
std::unique_lock<std::mutex> lk(m_mutex);
while (is_image_arrived_ == false) {
m_cv.wait(lk); // ★条件変数availへの通知を待機
}
img = m_buffer_img.clone();
is_image_arrived_ = false;
}
cv::flip(img, img, 0);// flip image
}
//void DirectShowFrameGrabber::handleFrame( Measurement::Timestamp utTime, const Vision::Image& bufferImage )
void DirectShowFrameGrabber::handleFrame( const cv::Mat& bufferImage )
{
#if 0
// boost::shared_ptr< Vision::Image > pColorImage;
cv::Mat pColorImage;
bool bColorImageDistorted = true;
if ( ( m_desiredWidth > 0 && m_desiredHeight > 0 ) &&
( bufferImage.cols > m_desiredWidth || bufferImage.rows > m_desiredHeight ) )
{
LOG4CPP_DEBUG( logger, "downsampling" );
pColorImage = cv::Mat( m_desiredHeight, m_desiredWidth, CV_8UC3 ) ;
// pColorImage->origin = bufferImage.origin;
// cvResize( bufferImage, *pColorImage );
}
// cv::imshow(m_desiredName,bufferImage);
#endif
{
std::lock_guard<std::mutex> lk(m_mutex);
m_buffer_img = bufferImage.clone();
is_image_arrived_ = true;
}
m_cv.notify_one();
#if 0
if ( ( m_desiredWidth > 0 && m_desiredHeight > 0 ) &&
( bufferImage.width > m_desiredWidth || bufferImage.height > m_desiredHeight ) )
{
LOG4CPP_DEBUG( logger, "downsampling" );
pColorImage.reset( new Vision::Image( m_desiredWidth, m_desiredHeight, 3 ) );
pColorImage->origin = bufferImage.origin;
cvResize( bufferImage, *pColorImage );
}
if (m_outPortRAW.isConnected()) {
m_outPortRAW.send(Measurement::ImageMeasurement(utTime, bufferImage.Clone()));
}
if ( m_colorOutPort.isConnected() )
{
if ( pColorImage )
pColorImage = m_undistorter->undistort( pColorImage );
else
pColorImage = m_undistorter->undistort( bufferImage );
bColorImageDistorted = false;
memcpy( pColorImage->channelSeq, "BGR", 4 );
m_colorOutPort.send( Measurement::ImageMeasurement( utTime, pColorImage ) );
}
if ( m_outPort.isConnected() )
{
boost::shared_ptr< Vision::Image > pGreyImage;
if ( pColorImage )
pGreyImage = pColorImage->CvtColor( CV_BGR2GRAY, 1 );
else
pGreyImage = bufferImage.CvtColor( CV_BGR2GRAY, 1 );
if ( bColorImageDistorted )
pGreyImage = m_undistorter->undistort( pGreyImage );
m_outPort.send( Measurement::ImageMeasurement( utTime, pGreyImage ) );
}
#endif
}
STDMETHODIMP DirectShowFrameGrabber::SampleCB( double Time, IMediaSample *pSample )
{
// TODO: check for double frames when using multiple cameras...
// LOG4CPP_DEBUG( logger, "SampleCB called" );
if ( Time == m_lastTime )
{
// this was a problem with DSVideoLib and multiple cameras
LOG4CPP_INFO( logger, "Got double frame" );
return S_OK;
}
m_lastTime = Time;
if ( !m_running || ( ++m_nFrames % m_divisor ) )
return S_OK;
if ( pSample->GetSize() < m_sampleWidth * m_sampleHeight * 3 )
{
LOG4CPP_INFO( logger, "Invalid sample size" );
return S_OK;
}
BYTE* pBuffer;
if ( FAILED( pSample->GetPointer( &pBuffer ) ) )
{
LOG4CPP_INFO( logger, "GetPointer failed" );
return S_OK;
}
// create IplImage, convert and send
#if 0
Vision::Image bufferImage( m_sampleWidth, m_sampleHeight, 3, pBuffer, IPL_DEPTH_8U, 1 );
Measurement::Timestamp utTime = m_syncer.convertNativeToLocal( Time );
handleFrame( utTime + 1000000L * m_timeOffset, bufferImage );
#endif
cv::Mat bufferImage(m_sampleHeight, m_sampleWidth, CV_8UC3,pBuffer);
handleFrame(bufferImage );
// cv::waitKey(5);
return S_OK;
}
} } // namespace Ubitrack::Driver

View File

@ -0,0 +1,251 @@
#ifndef DIRECT_SHOW_FRAME_GRABBER_H
#define DIRECT_SHOW_FRAME_GRABBER_H
/*
* Ubitrack - Library for Ubiquitous Tracking
* Copyright 2006, Technische Universitaet Muenchen, and individual
* contributors as indicated by the @authors tag. See the
* copyright.txt in the distribution for a full listing of individual
* contributors.
*
* This 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 software 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 software; if not, write to the Free
* Software Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA
* 02110-1301 USA, or see the FSF site: http://www.fsf.org.
*/
/**
* @ingroup vision_components
* @file
* Reads camera images using DirectShow
*
* @author Daniel Pustka <daniel.pustka@in.tum.de>
*/
#include <objbase.h>
#include <mutex>
#include <thread>
#ifdef HAVE_DIRECTSHOW
#include <DShow.h>
#pragma include_alias( "dxtrans.h", "qedit.h" )
#define __IDxtCompositor_INTERFACE_DEFINED__
#define __IDxtAlphaSetter_INTERFACE_DEFINED__
#define __IDxtJpeg_INTERFACE_DEFINED__
#define __IDxtKey_INTERFACE_DEFINED__
#include <Qedit.h>
#include <strmif.h>
struct __declspec(uuid("0579154A-2B53-4994-B0D0-E773148EFF85")) ISampleGrabberCB;
struct __declspec(uuid("6B652FFF-11FE-4fce-92AD-0266B5D7C78F")) ISampleGrabber;
#pragma warning(disable: 4995)
#else
// this include file contains just the directshow interfaces necessary to compile this component
// if you need more, install the Windows SDK, the DirectX SDK (old version with dxtrans.h, e.g. August 2007) and
// use the alternative above
#include "DirectShowInterfaces.h"
#endif
#include "AutoComPtr.h"
#include <string>
#include <list>
#include <iostream>
#include <iomanip>
#include <strstream>
//#include <log4cpp/Category.hh>
#include <boost/thread.hpp>
#include <boost/bind.hpp>
#include <boost/scoped_ptr.hpp>
#include <boost/scoped_array.hpp>
//#include <opencv/cv.h>
#include <opencv2/imgproc/imgproc.hpp>
#include <iosfwd>
#include <string>
#include <stdexcept>
#define UBITRACK_THROW( message ) {std::cerr<< message <<std::endl;throw;}
#define LOG4CPP_INFO( logger, message ) std::cout<<message<<std::endl;
#ifdef NDEBUG
#define LOG4CPP_DEBUG( logger, message ) std::cout<<message<<std::endl;
#else
#define LOG4CPP_DEBUG( logger, message ) ;
#endif
#define LOG4CPP_WARN( logger, message ) std::cout<<message<<std::endl;
// get a logger
//static log4cpp::Category& logger( log4cpp::Category::getInstance( "Ubitrack.Vision.DirectShowFrameGrabber" ) );
//using namespace Ubitrack;
//using namespace Ubitrack::Vision;
namespace Ubitrack { namespace Drivers {
/**
* @ingroup vision_components
*
* @par Input Ports
* None.
*
* @par Output Ports
* \c Output push port of type Ubitrack::Measurement::ImageMeasurement.
*
* @par Configuration
* The configuration tag contains a \c <dsvl_input> configuration.
* For details, see the DirectShow documentation...
*
*/
class DirectShowFrameGrabber
: protected ISampleGrabberCB
{
public:
/** constructor */
DirectShowFrameGrabber( const std::string& sName );
/** destructor, waits until thread stops */
~DirectShowFrameGrabber();
/** starts the camera */
void start();
/** stops the camera */
void stop();
void getFrame(cv::Mat &img);
protected:
/** initializes the direct show filter graph */
void initGraph();
/** handles a frame after being converted to Vision::Image */
void handleFrame( const cv::Mat& bufferImage );
// width of resulting image
LONG m_sampleWidth;
// height of resulting image
LONG m_sampleHeight;
// shift timestamps (ms)
int m_timeOffset;
// only send every nth image
int m_divisor;
/** desired width */
int m_desiredWidth;
/** desired image height */
int m_desiredHeight;
// desired camera name
std::string m_desiredName;
// desired camera index (e.g. for multiple cameras with the same name as the Vuzix HMD)
std::string m_desiredDevicePath;
/** exposure control */
int m_cameraExposure;
bool m_cameraExposureAuto;
/** brightness control */
int m_cameraBrightness;
/** contrast control */
int m_cameraContrast;
/** saturation control */
int m_cameraSaturation;
/** sharpness control */
int m_cameraSharpness;
/** gamma control */
int m_cameraGamma;
/** whitebalance control */
int m_cameraWhitebalance;
bool m_cameraWhitebalanceAuto;
/** gain control */
bool m_cameraBacklightComp;
/** gain control */
int m_cameraGain;
/** number of frames received */
int m_nFrames;
/** timestamp of last frame */
double m_lastTime;
bool m_running;
cv::Mat m_buffer_img;
/** pointer to DirectShow filter graph */
AutoComPtr< IMediaControl > m_pMediaControl;
// ISampleGrabberCB: fake reference counting.
STDMETHODIMP_(ULONG) AddRef()
{ return 1; }
STDMETHODIMP_(ULONG) Release()
{ return 2; }
STDMETHODIMP QueryInterface( REFIID riid, void **ppvObject )
{
if ( NULL == ppvObject )
return E_POINTER;
if ( riid == __uuidof( IUnknown ) )
{
*ppvObject = static_cast<IUnknown*>( this );
return S_OK;
}
if ( riid == __uuidof( ISampleGrabberCB ) )
{
*ppvObject = static_cast<ISampleGrabberCB*>( this );
return S_OK;
}
return E_NOTIMPL;
}
STDMETHODIMP SampleCB( double Time, IMediaSample *pSample );
STDMETHODIMP BufferCB( double Time, BYTE *pBuffer, long BufferLen )
{
LOG4CPP_INFO( logger, "BufferCB called" );
return E_NOTIMPL;
}
std::mutex m_mutex;
std::condition_variable m_cv;
bool is_image_arrived_;
};
} } // namespace Ubitrack::Driver
#endif // DIRECT_SHOW_FRAME_GRABBER_H

View File

@ -0,0 +1,9 @@
// compile additional guids
#define INITGUID
#include <guiddef.h>
#include "DirectShowGuids.h"

View File

@ -0,0 +1,12 @@
// additional guids needed for directshow
DEFINE_GUID(CLSID_SystemDeviceEnum,0x62BE5D10,0x60EB,0x11d0,0xBD,0x3B,0x00,0xA0,0xC9,0x11,0xCE,0x86);
DEFINE_GUID(CLSID_VideoInputDeviceCategory,0x860BB310,0x5D01,0x11d0,0xBD,0x3B,0x00,0xA0,0xC9,0x11,0xCE,0x86);
DEFINE_GUID(CLSID_CaptureGraphBuilder2,0xBF87B6E1, 0x8C27, 0x11d0, 0xB3, 0xF0, 0x0, 0xAA, 0x00, 0x37, 0x61, 0xC5);
DEFINE_GUID(CLSID_FilterGraph,0xe436ebb3, 0x524f, 0x11ce, 0x9f, 0x53, 0x00, 0x20, 0xaf, 0x0b, 0xa7, 0x70);
DEFINE_GUID(CLSID_SampleGrabber,0xC1F400A0, 0x3F08, 0x11d3, 0x9F, 0x0B, 0x00, 0x60, 0x08, 0x03, 0x9E, 0x37);
DEFINE_GUID(CLSID_NullRenderer,0xC1F400A4, 0x3F08, 0x11d3, 0x9F, 0x0B, 0x00, 0x60, 0x08, 0x03, 0x9E, 0x37);
DEFINE_GUID(PIN_CATEGORY_CAPTURE,0xfb6c4281, 0x0353, 0x11d1, 0x90, 0x5f, 0x00, 0x00, 0xc0, 0xcc, 0x16, 0xba);
DEFINE_GUID(MEDIATYPE_Video,0x73646976, 0x0000, 0x0010, 0x80, 0x00, 0x00, 0xaa, 0x00, 0x38, 0x9b, 0x71);
DEFINE_GUID(MEDIASUBTYPE_RGB24,0xe436eb7d, 0x524f, 0x11ce, 0x9f, 0x53, 0x00, 0x20, 0xaf, 0x0b, 0xa7, 0x70);
DEFINE_GUID(FORMAT_VideoInfo,0x05589f80, 0xc356, 0x11ce, 0xbf, 0x01, 0x00, 0xaa, 0x00, 0x55, 0x59, 0x5a);

File diff suppressed because it is too large Load Diff

File diff suppressed because it is too large Load Diff

View File

@ -0,0 +1,74 @@
# HOW TO BUILD (A) LIBRAR-Y/IES FROM A MODULE
# Building libraries from modules should be consistent for each module.
# Use the following scheme for each new library created:
#
# a) Check for required libraries. If they are not available return False and export flags if needed
# b) Define the src-subdirectories for which the library should be compiled
# and glob all files in there
# c) Define a [LIBRARY]_options variable for the library containing all dependencies
# from other libraries. Create a clone from the master environment and add those options.
# d) WINDOWS ONLY: Create a header file which implements dll interface functionality
# dependent from a [LIBRARY]_DLL define. If the define is set, the dll should be exported,
# if not, the dll should be imported. Set the define in the preprocessor options for the environment.
# e) Build the library!
# f) Remove all entries for (at least) the LIBS and LIBPATH keys in the options and
# add only the newly-compiled library as a dependcy. This will avoid multiple includes
# of libraries. Also verify that the import/export-switch define is not set.
# g) Export the options so that other libraries can base on this library
# h) Optionally setup help and ide projects
#
# The use of options and possibility to export them makes hierarchical build environments
# obsolete. Avoid exporting new environments to the build system.
import glob
import os.path
import sys
have_utvision = False
Import( '*' )
# a)
if not (sys.platform == "win32" and have_utvision):
print "utVision missing or not on ms windows -- not building DirectShowFrameGrabber"
result = False
Return ('result')
# b)
headers = globSourceFiles( '*.h' )
sources = globSourceFiles( '*.cpp' )
sources += globSourceFiles( '*.c' )
if have_directshow:
for src in [ 'DirectShowInterfaces_i.c' ]:
sources.remove( src );
# c)
framegrabber_options = mergeOptions(utvision_all_options, utdataflow_all_options, directshow_options)
env = masterEnv.Clone()
env.AppendUnique( **framegrabber_options )
if have_directshow:
env.AppendUnique(directshow_options)
env.Append( CPPDEFINES = [ 'HAVE_DIRECTSHOW'] )
env.AppendUnique( LIBS = [ "ole32", "oleaut32" ] )
# d)
# library is component, nothing to export
# e)
# compile all source files into single library
# {buildenvironment, source files, name of the library, build target}
setupSingleComponentBuild(env, sources, "DirectShowFrameGrabber", "DirectShowFrameGrabber")
# f)
# nothing to do this time
# g)
# nothing to do this time
# h)
createVisualStudioProject(env, sources, headers, "DirectShowFrameGrabber")

11
external/Eigen-3.2.7/Eigen/Array vendored Normal file
View File

@ -0,0 +1,11 @@
#ifndef EIGEN_ARRAY_MODULE_H
#define EIGEN_ARRAY_MODULE_H
// include Core first to handle Eigen2 support macros
#include "Core"
#ifndef EIGEN2_SUPPORT
#error The Eigen/Array header does no longer exist in Eigen3. All that functionality has moved to Eigen/Core.
#endif
#endif // EIGEN_ARRAY_MODULE_H

View File

@ -0,0 +1,19 @@
include(RegexUtils)
test_escape_string_as_regex()
file(GLOB Eigen_directory_files "*")
escape_string_as_regex(ESCAPED_CMAKE_CURRENT_SOURCE_DIR "${CMAKE_CURRENT_SOURCE_DIR}")
foreach(f ${Eigen_directory_files})
if(NOT f MATCHES "\\.txt" AND NOT f MATCHES "${ESCAPED_CMAKE_CURRENT_SOURCE_DIR}/[.].+" AND NOT f MATCHES "${ESCAPED_CMAKE_CURRENT_SOURCE_DIR}/src")
list(APPEND Eigen_directory_files_to_install ${f})
endif()
endforeach(f ${Eigen_directory_files})
install(FILES
${Eigen_directory_files_to_install}
DESTINATION ${INCLUDE_INSTALL_DIR}/Eigen COMPONENT Devel
)
add_subdirectory(src)

32
external/Eigen-3.2.7/Eigen/Cholesky vendored Normal file
View File

@ -0,0 +1,32 @@
#ifndef EIGEN_CHOLESKY_MODULE_H
#define EIGEN_CHOLESKY_MODULE_H
#include "Core"
#include "src/Core/util/DisableStupidWarnings.h"
/** \defgroup Cholesky_Module Cholesky module
*
*
*
* This module provides two variants of the Cholesky decomposition for selfadjoint (hermitian) matrices.
* Those decompositions are accessible via the following MatrixBase methods:
* - MatrixBase::llt(),
* - MatrixBase::ldlt()
*
* \code
* #include <Eigen/Cholesky>
* \endcode
*/
#include "src/misc/Solve.h"
#include "src/Cholesky/LLT.h"
#include "src/Cholesky/LDLT.h"
#ifdef EIGEN_USE_LAPACKE
#include "src/Cholesky/LLT_MKL.h"
#endif
#include "src/Core/util/ReenableStupidWarnings.h"
#endif // EIGEN_CHOLESKY_MODULE_H
/* vim: set filetype=cpp et sw=2 ts=2 ai: */

View File

@ -0,0 +1,45 @@
#ifndef EIGEN_CHOLMODSUPPORT_MODULE_H
#define EIGEN_CHOLMODSUPPORT_MODULE_H
#include "SparseCore"
#include "src/Core/util/DisableStupidWarnings.h"
extern "C" {
#include <cholmod.h>
}
/** \ingroup Support_modules
* \defgroup CholmodSupport_Module CholmodSupport module
*
* This module provides an interface to the Cholmod library which is part of the <a href="http://www.cise.ufl.edu/research/sparse/SuiteSparse/">suitesparse</a> package.
* It provides the two following main factorization classes:
* - class CholmodSupernodalLLT: a supernodal LLT Cholesky factorization.
* - class CholmodDecomposiiton: a general L(D)LT Cholesky factorization with automatic or explicit runtime selection of the underlying factorization method (supernodal or simplicial).
*
* For the sake of completeness, this module also propose the two following classes:
* - class CholmodSimplicialLLT
* - class CholmodSimplicialLDLT
* Note that these classes does not bring any particular advantage compared to the built-in
* SimplicialLLT and SimplicialLDLT factorization classes.
*
* \code
* #include <Eigen/CholmodSupport>
* \endcode
*
* In order to use this module, the cholmod headers must be accessible from the include paths, and your binary must be linked to the cholmod library and its dependencies.
* The dependencies depend on how cholmod has been compiled.
* For a cmake based project, you can use our FindCholmod.cmake module to help you in this task.
*
*/
#include "src/misc/Solve.h"
#include "src/misc/SparseSolve.h"
#include "src/CholmodSupport/CholmodSupport.h"
#include "src/Core/util/ReenableStupidWarnings.h"
#endif // EIGEN_CHOLMODSUPPORT_MODULE_H

376
external/Eigen-3.2.7/Eigen/Core vendored Normal file
View File

@ -0,0 +1,376 @@
// This file is part of Eigen, a lightweight C++ template library
// for linear algebra.
//
// Copyright (C) 2008 Gael Guennebaud <gael.guennebaud@inria.fr>
// Copyright (C) 2007-2011 Benoit Jacob <jacob.benoit.1@gmail.com>
//
// This Source Code Form is subject to the terms of the Mozilla
// Public License v. 2.0. If a copy of the MPL was not distributed
// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
#ifndef EIGEN_CORE_H
#define EIGEN_CORE_H
// first thing Eigen does: stop the compiler from committing suicide
#include "src/Core/util/DisableStupidWarnings.h"
// then include this file where all our macros are defined. It's really important to do it first because
// it's where we do all the alignment settings (platform detection and honoring the user's will if he
// defined e.g. EIGEN_DONT_ALIGN) so it needs to be done before we do anything with vectorization.
#include "src/Core/util/Macros.h"
// Disable the ipa-cp-clone optimization flag with MinGW 6.x or newer (enabled by default with -O3)
// See http://eigen.tuxfamily.org/bz/show_bug.cgi?id=556 for details.
#if defined(__MINGW32__) && EIGEN_GNUC_AT_LEAST(4,6)
#pragma GCC optimize ("-fno-ipa-cp-clone")
#endif
#include <complex>
// this include file manages BLAS and MKL related macros
// and inclusion of their respective header files
#include "src/Core/util/MKL_support.h"
// if alignment is disabled, then disable vectorization. Note: EIGEN_ALIGN is the proper check, it takes into
// account both the user's will (EIGEN_DONT_ALIGN) and our own platform checks
#if !EIGEN_ALIGN
#ifndef EIGEN_DONT_VECTORIZE
#define EIGEN_DONT_VECTORIZE
#endif
#endif
#ifdef _MSC_VER
#include <malloc.h> // for _aligned_malloc -- need it regardless of whether vectorization is enabled
#if (_MSC_VER >= 1500) // 2008 or later
// Remember that usage of defined() in a #define is undefined by the standard.
// a user reported that in 64-bit mode, MSVC doesn't care to define _M_IX86_FP.
#if (defined(_M_IX86_FP) && (_M_IX86_FP >= 2)) || defined(_M_X64)
#define EIGEN_SSE2_ON_MSVC_2008_OR_LATER
#endif
#endif
#else
// Remember that usage of defined() in a #define is undefined by the standard
#if (defined __SSE2__) && ( (!defined __GNUC__) || (defined __INTEL_COMPILER) || EIGEN_GNUC_AT_LEAST(4,2) )
#define EIGEN_SSE2_ON_NON_MSVC_BUT_NOT_OLD_GCC
#endif
#endif
#ifndef EIGEN_DONT_VECTORIZE
#if defined (EIGEN_SSE2_ON_NON_MSVC_BUT_NOT_OLD_GCC) || defined(EIGEN_SSE2_ON_MSVC_2008_OR_LATER)
// Defines symbols for compile-time detection of which instructions are
// used.
// EIGEN_VECTORIZE_YY is defined if and only if the instruction set YY is used
#define EIGEN_VECTORIZE
#define EIGEN_VECTORIZE_SSE
#define EIGEN_VECTORIZE_SSE2
// Detect sse3/ssse3/sse4:
// gcc and icc defines __SSE3__, ...
// there is no way to know about this on msvc. You can define EIGEN_VECTORIZE_SSE* if you
// want to force the use of those instructions with msvc.
#ifdef __SSE3__
#define EIGEN_VECTORIZE_SSE3
#endif
#ifdef __SSSE3__
#define EIGEN_VECTORIZE_SSSE3
#endif
#ifdef __SSE4_1__
#define EIGEN_VECTORIZE_SSE4_1
#endif
#ifdef __SSE4_2__
#define EIGEN_VECTORIZE_SSE4_2
#endif
// include files
// This extern "C" works around a MINGW-w64 compilation issue
// https://sourceforge.net/tracker/index.php?func=detail&aid=3018394&group_id=202880&atid=983354
// In essence, intrin.h is included by windows.h and also declares intrinsics (just as emmintrin.h etc. below do).
// However, intrin.h uses an extern "C" declaration, and g++ thus complains of duplicate declarations
// with conflicting linkage. The linkage for intrinsics doesn't matter, but at that stage the compiler doesn't know;
// so, to avoid compile errors when windows.h is included after Eigen/Core, ensure intrinsics are extern "C" here too.
// notice that since these are C headers, the extern "C" is theoretically needed anyways.
extern "C" {
// In theory we should only include immintrin.h and not the other *mmintrin.h header files directly.
// Doing so triggers some issues with ICC. However old gcc versions seems to not have this file, thus:
#if defined(__INTEL_COMPILER) && __INTEL_COMPILER >= 1110
#include <immintrin.h>
#else
#include <emmintrin.h>
#include <xmmintrin.h>
#ifdef EIGEN_VECTORIZE_SSE3
#include <pmmintrin.h>
#endif
#ifdef EIGEN_VECTORIZE_SSSE3
#include <tmmintrin.h>
#endif
#ifdef EIGEN_VECTORIZE_SSE4_1
#include <smmintrin.h>
#endif
#ifdef EIGEN_VECTORIZE_SSE4_2
#include <nmmintrin.h>
#endif
#endif
} // end extern "C"
#elif defined __ALTIVEC__
#define EIGEN_VECTORIZE
#define EIGEN_VECTORIZE_ALTIVEC
#include <altivec.h>
// We need to #undef all these ugly tokens defined in <altivec.h>
// => use __vector instead of vector
#undef bool
#undef vector
#undef pixel
#elif defined __ARM_NEON
#define EIGEN_VECTORIZE
#define EIGEN_VECTORIZE_NEON
#include <arm_neon.h>
#endif
#endif
#if (defined _OPENMP) && (!defined EIGEN_DONT_PARALLELIZE)
#define EIGEN_HAS_OPENMP
#endif
#ifdef EIGEN_HAS_OPENMP
#include <omp.h>
#endif
// MSVC for windows mobile does not have the errno.h file
#if !(defined(_MSC_VER) && defined(_WIN32_WCE)) && !defined(__ARMCC_VERSION)
#define EIGEN_HAS_ERRNO
#endif
#ifdef EIGEN_HAS_ERRNO
#include <cerrno>
#endif
#include <cstddef>
#include <cstdlib>
#include <cmath>
#include <cassert>
#include <functional>
#include <iosfwd>
#include <cstring>
#include <string>
#include <limits>
#include <climits> // for CHAR_BIT
// for min/max:
#include <algorithm>
// for outputting debug info
#ifdef EIGEN_DEBUG_ASSIGN
#include <iostream>
#endif
// required for __cpuid, needs to be included after cmath
#if defined(_MSC_VER) && (defined(_M_IX86)||defined(_M_X64)) && (!defined(_WIN32_WCE))
#include <intrin.h>
#endif
#if defined(_CPPUNWIND) || defined(__EXCEPTIONS)
#define EIGEN_EXCEPTIONS
#endif
#ifdef EIGEN_EXCEPTIONS
#include <new>
#endif
/** \brief Namespace containing all symbols from the %Eigen library. */
namespace Eigen {
inline static const char *SimdInstructionSetsInUse(void) {
#if defined(EIGEN_VECTORIZE_SSE4_2)
return "SSE, SSE2, SSE3, SSSE3, SSE4.1, SSE4.2";
#elif defined(EIGEN_VECTORIZE_SSE4_1)
return "SSE, SSE2, SSE3, SSSE3, SSE4.1";
#elif defined(EIGEN_VECTORIZE_SSSE3)
return "SSE, SSE2, SSE3, SSSE3";
#elif defined(EIGEN_VECTORIZE_SSE3)
return "SSE, SSE2, SSE3";
#elif defined(EIGEN_VECTORIZE_SSE2)
return "SSE, SSE2";
#elif defined(EIGEN_VECTORIZE_ALTIVEC)
return "AltiVec";
#elif defined(EIGEN_VECTORIZE_NEON)
return "ARM NEON";
#else
return "None";
#endif
}
} // end namespace Eigen
#define STAGE10_FULL_EIGEN2_API 10
#define STAGE20_RESOLVE_API_CONFLICTS 20
#define STAGE30_FULL_EIGEN3_API 30
#define STAGE40_FULL_EIGEN3_STRICTNESS 40
#define STAGE99_NO_EIGEN2_SUPPORT 99
#if defined EIGEN2_SUPPORT_STAGE40_FULL_EIGEN3_STRICTNESS
#define EIGEN2_SUPPORT
#define EIGEN2_SUPPORT_STAGE STAGE40_FULL_EIGEN3_STRICTNESS
#elif defined EIGEN2_SUPPORT_STAGE30_FULL_EIGEN3_API
#define EIGEN2_SUPPORT
#define EIGEN2_SUPPORT_STAGE STAGE30_FULL_EIGEN3_API
#elif defined EIGEN2_SUPPORT_STAGE20_RESOLVE_API_CONFLICTS
#define EIGEN2_SUPPORT
#define EIGEN2_SUPPORT_STAGE STAGE20_RESOLVE_API_CONFLICTS
#elif defined EIGEN2_SUPPORT_STAGE10_FULL_EIGEN2_API
#define EIGEN2_SUPPORT
#define EIGEN2_SUPPORT_STAGE STAGE10_FULL_EIGEN2_API
#elif defined EIGEN2_SUPPORT
// default to stage 3, that's what it's always meant
#define EIGEN2_SUPPORT_STAGE30_FULL_EIGEN3_API
#define EIGEN2_SUPPORT_STAGE STAGE30_FULL_EIGEN3_API
#else
#define EIGEN2_SUPPORT_STAGE STAGE99_NO_EIGEN2_SUPPORT
#endif
#ifdef EIGEN2_SUPPORT
#undef minor
#endif
// we use size_t frequently and we'll never remember to prepend it with std:: everytime just to
// ensure QNX/QCC support
using std::size_t;
// gcc 4.6.0 wants std:: for ptrdiff_t
using std::ptrdiff_t;
/** \defgroup Core_Module Core module
* This is the main module of Eigen providing dense matrix and vector support
* (both fixed and dynamic size) with all the features corresponding to a BLAS library
* and much more...
*
* \code
* #include <Eigen/Core>
* \endcode
*/
#include "src/Core/util/Constants.h"
#include "src/Core/util/ForwardDeclarations.h"
#include "src/Core/util/Meta.h"
#include "src/Core/util/StaticAssert.h"
#include "src/Core/util/XprHelper.h"
#include "src/Core/util/Memory.h"
#include "src/Core/NumTraits.h"
#include "src/Core/MathFunctions.h"
#include "src/Core/GenericPacketMath.h"
#if defined EIGEN_VECTORIZE_SSE
#include "src/Core/arch/SSE/PacketMath.h"
#include "src/Core/arch/SSE/MathFunctions.h"
#include "src/Core/arch/SSE/Complex.h"
#elif defined EIGEN_VECTORIZE_ALTIVEC
#include "src/Core/arch/AltiVec/PacketMath.h"
#include "src/Core/arch/AltiVec/Complex.h"
#elif defined EIGEN_VECTORIZE_NEON
#include "src/Core/arch/NEON/PacketMath.h"
#include "src/Core/arch/NEON/Complex.h"
#endif
#include "src/Core/arch/Default/Settings.h"
#include "src/Core/Functors.h"
#include "src/Core/DenseCoeffsBase.h"
#include "src/Core/DenseBase.h"
#include "src/Core/MatrixBase.h"
#include "src/Core/EigenBase.h"
#ifndef EIGEN_PARSED_BY_DOXYGEN // work around Doxygen bug triggered by Assign.h r814874
// at least confirmed with Doxygen 1.5.5 and 1.5.6
#include "src/Core/Assign.h"
#endif
#include "src/Core/util/BlasUtil.h"
#include "src/Core/DenseStorage.h"
#include "src/Core/NestByValue.h"
#include "src/Core/ForceAlignedAccess.h"
#include "src/Core/ReturnByValue.h"
#include "src/Core/NoAlias.h"
#include "src/Core/PlainObjectBase.h"
#include "src/Core/Matrix.h"
#include "src/Core/Array.h"
#include "src/Core/CwiseBinaryOp.h"
#include "src/Core/CwiseUnaryOp.h"
#include "src/Core/CwiseNullaryOp.h"
#include "src/Core/CwiseUnaryView.h"
#include "src/Core/SelfCwiseBinaryOp.h"
#include "src/Core/Dot.h"
#include "src/Core/StableNorm.h"
#include "src/Core/MapBase.h"
#include "src/Core/Stride.h"
#include "src/Core/Map.h"
#include "src/Core/Block.h"
#include "src/Core/VectorBlock.h"
#include "src/Core/Ref.h"
#include "src/Core/Transpose.h"
#include "src/Core/DiagonalMatrix.h"
#include "src/Core/Diagonal.h"
#include "src/Core/DiagonalProduct.h"
#include "src/Core/PermutationMatrix.h"
#include "src/Core/Transpositions.h"
#include "src/Core/Redux.h"
#include "src/Core/Visitor.h"
#include "src/Core/Fuzzy.h"
#include "src/Core/IO.h"
#include "src/Core/Swap.h"
#include "src/Core/CommaInitializer.h"
#include "src/Core/Flagged.h"
#include "src/Core/ProductBase.h"
#include "src/Core/GeneralProduct.h"
#include "src/Core/TriangularMatrix.h"
#include "src/Core/SelfAdjointView.h"
#include "src/Core/products/GeneralBlockPanelKernel.h"
#include "src/Core/products/Parallelizer.h"
#include "src/Core/products/CoeffBasedProduct.h"
#include "src/Core/products/GeneralMatrixVector.h"
#include "src/Core/products/GeneralMatrixMatrix.h"
#include "src/Core/SolveTriangular.h"
#include "src/Core/products/GeneralMatrixMatrixTriangular.h"
#include "src/Core/products/SelfadjointMatrixVector.h"
#include "src/Core/products/SelfadjointMatrixMatrix.h"
#include "src/Core/products/SelfadjointProduct.h"
#include "src/Core/products/SelfadjointRank2Update.h"
#include "src/Core/products/TriangularMatrixVector.h"
#include "src/Core/products/TriangularMatrixMatrix.h"
#include "src/Core/products/TriangularSolverMatrix.h"
#include "src/Core/products/TriangularSolverVector.h"
#include "src/Core/BandMatrix.h"
#include "src/Core/CoreIterators.h"
#include "src/Core/BooleanRedux.h"
#include "src/Core/Select.h"
#include "src/Core/VectorwiseOp.h"
#include "src/Core/Random.h"
#include "src/Core/Replicate.h"
#include "src/Core/Reverse.h"
#include "src/Core/ArrayBase.h"
#include "src/Core/ArrayWrapper.h"
#ifdef EIGEN_USE_BLAS
#include "src/Core/products/GeneralMatrixMatrix_MKL.h"
#include "src/Core/products/GeneralMatrixVector_MKL.h"
#include "src/Core/products/GeneralMatrixMatrixTriangular_MKL.h"
#include "src/Core/products/SelfadjointMatrixMatrix_MKL.h"
#include "src/Core/products/SelfadjointMatrixVector_MKL.h"
#include "src/Core/products/TriangularMatrixMatrix_MKL.h"
#include "src/Core/products/TriangularMatrixVector_MKL.h"
#include "src/Core/products/TriangularSolverMatrix_MKL.h"
#endif // EIGEN_USE_BLAS
#ifdef EIGEN_USE_MKL_VML
#include "src/Core/Assign_MKL.h"
#endif
#include "src/Core/GlobalFunctions.h"
#include "src/Core/util/ReenableStupidWarnings.h"
#ifdef EIGEN2_SUPPORT
#include "Eigen2Support"
#endif
#endif // EIGEN_CORE_H

7
external/Eigen-3.2.7/Eigen/Dense vendored Normal file
View File

@ -0,0 +1,7 @@
#include "Core"
#include "LU"
#include "Cholesky"
#include "QR"
#include "SVD"
#include "Geometry"
#include "Eigenvalues"

2
external/Eigen-3.2.7/Eigen/Eigen vendored Normal file
View File

@ -0,0 +1,2 @@
#include "Dense"
//#include "Sparse"

View File

@ -0,0 +1,95 @@
// This file is part of Eigen, a lightweight C++ template library
// for linear algebra.
//
// Copyright (C) 2009 Gael Guennebaud <gael.guennebaud@inria.fr>
//
// This Source Code Form is subject to the terms of the Mozilla
// Public License v. 2.0. If a copy of the MPL was not distributed
// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
#ifndef EIGEN2SUPPORT_H
#define EIGEN2SUPPORT_H
#if (!defined(EIGEN2_SUPPORT)) || (!defined(EIGEN_CORE_H))
#error Eigen2 support must be enabled by defining EIGEN2_SUPPORT before including any Eigen header
#endif
#ifndef EIGEN_NO_EIGEN2_DEPRECATED_WARNING
#if defined(__GNUC__) || defined(__INTEL_COMPILER) || defined(__clang__)
#warning "Eigen2 support is deprecated in Eigen 3.2.x and it will be removed in Eigen 3.3. (Define EIGEN_NO_EIGEN2_DEPRECATED_WARNING to disable this warning)"
#else
#pragma message ("Eigen2 support is deprecated in Eigen 3.2.x and it will be removed in Eigen 3.3. (Define EIGEN_NO_EIGEN2_DEPRECATED_WARNING to disable this warning)")
#endif
#endif // EIGEN_NO_EIGEN2_DEPRECATED_WARNING
#include "src/Core/util/DisableStupidWarnings.h"
/** \ingroup Support_modules
* \defgroup Eigen2Support_Module Eigen2 support module
*
* \warning Eigen2 support is deprecated in Eigen 3.2.x and it will be removed in Eigen 3.3.
*
* This module provides a couple of deprecated functions improving the compatibility with Eigen2.
*
* To use it, define EIGEN2_SUPPORT before including any Eigen header
* \code
* #define EIGEN2_SUPPORT
* \endcode
*
*/
#include "src/Eigen2Support/Macros.h"
#include "src/Eigen2Support/Memory.h"
#include "src/Eigen2Support/Meta.h"
#include "src/Eigen2Support/Lazy.h"
#include "src/Eigen2Support/Cwise.h"
#include "src/Eigen2Support/CwiseOperators.h"
#include "src/Eigen2Support/TriangularSolver.h"
#include "src/Eigen2Support/Block.h"
#include "src/Eigen2Support/VectorBlock.h"
#include "src/Eigen2Support/Minor.h"
#include "src/Eigen2Support/MathFunctions.h"
#include "src/Core/util/ReenableStupidWarnings.h"
// Eigen2 used to include iostream
#include<iostream>
#define EIGEN_USING_MATRIX_TYPEDEFS_FOR_TYPE_AND_SIZE(TypeSuffix, SizeSuffix) \
using Eigen::Matrix##SizeSuffix##TypeSuffix; \
using Eigen::Vector##SizeSuffix##TypeSuffix; \
using Eigen::RowVector##SizeSuffix##TypeSuffix;
#define EIGEN_USING_MATRIX_TYPEDEFS_FOR_TYPE(TypeSuffix) \
EIGEN_USING_MATRIX_TYPEDEFS_FOR_TYPE_AND_SIZE(TypeSuffix, 2) \
EIGEN_USING_MATRIX_TYPEDEFS_FOR_TYPE_AND_SIZE(TypeSuffix, 3) \
EIGEN_USING_MATRIX_TYPEDEFS_FOR_TYPE_AND_SIZE(TypeSuffix, 4) \
EIGEN_USING_MATRIX_TYPEDEFS_FOR_TYPE_AND_SIZE(TypeSuffix, X) \
#define EIGEN_USING_MATRIX_TYPEDEFS \
EIGEN_USING_MATRIX_TYPEDEFS_FOR_TYPE(i) \
EIGEN_USING_MATRIX_TYPEDEFS_FOR_TYPE(f) \
EIGEN_USING_MATRIX_TYPEDEFS_FOR_TYPE(d) \
EIGEN_USING_MATRIX_TYPEDEFS_FOR_TYPE(cf) \
EIGEN_USING_MATRIX_TYPEDEFS_FOR_TYPE(cd)
#define USING_PART_OF_NAMESPACE_EIGEN \
EIGEN_USING_MATRIX_TYPEDEFS \
using Eigen::Matrix; \
using Eigen::MatrixBase; \
using Eigen::ei_random; \
using Eigen::ei_real; \
using Eigen::ei_imag; \
using Eigen::ei_conj; \
using Eigen::ei_abs; \
using Eigen::ei_abs2; \
using Eigen::ei_sqrt; \
using Eigen::ei_exp; \
using Eigen::ei_log; \
using Eigen::ei_sin; \
using Eigen::ei_cos;
#endif // EIGEN2SUPPORT_H

48
external/Eigen-3.2.7/Eigen/Eigenvalues vendored Normal file
View File

@ -0,0 +1,48 @@
#ifndef EIGEN_EIGENVALUES_MODULE_H
#define EIGEN_EIGENVALUES_MODULE_H
#include "Core"
#include "src/Core/util/DisableStupidWarnings.h"
#include "Cholesky"
#include "Jacobi"
#include "Householder"
#include "LU"
#include "Geometry"
/** \defgroup Eigenvalues_Module Eigenvalues module
*
*
*
* This module mainly provides various eigenvalue solvers.
* This module also provides some MatrixBase methods, including:
* - MatrixBase::eigenvalues(),
* - MatrixBase::operatorNorm()
*
* \code
* #include <Eigen/Eigenvalues>
* \endcode
*/
#include "src/Eigenvalues/Tridiagonalization.h"
#include "src/Eigenvalues/RealSchur.h"
#include "src/Eigenvalues/EigenSolver.h"
#include "src/Eigenvalues/SelfAdjointEigenSolver.h"
#include "src/Eigenvalues/GeneralizedSelfAdjointEigenSolver.h"
#include "src/Eigenvalues/HessenbergDecomposition.h"
#include "src/Eigenvalues/ComplexSchur.h"
#include "src/Eigenvalues/ComplexEigenSolver.h"
#include "src/Eigenvalues/RealQZ.h"
#include "src/Eigenvalues/GeneralizedEigenSolver.h"
#include "src/Eigenvalues/MatrixBaseEigenvalues.h"
#ifdef EIGEN_USE_LAPACKE
#include "src/Eigenvalues/RealSchur_MKL.h"
#include "src/Eigenvalues/ComplexSchur_MKL.h"
#include "src/Eigenvalues/SelfAdjointEigenSolver_MKL.h"
#endif
#include "src/Core/util/ReenableStupidWarnings.h"
#endif // EIGEN_EIGENVALUES_MODULE_H
/* vim: set filetype=cpp et sw=2 ts=2 ai: */

63
external/Eigen-3.2.7/Eigen/Geometry vendored Normal file
View File

@ -0,0 +1,63 @@
#ifndef EIGEN_GEOMETRY_MODULE_H
#define EIGEN_GEOMETRY_MODULE_H
#include "Core"
#include "src/Core/util/DisableStupidWarnings.h"
#include "SVD"
#include "LU"
#include <limits>
#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif
/** \defgroup Geometry_Module Geometry module
*
*
*
* This module provides support for:
* - fixed-size homogeneous transformations
* - translation, scaling, 2D and 3D rotations
* - quaternions
* - \ref MatrixBase::cross() "cross product"
* - \ref MatrixBase::unitOrthogonal() "orthognal vector generation"
* - some linear components: parametrized-lines and hyperplanes
*
* \code
* #include <Eigen/Geometry>
* \endcode
*/
#include "src/Geometry/OrthoMethods.h"
#include "src/Geometry/EulerAngles.h"
#if EIGEN2_SUPPORT_STAGE > STAGE20_RESOLVE_API_CONFLICTS
#include "src/Geometry/Homogeneous.h"
#include "src/Geometry/RotationBase.h"
#include "src/Geometry/Rotation2D.h"
#include "src/Geometry/Quaternion.h"
#include "src/Geometry/AngleAxis.h"
#include "src/Geometry/Transform.h"
#include "src/Geometry/Translation.h"
#include "src/Geometry/Scaling.h"
#include "src/Geometry/Hyperplane.h"
#include "src/Geometry/ParametrizedLine.h"
#include "src/Geometry/AlignedBox.h"
#include "src/Geometry/Umeyama.h"
#if defined EIGEN_VECTORIZE_SSE
#include "src/Geometry/arch/Geometry_SSE.h"
#endif
#endif
#ifdef EIGEN2_SUPPORT
#include "src/Eigen2Support/Geometry/All.h"
#endif
#include "src/Core/util/ReenableStupidWarnings.h"
#endif // EIGEN_GEOMETRY_MODULE_H
/* vim: set filetype=cpp et sw=2 ts=2 ai: */

23
external/Eigen-3.2.7/Eigen/Householder vendored Normal file
View File

@ -0,0 +1,23 @@
#ifndef EIGEN_HOUSEHOLDER_MODULE_H
#define EIGEN_HOUSEHOLDER_MODULE_H
#include "Core"
#include "src/Core/util/DisableStupidWarnings.h"
/** \defgroup Householder_Module Householder module
* This module provides Householder transformations.
*
* \code
* #include <Eigen/Householder>
* \endcode
*/
#include "src/Householder/Householder.h"
#include "src/Householder/HouseholderSequence.h"
#include "src/Householder/BlockHouseholder.h"
#include "src/Core/util/ReenableStupidWarnings.h"
#endif // EIGEN_HOUSEHOLDER_MODULE_H
/* vim: set filetype=cpp et sw=2 ts=2 ai: */

View File

@ -0,0 +1,40 @@
#ifndef EIGEN_ITERATIVELINEARSOLVERS_MODULE_H
#define EIGEN_ITERATIVELINEARSOLVERS_MODULE_H
#include "SparseCore"
#include "OrderingMethods"
#include "src/Core/util/DisableStupidWarnings.h"
/**
* \defgroup IterativeLinearSolvers_Module IterativeLinearSolvers module
*
* This module currently provides iterative methods to solve problems of the form \c A \c x = \c b, where \c A is a squared matrix, usually very large and sparse.
* Those solvers are accessible via the following classes:
* - ConjugateGradient for selfadjoint (hermitian) matrices,
* - BiCGSTAB for general square matrices.
*
* These iterative solvers are associated with some preconditioners:
* - IdentityPreconditioner - not really useful
* - DiagonalPreconditioner - also called JAcobi preconditioner, work very well on diagonal dominant matrices.
* - IncompleteILUT - incomplete LU factorization with dual thresholding
*
* Such problems can also be solved using the direct sparse decomposition modules: SparseCholesky, CholmodSupport, UmfPackSupport, SuperLUSupport.
*
* \code
* #include <Eigen/IterativeLinearSolvers>
* \endcode
*/
#include "src/misc/Solve.h"
#include "src/misc/SparseSolve.h"
#include "src/IterativeLinearSolvers/IterativeSolverBase.h"
#include "src/IterativeLinearSolvers/BasicPreconditioners.h"
#include "src/IterativeLinearSolvers/ConjugateGradient.h"
#include "src/IterativeLinearSolvers/BiCGSTAB.h"
#include "src/IterativeLinearSolvers/IncompleteLUT.h"
#include "src/Core/util/ReenableStupidWarnings.h"
#endif // EIGEN_ITERATIVELINEARSOLVERS_MODULE_H

26
external/Eigen-3.2.7/Eigen/Jacobi vendored Normal file
View File

@ -0,0 +1,26 @@
#ifndef EIGEN_JACOBI_MODULE_H
#define EIGEN_JACOBI_MODULE_H
#include "Core"
#include "src/Core/util/DisableStupidWarnings.h"
/** \defgroup Jacobi_Module Jacobi module
* This module provides Jacobi and Givens rotations.
*
* \code
* #include <Eigen/Jacobi>
* \endcode
*
* In addition to listed classes, it defines the two following MatrixBase methods to apply a Jacobi or Givens rotation:
* - MatrixBase::applyOnTheLeft()
* - MatrixBase::applyOnTheRight().
*/
#include "src/Jacobi/Jacobi.h"
#include "src/Core/util/ReenableStupidWarnings.h"
#endif // EIGEN_JACOBI_MODULE_H
/* vim: set filetype=cpp et sw=2 ts=2 ai: */

41
external/Eigen-3.2.7/Eigen/LU vendored Normal file
View File

@ -0,0 +1,41 @@
#ifndef EIGEN_LU_MODULE_H
#define EIGEN_LU_MODULE_H
#include "Core"
#include "src/Core/util/DisableStupidWarnings.h"
/** \defgroup LU_Module LU module
* This module includes %LU decomposition and related notions such as matrix inversion and determinant.
* This module defines the following MatrixBase methods:
* - MatrixBase::inverse()
* - MatrixBase::determinant()
*
* \code
* #include <Eigen/LU>
* \endcode
*/
#include "src/misc/Solve.h"
#include "src/misc/Kernel.h"
#include "src/misc/Image.h"
#include "src/LU/FullPivLU.h"
#include "src/LU/PartialPivLU.h"
#ifdef EIGEN_USE_LAPACKE
#include "src/LU/PartialPivLU_MKL.h"
#endif
#include "src/LU/Determinant.h"
#include "src/LU/Inverse.h"
#if defined EIGEN_VECTORIZE_SSE
#include "src/LU/arch/Inverse_SSE.h"
#endif
#ifdef EIGEN2_SUPPORT
#include "src/Eigen2Support/LU.h"
#endif
#include "src/Core/util/ReenableStupidWarnings.h"
#endif // EIGEN_LU_MODULE_H
/* vim: set filetype=cpp et sw=2 ts=2 ai: */

32
external/Eigen-3.2.7/Eigen/LeastSquares vendored Normal file
View File

@ -0,0 +1,32 @@
#ifndef EIGEN_REGRESSION_MODULE_H
#define EIGEN_REGRESSION_MODULE_H
#ifndef EIGEN2_SUPPORT
#error LeastSquares is only available in Eigen2 support mode (define EIGEN2_SUPPORT)
#endif
// exclude from normal eigen3-only documentation
#ifdef EIGEN2_SUPPORT
#include "Core"
#include "src/Core/util/DisableStupidWarnings.h"
#include "Eigenvalues"
#include "Geometry"
/** \defgroup LeastSquares_Module LeastSquares module
* This module provides linear regression and related features.
*
* \code
* #include <Eigen/LeastSquares>
* \endcode
*/
#include "src/Eigen2Support/LeastSquares.h"
#include "src/Core/util/ReenableStupidWarnings.h"
#endif // EIGEN2_SUPPORT
#endif // EIGEN_REGRESSION_MODULE_H

28
external/Eigen-3.2.7/Eigen/MetisSupport vendored Normal file
View File

@ -0,0 +1,28 @@
#ifndef EIGEN_METISSUPPORT_MODULE_H
#define EIGEN_METISSUPPORT_MODULE_H
#include "SparseCore"
#include "src/Core/util/DisableStupidWarnings.h"
extern "C" {
#include <metis.h>
}
/** \ingroup Support_modules
* \defgroup MetisSupport_Module MetisSupport module
*
* \code
* #include <Eigen/MetisSupport>
* \endcode
* This module defines an interface to the METIS reordering package (http://glaros.dtc.umn.edu/gkhome/views/metis).
* It can be used just as any other built-in method as explained in \link OrderingMethods_Module here. \endlink
*/
#include "src/MetisSupport/MetisSupport.h"
#include "src/Core/util/ReenableStupidWarnings.h"
#endif // EIGEN_METISSUPPORT_MODULE_H

View File

@ -0,0 +1,66 @@
#ifndef EIGEN_ORDERINGMETHODS_MODULE_H
#define EIGEN_ORDERINGMETHODS_MODULE_H
#include "SparseCore"
#include "src/Core/util/DisableStupidWarnings.h"
/**
* \defgroup OrderingMethods_Module OrderingMethods module
*
* This module is currently for internal use only
*
* It defines various built-in and external ordering methods for sparse matrices.
* They are typically used to reduce the number of elements during
* the sparse matrix decomposition (LLT, LU, QR).
* Precisely, in a preprocessing step, a permutation matrix P is computed using
* those ordering methods and applied to the columns of the matrix.
* Using for instance the sparse Cholesky decomposition, it is expected that
* the nonzeros elements in LLT(A*P) will be much smaller than that in LLT(A).
*
*
* Usage :
* \code
* #include <Eigen/OrderingMethods>
* \endcode
*
* A simple usage is as a template parameter in the sparse decomposition classes :
*
* \code
* SparseLU<MatrixType, COLAMDOrdering<int> > solver;
* \endcode
*
* \code
* SparseQR<MatrixType, COLAMDOrdering<int> > solver;
* \endcode
*
* It is possible as well to call directly a particular ordering method for your own purpose,
* \code
* AMDOrdering<int> ordering;
* PermutationMatrix<Dynamic, Dynamic, int> perm;
* SparseMatrix<double> A;
* //Fill the matrix ...
*
* ordering(A, perm); // Call AMD
* \endcode
*
* \note Some of these methods (like AMD or METIS), need the sparsity pattern
* of the input matrix to be symmetric. When the matrix is structurally unsymmetric,
* Eigen computes internally the pattern of \f$A^T*A\f$ before calling the method.
* If your matrix is already symmetric (at leat in structure), you can avoid that
* by calling the method with a SelfAdjointView type.
*
* \code
* // Call the ordering on the pattern of the lower triangular matrix A
* ordering(A.selfadjointView<Lower>(), perm);
* \endcode
*/
#ifndef EIGEN_MPL2_ONLY
#include "src/OrderingMethods/Amd.h"
#endif
#include "src/OrderingMethods/Ordering.h"
#include "src/Core/util/ReenableStupidWarnings.h"
#endif // EIGEN_ORDERINGMETHODS_MODULE_H

View File

@ -0,0 +1,46 @@
#ifndef EIGEN_PASTIXSUPPORT_MODULE_H
#define EIGEN_PASTIXSUPPORT_MODULE_H
#include "SparseCore"
#include "src/Core/util/DisableStupidWarnings.h"
#include <complex.h>
extern "C" {
#include <pastix_nompi.h>
#include <pastix.h>
}
#ifdef complex
#undef complex
#endif
/** \ingroup Support_modules
* \defgroup PaStiXSupport_Module PaStiXSupport module
*
* This module provides an interface to the <a href="http://pastix.gforge.inria.fr/">PaSTiX</a> library.
* PaSTiX is a general \b supernodal, \b parallel and \b opensource sparse solver.
* It provides the two following main factorization classes:
* - class PastixLLT : a supernodal, parallel LLt Cholesky factorization.
* - class PastixLDLT: a supernodal, parallel LDLt Cholesky factorization.
* - class PastixLU : a supernodal, parallel LU factorization (optimized for a symmetric pattern).
*
* \code
* #include <Eigen/PaStiXSupport>
* \endcode
*
* In order to use this module, the PaSTiX headers must be accessible from the include paths, and your binary must be linked to the PaSTiX library and its dependencies.
* The dependencies depend on how PaSTiX has been compiled.
* For a cmake based project, you can use our FindPaSTiX.cmake module to help you in this task.
*
*/
#include "src/misc/Solve.h"
#include "src/misc/SparseSolve.h"
#include "src/PaStiXSupport/PaStiXSupport.h"
#include "src/Core/util/ReenableStupidWarnings.h"
#endif // EIGEN_PASTIXSUPPORT_MODULE_H

View File

@ -0,0 +1,30 @@
#ifndef EIGEN_PARDISOSUPPORT_MODULE_H
#define EIGEN_PARDISOSUPPORT_MODULE_H
#include "SparseCore"
#include "src/Core/util/DisableStupidWarnings.h"
#include <mkl_pardiso.h>
#include <unsupported/Eigen/SparseExtra>
/** \ingroup Support_modules
* \defgroup PardisoSupport_Module PardisoSupport module
*
* This module brings support for the Intel(R) MKL PARDISO direct sparse solvers.
*
* \code
* #include <Eigen/PardisoSupport>
* \endcode
*
* In order to use this module, the MKL headers must be accessible from the include paths, and your binary must be linked to the MKL library and its dependencies.
* See this \ref TopicUsingIntelMKL "page" for more information on MKL-Eigen integration.
*
*/
#include "src/PardisoSupport/PardisoSupport.h"
#include "src/Core/util/ReenableStupidWarnings.h"
#endif // EIGEN_PARDISOSUPPORT_MODULE_H

45
external/Eigen-3.2.7/Eigen/QR vendored Normal file
View File

@ -0,0 +1,45 @@
#ifndef EIGEN_QR_MODULE_H
#define EIGEN_QR_MODULE_H
#include "Core"
#include "src/Core/util/DisableStupidWarnings.h"
#include "Cholesky"
#include "Jacobi"
#include "Householder"
/** \defgroup QR_Module QR module
*
*
*
* This module provides various QR decompositions
* This module also provides some MatrixBase methods, including:
* - MatrixBase::qr(),
*
* \code
* #include <Eigen/QR>
* \endcode
*/
#include "src/misc/Solve.h"
#include "src/QR/HouseholderQR.h"
#include "src/QR/FullPivHouseholderQR.h"
#include "src/QR/ColPivHouseholderQR.h"
#ifdef EIGEN_USE_LAPACKE
#include "src/QR/HouseholderQR_MKL.h"
#include "src/QR/ColPivHouseholderQR_MKL.h"
#endif
#ifdef EIGEN2_SUPPORT
#include "src/Eigen2Support/QR.h"
#endif
#include "src/Core/util/ReenableStupidWarnings.h"
#ifdef EIGEN2_SUPPORT
#include "Eigenvalues"
#endif
#endif // EIGEN_QR_MODULE_H
/* vim: set filetype=cpp et sw=2 ts=2 ai: */

View File

@ -0,0 +1,34 @@
#ifndef EIGEN_QTMALLOC_MODULE_H
#define EIGEN_QTMALLOC_MODULE_H
#include "Core"
#if (!EIGEN_MALLOC_ALREADY_ALIGNED)
#include "src/Core/util/DisableStupidWarnings.h"
void *qMalloc(size_t size)
{
return Eigen::internal::aligned_malloc(size);
}
void qFree(void *ptr)
{
Eigen::internal::aligned_free(ptr);
}
void *qRealloc(void *ptr, size_t size)
{
void* newPtr = Eigen::internal::aligned_malloc(size);
memcpy(newPtr, ptr, size);
Eigen::internal::aligned_free(ptr);
return newPtr;
}
#include "src/Core/util/ReenableStupidWarnings.h"
#endif
#endif // EIGEN_QTMALLOC_MODULE_H
/* vim: set filetype=cpp et sw=2 ts=2 ai: */

29
external/Eigen-3.2.7/Eigen/SPQRSupport vendored Normal file
View File

@ -0,0 +1,29 @@
#ifndef EIGEN_SPQRSUPPORT_MODULE_H
#define EIGEN_SPQRSUPPORT_MODULE_H
#include "SparseCore"
#include "src/Core/util/DisableStupidWarnings.h"
#include "SuiteSparseQR.hpp"
/** \ingroup Support_modules
* \defgroup SPQRSupport_Module SuiteSparseQR module
*
* This module provides an interface to the SPQR library, which is part of the <a href="http://www.cise.ufl.edu/research/sparse/SuiteSparse/">suitesparse</a> package.
*
* \code
* #include <Eigen/SPQRSupport>
* \endcode
*
* In order to use this module, the SPQR headers must be accessible from the include paths, and your binary must be linked to the SPQR library and its dependencies (Cholmod, AMD, COLAMD,...).
* For a cmake based project, you can use our FindSPQR.cmake and FindCholmod.Cmake modules
*
*/
#include "src/misc/Solve.h"
#include "src/misc/SparseSolve.h"
#include "src/CholmodSupport/CholmodSupport.h"
#include "src/SPQRSupport/SuiteSparseQRSupport.h"
#endif

37
external/Eigen-3.2.7/Eigen/SVD vendored Normal file
View File

@ -0,0 +1,37 @@
#ifndef EIGEN_SVD_MODULE_H
#define EIGEN_SVD_MODULE_H
#include "QR"
#include "Householder"
#include "Jacobi"
#include "src/Core/util/DisableStupidWarnings.h"
/** \defgroup SVD_Module SVD module
*
*
*
* This module provides SVD decomposition for matrices (both real and complex).
* This decomposition is accessible via the following MatrixBase method:
* - MatrixBase::jacobiSvd()
*
* \code
* #include <Eigen/SVD>
* \endcode
*/
#include "src/misc/Solve.h"
#include "src/SVD/JacobiSVD.h"
#if defined(EIGEN_USE_LAPACKE) && !defined(EIGEN_USE_LAPACKE_STRICT)
#include "src/SVD/JacobiSVD_MKL.h"
#endif
#include "src/SVD/UpperBidiagonalization.h"
#ifdef EIGEN2_SUPPORT
#include "src/Eigen2Support/SVD.h"
#endif
#include "src/Core/util/ReenableStupidWarnings.h"
#endif // EIGEN_SVD_MODULE_H
/* vim: set filetype=cpp et sw=2 ts=2 ai: */

27
external/Eigen-3.2.7/Eigen/Sparse vendored Normal file
View File

@ -0,0 +1,27 @@
#ifndef EIGEN_SPARSE_MODULE_H
#define EIGEN_SPARSE_MODULE_H
/** \defgroup Sparse_Module Sparse meta-module
*
* Meta-module including all related modules:
* - \ref SparseCore_Module
* - \ref OrderingMethods_Module
* - \ref SparseCholesky_Module
* - \ref SparseLU_Module
* - \ref SparseQR_Module
* - \ref IterativeLinearSolvers_Module
*
* \code
* #include <Eigen/Sparse>
* \endcode
*/
#include "SparseCore"
#include "OrderingMethods"
#include "SparseCholesky"
#include "SparseLU"
#include "SparseQR"
#include "IterativeLinearSolvers"
#endif // EIGEN_SPARSE_MODULE_H

View File

@ -0,0 +1,47 @@
// This file is part of Eigen, a lightweight C++ template library
// for linear algebra.
//
// Copyright (C) 2008-2013 Gael Guennebaud <gael.guennebaud@inria.fr>
//
// This Source Code Form is subject to the terms of the Mozilla
// Public License v. 2.0. If a copy of the MPL was not distributed
// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
#ifndef EIGEN_SPARSECHOLESKY_MODULE_H
#define EIGEN_SPARSECHOLESKY_MODULE_H
#include "SparseCore"
#include "OrderingMethods"
#include "src/Core/util/DisableStupidWarnings.h"
/**
* \defgroup SparseCholesky_Module SparseCholesky module
*
* This module currently provides two variants of the direct sparse Cholesky decomposition for selfadjoint (hermitian) matrices.
* Those decompositions are accessible via the following classes:
* - SimplicialLLt,
* - SimplicialLDLt
*
* Such problems can also be solved using the ConjugateGradient solver from the IterativeLinearSolvers module.
*
* \code
* #include <Eigen/SparseCholesky>
* \endcode
*/
#ifdef EIGEN_MPL2_ONLY
#error The SparseCholesky module has nothing to offer in MPL2 only mode
#endif
#include "src/misc/Solve.h"
#include "src/misc/SparseSolve.h"
#include "src/SparseCholesky/SimplicialCholesky.h"
#ifndef EIGEN_MPL2_ONLY
#include "src/SparseCholesky/SimplicialCholesky_impl.h"
#endif
#include "src/Core/util/ReenableStupidWarnings.h"
#endif // EIGEN_SPARSECHOLESKY_MODULE_H

64
external/Eigen-3.2.7/Eigen/SparseCore vendored Normal file
View File

@ -0,0 +1,64 @@
#ifndef EIGEN_SPARSECORE_MODULE_H
#define EIGEN_SPARSECORE_MODULE_H
#include "Core"
#include "src/Core/util/DisableStupidWarnings.h"
#include <vector>
#include <map>
#include <cstdlib>
#include <cstring>
#include <algorithm>
/**
* \defgroup SparseCore_Module SparseCore module
*
* This module provides a sparse matrix representation, and basic associated matrix manipulations
* and operations.
*
* See the \ref TutorialSparse "Sparse tutorial"
*
* \code
* #include <Eigen/SparseCore>
* \endcode
*
* This module depends on: Core.
*/
namespace Eigen {
/** The type used to identify a general sparse storage. */
struct Sparse {};
}
#include "src/SparseCore/SparseUtil.h"
#include "src/SparseCore/SparseMatrixBase.h"
#include "src/SparseCore/CompressedStorage.h"
#include "src/SparseCore/AmbiVector.h"
#include "src/SparseCore/SparseMatrix.h"
#include "src/SparseCore/MappedSparseMatrix.h"
#include "src/SparseCore/SparseVector.h"
#include "src/SparseCore/SparseBlock.h"
#include "src/SparseCore/SparseTranspose.h"
#include "src/SparseCore/SparseCwiseUnaryOp.h"
#include "src/SparseCore/SparseCwiseBinaryOp.h"
#include "src/SparseCore/SparseDot.h"
#include "src/SparseCore/SparsePermutation.h"
#include "src/SparseCore/SparseRedux.h"
#include "src/SparseCore/SparseFuzzy.h"
#include "src/SparseCore/ConservativeSparseSparseProduct.h"
#include "src/SparseCore/SparseSparseProductWithPruning.h"
#include "src/SparseCore/SparseProduct.h"
#include "src/SparseCore/SparseDenseProduct.h"
#include "src/SparseCore/SparseDiagonalProduct.h"
#include "src/SparseCore/SparseTriangularView.h"
#include "src/SparseCore/SparseSelfAdjointView.h"
#include "src/SparseCore/TriangularSolver.h"
#include "src/SparseCore/SparseView.h"
#include "src/Core/util/ReenableStupidWarnings.h"
#endif // EIGEN_SPARSECORE_MODULE_H

49
external/Eigen-3.2.7/Eigen/SparseLU vendored Normal file
View File

@ -0,0 +1,49 @@
// This file is part of Eigen, a lightweight C++ template library
// for linear algebra.
//
// Copyright (C) 2012 Désiré Nuentsa-Wakam <desire.nuentsa_wakam@inria.fr>
// Copyright (C) 2012 Gael Guennebaud <gael.guennebaud@inria.fr>
//
// This Source Code Form is subject to the terms of the Mozilla
// Public License v. 2.0. If a copy of the MPL was not distributed
// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
#ifndef EIGEN_SPARSELU_MODULE_H
#define EIGEN_SPARSELU_MODULE_H
#include "SparseCore"
/**
* \defgroup SparseLU_Module SparseLU module
* This module defines a supernodal factorization of general sparse matrices.
* The code is fully optimized for supernode-panel updates with specialized kernels.
* Please, see the documentation of the SparseLU class for more details.
*/
#include "src/misc/Solve.h"
#include "src/misc/SparseSolve.h"
// Ordering interface
#include "OrderingMethods"
#include "src/SparseLU/SparseLU_gemm_kernel.h"
#include "src/SparseLU/SparseLU_Structs.h"
#include "src/SparseLU/SparseLU_SupernodalMatrix.h"
#include "src/SparseLU/SparseLUImpl.h"
#include "src/SparseCore/SparseColEtree.h"
#include "src/SparseLU/SparseLU_Memory.h"
#include "src/SparseLU/SparseLU_heap_relax_snode.h"
#include "src/SparseLU/SparseLU_relax_snode.h"
#include "src/SparseLU/SparseLU_pivotL.h"
#include "src/SparseLU/SparseLU_panel_dfs.h"
#include "src/SparseLU/SparseLU_kernel_bmod.h"
#include "src/SparseLU/SparseLU_panel_bmod.h"
#include "src/SparseLU/SparseLU_column_dfs.h"
#include "src/SparseLU/SparseLU_column_bmod.h"
#include "src/SparseLU/SparseLU_copy_to_ucol.h"
#include "src/SparseLU/SparseLU_pruneL.h"
#include "src/SparseLU/SparseLU_Utils.h"
#include "src/SparseLU/SparseLU.h"
#endif // EIGEN_SPARSELU_MODULE_H

33
external/Eigen-3.2.7/Eigen/SparseQR vendored Normal file
View File

@ -0,0 +1,33 @@
#ifndef EIGEN_SPARSEQR_MODULE_H
#define EIGEN_SPARSEQR_MODULE_H
#include "SparseCore"
#include "OrderingMethods"
#include "src/Core/util/DisableStupidWarnings.h"
/** \defgroup SparseQR_Module SparseQR module
* \brief Provides QR decomposition for sparse matrices
*
* This module provides a simplicial version of the left-looking Sparse QR decomposition.
* The columns of the input matrix should be reordered to limit the fill-in during the
* decomposition. Built-in methods (COLAMD, AMD) or external methods (METIS) can be used to this end.
* See the \link OrderingMethods_Module OrderingMethods\endlink module for the list
* of built-in and external ordering methods.
*
* \code
* #include <Eigen/SparseQR>
* \endcode
*
*
*/
#include "src/misc/Solve.h"
#include "src/misc/SparseSolve.h"
#include "OrderingMethods"
#include "src/SparseCore/SparseColEtree.h"
#include "src/SparseQR/SparseQR.h"
#include "src/Core/util/ReenableStupidWarnings.h"
#endif

27
external/Eigen-3.2.7/Eigen/StdDeque vendored Normal file
View File

@ -0,0 +1,27 @@
// This file is part of Eigen, a lightweight C++ template library
// for linear algebra.
//
// Copyright (C) 2009 Gael Guennebaud <gael.guennebaud@inria.fr>
// Copyright (C) 2009 Hauke Heibel <hauke.heibel@googlemail.com>
//
// This Source Code Form is subject to the terms of the Mozilla
// Public License v. 2.0. If a copy of the MPL was not distributed
// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
#ifndef EIGEN_STDDEQUE_MODULE_H
#define EIGEN_STDDEQUE_MODULE_H
#include "Core"
#include <deque>
#if (defined(_MSC_VER) && defined(_WIN64)) /* MSVC auto aligns in 64 bit builds */
#define EIGEN_DEFINE_STL_DEQUE_SPECIALIZATION(...)
#else
#include "src/StlSupport/StdDeque.h"
#endif
#endif // EIGEN_STDDEQUE_MODULE_H

26
external/Eigen-3.2.7/Eigen/StdList vendored Normal file
View File

@ -0,0 +1,26 @@
// This file is part of Eigen, a lightweight C++ template library
// for linear algebra.
//
// Copyright (C) 2009 Hauke Heibel <hauke.heibel@googlemail.com>
//
// This Source Code Form is subject to the terms of the Mozilla
// Public License v. 2.0. If a copy of the MPL was not distributed
// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
#ifndef EIGEN_STDLIST_MODULE_H
#define EIGEN_STDLIST_MODULE_H
#include "Core"
#include <list>
#if (defined(_MSC_VER) && defined(_WIN64)) /* MSVC auto aligns in 64 bit builds */
#define EIGEN_DEFINE_STL_LIST_SPECIALIZATION(...)
#else
#include "src/StlSupport/StdList.h"
#endif
#endif // EIGEN_STDLIST_MODULE_H

27
external/Eigen-3.2.7/Eigen/StdVector vendored Normal file
View File

@ -0,0 +1,27 @@
// This file is part of Eigen, a lightweight C++ template library
// for linear algebra.
//
// Copyright (C) 2009 Gael Guennebaud <gael.guennebaud@inria.fr>
// Copyright (C) 2009 Hauke Heibel <hauke.heibel@googlemail.com>
//
// This Source Code Form is subject to the terms of the Mozilla
// Public License v. 2.0. If a copy of the MPL was not distributed
// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
#ifndef EIGEN_STDVECTOR_MODULE_H
#define EIGEN_STDVECTOR_MODULE_H
#include "Core"
#include <vector>
#if (defined(_MSC_VER) && defined(_WIN64)) /* MSVC auto aligns in 64 bit builds */
#define EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(...)
#else
#include "src/StlSupport/StdVector.h"
#endif
#endif // EIGEN_STDVECTOR_MODULE_H

View File

@ -0,0 +1,59 @@
#ifndef EIGEN_SUPERLUSUPPORT_MODULE_H
#define EIGEN_SUPERLUSUPPORT_MODULE_H
#include "SparseCore"
#include "src/Core/util/DisableStupidWarnings.h"
#ifdef EMPTY
#define EIGEN_EMPTY_WAS_ALREADY_DEFINED
#endif
typedef int int_t;
#include <slu_Cnames.h>
#include <supermatrix.h>
#include <slu_util.h>
// slu_util.h defines a preprocessor token named EMPTY which is really polluting,
// so we remove it in favor of a SUPERLU_EMPTY token.
// If EMPTY was already defined then we don't undef it.
#if defined(EIGEN_EMPTY_WAS_ALREADY_DEFINED)
# undef EIGEN_EMPTY_WAS_ALREADY_DEFINED
#elif defined(EMPTY)
# undef EMPTY
#endif
#define SUPERLU_EMPTY (-1)
namespace Eigen { struct SluMatrix; }
/** \ingroup Support_modules
* \defgroup SuperLUSupport_Module SuperLUSupport module
*
* This module provides an interface to the <a href="http://crd-legacy.lbl.gov/~xiaoye/SuperLU/">SuperLU</a> library.
* It provides the following factorization class:
* - class SuperLU: a supernodal sequential LU factorization.
* - class SuperILU: a supernodal sequential incomplete LU factorization (to be used as a preconditioner for iterative methods).
*
* \warning When including this module, you have to use SUPERLU_EMPTY instead of EMPTY which is no longer defined because it is too polluting.
*
* \code
* #include <Eigen/SuperLUSupport>
* \endcode
*
* In order to use this module, the superlu headers must be accessible from the include paths, and your binary must be linked to the superlu library and its dependencies.
* The dependencies depend on how superlu has been compiled.
* For a cmake based project, you can use our FindSuperLU.cmake module to help you in this task.
*
*/
#include "src/misc/Solve.h"
#include "src/misc/SparseSolve.h"
#include "src/SuperLUSupport/SuperLUSupport.h"
#include "src/Core/util/ReenableStupidWarnings.h"
#endif // EIGEN_SUPERLUSUPPORT_MODULE_H

View File

@ -0,0 +1,36 @@
#ifndef EIGEN_UMFPACKSUPPORT_MODULE_H
#define EIGEN_UMFPACKSUPPORT_MODULE_H
#include "SparseCore"
#include "src/Core/util/DisableStupidWarnings.h"
extern "C" {
#include <umfpack.h>
}
/** \ingroup Support_modules
* \defgroup UmfPackSupport_Module UmfPackSupport module
*
* This module provides an interface to the UmfPack library which is part of the <a href="http://www.cise.ufl.edu/research/sparse/SuiteSparse/">suitesparse</a> package.
* It provides the following factorization class:
* - class UmfPackLU: a multifrontal sequential LU factorization.
*
* \code
* #include <Eigen/UmfPackSupport>
* \endcode
*
* In order to use this module, the umfpack headers must be accessible from the include paths, and your binary must be linked to the umfpack library and its dependencies.
* The dependencies depend on how umfpack has been compiled.
* For a cmake based project, you can use our FindUmfPack.cmake module to help you in this task.
*
*/
#include "src/misc/Solve.h"
#include "src/misc/SparseSolve.h"
#include "src/UmfPackSupport/UmfPackSupport.h"
#include "src/Core/util/ReenableStupidWarnings.h"
#endif // EIGEN_UMFPACKSUPPORT_MODULE_H

View File

@ -0,0 +1,7 @@
file(GLOB Eigen_src_subdirectories "*")
escape_string_as_regex(ESCAPED_CMAKE_CURRENT_SOURCE_DIR "${CMAKE_CURRENT_SOURCE_DIR}")
foreach(f ${Eigen_src_subdirectories})
if(NOT f MATCHES "\\.txt" AND NOT f MATCHES "${ESCAPED_CMAKE_CURRENT_SOURCE_DIR}/[.].+" )
add_subdirectory(${f})
endif()
endforeach()

View File

@ -0,0 +1,6 @@
FILE(GLOB Eigen_Cholesky_SRCS "*.h")
INSTALL(FILES
${Eigen_Cholesky_SRCS}
DESTINATION ${INCLUDE_INSTALL_DIR}/Eigen/src/Cholesky COMPONENT Devel
)

View File

@ -0,0 +1,611 @@
// This file is part of Eigen, a lightweight C++ template library
// for linear algebra.
//
// Copyright (C) 2008-2011 Gael Guennebaud <gael.guennebaud@inria.fr>
// Copyright (C) 2009 Keir Mierle <mierle@gmail.com>
// Copyright (C) 2009 Benoit Jacob <jacob.benoit.1@gmail.com>
// Copyright (C) 2011 Timothy E. Holy <tim.holy@gmail.com >
//
// This Source Code Form is subject to the terms of the Mozilla
// Public License v. 2.0. If a copy of the MPL was not distributed
// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
#ifndef EIGEN_LDLT_H
#define EIGEN_LDLT_H
namespace Eigen {
namespace internal {
template<typename MatrixType, int UpLo> struct LDLT_Traits;
// PositiveSemiDef means positive semi-definite and non-zero; same for NegativeSemiDef
enum SignMatrix { PositiveSemiDef, NegativeSemiDef, ZeroSign, Indefinite };
}
/** \ingroup Cholesky_Module
*
* \class LDLT
*
* \brief Robust Cholesky decomposition of a matrix with pivoting
*
* \param MatrixType the type of the matrix of which to compute the LDL^T Cholesky decomposition
* \param UpLo the triangular part that will be used for the decompositon: Lower (default) or Upper.
* The other triangular part won't be read.
*
* Perform a robust Cholesky decomposition of a positive semidefinite or negative semidefinite
* matrix \f$ A \f$ such that \f$ A = P^TLDL^*P \f$, where P is a permutation matrix, L
* is lower triangular with a unit diagonal and D is a diagonal matrix.
*
* The decomposition uses pivoting to ensure stability, so that L will have
* zeros in the bottom right rank(A) - n submatrix. Avoiding the square root
* on D also stabilizes the computation.
*
* Remember that Cholesky decompositions are not rank-revealing. Also, do not use a Cholesky
* decomposition to determine whether a system of equations has a solution.
*
* \sa MatrixBase::ldlt(), class LLT
*/
template<typename _MatrixType, int _UpLo> class LDLT
{
public:
typedef _MatrixType MatrixType;
enum {
RowsAtCompileTime = MatrixType::RowsAtCompileTime,
ColsAtCompileTime = MatrixType::ColsAtCompileTime,
Options = MatrixType::Options & ~RowMajorBit, // these are the options for the TmpMatrixType, we need a ColMajor matrix here!
MaxRowsAtCompileTime = MatrixType::MaxRowsAtCompileTime,
MaxColsAtCompileTime = MatrixType::MaxColsAtCompileTime,
UpLo = _UpLo
};
typedef typename MatrixType::Scalar Scalar;
typedef typename NumTraits<typename MatrixType::Scalar>::Real RealScalar;
typedef typename MatrixType::Index Index;
typedef Matrix<Scalar, RowsAtCompileTime, 1, Options, MaxRowsAtCompileTime, 1> TmpMatrixType;
typedef Transpositions<RowsAtCompileTime, MaxRowsAtCompileTime> TranspositionType;
typedef PermutationMatrix<RowsAtCompileTime, MaxRowsAtCompileTime> PermutationType;
typedef internal::LDLT_Traits<MatrixType,UpLo> Traits;
/** \brief Default Constructor.
*
* The default constructor is useful in cases in which the user intends to
* perform decompositions via LDLT::compute(const MatrixType&).
*/
LDLT()
: m_matrix(),
m_transpositions(),
m_sign(internal::ZeroSign),
m_isInitialized(false)
{}
/** \brief Default Constructor with memory preallocation
*
* Like the default constructor but with preallocation of the internal data
* according to the specified problem \a size.
* \sa LDLT()
*/
LDLT(Index size)
: m_matrix(size, size),
m_transpositions(size),
m_temporary(size),
m_sign(internal::ZeroSign),
m_isInitialized(false)
{}
/** \brief Constructor with decomposition
*
* This calculates the decomposition for the input \a matrix.
* \sa LDLT(Index size)
*/
LDLT(const MatrixType& matrix)
: m_matrix(matrix.rows(), matrix.cols()),
m_transpositions(matrix.rows()),
m_temporary(matrix.rows()),
m_sign(internal::ZeroSign),
m_isInitialized(false)
{
compute(matrix);
}
/** Clear any existing decomposition
* \sa rankUpdate(w,sigma)
*/
void setZero()
{
m_isInitialized = false;
}
/** \returns a view of the upper triangular matrix U */
inline typename Traits::MatrixU matrixU() const
{
eigen_assert(m_isInitialized && "LDLT is not initialized.");
return Traits::getU(m_matrix);
}
/** \returns a view of the lower triangular matrix L */
inline typename Traits::MatrixL matrixL() const
{
eigen_assert(m_isInitialized && "LDLT is not initialized.");
return Traits::getL(m_matrix);
}
/** \returns the permutation matrix P as a transposition sequence.
*/
inline const TranspositionType& transpositionsP() const
{
eigen_assert(m_isInitialized && "LDLT is not initialized.");
return m_transpositions;
}
/** \returns the coefficients of the diagonal matrix D */
inline Diagonal<const MatrixType> vectorD() const
{
eigen_assert(m_isInitialized && "LDLT is not initialized.");
return m_matrix.diagonal();
}
/** \returns true if the matrix is positive (semidefinite) */
inline bool isPositive() const
{
eigen_assert(m_isInitialized && "LDLT is not initialized.");
return m_sign == internal::PositiveSemiDef || m_sign == internal::ZeroSign;
}
#ifdef EIGEN2_SUPPORT
inline bool isPositiveDefinite() const
{
return isPositive();
}
#endif
/** \returns true if the matrix is negative (semidefinite) */
inline bool isNegative(void) const
{
eigen_assert(m_isInitialized && "LDLT is not initialized.");
return m_sign == internal::NegativeSemiDef || m_sign == internal::ZeroSign;
}
/** \returns a solution x of \f$ A x = b \f$ using the current decomposition of A.
*
* This function also supports in-place solves using the syntax <tt>x = decompositionObject.solve(x)</tt> .
*
* \note_about_checking_solutions
*
* More precisely, this method solves \f$ A x = b \f$ using the decomposition \f$ A = P^T L D L^* P \f$
* by solving the systems \f$ P^T y_1 = b \f$, \f$ L y_2 = y_1 \f$, \f$ D y_3 = y_2 \f$,
* \f$ L^* y_4 = y_3 \f$ and \f$ P x = y_4 \f$ in succession. If the matrix \f$ A \f$ is singular, then
* \f$ D \f$ will also be singular (all the other matrices are invertible). In that case, the
* least-square solution of \f$ D y_3 = y_2 \f$ is computed. This does not mean that this function
* computes the least-square solution of \f$ A x = b \f$ is \f$ A \f$ is singular.
*
* \sa MatrixBase::ldlt()
*/
template<typename Rhs>
inline const internal::solve_retval<LDLT, Rhs>
solve(const MatrixBase<Rhs>& b) const
{
eigen_assert(m_isInitialized && "LDLT is not initialized.");
eigen_assert(m_matrix.rows()==b.rows()
&& "LDLT::solve(): invalid number of rows of the right hand side matrix b");
return internal::solve_retval<LDLT, Rhs>(*this, b.derived());
}
#ifdef EIGEN2_SUPPORT
template<typename OtherDerived, typename ResultType>
bool solve(const MatrixBase<OtherDerived>& b, ResultType *result) const
{
*result = this->solve(b);
return true;
}
#endif
template<typename Derived>
bool solveInPlace(MatrixBase<Derived> &bAndX) const;
LDLT& compute(const MatrixType& matrix);
template <typename Derived>
LDLT& rankUpdate(const MatrixBase<Derived>& w, const RealScalar& alpha=1);
/** \returns the internal LDLT decomposition matrix
*
* TODO: document the storage layout
*/
inline const MatrixType& matrixLDLT() const
{
eigen_assert(m_isInitialized && "LDLT is not initialized.");
return m_matrix;
}
MatrixType reconstructedMatrix() const;
inline Index rows() const { return m_matrix.rows(); }
inline Index cols() const { return m_matrix.cols(); }
/** \brief Reports whether previous computation was successful.
*
* \returns \c Success if computation was succesful,
* \c NumericalIssue if the matrix.appears to be negative.
*/
ComputationInfo info() const
{
eigen_assert(m_isInitialized && "LDLT is not initialized.");
return Success;
}
protected:
static void check_template_parameters()
{
EIGEN_STATIC_ASSERT_NON_INTEGER(Scalar);
}
/** \internal
* Used to compute and store the Cholesky decomposition A = L D L^* = U^* D U.
* The strict upper part is used during the decomposition, the strict lower
* part correspond to the coefficients of L (its diagonal is equal to 1 and
* is not stored), and the diagonal entries correspond to D.
*/
MatrixType m_matrix;
TranspositionType m_transpositions;
TmpMatrixType m_temporary;
internal::SignMatrix m_sign;
bool m_isInitialized;
};
namespace internal {
template<int UpLo> struct ldlt_inplace;
template<> struct ldlt_inplace<Lower>
{
template<typename MatrixType, typename TranspositionType, typename Workspace>
static bool unblocked(MatrixType& mat, TranspositionType& transpositions, Workspace& temp, SignMatrix& sign)
{
using std::abs;
typedef typename MatrixType::Scalar Scalar;
typedef typename MatrixType::RealScalar RealScalar;
typedef typename MatrixType::Index Index;
eigen_assert(mat.rows()==mat.cols());
const Index size = mat.rows();
if (size <= 1)
{
transpositions.setIdentity();
if (numext::real(mat.coeff(0,0)) > 0) sign = PositiveSemiDef;
else if (numext::real(mat.coeff(0,0)) < 0) sign = NegativeSemiDef;
else sign = ZeroSign;
return true;
}
for (Index k = 0; k < size; ++k)
{
// Find largest diagonal element
Index index_of_biggest_in_corner;
mat.diagonal().tail(size-k).cwiseAbs().maxCoeff(&index_of_biggest_in_corner);
index_of_biggest_in_corner += k;
transpositions.coeffRef(k) = index_of_biggest_in_corner;
if(k != index_of_biggest_in_corner)
{
// apply the transposition while taking care to consider only
// the lower triangular part
Index s = size-index_of_biggest_in_corner-1; // trailing size after the biggest element
mat.row(k).head(k).swap(mat.row(index_of_biggest_in_corner).head(k));
mat.col(k).tail(s).swap(mat.col(index_of_biggest_in_corner).tail(s));
std::swap(mat.coeffRef(k,k),mat.coeffRef(index_of_biggest_in_corner,index_of_biggest_in_corner));
for(int i=k+1;i<index_of_biggest_in_corner;++i)
{
Scalar tmp = mat.coeffRef(i,k);
mat.coeffRef(i,k) = numext::conj(mat.coeffRef(index_of_biggest_in_corner,i));
mat.coeffRef(index_of_biggest_in_corner,i) = numext::conj(tmp);
}
if(NumTraits<Scalar>::IsComplex)
mat.coeffRef(index_of_biggest_in_corner,k) = numext::conj(mat.coeff(index_of_biggest_in_corner,k));
}
// partition the matrix:
// A00 | - | -
// lu = A10 | A11 | -
// A20 | A21 | A22
Index rs = size - k - 1;
Block<MatrixType,Dynamic,1> A21(mat,k+1,k,rs,1);
Block<MatrixType,1,Dynamic> A10(mat,k,0,1,k);
Block<MatrixType,Dynamic,Dynamic> A20(mat,k+1,0,rs,k);
if(k>0)
{
temp.head(k) = mat.diagonal().real().head(k).asDiagonal() * A10.adjoint();
mat.coeffRef(k,k) -= (A10 * temp.head(k)).value();
if(rs>0)
A21.noalias() -= A20 * temp.head(k);
}
// In some previous versions of Eigen (e.g., 3.2.1), the scaling was omitted if the pivot
// was smaller than the cutoff value. However, soince LDLT is not rank-revealing
// we should only make sure we do not introduce INF or NaN values.
// LAPACK also uses 0 as the cutoff value.
RealScalar realAkk = numext::real(mat.coeffRef(k,k));
if((rs>0) && (abs(realAkk) > RealScalar(0)))
A21 /= realAkk;
if (sign == PositiveSemiDef) {
if (realAkk < 0) sign = Indefinite;
} else if (sign == NegativeSemiDef) {
if (realAkk > 0) sign = Indefinite;
} else if (sign == ZeroSign) {
if (realAkk > 0) sign = PositiveSemiDef;
else if (realAkk < 0) sign = NegativeSemiDef;
}
}
return true;
}
// Reference for the algorithm: Davis and Hager, "Multiple Rank
// Modifications of a Sparse Cholesky Factorization" (Algorithm 1)
// Trivial rearrangements of their computations (Timothy E. Holy)
// allow their algorithm to work for rank-1 updates even if the
// original matrix is not of full rank.
// Here only rank-1 updates are implemented, to reduce the
// requirement for intermediate storage and improve accuracy
template<typename MatrixType, typename WDerived>
static bool updateInPlace(MatrixType& mat, MatrixBase<WDerived>& w, const typename MatrixType::RealScalar& sigma=1)
{
using numext::isfinite;
typedef typename MatrixType::Scalar Scalar;
typedef typename MatrixType::RealScalar RealScalar;
typedef typename MatrixType::Index Index;
const Index size = mat.rows();
eigen_assert(mat.cols() == size && w.size()==size);
RealScalar alpha = 1;
// Apply the update
for (Index j = 0; j < size; j++)
{
// Check for termination due to an original decomposition of low-rank
if (!(isfinite)(alpha))
break;
// Update the diagonal terms
RealScalar dj = numext::real(mat.coeff(j,j));
Scalar wj = w.coeff(j);
RealScalar swj2 = sigma*numext::abs2(wj);
RealScalar gamma = dj*alpha + swj2;
mat.coeffRef(j,j) += swj2/alpha;
alpha += swj2/dj;
// Update the terms of L
Index rs = size-j-1;
w.tail(rs) -= wj * mat.col(j).tail(rs);
if(gamma != 0)
mat.col(j).tail(rs) += (sigma*numext::conj(wj)/gamma)*w.tail(rs);
}
return true;
}
template<typename MatrixType, typename TranspositionType, typename Workspace, typename WType>
static bool update(MatrixType& mat, const TranspositionType& transpositions, Workspace& tmp, const WType& w, const typename MatrixType::RealScalar& sigma=1)
{
// Apply the permutation to the input w
tmp = transpositions * w;
return ldlt_inplace<Lower>::updateInPlace(mat,tmp,sigma);
}
};
template<> struct ldlt_inplace<Upper>
{
template<typename MatrixType, typename TranspositionType, typename Workspace>
static EIGEN_STRONG_INLINE bool unblocked(MatrixType& mat, TranspositionType& transpositions, Workspace& temp, SignMatrix& sign)
{
Transpose<MatrixType> matt(mat);
return ldlt_inplace<Lower>::unblocked(matt, transpositions, temp, sign);
}
template<typename MatrixType, typename TranspositionType, typename Workspace, typename WType>
static EIGEN_STRONG_INLINE bool update(MatrixType& mat, TranspositionType& transpositions, Workspace& tmp, WType& w, const typename MatrixType::RealScalar& sigma=1)
{
Transpose<MatrixType> matt(mat);
return ldlt_inplace<Lower>::update(matt, transpositions, tmp, w.conjugate(), sigma);
}
};
template<typename MatrixType> struct LDLT_Traits<MatrixType,Lower>
{
typedef const TriangularView<const MatrixType, UnitLower> MatrixL;
typedef const TriangularView<const typename MatrixType::AdjointReturnType, UnitUpper> MatrixU;
static inline MatrixL getL(const MatrixType& m) { return m; }
static inline MatrixU getU(const MatrixType& m) { return m.adjoint(); }
};
template<typename MatrixType> struct LDLT_Traits<MatrixType,Upper>
{
typedef const TriangularView<const typename MatrixType::AdjointReturnType, UnitLower> MatrixL;
typedef const TriangularView<const MatrixType, UnitUpper> MatrixU;
static inline MatrixL getL(const MatrixType& m) { return m.adjoint(); }
static inline MatrixU getU(const MatrixType& m) { return m; }
};
} // end namespace internal
/** Compute / recompute the LDLT decomposition A = L D L^* = U^* D U of \a matrix
*/
template<typename MatrixType, int _UpLo>
LDLT<MatrixType,_UpLo>& LDLT<MatrixType,_UpLo>::compute(const MatrixType& a)
{
check_template_parameters();
eigen_assert(a.rows()==a.cols());
const Index size = a.rows();
m_matrix = a;
m_transpositions.resize(size);
m_isInitialized = false;
m_temporary.resize(size);
m_sign = internal::ZeroSign;
internal::ldlt_inplace<UpLo>::unblocked(m_matrix, m_transpositions, m_temporary, m_sign);
m_isInitialized = true;
return *this;
}
/** Update the LDLT decomposition: given A = L D L^T, efficiently compute the decomposition of A + sigma w w^T.
* \param w a vector to be incorporated into the decomposition.
* \param sigma a scalar, +1 for updates and -1 for "downdates," which correspond to removing previously-added column vectors. Optional; default value is +1.
* \sa setZero()
*/
template<typename MatrixType, int _UpLo>
template<typename Derived>
LDLT<MatrixType,_UpLo>& LDLT<MatrixType,_UpLo>::rankUpdate(const MatrixBase<Derived>& w, const typename LDLT<MatrixType,_UpLo>::RealScalar& sigma)
{
const Index size = w.rows();
if (m_isInitialized)
{
eigen_assert(m_matrix.rows()==size);
}
else
{
m_matrix.resize(size,size);
m_matrix.setZero();
m_transpositions.resize(size);
for (Index i = 0; i < size; i++)
m_transpositions.coeffRef(i) = i;
m_temporary.resize(size);
m_sign = sigma>=0 ? internal::PositiveSemiDef : internal::NegativeSemiDef;
m_isInitialized = true;
}
internal::ldlt_inplace<UpLo>::update(m_matrix, m_transpositions, m_temporary, w, sigma);
return *this;
}
namespace internal {
template<typename _MatrixType, int _UpLo, typename Rhs>
struct solve_retval<LDLT<_MatrixType,_UpLo>, Rhs>
: solve_retval_base<LDLT<_MatrixType,_UpLo>, Rhs>
{
typedef LDLT<_MatrixType,_UpLo> LDLTType;
EIGEN_MAKE_SOLVE_HELPERS(LDLTType,Rhs)
template<typename Dest> void evalTo(Dest& dst) const
{
eigen_assert(rhs().rows() == dec().matrixLDLT().rows());
// dst = P b
dst = dec().transpositionsP() * rhs();
// dst = L^-1 (P b)
dec().matrixL().solveInPlace(dst);
// dst = D^-1 (L^-1 P b)
// more precisely, use pseudo-inverse of D (see bug 241)
using std::abs;
using std::max;
typedef typename LDLTType::MatrixType MatrixType;
typedef typename LDLTType::RealScalar RealScalar;
const typename Diagonal<const MatrixType>::RealReturnType vectorD(dec().vectorD());
// In some previous versions, tolerance was set to the max of 1/highest and the maximal diagonal entry * epsilon
// as motivated by LAPACK's xGELSS:
// RealScalar tolerance = (max)(vectorD.array().abs().maxCoeff() *NumTraits<RealScalar>::epsilon(),RealScalar(1) / NumTraits<RealScalar>::highest());
// However, LDLT is not rank revealing, and so adjusting the tolerance wrt to the highest
// diagonal element is not well justified and to numerical issues in some cases.
// Moreover, Lapack's xSYTRS routines use 0 for the tolerance.
RealScalar tolerance = RealScalar(1) / NumTraits<RealScalar>::highest();
for (Index i = 0; i < vectorD.size(); ++i) {
if(abs(vectorD(i)) > tolerance)
dst.row(i) /= vectorD(i);
else
dst.row(i).setZero();
}
// dst = L^-T (D^-1 L^-1 P b)
dec().matrixU().solveInPlace(dst);
// dst = P^-1 (L^-T D^-1 L^-1 P b) = A^-1 b
dst = dec().transpositionsP().transpose() * dst;
}
};
}
/** \internal use x = ldlt_object.solve(x);
*
* This is the \em in-place version of solve().
*
* \param bAndX represents both the right-hand side matrix b and result x.
*
* \returns true always! If you need to check for existence of solutions, use another decomposition like LU, QR, or SVD.
*
* This version avoids a copy when the right hand side matrix b is not
* needed anymore.
*
* \sa LDLT::solve(), MatrixBase::ldlt()
*/
template<typename MatrixType,int _UpLo>
template<typename Derived>
bool LDLT<MatrixType,_UpLo>::solveInPlace(MatrixBase<Derived> &bAndX) const
{
eigen_assert(m_isInitialized && "LDLT is not initialized.");
eigen_assert(m_matrix.rows() == bAndX.rows());
bAndX = this->solve(bAndX);
return true;
}
/** \returns the matrix represented by the decomposition,
* i.e., it returns the product: P^T L D L^* P.
* This function is provided for debug purpose. */
template<typename MatrixType, int _UpLo>
MatrixType LDLT<MatrixType,_UpLo>::reconstructedMatrix() const
{
eigen_assert(m_isInitialized && "LDLT is not initialized.");
const Index size = m_matrix.rows();
MatrixType res(size,size);
// P
res.setIdentity();
res = transpositionsP() * res;
// L^* P
res = matrixU() * res;
// D(L^*P)
res = vectorD().real().asDiagonal() * res;
// L(DL^*P)
res = matrixL() * res;
// P^T (LDL^*P)
res = transpositionsP().transpose() * res;
return res;
}
/** \cholesky_module
* \returns the Cholesky decomposition with full pivoting without square root of \c *this
*/
template<typename MatrixType, unsigned int UpLo>
inline const LDLT<typename SelfAdjointView<MatrixType, UpLo>::PlainObject, UpLo>
SelfAdjointView<MatrixType, UpLo>::ldlt() const
{
return LDLT<PlainObject,UpLo>(m_matrix);
}
/** \cholesky_module
* \returns the Cholesky decomposition with full pivoting without square root of \c *this
*/
template<typename Derived>
inline const LDLT<typename MatrixBase<Derived>::PlainObject>
MatrixBase<Derived>::ldlt() const
{
return LDLT<PlainObject>(derived());
}
} // end namespace Eigen
#endif // EIGEN_LDLT_H

Some files were not shown because too many files have changed in this diff Show More