├── .gitignore ├── CMakeLists.txt ├── README.md ├── cmake ├── FindEigen3.cmake └── FindGlog.cmake ├── src ├── CMakeLists.txt ├── factor │ ├── binary_pose_error_factor.cpp │ ├── binary_pose_error_factor.hpp │ ├── marginalization_factor.cpp │ ├── marginalization_factor.h │ ├── pinhole_project_factor.cpp │ ├── pinhole_project_factor.h │ ├── pose_local_parameterization.cpp │ ├── pose_local_parameterization.h │ ├── project_error.cpp │ └── project_error.h └── utility │ ├── num-diff.hpp │ ├── utility.cpp │ └── utility.h └── test ├── testMarginalization.cpp ├── testPinholeProjectFactor.cpp └── testProjectError.cpp /.gitignore: -------------------------------------------------------------------------------- 1 | .idea/* 2 | cmake-build-debug/* 3 | cmake-build-release/* 4 | build/* 5 | .vscode/ 6 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | set(PROJECT_NAME marginalization) 3 | project(${PROJECT_NAME}) 4 | 5 | set(CMAKE_BUILD_TYPE release) 6 | 7 | set(CMAKE_CXX_FLAGS "-std=c++11") 8 | 9 | set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -fpic") 10 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fpic") 11 | 12 | #-DEIGEN_USE_MKL_ALL") 13 | set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -g") 14 | set(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake) 15 | MESSAGE("PROJECT_SOURCE_DIR " ${PROJECT_SOURCE_DIR}) 16 | 17 | set( CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -mssse3" ) 18 | set( CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wall -mssse3" ) 19 | 20 | find_package(Eigen3 REQUIRED) 21 | find_package(Ceres REQUIRED) 22 | find_package(Glog REQUIRED) 23 | 24 | 25 | include_directories(${EIGEN3_INCLUDE_DIR} 26 | ${CERES_INCLUDE_DIRS}) 27 | 28 | 29 | #Build the main lib 30 | add_subdirectory(src) 31 | 32 | 33 | add_executable(testPinholeProjectFactor test/testPinholeProjectFactor.cpp) 34 | target_link_libraries(testPinholeProjectFactor ${PROJECT_NAME} ) 35 | 36 | 37 | add_executable(testProjectError test/testProjectError.cpp) 38 | target_link_libraries(testProjectError ${PROJECT_NAME} ) 39 | 40 | 41 | 42 | add_executable(testMarginalization test/testMarginalization.cpp) 43 | target_link_libraries(testMarginalization ${PROJECT_NAME} ) 44 | 45 | 46 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # marginalization 2 | Sample code for marginalization of SLAM, Visual-Inertial Odometry. 3 | The main reference is code form OKVIS and VINS_MONO. 4 | I will try to show how marginlaization works and optimize the performance at the same time. 5 | -------------------------------------------------------------------------------- /cmake/FindEigen3.cmake: -------------------------------------------------------------------------------- 1 | # Ceres Solver - A fast non-linear least squares minimizer 2 | # Copyright 2015 Google Inc. All rights reserved. 3 | # http://ceres-solver.org/ 4 | # 5 | # Redistribution and use in source and binary forms, with or without 6 | # modification, are permitted provided that the following conditions are met: 7 | # 8 | # * Redistributions of source code must retain the above copyright notice, 9 | # this list of conditions and the following disclaimer. 10 | # * Redistributions in binary form must reproduce the above copyright notice, 11 | # this list of conditions and the following disclaimer in the documentation 12 | # and/or other materials provided with the distribution. 13 | # * Neither the name of Google Inc. nor the names of its contributors may be 14 | # used to endorse or promote products derived from this software without 15 | # specific prior written permission. 16 | # 17 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | # POSSIBILITY OF SUCH DAMAGE. 28 | # 29 | # Author: alexs.mac@gmail.com (Alex Stewart) 30 | # 31 | 32 | # FindEigen.cmake - Find Eigen library, version >= 3. 33 | # 34 | # This module defines the following variables: 35 | # 36 | # EIGEN_FOUND: TRUE iff Eigen is found. 37 | # EIGEN_INCLUDE_DIRS: Include directories for Eigen. 38 | # 39 | # EIGEN_VERSION: Extracted from Eigen/src/Core/util/Macros.h 40 | # EIGEN_WORLD_VERSION: Equal to 3 if EIGEN_VERSION = 3.2.0 41 | # EIGEN_MAJOR_VERSION: Equal to 2 if EIGEN_VERSION = 3.2.0 42 | # EIGEN_MINOR_VERSION: Equal to 0 if EIGEN_VERSION = 3.2.0 43 | # 44 | # The following variables control the behaviour of this module: 45 | # 46 | # EIGEN_INCLUDE_DIR_HINTS: List of additional directories in which to 47 | # search for eigen includes, e.g: /timbuktu/eigen3. 48 | # 49 | # The following variables are also defined by this module, but in line with 50 | # CMake recommended FindPackage() module style should NOT be referenced directly 51 | # by callers (use the plural variables detailed above instead). These variables 52 | # do however affect the behaviour of the module via FIND_[PATH/LIBRARY]() which 53 | # are NOT re-called (i.e. search for library is not repeated) if these variables 54 | # are set with valid values _in the CMake cache_. This means that if these 55 | # variables are set directly in the cache, either by the user in the CMake GUI, 56 | # or by the user passing -DVAR=VALUE directives to CMake when called (which 57 | # explicitly defines a cache variable), then they will be used verbatim, 58 | # bypassing the HINTS variables and other hard-coded search locations. 59 | # 60 | # EIGEN_INCLUDE_DIR: Include directory for CXSparse, not including the 61 | # include directory of any dependencies. 62 | 63 | # Called if we failed to find Eigen or any of it's required dependencies, 64 | # unsets all public (designed to be used externally) variables and reports 65 | # error message at priority depending upon [REQUIRED/QUIET/] argument. 66 | macro(EIGEN_REPORT_NOT_FOUND REASON_MSG) 67 | unset(EIGEN_FOUND) 68 | unset(EIGEN_INCLUDE_DIRS) 69 | # Make results of search visible in the CMake GUI if Eigen has not 70 | # been found so that user does not have to toggle to advanced view. 71 | mark_as_advanced(CLEAR EIGEN_INCLUDE_DIR) 72 | # Note _FIND_[REQUIRED/QUIETLY] variables defined by FindPackage() 73 | # use the camelcase library name, not uppercase. 74 | if (Eigen_FIND_QUIETLY) 75 | message(STATUS "Failed to find Eigen - " ${REASON_MSG} ${ARGN}) 76 | elseif (Eigen_FIND_REQUIRED) 77 | message(FATAL_ERROR "Failed to find Eigen - " ${REASON_MSG} ${ARGN}) 78 | else() 79 | # Neither QUIETLY nor REQUIRED, use no priority which emits a message 80 | # but continues configuration and allows generation. 81 | message("-- Failed to find Eigen - " ${REASON_MSG} ${ARGN}) 82 | endif () 83 | return() 84 | endmacro(EIGEN_REPORT_NOT_FOUND) 85 | 86 | # Protect against any alternative find_package scripts for this library having 87 | # been called previously (in a client project) which set EIGEN_FOUND, but not 88 | # the other variables we require / set here which could cause the search logic 89 | # here to fail. 90 | unset(EIGEN_FOUND) 91 | 92 | # Search user-installed locations first, so that we prefer user installs 93 | # to system installs where both exist. 94 | list(APPEND EIGEN_CHECK_INCLUDE_DIRS 95 | /usr/local/include 96 | /usr/local/homebrew/include # Mac OS X 97 | /opt/local/var/macports/software # Mac OS X. 98 | /opt/local/include 99 | /usr/include) 100 | # Additional suffixes to try appending to each search path. 101 | list(APPEND EIGEN_CHECK_PATH_SUFFIXES 102 | eigen3 # Default root directory for Eigen. 103 | Eigen/include/eigen3 # Windows (for C:/Program Files prefix) < 3.3 104 | Eigen3/include/eigen3 ) # Windows (for C:/Program Files prefix) >= 3.3 105 | 106 | # Search supplied hint directories first if supplied. 107 | find_path(EIGEN_INCLUDE_DIR 108 | NAMES Eigen/Core 109 | PATHS ${EIGEN_INCLUDE_DIR_HINTS} 110 | ${EIGEN_CHECK_INCLUDE_DIRS} 111 | PATH_SUFFIXES ${EIGEN_CHECK_PATH_SUFFIXES}) 112 | 113 | if (NOT EIGEN_INCLUDE_DIR OR 114 | NOT EXISTS ${EIGEN_INCLUDE_DIR}) 115 | eigen_report_not_found( 116 | "Could not find eigen3 include directory, set EIGEN_INCLUDE_DIR to " 117 | "path to eigen3 include directory, e.g. /usr/local/include/eigen3.") 118 | endif (NOT EIGEN_INCLUDE_DIR OR 119 | NOT EXISTS ${EIGEN_INCLUDE_DIR}) 120 | 121 | # Mark internally as found, then verify. EIGEN_REPORT_NOT_FOUND() unsets 122 | # if called. 123 | set(EIGEN_FOUND TRUE) 124 | 125 | # Extract Eigen version from Eigen/src/Core/util/Macros.h 126 | if (EIGEN_INCLUDE_DIR) 127 | set(EIGEN_VERSION_FILE ${EIGEN_INCLUDE_DIR}/Eigen/src/Core/util/Macros.h) 128 | if (NOT EXISTS ${EIGEN_VERSION_FILE}) 129 | eigen_report_not_found( 130 | "Could not find file: ${EIGEN_VERSION_FILE} " 131 | "containing version information in Eigen install located at: " 132 | "${EIGEN_INCLUDE_DIR}.") 133 | else (NOT EXISTS ${EIGEN_VERSION_FILE}) 134 | file(READ ${EIGEN_VERSION_FILE} EIGEN_VERSION_FILE_CONTENTS) 135 | 136 | string(REGEX MATCH "#define EIGEN_WORLD_VERSION [0-9]+" 137 | EIGEN_WORLD_VERSION "${EIGEN_VERSION_FILE_CONTENTS}") 138 | string(REGEX REPLACE "#define EIGEN_WORLD_VERSION ([0-9]+)" "\\1" 139 | EIGEN_WORLD_VERSION "${EIGEN_WORLD_VERSION}") 140 | 141 | string(REGEX MATCH "#define EIGEN_MAJOR_VERSION [0-9]+" 142 | EIGEN_MAJOR_VERSION "${EIGEN_VERSION_FILE_CONTENTS}") 143 | string(REGEX REPLACE "#define EIGEN_MAJOR_VERSION ([0-9]+)" "\\1" 144 | EIGEN_MAJOR_VERSION "${EIGEN_MAJOR_VERSION}") 145 | 146 | string(REGEX MATCH "#define EIGEN_MINOR_VERSION [0-9]+" 147 | EIGEN_MINOR_VERSION "${EIGEN_VERSION_FILE_CONTENTS}") 148 | string(REGEX REPLACE "#define EIGEN_MINOR_VERSION ([0-9]+)" "\\1" 149 | EIGEN_MINOR_VERSION "${EIGEN_MINOR_VERSION}") 150 | 151 | # This is on a single line s/t CMake does not interpret it as a list of 152 | # elements and insert ';' separators which would result in 3.;2.;0 nonsense. 153 | set(EIGEN_VERSION "${EIGEN_WORLD_VERSION}.${EIGEN_MAJOR_VERSION}.${EIGEN_MINOR_VERSION}") 154 | endif (NOT EXISTS ${EIGEN_VERSION_FILE}) 155 | endif (EIGEN_INCLUDE_DIR) 156 | 157 | # Set standard CMake FindPackage variables if found. 158 | if (EIGEN_FOUND) 159 | set(EIGEN_INCLUDE_DIRS ${EIGEN_INCLUDE_DIR}) 160 | endif (EIGEN_FOUND) 161 | 162 | # Handle REQUIRED / QUIET optional arguments and version. 163 | include(FindPackageHandleStandardArgs) 164 | find_package_handle_standard_args(Eigen 165 | REQUIRED_VARS EIGEN_INCLUDE_DIRS 166 | VERSION_VAR EIGEN_VERSION) 167 | 168 | # Only mark internal variables as advanced if we found Eigen, otherwise 169 | # leave it visible in the standard GUI for the user to set manually. 170 | if (EIGEN_FOUND) 171 | mark_as_advanced(FORCE EIGEN_INCLUDE_DIR) 172 | endif (EIGEN_FOUND) 173 | -------------------------------------------------------------------------------- /cmake/FindGlog.cmake: -------------------------------------------------------------------------------- 1 | # Ceres Solver - A fast non-linear least squares minimizer 2 | # Copyright 2015 Google Inc. All rights reserved. 3 | # http://ceres-solver.org/ 4 | # 5 | # Redistribution and use in source and binary forms, with or without 6 | # modification, are permitted provided that the following conditions are met: 7 | # 8 | # * Redistributions of source code must retain the above copyright notice, 9 | # this list of conditions and the following disclaimer. 10 | # * Redistributions in binary form must reproduce the above copyright notice, 11 | # this list of conditions and the following disclaimer in the documentation 12 | # and/or other materials provided with the distribution. 13 | # * Neither the name of Google Inc. nor the names of its contributors may be 14 | # used to endorse or promote products derived from this software without 15 | # specific prior written permission. 16 | # 17 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | # POSSIBILITY OF SUCH DAMAGE. 28 | # 29 | # Author: alexs.mac@gmail.com (Alex Stewart) 30 | # 31 | 32 | # FindGlog.cmake - Find Google glog logging library. 33 | # 34 | # This module defines the following variables: 35 | # 36 | # GLOG_FOUND: TRUE iff glog is found. 37 | # GLOG_INCLUDE_DIRS: Include directories for glog. 38 | # GLOG_LIBRARIES: Libraries required to link glog. 39 | # FOUND_INSTALLED_GLOG_CMAKE_CONFIGURATION: True iff the version of glog found 40 | # was built & installed / exported 41 | # as a CMake package. 42 | # 43 | # The following variables control the behaviour of this module: 44 | # 45 | # GLOG_PREFER_EXPORTED_GLOG_CMAKE_CONFIGURATION: TRUE/FALSE, iff TRUE then 46 | # then prefer using an exported CMake configuration 47 | # generated by glog > 0.3.4 over searching for the 48 | # glog components manually. Otherwise (FALSE) 49 | # ignore any exported glog CMake configurations and 50 | # always perform a manual search for the components. 51 | # Default: TRUE iff user does not define this variable 52 | # before we are called, and does NOT specify either 53 | # GLOG_INCLUDE_DIR_HINTS or GLOG_LIBRARY_DIR_HINTS 54 | # otherwise FALSE. 55 | # GLOG_INCLUDE_DIR_HINTS: List of additional directories in which to 56 | # search for glog includes, e.g: /timbuktu/include. 57 | # GLOG_LIBRARY_DIR_HINTS: List of additional directories in which to 58 | # search for glog libraries, e.g: /timbuktu/lib. 59 | # 60 | # The following variables are also defined by this module, but in line with 61 | # CMake recommended FindPackage() module style should NOT be referenced directly 62 | # by callers (use the plural variables detailed above instead). These variables 63 | # do however affect the behaviour of the module via FIND_[PATH/LIBRARY]() which 64 | # are NOT re-called (i.e. search for library is not repeated) if these variables 65 | # are set with valid values _in the CMake cache_. This means that if these 66 | # variables are set directly in the cache, either by the user in the CMake GUI, 67 | # or by the user passing -DVAR=VALUE directives to CMake when called (which 68 | # explicitly defines a cache variable), then they will be used verbatim, 69 | # bypassing the HINTS variables and other hard-coded search locations. 70 | # 71 | # GLOG_INCLUDE_DIR: Include directory for glog, not including the 72 | # include directory of any dependencies. 73 | # GLOG_LIBRARY: glog library, not including the libraries of any 74 | # dependencies. 75 | 76 | # Reset CALLERS_CMAKE_FIND_LIBRARY_PREFIXES to its value when 77 | # FindGlog was invoked. 78 | macro(GLOG_RESET_FIND_LIBRARY_PREFIX) 79 | if (MSVC AND CALLERS_CMAKE_FIND_LIBRARY_PREFIXES) 80 | set(CMAKE_FIND_LIBRARY_PREFIXES "${CALLERS_CMAKE_FIND_LIBRARY_PREFIXES}") 81 | endif() 82 | endmacro(GLOG_RESET_FIND_LIBRARY_PREFIX) 83 | 84 | # Called if we failed to find glog or any of it's required dependencies, 85 | # unsets all public (designed to be used externally) variables and reports 86 | # error message at priority depending upon [REQUIRED/QUIET/] argument. 87 | macro(GLOG_REPORT_NOT_FOUND REASON_MSG) 88 | unset(GLOG_FOUND) 89 | unset(GLOG_INCLUDE_DIRS) 90 | unset(GLOG_LIBRARIES) 91 | # Make results of search visible in the CMake GUI if glog has not 92 | # been found so that user does not have to toggle to advanced view. 93 | mark_as_advanced(CLEAR GLOG_INCLUDE_DIR 94 | GLOG_LIBRARY) 95 | 96 | glog_reset_find_library_prefix() 97 | 98 | # Note _FIND_[REQUIRED/QUIETLY] variables defined by FindPackage() 99 | # use the camelcase library name, not uppercase. 100 | if (Glog_FIND_QUIETLY) 101 | message(STATUS "Failed to find glog - " ${REASON_MSG} ${ARGN}) 102 | elseif (Glog_FIND_REQUIRED) 103 | message(FATAL_ERROR "Failed to find glog - " ${REASON_MSG} ${ARGN}) 104 | else() 105 | # Neither QUIETLY nor REQUIRED, use no priority which emits a message 106 | # but continues configuration and allows generation. 107 | message("-- Failed to find glog - " ${REASON_MSG} ${ARGN}) 108 | endif () 109 | return() 110 | endmacro(GLOG_REPORT_NOT_FOUND) 111 | 112 | # Protect against any alternative find_package scripts for this library having 113 | # been called previously (in a client project) which set GLOG_FOUND, but not 114 | # the other variables we require / set here which could cause the search logic 115 | # here to fail. 116 | unset(GLOG_FOUND) 117 | 118 | # ----------------------------------------------------------------- 119 | # By default, if the user has expressed no preference for using an exported 120 | # glog CMake configuration over performing a search for the installed 121 | # components, and has not specified any hints for the search locations, then 122 | # prefer a glog exported configuration if available. 123 | if (NOT DEFINED GLOG_PREFER_EXPORTED_GLOG_CMAKE_CONFIGURATION 124 | AND NOT GLOG_INCLUDE_DIR_HINTS 125 | AND NOT GLOG_LIBRARY_DIR_HINTS) 126 | message(STATUS "No preference for use of exported glog CMake configuration " 127 | "set, and no hints for include/library directories provided. " 128 | "Defaulting to preferring an installed/exported glog CMake configuration " 129 | "if available.") 130 | set(GLOG_PREFER_EXPORTED_GLOG_CMAKE_CONFIGURATION TRUE) 131 | endif() 132 | 133 | if (GLOG_PREFER_EXPORTED_GLOG_CMAKE_CONFIGURATION) 134 | # Try to find an exported CMake configuration for glog, as generated by 135 | # glog versions > 0.3.4 136 | # 137 | # We search twice, s/t we can invert the ordering of precedence used by 138 | # find_package() for exported package build directories, and installed 139 | # packages (found via CMAKE_SYSTEM_PREFIX_PATH), listed as items 6) and 7) 140 | # respectively in [1]. 141 | # 142 | # By default, exported build directories are (in theory) detected first, and 143 | # this is usually the case on Windows. However, on OS X & Linux, the install 144 | # path (/usr/local) is typically present in the PATH environment variable 145 | # which is checked in item 4) in [1] (i.e. before both of the above, unless 146 | # NO_SYSTEM_ENVIRONMENT_PATH is passed). As such on those OSs installed 147 | # packages are usually detected in preference to exported package build 148 | # directories. 149 | # 150 | # To ensure a more consistent response across all OSs, and as users usually 151 | # want to prefer an installed version of a package over a locally built one 152 | # where both exist (esp. as the exported build directory might be removed 153 | # after installation), we first search with NO_CMAKE_PACKAGE_REGISTRY which 154 | # means any build directories exported by the user are ignored, and thus 155 | # installed directories are preferred. If this fails to find the package 156 | # we then research again, but without NO_CMAKE_PACKAGE_REGISTRY, so any 157 | # exported build directories will now be detected. 158 | # 159 | # To prevent confusion on Windows, we also pass NO_CMAKE_BUILDS_PATH (which 160 | # is item 5) in [1]), to not preferentially use projects that were built 161 | # recently with the CMake GUI to ensure that we always prefer an installed 162 | # version if available. 163 | # 164 | # NOTE: We use the NAMES option as glog erroneously uses 'google-glog' as its 165 | # project name when built with CMake, but exports itself as just 'glog'. 166 | # On Linux/OS X this does not break detection as the project name is 167 | # not used as part of the install path for the CMake package files, 168 | # e.g. /usr/local/lib/cmake/glog, where the suffix is hardcoded 169 | # in glog's CMakeLists. However, on Windows the project name *is* 170 | # part of the install prefix: C:/Program Files/google-glog/[include,lib]. 171 | # However, by default CMake checks: 172 | # C:/Program Files/ which does not 173 | # exist and thus detection fails. Thus we use the NAMES to force the 174 | # search to use both google-glog & glog. 175 | # 176 | # [1] http://www.cmake.org/cmake/help/v2.8.11/cmake.html#command:find_package 177 | find_package(glog QUIET 178 | NAMES google-glog glog 179 | NO_MODULE 180 | NO_CMAKE_PACKAGE_REGISTRY 181 | NO_CMAKE_BUILDS_PATH) 182 | if (glog_FOUND) 183 | message(STATUS "Found installed version of glog: ${glog_DIR}") 184 | else() 185 | # Failed to find an installed version of glog, repeat search allowing 186 | # exported build directories. 187 | message(STATUS "Failed to find installed glog CMake configuration, " 188 | "searching for glog build directories exported with CMake.") 189 | # Again pass NO_CMAKE_BUILDS_PATH, as we know that glog is exported and 190 | # do not want to treat projects built with the CMake GUI preferentially. 191 | find_package(glog QUIET 192 | NAMES google-glog glog 193 | NO_MODULE 194 | NO_CMAKE_BUILDS_PATH) 195 | if (glog_FOUND) 196 | message(STATUS "Found exported glog build directory: ${glog_DIR}") 197 | endif(glog_FOUND) 198 | endif(glog_FOUND) 199 | 200 | set(FOUND_INSTALLED_GLOG_CMAKE_CONFIGURATION ${glog_FOUND}) 201 | 202 | if (FOUND_INSTALLED_GLOG_CMAKE_CONFIGURATION) 203 | message(STATUS "Detected glog version: ${glog_VERSION}") 204 | set(GLOG_FOUND ${glog_FOUND}) 205 | # glog wraps the include directories into the exported glog::glog target. 206 | set(GLOG_INCLUDE_DIR "") 207 | set(GLOG_LIBRARY glog::glog) 208 | else (FOUND_INSTALLED_GLOG_CMAKE_CONFIGURATION) 209 | message(STATUS "Failed to find an installed/exported CMake configuration " 210 | "for glog, will perform search for installed glog components.") 211 | endif (FOUND_INSTALLED_GLOG_CMAKE_CONFIGURATION) 212 | endif(GLOG_PREFER_EXPORTED_GLOG_CMAKE_CONFIGURATION) 213 | 214 | if (NOT GLOG_FOUND) 215 | # Either failed to find an exported glog CMake configuration, or user 216 | # told us not to use one. Perform a manual search for all glog components. 217 | 218 | # Handle possible presence of lib prefix for libraries on MSVC, see 219 | # also GLOG_RESET_FIND_LIBRARY_PREFIX(). 220 | if (MSVC) 221 | # Preserve the caller's original values for CMAKE_FIND_LIBRARY_PREFIXES 222 | # s/t we can set it back before returning. 223 | set(CALLERS_CMAKE_FIND_LIBRARY_PREFIXES "${CMAKE_FIND_LIBRARY_PREFIXES}") 224 | # The empty string in this list is important, it represents the case when 225 | # the libraries have no prefix (shared libraries / DLLs). 226 | set(CMAKE_FIND_LIBRARY_PREFIXES "lib" "" "${CMAKE_FIND_LIBRARY_PREFIXES}") 227 | endif (MSVC) 228 | 229 | # Search user-installed locations first, so that we prefer user installs 230 | # to system installs where both exist. 231 | list(APPEND GLOG_CHECK_INCLUDE_DIRS 232 | /usr/local/include 233 | /usr/local/homebrew/include # Mac OS X 234 | /opt/local/var/macports/software # Mac OS X. 235 | /opt/local/include 236 | /usr/include) 237 | # Windows (for C:/Program Files prefix). 238 | list(APPEND GLOG_CHECK_PATH_SUFFIXES 239 | glog/include 240 | glog/Include 241 | Glog/include 242 | Glog/Include 243 | google-glog/include # CMake installs with project name prefix. 244 | google-glog/Include) 245 | 246 | list(APPEND GLOG_CHECK_LIBRARY_DIRS 247 | /usr/local/lib 248 | /usr/local/homebrew/lib # Mac OS X. 249 | /opt/local/lib 250 | /usr/lib) 251 | # Windows (for C:/Program Files prefix). 252 | list(APPEND GLOG_CHECK_LIBRARY_SUFFIXES 253 | glog/lib 254 | glog/Lib 255 | Glog/lib 256 | Glog/Lib 257 | google-glog/lib # CMake installs with project name prefix. 258 | google-glog/Lib) 259 | 260 | # Search supplied hint directories first if supplied. 261 | find_path(GLOG_INCLUDE_DIR 262 | NAMES glog/logging.h 263 | HINTS ${GLOG_INCLUDE_DIR_HINTS} 264 | PATHS ${GLOG_CHECK_INCLUDE_DIRS} 265 | PATH_SUFFIXES ${GLOG_CHECK_PATH_SUFFIXES}) 266 | if (NOT GLOG_INCLUDE_DIR OR 267 | NOT EXISTS ${GLOG_INCLUDE_DIR}) 268 | glog_report_not_found( 269 | "Could not find glog include directory, set GLOG_INCLUDE_DIR " 270 | "to directory containing glog/logging.h") 271 | endif (NOT GLOG_INCLUDE_DIR OR 272 | NOT EXISTS ${GLOG_INCLUDE_DIR}) 273 | 274 | find_library(GLOG_LIBRARY NAMES glog 275 | HINTS ${GLOG_LIBRARY_DIR_HINTS} 276 | PATHS ${GLOG_CHECK_LIBRARY_DIRS} 277 | PATH_SUFFIXES ${GLOG_CHECK_LIBRARY_SUFFIXES}) 278 | if (NOT GLOG_LIBRARY OR 279 | NOT EXISTS ${GLOG_LIBRARY}) 280 | glog_report_not_found( 281 | "Could not find glog library, set GLOG_LIBRARY " 282 | "to full path to libglog.") 283 | endif (NOT GLOG_LIBRARY OR 284 | NOT EXISTS ${GLOG_LIBRARY}) 285 | 286 | # Mark internally as found, then verify. GLOG_REPORT_NOT_FOUND() unsets 287 | # if called. 288 | set(GLOG_FOUND TRUE) 289 | 290 | # Glog does not seem to provide any record of the version in its 291 | # source tree, thus cannot extract version. 292 | 293 | # Catch case when caller has set GLOG_INCLUDE_DIR in the cache / GUI and 294 | # thus FIND_[PATH/LIBRARY] are not called, but specified locations are 295 | # invalid, otherwise we would report the library as found. 296 | if (GLOG_INCLUDE_DIR AND 297 | NOT EXISTS ${GLOG_INCLUDE_DIR}/glog/logging.h) 298 | glog_report_not_found( 299 | "Caller defined GLOG_INCLUDE_DIR:" 300 | " ${GLOG_INCLUDE_DIR} does not contain glog/logging.h header.") 301 | endif (GLOG_INCLUDE_DIR AND 302 | NOT EXISTS ${GLOG_INCLUDE_DIR}/glog/logging.h) 303 | # TODO: This regex for glog library is pretty primitive, we use lowercase 304 | # for comparison to handle Windows using CamelCase library names, could 305 | # this check be better? 306 | string(TOLOWER "${GLOG_LIBRARY}" LOWERCASE_GLOG_LIBRARY) 307 | if (GLOG_LIBRARY AND 308 | NOT "${LOWERCASE_GLOG_LIBRARY}" MATCHES ".*glog[^/]*") 309 | glog_report_not_found( 310 | "Caller defined GLOG_LIBRARY: " 311 | "${GLOG_LIBRARY} does not match glog.") 312 | endif (GLOG_LIBRARY AND 313 | NOT "${LOWERCASE_GLOG_LIBRARY}" MATCHES ".*glog[^/]*") 314 | 315 | glog_reset_find_library_prefix() 316 | 317 | endif(NOT GLOG_FOUND) 318 | 319 | # Set standard CMake FindPackage variables if found. 320 | if (GLOG_FOUND) 321 | set(GLOG_INCLUDE_DIRS ${GLOG_INCLUDE_DIR}) 322 | set(GLOG_LIBRARIES ${GLOG_LIBRARY}) 323 | endif (GLOG_FOUND) 324 | 325 | # If we are using an exported CMake glog target, the include directories are 326 | # wrapped into the target itself, and do not have to be (and are not) 327 | # separately specified. In which case, we should not add GLOG_INCLUDE_DIRS 328 | # to the list of required variables in order that glog be reported as found. 329 | if (FOUND_INSTALLED_GLOG_CMAKE_CONFIGURATION) 330 | set(GLOG_REQUIRED_VARIABLES GLOG_LIBRARIES) 331 | else() 332 | set(GLOG_REQUIRED_VARIABLES GLOG_INCLUDE_DIRS GLOG_LIBRARIES) 333 | endif() 334 | 335 | # Handle REQUIRED / QUIET optional arguments. 336 | include(FindPackageHandleStandardArgs) 337 | find_package_handle_standard_args(Glog DEFAULT_MSG 338 | ${GLOG_REQUIRED_VARIABLES}) 339 | 340 | # Only mark internal variables as advanced if we found glog, otherwise 341 | # leave them visible in the standard GUI for the user to set manually. 342 | if (GLOG_FOUND) 343 | mark_as_advanced(FORCE GLOG_INCLUDE_DIR 344 | GLOG_LIBRARY 345 | glog_DIR) # Autogenerated by find_package(glog) 346 | endif (GLOG_FOUND) 347 | -------------------------------------------------------------------------------- /src/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # Build the main lib 2 | add_library( ${PROJECT_NAME} 3 | factor/pose_local_parameterization.cpp 4 | factor/marginalization_factor.cpp 5 | factor/pinhole_project_factor.cpp 6 | factor/project_error.cpp 7 | factor/binary_pose_error_factor.cpp 8 | utility/utility.cpp 9 | ) 10 | 11 | # Build main lib 12 | target_link_libraries( ${PROJECT_NAME} 13 | ${CERES_LIBRARIES}) 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /src/factor/binary_pose_error_factor.cpp: -------------------------------------------------------------------------------- 1 | #include "binary_pose_error_factor.hpp" 2 | 3 | Binary_pose_error_facotr::Binary_pose_error_facotr(const Eigen::Isometry3d& _Pose0, 4 | const Eigen::Isometry3d& _Pose1, 5 | double _t0, double _t1, 6 | double weightScalar): 7 | Pose0_(_Pose0),Pose1_(_Pose1),t0(_t0),t1(_t1),weightScalar_(weightScalar){ 8 | dq_meas_ = Eigen::Quaterniond(Pose0_.rotation().transpose()*Pose1_.rotation()); 9 | dt_meas_ = Pose1_.translation() - Pose0_.translation(); 10 | } 11 | 12 | bool Binary_pose_error_facotr::Evaluate(double const *const *parameters, double *residuals, 13 | double **jacobians) const 14 | { 15 | return EvaluateWithMinimalJacobians(parameters, 16 | residuals, 17 | jacobians, NULL); 18 | } 19 | 20 | bool Binary_pose_error_facotr::EvaluateWithMinimalJacobians(double const *const *parameters, 21 | double *residuals, 22 | double **jacobians, 23 | double **jacobiansMinimal) const{ 24 | Eigen::Vector3d t_hat0 = Eigen::Vector3d(parameters[0][0], parameters[0][1], parameters[0][2]); 25 | Eigen::Quaterniond Q_hat0(parameters[0][6], parameters[0][3], parameters[0][4], parameters[0][5]); 26 | 27 | Eigen::Vector3d t_hat1 = Eigen::Vector3d(parameters[1][0], parameters[1][1], parameters[1][2]); 28 | Eigen::Quaterniond Q_hat1(parameters[1][6], parameters[1][3], parameters[1][4], parameters[1][5]); 29 | 30 | 31 | Eigen::Matrix error; 32 | 33 | Eigen::Quaterniond invdQ_meas = dq_meas_.inverse(); 34 | Eigen::Quaterniond dq_hat = Q_hat0.inverse()*Q_hat1; 35 | Eigen::Quaterniond ddq = invdQ_meas*dq_hat; 36 | //std::cout<<"dq_hat: "<(); 41 | 42 | Eigen::Vector3d dt_hat = (t_hat1 - t_hat0); 43 | error.head<3>() = dt_hat - dt_meas_; 44 | error.tail<3>() = dtheta; 45 | 46 | squareRootInformation_.setIdentity(); 47 | squareRootInformation_ = weightScalar_* squareRootInformation_; //Weighted 48 | 49 | // weight it 50 | Eigen::Map > weighted_error(residuals); 51 | weighted_error = squareRootInformation_ * error; 52 | 53 | // calculate jacobians 54 | if(jacobians != NULL){ 55 | 56 | if(jacobians[0] != NULL){ 57 | 58 | Eigen::Matrix jacobian0_min; 59 | Eigen::Map> jacobian0(jacobians[0]); 60 | 61 | jacobian0_min.setIdentity(); 62 | jacobian0_min.topLeftCorner(3,3) = - Eigen::Matrix3d::Identity(); 63 | jacobian0_min.bottomRightCorner(3,3) 64 | = -(Utility::quatPlus(invdQ_meas)*Utility::quatOplus(dq_hat)).topLeftCorner(3,3); 65 | 66 | 67 | jacobian0_min = squareRootInformation_*jacobian0_min; 68 | 69 | jacobian0 << jacobian0_min, Eigen::Matrix::Zero(); // lift 70 | 71 | if(jacobiansMinimal != NULL && jacobiansMinimal[0] != NULL){ 72 | Eigen::Map> map_jacobian0_min(jacobiansMinimal[0]); 73 | map_jacobian0_min = jacobian0_min; 74 | } 75 | } 76 | 77 | if(jacobians[1] != NULL){ 78 | 79 | Eigen::Matrix jacobian1_min; 80 | Eigen::Map> jacobian1(jacobians[1]); 81 | 82 | jacobian1_min.setIdentity(); 83 | jacobian1_min.bottomRightCorner(3,3) 84 | = Utility::quatPlus(ddq).topLeftCorner(3,3); 85 | 86 | 87 | jacobian1_min = squareRootInformation_*jacobian1_min; 88 | 89 | jacobian1 << jacobian1_min, Eigen::Matrix::Zero(); // lift 90 | 91 | if(jacobiansMinimal != NULL && jacobiansMinimal[1] != NULL){ 92 | Eigen::Map> map_jacobian1_min(jacobiansMinimal[1]); 93 | map_jacobian1_min = jacobian1_min; 94 | } 95 | } 96 | } 97 | 98 | return true; 99 | } 100 | -------------------------------------------------------------------------------- /src/factor/binary_pose_error_factor.hpp: -------------------------------------------------------------------------------- 1 | 2 | #ifndef BINARY_POSE_ERROR_FACTOR_H 3 | #define BINARY_POSE_ERROR_FACTOR_H 4 | #include 5 | #include 6 | #include "ceres/ceres.h" 7 | #include 8 | #include 9 | #include "../utility/utility.h" 10 | 11 | /** 12 | * Binary_pose_error_facotr 13 | * Binary edge to constraint two SE3 poses 14 | * 15 | */ 16 | class Binary_pose_error_facotr: public ceres::SizedCostFunction<6, 7,7>{ 17 | public: 18 | 19 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 20 | /// \brief The base in ceres we derive from 21 | typedef ::ceres::SizedCostFunction<6, 7> base_t; 22 | 23 | /// \brief The number of residuals 24 | static const int kNumResiduals = 6; 25 | 26 | /// \brief The type of the covariance. 27 | typedef Eigen::Matrix covariance_t; 28 | 29 | /// \brief The type of the information (same matrix dimension as covariance). 30 | typedef covariance_t information_t; 31 | 32 | /// \brief The type of hte overall Jacobian. 33 | typedef Eigen::Matrix jacobian_t; 34 | 35 | /// \brief The type of the Jacobian w.r.t. poses -- 36 | /// \warning This is w.r.t. minimal tangential space coordinates... 37 | typedef Eigen::Matrix jacobian0_t; 38 | 39 | Binary_pose_error_facotr(const Eigen::Isometry3d& _Pose0, const Eigen::Isometry3d& _Pose1, 40 | double _t0, double _t1, 41 | double weightScalar = 1); 42 | 43 | /// \brief Trivial destructor. 44 | virtual ~Binary_pose_error_facotr() { } 45 | virtual bool Evaluate(double const *const *parameters, double *residuals, 46 | double **jacobians) const; 47 | 48 | bool EvaluateWithMinimalJacobians(double const *const *parameters, 49 | double *residuals, 50 | double **jacobians, 51 | double **jacobiansMinimal) const; 52 | 53 | private: 54 | Eigen::Isometry3d Pose0_, Pose1_; 55 | Eigen::Quaterniond dq_meas_; 56 | Eigen::Vector3d dt_meas_; 57 | // times, Not uesed now; 58 | double t0,t1; 59 | double weightScalar_; 60 | mutable covariance_t covariance_; ///< 61 | // information matrix and its square root 62 | mutable information_t information_; ///< The information matrix for this error term. 63 | mutable information_t squareRootInformation_; ///< The square root information matrix for this error term. 64 | }; 65 | #endif -------------------------------------------------------------------------------- /src/factor/marginalization_factor.cpp: -------------------------------------------------------------------------------- 1 | #include "marginalization_factor.h" 2 | #include 3 | void ResidualBlockInfo::Evaluate() 4 | { 5 | residuals.resize(cost_function->num_residuals()); 6 | 7 | std::vector block_sizes = cost_function->parameter_block_sizes(); 8 | raw_jacobians = new double *[block_sizes.size()]; 9 | jacobians.resize(block_sizes.size()); 10 | 11 | for (int i = 0; i < static_cast(block_sizes.size()); i++) 12 | { 13 | jacobians[i].resize(cost_function->num_residuals(), block_sizes[i]); 14 | raw_jacobians[i] = jacobians[i].data(); 15 | //dim += block_sizes[i] == 7 ? 6 : block_sizes[i]; 16 | } 17 | cost_function->Evaluate(parameter_blocks.data(), residuals.data(), raw_jacobians); 18 | 19 | //std::vector tmp_idx(block_sizes.size()); 20 | //Eigen::MatrixXd tmp(dim, dim); 21 | //for (int i = 0; i < static_cast(parameter_blocks.size()); i++) 22 | //{ 23 | // int size_i = localSize(block_sizes[i]); 24 | // Eigen::MatrixXd jacobian_i = jacobians[i].leftCols(size_i); 25 | // for (int j = 0, sub_idx = 0; j < static_cast(parameter_blocks.size()); sub_idx += block_sizes[j] == 7 ? 6 : block_sizes[j], j++) 26 | // { 27 | // int size_j = localSize(block_sizes[j]); 28 | // Eigen::MatrixXd jacobian_j = jacobians[j].leftCols(size_j); 29 | // tmp_idx[j] = sub_idx; 30 | // tmp.block(tmp_idx[i], tmp_idx[j], size_i, size_j) = jacobian_i.transpose() * jacobian_j; 31 | // } 32 | //} 33 | //Eigen::SelfAdjointEigenSolver saes(tmp); 34 | //std::cout << saes.eigenvalues() << std::endl; 35 | //ROS_ASSERT(saes.eigenvalues().minCoeff() >= -1e-6); 36 | 37 | // rescale residual and jacobian 38 | if (loss_function) 39 | { 40 | double residual_scaling_, alpha_sq_norm_; 41 | 42 | double sq_norm, rho[3]; 43 | 44 | sq_norm = residuals.squaredNorm(); 45 | loss_function->Evaluate(sq_norm, rho); 46 | //printf("sq_norm: %f, rho[0]: %f, rho[1]: %f, rho[2]: %f\n", sq_norm, rho[0], rho[1], rho[2]); 47 | 48 | double sqrt_rho1_ = sqrt(rho[1]); 49 | 50 | if ((sq_norm == 0.0) || (rho[2] <= 0.0)) 51 | { 52 | residual_scaling_ = sqrt_rho1_; 53 | alpha_sq_norm_ = 0.0; 54 | } 55 | else 56 | { 57 | const double D = 1.0 + 2.0 * sq_norm * rho[2] / rho[1]; 58 | const double alpha = 1.0 - sqrt(D); 59 | residual_scaling_ = sqrt_rho1_ / (1 - alpha); 60 | alpha_sq_norm_ = alpha / sq_norm; 61 | } 62 | 63 | for (int i = 0; i < static_cast(parameter_blocks.size()); i++) 64 | { 65 | jacobians[i] = sqrt_rho1_ * (jacobians[i] - alpha_sq_norm_ * residuals * (residuals.transpose() * jacobians[i])); 66 | } 67 | 68 | residuals *= residual_scaling_; 69 | } 70 | } 71 | 72 | MarginalizationInfo::~MarginalizationInfo() 73 | { 74 | //ROS_WARN("release marginlizationinfo"); 75 | 76 | for (auto it = parameter_block_data.begin(); it != parameter_block_data.end(); ++it) 77 | delete it->second; 78 | 79 | for (int i = 0; i < (int)factors.size(); i++) 80 | { 81 | 82 | delete[] factors[i]->raw_jacobians; 83 | 84 | delete factors[i]->cost_function; 85 | 86 | delete factors[i]; 87 | } 88 | } 89 | 90 | void MarginalizationInfo::addResidualBlockInfo(ResidualBlockInfo *residual_block_info) 91 | { 92 | factors.emplace_back(residual_block_info); 93 | 94 | // book size and which ones to be marginalized 95 | std::vector ¶meter_blocks = residual_block_info->parameter_blocks; 96 | std::vector parameter_block_sizes = residual_block_info->cost_function->parameter_block_sizes(); 97 | 98 | for (int i = 0; i < static_cast(residual_block_info->parameter_blocks.size()); i++) 99 | { 100 | double *addr = parameter_blocks[i]; 101 | int size = parameter_block_sizes[i]; 102 | parameter_block_size[reinterpret_cast(addr)] = size; 103 | } 104 | 105 | // record drop_set in parameter_block_idx 106 | for (int i = 0; i < static_cast(residual_block_info->drop_set.size()); i++) 107 | { 108 | double *addr = parameter_blocks[residual_block_info->drop_set[i]]; 109 | parameter_block_idx[reinterpret_cast(addr)] = 0; 110 | } 111 | } 112 | 113 | void MarginalizationInfo::preMarginalize() 114 | { 115 | 116 | // fill parameter_block_data according address 117 | //std::cout<<"factors size: "<Evaluate(); 121 | 122 | 123 | std::vector block_sizes = it->cost_function->parameter_block_sizes(); 124 | for (int i = 0; i < static_cast(block_sizes.size()); i++) 125 | { 126 | long addr = reinterpret_cast(it->parameter_blocks[i]); 127 | int size = block_sizes[i]; 128 | if (parameter_block_data.find(addr) == parameter_block_data.end()) 129 | { 130 | // copy the paramter 131 | double *data = new double[size]; 132 | memcpy(data, it->parameter_blocks[i], sizeof(double) * size); 133 | parameter_block_data[addr] = data; 134 | } 135 | } 136 | } 137 | } 138 | 139 | int MarginalizationInfo::localSize(int size) const 140 | { 141 | return size == 7 ? 6 : size; 142 | } 143 | 144 | int MarginalizationInfo::globalSize(int size) const 145 | { 146 | return size == 6 ? 7 : size; 147 | } 148 | 149 | void MarginalizationInfo::marginalize() 150 | { 151 | // re-order the marginalization parts and residual parts 152 | // Marginalizations lie first, followed by the residual parts. 153 | // m for total size to marginalize; n for the totall size of residual 154 | int pos = 0; 155 | for (auto &it : parameter_block_idx) 156 | { 157 | it.second = pos; 158 | pos += localSize(parameter_block_size[it.first]); 159 | } 160 | 161 | m = pos; 162 | 163 | for (const auto &it : parameter_block_size) 164 | { 165 | if (parameter_block_idx.find(it.first) == parameter_block_idx.end()) 166 | { 167 | // record residual parameter in parameter_block_idx 168 | parameter_block_idx[it.first] = pos; 169 | pos += localSize(it.second); 170 | } 171 | } 172 | 173 | n = pos - m; 174 | 175 | // Form the Linear H * dx = -b 176 | Eigen::MatrixXd H(pos, pos); 177 | Eigen::VectorXd b(pos); 178 | H.setZero(); 179 | b.setZero(); 180 | for (auto it : factors) 181 | { 182 | for (int i = 0; i < static_cast(it->parameter_blocks.size()); i++) 183 | { 184 | int idx_i = parameter_block_idx[reinterpret_cast(it->parameter_blocks[i])]; 185 | int size_i = localSize(parameter_block_size[reinterpret_cast(it->parameter_blocks[i])]); 186 | Eigen::MatrixXd jacobian_i = it->jacobians[i].leftCols(size_i); 187 | for (int j = i; j < static_cast(it->parameter_blocks.size()); j++) 188 | { 189 | int idx_j = parameter_block_idx[reinterpret_cast(it->parameter_blocks[j])]; 190 | int size_j = localSize(parameter_block_size[reinterpret_cast(it->parameter_blocks[j])]); 191 | Eigen::MatrixXd jacobian_j = it->jacobians[j].leftCols(size_j); 192 | if (i == j) 193 | H.block(idx_i, idx_j, size_i, size_j) += jacobian_i.transpose() * jacobian_j; 194 | else 195 | { 196 | H.block(idx_i, idx_j, size_i, size_j) += jacobian_i.transpose() * jacobian_j; 197 | H.block(idx_j, idx_i, size_j, size_i) = H.block(idx_i, idx_j, size_i, size_j).transpose(); 198 | } 199 | } 200 | b.segment(idx_i, size_i) += jacobian_i.transpose() * it->residuals; 201 | } 202 | } 203 | //ALGO_INFO("summing up costs %f ms", t_summing.toc()); 204 | 205 | //TODO 206 | Eigen::MatrixXd Hmm = 0.5 * (H.block(0, 0, m, m) + H.block(0, 0, m, m).transpose()); 207 | Eigen::SelfAdjointEigenSolver saes(Hmm); 208 | 209 | //ROS_ASSERT_MSG(saes.eigenvalues().minCoeff() >= -1e-4, "min eigenvalue %f", saes.eigenvalues().minCoeff()); 210 | 211 | Eigen::MatrixXd Hmm_inv = 212 | saes.eigenvectors() 213 | * Eigen::VectorXd((saes.eigenvalues().array() > eps).select(saes.eigenvalues().array().inverse(), 0)).asDiagonal() 214 | * saes.eigenvectors().transpose(); 215 | //printf("error1: %f\n", (Amm * Amm_inv - Eigen::MatrixXd::Identity(m, m)).sum()); 216 | 217 | //Shur 218 | Eigen::VectorXd bmm = b.segment(0, m); 219 | Eigen::MatrixXd Hmr = H.block(0, m, m, n); 220 | Eigen::MatrixXd Hrm = H.block(m, 0, n, m); 221 | Eigen::MatrixXd Hrr = H.block(m, m, n, n); 222 | Eigen::VectorXd brr = b.segment(m, n); 223 | H = Hrr - Hrm * Hmm_inv * Hmr; 224 | b = brr - Hrm * Hmm_inv * bmm; 225 | 226 | // 227 | // std::cout<<"after marginalize: "< saes2(H); 234 | Eigen::VectorXd S = Eigen::VectorXd((saes2.eigenvalues().array() > eps).select(saes2.eigenvalues().array(), 0)); 235 | Eigen::VectorXd S_inv = Eigen::VectorXd((saes2.eigenvalues().array() > eps).select(saes2.eigenvalues().array().inverse(), 0)); 236 | 237 | Eigen::VectorXd S_sqrt = S.cwiseSqrt(); 238 | Eigen::VectorXd S_inv_sqrt = S_inv.cwiseSqrt(); 239 | 240 | linearized_jacobians = S_sqrt.asDiagonal() * saes2.eigenvectors().transpose(); 241 | linearized_residuals = S_inv_sqrt.asDiagonal() * saes2.eigenvectors().transpose() * b; 242 | //std::cout << A << std::endl 243 | // << std::endl; 244 | //std::cout << linearized_jacobians << std::endl; 245 | //printf("error2: %f %f\n", (linearized_jacobians.transpose() * linearized_jacobians - A).sum(), 246 | // (linearized_jacobians.transpose() * linearized_residuals - b).sum()); 247 | } 248 | 249 | std::vector MarginalizationInfo::getParameterBlocks(std::unordered_map &addr_shift) 250 | { 251 | std::vector keep_block_addr; 252 | keep_block_size.clear(); 253 | keep_block_idx.clear(); 254 | keep_block_data.clear(); 255 | 256 | for (const auto &it : parameter_block_idx) 257 | { 258 | // book-keep the residual parameters 259 | if (it.second >= m) 260 | { 261 | keep_block_size.push_back(parameter_block_size[it.first]); 262 | keep_block_idx.push_back(parameter_block_idx[it.first]); 263 | keep_block_data.push_back(parameter_block_data[it.first]); 264 | keep_block_addr.push_back(addr_shift[it.first]); 265 | } 266 | } 267 | sum_block_size = std::accumulate(std::begin(keep_block_size), std::end(keep_block_size), 0); 268 | 269 | return keep_block_addr; 270 | } 271 | 272 | MarginalizationFactor::MarginalizationFactor(MarginalizationInfo* _marginalization_info): 273 | marginalization_info(_marginalization_info) 274 | { 275 | int cnt = 0; 276 | for (auto it : marginalization_info->keep_block_size) 277 | { 278 | mutable_parameter_block_sizes()->push_back(it); 279 | cnt += it; 280 | } 281 | //printf("residual size: %d, %d\n", cnt, n); 282 | set_num_residuals(marginalization_info->n); 283 | }; 284 | 285 | bool MarginalizationFactor::Evaluate(double const *const *parameters, double *residuals, double **jacobians) const 286 | { 287 | return EvaluateWithMinimalJacobians(parameters, 288 | residuals, 289 | jacobians, NULL); 290 | } 291 | 292 | bool MarginalizationFactor::EvaluateWithMinimalJacobians(double const *const *parameters, 293 | double *residuals, 294 | double **jacobians, 295 | double **jacobiansMinimal) const { 296 | 297 | //printf("internal addr,%d, %d\n", (int)parameter_block_sizes().size(), num_residuals()); 298 | //for (int i = 0; i < static_cast(keep_block_size.size()); i++) 299 | //{ 300 | // //printf("unsigned %x\n", reinterpret_cast(parameters[i])); 301 | // //printf("signed %x\n", reinterpret_cast(parameters[i])); 302 | //printf("jacobian %x\n", reinterpret_cast(jacobians)); 303 | //printf("residual %x\n", reinterpret_cast(residuals)); 304 | //} 305 | 306 | 307 | // Fix the linear point 308 | int n = marginalization_info->n; 309 | int m = marginalization_info->m; 310 | Eigen::VectorXd dx(n); 311 | for (int i = 0; i < static_cast(marginalization_info->keep_block_size.size()); i++) 312 | { 313 | int size = marginalization_info->keep_block_size[i]; 314 | int idx = marginalization_info->keep_block_idx[i] - m; 315 | Eigen::VectorXd x = Eigen::Map(parameters[i], size); 316 | Eigen::VectorXd x0 = Eigen::Map(marginalization_info->keep_block_data[i], size); 317 | // std::cout<<"X: "<(idx + 0) = x.head<3>() - x0.head<3>(); 325 | dx.segment<3>(idx + 3) 326 | = 2.0 * Utility::positify(Eigen::Quaterniond(x0(6), x0(3), x0(4), x0(5)).inverse() * Eigen::Quaterniond(x(6), x(3), x(4), x(5))).vec(); 327 | if (!((Eigen::Quaterniond(x0(6), x0(3), x0(4), x0(5)).inverse() * Eigen::Quaterniond(x(6), x(3), x(4), x(5))).w() >= 0)) 328 | { 329 | dx.segment<3>(idx + 3) 330 | = 2.0 * -Utility::positify(Eigen::Quaterniond(x0(6), x0(3), x0(4), x0(5)).inverse() * Eigen::Quaterniond(x(6), x(3), x(4), x(5))).vec(); 331 | } 332 | } 333 | // std::cout<<"dx: "<linearized_residuals.transpose()<linearized_jacobians<(residuals, n) = marginalization_info->linearized_residuals 342 | + marginalization_info->linearized_jacobians * dx; 343 | 344 | if (jacobians) 345 | { 346 | for (int i = 0; i < static_cast(marginalization_info->keep_block_size.size()); i++) 347 | { 348 | if (jacobians[i]) 349 | { 350 | int size = marginalization_info->keep_block_size[i]; 351 | int local_size = marginalization_info->localSize(size); 352 | int idx = marginalization_info->keep_block_idx[i] - m; 353 | Eigen::Map> 354 | jacobian(jacobians[i], n, size); 355 | 356 | // quaternion lift 357 | jacobian.setZero(); 358 | jacobian.leftCols(local_size) 359 | = marginalization_info->linearized_jacobians.middleCols(idx, local_size); 360 | 361 | 362 | if (jacobiansMinimal && jacobiansMinimal[i]) { 363 | Eigen::Map> 364 | jacobian_min(jacobiansMinimal[i], n, local_size); 365 | jacobian_min.leftCols(local_size) 366 | = marginalization_info->linearized_jacobians.middleCols(idx, local_size); 367 | } 368 | } 369 | } 370 | } 371 | return true; 372 | } 373 | -------------------------------------------------------------------------------- /src/factor/marginalization_factor.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | #include "../utility/utility.h" 9 | 10 | 11 | /* 12 | * Used to store any kinds of factors information 13 | * 14 | */ 15 | struct ResidualBlockInfo 16 | { 17 | ResidualBlockInfo(ceres::CostFunction *_cost_function, 18 | ceres::LossFunction *_loss_function, 19 | std::vector _parameter_blocks, 20 | std::vector _drop_set) 21 | : cost_function(_cost_function), 22 | loss_function(_loss_function), 23 | parameter_blocks(_parameter_blocks), // 24 | drop_set(_drop_set) {} 25 | 26 | void Evaluate(); 27 | 28 | ceres::CostFunction *cost_function; 29 | ceres::LossFunction *loss_function; 30 | std::vector parameter_blocks; 31 | std::vector drop_set; 32 | 33 | double **raw_jacobians; 34 | std::vector> jacobians; 35 | Eigen::VectorXd residuals; 36 | 37 | int localSize(int size) 38 | { 39 | return size == 7 ? 6 : size; 40 | } 41 | }; 42 | 43 | struct ThreadsStruct 44 | { 45 | std::vector sub_factors; 46 | Eigen::MatrixXd A; 47 | Eigen::VectorXd b; 48 | std::unordered_map parameter_block_size; //global size 49 | std::unordered_map parameter_block_idx; //local size 50 | }; 51 | 52 | class MarginalizationInfo 53 | { 54 | public: 55 | ~MarginalizationInfo(); 56 | int localSize(int size) const; 57 | int globalSize(int size) const; 58 | void addResidualBlockInfo(ResidualBlockInfo *residual_block_info); 59 | void preMarginalize(); 60 | void marginalize(); 61 | std::vector getParameterBlocks(std::unordered_map &addr_shift); 62 | 63 | std::vector factors; // factors involing to parametre need be marginalized 64 | int m, n; 65 | std::unordered_map parameter_block_size; //global size, map address to size 66 | int sum_block_size; 67 | std::unordered_map parameter_block_idx; //local size, map address 68 | std::unordered_map parameter_block_data; // map address 69 | 70 | std::vector keep_block_size; //global size 71 | std::vector keep_block_idx; //local size 72 | std::vector keep_block_data; 73 | 74 | Eigen::MatrixXd linearized_jacobians; // H 75 | Eigen::VectorXd linearized_residuals; // b 76 | const double eps = 1e-8; 77 | 78 | }; 79 | 80 | class MarginalizationFactor : public ceres::CostFunction 81 | { 82 | public: 83 | MarginalizationFactor(MarginalizationInfo* _marginalization_info); 84 | 85 | virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const; 86 | 87 | bool EvaluateWithMinimalJacobians(double const *const *parameters, 88 | double *residuals, 89 | double **jacobians, 90 | double **jacobiansMinimal) const; 91 | 92 | MarginalizationInfo* marginalization_info; 93 | }; 94 | -------------------------------------------------------------------------------- /src/factor/pinhole_project_factor.cpp: -------------------------------------------------------------------------------- 1 | #include "pinhole_project_factor.h" 2 | PinholeProjectFactor::PinholeProjectFactor(const Eigen::Vector3d& uv_C0, 3 | const Eigen::Vector3d& uv_C1): 4 | C0uv(uv_C0), 5 | C1uv(uv_C1) {} 6 | 7 | 8 | bool PinholeProjectFactor::Evaluate(double const *const *parameters, 9 | double *residuals, 10 | double **jacobians) const { 11 | return EvaluateWithMinimalJacobians(parameters, 12 | residuals, 13 | jacobians, NULL); 14 | } 15 | 16 | 17 | bool PinholeProjectFactor::EvaluateWithMinimalJacobians(double const *const *parameters, 18 | double *residuals, 19 | double **jacobians, 20 | double **jacobiansMinimal) const { 21 | 22 | // T_WI0 23 | Eigen::Vector3d t_WI0(parameters[0][0], parameters[0][1], parameters[0][2]); 24 | Eigen::Quaterniond Q_WI0(parameters[0][6], parameters[0][3], parameters[0][4], parameters[0][5]); 25 | 26 | // T_WI1 27 | Eigen::Vector3d t_WI1(parameters[1][0], parameters[1][1], parameters[1][2]); 28 | Eigen::Quaterniond Q_WI1(parameters[1][6], parameters[1][3], parameters[1][4], parameters[1][5]); 29 | 30 | // T_IC 31 | Eigen::Vector3d t_IC(parameters[2][0], parameters[2][1], parameters[2][2]); 32 | Eigen::Quaterniond Q_IC(parameters[2][6], parameters[2][3], parameters[2][4], parameters[2][5]); 33 | 34 | // rho 35 | double inv_dep = parameters[3][0]; 36 | 37 | 38 | Eigen::Vector3d C0p = C0uv / inv_dep; 39 | Eigen::Vector3d I0p = Q_IC * C0p + t_IC; 40 | Eigen::Vector3d Wp = Q_WI0 * I0p + t_WI0; 41 | Eigen::Vector3d I1p = Q_WI1.inverse() * (Wp - t_WI1); 42 | Eigen::Vector3d C1p = Q_IC.inverse() * (I1p - t_IC); 43 | 44 | Eigen::Matrix error; 45 | 46 | double inv_z = 1/C1p(2); 47 | Eigen::Vector2d hat_C1uv(C1p(0)*inv_z, C1p(1)*inv_z); 48 | 49 | Eigen::Matrix H; 50 | H << 1, 0, -C1p(0)*inv_z, 51 | 0, 1, -C1p(1)*inv_z; 52 | H *= inv_z; 53 | 54 | error = hat_C1uv - C1uv.head<2>(); 55 | squareRootInformation_.setIdentity(); 56 | //squareRootInformation_ = weightScalar_* squareRootInformation_; //Weighted 57 | 58 | // weight it 59 | Eigen::Map > weighted_error(residuals); 60 | weighted_error = squareRootInformation_ * error; 61 | 62 | Eigen::Matrix3d R_WI0 = Q_WI0.toRotationMatrix(); 63 | Eigen::Matrix3d R_WI1 = Q_WI1.toRotationMatrix(); 64 | Eigen::Matrix3d R_IC = Q_IC.toRotationMatrix(); 65 | 66 | // calculate jacobians 67 | if(jacobians != NULL){ 68 | 69 | if(jacobians[0] != NULL){ 70 | 71 | Eigen::Matrix jacobian0_min; 72 | Eigen::Map> jacobian0(jacobians[0]); 73 | 74 | Eigen::Matrix tmp; 75 | tmp.setIdentity(); 76 | tmp.topLeftCorner(3,3) = R_IC.transpose()*R_WI1.transpose(); 77 | tmp.topRightCorner(3,3) = - R_IC.transpose()*R_WI1.transpose()*R_WI0*Utility::skewSymmetric(I0p); 78 | 79 | 80 | jacobian0_min = H*tmp; 81 | 82 | jacobian0_min = squareRootInformation_*jacobian0_min; 83 | 84 | jacobian0 << jacobian0_min, Eigen::Matrix::Zero(); // lift 85 | 86 | if(jacobiansMinimal != NULL && jacobiansMinimal[0] != NULL){ 87 | Eigen::Map> map_jacobian0_min(jacobiansMinimal[0]); 88 | map_jacobian0_min = jacobian0_min; 89 | } 90 | } 91 | 92 | if(jacobians[1] != NULL){ 93 | 94 | Eigen::Matrix jacobian1_min; 95 | Eigen::Map> jacobian1(jacobians[1]); 96 | 97 | Eigen::Matrix tmp; 98 | 99 | tmp.setIdentity(); 100 | tmp.topLeftCorner(3,3) = -R_IC.transpose()*R_WI1.transpose(); 101 | tmp.bottomRightCorner(3,3) = R_IC.transpose() * Utility::skewSymmetric(I1p); 102 | 103 | jacobian1_min = H*tmp; 104 | jacobian1_min = squareRootInformation_*jacobian1_min; 105 | 106 | jacobian1 << jacobian1_min, Eigen::Matrix::Zero(); // lift 107 | 108 | if(jacobiansMinimal != NULL && jacobiansMinimal[1] != NULL){ 109 | Eigen::Map> map_jacobian1_min(jacobiansMinimal[1]); 110 | map_jacobian1_min = jacobian1_min; 111 | } 112 | } 113 | if(jacobians[2] != NULL){ 114 | 115 | Eigen::Matrix jacobian2_min; 116 | Eigen::Map> jacobian2(jacobians[2]); 117 | 118 | Eigen::Matrix tmp; 119 | 120 | tmp.setIdentity(); 121 | tmp.topLeftCorner(3,3) = R_IC.transpose() * (R_WI1.transpose() * R_WI0 - Eigen::Matrix3d::Identity()); 122 | Eigen::Matrix3d tmp_r = R_IC.transpose() * R_WI1.transpose() * R_WI0 * R_IC; 123 | tmp.bottomRightCorner(3,3) = -tmp_r * Utility::skewSymmetric(C0p) + Utility::skewSymmetric(tmp_r * C0p) + 124 | Utility::skewSymmetric(R_IC.transpose() * (R_WI1.transpose() * (R_WI0 * t_IC + t_WI0 - t_WI1) - t_IC)); 125 | 126 | jacobian2_min = H*tmp; 127 | jacobian2_min = squareRootInformation_*jacobian2_min; 128 | 129 | jacobian2 << jacobian2_min, Eigen::Matrix::Zero(); // lift 130 | 131 | if(jacobiansMinimal != NULL && jacobiansMinimal[2] != NULL){ 132 | Eigen::Map> map_jacobian2_min(jacobiansMinimal[2]); 133 | map_jacobian2_min = jacobian2_min; 134 | } 135 | } 136 | 137 | if(jacobians[3] != NULL){ 138 | Eigen::Map> jacobian3(jacobians[3]); 139 | jacobian3 = - H*R_IC.transpose()*R_WI1.transpose()*R_WI0*R_IC*C0uv/(inv_dep*inv_dep); 140 | jacobian3 = squareRootInformation_*jacobian3; 141 | 142 | 143 | if(jacobiansMinimal != NULL && jacobiansMinimal[3] != NULL){ 144 | Eigen::Map> map_jacobian3_min(jacobiansMinimal[3]); 145 | map_jacobian3_min = jacobian3; 146 | } 147 | } 148 | 149 | 150 | } 151 | 152 | return true; 153 | } 154 | -------------------------------------------------------------------------------- /src/factor/pinhole_project_factor.h: -------------------------------------------------------------------------------- 1 | 2 | #ifndef DEPTH_PROJECT_FACTOR_H 3 | #define DEPTH_PROJECT_FACTOR_H 4 | #include 5 | #include 6 | #include "ceres/ceres.h" 7 | #include 8 | #include 9 | #include "../utility/utility.h" 10 | 11 | /** 12 | * 13 | */ 14 | class PinholeProjectFactor:public ceres::SizedCostFunction<2, /* num of residual */ 15 | 7, /* parameter of pose */ 16 | 7, /* parameter of pose */ 17 | 7, 18 | 1>{ 19 | 20 | public: 21 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 22 | /// \brief The base in ceres we derive from 23 | typedef ::ceres::SizedCostFunction<2, 7, 7> base_t; 24 | 25 | /// \brief The number of residuals 26 | static const int kNumResiduals = 2; 27 | 28 | /// \brief The type of the covariance. 29 | typedef Eigen::Matrix covariance_t; 30 | 31 | /// \brief The type of the information (same matrix dimension as covariance). 32 | typedef covariance_t information_t; 33 | 34 | /// \brief The type of hte overall Jacobian. 35 | typedef Eigen::Matrix jacobian_t; 36 | 37 | /// \brief The type of the Jacobian w.r.t. poses -- 38 | /// \warning This is w.r.t. minimal tangential space coordinates... 39 | typedef Eigen::Matrix jacobian0_t; 40 | 41 | PinholeProjectFactor() = delete; 42 | PinholeProjectFactor(const Eigen::Vector3d& uv_C0, 43 | const Eigen::Vector3d& uv_C1); 44 | 45 | /// \brief Trivial destructor. 46 | virtual ~PinholeProjectFactor() {} 47 | 48 | virtual bool Evaluate(double const *const *parameters, double *residuals, 49 | double **jacobians) const; 50 | 51 | bool EvaluateWithMinimalJacobians(double const *const *parameters, 52 | double *residuals, 53 | double **jacobians, 54 | double **jacobiansMinimal) const; 55 | 56 | private: 57 | 58 | Eigen::Vector3d C0uv; 59 | Eigen::Vector3d C1uv; 60 | 61 | // information matrix and its square root 62 | mutable information_t information_; ///< The information matrix for this error term. 63 | mutable information_t squareRootInformation_; ///< The square root information matrix for this error term. 64 | }; 65 | 66 | #endif 67 | 68 | 69 | /* 70 | * 71 | */ -------------------------------------------------------------------------------- /src/factor/pose_local_parameterization.cpp: -------------------------------------------------------------------------------- 1 | #include "pose_local_parameterization.h" 2 | 3 | //Instances of LocalParameterization implement the ⊞ operation. 4 | bool PoseLocalParameterization::Plus(const double *x, const double *delta, double *x_plus_delta) const 5 | { 6 | Eigen::Map _p(x); 7 | Eigen::Map _q(x + 3); 8 | 9 | Eigen::Map dp(delta); 10 | 11 | Eigen::Quaterniond dq = Utility::deltaQ(Eigen::Map(delta + 3)); 12 | 13 | Eigen::Map p(x_plus_delta); 14 | Eigen::Map q(x_plus_delta + 3); 15 | 16 | p = _p + dp; 17 | q = (_q * dq).normalized(); 18 | 19 | return true; 20 | } 21 | 22 | //And its derivative with respect to Δx at Δx=0. 23 | // This is correct! This jacobian should be 4*3 matrix and be the transpose matrix of lift jacobian matrix(3*4) 24 | // In projectotion_factor we can deduce the lift jacobian is [I3 0]. 25 | bool PoseLocalParameterization::ComputeJacobian(const double *x, double *jacobian) const 26 | { 27 | Eigen::Map> j(jacobian); 28 | j.topRows<6>().setIdentity(); 29 | j.bottomRows<1>().setZero(); 30 | 31 | 32 | return true; 33 | } 34 | 35 | 36 | 37 | -------------------------------------------------------------------------------- /src/factor/pose_local_parameterization.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include "../utility/utility.h" 6 | 7 | class PoseLocalParameterization : public ceres::LocalParameterization 8 | { 9 | public: 10 | virtual bool Plus(const double *x, const double *delta, double *x_plus_delta) const; 11 | virtual bool ComputeJacobian(const double *x, double *jacobian) const; 12 | virtual int GlobalSize() const { return 7; }; 13 | virtual int LocalSize() const { return 6; }; 14 | }; 15 | -------------------------------------------------------------------------------- /src/factor/project_error.cpp: -------------------------------------------------------------------------------- 1 | #include "project_error.h" 2 | #include "pose_local_parameterization.h" 3 | ProjectError::ProjectError(const Eigen::Vector3d& uv_C0): 4 | C0uv(uv_C0){ 5 | } 6 | 7 | bool ProjectError::Evaluate(double const *const *parameters, 8 | double *residuals, 9 | double **jacobians) const { 10 | return EvaluateWithMinimalJacobians(parameters, 11 | residuals, 12 | jacobians, NULL); 13 | } 14 | 15 | 16 | bool ProjectError::EvaluateWithMinimalJacobians(double const *const *parameters, 17 | double *residuals, 18 | double **jacobians, 19 | double **jacobiansMinimal) const { 20 | 21 | // T_WC 22 | Eigen::Vector3d t_WC(parameters[0][0], parameters[0][1], parameters[0][2]); 23 | Eigen::Quaterniond Q_WC(parameters[0][6], parameters[0][3], parameters[0][4], parameters[0][5]); 24 | 25 | // Wp 26 | Eigen::Vector3d Wp(parameters[1][0], parameters[1][1], parameters[1][2]); 27 | 28 | Eigen::Matrix3d R_WC = Q_WC.toRotationMatrix(); 29 | Eigen::Vector3d Cp = R_WC.transpose()*(Wp - t_WC); 30 | Eigen::Matrix error; 31 | 32 | double inv_z = 1/Cp(2); 33 | Eigen::Vector2d hat_C0uv(Cp(0)*inv_z, Cp(1)*inv_z); 34 | 35 | Eigen::Matrix H; 36 | H << 1, 0, -Cp(0)*inv_z, 37 | 0, 1, -Cp(1)*inv_z; 38 | H *= inv_z; 39 | 40 | error = hat_C0uv - C0uv.head<2>(); 41 | squareRootInformation_.setIdentity(); 42 | //squareRootInformation_ = weightScalar_* squareRootInformation_; //Weighted 43 | 44 | // weight it 45 | Eigen::Map > weighted_error(residuals); 46 | weighted_error = squareRootInformation_ * error; 47 | 48 | 49 | 50 | // calculate jacobians 51 | if(jacobians != NULL){ 52 | 53 | if(jacobians[0] != NULL){ 54 | Eigen::Matrix jacobian0_min; 55 | Eigen::Map> jacobian0(jacobians[0]); 56 | 57 | Eigen::Matrix tmp; 58 | tmp.setIdentity(); 59 | tmp.topLeftCorner(3,3) = - R_WC.transpose(); 60 | tmp.topRightCorner(3,3) = Utility::skewSymmetric(R_WC.transpose()*(Wp - t_WC)); 61 | 62 | jacobian0_min = H*tmp; 63 | 64 | 65 | jacobian0 << squareRootInformation_*jacobian0_min, Eigen::Matrix::Zero(); 66 | 67 | 68 | 69 | if(jacobiansMinimal != NULL && jacobiansMinimal[0] != NULL){ 70 | Eigen::Map> map_jacobian0_min(jacobiansMinimal[0]); 71 | map_jacobian0_min = squareRootInformation_*jacobian0_min; 72 | } 73 | } 74 | 75 | if(jacobians[1] != NULL){ 76 | Eigen::Matrix jacobian1_min; 77 | Eigen::Map> jacobian1(jacobians[1]); 78 | 79 | Eigen::Matrix tmp; 80 | 81 | tmp = R_WC.transpose(); 82 | jacobian1_min = H*tmp; 83 | 84 | jacobian1 = squareRootInformation_*jacobian1_min; 85 | 86 | if(jacobiansMinimal != NULL && jacobiansMinimal[1] != NULL){ 87 | Eigen::Map> map_jacobian1_min(jacobiansMinimal[1]); 88 | map_jacobian1_min = squareRootInformation_*jacobian1_min; 89 | } 90 | } 91 | 92 | } 93 | return true; 94 | } 95 | -------------------------------------------------------------------------------- /src/factor/project_error.h: -------------------------------------------------------------------------------- 1 | 2 | #ifndef PROJECT_FACTOR_H 3 | #define PROJECT_FACTOR_H 4 | #include 5 | #include 6 | #include "ceres/ceres.h" 7 | #include 8 | #include 9 | 10 | /** 11 | * 12 | */ 13 | class ProjectError:public ceres::SizedCostFunction<2, /* num of residual */ 14 | 7, /* parameter of pose */ 15 | 3>{ 16 | 17 | public: 18 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 19 | /// \brief The base in ceres we derive from 20 | typedef ::ceres::SizedCostFunction<2, 7, 7> base_t; 21 | 22 | /// \brief The number of residuals 23 | static const int kNumResiduals = 2; 24 | 25 | /// \brief The type of the covariance. 26 | typedef Eigen::Matrix covariance_t; 27 | 28 | /// \brief The type of the information (same matrix dimension as covariance). 29 | typedef covariance_t information_t; 30 | 31 | /// \brief The type of hte overall Jacobian. 32 | typedef Eigen::Matrix jacobian_t; 33 | 34 | /// \brief The type of the Jacobian w.r.t. poses -- 35 | /// \warning This is w.r.t. minimal tangential space coordinates... 36 | typedef Eigen::Matrix jacobian0_t; 37 | 38 | ProjectError() = delete; 39 | ProjectError(const Eigen::Vector3d& uv_C0); 40 | 41 | /// \brief Trivial destructor. 42 | virtual ~ProjectError() {} 43 | 44 | virtual bool Evaluate(double const *const *parameters, double *residuals, 45 | double **jacobians) const; 46 | 47 | bool EvaluateWithMinimalJacobians(double const *const *parameters, 48 | double *residuals, 49 | double **jacobians, 50 | double **jacobiansMinimal) const; 51 | 52 | private: 53 | 54 | Eigen::Vector3d C0uv; 55 | 56 | // information matrix and its square root 57 | mutable information_t information_; ///< The information matrix for this error term. 58 | mutable information_t squareRootInformation_; ///< The square root information matrix for this error term. 59 | }; 60 | 61 | #endif 62 | 63 | 64 | /* 65 | * 66 | */ -------------------------------------------------------------------------------- /src/utility/num-diff.hpp: -------------------------------------------------------------------------------- 1 | /*! A simple tools to do finity num-diff 2 | * 3 | * Filename: num-diff.hpp 4 | * Version: 0.10 5 | * Author: Pang Fumin 6 | */ 7 | 8 | #ifndef NUMBDIFFERENTIATOR_H 9 | #define NUMBDIFFERENTIATOR_H 10 | #include 11 | 12 | #include 13 | 14 | #define Eps (1e-6) 15 | 16 | 17 | /** 18 | * A simple class to do quickly finite differencing of an error functor to get 19 | * a numeric-diff jacobian for minimal parameterization. 20 | * 21 | * You can also use ceres::GradientChecker. 22 | * 23 | * @Author: Pang Fumin 24 | */ 25 | 26 | template 27 | class NumDiff{ 28 | 29 | public: 30 | 31 | 32 | NumDiff(Functor* ptrErrorFunctor): 33 | ptrErrorFunctor_(ptrErrorFunctor){ 34 | 35 | if(ptrErrorFunctor_ == NULL){ 36 | LOG(FATAL)<<"Error functor pointor is NULL!"; 37 | } 38 | 39 | }; 40 | 41 | /** 42 | * Diff function for the parameters with minimal-parameterization 43 | * @tparam ResidualDim 44 | * @tparam ParamDim 45 | * @tparam MinimalParamDim 46 | * @tparam LoaclPrameter 47 | * @param parameters 48 | * @param paramId 49 | * @param jacobiansMinimal 50 | * @return 51 | */ 52 | template 56 | bool df_r_xi(double** parameters, /* full parameters*/ 57 | unsigned int paramId, 58 | double* jacobiansMinimal){ 59 | 60 | 61 | std::shared_ptr ptrlocalParemeter(new LoaclPrameter); 62 | Eigen::Map> miniJacobian(jacobiansMinimal); 63 | Eigen::Map> xi(parameters[paramId]); 64 | Eigen::Matrix residual_plus; 65 | Eigen::Matrix residual_minus; 66 | 67 | Eigen::Matrix xi_plus_delta, xi_minus_delta; 68 | Eigen::Matrix delta; 69 | 70 | for(unsigned int i = 0; i < MinimalParamDim; i++){ 71 | 72 | // plus 73 | delta.setZero(); 74 | delta(i) = Eps; 75 | ptrlocalParemeter->Plus(xi.data(),delta.data(),xi_plus_delta.data()); 76 | double* parameter_plus[ParamBlockSize]; 77 | applyDistribance(parameters,xi_plus_delta.data(),parameter_plus,paramId); 78 | ptrErrorFunctor_->Evaluate(parameter_plus,residual_plus.data(),NULL); 79 | 80 | // minus 81 | delta.setZero(); 82 | delta(i) = -Eps; 83 | ptrlocalParemeter->Plus(xi.data(),delta.data(),xi_minus_delta.data()); 84 | double* parameter_minus[ParamBlockSize]; 85 | applyDistribance(parameters,xi_minus_delta.data(),parameter_minus,paramId); 86 | ptrErrorFunctor_->Evaluate(parameter_minus,residual_minus.data(),NULL); 87 | 88 | // diff 89 | miniJacobian.col(i) = (residual_plus - residual_minus)/(2.0*Eps); 90 | 91 | 92 | } 93 | 94 | return true; 95 | }; 96 | 97 | template 99 | bool df_r_xi(double** parameters, 100 | unsigned int paramId, 101 | double* jacobian){ 102 | typedef Eigen::Matrix 1? Eigen::RowMajor : Eigen::ColMajor)> JocabinType; 103 | 104 | Eigen::Map Jacobian(jacobian); 105 | Eigen::Map> xi(parameters[paramId]); 106 | Eigen::Matrix residual_plus; 107 | Eigen::Matrix residual_minus; 108 | 109 | Eigen::Matrix xi_plus_delta, xi_minus_delta; 110 | Eigen::Matrix delta; 111 | 112 | for (unsigned int i = 0; i < ParamDim; i++) { 113 | 114 | delta.setZero(); 115 | delta(i) = Eps; 116 | xi_plus_delta = xi + delta; 117 | double *parameter_plus[ParamBlockSize]; 118 | applyDistribance(parameters, xi_plus_delta.data(), parameter_plus, paramId); 119 | ptrErrorFunctor_->Evaluate(parameter_plus, residual_plus.data(), NULL); 120 | 121 | 122 | xi_minus_delta = xi - delta; 123 | double *parameter_minus[ParamBlockSize]; 124 | applyDistribance(parameters, xi_minus_delta.data(), parameter_minus, paramId); 125 | ptrErrorFunctor_->Evaluate(parameter_minus, residual_minus.data(), NULL); 126 | 127 | Jacobian.col(i) = (residual_plus - residual_minus) / (2.0 * Eps); 128 | 129 | } 130 | 131 | return true; 132 | }; 133 | 134 | 135 | /** 136 | * 137 | * @tparam ResidualDim 138 | * @tparam ParamDim 139 | * @param Jacobian_a 140 | * @param Jacobian_b 141 | * @param relTol 142 | * @return 143 | */ 144 | template 146 | static bool isJacobianEqual(double* Jacobian_a, 147 | double* Jacobian_b, 148 | double relTol = 1e-4) { 149 | 150 | Eigen::Map> jacobian_a(Jacobian_a); 151 | Eigen::Map> jacobian_b(Jacobian_b); 152 | 153 | 154 | bool isCorrect = true; 155 | // check 156 | double norm = jacobian_a.norm(); 157 | Eigen::MatrixXd J_diff = jacobian_a - jacobian_b; 158 | double maxDiff = std::max(-J_diff.minCoeff(), J_diff.maxCoeff()); 159 | if (maxDiff / norm > relTol) { 160 | std::cout << "Jacobian inconsistent: " << std::endl; 161 | std::cout << " Jacobian a: "; 162 | std::cout << std::endl<< jacobian_a << std::endl; 163 | std::cout << "provided Jacobian b: "; 164 | std::cout << std::endl<< jacobian_b; 165 | std::cout << std::endl << "relative error: " << maxDiff / norm 166 | << ", relative tolerance: " << relTol << std::endl; 167 | isCorrect = false; 168 | } 169 | 170 | return isCorrect; 171 | 172 | } 173 | 174 | private: 175 | /** 176 | * 177 | * @param parameters 178 | * @param parameter_i 179 | * @param parameters_plus 180 | * @param ith 181 | */ 182 | void applyDistribance(double** parameters, 183 | double* parameter_i, 184 | double** parameters_plus, 185 | unsigned int ith){ 186 | for(unsigned int i = 0; i < ith; i++){ 187 | parameters_plus[i] = parameters[i]; 188 | } 189 | parameters_plus[ith] = parameter_i; 190 | for(unsigned int i = ith + 1; i < ParamBlockSize; i++){ 191 | parameters_plus[i] = parameters[i]; 192 | } 193 | } 194 | Functor* ptrErrorFunctor_; 195 | }; 196 | 197 | 198 | #endif 199 | -------------------------------------------------------------------------------- /src/utility/utility.cpp: -------------------------------------------------------------------------------- 1 | #include "utility.h" 2 | 3 | 4 | 5 | Eigen::Matrix3d rotX(double angle){ 6 | Eigen::Matrix3d R; 7 | R<<1, 0, 0, 8 | 0, cos(angle), -sin(angle), 9 | 0, sin(angle), cos(angle); 10 | return R; 11 | } 12 | 13 | Eigen::Matrix3d rotY(double angle){ 14 | Eigen::Matrix3d R; 15 | R< 4 | #include 5 | #include 6 | #include 7 | 8 | Eigen::Matrix3d rotX(double angle); 9 | Eigen::Matrix3d rotY(double angle); 10 | Eigen::Matrix3d rotZ(double angle); 11 | 12 | class Utility 13 | { 14 | public: 15 | template 16 | static Eigen::Quaternion deltaQ(const Eigen::MatrixBase &theta) 17 | { 18 | typedef typename Derived::Scalar Scalar_t; 19 | 20 | Eigen::Quaternion dq; 21 | Eigen::Matrix half_theta = theta; 22 | half_theta /= static_cast(2.0); 23 | dq.w() = static_cast(1.0); 24 | dq.x() = half_theta.x(); 25 | dq.y() = half_theta.y(); 26 | dq.z() = half_theta.z(); 27 | return dq; 28 | } 29 | 30 | template 31 | static Eigen::Matrix skewSymmetric(const Eigen::MatrixBase &q) 32 | { 33 | Eigen::Matrix ans; 34 | ans << typename Derived::Scalar(0), -q(2), q(1), 35 | q(2), typename Derived::Scalar(0), -q(0), 36 | -q(1), q(0), typename Derived::Scalar(0); 37 | return ans; 38 | } 39 | 40 | 41 | static Eigen::Vector3d unskew3d(const Eigen::Matrix3d & Omega); 42 | template 43 | static Eigen::Quaternion positify(const Eigen::QuaternionBase &q) 44 | { 45 | //printf("a: %f %f %f %f", q.w(), q.x(), q.y(), q.z()); 46 | //Eigen::Quaternion p(-q.w(), -q.x(), -q.y(), -q.z()); 47 | //printf("b: %f %f %f %f", p.w(), p.x(), p.y(), p.z()); 48 | //return q.template w() >= (typename Derived::Scalar)(0.0) ? q : Eigen::Quaternion(-q.w(), -q.x(), -q.y(), -q.z()); 49 | return q; 50 | } 51 | 52 | template 53 | static Eigen::Matrix Qleft(const Eigen::QuaternionBase &q) 54 | { 55 | Eigen::Quaternion qq = positify(q); 56 | Eigen::Matrix ans; 57 | ans(0, 0) = qq.w(), ans.template block<1, 3>(0, 1) = -qq.vec().transpose(); 58 | ans.template block<3, 1>(1, 0) = qq.vec(), ans.template block<3, 3>(1, 1) = qq.w() * Eigen::Matrix::Identity() + skewSymmetric(qq.vec()); 59 | return ans; 60 | } 61 | 62 | template 63 | static Eigen::Matrix Qright(const Eigen::QuaternionBase &p) 64 | { 65 | Eigen::Quaternion pp = positify(p); 66 | Eigen::Matrix ans; 67 | ans(0, 0) = pp.w(), ans.template block<1, 3>(0, 1) = -pp.vec().transpose(); 68 | ans.template block<3, 1>(1, 0) = pp.vec(), ans.template block<3, 3>(1, 1) = pp.w() * Eigen::Matrix::Identity() - skewSymmetric(pp.vec()); 69 | return ans; 70 | } 71 | 72 | static Eigen::Vector3d R2ypr(const Eigen::Matrix3d &R) 73 | { 74 | Eigen::Vector3d n = R.col(0); 75 | Eigen::Vector3d o = R.col(1); 76 | Eigen::Vector3d a = R.col(2); 77 | 78 | Eigen::Vector3d ypr(3); 79 | double y = atan2(n(1), n(0)); 80 | double p = atan2(-n(2), n(0) * cos(y) + n(1) * sin(y)); 81 | double r = atan2(a(0) * sin(y) - a(1) * cos(y), -o(0) * sin(y) + o(1) * cos(y)); 82 | ypr(0) = y; 83 | ypr(1) = p; 84 | ypr(2) = r; 85 | 86 | return ypr / M_PI * 180.0; 87 | } 88 | 89 | template 90 | static Eigen::Matrix ypr2R(const Eigen::MatrixBase &ypr) 91 | { 92 | typedef typename Derived::Scalar Scalar_t; 93 | 94 | Scalar_t y = ypr(0) / 180.0 * M_PI; 95 | Scalar_t p = ypr(1) / 180.0 * M_PI; 96 | Scalar_t r = ypr(2) / 180.0 * M_PI; 97 | 98 | Eigen::Matrix Rz; 99 | Rz << cos(y), -sin(y), 0, 100 | sin(y), cos(y), 0, 101 | 0, 0, 1; 102 | 103 | Eigen::Matrix Ry; 104 | Ry << cos(p), 0., sin(p), 105 | 0., 1., 0., 106 | -sin(p), 0., cos(p); 107 | 108 | Eigen::Matrix Rx; 109 | Rx << 1., 0., 0., 110 | 0., cos(r), -sin(r), 111 | 0., sin(r), cos(r); 112 | 113 | return Rz * Ry * Rx; 114 | } 115 | 116 | static Eigen::Matrix3d g2R(const Eigen::Vector3d &g); 117 | 118 | template 119 | struct uint_ 120 | { 121 | }; 122 | 123 | template 124 | void unroller(const Lambda &f, const IterT &iter, uint_) 125 | { 126 | unroller(f, iter, uint_()); 127 | f(iter + N); 128 | } 129 | 130 | template 131 | void unroller(const Lambda &f, const IterT &iter, uint_<0>) 132 | { 133 | f(iter); 134 | } 135 | 136 | template 137 | static T normalizeAngle(const T& angle_degrees) { 138 | T two_pi(2.0 * 180); 139 | if (angle_degrees > 0) 140 | return angle_degrees - 141 | two_pi * std::floor((angle_degrees + T(180)) / two_pi); 142 | else 143 | return angle_degrees + 144 | two_pi * std::floor((-angle_degrees + T(180)) / two_pi); 145 | }; 146 | 147 | static Eigen::Matrix4d quatPlus(Eigen::Quaterniond& quat); // left 148 | static Eigen::Matrix4d quatOplus(Eigen::Quaterniond& quat); // right 149 | 150 | 151 | static Eigen::Matrix Isometry3dMinus(Eigen::Isometry3d pose_plus_delta, Eigen::Isometry3d pose){ 152 | Eigen::Matrix delta; 153 | 154 | Eigen::Quaterniond q_plus_delta(pose_plus_delta.rotation()); 155 | Eigen::Quaterniond q(pose.rotation()); 156 | Eigen::Quaterniond deltaQ = q.inverse()*q_plus_delta; 157 | Eigen::Vector3d deltat = pose_plus_delta.translation() - pose.translation(); 158 | 159 | delta.head<3>() = deltat; 160 | delta.tail<3>() = 2*deltaQ.coeffs().head<3>(); 161 | 162 | return delta; 163 | }; 164 | 165 | static void double2T(double* ptr,Eigen::Isometry3d& T){ 166 | 167 | 168 | 169 | Eigen::Vector3d trans; 170 | trans(0) = ptr[0]; 171 | trans(1) = ptr[1]; 172 | trans(2) = ptr[2]; 173 | 174 | Eigen::Quaterniond q; 175 | q.x() = ptr[3]; 176 | q.y() = ptr[4]; 177 | q.z() = ptr[5]; 178 | q.w() = ptr[6]; 179 | 180 | 181 | T.setIdentity(); 182 | T.matrix().topLeftCorner(3,3) = q.toRotationMatrix(); 183 | T.matrix().topRightCorner(3,1) = trans; 184 | } 185 | 186 | }; 187 | -------------------------------------------------------------------------------- /test/testMarginalization.cpp: -------------------------------------------------------------------------------- 1 | #include "../src/factor/pinhole_project_factor.h" 2 | #include "../src/factor/binary_pose_error_factor.hpp" 3 | 4 | #include "../src/factor/marginalization_factor.h" 5 | #include 6 | #include "../src/utility/num-diff.hpp" 7 | #include "../src/factor/pose_local_parameterization.h" 8 | void T2double(Eigen::Isometry3d& T,double* ptr){ 9 | 10 | Eigen::Vector3d trans = T.matrix().topRightCorner(3,1); 11 | Eigen::Matrix3d R = T.matrix().topLeftCorner(3,3); 12 | Eigen::Quaterniond q(R); 13 | 14 | ptr[0] = trans(0); 15 | ptr[1] = trans(1); 16 | ptr[2] = trans(2); 17 | ptr[3] = q.x(); 18 | ptr[4] = q.y(); 19 | ptr[5] = q.z(); 20 | ptr[6] = q.w(); 21 | } 22 | 23 | 24 | void double2T(double* ptr, Eigen::Isometry3d& T){ 25 | Eigen::Vector3d trans( ptr[0], ptr[1], ptr[2]) ; 26 | Eigen::Quaterniond q(ptr[6], ptr[3], ptr[4], ptr[5]); 27 | 28 | T.setIdentity(); 29 | T.matrix().topLeftCorner(3,3) = q.toRotationMatrix(); 30 | T.matrix().topRightCorner(3,1) = trans; 31 | } 32 | 33 | void applyNoise(const Eigen::Isometry3d Tin,Eigen::Isometry3d& Tout) { 34 | Tout.setIdentity(); 35 | 36 | Eigen::Vector3d delat_trans = 0.14*Eigen::Matrix::Random(); 37 | Eigen::Vector3d delat_rot = 0.16*Eigen::Matrix::Random(); 38 | 39 | Eigen::Quaterniond delat_quat(1.0,delat_rot(0),delat_rot(1),delat_rot(2)) ; 40 | delat_quat.normalize(); 41 | Tout.matrix().topRightCorner(3,1) = Tin.matrix().topRightCorner(3,1) + delat_trans; 42 | Tout.matrix().topLeftCorner(3,3) 43 | = Tin.matrix().topLeftCorner(3,3)*delat_quat.toRotationMatrix(); 44 | } 45 | 46 | 47 | void printParameter(double* param, int dim) { 48 | std::cout<<"Parameter: "< C0p_vec, C1p_vec, p0_vec, p1_vec; 72 | std::vector rho_vec; 73 | for (int i = 0; i < 2; i ++) { 74 | for (int j=0; j < 2 ; j++) { 75 | Eigen::Vector3d C0p = Eigen::Vector3d(-3 + i*6.0/10, -4 + j*8.0/10, 8+ i*10.0/10); 76 | Eigen::Vector3d C1p = T_C1C0.matrix().topLeftCorner(3,3)*C0p+T_C1C0.matrix().topRightCorner(3,1); 77 | Eigen::Vector3d p0(C0p(0)/C0p(2), C0p(1)/C0p(2), 1); 78 | 79 | double z = C0p(2); 80 | double rho = 1.0/z; 81 | Eigen::Vector3d p1(C1p(0)/C1p(2), C1p(1)/C1p(2), 1); 82 | 83 | p0_vec.push_back(p0); 84 | p1_vec.push_back(p1); 85 | rho_vec.push_back(rho); 86 | } 87 | } 88 | 89 | 90 | 91 | /* 92 | * Zero Test 93 | * Passed! 94 | */ 95 | 96 | std::cout<<"------------ Zero Test -----------------"< param_rho; 101 | 102 | T2double(T_WI0,param_T_WI0); 103 | T2double(T_WI1,param_T_WI1); 104 | T2double(T_IC,param_T_IC); 105 | for(int i = 0; i < rho_vec.size(); i++ ) { 106 | param_rho.push_back(&rho_vec[i]); 107 | } 108 | 109 | 110 | MarginalizationInfo *marginalization_info = new MarginalizationInfo(); 111 | 112 | for (int i = 0; i < rho_vec.size(); i++) { 113 | 114 | PinholeProjectFactor *pinholeProjectFactor = new PinholeProjectFactor(p0_vec.at(i), p1_vec.at(i)); 115 | 116 | { 117 | ResidualBlockInfo *residual_block_info = 118 | new ResidualBlockInfo(pinholeProjectFactor, 119 | NULL, 120 | std::vector{param_T_WI0, 121 | param_T_WI1, 122 | param_T_IC, 123 | param_rho.at(i)}, 124 | std::vector{0, 3}); 125 | marginalization_info->addResidualBlockInfo(residual_block_info); 126 | } 127 | } 128 | 129 | 130 | 131 | 132 | Binary_pose_error_facotr* binary_pose_error_facotr 133 | = new Binary_pose_error_facotr(T_WC0, T_WC1, 0,0); 134 | { 135 | ResidualBlockInfo *residual_block_info = 136 | new ResidualBlockInfo(binary_pose_error_facotr, 137 | NULL, 138 | std::vector{param_T_WI0, 139 | param_T_WI1}, 140 | std::vector{0}); 141 | marginalization_info->addResidualBlockInfo(residual_block_info); 142 | } 143 | 144 | 145 | marginalization_info->preMarginalize(); 146 | marginalization_info->marginalize(); 147 | 148 | std::unordered_map addr_shift; 149 | addr_shift[reinterpret_cast(param_T_WI1)] = param_T_WI1; 150 | addr_shift[reinterpret_cast(param_T_IC)] = param_T_IC; 151 | std::vector residual_parameter = marginalization_info->getParameterBlocks(addr_shift); 152 | 153 | 154 | 155 | MarginalizationFactor* marginalization_factor = new MarginalizationFactor(marginalization_info); 156 | 157 | double** paramters = &residual_parameter[0]; // vector to array 158 | 159 | 160 | Eigen::VectorXd residual(marginalization_factor->num_residuals()); 161 | std::cout<<"num_residuals: "<num_residuals()<parameter_block_sizes().size()<parameter_block_sizes()) { 164 | std::cout<<"size: "<< i <EvaluateWithMinimalJacobians(paramters,residual.data(), NULL, NULL); 168 | std::cout<<"residual: "<< residual.transpose()< jacobian0_min; 195 | Eigen::Matrix jacobian1_min; 196 | 197 | double* jacobians_min[2] = {jacobian0_min.data(), jacobian1_min.data()}; 198 | 199 | 200 | Eigen::Matrix jacobian0; 201 | Eigen::Matrix jacobian1; 202 | 203 | double* jacobians[2] = {jacobian0.data(), jacobian1.data()}; 204 | 205 | double* parameters_noised[2] = {param0_noised, param1_noised}; 206 | marginalization_factor->EvaluateWithMinimalJacobians(parameters_noised,residual.data(), jacobians, jacobians_min); 207 | std::cout<<"residual: "<< residual.transpose()< num_jacobian0_min; 211 | Eigen::Matrix num_jacobian1_min; 212 | 213 | NumDiff localizer_num_differ(marginalization_factor); 214 | 215 | localizer_num_differ.df_r_xi<12,7,6,PoseLocalParameterization>(parameters_noised,0,num_jacobian0_min.data()); 216 | 217 | std::cout<<"jacobian0_min: "<(parameters_noised,1,num_jacobian1_min.data()); 222 | 223 | std::cout<<"jacobian1_min: "< 3 | #include "../src/utility/num-diff.hpp" 4 | #include "../src/factor/pose_local_parameterization.h" 5 | void T2double(Eigen::Isometry3d& T,double* ptr){ 6 | 7 | Eigen::Vector3d trans = T.matrix().topRightCorner(3,1); 8 | Eigen::Matrix3d R = T.matrix().topLeftCorner(3,3); 9 | Eigen::Quaterniond q(R); 10 | 11 | ptr[0] = trans(0); 12 | ptr[1] = trans(1); 13 | ptr[2] = trans(2); 14 | ptr[3] = q.x(); 15 | ptr[4] = q.y(); 16 | ptr[5] = q.z(); 17 | ptr[6] = q.w(); 18 | } 19 | 20 | void applyNoise(const Eigen::Isometry3d& Tin,Eigen::Isometry3d& Tout){ 21 | 22 | 23 | Tout.setIdentity(); 24 | 25 | Eigen::Vector3d delat_trans = 0.5*Eigen::Matrix::Random(); 26 | Eigen::Vector3d delat_rot = 0.16*Eigen::Matrix::Random(); 27 | 28 | Eigen::Quaterniond delat_quat(1.0,delat_rot(0),delat_rot(1),delat_rot(2)) ; 29 | 30 | Tout.matrix().topRightCorner(3,1) = Tin.matrix().topRightCorner(3,1) + delat_trans; 31 | Tout.matrix().topLeftCorner(3,3) = Tin.matrix().topLeftCorner(3,3)*delat_quat.toRotationMatrix(); 32 | } 33 | 34 | int main(){ 35 | 36 | // simulate 37 | 38 | Eigen::Isometry3d T_WI0, T_WI1, T_IC; 39 | Eigen::Vector3d C0p(4, 3, 10); 40 | T_WI0 = T_WI1 = T_IC = Eigen::Isometry3d::Identity(); 41 | 42 | T_WI1.matrix().topRightCorner(3,1) = Eigen::Vector3d(1,0,0); 43 | T_IC.matrix().topRightCorner(3,1) = Eigen::Vector3d(0.1,0.10,0); 44 | 45 | Eigen::Isometry3d T_WC0, T_WC1; 46 | T_WC0 = T_WI0 * T_IC; 47 | T_WC1 = T_WI1 * T_IC; 48 | 49 | Eigen::Isometry3d T_C0C1 = T_WC0.inverse()*T_WC1; 50 | Eigen::Isometry3d T_C1C0 = T_C0C1.inverse(); 51 | 52 | Eigen::Vector3d C1p = T_C1C0.matrix().topLeftCorner(3,3)*C0p+T_C1C0.matrix().topRightCorner(3,1); 53 | 54 | Eigen::Vector3d p0(C0p(0)/C0p(2), C0p(1)/C0p(2), 1); 55 | double z = C0p(2); 56 | double rho = 1.0/z; 57 | 58 | Eigen::Vector3d p1(C1p(0)/C1p(2), C1p(1)/C1p(2), 1); 59 | 60 | 61 | 62 | /* 63 | * Zero Test 64 | * Passed! 65 | */ 66 | 67 | std::cout<<"------------ Zero Test -----------------"< residual; 87 | 88 | Eigen::Matrix jacobian0_min; 89 | Eigen::Matrix jacobian1_min; 90 | Eigen::Matrix jacobian2_min; 91 | Eigen::Matrix jacobian3_min; 92 | double* jacobians_min[4] = {jacobian0_min.data(), jacobian1_min.data(), 93 | jacobian2_min.data(), jacobian3_min.data()}; 94 | 95 | 96 | Eigen::Matrix jacobian0; 97 | Eigen::Matrix jacobian1; 98 | Eigen::Matrix jacobian2; 99 | Eigen::Matrix jacobian3; 100 | double* jacobians[4] = {jacobian0.data(), jacobian1.data(), jacobian2.data(), jacobian3.data()}; 101 | 102 | pinholeProjectFactor->EvaluateWithMinimalJacobians(paramters,residual.data(),jacobians,jacobians_min); 103 | 104 | std::cout<<"residual: "<EvaluateWithMinimalJacobians(parameters_noised,residual.data(),jacobians,jacobians_min); 139 | 140 | 141 | std::cout<<"residual: "< num_jacobian0_min; 144 | Eigen::Matrix num_jacobian1_min; 145 | Eigen::Matrix num_jacobian2_min; 146 | Eigen::Matrix num_jacobian3_min; 147 | 148 | NumDiff localizer_num_differ(pinholeProjectFactor); 149 | 150 | localizer_num_differ.df_r_xi<2,7,6,PoseLocalParameterization>(parameters_noised,0,num_jacobian0_min.data()); 151 | 152 | std::cout<<"jacobian0_min: "<(jacobian0_min.data(),num_jacobian0_min.data(),1e-2); 157 | 158 | localizer_num_differ.df_r_xi<2,7,6,PoseLocalParameterization>(parameters_noised,1,num_jacobian1_min.data()); 159 | 160 | std::cout<<"jacobian1_min: "<(jacobian1_min.data(),num_jacobian1_min.data(),1e-2); 165 | 166 | localizer_num_differ.df_r_xi<2,7,6,PoseLocalParameterization>(parameters_noised,2,num_jacobian2_min.data()); 167 | 168 | std::cout<<"jacobian2_min: "<(jacobian2_min.data(),num_jacobian2_min.data(),1e-2); 173 | 174 | 175 | localizer_num_differ.df_r_xi<2,1>(parameters_noised,3,num_jacobian3_min.data()); 176 | std::cout<<"jacobian3_min: "< 3 | #include "../src/utility/num-diff.hpp" 4 | #include "../src/factor/pose_local_parameterization.h" 5 | void T2double(Eigen::Isometry3d& T,double* ptr){ 6 | 7 | Eigen::Vector3d trans = T.matrix().topRightCorner(3,1); 8 | Eigen::Matrix3d R = T.matrix().topLeftCorner(3,3); 9 | Eigen::Quaterniond q(R); 10 | 11 | ptr[0] = trans(0); 12 | ptr[1] = trans(1); 13 | ptr[2] = trans(2); 14 | ptr[3] = q.x(); 15 | ptr[4] = q.y(); 16 | ptr[5] = q.z(); 17 | ptr[6] = q.w(); 18 | } 19 | 20 | void applyNoise(const Eigen::Isometry3d& Tin,Eigen::Isometry3d& Tout){ 21 | 22 | 23 | Tout.setIdentity(); 24 | 25 | Eigen::Vector3d delat_trans = 0.5*Eigen::Matrix::Random(); 26 | Eigen::Vector3d delat_rot = 0.16*Eigen::Matrix::Random(); 27 | 28 | Eigen::Quaterniond delat_quat(1.0,delat_rot(0),delat_rot(1),delat_rot(2)) ; 29 | 30 | Tout.matrix().topRightCorner(3,1) = Tin.matrix().topRightCorner(3,1) + delat_trans; 31 | Tout.matrix().topLeftCorner(3,3) = Tin.matrix().topLeftCorner(3,3)*delat_quat.toRotationMatrix(); 32 | } 33 | 34 | int main(){ 35 | 36 | // simulate 37 | 38 | Eigen::Isometry3d T_WI0, T_WI1, T_IC; 39 | Eigen::Vector3d C0p(4, 3, 10); 40 | T_WI0 = T_WI1 = T_IC = Eigen::Isometry3d::Identity(); 41 | 42 | T_WI1.matrix().topRightCorner(3,1) = Eigen::Vector3d(1,0,0); 43 | T_IC.matrix().topRightCorner(3,1) = Eigen::Vector3d(0.1,0.10,0); 44 | 45 | Eigen::Isometry3d T_WC0, T_WC1; 46 | T_WC0 = T_WI0 * T_IC; 47 | T_WC1 = T_WI1 * T_IC; 48 | 49 | Eigen::Isometry3d T_C0C1 = T_WC0.inverse()*T_WC1; 50 | Eigen::Isometry3d T_C1C0 = T_C0C1.inverse(); 51 | 52 | Eigen::Vector3d Wp = T_WC0.matrix().topLeftCorner(3,3)*C0p+T_WC0.matrix().topRightCorner(3,1); 53 | 54 | Eigen::Vector3d C1p = T_C1C0.matrix().topLeftCorner(3,3)*C0p+T_C1C0.matrix().topRightCorner(3,1); 55 | 56 | Eigen::Vector3d p0(C0p(0)/C0p(2), C0p(1)/C0p(2), 1); 57 | double z = C0p(2); 58 | double rho = 1.0/z; 59 | 60 | Eigen::Vector3d p1(C1p(0)/C1p(2), C1p(1)/C1p(2), 1); 61 | 62 | 63 | 64 | /* 65 | * Zero Test 66 | * Passed! 67 | */ 68 | 69 | std::cout<<"------------ Zero Test -----------------"< residual; 82 | 83 | Eigen::Matrix jacobian0_min; 84 | Eigen::Matrix jacobian1_min; 85 | double* jacobians_min[2] = {jacobian0_min.data(), jacobian1_min.data()}; 86 | 87 | 88 | Eigen::Matrix jacobian0; 89 | Eigen::Matrix jacobian1; 90 | double* jacobians[2] = {jacobian0.data(), jacobian1.data()}; 91 | 92 | projectFactor->EvaluateWithMinimalJacobians(paramters,residual.data(),jacobians,jacobians_min); 93 | 94 | std::cout<<"residual: "<EvaluateWithMinimalJacobians(parameters_noised,residual.data(),jacobians,jacobians_min); 118 | 119 | 120 | std::cout<<"residual: "< num_jacobian0_min; 123 | Eigen::Matrix num_jacobian1_min; 124 | 125 | NumDiff num_differ(projectFactor); 126 | 127 | num_differ.df_r_xi<2,7,6,PoseLocalParameterization>(parameters_noised,0,num_jacobian0_min.data()); 128 | 129 | std::cout<<"jacobian0_min: "<(jacobian0_min.data(),num_jacobian0_min.data(),1e-2); 134 | // 135 | num_differ.df_r_xi<2,3>(parameters_noised,1,num_jacobian1_min.data()); 136 | 137 | std::cout<<"jacobian1_min: "<