Squashed 'third_party/ceres/' changes from 399cda773..7f9cc571b
7f9cc571b Set CMAKE_CUDA_ARCHITECTURES depending on CUDAToolkit_VERSION
6a74af202 Remove a level of indirection when using CellInfo
f8c2994da Drop Ubuntu 20.04 and add Ubuntu 24.04 support
20954e693 Eliminate CUDA set but unused variable warning
57aba3ed0 Enable Apple linker library deduplication
1f15197be Drop macos-11 runner and support macos-14 instead
5de0fda0f Update Github actions
308a5bb43 Add missing include
715865101 Remove 32-bit MinGW from CI matrix
f71181a92 Remove remaining references to CXSparse
522210a08 Reuse macro to format version string
cd2dd06e9 Fix clang16 compiler warnings
1f2e6313a Add missing std qualifiers
125c06882 Link static cuda libs when ceres is build static
62c03d6ff Revert "Update Eigen::BDCSVD usage to comply with Eigen 3.4"
4027f6997 Update Eigen::BDCSVD usage to comply with Eigen 3.4
1b2ebabf5 Typo fixes in the documentation
2ffeb943a Fix typo in AutoDiffManifold comment and docs
da34da3da Remove CreateFakeBundleAdjustmentPartitionedJacobian
85b2c418a ClangTidy fixes
91773746b Simplify instantiation of cost functions and their functors
8b88a9ab4 Use C++17 Bessel functions
84436f791 Use native CMake TBB package configuration
556a56f21 Do not assume Homebrew usage
3fd2a72cc MinGW no longer provides a 32-bit METIS package
e776a6f1a Unbreak the Bazel build.
095d48392 Skip structure detection on preprocessor failure
a0876309a Fix typos in comments
774973731 Fix search on ceres-solver.org
85331393d Update docs for 2.2.0.
2120eae67 Optimize the computation of the LM diagonal in TinySolver
611b139b1 Fix Solver::Options::callbacks type in documentation
b652d3b4f Unbreak the windows build
b79c4d350 ClangTidy fixes
76af132d0 Update docs for 2.2.0RC3
dc7a85975 Single-threaded operations on small vectors
b379ab768 Remove MaxNumThreadsAvailable
354002f98 Schedule task in ParallerFor from the previous task
a9b3fcff4 Minor update to docs
5ccab18be Drop use of POSIX M_PI_2 and M_PI_4
4519b8d77 Drop use of POSIX M_PI
b83abdcb1 Add a default value for Solver::Summary::linear_solver_ordering_type
8d875a312 Fix checks for CUDA memory pools support
94335e3b9 More ClangTidy fixes
399395c4f Miscellaneous ClangTidy fixes
c8bed4b93 Update version_history for 2.2.0rc2
489339219 Rework MSVC warning suppression
0cea191d4 Move stream-ordered memory allocations
8e3b7d89e Fix a copy-pasta error
dc0bb8508 Various cleanups to the documentation
4588b0fbb Add an example for EvaluationCallback
0fc3fd4fc Add documentation for examples
e6b2f532b Parallelize PSE preconditioner
41672dff8 Add end-to-end BA tests for SCHUR_POWER_SERIES_EXPANSION
83ee376d8 Add an example for IterationCallback
371265094 Cleanup example code
59182a42c Update documentation
d4db6e6fe Fix typos in the documentation for EvaluationCallback
dffd8cd71 Add an accessor for the CostFunctor in DynamicAutoDiffCostFunction
bea247701 Add a missing include dir to the cuda kernels target.
18ea7d1c2 Runtime check for cudaMallocAsync support
a227045be Remove cuda-memcheck based tests
d10e786ca Remove an unused variable from CudaSparseMatrix
5a30cae58 Preparing for 2.2.0rc1
9cca67127 Enable compatibility with SuiteSparse 7.2.0
a1c02e8d3 Rework the Sphinx find module
a57e35bba Require at least CMake 3.16
863db948f Eliminate macOS sprintf warning
6f5342db6 Export ceres_cuda_kernels to project's build tree
d864d146f Add macOS 13 runner to Github workflows
01a23504d Add a workaround for CMake Policy CMP0148
de9cbde95 Work around MinGW32 manifold_test segfault
5e4b22f7f Update CudaSparseMatrix class
ed9921fc2 Fix Solver::Options in documentation
de62bf220 Two minor fixes
5f97455be Fix typos in documentation
5ba62abec Add CUDA support to windows CI builds
a98fdf582 Update CMakeLists.txt to fix Windows CUDA build
ec4907399 Fix block-sparse to crs conversion on windows
799ee91bb Fix check in CompressedRowJacobianWriter::CreateJacobian()
ee90f6cbf Detect large Jacobians and return failure instead of crashing.
310a252fb Deal with infinite initial cost correctly.
357482db7 Add NumTraits::max_digits10 for Jets
908b5b1b5 Fix type mismatch in documentation
75bacedf7 CUDA partitioned matrix view
fd6197ce0 Fixed GCC 13.1 compilation errors
df97a8f05 Improve support of older CUDA toolkit versions
085214ea7 Fix test CompressedRowSparseMatrix.Transpose
d880df09f Match new[] with delete[] in BSM
bdee4d617 Block-sparse to CRS conversion using block-structure
0f9de3daf Use page locked memory in BlockSparseMatrix
e7bd72d41 Permutation-based conversion from block-sparse to crs
abbc4e797 Explicitly compute number of non-zeros in row
96fdfd2e7 Implement tests for Euler conversion with jets
db1ebd3ff Work around lack of constexpr constructors for Jet
16a4fa04e Further Jet conversion fixes
92ad18b8a Fix a Jet conversion bug in rotation.h
a5e745d4e ClangTidy fixes
77ad8bb4e Change storage in BlockRandomAccessSparseMatrix
d340f81bd Clang Tidy fixes
54ad3dd03 Reorganize ParallelFor source files
ba360ab07 Change the value of BlockRandomAccessSparseMatrix::kMaxRowBlocks
0315c6ca9 Provide DynamicAutoDiffCostFunction deduction guide
3cdfae110 Replace Hertzberg mentions with citations
0af38a9fc Fix typos in documentation
4c969a6c1 Improve image of loss functions shape
b54f05b8e Add missing TukeyLoss to documentation
8bf4a2f42 Inexact check for ParallelAssign test
9cddce73a Explicit conversions from long to int in benchmarks (for num_threads)
5e787ab70 Using const ints in jacobian writers
f9bffbb6f Removing -Wshorten-64-to-32 warnings from examples (part 1)
e269b64f5 More ClangTidy fixes
f4eb768e0 Using int64 in file.cc. Fixing compilation error in array_utils.cc
74a0f0d24 Using int64_t for sizes and indexes in array utils
749a442d9 Clang-Tidy fixes
c4ba975ae Fix rotation_test.cc to work with older versions of Eigen
9602ed7b7 ClangFormat changes
79a554ffc Fix a bug in QuaternionRotatePoint.
f1113c08a Commenting unused parameters for better readibility
772d927e1 Replacing old style typedefs with new style usings
53df5ddcf Removing using std::...
e1ca3302a Increasing bazel timeout for heavy BA tests
f982d3071 Fixing bazel build
cb6b30662 Use hypot to compute the L^2 norm
1e2a24a8b Update Github actions to avoid deprecation warnings
285e5f9f4 Do not update brew formulae upon install
73d95b03f Clang-Tidy fixes
51d52c3ea Correct epsilon in CUDA QR test changed by last commit.
546f5337b Updates to CUDA dense linear algebra tests
19ab2c179 BlockRandomAccessMatrix Refactor
a3a062d72 Add a missing header
2b88bedb2 Remove unused variables
9beea728f Fix a bug in CoordinateDescentMinimizer
8e5d83f07 ClangFormat and ClangTidy changes
b15851508 Parallel operations on vectors
2fd81de12 Add build configuration with CUDA on Linux
06bfe6ffa Remove OpenMP and No threading backends.
352b320ab Fixed SuiteSparse 6.0 version parsing
8fd7828e3 ClangTidy fixes
0424615dc Fix PartitionedMatrixView usage in evaluation_benchmark
77a54dd3d Parallel updates to block-diagonal EtE FtF
946fa50de ClangTidy fixes
e4bef9505 Refactor PartitionedMatrixView to cache the partitions
d3201798e Clean up sparse_cholesky_test
addcd342f ClangTidy fixes
c2e7002d2 Remove an unused variable from evaluation_benchmark.cc
fef6d5875 Parallel left products for PartitionedMatrixView
37a3cb384 Update SuiteSparse in MSVC Github workflow
5d53d1ee3 Parallel for with iteration costs and left product
9aa52c6ff Use FindCUDAToolkit for CMake >= 3.17
47e03a6d8 Add const accessor for Problem::Options used by Problem
d6a931009 Clang Tidy Fixes
9364e31ee Fix a regression in SuiteSparse::AnalyzeCholesky
89b3e1f88 Remove unused includes of gflags and gtest
984079003 Fix missing regex dependency for gtest on QNX
6b296f27f Fix missing namespace qualification and docs for Manifold gtest macro
6685e629f AddBlockStructureTranspose to BlockSparseMatrix
699e3f3b3 Fix a link error in evaluation_benchmark.cc
19a3d07f9 Add multiplication benchmarks on BAL data
b221b1294 Format code with clang-format.
ccf32d70c Purge all remaining references to (defunct) LocalParameterization
5f8c406e2 struct ContextImpl -> class ContextImpl
9893c534c Several cleanups.
a78a57472 ClangTidy fixes
b1fe60330 Parallel right products for partitioned view
16668eedf Fix a memory leak in ContextImpl
d129938d5 Third time is the charm
4129b214a More fixes to cuda_dense_cholesky_test.cc
5c01d2573 Remove unused variables from cuda_dense_cholesky_test.cc
5e877ae69 Fix the Bazel build
d89290ffa Fix evalution_benchmark compilability
ae7f456e3 ClangTidy fixes
9438c370f Restore the semantics of TrustRegionMinimizer
c964fce90 Fix bug in cuda_kernels_test
afaad5678 Fix a typo
b7116824b Evaluation benchmark
2b89ce66f Add generalized Euler Angle conversions
8230edc6c ClangTidy fixes
9a2894763 Speed up locking when num_threads = 1.
739f2a25a Parallelize block_jacobi_preconditioner
c0c4f9394 Change implementation of parallel for
fc826c578 CUDA Cleanup
660af905f Fix a bug in TrustRegionMinimizer.
4cd257cf4 Let NumericDiffFirstOrderFunction take a dynamically sized parameter vector
6c27ac6d5 Fix repeated SpMV Benchmark
430a292ac Add a missing include
858b4b89b More ClangTidy fixes
e9e995740 ClangTidy fixes
7f8e930a0 Fix lint errors
f86a3bdbe Unify Block handling across matrix types
5f1946879 clang-formated source
00a05cf70 CUDA SDK Version-based SpMV Selection
de0f74e40 Optimize the BlockCRSJacobiPreconditioner
ba65ddd31 Improvements to benchmarks
42352e2e2 Added CUDA Jacobi Preconditioner
f802a09ff &foo[0] -> foo.data()
344929647 Add CUDA GPU and Runtime Detection
6085e45be Minor CUDA cleanup.
e15ec89f3 Speed up bundle_adjuster
058a72782 One more ClangTidy fix.
f6f2f0d16 ClangTidy cleanups Also some clang-format cleanups.
9d74c6913 Fix some more errant CATD warnings
a4f744095 Remove an unused variable from compressed_row_sparse_matrix.cc
388c14286 Fix GCC 12.1.1 LTO -Walloc-size-larger-than= warnings
9ec4f7e44 Refactor BlockJacobiPreconditioner
adda97acd Fixed a few more missing CERES_NO_CUDA guards
22aeb3584 Fix a missing string assignment in solver.cc
3b891f767 Insert missing CUDA guards.
829089053 CUDA CGNR, Part 4: CudaCgnrSolver
6ab435d77 Fix a missing CERES_NO_CUDA guard
c560bc2be CUDA CGNR, Part 3: CudaSparseMatrix
c914c7a2b CUDA CGNR, Part 2: CudaVector
3af3dee18 Simplify the implementation to convert from BlockSparseMatrix to CompressedRowSparseMatrix.
242fc0795 Remove unnecessary destructors
737200ac8 Add macos-12 to Github workflow runners
9c968d406 Silence Clang warning
2c78c5f33 Small naming fixups.
2a25d86b0 Integrate schur power series expansion options to bundle adjuster
4642e4b0c Use if constexpr and map SparseMatrix as const
92d837953 Enable usage of schur power series expansion preconditioner.
f1dfac8cd Reduce the number of individual PRNG instances
79e403b15 Expand vcpkg installation instructions
7b0bb0e3f ClangTidy cleanups
c5c2afcc9 Fix solver_test.cc for preconditioners and sparse linear algebra libraries
07d333fb6 Refactor options checking for linear solvers
ba7207b0b A number of small changes.
20e85bbe3 Add power series expansion preconditioner
04899645c LinearOperator::FooMultiply -> LinearOperator::FooMultiplyAndAccumulate
288a3fde6 Add missing virtual destructors to matrix adapters
6483a2b4c Add Sergiu's name to the list of maintainers
1cf49d688 Update FindGlog.cmake to create glog::glog target
1da72ac39 Refactor ConjugateGradientsSolver
f62dccdb3 Fix the Sphere and Line Manifold formulations
3e1cc89f6 A bunch of clang-tidy fixes.
80380538a One more CATD fix
560ef46fb A bunch of minor fixes.
67bae28c1 CUDA CGNR, Part 1: Misc. CLeanup
5d0bca14d Remove ceres/internal/random.h in favor of <random>
d881b5ccf Minor fixes in comments
37516c968 Fix a bug in InnerProductComputer.
d8dad14ee CUDA Cleanup
c9d2ec8a9 Updates to sparse block matrix structures to support new sparse linear solvers.
5fe0bd45a Added MinGW to Windows Github workflow
738c027c1 Fix a logic error in iterative_refiner_test
cb6ad463d Add mixed precision support for CPU based DenseCholesky
df55682ba Fix Eigen error in 2D sphere manifolds Since Eigen does not allow to have a RowMajor column vector (see https://gitlab.com/libeigen/eigen/-/issues/416), the storage order must be set to ColMajor in that case. This fix adds that special case when generating 2D sphere manifolds.
1cf59f61e Set Github workflow NDK path explicitly
68c53bb39 Remove ceres::LocalParameterization
2f660464c Fix build issue with CUDA testing targets when compiling without gflags.
c801192d4 Minor fixes
ce9e902b8 Fix missing CERES_METIS_VERSION
d9a3dfbf2 Add a missing ifdef guard to dense_cholesky_test
5bd43a1fa Speed up DenseSparseMatrix::SquareColumnNorm.
cbc86f651 Fix the build when CUDA is not present
5af8e6449 Update year in solver.h
88e08cfe7 Mixed-precision Iterative Refinement Cholesky With CUDA
290b34ef0 Fix optional SuiteSparse + METIS test-suite names to be unique
d038e2d83 Fix use of NESDIS with SuiteSparse in tests if METIS is not found
027e741a1 Eliminated MinGW warning
4e5ea292b Fixed MSVC 2022 warning
83f6e0853 Fix use of conditional preprocessor checks within a macro in tests
70f1aac31 Fix fmin/fmax() when using Jets with float as their scalar type
5de77f399 Fix reporting of METIS version
11e637667 Fix #ifdef guards around METIS usage in EigenSparse backend and tests
0c88301e6 Provide optional METIS support
f11c25626 Fix fmin/fmax() to use Jet averaging on equality
b90053f1a Revert C++17 usage of std::exclusive_scan
dfce1e128 Link against threading library only if necessary
69eddfb6d Use find module to link against OpenMP
b4803778c Update documentation for linear_solver_ordering_type
2e764df06 Update Cuda memcheck test
443ae9ce2 Update Cuda memcheck test
55b4c3f44 Retain terminal formatting when building docs
786866d9f Generate version string at compile time
5bd83c4ac Unbreak the build with EIGENSPARSE is disabled
2335b5b4b Remove support for CXSparse
fbc2eea16 Nested dissection for ACCELERATE_SPARSE & EIGEN_SPARSE
d87fd551b Fix Ubuntu 20.04 workflow tests
71717f37c Use glog 0.6 release to run Windows Github workflow
66e0adfa7 Fix detection of sphinx-rtd-theme
d09f7e9d5 Enable postordering when computing the sparse factorization.
9b34ecef1 Unbreak the build on MacOS
8ba8fbb17 Remove Solver::Options::use_postordering
30b4d5df3 Fix the ceres.bzl to add missing cc files.
39ec5e8f9 Add Nested Dissection based fill reducing ordering
aa62dd86a Fix a build breakage
41c5fb1e8 Refactor suitesparse.h/cc
12263e283 Make the min. required version of SuiteSparse to be 4.5.6
c8493fc36 Convert internal enums to be class enums.
bb3a40c09 Add Nested Dissection ordering method to SuiteSparse
f1414cb5b Correct spelling in comments and docs.
fd2b0ceed Correct spelling (contiguous, BANS)
464abc198 Run Linux Github workflow on Ubuntu 22.04
caf614a6c Modernize code using c++17 constructs
be618133e Simplify some template metaprograms using fold expressions.
3b0096c1b Add the ability to specify the pivot threshold in Covariance::Options
40c1a7e18 Fix Github workflows
127474360 Ceres Solver now requires C++17
b5f1b7877 clang-format cleanup
32cd1115c Make the code in small_blas_generic.h more compiler friendly.
f68321e7d Update version history
b34280207 Fix MSVC small_blas_test failures
b246991b6 Update the citation instructions in the docs
c0c14abca Fix version history item numbering
d23dbac25 Update Windows install guide
e669c9fc7 Provide citation file
ff57c2e91 Update version history for 2.1.0rc2
ab9436cb9 Workaround MSVC STL deficiency in C++17 mode
97c232857 Update the included gtest to version 1.11.0
4eac7ddd2 Fix Jet lerp test regression
2ffbe126d Fix Jet test failures on ARMv8 with recent Xcode
0d6a0292c Fix unused arguments of Make1stOrderPerturbation
93511bfdc Fix SuiteSparse path and version reporting
bf329b30f Fix link to macOS badge
0133dada2 Add Github workflows
3d3d6ed71 Add missing includes
0a9c0df8a Fix path for cuda-memcheck tests
ee35ef66f ClangFormat cleanup via scripts/all_format.sh
470515985 Add missing includes for config.h
d3612c12c Set CMP0057 policy for IN_LIST operator in FindSuiteSparse.cmake
4bc100c13 Do not define unusable import targets
e91995cce Fix Ubuntu 18.04 shared library build
94af09186 Force C++ linker
a65e73885 Update installation docs
1a377d707 Fix Ubuntu 20.04 SPQR build
817f5a068 Switch to imported SuiteSparse, CXSparse, and METIS targets
b0f32a20d Hide remaining internal symbols
572395098 Add a missing include
b0aef211d Allow to store pointers in ProductManifold
9afe8cc45 Small compile fix to context_impl
284be88ca Allow ProductManifold default construction
f59059fff Bugfix to CUDA workspace handling
779634164 Fix MSVC linker error
7743d2e73 Store ProductManifold instances in a tuple
9f32c42ba Update Travis-CI status badge to .com from .org
eadfead69 Move LineManifold and SphereManifold into their own headers.
f0f8f93bb Fix docs inconsistencies
e40391efa Update version history in preparation for 2.1.0
6a37fbf9b Add static/compile time sizing to EuclideanManifold
4ad787ce1 Fix the bazel build
ae4d95df6 Two small clang-tidy fixes
f0851667b Fix MSVC compilation errors
c8658c899 Modernize more
46b3495a4 Standardize path handling using GNUInstallDirs
98bc3ca17 Fix shared library build due to missing compile features specification
8fe8ebc3a Add final specifier to public classes
84e1696f4 Add final specifier to internal classes.
518970f81 Context should be exported
09ec4997f Cleanup examples
90e58e10f Add missing #include.
15348abe9 Add CUDA based bundle adjustment tests.
57ec9dc92 Do not enforce a specific C++ standard
99698f053 Fix Apple Clang weak symbols warnings
8e0842162 Add support for dense CUDA solvers #3
aff51c907 Revert "Do not enforce a specific C++ standard"
527c3f7da Fixed gflags dependency
d839b7792 Do not enforce a specific C++ standard
f71167c62 Fixed missing include in minilog build
47502b833 Miscellaneous CUDA related changes.
bb2996681 Check CUDA is available in solver.cc
7d2e4152e Add support for dense CUDA solvers #2
f90833f5f Simplify symbol export
c6158e0ab Replace NULL by nullptr
7e4f5a51b Remove blas.h/cc as they are not used anymore.
e0fef6ef0 Add cmake option ENABLE_BITCODE for iOS builds
446487c54 Add <memory> header to all files using std::unique_ptr.
9c5f29d46 Use compiler attributes instead of [[deprecated]]
44039af2c Convert factory functions to return std::unique_ptrs.
708a2a723 Silence LocalParameterization deprecation warnings
de69e657a Fix another missing declaration warning
677711138 Fix segmentation fault in AVX2 builds
0141ca090 Deprecate LocalParameterizations
fdfa5184a Fix missing declaration warning
4742bf386 Add LineManifold.
c14f360e6 Drop trivial special members
ae65219e0 ClangTidy cleanups
a35bd1bf9 Use = default for trivial special members
db67e621e Fix dense_cholesky_test.cc comma handling.
484d3414e Replace virtual keyword by override
2092a720e Fix some nits.
36d6d8690 Add support for dense CUDA solvers #1
af5e48c71 Add SphereManifold.
182cb01c5 Normalize Jet classification and comparison
9dbd28989 Loosen tolerances in dense_qr_test.cc
cab853fd5 Add DenseQR Interface
8ae054ad9 Fix missing declaration warning in autodiff_manifold_test
408af7b1a Move the constructor and destructor for SchurComplementSolver
d51672d1c Move the constructor and destructor for DenseSchurComplementSolver
177b2f99d Use benchmark version 1.6 compatible syntax.
ce9669003 Add const accessor for functor wrapped by auto/numeric-diff objects
9367ec9cc Add a benchmark for dense linear solvers.
7d6524daf Support fma Jet
a0d81ad63 Fix a bug in AutoDiffManifold
5a99e42e1 ClangTidy fixes
40fb41355 Update .gitignore
e6e6ae087 Unbreak the bazel build
0572efc57 Fix a compilation warning in autodiff_manifold.h
475db73d0 Fix build breakage when LAPACK support is disabled.
095c9197f Fix iterative_refiner_test.cc
6d06e9b98 Add DenseCholesky
77c0c4d09 Migrate examples to use Manifolds
19eef54fc Rename Quaternion to QuaternionManifold. Also rename EigenQuaternion to EigenQuaternionManiold.
ca6d841c2 Add AutoDiffManifold
97d7e0737 Move the manifold testing matchers to manifold_test_utils.h
16436b34b Fix some more clang-tidy suggestions.
dcdefc216 Fix a bunch of clang-tidy suggestions.
d8a1b69ab Remove an unused variable from gradient_checker.cc
611b46b54 Remove the use of CHECK_NOTNULL.
125a0e9be LocalParameterization -> Manifold #1
00bfbae11 Add missing algorithm header to manifold.cc
1d5aff059 Refactor jet_tests.cc
fbd693091 Fix two unused variable warnings.
c0cb42e5f Add Problem::HasParameterization
7e2f9d9d4 Add EigenQuaternion manifold and tests for it.
b81a8bbb7 Add the Quaternion manifold and tests.
4a01dcb88 Add more invariants and documentation to manifold_test.cc
bdd80fcce Improve Manifold testing
ce1537030 Fixed missing headers in manifold.h
23b204d7e LocalParameterization -> Manifold #1
c2fab6502 Fix docs of supported sparse backends for mixed_precision_solves option
8cb441c49 Fix missing declaration warnings in GCC
d2b7f337c Remove split.cc from the bazel sources.
206061a6b Use standard c++ types in jet_test.cc
1f374a9fe Support promotion in comparison between Jet and scalars
06e68dbc5 Avoid midpoint overflow in the differential
276d24c73 Fix C++20 compilation
b1391e062 Support midpoint Jet
8426526df Support lerp Jet
57c279689 support 3-argument hypot jet
123fba61c Eigen::MappedSparseMatrix -> Eigen::Map<Eigen::SparseMatrix>
3f950c66d reworked copysign tests
48cb54d1b fix fmin and fmax NaN handling
552a4e517 support log10 jet
4e49c5422 reuse expm1 result for differential
8d3e64dd5 Use modern-style Eigen3 CMake variables
2fba61434 support log1p and expm1 jet
a668cabbc support norm jet
a3a4b6d77 support copysign jet
b75dac169 fix abs jet test comment
034bf566f use copysign for abs jet
31008453f Add example for BiCubicInterpolator
7ef4a1221 Add a section on implicit and inverse function theorems
686428f5c Move the further reading section to bibliography
e47d87fdd Add a note about Trigg's correction
d2852518d Fix the docs for Problem::RemoveResidualBlock & Problem::RemoveParameterBlock
06e02a173 Delete unused files split.h/cc
ac7268da5 Fix an 80cols issue in covariance_impl.cc
17dccef91 Add NumericDiffFirstOrderFunction
03d64141a Fix a incorrect check in reorder_program.cc
884111913 Two changes to TinySolver
4dff3ea2c Fix a number of typos in rotation.h
98719ced4 Fix a type in interfacing_with_autodiff.html
0299ce944 Update conf.py to be compatible with Sphinx 4.1.2
2a2b9bd6f Fix a bug in covariance_impl.cc
27fade7b8 Fix a bug in system_test.cc
d4eb83ee5 Fix the Jacobian in trust_region_minimizer_test.cc
5f6071a1c Fix a bug in local_parameterization_test.cc
b2e732b1e Fix errors in comments from William Gandler.
42f1d6717 Add accessors to GradientProblem
aefd37b18 Refactor small_blas_gemm_benchmark
dc20db303 [docs] Fix `IterationSummary` section. Add missing `IterationCallback`
90ba7d1ef [docs] Fix typos
c3129c3d4 Fix tests not executing
7de561e8e Fix dependency check for building documentation
4fbe218f2 Refactor small_blas_test
3fdde8ede Remove an errant double link.
20ad431f7 Fixing a typo in the version history
0c85c4092 Revert "Reduce copies involved in Jet operations"
3a02d5aaf Fix typo in LossFunctionWrapper sample code
7b2c223be Add fmax/fmin overloads for scalars
c036c7819 Reduce copies involved in Jet operations
51945e061 Introduce benchmark for Jet operations
ec4f2995b Do not check MaxNumThreadsAvailable if the thread number is set to 1.
98f639f54 Add a macro CERES_GET_FLAG.
766f2cab5 Reduce log spam in covariance_impl.cc.
941ea1347 Fix FindTBB version detection with TBB >= 2021.1.1
323c350a6 fix Eigen3_VERSION
2b32b3212 Revert "Group specializations into groups of four"
313caf1ae Allow Unity build.
4ba244cdb Group specializations into groups of four
d77a8100a Make miniglog's InitGoogleLogging argument const.
863724994 Use portable expression for constant 2/sqrt(pi)
97873ea65 Add some missing includes for glog/logging.h
d15b1bcd3 Increase tolerance in small_blas_test.cc
17cf01831 Hide 'format not a string literal' error in examples
64029909b Fix -Wno-maybe-uninitialized error
21294123d Fix nonnull arg compared to NULL error.
1dd417410 Fix -Wno-format-nonliteral
6c106bf51 Fix -Wmissing-field-initializers error
c48a32792 Use cc_binary includes so examples build as external repo
e0e14a5cd Fix errors found by -Werror
e84cf10e1 Fix an errant double in TinySolver.
66b4c33e8 updated unit quaternion rotation
d45ec47b5 Fix a typo in schur_eliminator.h
Change-Id: I60db062e44d051d50dbb3a145eec2f74d5190481
git-subtree-dir: third_party/ceres
git-subtree-split: 7f9cc571b03632f1df93ea35725a1f5dfffe2c72
Signed-off-by: Austin Schuh <austin.linux@gmail.com>
diff --git a/include/ceres/autodiff_cost_function.h b/include/ceres/autodiff_cost_function.h
index 207f0a4..878b2ec 100644
--- a/include/ceres/autodiff_cost_function.h
+++ b/include/ceres/autodiff_cost_function.h
@@ -1,5 +1,5 @@
// Ceres Solver - A fast non-linear least squares minimizer
-// Copyright 2019 Google Inc. All rights reserved.
+// Copyright 2024 Google Inc. All rights reserved.
// http://ceres-solver.org/
//
// Redistribution and use in source and binary forms, with or without
@@ -82,9 +82,9 @@
// 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)); ^ ^ ^
+// auto* cost_function
+// = new AutoDiffCostFunction<MyScalarCostFunctor, 1, 2, 2>(1.0);
+// ^ ^ ^
// | | |
// Dimension of residual -----+ | |
// Dimension of x ---------------+ |
@@ -99,9 +99,11 @@
// 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), ^ ^ ^
+// auto functor = std::make_unique<CostFunctorWithDynamicNumResiduals>(1.0);
+// auto* cost_function
+// = new AutoDiffCostFunction<CostFunctorWithDynamicNumResiduals,
+// DYNAMIC, 2, 2>(
+// std::move(functor), ^ ^ ^
// runtime_number_of_residuals); <----+ | | |
// | | | |
// | | | |
@@ -126,11 +128,11 @@
#define CERES_PUBLIC_AUTODIFF_COST_FUNCTION_H_
#include <memory>
+#include <type_traits>
#include "ceres/internal/autodiff.h"
#include "ceres/sized_cost_function.h"
#include "ceres/types.h"
-#include "glog/logging.h"
namespace ceres {
@@ -151,17 +153,36 @@
template <typename CostFunctor,
int kNumResiduals, // Number of residuals, or ceres::DYNAMIC.
int... Ns> // Number of parameters in each parameter block.
-class AutoDiffCostFunction : public SizedCostFunction<kNumResiduals, Ns...> {
+class AutoDiffCostFunction final
+ : public SizedCostFunction<kNumResiduals, Ns...> {
public:
// Takes ownership of functor by default. Uses the template-provided
// value for the number of residuals ("kNumResiduals").
+ explicit AutoDiffCostFunction(std::unique_ptr<CostFunctor> functor)
+ : AutoDiffCostFunction{std::move(functor), TAKE_OWNERSHIP, FIXED_INIT} {}
+
+ // Constructs the CostFunctor on the heap and takes the ownership.
+ // Invocable only if the number of residuals is known at compile-time.
+ template <class... Args,
+ bool kIsDynamic = kNumResiduals == DYNAMIC,
+ std::enable_if_t<!kIsDynamic &&
+ std::is_constructible_v<CostFunctor, Args&&...>>* =
+ nullptr>
+ explicit AutoDiffCostFunction(Args&&... args)
+ // NOTE We explicitly use direct initialization using parentheses instead
+ // of uniform initialization using braces to avoid narrowing conversion
+ // warnings.
+ : AutoDiffCostFunction{
+ std::make_unique<CostFunctor>(std::forward<Args>(args)...)} {}
+
+ AutoDiffCostFunction(std::unique_ptr<CostFunctor> functor, int num_residuals)
+ : AutoDiffCostFunction{
+ std::move(functor), num_residuals, TAKE_OWNERSHIP, DYNAMIC_INIT} {}
+
explicit AutoDiffCostFunction(CostFunctor* functor,
Ownership ownership = TAKE_OWNERSHIP)
- : functor_(functor), ownership_(ownership) {
- static_assert(kNumResiduals != DYNAMIC,
- "Can't run the fixed-size constructor if the number of "
- "residuals is set to ceres::DYNAMIC.");
- }
+ : AutoDiffCostFunction{
+ std::unique_ptr<CostFunctor>{functor}, ownership, FIXED_INIT} {}
// Takes ownership of functor by default. Ignores the template-provided
// kNumResiduals in favor of the "num_residuals" argument provided.
@@ -171,17 +192,18 @@
AutoDiffCostFunction(CostFunctor* functor,
int num_residuals,
Ownership ownership = TAKE_OWNERSHIP)
- : functor_(functor), ownership_(ownership) {
- static_assert(kNumResiduals == DYNAMIC,
- "Can't run the dynamic-size constructor if the number of "
- "residuals is not ceres::DYNAMIC.");
- SizedCostFunction<kNumResiduals, Ns...>::set_num_residuals(num_residuals);
- }
+ : AutoDiffCostFunction{std::unique_ptr<CostFunctor>{functor},
+ num_residuals,
+ ownership,
+ DYNAMIC_INIT} {}
- explicit AutoDiffCostFunction(AutoDiffCostFunction&& other)
- : functor_(std::move(other.functor_)), ownership_(other.ownership_) {}
+ AutoDiffCostFunction(AutoDiffCostFunction&& other) noexcept = default;
+ AutoDiffCostFunction& operator=(AutoDiffCostFunction&& other) noexcept =
+ default;
+ AutoDiffCostFunction(const AutoDiffCostFunction& other) = delete;
+ AutoDiffCostFunction& operator=(const AutoDiffCostFunction& other) = delete;
- virtual ~AutoDiffCostFunction() {
+ ~AutoDiffCostFunction() override {
// Manually release pointer if configured to not take ownership rather than
// deleting only if ownership is taken.
// This is to stay maximally compatible to old user code which may have
@@ -203,7 +225,7 @@
using ParameterDims =
typename SizedCostFunction<kNumResiduals, Ns...>::ParameterDims;
- if (!jacobians) {
+ if (jacobians == nullptr) {
return internal::VariadicEvaluate<ParameterDims>(
*functor_, parameters, residuals);
}
@@ -215,7 +237,36 @@
jacobians);
};
+ const CostFunctor& functor() const { return *functor_; }
+
private:
+ // Tags used to differentiate between dynamic and fixed size constructor
+ // delegate invocations.
+ static constexpr std::integral_constant<int, DYNAMIC> DYNAMIC_INIT{};
+ static constexpr std::integral_constant<int, kNumResiduals> FIXED_INIT{};
+
+ template <class InitTag>
+ AutoDiffCostFunction(std::unique_ptr<CostFunctor> functor,
+ int num_residuals,
+ Ownership ownership,
+ InitTag /*unused*/)
+ : functor_{std::move(functor)}, ownership_{ownership} {
+ static_assert(kNumResiduals == FIXED_INIT,
+ "Can't run the fixed-size constructor if the number of "
+ "residuals is set to ceres::DYNAMIC.");
+
+ if constexpr (InitTag::value == DYNAMIC_INIT) {
+ SizedCostFunction<kNumResiduals, Ns...>::set_num_residuals(num_residuals);
+ }
+ }
+
+ template <class InitTag>
+ AutoDiffCostFunction(std::unique_ptr<CostFunctor> functor,
+ Ownership ownership,
+ InitTag tag)
+ : AutoDiffCostFunction{
+ std::move(functor), kNumResiduals, ownership, tag} {}
+
std::unique_ptr<CostFunctor> functor_;
Ownership ownership_;
};
diff --git a/include/ceres/autodiff_first_order_function.h b/include/ceres/autodiff_first_order_function.h
index b98d845..6cd1b13 100644
--- a/include/ceres/autodiff_first_order_function.h
+++ b/include/ceres/autodiff_first_order_function.h
@@ -1,5 +1,5 @@
// Ceres Solver - A fast non-linear least squares minimizer
-// Copyright 2019 Google Inc. All rights reserved.
+// Copyright 2024 Google Inc. All rights reserved.
// http://ceres-solver.org/
//
// Redistribution and use in source and binary forms, with or without
@@ -32,6 +32,7 @@
#define CERES_PUBLIC_AUTODIFF_FIRST_ORDER_FUNCTION_H_
#include <memory>
+#include <type_traits>
#include "ceres/first_order_function.h"
#include "ceres/internal/eigen.h"
@@ -102,15 +103,25 @@
// seen where instead of using a_ directly, a_ is wrapped with T(a_).
template <typename FirstOrderFunctor, int kNumParameters>
-class AutoDiffFirstOrderFunction : public FirstOrderFunction {
+class AutoDiffFirstOrderFunction final : public FirstOrderFunction {
public:
// Takes ownership of functor.
explicit AutoDiffFirstOrderFunction(FirstOrderFunctor* functor)
- : functor_(functor) {
+ : AutoDiffFirstOrderFunction{
+ std::unique_ptr<FirstOrderFunctor>{functor}} {}
+
+ explicit AutoDiffFirstOrderFunction(
+ std::unique_ptr<FirstOrderFunctor> functor)
+ : functor_(std::move(functor)) {
static_assert(kNumParameters > 0, "kNumParameters must be positive");
}
- virtual ~AutoDiffFirstOrderFunction() {}
+ template <class... Args,
+ std::enable_if_t<std::is_constructible_v<FirstOrderFunctor,
+ Args&&...>>* = nullptr>
+ explicit AutoDiffFirstOrderFunction(Args&&... args)
+ : AutoDiffFirstOrderFunction{
+ std::make_unique<FirstOrderFunctor>(std::forward<Args>(args)...)} {}
bool Evaluate(const double* const parameters,
double* cost,
@@ -119,7 +130,7 @@
return (*functor_)(parameters, cost);
}
- typedef Jet<double, kNumParameters> JetT;
+ using JetT = Jet<double, kNumParameters>;
internal::FixedArray<JetT, (256 * 7) / sizeof(JetT)> x(kNumParameters);
for (int i = 0; i < kNumParameters; ++i) {
x[i].a = parameters[i];
@@ -142,6 +153,8 @@
int NumParameters() const override { return kNumParameters; }
+ const FirstOrderFunctor& functor() const { return *functor_; }
+
private:
std::unique_ptr<FirstOrderFunctor> functor_;
};
diff --git a/include/ceres/autodiff_local_parameterization.h b/include/ceres/autodiff_local_parameterization.h
deleted file mode 100644
index d694376..0000000
--- a/include/ceres/autodiff_local_parameterization.h
+++ /dev/null
@@ -1,152 +0,0 @@
-// Ceres Solver - A fast non-linear least squares minimizer
-// Copyright 2019 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 <memory>
-
-#include "ceres/internal/autodiff.h"
-#include "ceres/local_parameterization.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() {}
- bool Plus(const double* x,
- const double* delta,
- double* x_plus_delta) const override {
- return (*functor_)(x, delta, x_plus_delta);
- }
-
- bool ComputeJacobian(const double* x, double* jacobian) const override {
- 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::AutoDifferentiate<
- kGlobalSize,
- internal::StaticParameterDims<kGlobalSize, kLocalSize>>(
- *functor_, parameter_ptrs, kGlobalSize, x_plus_delta, jacobian_ptrs);
- }
-
- int GlobalSize() const override { return kGlobalSize; }
- int LocalSize() const override { return kLocalSize; }
-
- private:
- std::unique_ptr<Functor> functor_;
-};
-
-} // namespace ceres
-
-#endif // CERES_PUBLIC_AUTODIFF_LOCAL_PARAMETERIZATION_H_
diff --git a/include/ceres/autodiff_manifold.h b/include/ceres/autodiff_manifold.h
new file mode 100644
index 0000000..4bf7e56
--- /dev/null
+++ b/include/ceres/autodiff_manifold.h
@@ -0,0 +1,259 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2023 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_AUTODIFF_MANIFOLD_H_
+#define CERES_PUBLIC_AUTODIFF_MANIFOLD_H_
+
+#include <memory>
+
+#include "ceres/internal/autodiff.h"
+#include "ceres/manifold.h"
+
+namespace ceres {
+
+// Create a Manifold with Jacobians computed via automatic differentiation. For
+// more information on manifolds, see include/ceres/manifold.h
+//
+// To get an auto differentiated manifold, you must define a class/struct with
+// templated Plus and Minus functions that compute
+//
+// x_plus_delta = Plus(x, delta);
+// y_minus_x = Minus(y, x);
+//
+// Where, x, y and x_plus_delta are vectors on the manifold in the ambient space
+// (so they are kAmbientSize vectors) and delta, y_minus_x are vectors in the
+// tangent space (so they are kTangentSize vectors).
+//
+// The Functor should have the signature:
+//
+// struct Functor {
+// template <typename T>
+// bool Plus(const T* x, const T* delta, T* x_plus_delta) const;
+//
+// template <typename T>
+// bool Minus(const T* y, const T* x, T* y_minus_x) const;
+// };
+//
+// Observe that the Plus and Minus operations are templated on the parameter T.
+// The autodiff framework substitutes appropriate "Jet" objects for T in order
+// to compute the derivative when necessary. This is the same mechanism that is
+// used to compute derivatives when using AutoDiffCostFunction.
+//
+// Plus and Minus should return true if the computation is successful and false
+// otherwise, in which case the result will not be used.
+//
+// Given this Functor, the corresponding Manifold can be constructed as:
+//
+// AutoDiffManifold<Functor, kAmbientSize, kTangentSize> manifold;
+//
+// As a concrete example consider the case of Quaternions. Quaternions form a
+// three dimensional manifold embedded in R^4, i.e. they have an ambient
+// dimension of 4 and their tangent space has dimension 3. The following Functor
+// (taken from autodiff_manifold_test.cc) defines the Plus and Minus operations
+// on the Quaternion manifold:
+//
+// NOTE: The following is only used for illustration purposes. Ceres Solver
+// ships with optimized production grade QuaternionManifold implementation. See
+// manifold.h.
+//
+// This functor assumes that the quaternions are laid out as [w,x,y,z] in
+// memory, i.e. the real or scalar part is the first coordinate.
+//
+// struct QuaternionFunctor {
+// template <typename T>
+// bool Plus(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;
+// }
+//
+// template <typename T>
+// bool Minus(const T* y, const T* x, T* y_minus_x) const {
+// T minus_x[4] = {x[0], -x[1], -x[2], -x[3]};
+// T ambient_y_minus_x[4];
+// QuaternionProduct(y, minus_x, ambient_y_minus_x);
+// T u_norm = sqrt(ambient_y_minus_x[1] * ambient_y_minus_x[1] +
+// ambient_y_minus_x[2] * ambient_y_minus_x[2] +
+// ambient_y_minus_x[3] * ambient_y_minus_x[3]);
+// if (u_norm > 0.0) {
+// T theta = atan2(u_norm, ambient_y_minus_x[0]);
+// y_minus_x[0] = theta * ambient_y_minus_x[1] / u_norm;
+// y_minus_x[1] = theta * ambient_y_minus_x[2] / u_norm;
+// y_minus_x[2] = theta * ambient_y_minus_x[3] / u_norm;
+// } else {
+// // We do not use [0,0,0] here because even though the value part is
+// // a constant, the derivative part is not.
+// y_minus_x[0] = ambient_y_minus_x[1];
+// y_minus_x[1] = ambient_y_minus_x[2];
+// y_minus_x[2] = ambient_y_minus_x[3];
+// }
+// return true;
+// }
+// };
+//
+// Then given this struct, the auto differentiated Quaternion Manifold can now
+// be constructed as
+//
+// Manifold* manifold = new AutoDiffManifold<QuaternionFunctor, 4, 3>;
+
+template <typename Functor, int kAmbientSize, int kTangentSize>
+class AutoDiffManifold final : public Manifold {
+ public:
+ AutoDiffManifold() : functor_(std::make_unique<Functor>()) {}
+
+ // Takes ownership of functor.
+ explicit AutoDiffManifold(Functor* functor) : functor_(functor) {}
+
+ int AmbientSize() const override { return kAmbientSize; }
+ int TangentSize() const override { return kTangentSize; }
+
+ bool Plus(const double* x,
+ const double* delta,
+ double* x_plus_delta) const override {
+ return functor_->Plus(x, delta, x_plus_delta);
+ }
+
+ bool PlusJacobian(const double* x, double* jacobian) const override;
+
+ bool Minus(const double* y,
+ const double* x,
+ double* y_minus_x) const override {
+ return functor_->Minus(y, x, y_minus_x);
+ }
+
+ bool MinusJacobian(const double* x, double* jacobian) const override;
+
+ const Functor& functor() const { return *functor_; }
+
+ private:
+ std::unique_ptr<Functor> functor_;
+};
+
+namespace internal {
+
+// The following two helper structs are needed to interface the Plus and Minus
+// methods of the ManifoldFunctor with the automatic differentiation which
+// expects a Functor with operator().
+template <typename Functor>
+struct PlusWrapper {
+ explicit PlusWrapper(const Functor& functor) : functor(functor) {}
+ template <typename T>
+ bool operator()(const T* x, const T* delta, T* x_plus_delta) const {
+ return functor.Plus(x, delta, x_plus_delta);
+ }
+ const Functor& functor;
+};
+
+template <typename Functor>
+struct MinusWrapper {
+ explicit MinusWrapper(const Functor& functor) : functor(functor) {}
+ template <typename T>
+ bool operator()(const T* y, const T* x, T* y_minus_x) const {
+ return functor.Minus(y, x, y_minus_x);
+ }
+ const Functor& functor;
+};
+} // namespace internal
+
+template <typename Functor, int kAmbientSize, int kTangentSize>
+bool AutoDiffManifold<Functor, kAmbientSize, kTangentSize>::PlusJacobian(
+ const double* x, double* jacobian) const {
+ double zero_delta[kTangentSize];
+ for (int i = 0; i < kTangentSize; ++i) {
+ zero_delta[i] = 0.0;
+ }
+
+ double x_plus_delta[kAmbientSize];
+ for (int i = 0; i < kAmbientSize; ++i) {
+ x_plus_delta[i] = 0.0;
+ }
+
+ const double* parameter_ptrs[2] = {x, zero_delta};
+
+ // PlusJacobian is D_2 Plus(x,0) so we only need to compute the Jacobian
+ // w.r.t. the second argument.
+ double* jacobian_ptrs[2] = {nullptr, jacobian};
+ return internal::AutoDifferentiate<
+ kAmbientSize,
+ internal::StaticParameterDims<kAmbientSize, kTangentSize>>(
+ internal::PlusWrapper<Functor>(*functor_),
+ parameter_ptrs,
+ kAmbientSize,
+ x_plus_delta,
+ jacobian_ptrs);
+}
+
+template <typename Functor, int kAmbientSize, int kTangentSize>
+bool AutoDiffManifold<Functor, kAmbientSize, kTangentSize>::MinusJacobian(
+ const double* x, double* jacobian) const {
+ double y_minus_x[kTangentSize];
+ for (int i = 0; i < kTangentSize; ++i) {
+ y_minus_x[i] = 0.0;
+ }
+
+ const double* parameter_ptrs[2] = {x, x};
+
+ // MinusJacobian is D_1 Minus(x,x), so we only need to compute the Jacobian
+ // w.r.t. the first argument.
+ double* jacobian_ptrs[2] = {jacobian, nullptr};
+ return internal::AutoDifferentiate<
+ kTangentSize,
+ internal::StaticParameterDims<kAmbientSize, kAmbientSize>>(
+ internal::MinusWrapper<Functor>(*functor_),
+ parameter_ptrs,
+ kTangentSize,
+ y_minus_x,
+ jacobian_ptrs);
+}
+
+} // namespace ceres
+
+#endif // CERES_PUBLIC_AUTODIFF_MANIFOLD_H_
diff --git a/include/ceres/c_api.h b/include/ceres/c_api.h
index 91b82bf..30bcaaf 100644
--- a/include/ceres/c_api.h
+++ b/include/ceres/c_api.h
@@ -1,5 +1,5 @@
/* Ceres Solver - A fast non-linear least squares minimizer
- * Copyright 2019 Google Inc. All rights reserved.
+ * Copyright 2023 Google Inc. All rights reserved.
* http://ceres-solver.org/
*
* Redistribution and use in source and binary forms, with or without
@@ -39,7 +39,7 @@
#define CERES_PUBLIC_C_API_H_
// clang-format off
-#include "ceres/internal/port.h"
+#include "ceres/internal/export.h"
#include "ceres/internal/disable_warnings.h"
// clang-format on
diff --git a/include/ceres/ceres.h b/include/ceres/ceres.h
index d249351..51f9d89 100644
--- a/include/ceres/ceres.h
+++ b/include/ceres/ceres.h
@@ -1,5 +1,5 @@
// Ceres Solver - A fast non-linear least squares minimizer
-// Copyright 2019 Google Inc. All rights reserved.
+// Copyright 2023 Google Inc. All rights reserved.
// http://ceres-solver.org/
//
// Redistribution and use in source and binary forms, with or without
@@ -34,9 +34,12 @@
#ifndef CERES_PUBLIC_CERES_H_
#define CERES_PUBLIC_CERES_H_
+// IWYU pragma: begin_exports
#include "ceres/autodiff_cost_function.h"
-#include "ceres/autodiff_local_parameterization.h"
+#include "ceres/autodiff_first_order_function.h"
+#include "ceres/autodiff_manifold.h"
#include "ceres/conditioned_cost_function.h"
+#include "ceres/constants.h"
#include "ceres/context.h"
#include "ceres/cost_function.h"
#include "ceres/cost_function_to_functor.h"
@@ -47,20 +50,26 @@
#include "ceres/dynamic_cost_function_to_functor.h"
#include "ceres/dynamic_numeric_diff_cost_function.h"
#include "ceres/evaluation_callback.h"
+#include "ceres/first_order_function.h"
#include "ceres/gradient_checker.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/line_manifold.h"
#include "ceres/loss_function.h"
+#include "ceres/manifold.h"
#include "ceres/numeric_diff_cost_function.h"
+#include "ceres/numeric_diff_first_order_function.h"
#include "ceres/numeric_diff_options.h"
#include "ceres/ordered_groups.h"
#include "ceres/problem.h"
+#include "ceres/product_manifold.h"
#include "ceres/sized_cost_function.h"
#include "ceres/solver.h"
+#include "ceres/sphere_manifold.h"
#include "ceres/types.h"
#include "ceres/version.h"
+// IWYU pragma: end_exports
#endif // CERES_PUBLIC_CERES_H_
diff --git a/include/ceres/conditioned_cost_function.h b/include/ceres/conditioned_cost_function.h
index a57ee20..1edc006 100644
--- a/include/ceres/conditioned_cost_function.h
+++ b/include/ceres/conditioned_cost_function.h
@@ -1,5 +1,5 @@
// Ceres Solver - A fast non-linear least squares minimizer
-// Copyright 2019 Google Inc. All rights reserved.
+// Copyright 2023 Google Inc. All rights reserved.
// http://ceres-solver.org/
//
// Redistribution and use in source and binary forms, with or without
@@ -71,18 +71,18 @@
// ccf_residual[i] = f_i(my_cost_function_residual[i])
//
// and the Jacobian will be affected appropriately.
-class CERES_EXPORT ConditionedCostFunction : public CostFunction {
+class CERES_EXPORT ConditionedCostFunction final : 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.
+ // may be nullptr, in which case the corresponding residual is not modified.
//
// The conditioners can repeat.
ConditionedCostFunction(CostFunction* wrapped_cost_function,
const std::vector<CostFunction*>& conditioners,
Ownership ownership);
- virtual ~ConditionedCostFunction();
+ ~ConditionedCostFunction() override;
bool Evaluate(double const* const* parameters,
double* residuals,
diff --git a/include/ceres/constants.h b/include/ceres/constants.h
new file mode 100644
index 0000000..584b669
--- /dev/null
+++ b/include/ceres/constants.h
@@ -0,0 +1,42 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2023 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: hellston20a@gmail.com (H S Helson Go)
+
+#ifndef CERES_PUBLIC_CONSTANTS_H_
+#define CERES_PUBLIC_CONSTANTS_H_
+
+// TODO(HSHelson): This header should no longer be necessary once C++20's
+// <numbers> (e.g. std::numbers::pi_v) becomes usable
+namespace ceres::constants {
+template <typename T>
+inline constexpr T pi_v(3.141592653589793238462643383279502884);
+inline constexpr double pi = pi_v<double>;
+} // namespace ceres::constants
+
+#endif // CERES_PUBLIC_CONSTANTS_H_
diff --git a/include/ceres/context.h b/include/ceres/context.h
index d08e32b..fe18726 100644
--- a/include/ceres/context.h
+++ b/include/ceres/context.h
@@ -1,5 +1,5 @@
// Ceres Solver - A fast non-linear least squares minimizer
-// Copyright 2019 Google Inc. All rights reserved.
+// Copyright 2023 Google Inc. All rights reserved.
// http://ceres-solver.org/
//
// Redistribution and use in source and binary forms, with or without
@@ -31,6 +31,8 @@
#ifndef CERES_PUBLIC_CONTEXT_H_
#define CERES_PUBLIC_CONTEXT_H_
+#include "ceres/internal/export.h"
+
namespace ceres {
// A global context for processing data in Ceres. This provides a mechanism to
@@ -39,13 +41,13 @@
// Problems, either serially or in parallel. When using it with multiple
// Problems at the same time, they may end up contending for resources
// (e.g. threads) managed by the Context.
-class Context {
+class CERES_EXPORT Context {
public:
- Context() {}
+ Context();
Context(const Context&) = delete;
void operator=(const Context&) = delete;
- virtual ~Context() {}
+ virtual ~Context();
// Creates a context object and the caller takes ownership.
static Context* Create();
diff --git a/include/ceres/cost_function.h b/include/ceres/cost_function.h
index d1550c1..2e5b1dd 100644
--- a/include/ceres/cost_function.h
+++ b/include/ceres/cost_function.h
@@ -1,5 +1,5 @@
// Ceres Solver - A fast non-linear least squares minimizer
-// Copyright 2019 Google Inc. All rights reserved.
+// Copyright 2024 Google Inc. All rights reserved.
// http://ceres-solver.org/
//
// Redistribution and use in source and binary forms, with or without
@@ -48,7 +48,7 @@
#include <vector>
#include "ceres/internal/disable_warnings.h"
-#include "ceres/internal/port.h"
+#include "ceres/internal/export.h"
namespace ceres {
@@ -63,11 +63,11 @@
// when added with AddResidualBlock().
class CERES_EXPORT CostFunction {
public:
- CostFunction() : num_residuals_(0) {}
+ CostFunction();
CostFunction(const CostFunction&) = delete;
- void operator=(const CostFunction&) = delete;
+ CostFunction& operator=(const CostFunction&) = delete;
- virtual ~CostFunction() {}
+ virtual ~CostFunction();
// Inputs:
//
@@ -92,8 +92,8 @@
// 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
+ // If jacobians is nullptr, then no derivatives are returned; this is
+ // the case when computing cost only. If jacobians[i] is nullptr, then
// the jacobian block corresponding to the i'th parameter block must
// not to be returned.
//
@@ -124,6 +124,10 @@
int num_residuals() const { return num_residuals_; }
protected:
+ // Prevent moving through the base class
+ CostFunction(CostFunction&& other) noexcept;
+ CostFunction& operator=(CostFunction&& other) noexcept;
+
std::vector<int32_t>* mutable_parameter_block_sizes() {
return ¶meter_block_sizes_;
}
diff --git a/include/ceres/cost_function_to_functor.h b/include/ceres/cost_function_to_functor.h
index 9364293..573508e 100644
--- a/include/ceres/cost_function_to_functor.h
+++ b/include/ceres/cost_function_to_functor.h
@@ -1,5 +1,5 @@
// Ceres Solver - A fast non-linear least squares minimizer
-// Copyright 2019 Google Inc. All rights reserved.
+// Copyright 2024 Google Inc. All rights reserved.
// http://ceres-solver.org/
//
// Redistribution and use in source and binary forms, with or without
@@ -94,10 +94,9 @@
#include "ceres/cost_function.h"
#include "ceres/dynamic_cost_function_to_functor.h"
-#include "ceres/internal/fixed_array.h"
#include "ceres/internal/parameter_dims.h"
-#include "ceres/internal/port.h"
#include "ceres/types.h"
+#include "glog/logging.h"
namespace ceres {
@@ -106,12 +105,16 @@
public:
// Takes ownership of cost_function.
explicit CostFunctionToFunctor(CostFunction* cost_function)
- : cost_functor_(cost_function) {
- CHECK(cost_function != nullptr);
+ : CostFunctionToFunctor{std::unique_ptr<CostFunction>{cost_function}} {}
+
+ // Takes ownership of cost_function.
+ explicit CostFunctionToFunctor(std::unique_ptr<CostFunction> cost_function)
+ : cost_functor_(std::move(cost_function)) {
+ CHECK(cost_functor_.function() != nullptr);
CHECK(kNumResiduals > 0 || kNumResiduals == DYNAMIC);
const std::vector<int32_t>& parameter_block_sizes =
- cost_function->parameter_block_sizes();
+ cost_functor_.function()->parameter_block_sizes();
const int num_parameter_blocks = ParameterDims::kNumParameterBlocks;
CHECK_EQ(static_cast<int>(parameter_block_sizes.size()),
num_parameter_blocks);
@@ -119,7 +122,7 @@
if (parameter_block_sizes.size() == num_parameter_blocks) {
for (int block = 0; block < num_parameter_blocks; ++block) {
CHECK_EQ(ParameterDims::GetDim(block), parameter_block_sizes[block])
- << "Parameter block size missmatch. The specified static parameter "
+ << "Parameter block size mismatch. The specified static parameter "
"block dimension does not match the one from the cost function.";
}
}
diff --git a/include/ceres/covariance.h b/include/ceres/covariance.h
index 2fe025d..d477f31 100644
--- a/include/ceres/covariance.h
+++ b/include/ceres/covariance.h
@@ -1,5 +1,5 @@
// Ceres Solver - A fast non-linear least squares minimizer
-// Copyright 2019 Google Inc. All rights reserved.
+// Copyright 2023 Google Inc. All rights reserved.
// http://ceres-solver.org/
//
// Redistribution and use in source and binary forms, with or without
@@ -35,8 +35,9 @@
#include <utility>
#include <vector>
+#include "ceres/internal/config.h"
#include "ceres/internal/disable_warnings.h"
-#include "ceres/internal/port.h"
+#include "ceres/internal/export.h"
#include "ceres/types.h"
namespace ceres {
@@ -145,7 +146,7 @@
// 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
+// use an appropriate Manifold. 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.
@@ -245,6 +246,20 @@
// used.
CovarianceAlgorithmType algorithm_type = SPARSE_QR;
+ // During QR factorization, if a column with Euclidean norm less
+ // than column_pivot_threshold is encountered it is treated as
+ // zero.
+ //
+ // If column_pivot_threshold < 0, then an automatic default value
+ // of 20*(m+n)*eps*sqrt(max(diag(J’*J))) is used. Here m and n are
+ // the number of rows and columns of the Jacobian (J)
+ // respectively.
+ //
+ // This is an advanced option meant for users who know enough
+ // about their Jacobian matrices that they can determine a value
+ // better than the default.
+ double column_pivot_threshold = -1;
+
// If the Jacobian matrix is near singular, then inverting J'J
// will result in unreliable results, e.g, if
//
@@ -265,7 +280,7 @@
//
// min_sigma / max_sigma < sqrt(min_reciprocal_condition_number)
//
- // where min_sigma and max_sigma are the minimum and maxiumum
+ // where min_sigma and max_sigma are the minimum and maximum
// singular values of J respectively.
//
// 2. SPARSE_QR
@@ -393,11 +408,9 @@
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.
+ // Returns the block of the cross-covariance in the tangent space if a
+ // manifold 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,
@@ -429,9 +442,8 @@
double* covariance_matrix) const;
// Return the covariance matrix corresponding to parameter_blocks
- // in the tangent space if a local parameterization is associated
- // with one of the parameter blocks else returns the covariance
- // matrix in the ambient space.
+ // in the tangent space if a manifold is associated with one of the parameter
+ // blocks else returns the covariance matrix in the ambient space.
//
// Compute must be called before calling GetCovarianceMatrix and all
// parameter_blocks must have been present in the vector
diff --git a/include/ceres/crs_matrix.h b/include/ceres/crs_matrix.h
index bc618fa..787b6a3 100644
--- a/include/ceres/crs_matrix.h
+++ b/include/ceres/crs_matrix.h
@@ -1,5 +1,5 @@
// Ceres Solver - A fast non-linear least squares minimizer
-// Copyright 2019 Google Inc. All rights reserved.
+// Copyright 2023 Google Inc. All rights reserved.
// http://ceres-solver.org/
//
// Redistribution and use in source and binary forms, with or without
@@ -34,17 +34,17 @@
#include <vector>
#include "ceres/internal/disable_warnings.h"
-#include "ceres/internal/port.h"
+#include "ceres/internal/export.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) {}
+ CRSMatrix() = default;
- int num_rows;
- int num_cols;
+ int num_rows{0};
+ int num_cols{0};
// A compressed row matrix stores its contents in three arrays,
// rows, cols and values.
diff --git a/include/ceres/cubic_interpolation.h b/include/ceres/cubic_interpolation.h
index 9b9ea4a..f165d2b 100644
--- a/include/ceres/cubic_interpolation.h
+++ b/include/ceres/cubic_interpolation.h
@@ -1,5 +1,5 @@
// Ceres Solver - A fast non-linear least squares minimizer
-// Copyright 2019 Google Inc. All rights reserved.
+// Copyright 2023 Google Inc. All rights reserved.
// http://ceres-solver.org/
//
// Redistribution and use in source and binary forms, with or without
@@ -32,7 +32,7 @@
#define CERES_PUBLIC_CUBIC_INTERPOLATION_H_
#include "Eigen/Core"
-#include "ceres/internal/port.h"
+#include "ceres/internal/export.h"
#include "glog/logging.h"
namespace ceres {
@@ -59,8 +59,8 @@
// 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.
+// f if not nullptr will contain the interpolated function values.
+// dfdx if not nullptr 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,
@@ -69,7 +69,7 @@
const double x,
double* f,
double* dfdx) {
- typedef Eigen::Matrix<double, kDataDimension, 1> VType;
+ using VType = Eigen::Matrix<double, kDataDimension, 1>;
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);
@@ -79,12 +79,12 @@
// derivative.
// f = ax^3 + bx^2 + cx + d
- if (f != NULL) {
+ if (f != nullptr) {
Eigen::Map<VType>(f, kDataDimension) = d + x * (c + x * (b + x * a));
}
// dfdx = 3ax^2 + 2bx + c
- if (dfdx != NULL) {
+ if (dfdx != nullptr) {
Eigen::Map<VType>(dfdx, kDataDimension) = c + x * (2.0 * b + 3.0 * a * x);
}
}
@@ -143,7 +143,7 @@
// 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); }
+ void Evaluate(const double& x, double* f) const { Evaluate(x, f, nullptr); }
template <typename JetT>
void Evaluate(const JetT& x, JetT* f) const {
@@ -191,7 +191,7 @@
}
EIGEN_STRONG_INLINE void GetValue(const int n, double* f) const {
- const int idx = std::min(std::max(begin_, n), end_ - 1) - begin_;
+ 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]);
@@ -317,10 +317,10 @@
// 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) {
+ if (dfdc != nullptr) {
// Interpolate vertically the derivative along the columns.
CubicHermiteSpline<Grid::DATA_DIMENSION>(
- df0dc, df1dc, df2dc, df3dc, r - row, dfdc, NULL);
+ df0dc, df1dc, df2dc, df3dc, r - row, dfdc, nullptr);
}
}
@@ -328,7 +328,7 @@
// 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);
+ Evaluate(r, c, f, nullptr, nullptr);
}
template <typename JetT>
@@ -368,7 +368,7 @@
//
// f001, f002, f011, f012, ...
//
-// A commonly occuring example are color images (RGB) where the three
+// A commonly occurring example are color images (RGB) where the three
// channels are stored interleaved.
//
// If kInterleaved = false, then it is stored as
@@ -402,9 +402,9 @@
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_;
+ (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_;
+ (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;
diff --git a/include/ceres/dynamic_autodiff_cost_function.h b/include/ceres/dynamic_autodiff_cost_function.h
index 7ccf6a8..2b8724d 100644
--- a/include/ceres/dynamic_autodiff_cost_function.h
+++ b/include/ceres/dynamic_autodiff_cost_function.h
@@ -1,5 +1,5 @@
// Ceres Solver - A fast non-linear least squares minimizer
-// Copyright 2019 Google Inc. All rights reserved.
+// Copyright 2024 Google Inc. All rights reserved.
// http://ceres-solver.org/
//
// Redistribution and use in source and binary forms, with or without
@@ -35,6 +35,7 @@
#include <cmath>
#include <memory>
#include <numeric>
+#include <type_traits>
#include <vector>
#include "ceres/dynamic_cost_function.h"
@@ -65,8 +66,7 @@
// also specify the sizes after creating the dynamic autodiff cost
// function. For example:
//
-// DynamicAutoDiffCostFunction<MyCostFunctor, 3> cost_function(
-// new MyCostFunctor());
+// DynamicAutoDiffCostFunction<MyCostFunctor, 3> cost_function;
// cost_function.AddParameterBlock(5);
// cost_function.AddParameterBlock(10);
// cost_function.SetNumResiduals(21);
@@ -77,17 +77,38 @@
// pass. There is a tradeoff with the size of the passes; you may want
// to experiment with the stride.
template <typename CostFunctor, int Stride = 4>
-class DynamicAutoDiffCostFunction : public DynamicCostFunction {
+class DynamicAutoDiffCostFunction final : public DynamicCostFunction {
public:
+ // Constructs the CostFunctor on the heap and takes the ownership.
+ template <class... Args,
+ std::enable_if_t<std::is_constructible_v<CostFunctor, Args&&...>>* =
+ nullptr>
+ explicit DynamicAutoDiffCostFunction(Args&&... args)
+ // NOTE We explicitly use direct initialization using parentheses instead
+ // of uniform initialization using braces to avoid narrowing conversion
+ // warnings.
+ : DynamicAutoDiffCostFunction{
+ std::make_unique<CostFunctor>(std::forward<Args>(args)...)} {}
+
// Takes ownership by default.
- DynamicAutoDiffCostFunction(CostFunctor* functor,
- Ownership ownership = TAKE_OWNERSHIP)
- : functor_(functor), ownership_(ownership) {}
+ explicit DynamicAutoDiffCostFunction(CostFunctor* functor,
+ Ownership ownership = TAKE_OWNERSHIP)
+ : DynamicAutoDiffCostFunction{std::unique_ptr<CostFunctor>{functor},
+ ownership} {}
- explicit DynamicAutoDiffCostFunction(DynamicAutoDiffCostFunction&& other)
- : functor_(std::move(other.functor_)), ownership_(other.ownership_) {}
+ explicit DynamicAutoDiffCostFunction(std::unique_ptr<CostFunctor> functor)
+ : DynamicAutoDiffCostFunction{std::move(functor), TAKE_OWNERSHIP} {}
- virtual ~DynamicAutoDiffCostFunction() {
+ DynamicAutoDiffCostFunction(const DynamicAutoDiffCostFunction& other) =
+ delete;
+ DynamicAutoDiffCostFunction& operator=(
+ const DynamicAutoDiffCostFunction& other) = delete;
+ DynamicAutoDiffCostFunction(DynamicAutoDiffCostFunction&& other) noexcept =
+ default;
+ DynamicAutoDiffCostFunction& operator=(
+ DynamicAutoDiffCostFunction&& other) noexcept = default;
+
+ ~DynamicAutoDiffCostFunction() override {
// Manually release pointer if configured to not take ownership
// rather than deleting only if ownership is taken. This is to
// stay maximally compatible to old user code which may have
@@ -105,7 +126,7 @@
<< "You must call DynamicAutoDiffCostFunction::SetNumResiduals() "
<< "before DynamicAutoDiffCostFunction::Evaluate().";
- if (jacobians == NULL) {
+ if (jacobians == nullptr) {
return (*functor_)(parameters, residuals);
}
@@ -150,7 +171,7 @@
jet_parameters[i] = &input_jets[parameter_cursor];
const int parameter_block_size = parameter_block_sizes()[i];
- if (jacobians[i] != NULL) {
+ if (jacobians[i] != nullptr) {
if (!in_derivative_section) {
start_derivative_section.push_back(parameter_cursor);
in_derivative_section = true;
@@ -209,7 +230,7 @@
parameter_cursor >=
(start_derivative_section[current_derivative_section] +
current_derivative_section_cursor)) {
- if (jacobians[i] != NULL) {
+ if (jacobians[i] != nullptr) {
input_jets[parameter_cursor].v[active_parameter_count] = 1.0;
++active_parameter_count;
++current_derivative_section_cursor;
@@ -238,7 +259,7 @@
parameter_cursor >=
(start_derivative_section[current_derivative_section] +
current_derivative_section_cursor)) {
- if (jacobians[i] != NULL) {
+ if (jacobians[i] != nullptr) {
for (int k = 0; k < num_residuals(); ++k) {
jacobians[i][k * parameter_block_sizes()[i] + j] =
output_jets[k].v[active_parameter_count];
@@ -264,11 +285,34 @@
return true;
}
+ const CostFunctor& functor() const { return *functor_; }
+
private:
+ explicit DynamicAutoDiffCostFunction(std::unique_ptr<CostFunctor> functor,
+ Ownership ownership)
+ : functor_(std::move(functor)), ownership_(ownership) {}
+
std::unique_ptr<CostFunctor> functor_;
Ownership ownership_;
};
+// Deduction guide that allows the user to avoid explicitly specifying the
+// template parameter of DynamicAutoDiffCostFunction. The class can instead be
+// instantiated as follows:
+//
+// new DynamicAutoDiffCostFunction{new MyCostFunctor{}};
+// new DynamicAutoDiffCostFunction{std::make_unique<MyCostFunctor>()};
+//
+template <typename CostFunctor>
+DynamicAutoDiffCostFunction(CostFunctor* functor)
+ -> DynamicAutoDiffCostFunction<CostFunctor>;
+template <typename CostFunctor>
+DynamicAutoDiffCostFunction(CostFunctor* functor, Ownership ownership)
+ -> DynamicAutoDiffCostFunction<CostFunctor>;
+template <typename CostFunctor>
+DynamicAutoDiffCostFunction(std::unique_ptr<CostFunctor> functor)
+ -> DynamicAutoDiffCostFunction<CostFunctor>;
+
} // namespace ceres
#endif // CERES_PUBLIC_DYNAMIC_AUTODIFF_COST_FUNCTION_H_
diff --git a/include/ceres/dynamic_cost_function.h b/include/ceres/dynamic_cost_function.h
index 6e8a076..02ce1e9 100644
--- a/include/ceres/dynamic_cost_function.h
+++ b/include/ceres/dynamic_cost_function.h
@@ -1,5 +1,5 @@
// Ceres Solver - A fast non-linear least squares minimizer
-// Copyright 2019 Google Inc. All rights reserved.
+// Copyright 2023 Google Inc. All rights reserved.
// http://ceres-solver.org/
//
// Redistribution and use in source and binary forms, with or without
@@ -32,6 +32,7 @@
#define CERES_PUBLIC_DYNAMIC_COST_FUNCTION_H_
#include "ceres/cost_function.h"
+#include "ceres/internal/disable_warnings.h"
namespace ceres {
@@ -40,8 +41,6 @@
// parameter blocks and set the number of residuals at run time.
class CERES_EXPORT DynamicCostFunction : public CostFunction {
public:
- ~DynamicCostFunction() {}
-
virtual void AddParameterBlock(int size) {
mutable_parameter_block_sizes()->push_back(size);
}
@@ -53,4 +52,6 @@
} // namespace ceres
+#include "ceres/internal/reenable_warnings.h"
+
#endif // CERES_PUBLIC_DYNAMIC_COST_FUNCTION_H_
diff --git a/include/ceres/dynamic_cost_function_to_functor.h b/include/ceres/dynamic_cost_function_to_functor.h
index 8d174d8..45ed90f 100644
--- a/include/ceres/dynamic_cost_function_to_functor.h
+++ b/include/ceres/dynamic_cost_function_to_functor.h
@@ -1,5 +1,5 @@
// Ceres Solver - A fast non-linear least squares minimizer
-// Copyright 2019 Google Inc. All rights reserved.
+// Copyright 2023 Google Inc. All rights reserved.
// http://ceres-solver.org/
//
// Redistribution and use in source and binary forms, with or without
@@ -37,8 +37,10 @@
#include <vector>
#include "ceres/dynamic_cost_function.h"
+#include "ceres/internal/disable_warnings.h"
+#include "ceres/internal/export.h"
#include "ceres/internal/fixed_array.h"
-#include "ceres/internal/port.h"
+#include "glog/logging.h"
namespace ceres {
@@ -100,16 +102,22 @@
// private:
// DynamicCostFunctionToFunctor intrinsic_projection_;
// };
-class DynamicCostFunctionToFunctor {
+class CERES_EXPORT DynamicCostFunctionToFunctor {
public:
// Takes ownership of cost_function.
explicit DynamicCostFunctionToFunctor(CostFunction* cost_function)
- : cost_function_(cost_function) {
- CHECK(cost_function != nullptr);
+ : DynamicCostFunctionToFunctor{
+ std::unique_ptr<CostFunction>{cost_function}} {}
+
+ // Takes ownership of cost_function.
+ explicit DynamicCostFunctionToFunctor(
+ std::unique_ptr<CostFunction> cost_function)
+ : cost_function_(std::move(cost_function)) {
+ CHECK(cost_function_ != nullptr);
}
bool operator()(double const* const* parameters, double* residuals) const {
- return cost_function_->Evaluate(parameters, residuals, NULL);
+ return cost_function_->Evaluate(parameters, residuals, nullptr);
}
template <typename JetT>
@@ -181,10 +189,14 @@
return true;
}
+ CostFunction* function() const noexcept { return cost_function_.get(); }
+
private:
std::unique_ptr<CostFunction> cost_function_;
};
} // namespace ceres
+#include "ceres/internal/reenable_warnings.h"
+
#endif // CERES_PUBLIC_DYNAMIC_COST_FUNCTION_TO_FUNCTOR_H_
diff --git a/include/ceres/dynamic_numeric_diff_cost_function.h b/include/ceres/dynamic_numeric_diff_cost_function.h
index ccc8f66..1ce384f 100644
--- a/include/ceres/dynamic_numeric_diff_cost_function.h
+++ b/include/ceres/dynamic_numeric_diff_cost_function.h
@@ -1,5 +1,5 @@
// Ceres Solver - A fast non-linear least squares minimizer
-// Copyright 2019 Google Inc. All rights reserved.
+// Copyright 2024 Google Inc. All rights reserved.
// http://ceres-solver.org/
//
// Redistribution and use in source and binary forms, with or without
@@ -37,6 +37,7 @@
#include <cmath>
#include <memory>
#include <numeric>
+#include <type_traits>
#include <vector>
#include "ceres/dynamic_cost_function.h"
@@ -71,25 +72,47 @@
// also specify the sizes after creating the
// DynamicNumericDiffCostFunction. For example:
//
-// DynamicAutoDiffCostFunction<MyCostFunctor, CENTRAL> cost_function(
-// new MyCostFunctor());
+// DynamicAutoDiffCostFunction<MyCostFunctor, CENTRAL> cost_function;
// cost_function.AddParameterBlock(5);
// cost_function.AddParameterBlock(10);
// cost_function.SetNumResiduals(21);
-template <typename CostFunctor, NumericDiffMethodType method = CENTRAL>
-class DynamicNumericDiffCostFunction : public DynamicCostFunction {
+template <typename CostFunctor, NumericDiffMethodType kMethod = CENTRAL>
+class DynamicNumericDiffCostFunction final : public DynamicCostFunction {
public:
explicit DynamicNumericDiffCostFunction(
const CostFunctor* functor,
Ownership ownership = TAKE_OWNERSHIP,
const NumericDiffOptions& options = NumericDiffOptions())
- : functor_(functor), ownership_(ownership), options_(options) {}
+ : DynamicNumericDiffCostFunction{
+ std::unique_ptr<const CostFunctor>{functor}, ownership, options} {}
explicit DynamicNumericDiffCostFunction(
- DynamicNumericDiffCostFunction&& other)
- : functor_(std::move(other.functor_)), ownership_(other.ownership_) {}
+ std::unique_ptr<const CostFunctor> functor,
+ const NumericDiffOptions& options = NumericDiffOptions())
+ : DynamicNumericDiffCostFunction{
+ std::move(functor), TAKE_OWNERSHIP, options} {}
- virtual ~DynamicNumericDiffCostFunction() {
+ // Constructs the CostFunctor on the heap and takes the ownership.
+ template <class... Args,
+ std::enable_if_t<std::is_constructible_v<CostFunctor, Args&&...>>* =
+ nullptr>
+ explicit DynamicNumericDiffCostFunction(Args&&... args)
+ // NOTE We explicitly use direct initialization using parentheses instead
+ // of uniform initialization using braces to avoid narrowing conversion
+ // warnings.
+ : DynamicNumericDiffCostFunction{
+ std::make_unique<CostFunctor>(std::forward<Args>(args)...)} {}
+
+ DynamicNumericDiffCostFunction(const DynamicNumericDiffCostFunction&) =
+ delete;
+ DynamicNumericDiffCostFunction& operator=(
+ const DynamicNumericDiffCostFunction&) = delete;
+ DynamicNumericDiffCostFunction(
+ DynamicNumericDiffCostFunction&& other) noexcept = default;
+ DynamicNumericDiffCostFunction& operator=(
+ DynamicNumericDiffCostFunction&& other) noexcept = default;
+
+ ~DynamicNumericDiffCostFunction() override {
if (ownership_ != TAKE_OWNERSHIP) {
functor_.release();
}
@@ -111,7 +134,7 @@
const bool status =
internal::VariadicEvaluate<internal::DynamicParameterDims>(
*functor_.get(), parameters, residuals);
- if (jacobians == NULL || !status) {
+ if (jacobians == nullptr || !status) {
return status;
}
@@ -119,7 +142,7 @@
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] = ¶meters_copy[0];
+ parameters_references_copy[0] = parameters_copy.data();
for (size_t block = 1; block < block_sizes.size(); ++block) {
parameters_references_copy[block] =
parameters_references_copy[block - 1] + block_sizes[block - 1];
@@ -133,21 +156,22 @@
}
for (size_t block = 0; block < block_sizes.size(); ++block) {
- if (jacobians[block] != NULL &&
+ if (jacobians[block] != nullptr &&
!NumericDiff<CostFunctor,
- method,
+ kMethod,
ceres::DYNAMIC,
internal::DynamicParameterDims,
ceres::DYNAMIC,
ceres::DYNAMIC>::
- EvaluateJacobianForParameterBlock(functor_.get(),
- residuals,
- options_,
- this->num_residuals(),
- block,
- block_sizes[block],
- ¶meters_references_copy[0],
- jacobians[block])) {
+ EvaluateJacobianForParameterBlock(
+ functor_.get(),
+ residuals,
+ options_,
+ this->num_residuals(),
+ block,
+ block_sizes[block],
+ parameters_references_copy.data(),
+ jacobians[block])) {
return false;
}
}
@@ -155,11 +179,45 @@
}
private:
+ explicit DynamicNumericDiffCostFunction(
+ std::unique_ptr<const CostFunctor> functor,
+ Ownership ownership,
+ const NumericDiffOptions& options)
+ : functor_(std::move(functor)),
+ ownership_(ownership),
+ options_(options) {}
+
std::unique_ptr<const CostFunctor> functor_;
Ownership ownership_;
NumericDiffOptions options_;
};
+// Deduction guide that allows the user to avoid explicitly specifying the
+// template parameter of DynamicNumericDiffCostFunction. The class can instead
+// be instantiated as follows:
+//
+// new DynamicNumericDiffCostFunction{new MyCostFunctor{}};
+// new DynamicNumericDiffCostFunction{std::make_unique<MyCostFunctor>()};
+//
+template <typename CostFunctor>
+DynamicNumericDiffCostFunction(CostFunctor* functor)
+ -> DynamicNumericDiffCostFunction<CostFunctor>;
+template <typename CostFunctor>
+DynamicNumericDiffCostFunction(CostFunctor* functor, Ownership ownership)
+ -> DynamicNumericDiffCostFunction<CostFunctor>;
+template <typename CostFunctor>
+DynamicNumericDiffCostFunction(CostFunctor* functor,
+ Ownership ownership,
+ const NumericDiffOptions& options)
+ -> DynamicNumericDiffCostFunction<CostFunctor>;
+template <typename CostFunctor>
+DynamicNumericDiffCostFunction(std::unique_ptr<CostFunctor> functor)
+ -> DynamicNumericDiffCostFunction<CostFunctor>;
+template <typename CostFunctor>
+DynamicNumericDiffCostFunction(std::unique_ptr<CostFunctor> functor,
+ const NumericDiffOptions& options)
+ -> DynamicNumericDiffCostFunction<CostFunctor>;
+
} // namespace ceres
#endif // CERES_PUBLIC_DYNAMIC_AUTODIFF_COST_FUNCTION_H_
diff --git a/include/ceres/evaluation_callback.h b/include/ceres/evaluation_callback.h
index b9f5bbb..e582dc8 100644
--- a/include/ceres/evaluation_callback.h
+++ b/include/ceres/evaluation_callback.h
@@ -1,5 +1,5 @@
// Ceres Solver - A fast non-linear least squares minimizer
-// Copyright 2019 Google Inc. All rights reserved.
+// Copyright 2023 Google Inc. All rights reserved.
// http://ceres-solver.org/
//
// Redistribution and use in source and binary forms, with or without
@@ -31,7 +31,7 @@
#ifndef CERES_PUBLIC_EVALUATION_CALLBACK_H_
#define CERES_PUBLIC_EVALUATION_CALLBACK_H_
-#include "ceres/internal/port.h"
+#include "ceres/internal/export.h"
namespace ceres {
@@ -62,12 +62,16 @@
// execute faster.
class CERES_EXPORT EvaluationCallback {
public:
- virtual ~EvaluationCallback() {}
+ virtual ~EvaluationCallback();
// Called before Ceres requests residuals or jacobians for a given setting of
// the parameters. User parameters (the double* values provided to the cost
- // functions) are fixed until the next call to PrepareForEvaluation(). If
- // new_evaluation_point == true, then this is a new point that is different
+ // functions) are fixed until the next call to PrepareForEvaluation().
+ //
+ // If evaluate_jacobians == true, then the user provided CostFunctions will be
+ // asked to evaluate one or more of their Jacobians.
+ //
+ // If new_evaluation_point == true, then this is a new point that is different
// from the last evaluated point. Otherwise, it is the same point that was
// evaluated previously (either jacobian or residual) and the user can use
// cached results from previous evaluations.
diff --git a/include/ceres/first_order_function.h b/include/ceres/first_order_function.h
index 1420153..ea42732 100644
--- a/include/ceres/first_order_function.h
+++ b/include/ceres/first_order_function.h
@@ -1,5 +1,5 @@
// Ceres Solver - A fast non-linear least squares minimizer
-// Copyright 2019 Google Inc. All rights reserved.
+// Copyright 2023 Google Inc. All rights reserved.
// http://ceres-solver.org/
//
// Redistribution and use in source and binary forms, with or without
@@ -31,7 +31,7 @@
#ifndef CERES_PUBLIC_FIRST_ORDER_FUNCTION_H_
#define CERES_PUBLIC_FIRST_ORDER_FUNCTION_H_
-#include "ceres/internal/port.h"
+#include "ceres/internal/export.h"
namespace ceres {
@@ -39,7 +39,7 @@
// and its gradient.
class CERES_EXPORT FirstOrderFunction {
public:
- virtual ~FirstOrderFunction() {}
+ virtual ~FirstOrderFunction();
// cost is never null. gradient may be null. The return value
// indicates whether the evaluation was successful or not.
diff --git a/include/ceres/gradient_checker.h b/include/ceres/gradient_checker.h
index b79cf86..77f2c8e 100644
--- a/include/ceres/gradient_checker.h
+++ b/include/ceres/gradient_checker.h
@@ -1,5 +1,5 @@
// Ceres Solver - A fast non-linear least squares minimizer
-// Copyright 2019 Google Inc. All rights reserved.
+// Copyright 2023 Google Inc. All rights reserved.
// http://ceres-solver.org/
//
// Redistribution and use in source and binary forms, with or without
@@ -25,7 +25,7 @@
// 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.
+// Copyright 2023 Google Inc. All Rights Reserved.
//
// Authors: wjr@google.com (William Rucklidge),
// keir@google.com (Keir Mierle),
@@ -40,9 +40,11 @@
#include "ceres/cost_function.h"
#include "ceres/dynamic_numeric_diff_cost_function.h"
+#include "ceres/internal/disable_warnings.h"
#include "ceres/internal/eigen.h"
+#include "ceres/internal/export.h"
#include "ceres/internal/fixed_array.h"
-#include "ceres/local_parameterization.h"
+#include "ceres/manifold.h"
#include "glog/logging.h"
namespace ceres {
@@ -56,27 +58,27 @@
// ------------------------------------ < relative_precision
// max(J_actual(i, j), J_numeric(i, j))
//
-// where J_actual(i, j) is the jacobian as computed by the supplied cost
-// function (by the user) multiplied by the local parameterization Jacobian
-// and J_numeric is the jacobian as computed by finite differences, multiplied
-// by the local parameterization Jacobian as well.
+// where J_actual(i, j) is the Jacobian as computed by the supplied cost
+// function (by the user) multiplied by the manifold Jacobian and J_numeric is
+// the Jacobian as computed by finite differences, multiplied by the manifold
+// Jacobian as well.
//
// 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'.
class CERES_EXPORT GradientChecker {
public:
- // This will not take ownership of the cost function or local
- // parameterizations.
+ // This will not take ownership of the cost function or manifolds.
//
// function: The cost function to probe.
- // local_parameterizations: A vector of local parameterizations for each
- // parameter. May be NULL or contain NULL pointers to indicate that the
- // respective parameter does not have a local parameterization.
+ //
+ // manifolds: A vector of manifolds for each parameter. May be nullptr or
+ // contain nullptrs to indicate that the respective parameter blocks are
+ // Euclidean.
+ //
// options: Options to use for numerical differentiation.
- GradientChecker(
- const CostFunction* function,
- const std::vector<const LocalParameterization*>* local_parameterizations,
- const NumericDiffOptions& options);
+ GradientChecker(const CostFunction* function,
+ const std::vector<const Manifold*>* manifolds,
+ const NumericDiffOptions& options);
// Contains results from a call to Probe for later inspection.
struct CERES_EXPORT ProbeResults {
@@ -87,11 +89,11 @@
Vector residuals;
// The sizes of the Jacobians below are dictated by the cost function's
- // parameter block size and residual block sizes. If a parameter block
- // has a local parameterization associated with it, the size of the "local"
- // Jacobian will be determined by the local parameterization dimension and
- // residual block size, otherwise it will be identical to the regular
- // Jacobian.
+ // parameter block size and residual block sizes. If a parameter block has a
+ // manifold associated with it, the size of the "local" Jacobian will be
+ // determined by the dimension of the manifold (which is the same as the
+ // dimension of the tangent space) and residual block size, otherwise it
+ // will be identical to the regular Jacobian.
// Derivatives as computed by the cost function.
std::vector<Matrix> jacobians;
@@ -114,20 +116,20 @@
};
// Call the cost function, compute alternative Jacobians using finite
- // differencing and compare results. If local parameterizations are given,
- // the Jacobians will be multiplied by the local parameterization Jacobians
- // before performing the check, which effectively means that all errors along
- // the null space of the local parameterization will be ignored.
- // Returns false if the Jacobians don't match, the cost function return false,
- // or if the cost function returns different residual when called with a
- // Jacobian output argument vs. calling it without. Otherwise returns true.
+ // differencing and compare results. If manifolds are given, the Jacobians
+ // will be multiplied by the manifold Jacobians before performing the check,
+ // which effectively means that all errors along the null space of the
+ // manifold will be ignored. Returns false if the Jacobians don't match, the
+ // cost function return false, or if a cost function returns a different
+ // residual when called with a Jacobian output argument vs. calling it
+ // without. Otherwise returns true.
//
// parameters: The parameter values at which to probe.
// relative_precision: A threshold for the relative difference between the
// Jacobians. If the Jacobians differ by more than this amount, then the
// probe fails.
// results: On return, the Jacobians (and other information) will be stored
- // here. May be NULL.
+ // here. May be nullptr.
//
// Returns true if no problems are detected and the difference between the
// Jacobians is less than error_tolerance.
@@ -140,11 +142,13 @@
GradientChecker(const GradientChecker&) = delete;
void operator=(const GradientChecker&) = delete;
- std::vector<const LocalParameterization*> local_parameterizations_;
+ std::vector<const Manifold*> manifolds_;
const CostFunction* function_;
std::unique_ptr<CostFunction> finite_diff_cost_function_;
};
} // namespace ceres
+#include "ceres/internal/reenable_warnings.h"
+
#endif // CERES_PUBLIC_GRADIENT_CHECKER_H_
diff --git a/include/ceres/gradient_problem.h b/include/ceres/gradient_problem.h
index 49d605e..96d6493 100644
--- a/include/ceres/gradient_problem.h
+++ b/include/ceres/gradient_problem.h
@@ -1,5 +1,5 @@
// Ceres Solver - A fast non-linear least squares minimizer
-// Copyright 2019 Google Inc. All rights reserved.
+// Copyright 2023 Google Inc. All rights reserved.
// http://ceres-solver.org/
//
// Redistribution and use in source and binary forms, with or without
@@ -34,8 +34,9 @@
#include <memory>
#include "ceres/first_order_function.h"
-#include "ceres/internal/port.h"
-#include "ceres/local_parameterization.h"
+#include "ceres/internal/disable_warnings.h"
+#include "ceres/internal/export.h"
+#include "ceres/manifold.h"
namespace ceres {
@@ -43,23 +44,22 @@
// 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.
+// 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 are not restricted in the form
+// of the objective function.
//
-// Structurally GradientProblem is a composition of a
-// FirstOrderFunction and optionally a LocalParameterization.
+// Structurally GradientProblem is a composition of a FirstOrderFunction and
+// optionally a Manifold.
//
-// The FirstOrderFunction is responsible for evaluating the cost and
-// gradient of the objective function.
+// 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.
+// The Manifold is responsible for going back and forth between the ambient
+// space and the local tangent space. (See manifold.h for more details). When a
+// Manifold is not provided, then the tangent space is assumed to coincide with
+// the ambient Euclidean space that the gradient vector lives in.
//
// Example usage:
//
@@ -78,7 +78,7 @@
// const double y = parameters[1];
//
// cost[0] = (1.0 - x) * (1.0 - x) + 100.0 * (y - x * x) * (y - x * x);
-// if (gradient != NULL) {
+// if (gradient != nullptr) {
// gradient[0] = -2.0 * (1.0 - x) - 200.0 * (y - x * x) * 2.0 * x;
// gradient[1] = 200.0 * (y - x * x);
// }
@@ -94,23 +94,32 @@
// Takes ownership of the function.
explicit GradientProblem(FirstOrderFunction* function);
- // Takes ownership of the function and the parameterization.
- GradientProblem(FirstOrderFunction* function,
- LocalParameterization* parameterization);
+ // Takes ownership of the function and the manifold.
+ GradientProblem(FirstOrderFunction* function, Manifold* manifold);
int NumParameters() const;
- int NumLocalParameters() const;
+
+ // Dimension of the manifold (and its tangent space).
+ int NumTangentParameters() 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;
+ const FirstOrderFunction* function() const { return function_.get(); }
+ FirstOrderFunction* mutable_function() { return function_.get(); }
+
+ const Manifold* manifold() const { return manifold_.get(); }
+ Manifold* mutable_manifold() { return manifold_.get(); }
+
private:
std::unique_ptr<FirstOrderFunction> function_;
- std::unique_ptr<LocalParameterization> parameterization_;
+ std::unique_ptr<Manifold> manifold_;
std::unique_ptr<double[]> scratch_;
};
} // namespace ceres
+#include "ceres/internal/reenable_warnings.h"
+
#endif // CERES_PUBLIC_GRADIENT_PROBLEM_H_
diff --git a/include/ceres/gradient_problem_solver.h b/include/ceres/gradient_problem_solver.h
index 9fab62e..f4c392f 100644
--- a/include/ceres/gradient_problem_solver.h
+++ b/include/ceres/gradient_problem_solver.h
@@ -1,5 +1,5 @@
// Ceres Solver - A fast non-linear least squares minimizer
-// Copyright 2019 Google Inc. All rights reserved.
+// Copyright 2023 Google Inc. All rights reserved.
// http://ceres-solver.org/
//
// Redistribution and use in source and binary forms, with or without
@@ -36,6 +36,7 @@
#include <vector>
#include "ceres/internal/disable_warnings.h"
+#include "ceres/internal/export.h"
#include "ceres/internal/port.h"
#include "ceres/iteration_callback.h"
#include "ceres/types.h"
@@ -305,7 +306,7 @@
int num_parameters = -1;
// Dimension of the tangent space of the problem.
- int num_local_parameters = -1;
+ int num_tangent_parameters = -1;
// Type of line search direction used.
LineSearchDirectionType line_search_direction_type = LBFGS;
diff --git a/include/ceres/internal/array_selector.h b/include/ceres/internal/array_selector.h
index 841797f..9480146 100644
--- a/include/ceres/internal/array_selector.h
+++ b/include/ceres/internal/array_selector.h
@@ -1,5 +1,5 @@
// Ceres Solver - A fast non-linear least squares minimizer
-// Copyright 2020 Google Inc. All rights reserved.
+// Copyright 2023 Google Inc. All rights reserved.
// http://ceres-solver.org/
//
// Redistribution and use in source and binary forms, with or without
@@ -38,8 +38,7 @@
#include "ceres/internal/fixed_array.h"
#include "ceres/types.h"
-namespace ceres {
-namespace internal {
+namespace ceres::internal {
// StaticFixedArray selects the best array implementation based on template
// arguments. If the size is not known at compile-time, pass
@@ -73,23 +72,24 @@
true,
fits_on_stack>
: ceres::internal::FixedArray<T, max_num_elements_on_stack> {
- ArraySelector(int s)
+ explicit ArraySelector(int s)
: ceres::internal::FixedArray<T, max_num_elements_on_stack>(s) {}
};
template <typename T, int num_elements, int max_num_elements_on_stack>
struct ArraySelector<T, num_elements, max_num_elements_on_stack, false, true>
: std::array<T, num_elements> {
- ArraySelector(int s) { CHECK_EQ(s, num_elements); }
+ explicit ArraySelector(int s) { CHECK_EQ(s, num_elements); }
};
template <typename T, int num_elements, int max_num_elements_on_stack>
struct ArraySelector<T, num_elements, max_num_elements_on_stack, false, false>
: std::vector<T> {
- ArraySelector(int s) : std::vector<T>(s) { CHECK_EQ(s, num_elements); }
+ explicit ArraySelector(int s) : std::vector<T>(s) {
+ CHECK_EQ(s, num_elements);
+ }
};
-} // namespace internal
-} // namespace ceres
+} // namespace ceres::internal
#endif // CERES_PUBLIC_INTERNAL_ARRAY_SELECTOR_H_
diff --git a/include/ceres/internal/autodiff.h b/include/ceres/internal/autodiff.h
index 9d7de75..8b02a2b 100644
--- a/include/ceres/internal/autodiff.h
+++ b/include/ceres/internal/autodiff.h
@@ -1,5 +1,5 @@
// Ceres Solver - A fast non-linear least squares minimizer
-// Copyright 2019 Google Inc. All rights reserved.
+// Copyright 2023 Google Inc. All rights reserved.
// http://ceres-solver.org/
//
// Redistribution and use in source and binary forms, with or without
@@ -132,17 +132,16 @@
// 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.
+// Jacobian null pointers (nullptr)
+// --------------------------------
+// In general, the functions below will accept nullptr 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 <array>
+#include <cstddef>
#include <utility>
#include "ceres/internal/array_selector.h"
@@ -165,8 +164,7 @@
#define CERES_AUTODIFF_MAX_RESIDUALS_ON_STACK 20
#endif
-namespace ceres {
-namespace internal {
+namespace ceres::internal {
// Extends src by a 1st order perturbation for every dimension and puts it in
// dst. The size of src is N. Since this is also used for perturbations in
@@ -198,7 +196,7 @@
template <int N, int Offset, typename T, typename JetT>
struct Make1stOrderPerturbation<N, N, Offset, T, JetT> {
public:
- static void Apply(const T* src, JetT* dst) {}
+ static void Apply(const T* /* NOT USED */, JetT* /* NOT USED */) {}
};
// Calls Make1stOrderPerturbation for every parameter block.
@@ -311,7 +309,7 @@
int dynamic_num_outputs,
T* function_value,
T** jacobians) {
- typedef Jet<T, ParameterDims::kNumParameters> JetT;
+ using JetT = Jet<T, ParameterDims::kNumParameters>;
using Parameters = typename ParameterDims::Parameters;
if (kNumResiduals != DYNAMIC) {
@@ -360,7 +358,6 @@
return true;
}
-} // namespace internal
-} // namespace ceres
+} // namespace ceres::internal
#endif // CERES_PUBLIC_INTERNAL_AUTODIFF_H_
diff --git a/include/ceres/internal/disable_warnings.h b/include/ceres/internal/disable_warnings.h
index d7766a0..b6e38aa 100644
--- a/include/ceres/internal/disable_warnings.h
+++ b/include/ceres/internal/disable_warnings.h
@@ -1,5 +1,5 @@
// Ceres Solver - A fast non-linear least squares minimizer
-// Copyright 2015 Google Inc. All rights reserved.
+// Copyright 2023 Google Inc. All rights reserved.
// http://ceres-solver.org/
//
// Redistribution and use in source and binary forms, with or without
diff --git a/include/ceres/internal/eigen.h b/include/ceres/internal/eigen.h
index b6d0b7f..fee6b52 100644
--- a/include/ceres/internal/eigen.h
+++ b/include/ceres/internal/eigen.h
@@ -1,5 +1,5 @@
// Ceres Solver - A fast non-linear least squares minimizer
-// Copyright 2015 Google Inc. All rights reserved.
+// Copyright 2023 Google Inc. All rights reserved.
// http://ceres-solver.org/
//
// Redistribution and use in source and binary forms, with or without
@@ -35,39 +35,39 @@
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;
+using Vector = Eigen::Matrix<double, Eigen::Dynamic, 1>;
+using Matrix =
+ Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>;
+using VectorRef = Eigen::Map<Vector>;
+using MatrixRef = Eigen::Map<Matrix>;
+using ConstVectorRef = Eigen::Map<const Vector>;
+using ConstMatrixRef = Eigen::Map<const Matrix>;
// Column major matrices for DenseSparseMatrix/DenseQRSolver
-typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor>
- ColMajorMatrix;
+using ColMajorMatrix =
+ Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor>;
-typedef Eigen::Map<ColMajorMatrix, 0, Eigen::Stride<Eigen::Dynamic, 1>>
- ColMajorMatrixRef;
+using ColMajorMatrixRef =
+ Eigen::Map<ColMajorMatrix, 0, Eigen::Stride<Eigen::Dynamic, 1>>;
-typedef Eigen::Map<const ColMajorMatrix, 0, Eigen::Stride<Eigen::Dynamic, 1>>
- ConstColMajorMatrixRef;
+using ConstColMajorMatrixRef =
+ Eigen::Map<const ColMajorMatrix, 0, Eigen::Stride<Eigen::Dynamic, 1>>;
// 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,
- num_cols == 1 ? Eigen::ColMajor : Eigen::RowMajor>
- Matrix;
+ using Matrix =
+ Eigen::Matrix<double,
+ num_rows,
+ num_cols,
+ num_cols == 1 ? Eigen::ColMajor : Eigen::RowMajor>;
- typedef Eigen::Map<Matrix> MatrixRef;
- typedef Eigen::Map<const Matrix> ConstMatrixRef;
- 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, 1>> ConstVectorRef;
+ using MatrixRef = Eigen::Map<Matrix>;
+ using ConstMatrixRef = Eigen::Map<const Matrix>;
+ using Vector = Eigen::Matrix<double, num_rows, 1>;
+ using VectorRef = Eigen::Map<Eigen::Matrix<double, num_rows, 1>>;
+ using ConstVectorRef = Eigen::Map<const Eigen::Matrix<double, num_rows, 1>>;
};
} // namespace ceres
diff --git a/include/ceres/internal/euler_angles.h b/include/ceres/internal/euler_angles.h
new file mode 100644
index 0000000..38f2702
--- /dev/null
+++ b/include/ceres/internal/euler_angles.h
@@ -0,0 +1,199 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2023 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.
+
+#ifndef CERES_PUBLIC_INTERNAL_EULER_ANGLES_H_
+#define CERES_PUBLIC_INTERNAL_EULER_ANGLES_H_
+
+#include <type_traits>
+
+namespace ceres {
+namespace internal {
+
+// The EulerSystem struct represents an Euler Angle Convention in compile time.
+// It acts like a trait structure and is also used as a tag for dispatching
+// Euler angle conversion function templates
+//
+// Internally, it implements the convention laid out in "Euler angle
+// conversion", Ken Shoemake, Graphics Gems IV, where a choice of axis for the
+// first rotation (out of 3) and 3 binary choices compactly specify all 24
+// rotation conventions
+//
+// - InnerAxis: Axis for the first rotation. This is specified by struct tags
+// axis::X, axis::Y, and axis::Z
+//
+// - Parity: Defines the parity of the axis permutation. The axis sequence has
+// Even parity if the second axis of rotation is 'greater-than' the first axis
+// of rotation according to the order X<Y<Z<X, otherwise it has Odd parity.
+// This is specified by struct tags Even and Odd
+//
+// - AngleConvention: Defines whether Proper Euler Angles (originally defined
+// by Euler, which has the last axis repeated, i.e. ZYZ, ZXZ, etc), or
+// Tait-Bryan Angles (introduced by the nautical and aerospace fields, i.e.
+// using ZYX for roll-pitch-yaw) are used. This is specified by struct Tags
+// ProperEuler and TaitBryan.
+//
+// - FrameConvention: Defines whether the three rotations are be in a global
+// frame of reference (extrinsic) or in a body centred frame of reference
+// (intrinsic). This is specified by struct tags Extrinsic and Intrinsic
+
+namespace axis {
+struct X : std::integral_constant<int, 0> {};
+struct Y : std::integral_constant<int, 1> {};
+struct Z : std::integral_constant<int, 2> {};
+} // namespace axis
+
+struct Even;
+struct Odd;
+
+struct ProperEuler;
+struct TaitBryan;
+
+struct Extrinsic;
+struct Intrinsic;
+
+template <typename InnerAxisType,
+ typename ParityType,
+ typename AngleConventionType,
+ typename FrameConventionType>
+struct EulerSystem {
+ static constexpr bool kIsParityOdd = std::is_same_v<ParityType, Odd>;
+ static constexpr bool kIsProperEuler =
+ std::is_same_v<AngleConventionType, ProperEuler>;
+ static constexpr bool kIsIntrinsic =
+ std::is_same_v<FrameConventionType, Intrinsic>;
+
+ static constexpr int kAxes[3] = {
+ InnerAxisType::value,
+ (InnerAxisType::value + 1 + static_cast<int>(kIsParityOdd)) % 3,
+ (InnerAxisType::value + 2 - static_cast<int>(kIsParityOdd)) % 3};
+};
+
+} // namespace internal
+
+// Define human readable aliases to the type of the tags
+using ExtrinsicXYZ = internal::EulerSystem<internal::axis::X,
+ internal::Even,
+ internal::TaitBryan,
+ internal::Extrinsic>;
+using ExtrinsicXYX = internal::EulerSystem<internal::axis::X,
+ internal::Even,
+ internal::ProperEuler,
+ internal::Extrinsic>;
+using ExtrinsicXZY = internal::EulerSystem<internal::axis::X,
+ internal::Odd,
+ internal::TaitBryan,
+ internal::Extrinsic>;
+using ExtrinsicXZX = internal::EulerSystem<internal::axis::X,
+ internal::Odd,
+ internal::ProperEuler,
+ internal::Extrinsic>;
+using ExtrinsicYZX = internal::EulerSystem<internal::axis::Y,
+ internal::Even,
+ internal::TaitBryan,
+ internal::Extrinsic>;
+using ExtrinsicYZY = internal::EulerSystem<internal::axis::Y,
+ internal::Even,
+ internal::ProperEuler,
+ internal::Extrinsic>;
+using ExtrinsicYXZ = internal::EulerSystem<internal::axis::Y,
+ internal::Odd,
+ internal::TaitBryan,
+ internal::Extrinsic>;
+using ExtrinsicYXY = internal::EulerSystem<internal::axis::Y,
+ internal::Odd,
+ internal::ProperEuler,
+ internal::Extrinsic>;
+using ExtrinsicZXY = internal::EulerSystem<internal::axis::Z,
+ internal::Even,
+ internal::TaitBryan,
+ internal::Extrinsic>;
+using ExtrinsicZXZ = internal::EulerSystem<internal::axis::Z,
+ internal::Even,
+ internal::ProperEuler,
+ internal::Extrinsic>;
+using ExtrinsicZYX = internal::EulerSystem<internal::axis::Z,
+ internal::Odd,
+ internal::TaitBryan,
+ internal::Extrinsic>;
+using ExtrinsicZYZ = internal::EulerSystem<internal::axis::Z,
+ internal::Odd,
+ internal::ProperEuler,
+ internal::Extrinsic>;
+/* Rotating axes */
+using IntrinsicZYX = internal::EulerSystem<internal::axis::X,
+ internal::Even,
+ internal::TaitBryan,
+ internal::Intrinsic>;
+using IntrinsicXYX = internal::EulerSystem<internal::axis::X,
+ internal::Even,
+ internal::ProperEuler,
+ internal::Intrinsic>;
+using IntrinsicYZX = internal::EulerSystem<internal::axis::X,
+ internal::Odd,
+ internal::TaitBryan,
+ internal::Intrinsic>;
+using IntrinsicXZX = internal::EulerSystem<internal::axis::X,
+ internal::Odd,
+ internal::ProperEuler,
+ internal::Intrinsic>;
+using IntrinsicXZY = internal::EulerSystem<internal::axis::Y,
+ internal::Even,
+ internal::TaitBryan,
+ internal::Intrinsic>;
+using IntrinsicYZY = internal::EulerSystem<internal::axis::Y,
+ internal::Even,
+ internal::ProperEuler,
+ internal::Intrinsic>;
+using IntrinsicZXY = internal::EulerSystem<internal::axis::Y,
+ internal::Odd,
+ internal::TaitBryan,
+ internal::Intrinsic>;
+using IntrinsicYXY = internal::EulerSystem<internal::axis::Y,
+ internal::Odd,
+ internal::ProperEuler,
+ internal::Intrinsic>;
+using IntrinsicYXZ = internal::EulerSystem<internal::axis::Z,
+ internal::Even,
+ internal::TaitBryan,
+ internal::Intrinsic>;
+using IntrinsicZXZ = internal::EulerSystem<internal::axis::Z,
+ internal::Even,
+ internal::ProperEuler,
+ internal::Intrinsic>;
+using IntrinsicXYZ = internal::EulerSystem<internal::axis::Z,
+ internal::Odd,
+ internal::TaitBryan,
+ internal::Intrinsic>;
+using IntrinsicZYZ = internal::EulerSystem<internal::axis::Z,
+ internal::Odd,
+ internal::ProperEuler,
+ internal::Intrinsic>;
+
+} // namespace ceres
+
+#endif // CERES_PUBLIC_INTERNAL_EULER_ANGLES_H_
diff --git a/include/ceres/internal/fixed_array.h b/include/ceres/internal/fixed_array.h
index dcbddcd..0e35f63 100644
--- a/include/ceres/internal/fixed_array.h
+++ b/include/ceres/internal/fixed_array.h
@@ -41,8 +41,7 @@
#include "ceres/internal/memory.h"
#include "glog/logging.h"
-namespace ceres {
-namespace internal {
+namespace ceres::internal {
constexpr static auto kFixedArrayUseDefault = static_cast<size_t>(-1);
@@ -372,8 +371,8 @@
return std::addressof(ptr->array);
}
- static_assert(sizeof(StorageElement) == sizeof(value_type), "");
- static_assert(alignof(StorageElement) == alignof(value_type), "");
+ static_assert(sizeof(StorageElement) == sizeof(value_type));
+ static_assert(alignof(StorageElement) == alignof(value_type));
class NonEmptyInlinedStorage {
public:
@@ -461,7 +460,6 @@
constexpr typename FixedArray<T, N, A>::size_type
FixedArray<T, N, A>::inline_elements;
-} // namespace internal
-} // namespace ceres
+} // namespace ceres::internal
#endif // CERES_PUBLIC_INTERNAL_FIXED_ARRAY_H_
diff --git a/include/ceres/internal/householder_vector.h b/include/ceres/internal/householder_vector.h
index 55f68e5..dd8361c 100644
--- a/include/ceres/internal/householder_vector.h
+++ b/include/ceres/internal/householder_vector.h
@@ -1,5 +1,5 @@
// Ceres Solver - A fast non-linear least squares minimizer
-// Copyright 2015 Google Inc. All rights reserved.
+// Copyright 2023 Google Inc. All rights reserved.
// http://code.google.com/p/ceres-solver/
//
// Redistribution and use in source and binary forms, with or without
@@ -34,8 +34,7 @@
#include "Eigen/Core"
#include "glog/logging.h"
-namespace ceres {
-namespace internal {
+namespace ceres::internal {
// Algorithm 5.1.1 from 'Matrix Computations' by Golub et al. (Johns Hopkins
// Studies in Mathematical Sciences) but using the nth element of the input
@@ -82,7 +81,14 @@
v->head(v->rows() - 1) /= v_pivot;
}
-} // namespace internal
-} // namespace ceres
+template <typename XVectorType, typename Derived>
+typename Derived::PlainObject ApplyHouseholderVector(
+ const XVectorType& y,
+ const Eigen::MatrixBase<Derived>& v,
+ const typename Derived::Scalar& beta) {
+ return (y - v * (beta * (v.transpose() * y)));
+}
+
+} // namespace ceres::internal
#endif // CERES_PUBLIC_INTERNAL_HOUSEHOLDER_VECTOR_H_
diff --git a/include/ceres/internal/integer_sequence_algorithm.h b/include/ceres/internal/integer_sequence_algorithm.h
index 8c0f3bc..0c27d72 100644
--- a/include/ceres/internal/integer_sequence_algorithm.h
+++ b/include/ceres/internal/integer_sequence_algorithm.h
@@ -1,5 +1,5 @@
// Ceres Solver - A fast non-linear least squares minimizer
-// Copyright 2018 Google Inc. All rights reserved.
+// Copyright 2023 Google Inc. All rights reserved.
// http://ceres-solver.org/
//
// Redistribution and use in source and binary forms, with or without
@@ -27,6 +27,7 @@
// POSSIBILITY OF SUCH DAMAGE.
//
// Author: jodebo_beck@gmx.de (Johannes Beck)
+// sergiu.deitsch@gmail.com (Sergiu Deitsch)
//
// Algorithms to be used together with integer_sequence, like computing the sum
// or the exclusive scan (sometimes called exclusive prefix sum) at compile
@@ -37,70 +38,9 @@
#include <utility>
-namespace ceres {
-namespace internal {
+#include "ceres/jet_fwd.h"
-// Implementation of calculating the sum of an integer sequence.
-// Recursively instantiate SumImpl and calculate the sum of the N first
-// numbers. This reduces the number of instantiations and speeds up
-// compilation.
-//
-// Examples:
-// 1) integer_sequence<int, 5>:
-// Value = 5
-//
-// 2) integer_sequence<int, 4, 2>:
-// Value = 4 + 2 + SumImpl<integer_sequence<int>>::Value
-// Value = 4 + 2 + 0
-//
-// 3) integer_sequence<int, 2, 1, 4>:
-// Value = 2 + 1 + SumImpl<integer_sequence<int, 4>>::Value
-// Value = 2 + 1 + 4
-template <typename Seq>
-struct SumImpl;
-
-// Strip of and sum the first number.
-template <typename T, T N, T... Ns>
-struct SumImpl<std::integer_sequence<T, N, Ns...>> {
- static constexpr T Value =
- N + SumImpl<std::integer_sequence<T, Ns...>>::Value;
-};
-
-// Strip of and sum the first two numbers.
-template <typename T, T N1, T N2, T... Ns>
-struct SumImpl<std::integer_sequence<T, N1, N2, Ns...>> {
- static constexpr T Value =
- N1 + N2 + SumImpl<std::integer_sequence<T, Ns...>>::Value;
-};
-
-// Strip of and sum the first four numbers.
-template <typename T, T N1, T N2, T N3, T N4, T... Ns>
-struct SumImpl<std::integer_sequence<T, N1, N2, N3, N4, Ns...>> {
- static constexpr T Value =
- N1 + N2 + N3 + N4 + SumImpl<std::integer_sequence<T, Ns...>>::Value;
-};
-
-// Only one number is left. 'Value' is just that number ('recursion' ends).
-template <typename T, T N>
-struct SumImpl<std::integer_sequence<T, N>> {
- static constexpr T Value = N;
-};
-
-// No number is left. 'Value' is the identity element (for sum this is zero).
-template <typename T>
-struct SumImpl<std::integer_sequence<T>> {
- static constexpr T Value = T(0);
-};
-
-// Calculate the sum of an integer sequence. The resulting sum will be stored in
-// 'Value'.
-template <typename Seq>
-class Sum {
- using T = typename Seq::value_type;
-
- public:
- static constexpr T Value = SumImpl<Seq>::Value;
-};
+namespace ceres::internal {
// Implementation of calculating an exclusive scan (exclusive prefix sum) of an
// integer sequence. Exclusive means that the i-th input element is not included
@@ -164,7 +104,96 @@
template <typename Seq>
using ExclusiveScan = typename ExclusiveScanT<Seq>::Type;
-} // namespace internal
-} // namespace ceres
+// Removes all elements from a integer sequence corresponding to specified
+// ValueToRemove.
+//
+// This type should not be used directly but instead RemoveValue.
+template <typename T, T ValueToRemove, typename... Sequence>
+struct RemoveValueImpl;
+
+// Final filtered sequence
+template <typename T, T ValueToRemove, T... Values>
+struct RemoveValueImpl<T,
+ ValueToRemove,
+ std::integer_sequence<T, Values...>,
+ std::integer_sequence<T>> {
+ using type = std::integer_sequence<T, Values...>;
+};
+
+// Found a matching value
+template <typename T, T ValueToRemove, T... Head, T... Tail>
+struct RemoveValueImpl<T,
+ ValueToRemove,
+ std::integer_sequence<T, Head...>,
+ std::integer_sequence<T, ValueToRemove, Tail...>>
+ : RemoveValueImpl<T,
+ ValueToRemove,
+ std::integer_sequence<T, Head...>,
+ std::integer_sequence<T, Tail...>> {};
+
+// Move one element from the tail to the head
+template <typename T, T ValueToRemove, T... Head, T MiddleValue, T... Tail>
+struct RemoveValueImpl<T,
+ ValueToRemove,
+ std::integer_sequence<T, Head...>,
+ std::integer_sequence<T, MiddleValue, Tail...>>
+ : RemoveValueImpl<T,
+ ValueToRemove,
+ std::integer_sequence<T, Head..., MiddleValue>,
+ std::integer_sequence<T, Tail...>> {};
+
+// Start recursion by splitting the integer sequence into two separate ones
+template <typename T, T ValueToRemove, T... Tail>
+struct RemoveValueImpl<T, ValueToRemove, std::integer_sequence<T, Tail...>>
+ : RemoveValueImpl<T,
+ ValueToRemove,
+ std::integer_sequence<T>,
+ std::integer_sequence<T, Tail...>> {};
+
+// RemoveValue takes an integer Sequence of arbitrary type and removes all
+// elements matching ValueToRemove.
+//
+// In contrast to RemoveValueImpl, this implementation deduces the value type
+// eliminating the need to specify it explicitly.
+//
+// As an example, RemoveValue<std::integer_sequence<int, 1, 2, 3>, 4>::type will
+// not transform the type of the original sequence. However,
+// RemoveValue<std::integer_sequence<int, 0, 0, 2>, 2>::type will generate a new
+// sequence of type std::integer_sequence<int, 0, 0> by removing the value 2.
+template <typename Sequence, typename Sequence::value_type ValueToRemove>
+struct RemoveValue
+ : RemoveValueImpl<typename Sequence::value_type, ValueToRemove, Sequence> {
+};
+
+// Convenience template alias for RemoveValue.
+template <typename Sequence, typename Sequence::value_type ValueToRemove>
+using RemoveValue_t = typename RemoveValue<Sequence, ValueToRemove>::type;
+
+// Returns true if all elements of Values are equal to HeadValue.
+//
+// Returns true if Values is empty.
+template <typename T, T HeadValue, T... Values>
+inline constexpr bool AreAllEqual_v = ((HeadValue == Values) && ...);
+
+// Predicate determining whether an integer sequence is either empty or all
+// values are equal.
+template <typename Sequence>
+struct IsEmptyOrAreAllEqual;
+
+// Empty case.
+template <typename T>
+struct IsEmptyOrAreAllEqual<std::integer_sequence<T>> : std::true_type {};
+
+// General case for sequences containing at least one value.
+template <typename T, T HeadValue, T... Values>
+struct IsEmptyOrAreAllEqual<std::integer_sequence<T, HeadValue, Values...>>
+ : std::integral_constant<bool, AreAllEqual_v<T, HeadValue, Values...>> {};
+
+// Convenience variable template for IsEmptyOrAreAllEqual.
+template <class Sequence>
+inline constexpr bool IsEmptyOrAreAllEqual_v =
+ IsEmptyOrAreAllEqual<Sequence>::value;
+
+} // namespace ceres::internal
#endif // CERES_PUBLIC_INTERNAL_INTEGER_SEQUENCE_ALGORITHM_H_
diff --git a/include/ceres/internal/jet_traits.h b/include/ceres/internal/jet_traits.h
new file mode 100644
index 0000000..f504a61
--- /dev/null
+++ b/include/ceres/internal/jet_traits.h
@@ -0,0 +1,195 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2023 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: sergiu.deitsch@gmail.com (Sergiu Deitsch)
+//
+
+#ifndef CERES_PUBLIC_INTERNAL_JET_TRAITS_H_
+#define CERES_PUBLIC_INTERNAL_JET_TRAITS_H_
+
+#include <tuple>
+#include <type_traits>
+#include <utility>
+
+#include "ceres/internal/integer_sequence_algorithm.h"
+#include "ceres/jet_fwd.h"
+
+namespace ceres {
+namespace internal {
+
+// Predicate that determines whether any of the Types is a Jet.
+template <typename... Types>
+struct AreAnyJet : std::false_type {};
+
+template <typename T, typename... Types>
+struct AreAnyJet<T, Types...> : AreAnyJet<Types...> {};
+
+template <typename T, int N, typename... Types>
+struct AreAnyJet<Jet<T, N>, Types...> : std::true_type {};
+
+// Convenience variable template for AreAnyJet.
+template <typename... Types>
+inline constexpr bool AreAnyJet_v = AreAnyJet<Types...>::value;
+
+// Extracts the underlying floating-point from a type T.
+template <typename T, typename E = void>
+struct UnderlyingScalar {
+ using type = T;
+};
+
+template <typename T, int N>
+struct UnderlyingScalar<Jet<T, N>> : UnderlyingScalar<T> {};
+
+// Convenience template alias for UnderlyingScalar type trait.
+template <typename T>
+using UnderlyingScalar_t = typename UnderlyingScalar<T>::type;
+
+// Predicate determining whether all Types in the pack are the same.
+//
+// Specifically, the predicate applies std::is_same recursively to pairs of
+// Types in the pack.
+template <typename T1, typename... Types>
+inline constexpr bool AreAllSame_v = (std::is_same<T1, Types>::value && ...);
+
+// Determines the rank of a type. This allows to ensure that types passed as
+// arguments are compatible to each other. The rank of Jet is determined by the
+// dimensions of the dual part. The rank of a scalar is always 0.
+// Non-specialized types default to a rank of -1.
+template <typename T, typename E = void>
+struct Rank : std::integral_constant<int, -1> {};
+
+// The rank of a scalar is 0.
+template <typename T>
+struct Rank<T, std::enable_if_t<std::is_scalar<T>::value>>
+ : std::integral_constant<int, 0> {};
+
+// The rank of a Jet is given by its dimensionality.
+template <typename T, int N>
+struct Rank<Jet<T, N>> : std::integral_constant<int, N> {};
+
+// Convenience variable template for Rank.
+template <typename T>
+inline constexpr int Rank_v = Rank<T>::value;
+
+// Constructs an integer sequence of ranks for each of the Types in the pack.
+template <typename... Types>
+using Ranks_t = std::integer_sequence<int, Rank_v<Types>...>;
+
+// Returns the scalar part of a type. This overload acts as an identity.
+template <typename T>
+constexpr decltype(auto) AsScalar(T&& value) noexcept {
+ return std::forward<T>(value);
+}
+
+// Recursively unwraps the scalar part of a Jet until a non-Jet scalar type is
+// encountered.
+template <typename T, int N>
+constexpr decltype(auto) AsScalar(const Jet<T, N>& value) noexcept(
+ noexcept(AsScalar(value.a))) {
+ return AsScalar(value.a);
+}
+
+} // namespace internal
+
+// Type trait ensuring at least one of the types is a Jet,
+// the underlying scalar types are the same and Jet dimensions match.
+//
+// The type trait can be further specialized if necessary.
+//
+// This trait is a candidate for a concept definition once C++20 features can
+// be used.
+template <typename... Types>
+// clang-format off
+struct CompatibleJetOperands : std::integral_constant
+<
+ bool,
+ // At least one of the types is a Jet
+ internal::AreAnyJet_v<Types...> &&
+ // The underlying floating-point types are exactly the same
+ internal::AreAllSame_v<internal::UnderlyingScalar_t<Types>...> &&
+ // Non-zero ranks of types are equal
+ internal::IsEmptyOrAreAllEqual_v<internal::RemoveValue_t<internal::Ranks_t<Types...>, 0>>
+>
+// clang-format on
+{};
+
+// Single Jet operand is always compatible.
+template <typename T, int N>
+struct CompatibleJetOperands<Jet<T, N>> : std::true_type {};
+
+// Single non-Jet operand is always incompatible.
+template <typename T>
+struct CompatibleJetOperands<T> : std::false_type {};
+
+// Empty operands are always incompatible.
+template <>
+struct CompatibleJetOperands<> : std::false_type {};
+
+// Convenience variable template ensuring at least one of the types is a Jet,
+// the underlying scalar types are the same and Jet dimensions match.
+//
+// This trait is a candidate for a concept definition once C++20 features can
+// be used.
+template <typename... Types>
+inline constexpr bool CompatibleJetOperands_v =
+ CompatibleJetOperands<Types...>::value;
+
+// Type trait ensuring at least one of the types is a Jet,
+// the underlying scalar types are compatible among each other and Jet
+// dimensions match.
+//
+// The type trait can be further specialized if necessary.
+//
+// This trait is a candidate for a concept definition once C++20 features can
+// be used.
+template <typename... Types>
+// clang-format off
+struct PromotableJetOperands : std::integral_constant
+<
+ bool,
+ // Types can be compatible among each other
+ internal::AreAnyJet_v<Types...> &&
+ // Non-zero ranks of types are equal
+ internal::IsEmptyOrAreAllEqual_v<internal::RemoveValue_t<internal::Ranks_t<Types...>, 0>>
+>
+// clang-format on
+{};
+
+// Convenience variable template ensuring at least one of the types is a Jet,
+// the underlying scalar types are compatible among each other and Jet
+// dimensions match.
+//
+// This trait is a candidate for a concept definition once C++20 features can
+// be used.
+template <typename... Types>
+inline constexpr bool PromotableJetOperands_v =
+ PromotableJetOperands<Types...>::value;
+
+} // namespace ceres
+
+#endif // CERES_PUBLIC_INTERNAL_JET_TRAITS_H_
diff --git a/include/ceres/internal/line_parameterization.h b/include/ceres/internal/line_parameterization.h
index eda3901..f50603d 100644
--- a/include/ceres/internal/line_parameterization.h
+++ b/include/ceres/internal/line_parameterization.h
@@ -1,5 +1,5 @@
// Ceres Solver - A fast non-linear least squares minimizer
-// Copyright 2020 Google Inc. All rights reserved.
+// Copyright 2023 Google Inc. All rights reserved.
// http://ceres-solver.org/
//
// Redistribution and use in source and binary forms, with or without
diff --git a/include/ceres/internal/memory.h b/include/ceres/internal/memory.h
index 45c5b67..e54cf2b 100644
--- a/include/ceres/internal/memory.h
+++ b/include/ceres/internal/memory.h
@@ -40,8 +40,7 @@
} while (false)
#endif // CERES_HAVE_EXCEPTIONS
-namespace ceres {
-namespace internal {
+namespace ceres::internal {
template <typename Allocator, typename Iterator, typename... Args>
void ConstructRange(Allocator& alloc,
@@ -84,7 +83,6 @@
}
}
-} // namespace internal
-} // namespace ceres
+} // namespace ceres::internal
#endif // CERES_PUBLIC_INTERNAL_MEMORY_H_
diff --git a/include/ceres/internal/numeric_diff.h b/include/ceres/internal/numeric_diff.h
index ff7a2c3..ba28bec 100644
--- a/include/ceres/internal/numeric_diff.h
+++ b/include/ceres/internal/numeric_diff.h
@@ -1,5 +1,5 @@
// Ceres Solver - A fast non-linear least squares minimizer
-// Copyright 2015 Google Inc. All rights reserved.
+// Copyright 2023 Google Inc. All rights reserved.
// http://ceres-solver.org/
//
// Redistribution and use in source and binary forms, with or without
@@ -47,8 +47,7 @@
#include "ceres/types.h"
#include "glog/logging.h"
-namespace ceres {
-namespace internal {
+namespace ceres::internal {
// 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
@@ -86,18 +85,18 @@
(kParameterBlockSize != ceres::DYNAMIC ? kParameterBlockSize
: parameter_block_size);
- typedef Matrix<double, kNumResiduals, 1> ResidualVector;
- typedef Matrix<double, kParameterBlockSize, 1> ParameterVector;
+ using ResidualVector = Matrix<double, kNumResiduals, 1>;
+ using ParameterVector = Matrix<double, kParameterBlockSize, 1>;
// 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;
+ using JacobianMatrix =
+ Matrix<double,
+ kNumResiduals,
+ kParameterBlockSize,
+ (kParameterBlockSize == 1) ? ColMajor : RowMajor>;
Map<JacobianMatrix> parameter_jacobian(
jacobian, num_residuals_internal, parameter_block_size_internal);
@@ -121,7 +120,7 @@
// 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);
+ (std::max)(min_step_size, options.ridders_relative_initial_step_size);
}
// For each parameter in the parameter block, use finite differences to
@@ -132,7 +131,7 @@
num_residuals_internal);
for (int j = 0; j < parameter_block_size_internal; ++j) {
- const double delta = std::max(min_step_size, step_size(j));
+ const double delta = (std::max)(min_step_size, step_size(j));
if (kMethod == RIDDERS) {
if (!EvaluateRiddersJacobianColumn(functor,
@@ -184,8 +183,8 @@
using Eigen::Map;
using Eigen::Matrix;
- typedef Matrix<double, kNumResiduals, 1> ResidualVector;
- typedef Matrix<double, kParameterBlockSize, 1> ParameterVector;
+ using ResidualVector = Matrix<double, kNumResiduals, 1>;
+ using ParameterVector = Matrix<double, kParameterBlockSize, 1>;
Map<const ParameterVector> x(x_ptr, parameter_block_size);
Map<ParameterVector> x_plus_delta(x_plus_delta_ptr, parameter_block_size);
@@ -260,10 +259,10 @@
using Eigen::Map;
using Eigen::Matrix;
- typedef Matrix<double, kNumResiduals, 1> ResidualVector;
- typedef Matrix<double, kNumResiduals, Eigen::Dynamic>
- ResidualCandidateMatrix;
- typedef Matrix<double, kParameterBlockSize, 1> ParameterVector;
+ using ResidualVector = Matrix<double, kNumResiduals, 1>;
+ using ResidualCandidateMatrix =
+ Matrix<double, kNumResiduals, Eigen::Dynamic>;
+ using ParameterVector = Matrix<double, kParameterBlockSize, 1>;
Map<const ParameterVector> x(x_ptr, parameter_block_size);
Map<ParameterVector> x_plus_delta(x_plus_delta_ptr, parameter_block_size);
@@ -296,7 +295,7 @@
// 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();
+ double norm_error = (std::numeric_limits<double>::max)();
// Loop over decreasing step sizes until:
// 1. Error is smaller than a given value (ridders_epsilon),
@@ -342,7 +341,7 @@
options.ridders_step_shrink_factor;
// Compute the difference between the previous value and the current.
- double candidate_error = std::max(
+ double candidate_error = (std::max)(
(current_candidates->col(k) - current_candidates->col(k - 1))
.norm(),
(current_candidates->col(k) - previous_candidates->col(k - 1))
@@ -502,7 +501,6 @@
}
};
-} // namespace internal
-} // namespace ceres
+} // namespace ceres::internal
#endif // CERES_PUBLIC_INTERNAL_NUMERIC_DIFF_H_
diff --git a/include/ceres/internal/parameter_dims.h b/include/ceres/internal/parameter_dims.h
index 2402106..b7cf935 100644
--- a/include/ceres/internal/parameter_dims.h
+++ b/include/ceres/internal/parameter_dims.h
@@ -1,5 +1,5 @@
// Ceres Solver - A fast non-linear least squares minimizer
-// Copyright 2018 Google Inc. All rights reserved.
+// Copyright 2023 Google Inc. All rights reserved.
// http://ceres-solver.org/
//
// Redistribution and use in source and binary forms, with or without
@@ -36,22 +36,7 @@
#include "ceres/internal/integer_sequence_algorithm.h"
-namespace ceres {
-namespace internal {
-
-// Checks, whether the given parameter block sizes are valid. Valid means every
-// dimension is bigger than zero.
-constexpr bool IsValidParameterDimensionSequence(std::integer_sequence<int>) {
- return true;
-}
-
-template <int N, int... Ts>
-constexpr bool IsValidParameterDimensionSequence(
- std::integer_sequence<int, N, Ts...>) {
- return (N <= 0) ? false
- : IsValidParameterDimensionSequence(
- std::integer_sequence<int, Ts...>());
-}
+namespace ceres::internal {
// Helper class that represents the parameter dimensions. The parameter
// dimensions are either dynamic or the sizes are known at compile time. It is
@@ -70,8 +55,7 @@
// The parameter dimensions are only valid if all parameter block dimensions
// are greater than zero.
- static constexpr bool kIsValid =
- IsValidParameterDimensionSequence(Parameters());
+ static constexpr bool kIsValid = ((Ns > 0) && ...);
static_assert(kIsValid,
"Invalid parameter block dimension detected. Each parameter "
"block dimension must be bigger than zero.");
@@ -81,8 +65,7 @@
static_assert(kIsDynamic || kNumParameterBlocks > 0,
"At least one parameter block must be specified.");
- static constexpr int kNumParameters =
- Sum<std::integer_sequence<int, Ns...>>::Value;
+ static constexpr int kNumParameters = (Ns + ... + 0);
static constexpr int GetDim(int dim) { return params_[dim]; }
@@ -118,7 +101,6 @@
using StaticParameterDims = ParameterDims<false, Ns...>;
using DynamicParameterDims = ParameterDims<true>;
-} // namespace internal
-} // namespace ceres
+} // namespace ceres::internal
#endif // CERES_PUBLIC_INTERNAL_PARAMETER_DIMS_H_
diff --git a/include/ceres/internal/port.h b/include/ceres/internal/port.h
index 040a1ef..d78ed51 100644
--- a/include/ceres/internal/port.h
+++ b/include/ceres/internal/port.h
@@ -1,5 +1,5 @@
// Ceres Solver - A fast non-linear least squares minimizer
-// Copyright 2015 Google Inc. All rights reserved.
+// Copyright 2024 Google Inc. All rights reserved.
// http://ceres-solver.org/
//
// Redistribution and use in source and binary forms, with or without
@@ -31,80 +31,81 @@
#ifndef CERES_PUBLIC_INTERNAL_PORT_H_
#define CERES_PUBLIC_INTERNAL_PORT_H_
-// This file needs to compile as c code.
-#include "ceres/internal/config.h"
+#include <cmath> // Necessary for __cpp_lib_math_special_functions feature test
-#if defined(CERES_USE_OPENMP)
-#if defined(CERES_USE_CXX_THREADS) || defined(CERES_NO_THREADS)
-#error CERES_USE_OPENMP is mutually exclusive to CERES_USE_CXX_THREADS and CERES_NO_THREADS
-#endif
-#elif defined(CERES_USE_CXX_THREADS)
-#if defined(CERES_USE_OPENMP) || defined(CERES_NO_THREADS)
-#error CERES_USE_CXX_THREADS is mutually exclusive to CERES_USE_OPENMP, CERES_USE_CXX_THREADS and CERES_NO_THREADS
-#endif
-#elif defined(CERES_NO_THREADS)
-#if defined(CERES_USE_OPENMP) || defined(CERES_USE_CXX_THREADS)
-#error CERES_NO_THREADS is mutually exclusive to CERES_USE_OPENMP and CERES_USE_CXX_THREADS
-#endif
-#else
-# error One of CERES_USE_OPENMP, CERES_USE_CXX_THREADS or CERES_NO_THREADS must be defined.
-#endif
-
-// CERES_NO_SPARSE should be automatically defined by config.h if Ceres was
-// compiled without any sparse back-end. Verify that it has not subsequently
-// been inconsistently redefined.
-#if defined(CERES_NO_SPARSE)
-#if !defined(CERES_NO_SUITESPARSE)
-#error CERES_NO_SPARSE requires CERES_NO_SUITESPARSE.
-#endif
-#if !defined(CERES_NO_CXSPARSE)
-#error CERES_NO_SPARSE requires CERES_NO_CXSPARSE
-#endif
-#if !defined(CERES_NO_ACCELERATE_SPARSE)
-#error CERES_NO_SPARSE requires CERES_NO_ACCELERATE_SPARSE
-#endif
-#if defined(CERES_USE_EIGEN_SPARSE)
-#error CERES_NO_SPARSE requires !CERES_USE_EIGEN_SPARSE
-#endif
-#endif
-
-// A macro to signal which functions and classes are exported when
-// building a shared library.
+// A macro to mark a function/variable/class as deprecated.
+// We use compiler specific attributes rather than the c++
+// attribute because they do not mix well with each other.
#if defined(_MSC_VER)
-#define CERES_API_SHARED_IMPORT __declspec(dllimport)
-#define CERES_API_SHARED_EXPORT __declspec(dllexport)
+#define CERES_DEPRECATED_WITH_MSG(message) __declspec(deprecated(message))
#elif defined(__GNUC__)
-#define CERES_API_SHARED_IMPORT __attribute__((visibility("default")))
-#define CERES_API_SHARED_EXPORT __attribute__((visibility("default")))
+#define CERES_DEPRECATED_WITH_MSG(message) __attribute__((deprecated(message)))
#else
-#define CERES_API_SHARED_IMPORT
-#define CERES_API_SHARED_EXPORT
+// In the worst case fall back to c++ attribute.
+#define CERES_DEPRECATED_WITH_MSG(message) [[deprecated(message)]]
#endif
-// CERES_BUILDING_SHARED_LIBRARY is only defined locally when Ceres itself is
-// compiled as a shared library, it is never exported to users. In order that
-// we do not have to configure config.h separately when building Ceres as either
-// a static or dynamic library, we define both CERES_USING_SHARED_LIBRARY and
-// CERES_BUILDING_SHARED_LIBRARY when building as a shared library.
-#if defined(CERES_USING_SHARED_LIBRARY)
-#if defined(CERES_BUILDING_SHARED_LIBRARY)
-// Compiling Ceres itself as a shared library.
-#define CERES_EXPORT CERES_API_SHARED_EXPORT
-#else
-// Using Ceres as a shared library.
-#define CERES_EXPORT CERES_API_SHARED_IMPORT
-#endif
-#else
-// Ceres was compiled as a static library, export everything.
-#define CERES_EXPORT
+#ifndef CERES_GET_FLAG
+#define CERES_GET_FLAG(X) X
#endif
-// Unit tests reach in and test internal functionality so we need a way to make
-// those symbols visible
-#ifdef CERES_EXPORT_INTERNAL_SYMBOLS
-#define CERES_EXPORT_INTERNAL CERES_EXPORT
-#else
-#define CERES_EXPORT_INTERNAL
+// Indicates whether C++20 is currently active
+#ifndef CERES_HAS_CPP20
+#if __cplusplus >= 202002L || (defined(_MSVC_LANG) && _MSVC_LANG >= 202002L)
+#define CERES_HAS_CPP20
+#endif // __cplusplus >= 202002L || (defined(_MSVC_LANG) && _MSVC_LANG >=
+ // 202002L)
+#endif // !defined(CERES_HAS_CPP20)
+
+// Prevents symbols from being substituted by the corresponding macro definition
+// under the same name. For instance, min and max are defined as macros on
+// Windows (unless NOMINMAX is defined) which causes compilation errors when
+// defining or referencing symbols under the same name.
+//
+// To be robust in all cases particularly when NOMINMAX cannot be used, use this
+// macro to annotate min/max declarations/definitions. Examples:
+//
+// int max CERES_PREVENT_MACRO_SUBSTITUTION();
+// min CERES_PREVENT_MACRO_SUBSTITUTION(a, b);
+// max CERES_PREVENT_MACRO_SUBSTITUTION(a, b);
+//
+// NOTE: In case the symbols for which the substitution must be prevented are
+// used within another macro, the substitution must be inhibited using parens as
+//
+// (std::numerical_limits<double>::max)()
+//
+// since the helper macro will not work here. Do not use this technique in
+// general case, because it will prevent argument-dependent lookup (ADL).
+//
+#define CERES_PREVENT_MACRO_SUBSTITUTION // Yes, it's empty
+
+// CERES_DISABLE_DEPRECATED_WARNING and CERES_RESTORE_DEPRECATED_WARNING allow
+// to temporarily disable deprecation warnings
+#if defined(_MSC_VER)
+#define CERES_DISABLE_DEPRECATED_WARNING \
+ _Pragma("warning(push)") _Pragma("warning(disable : 4996)")
+#define CERES_RESTORE_DEPRECATED_WARNING _Pragma("warning(pop)")
+#else // defined(_MSC_VER)
+#define CERES_DISABLE_DEPRECATED_WARNING
+#define CERES_RESTORE_DEPRECATED_WARNING
+#endif // defined(_MSC_VER)
+
+#if defined(__cpp_lib_math_special_functions) && \
+ ((__cpp_lib_math_special_functions >= 201603L) || \
+ defined(__STDCPP_MATH_SPEC_FUNCS__) && \
+ (__STDCPP_MATH_SPEC_FUNCS__ >= 201003L))
+// If defined, indicates whether C++17 Bessel functions (of the first kind) are
+// available. Some standard library implementations, such as libc++ (Android
+// NDK, Apple, Clang) do not yet provide these functions. Implementations that
+// do not support C++17, but support ISO 29124:2010, provide the functions if
+// __STDCPP_MATH_SPEC_FUNCS__ is defined by the implementation to a value at
+// least 201003L and if the user defines __STDCPP_WANT_MATH_SPEC_FUNCS__ before
+// including any standard library headers. Standard library Bessel functions are
+// preferred over any other implementation.
+#define CERES_HAS_CPP17_BESSEL_FUNCTIONS
+#elif defined(_SVID_SOURCE) || defined(_BSD_SOURCE) || defined(_XOPEN_SOURCE)
+// If defined, indicates that j0, j1, and jn from <math.h> are available.
+#define CERES_HAS_POSIX_BESSEL_FUNCTIONS
#endif
#endif // CERES_PUBLIC_INTERNAL_PORT_H_
diff --git a/include/ceres/internal/reenable_warnings.h b/include/ceres/internal/reenable_warnings.h
index 2c5db06..a183c25 100644
--- a/include/ceres/internal/reenable_warnings.h
+++ b/include/ceres/internal/reenable_warnings.h
@@ -1,5 +1,5 @@
// Ceres Solver - A fast non-linear least squares minimizer
-// Copyright 2015 Google Inc. All rights reserved.
+// Copyright 2023 Google Inc. All rights reserved.
// http://ceres-solver.org/
//
// Redistribution and use in source and binary forms, with or without
diff --git a/include/ceres/internal/sphere_manifold_functions.h b/include/ceres/internal/sphere_manifold_functions.h
new file mode 100644
index 0000000..4793442
--- /dev/null
+++ b/include/ceres/internal/sphere_manifold_functions.h
@@ -0,0 +1,163 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2023 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: vitus@google.com (Mike Vitus)
+// jodebo_beck@gmx.de (Johannes Beck)
+
+#ifndef CERES_PUBLIC_INTERNAL_SPHERE_MANIFOLD_HELPERS_H_
+#define CERES_PUBLIC_INTERNAL_SPHERE_MANIFOLD_HELPERS_H_
+
+#include "ceres/constants.h"
+#include "ceres/internal/householder_vector.h"
+
+// This module contains functions to compute the SphereManifold plus and minus
+// operator and their Jacobians.
+//
+// As the parameters to these functions are shared between them, they are
+// described here: The following variable names are used:
+// Plus(x, delta) = x + delta = x_plus_delta,
+// Minus(y, x) = y - x = y_minus_x.
+//
+// The remaining ones are v and beta which describe the Householder
+// transformation of x, and norm_delta which is the norm of delta.
+//
+// The types of x, y, x_plus_delta and y_minus_x need to be equivalent to
+// Eigen::Matrix<double, AmbientSpaceDimension, 1> and the type of delta needs
+// to be equivalent to Eigen::Matrix<double, TangentSpaceDimension, 1>.
+//
+// The type of Jacobian plus needs to be equivalent to Eigen::Matrix<double,
+// AmbientSpaceDimension, TangentSpaceDimension, Eigen::RowMajor> and for
+// Jacobian minus Eigen::Matrix<double, TangentSpaceDimension,
+// AmbientSpaceDimension, Eigen::RowMajor>.
+//
+// For all vector / matrix inputs and outputs, template parameters are
+// used in order to allow also Eigen::Ref and Eigen block expressions to
+// be passed to the function.
+
+namespace ceres::internal {
+
+template <typename VT, typename XT, typename DeltaT, typename XPlusDeltaT>
+inline void ComputeSphereManifoldPlus(const VT& v,
+ double beta,
+ const XT& x,
+ const DeltaT& delta,
+ const double norm_delta,
+ XPlusDeltaT* x_plus_delta) {
+ constexpr int AmbientDim = VT::RowsAtCompileTime;
+
+ // Map the delta from the minimum representation to the over parameterized
+ // homogeneous vector. See B.2 p.25 equation (106) - (107) for more details.
+ const double sin_delta_by_delta = std::sin(norm_delta) / norm_delta;
+
+ Eigen::Matrix<double, AmbientDim, 1> y(v.size());
+ y << sin_delta_by_delta * delta, std::cos(norm_delta);
+
+ // Apply the delta update to remain on the sphere.
+ *x_plus_delta = x.norm() * ApplyHouseholderVector(y, v, beta);
+}
+
+template <typename VT, typename JacobianT>
+inline void ComputeSphereManifoldPlusJacobian(const VT& x,
+ JacobianT* jacobian) {
+ constexpr int AmbientSpaceDim = VT::RowsAtCompileTime;
+ using AmbientVector = Eigen::Matrix<double, AmbientSpaceDim, 1>;
+ const int ambient_size = x.size();
+ const int tangent_size = x.size() - 1;
+
+ AmbientVector v(ambient_size);
+ double beta;
+
+ // NOTE: The explicit template arguments are needed here because
+ // ComputeHouseholderVector is templated and some versions of MSVC
+ // have trouble deducing the type of v automatically.
+ ComputeHouseholderVector<VT, double, AmbientSpaceDim>(x, &v, &beta);
+
+ // The Jacobian is equal to J = H.leftCols(size_ - 1) where H is the
+ // Householder matrix (H = I - beta * v * v').
+ for (int i = 0; i < tangent_size; ++i) {
+ (*jacobian).col(i) = -beta * v(i) * v;
+ (*jacobian)(i, i) += 1.0;
+ }
+ (*jacobian) *= x.norm();
+}
+
+template <typename VT, typename XT, typename YT, typename YMinusXT>
+inline void ComputeSphereManifoldMinus(
+ const VT& v, double beta, const XT& x, const YT& y, YMinusXT* y_minus_x) {
+ constexpr int AmbientSpaceDim = VT::RowsAtCompileTime;
+ constexpr int TangentSpaceDim =
+ AmbientSpaceDim == Eigen::Dynamic ? Eigen::Dynamic : AmbientSpaceDim - 1;
+ using AmbientVector = Eigen::Matrix<double, AmbientSpaceDim, 1>;
+
+ const int tangent_size = v.size() - 1;
+
+ const AmbientVector hy = ApplyHouseholderVector(y, v, beta) / x.norm();
+
+ // Calculate y - x. See B.2 p.25 equation (108).
+ const double y_last = hy[tangent_size];
+ const double hy_norm = hy.template head<TangentSpaceDim>(tangent_size).norm();
+ if (hy_norm == 0.0) {
+ y_minus_x->setZero();
+ y_minus_x->data()[tangent_size - 1] = y_last >= 0 ? 0.0 : constants::pi;
+ } else {
+ *y_minus_x = std::atan2(hy_norm, y_last) / hy_norm *
+ hy.template head<TangentSpaceDim>(tangent_size);
+ }
+}
+
+template <typename VT, typename JacobianT>
+inline void ComputeSphereManifoldMinusJacobian(const VT& x,
+ JacobianT* jacobian) {
+ constexpr int AmbientSpaceDim = VT::RowsAtCompileTime;
+ using AmbientVector = Eigen::Matrix<double, AmbientSpaceDim, 1>;
+ const int ambient_size = x.size();
+ const int tangent_size = x.size() - 1;
+
+ AmbientVector v(ambient_size);
+ double beta;
+
+ // NOTE: The explicit template arguments are needed here because
+ // ComputeHouseholderVector is templated and some versions of MSVC
+ // have trouble deducing the type of v automatically.
+ ComputeHouseholderVector<VT, double, AmbientSpaceDim>(x, &v, &beta);
+
+ // The Jacobian is equal to J = H.leftCols(size_ - 1) where H is the
+ // Householder matrix (H = I - beta * v * v').
+ for (int i = 0; i < tangent_size; ++i) {
+ // NOTE: The transpose is used for correctness (the product is expected to
+ // be a row vector), although here there seems to be no difference between
+ // transposing or not for Eigen (possibly a compile-time auto fix).
+ (*jacobian).row(i) = -beta * v(i) * v.transpose();
+ (*jacobian)(i, i) += 1.0;
+ }
+ (*jacobian) /= x.norm();
+}
+
+} // namespace ceres::internal
+
+#endif
diff --git a/include/ceres/internal/variadic_evaluate.h b/include/ceres/internal/variadic_evaluate.h
index 47ff6b1..61af6b2 100644
--- a/include/ceres/internal/variadic_evaluate.h
+++ b/include/ceres/internal/variadic_evaluate.h
@@ -1,5 +1,5 @@
// Ceres Solver - A fast non-linear least squares minimizer
-// Copyright 2015 Google Inc. All rights reserved.
+// Copyright 2023 Google Inc. All rights reserved.
// http://ceres-solver.org/
//
// Redistribution and use in source and binary forms, with or without
@@ -33,16 +33,14 @@
#ifndef CERES_PUBLIC_INTERNAL_VARIADIC_EVALUATE_H_
#define CERES_PUBLIC_INTERNAL_VARIADIC_EVALUATE_H_
-#include <stddef.h>
-
+#include <cstddef>
#include <type_traits>
#include <utility>
#include "ceres/cost_function.h"
#include "ceres/internal/parameter_dims.h"
-namespace ceres {
-namespace internal {
+namespace ceres::internal {
// For fixed size cost functors
template <typename Functor, typename T, int... Indices>
@@ -51,7 +49,7 @@
T* output,
std::false_type /*is_dynamic*/,
std::integer_sequence<int, Indices...>) {
- static_assert(sizeof...(Indices),
+ static_assert(sizeof...(Indices) > 0,
"Invalid number of parameter blocks. At least one parameter "
"block must be specified.");
return functor(input[Indices]..., output);
@@ -108,7 +106,29 @@
return VariadicEvaluateImpl<ParameterDims>(functor, input, output, &functor);
}
-} // namespace internal
-} // namespace ceres
+// When differentiating dynamically sized CostFunctions, VariadicEvaluate
+// expects a functor with the signature:
+//
+// bool operator()(double const* const* parameters, double* cost) const
+//
+// However for NumericDiffFirstOrderFunction, the functor has the signature
+//
+// bool operator()(double const* parameters, double* cost) const
+//
+// This thin wrapper adapts the latter to the former.
+template <typename Functor>
+class FirstOrderFunctorAdapter {
+ public:
+ explicit FirstOrderFunctorAdapter(const Functor& functor)
+ : functor_(functor) {}
+ bool operator()(double const* const* parameters, double* cost) const {
+ return functor_(*parameters, cost);
+ }
+
+ private:
+ const Functor& functor_;
+};
+
+} // namespace ceres::internal
#endif // CERES_PUBLIC_INTERNAL_VARIADIC_EVALUATE_H_
diff --git a/include/ceres/iteration_callback.h b/include/ceres/iteration_callback.h
index 4507fdf..955e2ad 100644
--- a/include/ceres/iteration_callback.h
+++ b/include/ceres/iteration_callback.h
@@ -1,5 +1,5 @@
// Ceres Solver - A fast non-linear least squares minimizer
-// Copyright 2019 Google Inc. All rights reserved.
+// Copyright 2023 Google Inc. All rights reserved.
// http://ceres-solver.org/
//
// Redistribution and use in source and binary forms, with or without
@@ -36,6 +36,7 @@
#define CERES_PUBLIC_ITERATION_CALLBACK_H_
#include "ceres/internal/disable_warnings.h"
+#include "ceres/internal/export.h"
#include "ceres/types.h"
namespace ceres {
@@ -164,8 +165,6 @@
// 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 "
@@ -194,7 +193,7 @@
//
class CERES_EXPORT IterationCallback {
public:
- virtual ~IterationCallback() {}
+ virtual ~IterationCallback();
virtual CallbackReturnType operator()(const IterationSummary& summary) = 0;
};
diff --git a/include/ceres/jet.h b/include/ceres/jet.h
index da49f32..3b7f23f 100644
--- a/include/ceres/jet.h
+++ b/include/ceres/jet.h
@@ -1,5 +1,5 @@
// Ceres Solver - A fast non-linear least squares minimizer
-// Copyright 2019 Google Inc. All rights reserved.
+// Copyright 2024 Google Inc. All rights reserved.
// http://ceres-solver.org/
//
// Redistribution and use in source and binary forms, with or without
@@ -158,20 +158,59 @@
#define CERES_PUBLIC_JET_H_
#include <cmath>
+#include <complex>
#include <iosfwd>
#include <iostream> // NOLINT
#include <limits>
+#include <numeric>
#include <string>
+#include <type_traits>
#include "Eigen/Core"
+#include "ceres/internal/jet_traits.h"
#include "ceres/internal/port.h"
+#include "ceres/jet_fwd.h"
+
+// Here we provide partial specializations of std::common_type for the Jet class
+// to allow determining a Jet type with a common underlying arithmetic type.
+// Such an arithmetic type can be either a scalar or an another Jet. An example
+// for a common type, say, between a float and a Jet<double, N> is a Jet<double,
+// N> (i.e., std::common_type_t<float, ceres::Jet<double, N>> and
+// ceres::Jet<double, N> refer to the same type.)
+//
+// The partial specialization are also used for determining compatible types by
+// means of SFINAE and thus allow such types to be expressed as operands of
+// logical comparison operators. Missing (partial) specialization of
+// std::common_type for a particular (custom) type will therefore disable the
+// use of comparison operators defined by Ceres.
+//
+// Since these partial specializations are used as SFINAE constraints, they
+// enable standard promotion rules between various scalar types and consequently
+// their use in comparison against a Jet without providing implicit
+// conversions from a scalar, such as an int, to a Jet (see the implementation
+// of logical comparison operators below).
+
+template <typename T, int N, typename U>
+struct std::common_type<T, ceres::Jet<U, N>> {
+ using type = ceres::Jet<common_type_t<T, U>, N>;
+};
+
+template <typename T, int N, typename U>
+struct std::common_type<ceres::Jet<T, N>, U> {
+ using type = ceres::Jet<common_type_t<T, U>, N>;
+};
+
+template <typename T, int N, typename U>
+struct std::common_type<ceres::Jet<T, N>, ceres::Jet<U, N>> {
+ using type = ceres::Jet<common_type_t<T, U>, N>;
+};
namespace ceres {
template <typename T, int N>
struct Jet {
enum { DIMENSION = N };
- typedef T Scalar;
+ using Scalar = T;
// Default-construct "a" because otherwise this can lead to false errors about
// uninitialized uses when other classes relying on default constructed T
@@ -352,19 +391,21 @@
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; \
+// Binary comparison operators for both scalars and jets. At least one of the
+// operands must be a Jet. Promotable scalars (e.g., int, float, double etc.)
+// can appear on either side of the operator. std::common_type_t is used as an
+// SFINAE constraint to selectively enable compatible operand types. This allows
+// comparison, for instance, against int literals without implicit conversion.
+// In case the Jet arithmetic type is a Jet itself, a recursive expansion of Jet
+// value is performed.
+#define CERES_DEFINE_JET_COMPARISON_OPERATOR(op) \
+ template <typename Lhs, \
+ typename Rhs, \
+ std::enable_if_t<PromotableJetOperands_v<Lhs, Rhs>>* = nullptr> \
+ constexpr bool operator op(const Lhs& f, const Rhs& g) noexcept( \
+ noexcept(internal::AsScalar(f) op internal::AsScalar(g))) { \
+ using internal::AsScalar; \
+ return AsScalar(f) op AsScalar(g); \
}
CERES_DEFINE_JET_COMPARISON_OPERATOR(<) // NOLINT
CERES_DEFINE_JET_COMPARISON_OPERATOR(<=) // NOLINT
@@ -386,43 +427,141 @@
using std::atan2;
using std::cbrt;
using std::ceil;
+using std::copysign;
using std::cos;
using std::cosh;
+#ifdef CERES_HAS_CPP17_BESSEL_FUNCTIONS
+using std::cyl_bessel_j;
+#endif // CERES_HAS_CPP17_BESSEL_FUNCTIONS
using std::erf;
using std::erfc;
using std::exp;
using std::exp2;
+using std::expm1;
+using std::fdim;
using std::floor;
+using std::fma;
using std::fmax;
using std::fmin;
+using std::fpclassify;
using std::hypot;
using std::isfinite;
using std::isinf;
using std::isnan;
using std::isnormal;
using std::log;
+using std::log10;
+using std::log1p;
using std::log2;
+using std::norm;
using std::pow;
+using std::signbit;
using std::sin;
using std::sinh;
using std::sqrt;
using std::tan;
using std::tanh;
+// MSVC (up to 1930) defines quiet comparison functions as template functions
+// which causes compilation errors due to ambiguity in the template parameter
+// type resolution for using declarations in the ceres namespace. Workaround the
+// issue by defining specific overload and bypass MSVC standard library
+// definitions.
+#if defined(_MSC_VER)
+inline bool isgreater(double lhs,
+ double rhs) noexcept(noexcept(std::isgreater(lhs, rhs))) {
+ return std::isgreater(lhs, rhs);
+}
+inline bool isless(double lhs,
+ double rhs) noexcept(noexcept(std::isless(lhs, rhs))) {
+ return std::isless(lhs, rhs);
+}
+inline bool islessequal(double lhs,
+ double rhs) noexcept(noexcept(std::islessequal(lhs,
+ rhs))) {
+ return std::islessequal(lhs, rhs);
+}
+inline bool isgreaterequal(double lhs, double rhs) noexcept(
+ noexcept(std::isgreaterequal(lhs, rhs))) {
+ return std::isgreaterequal(lhs, rhs);
+}
+inline bool islessgreater(double lhs, double rhs) noexcept(
+ noexcept(std::islessgreater(lhs, rhs))) {
+ return std::islessgreater(lhs, rhs);
+}
+inline bool isunordered(double lhs,
+ double rhs) noexcept(noexcept(std::isunordered(lhs,
+ rhs))) {
+ return std::isunordered(lhs, rhs);
+}
+#else
+using std::isgreater;
+using std::isgreaterequal;
+using std::isless;
+using std::islessequal;
+using std::islessgreater;
+using std::isunordered;
+#endif
+
+#ifdef CERES_HAS_CPP20
+using std::lerp;
+using std::midpoint;
+#endif // defined(CERES_HAS_CPP20)
+
// Legacy names from pre-C++11 days.
// clang-format off
+CERES_DEPRECATED_WITH_MSG("ceres::IsFinite will be removed in a future Ceres Solver release. Please use ceres::isfinite.")
inline bool IsFinite(double x) { return std::isfinite(x); }
+CERES_DEPRECATED_WITH_MSG("ceres::IsInfinite will be removed in a future Ceres Solver release. Please use ceres::isinf.")
inline bool IsInfinite(double x) { return std::isinf(x); }
+CERES_DEPRECATED_WITH_MSG("ceres::IsNaN will be removed in a future Ceres Solver release. Please use ceres::isnan.")
inline bool IsNaN(double x) { return std::isnan(x); }
+CERES_DEPRECATED_WITH_MSG("ceres::IsNormal will be removed in a future Ceres Solver release. Please use ceres::isnormal.")
inline bool IsNormal(double x) { return std::isnormal(x); }
// clang-format on
// In general, f(a + h) ~= f(a) + f'(a) h, via the chain rule.
-// abs(x + h) ~= x + h or -(x + h)
+// abs(x + h) ~= abs(x) + sgn(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);
+ return Jet<T, N>(abs(f.a), copysign(T(1), f.a) * f.v);
+}
+
+// copysign(a, b) composes a float with the magnitude of a and the sign of b.
+// Therefore, the function can be formally defined as
+//
+// copysign(a, b) = sgn(b)|a|
+//
+// where
+//
+// d/dx |x| = sgn(x)
+// d/dx sgn(x) = 2δ(x)
+//
+// sgn(x) being the signum function. Differentiating copysign(a, b) with respect
+// to a and b gives:
+//
+// d/da sgn(b)|a| = sgn(a) sgn(b)
+// d/db sgn(b)|a| = 2|a|δ(b)
+//
+// with the dual representation given by
+//
+// copysign(a + da, b + db) ~= sgn(b)|a| + (sgn(a)sgn(b) da + 2|a|δ(b) db)
+//
+// where δ(b) is the Dirac delta function.
+template <typename T, int N>
+inline Jet<T, N> copysign(const Jet<T, N>& f, const Jet<T, N> g) {
+ // The Dirac delta function δ(b) is undefined at b=0 (here it's
+ // infinite) and 0 everywhere else.
+ T d = fpclassify(g) == FP_ZERO ? std::numeric_limits<T>::infinity() : T(0);
+ T sa = copysign(T(1), f.a); // sgn(a)
+ T sb = copysign(T(1), g.a); // sgn(b)
+ // The second part of the infinitesimal is 2|a|δ(b) which is either infinity
+ // or 0 unless a or any of the values of the b infinitesimal are 0. In the
+ // latter case, the corresponding values become NaNs (multiplying 0 by
+ // infinity gives NaN). We drop the constant factor 2 since it does not change
+ // the result (its values will still be either 0, infinity or NaN).
+ return Jet<T, N>(copysign(f.a, g.a), sa * sb * f.v + abs(f.a) * d * g.v);
}
// log(a + h) ~= log(a) + h / a
@@ -432,6 +571,21 @@
return Jet<T, N>(log(f.a), f.v * a_inverse);
}
+// log10(a + h) ~= log10(a) + h / (a log(10))
+template <typename T, int N>
+inline Jet<T, N> log10(const Jet<T, N>& f) {
+ // Most compilers will expand log(10) to a constant.
+ const T a_inverse = T(1.0) / (f.a * log(T(10.0)));
+ return Jet<T, N>(log10(f.a), f.v * a_inverse);
+}
+
+// log1p(a + h) ~= log1p(a) + h / (1 + a)
+template <typename T, int N>
+inline Jet<T, N> log1p(const Jet<T, N>& f) {
+ const T a_inverse = T(1.0) / (T(1.0) + f.a);
+ return Jet<T, N>(log1p(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) {
@@ -439,6 +593,14 @@
return Jet<T, N>(tmp, tmp * f.v);
}
+// expm1(a + h) ~= expm1(a) + exp(a) h
+template <typename T, int N>
+inline Jet<T, N> expm1(const Jet<T, N>& f) {
+ const T tmp = expm1(f.a);
+ const T expa = tmp + T(1.0); // exp(a) = expm1(a) + 1
+ return Jet<T, N>(tmp, expa * 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) {
@@ -565,31 +727,152 @@
return Jet<T, N>(tmp, x.a / tmp * x.v + y.a / tmp * y.v);
}
+// Like sqrt(x^2 + y^2 + z^2),
+// but acts to prevent underflow/overflow for small/large x/y/z.
+// Note that the function is non-smooth at x=y=z=0,
+// so the derivative is undefined there.
template <typename T, int N>
-inline Jet<T, N> fmax(const Jet<T, N>& x, const Jet<T, N>& y) {
- return x < y ? y : x;
+inline Jet<T, N> hypot(const Jet<T, N>& x,
+ const Jet<T, N>& y,
+ const Jet<T, N>& z) {
+ // d/da sqrt(a) = 0.5 / sqrt(a)
+ // d/dx x^2 + y^2 + z^2 = 2x
+ // So by the chain rule:
+ // d/dx sqrt(x^2 + y^2 + z^2)
+ // = 0.5 / sqrt(x^2 + y^2 + z^2) * 2x
+ // = x / sqrt(x^2 + y^2 + z^2)
+ // d/dy sqrt(x^2 + y^2 + z^2) = y / sqrt(x^2 + y^2 + z^2)
+ // d/dz sqrt(x^2 + y^2 + z^2) = z / sqrt(x^2 + y^2 + z^2)
+ const T tmp = hypot(x.a, y.a, z.a);
+ return Jet<T, N>(tmp, x.a / tmp * x.v + y.a / tmp * y.v + z.a / tmp * z.v);
}
+// Like x * y + z but rounded only once.
template <typename T, int N>
-inline Jet<T, N> fmin(const Jet<T, N>& x, const Jet<T, N>& y) {
- return y < x ? y : x;
+inline Jet<T, N> fma(const Jet<T, N>& x,
+ const Jet<T, N>& y,
+ const Jet<T, N>& z) {
+ // d/dx fma(x, y, z) = y
+ // d/dy fma(x, y, z) = x
+ // d/dz fma(x, y, z) = 1
+ return Jet<T, N>(fma(x.a, y.a, z.a), y.a * x.v + x.a * y.v + z.v);
}
-// erf is defined as an integral that cannot be expressed analyticaly
+// Return value of fmax() and fmin() on equality
+// ---------------------------------------------
+//
+// There is arguably no good answer to what fmax() & fmin() should return on
+// equality, which for Jets by definition ONLY compares the scalar parts. We
+// choose what we think is the least worst option (averaging as Jets) which
+// minimises undesirable/unexpected behaviour as used, and also supports client
+// code written against Ceres versions prior to type promotion being supported
+// in Jet comparisons (< v2.1).
+//
+// The std::max() convention of returning the first argument on equality is
+// problematic, as it means that the derivative component may or may not be
+// preserved (when comparing a Jet with a scalar) depending upon the ordering.
+//
+// Always returning the Jet in {Jet, scalar} cases on equality is problematic
+// as it is inconsistent with the behaviour that would be obtained if the scalar
+// was first cast to Jet and the {Jet, Jet} case was used. Prior to type
+// promotion (Ceres v2.1) client code would typically cast constants to Jets
+// e.g: fmax(x, T(2.0)) which means the {Jet, Jet} case predominates, and we
+// still want the result to be order independent.
+//
+// Our intuition is that preserving a non-zero derivative is best, even if
+// its value does not match either of the inputs. Averaging achieves this
+// whilst ensuring argument ordering independence. This is also the approach
+// used by the Jax library, and TensorFlow's reduce_max().
+
+// Returns the larger of the two arguments, with Jet averaging on equality.
+// NaNs are treated as missing data.
+//
+// NOTE: This function is NOT subject to any of the error conditions specified
+// in `math_errhandling`.
+template <typename Lhs,
+ typename Rhs,
+ std::enable_if_t<CompatibleJetOperands_v<Lhs, Rhs>>* = nullptr>
+inline decltype(auto) fmax(const Lhs& x, const Rhs& y) {
+ using J = std::common_type_t<Lhs, Rhs>;
+ // As x == y may set FP exceptions in the presence of NaNs when used with
+ // non-default compiler options so we avoid its use here.
+ if (isnan(x) || isnan(y) || islessgreater(x, y)) {
+ return isnan(x) || isless(x, y) ? J{y} : J{x};
+ }
+ // x == y (scalar parts) return the average of their Jet representations.
+#if defined(CERES_HAS_CPP20)
+ return midpoint(J{x}, J{y});
+#else
+ return (J{x} + J{y}) * typename J::Scalar(0.5);
+#endif // defined(CERES_HAS_CPP20)
+}
+
+// Returns the smaller of the two arguments, with Jet averaging on equality.
+// NaNs are treated as missing data.
+//
+// NOTE: This function is NOT subject to any of the error conditions specified
+// in `math_errhandling`.
+template <typename Lhs,
+ typename Rhs,
+ std::enable_if_t<CompatibleJetOperands_v<Lhs, Rhs>>* = nullptr>
+inline decltype(auto) fmin(const Lhs& x, const Rhs& y) {
+ using J = std::common_type_t<Lhs, Rhs>;
+ // As x == y may set FP exceptions in the presence of NaNs when used with
+ // non-default compiler options so we avoid its use here.
+ if (isnan(x) || isnan(y) || islessgreater(x, y)) {
+ return isnan(x) || isgreater(x, y) ? J{y} : J{x};
+ }
+ // x == y (scalar parts) return the average of their Jet representations.
+#if defined(CERES_HAS_CPP20)
+ return midpoint(J{x}, J{y});
+#else
+ return (J{x} + J{y}) * typename J::Scalar(0.5);
+#endif // defined(CERES_HAS_CPP20)
+}
+
+// Returns the positive difference (f - g) of two arguments and zero if f <= g.
+// If at least one argument is NaN, a NaN is return.
+//
+// NOTE At least one of the argument types must be a Jet, the other one can be a
+// scalar. In case both arguments are Jets, their dimensionality must match.
+template <typename Lhs,
+ typename Rhs,
+ std::enable_if_t<CompatibleJetOperands_v<Lhs, Rhs>>* = nullptr>
+inline decltype(auto) fdim(const Lhs& f, const Rhs& g) {
+ using J = std::common_type_t<Lhs, Rhs>;
+ if (isnan(f) || isnan(g)) {
+ return std::numeric_limits<J>::quiet_NaN();
+ }
+ return isgreater(f, g) ? J{f - g} : J{};
+}
+
+// erf is defined as an integral that cannot be expressed analytically
// however, the derivative is trivial to compute
// erf(x + h) = erf(x) + h * 2*exp(-x^2)/sqrt(pi)
template <typename T, int N>
inline Jet<T, N> erf(const Jet<T, N>& x) {
- return Jet<T, N>(erf(x.a), x.v * M_2_SQRTPI * exp(-x.a * x.a));
+ // We evaluate the constant as follows:
+ // 2 / sqrt(pi) = 1 / sqrt(atan(1.))
+ // On POSIX systems it is defined as M_2_SQRTPI, but this is not
+ // portable and the type may not be T. The above expression
+ // evaluates to full precision with IEEE arithmetic and, since it's
+ // constant, the compiler can generate exactly the same code. gcc
+ // does so even at -O0.
+ return Jet<T, N>(erf(x.a), x.v * exp(-x.a * x.a) * (T(1) / sqrt(atan(T(1)))));
}
// erfc(x) = 1-erf(x)
// erfc(x + h) = erfc(x) + h * (-2*exp(-x^2)/sqrt(pi))
template <typename T, int N>
inline Jet<T, N> erfc(const Jet<T, N>& x) {
- return Jet<T, N>(erfc(x.a), -x.v * M_2_SQRTPI * exp(-x.a * x.a));
+ // See in erf() above for the evaluation of the constant in the derivative.
+ return Jet<T, N>(erfc(x.a),
+ -x.v * exp(-x.a * x.a) * (T(1) / sqrt(atan(T(1)))));
}
+#if defined(CERES_HAS_CPP17_BESSEL_FUNCTIONS) || \
+ defined(CERES_HAS_POSIX_BESSEL_FUNCTIONS)
+
// Bessel functions of the first kind with integer order equal to 0, 1, n.
//
// Microsoft has deprecated the j[0,1,n]() POSIX Bessel functions in favour of
@@ -597,25 +880,33 @@
// function errors in client code (the specific warning is suppressed when
// Ceres itself is built).
inline double BesselJ0(double x) {
-#if defined(CERES_MSVC_USE_UNDERSCORE_PREFIXED_BESSEL_FUNCTIONS)
- return _j0(x);
+#ifdef CERES_HAS_CPP17_BESSEL_FUNCTIONS
+ return cyl_bessel_j(0, x);
#else
+ CERES_DISABLE_DEPRECATED_WARNING
return j0(x);
-#endif
+ CERES_RESTORE_DEPRECATED_WARNING
+#endif // defined(CERES_HAS_CPP17_BESSEL_FUNCTIONS)
}
+
inline double BesselJ1(double x) {
-#if defined(CERES_MSVC_USE_UNDERSCORE_PREFIXED_BESSEL_FUNCTIONS)
- return _j1(x);
+#ifdef CERES_HAS_CPP17_BESSEL_FUNCTIONS
+ return cyl_bessel_j(1, x);
#else
+ CERES_DISABLE_DEPRECATED_WARNING
return j1(x);
-#endif
+ CERES_RESTORE_DEPRECATED_WARNING
+#endif // defined(CERES_HAS_CPP17_BESSEL_FUNCTIONS)
}
+
inline double BesselJn(int n, double x) {
-#if defined(CERES_MSVC_USE_UNDERSCORE_PREFIXED_BESSEL_FUNCTIONS)
- return _jn(n, x);
+#ifdef CERES_HAS_CPP17_BESSEL_FUNCTIONS
+ return cyl_bessel_j(static_cast<double>(n), x);
#else
+ CERES_DISABLE_DEPRECATED_WARNING
return jn(n, x);
-#endif
+ CERES_RESTORE_DEPRECATED_WARNING
+#endif // defined(CERES_HAS_CPP17_BESSEL_FUNCTIONS)
}
// For the formulae of the derivatives of the Bessel functions see the book:
@@ -628,100 +919,264 @@
// j0(a + h) ~= j0(a) - j1(a) h
template <typename T, int N>
inline Jet<T, N> BesselJ0(const Jet<T, N>& f) {
+#ifdef CERES_HAS_CPP17_BESSEL_FUNCTIONS
+ return cyl_bessel_j(0, f);
+#else
return Jet<T, N>(BesselJ0(f.a), -BesselJ1(f.a) * f.v);
+#endif // defined(CERES_HAS_CPP17_BESSEL_FUNCTIONS)
}
// See formula http://dlmf.nist.gov/10.6#E1
// j1(a + h) ~= j1(a) + 0.5 ( j0(a) - j2(a) ) h
template <typename T, int N>
inline Jet<T, N> BesselJ1(const Jet<T, N>& f) {
+#ifdef CERES_HAS_CPP17_BESSEL_FUNCTIONS
+ return cyl_bessel_j(1, f);
+#else
return Jet<T, N>(BesselJ1(f.a),
T(0.5) * (BesselJ0(f.a) - BesselJn(2, f.a)) * f.v);
+#endif // defined(CERES_HAS_CPP17_BESSEL_FUNCTIONS)
}
// See formula http://dlmf.nist.gov/10.6#E1
// j_n(a + h) ~= j_n(a) + 0.5 ( j_{n-1}(a) - j_{n+1}(a) ) h
template <typename T, int N>
inline Jet<T, N> BesselJn(int n, const Jet<T, N>& f) {
+#ifdef CERES_HAS_CPP17_BESSEL_FUNCTIONS
+ return cyl_bessel_j(n, f);
+#else
return Jet<T, N>(
BesselJn(n, f.a),
T(0.5) * (BesselJn(n - 1, f.a) - BesselJn(n + 1, f.a)) * f.v);
+#endif // defined(CERES_HAS_CPP17_BESSEL_FUNCTIONS)
}
-// Jet Classification. It is not clear what the appropriate semantics are for
-// these classifications. This picks that std::isfinite and std::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.
+#endif // defined(CERES_HAS_CPP17_BESSEL_FUNCTIONS) ||
+ // defined(CERES_HAS_POSIX_BESSEL_FUNCTIONS)
-// The jet is finite if all parts of the jet are finite.
+#ifdef CERES_HAS_CPP17_BESSEL_FUNCTIONS
+
+// See formula http://dlmf.nist.gov/10.6#E1
+// j_n(a + h) ~= j_n(a) + 0.5 ( j_{n-1}(a) - j_{n+1}(a) ) h
+template <typename T, int N>
+inline Jet<T, N> cyl_bessel_j(double v, const Jet<T, N>& f) {
+ // See formula http://dlmf.nist.gov/10.6#E3
+ // j0(a + h) ~= j0(a) - j1(a) h
+ if (fpclassify(v) == FP_ZERO) {
+ return Jet<T, N>(cyl_bessel_j(0, f.a), -cyl_bessel_j(1, f.a) * f.v);
+ }
+
+ return Jet<T, N>(
+ cyl_bessel_j(v, f.a),
+ T(0.5) * (cyl_bessel_j(v - 1, f.a) - cyl_bessel_j(v + 1, f.a)) * f.v);
+}
+
+#endif // CERES_HAS_CPP17_BESSEL_FUNCTIONS
+
+// Classification and comparison functionality referencing only the scalar part
+// of a Jet. To classify the derivatives (e.g., for sanity checks), the dual
+// part should be referenced explicitly. For instance, to check whether the
+// derivatives of a Jet 'f' are reasonable, one can use
+//
+// isfinite(f.v.array()).all()
+// !isnan(f.v.array()).any()
+//
+// etc., depending on the desired semantics.
+//
+// NOTE: Floating-point classification and comparison functions and operators
+// should be used with care as no derivatives can be propagated by such
+// functions directly but only by expressions resulting from corresponding
+// conditional statements. At the same time, conditional statements can possibly
+// introduce a discontinuity in the cost function making it impossible to
+// evaluate its derivative and thus the optimization problem intractable.
+
+// Determines whether the scalar part of the Jet is finite.
template <typename T, int N>
inline bool isfinite(const Jet<T, N>& f) {
- // Branchless implementation. This is more efficient for the false-case and
- // works with the codegen system.
- auto result = isfinite(f.a);
- for (int i = 0; i < N; ++i) {
- result = result & isfinite(f.v[i]);
- }
- return result;
+ return isfinite(f.a);
}
-// The jet is infinite if any part of the Jet is infinite.
+// Determines whether the scalar part of the Jet is infinite.
template <typename T, int N>
inline bool isinf(const Jet<T, N>& f) {
- auto result = isinf(f.a);
- for (int i = 0; i < N; ++i) {
- result = result | isinf(f.v[i]);
- }
- return result;
+ return isinf(f.a);
}
-// The jet is NaN if any part of the jet is NaN.
+// Determines whether the scalar part of the Jet is NaN.
template <typename T, int N>
inline bool isnan(const Jet<T, N>& f) {
- auto result = isnan(f.a);
- for (int i = 0; i < N; ++i) {
- result = result | isnan(f.v[i]);
- }
- return result;
+ return isnan(f.a);
}
-// The jet is normal if all parts of the jet are normal.
+// Determines whether the scalar part of the Jet is neither zero, subnormal,
+// infinite, nor NaN.
template <typename T, int N>
inline bool isnormal(const Jet<T, N>& f) {
- auto result = isnormal(f.a);
- for (int i = 0; i < N; ++i) {
- result = result & isnormal(f.v[i]);
- }
- return result;
+ return isnormal(f.a);
+}
+
+// Determines whether the scalar part of the Jet f is less than the scalar
+// part of g.
+//
+// NOTE: This function does NOT set any floating-point exceptions.
+template <typename Lhs,
+ typename Rhs,
+ std::enable_if_t<CompatibleJetOperands_v<Lhs, Rhs>>* = nullptr>
+inline bool isless(const Lhs& f, const Rhs& g) {
+ using internal::AsScalar;
+ return isless(AsScalar(f), AsScalar(g));
+}
+
+// Determines whether the scalar part of the Jet f is greater than the scalar
+// part of g.
+//
+// NOTE: This function does NOT set any floating-point exceptions.
+template <typename Lhs,
+ typename Rhs,
+ std::enable_if_t<CompatibleJetOperands_v<Lhs, Rhs>>* = nullptr>
+inline bool isgreater(const Lhs& f, const Rhs& g) {
+ using internal::AsScalar;
+ return isgreater(AsScalar(f), AsScalar(g));
+}
+
+// Determines whether the scalar part of the Jet f is less than or equal to the
+// scalar part of g.
+//
+// NOTE: This function does NOT set any floating-point exceptions.
+template <typename Lhs,
+ typename Rhs,
+ std::enable_if_t<CompatibleJetOperands_v<Lhs, Rhs>>* = nullptr>
+inline bool islessequal(const Lhs& f, const Rhs& g) {
+ using internal::AsScalar;
+ return islessequal(AsScalar(f), AsScalar(g));
+}
+
+// Determines whether the scalar part of the Jet f is less than or greater than
+// (f < g || f > g) the scalar part of g.
+//
+// NOTE: This function does NOT set any floating-point exceptions.
+template <typename Lhs,
+ typename Rhs,
+ std::enable_if_t<CompatibleJetOperands_v<Lhs, Rhs>>* = nullptr>
+inline bool islessgreater(const Lhs& f, const Rhs& g) {
+ using internal::AsScalar;
+ return islessgreater(AsScalar(f), AsScalar(g));
+}
+
+// Determines whether the scalar part of the Jet f is greater than or equal to
+// the scalar part of g.
+//
+// NOTE: This function does NOT set any floating-point exceptions.
+template <typename Lhs,
+ typename Rhs,
+ std::enable_if_t<CompatibleJetOperands_v<Lhs, Rhs>>* = nullptr>
+inline bool isgreaterequal(const Lhs& f, const Rhs& g) {
+ using internal::AsScalar;
+ return isgreaterequal(AsScalar(f), AsScalar(g));
+}
+
+// Determines if either of the scalar parts of the arguments are NaN and
+// thus cannot be ordered with respect to each other.
+template <typename Lhs,
+ typename Rhs,
+ std::enable_if_t<CompatibleJetOperands_v<Lhs, Rhs>>* = nullptr>
+inline bool isunordered(const Lhs& f, const Rhs& g) {
+ using internal::AsScalar;
+ return isunordered(AsScalar(f), AsScalar(g));
+}
+
+// Categorize scalar part as zero, subnormal, normal, infinite, NaN, or
+// implementation-defined.
+template <typename T, int N>
+inline int fpclassify(const Jet<T, N>& f) {
+ return fpclassify(f.a);
+}
+
+// Determines whether the scalar part of the argument is negative.
+template <typename T, int N>
+inline bool signbit(const Jet<T, N>& f) {
+ return signbit(f.a);
}
// Legacy functions from the pre-C++11 days.
template <typename T, int N>
+CERES_DEPRECATED_WITH_MSG(
+ "ceres::IsFinite will be removed in a future Ceres Solver release. Please "
+ "use ceres::isfinite.")
inline bool IsFinite(const Jet<T, N>& f) {
return isfinite(f);
}
template <typename T, int N>
+CERES_DEPRECATED_WITH_MSG(
+ "ceres::IsNaN will be removed in a future Ceres Solver release. Please use "
+ "ceres::isnan.")
inline bool IsNaN(const Jet<T, N>& f) {
return isnan(f);
}
template <typename T, int N>
+CERES_DEPRECATED_WITH_MSG(
+ "ceres::IsNormal will be removed in a future Ceres Solver release. Please "
+ "use ceres::isnormal.")
inline bool IsNormal(const Jet<T, N>& f) {
return isnormal(f);
}
// The jet is infinite if any part of the jet is infinite.
template <typename T, int N>
+CERES_DEPRECATED_WITH_MSG(
+ "ceres::IsInfinite will be removed in a future Ceres Solver release. "
+ "Please use ceres::isinf.")
inline bool IsInfinite(const Jet<T, N>& f) {
return isinf(f);
}
+#ifdef CERES_HAS_CPP20
+// Computes the linear interpolation a + t(b - a) between a and b at the value
+// t. For arguments outside of the range 0 <= t <= 1, the values are
+// extrapolated.
+//
+// Differentiating lerp(a, b, t) with respect to a, b, and t gives:
+//
+// d/da lerp(a, b, t) = 1 - t
+// d/db lerp(a, b, t) = t
+// d/dt lerp(a, b, t) = b - a
+//
+// with the dual representation given by
+//
+// lerp(a + da, b + db, t + dt)
+// ~= lerp(a, b, t) + (1 - t) da + t db + (b - a) dt .
+template <typename T, int N>
+inline Jet<T, N> lerp(const Jet<T, N>& a,
+ const Jet<T, N>& b,
+ const Jet<T, N>& t) {
+ return Jet<T, N>{lerp(a.a, b.a, t.a),
+ (T(1) - t.a) * a.v + t.a * b.v + (b.a - a.a) * t.v};
+}
+
+// Computes the midpoint a + (b - a) / 2.
+//
+// Differentiating midpoint(a, b) with respect to a and b gives:
+//
+// d/da midpoint(a, b) = 1/2
+// d/db midpoint(a, b) = 1/2
+//
+// with the dual representation given by
+//
+// midpoint(a + da, b + db) ~= midpoint(a, b) + (da + db) / 2 .
+template <typename T, int N>
+inline Jet<T, N> midpoint(const Jet<T, N>& a, const Jet<T, N>& b) {
+ Jet<T, N> result{midpoint(a.a, b.a)};
+ // To avoid overflow in the differential, compute
+ // (da + db) / 2 using midpoint.
+ for (int i = 0; i < N; ++i) {
+ result.v[i] = midpoint(a.v[i], b.v[i]);
+ }
+ return result;
+}
+#endif // defined(CERES_HAS_CPP20)
+
// 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
@@ -737,6 +1192,22 @@
return Jet<T, N>(atan2(g.a, f.a), tmp * (-g.a * f.v + f.a * g.v));
}
+// Computes the square x^2 of a real number x (not the Euclidean L^2 norm as
+// the name might suggest).
+//
+// NOTE: While std::norm is primarily intended for computing the squared
+// magnitude of a std::complex<> number, the current Jet implementation does not
+// support mixing a scalar T in its real part and std::complex<T> and in the
+// infinitesimal. Mixed Jet support is necessary for the type decay from
+// std::complex<T> to T (the squared magnitude of a complex number is always
+// real) performed by std::norm.
+//
+// norm(x + h) ~= norm(x) + 2x h
+template <typename T, int N>
+inline Jet<T, N> norm(const Jet<T, N>& f) {
+ return Jet<T, N>(norm(f.a), T(2) * f.a * f.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>
@@ -760,14 +1231,14 @@
inline Jet<T, N> pow(T f, const Jet<T, N>& g) {
Jet<T, N> result;
- if (f == T(0) && g.a > T(0)) {
+ if (fpclassify(f) == FP_ZERO && g > 0) {
// Handle case 2.
result = Jet<T, N>(T(0.0));
} else {
- if (f < 0 && g.a == floor(g.a)) { // Handle case 3.
+ if (f < 0 && g == floor(g.a)) { // Handle case 3.
result = Jet<T, N>(pow(f, g.a));
for (int i = 0; i < N; i++) {
- if (g.v[i] != T(0.0)) {
+ if (fpclassify(g.v[i]) != FP_ZERO) {
// Return a NaN when g.v != 0.
result.v[i] = std::numeric_limits<T>::quiet_NaN();
}
@@ -822,21 +1293,21 @@
inline Jet<T, N> pow(const Jet<T, N>& f, const Jet<T, N>& g) {
Jet<T, N> result;
- if (f.a == T(0) && g.a >= T(1)) {
+ if (fpclassify(f) == FP_ZERO && g >= 1) {
// Handle cases 2 and 3.
- if (g.a > T(1)) {
+ if (g > 1) {
result = Jet<T, N>(T(0.0));
} else {
result = f;
}
} else {
- if (f.a < T(0) && g.a == floor(g.a)) {
+ if (f < 0 && g == floor(g.a)) {
// Handle cases 7 and 8.
T const tmp = g.a * pow(f.a, g.a - T(1.0));
result = Jet<T, N>(pow(f.a, g.a), tmp * f.v);
for (int i = 0; i < N; i++) {
- if (g.v[i] != T(0.0)) {
+ if (fpclassify(g.v[i]) != FP_ZERO) {
// Return a NaN when g.v != 0.
result.v[i] = T(std::numeric_limits<double>::quiet_NaN());
}
@@ -887,8 +1358,13 @@
static constexpr bool is_bounded = std::numeric_limits<T>::is_bounded;
static constexpr bool is_modulo = std::numeric_limits<T>::is_modulo;
+ // has_denorm (and has_denorm_loss, not defined for Jet) has been deprecated
+ // in C++23. However, without an intent to remove the declaration. Disable
+ // deprecation warnings temporarily just for the corresponding symbols.
+ CERES_DISABLE_DEPRECATED_WARNING
static constexpr std::float_denorm_style has_denorm =
std::numeric_limits<T>::has_denorm;
+ CERES_RESTORE_DEPRECATED_WARNING
static constexpr std::float_round_style round_style =
std::numeric_limits<T>::round_style;
@@ -904,8 +1380,9 @@
static constexpr bool tinyness_before =
std::numeric_limits<T>::tinyness_before;
- static constexpr ceres::Jet<T, N> min() noexcept {
- return ceres::Jet<T, N>(std::numeric_limits<T>::min());
+ static constexpr ceres::Jet<T, N> min
+ CERES_PREVENT_MACRO_SUBSTITUTION() noexcept {
+ return ceres::Jet<T, N>((std::numeric_limits<T>::min)());
}
static constexpr ceres::Jet<T, N> lowest() noexcept {
return ceres::Jet<T, N>(std::numeric_limits<T>::lowest());
@@ -929,8 +1406,9 @@
return ceres::Jet<T, N>(std::numeric_limits<T>::denorm_min());
}
- static constexpr ceres::Jet<T, N> max() noexcept {
- return ceres::Jet<T, N>(std::numeric_limits<T>::max());
+ static constexpr ceres::Jet<T, N> max
+ CERES_PREVENT_MACRO_SUBSTITUTION() noexcept {
+ return ceres::Jet<T, N>((std::numeric_limits<T>::max)());
}
};
@@ -942,10 +1420,10 @@
// Eigen arrays, getting all the goodness of Eigen combined with autodiff.
template <typename T, int N>
struct NumTraits<ceres::Jet<T, N>> {
- typedef ceres::Jet<T, N> Real;
- typedef ceres::Jet<T, N> NonInteger;
- typedef ceres::Jet<T, N> Nested;
- typedef ceres::Jet<T, N> Literal;
+ using Real = ceres::Jet<T, N>;
+ using NonInteger = ceres::Jet<T, N>;
+ using Nested = ceres::Jet<T, N>;
+ using Literal = ceres::Jet<T, N>;
static typename ceres::Jet<T, N> dummy_precision() {
return ceres::Jet<T, N>(1e-12);
@@ -956,6 +1434,7 @@
}
static inline int digits10() { return NumTraits<T>::digits10(); }
+ static inline int max_digits10() { return NumTraits<T>::max_digits10(); }
enum {
IsComplex = 0,
@@ -984,8 +1463,8 @@
};
};
- static inline Real highest() { return Real(std::numeric_limits<T>::max()); }
- static inline Real lowest() { return Real(-std::numeric_limits<T>::max()); }
+ static inline Real highest() { return Real((std::numeric_limits<T>::max)()); }
+ static inline Real lowest() { return Real(-(std::numeric_limits<T>::max)()); }
};
// Specifying the return type of binary operations between Jets and scalar types
@@ -996,11 +1475,11 @@
// is only available on Eigen versions >= 3.3
template <typename BinaryOp, typename T, int N>
struct ScalarBinaryOpTraits<ceres::Jet<T, N>, T, BinaryOp> {
- typedef ceres::Jet<T, N> ReturnType;
+ using ReturnType = ceres::Jet<T, N>;
};
template <typename BinaryOp, typename T, int N>
struct ScalarBinaryOpTraits<T, ceres::Jet<T, N>, BinaryOp> {
- typedef ceres::Jet<T, N> ReturnType;
+ using ReturnType = ceres::Jet<T, N>;
};
} // namespace Eigen
diff --git a/include/ceres/jet_fwd.h b/include/ceres/jet_fwd.h
new file mode 100644
index 0000000..b5216da
--- /dev/null
+++ b/include/ceres/jet_fwd.h
@@ -0,0 +1,44 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2023 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: sergiu.deitsch@gmail.com (Sergiu Deitsch)
+//
+
+#ifndef CERES_PUBLIC_JET_FWD_H_
+#define CERES_PUBLIC_JET_FWD_H_
+
+namespace ceres {
+
+// Jet forward declaration necessary for the following partial specialization of
+// std::common_type and type traits.
+template <typename T, int N>
+struct Jet;
+
+} // namespace ceres
+
+#endif // CERES_PUBLIC_JET_FWD_H_
diff --git a/include/ceres/line_manifold.h b/include/ceres/line_manifold.h
new file mode 100644
index 0000000..dad9737
--- /dev/null
+++ b/include/ceres/line_manifold.h
@@ -0,0 +1,301 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2023 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: jodebo_beck@gmx.de (Johannes Beck)
+//
+
+#ifndef CERES_PUBLIC_LINE_MANIFOLD_H_
+#define CERES_PUBLIC_LINE_MANIFOLD_H_
+
+#include <Eigen/Core>
+#include <algorithm>
+#include <array>
+#include <memory>
+#include <vector>
+
+#include "ceres/internal/disable_warnings.h"
+#include "ceres/internal/export.h"
+#include "ceres/internal/householder_vector.h"
+#include "ceres/internal/sphere_manifold_functions.h"
+#include "ceres/manifold.h"
+#include "ceres/types.h"
+#include "glog/logging.h"
+
+namespace ceres {
+// This provides a manifold for lines, where the line is
+// over-parameterized by an origin point and a direction vector. So the
+// parameter vector size needs to be two times the ambient space dimension,
+// where the first half is interpreted as the origin point and the second half
+// as the direction.
+//
+// The plus operator for the line direction is the same as for the
+// SphereManifold. The update of the origin point is
+// perpendicular to the line direction before the update.
+//
+// This manifold is a special case of the affine Grassmannian
+// manifold (see https://en.wikipedia.org/wiki/Affine_Grassmannian_(manifold))
+// for the case Graff_1(R^n).
+//
+// The class works with dynamic and static ambient space dimensions. If the
+// ambient space dimensions is known at compile time use
+//
+// LineManifold<3> manifold;
+//
+// If the ambient space dimensions is not known at compile time the template
+// parameter needs to be set to ceres::DYNAMIC and the actual dimension needs
+// to be provided as a constructor argument:
+//
+// LineManifold<ceres::DYNAMIC> manifold(ambient_dim);
+//
+template <int AmbientSpaceDimension>
+class LineManifold final : public Manifold {
+ public:
+ static_assert(AmbientSpaceDimension == DYNAMIC || AmbientSpaceDimension >= 2,
+ "The ambient space must be at least 2.");
+ static_assert(ceres::DYNAMIC == Eigen::Dynamic,
+ "ceres::DYNAMIC needs to be the same as Eigen::Dynamic.");
+
+ LineManifold();
+ explicit LineManifold(int size);
+
+ int AmbientSize() const override { return 2 * size_; }
+ int TangentSize() const override { return 2 * (size_ - 1); }
+ bool Plus(const double* x,
+ const double* delta,
+ double* x_plus_delta) const override;
+ bool PlusJacobian(const double* x, double* jacobian) const override;
+ bool Minus(const double* y,
+ const double* x,
+ double* y_minus_x) const override;
+ bool MinusJacobian(const double* x, double* jacobian) const override;
+
+ private:
+ static constexpr bool IsDynamic = (AmbientSpaceDimension == ceres::DYNAMIC);
+ static constexpr int TangentSpaceDimension =
+ IsDynamic ? ceres::DYNAMIC : AmbientSpaceDimension - 1;
+
+ static constexpr int DAmbientSpaceDimension =
+ IsDynamic ? ceres::DYNAMIC : 2 * AmbientSpaceDimension;
+ static constexpr int DTangentSpaceDimension =
+ IsDynamic ? ceres::DYNAMIC : 2 * TangentSpaceDimension;
+
+ using AmbientVector = Eigen::Matrix<double, AmbientSpaceDimension, 1>;
+ using TangentVector = Eigen::Matrix<double, TangentSpaceDimension, 1>;
+ using MatrixPlusJacobian = Eigen::Matrix<double,
+ DAmbientSpaceDimension,
+ DTangentSpaceDimension,
+ Eigen::RowMajor>;
+ using MatrixMinusJacobian = Eigen::Matrix<double,
+ DTangentSpaceDimension,
+ DAmbientSpaceDimension,
+ Eigen::RowMajor>;
+
+ const int size_{AmbientSpaceDimension};
+};
+
+template <int AmbientSpaceDimension>
+LineManifold<AmbientSpaceDimension>::LineManifold()
+ : size_{AmbientSpaceDimension} {
+ static_assert(
+ AmbientSpaceDimension != Eigen::Dynamic,
+ "The size is set to dynamic. Please call the constructor with a size.");
+}
+
+template <int AmbientSpaceDimension>
+LineManifold<AmbientSpaceDimension>::LineManifold(int size) : size_{size} {
+ if (AmbientSpaceDimension != Eigen::Dynamic) {
+ CHECK_EQ(AmbientSpaceDimension, size)
+ << "Specified size by template parameter differs from the supplied "
+ "one.";
+ } else {
+ CHECK_GT(size_, 1)
+ << "The size of the manifold needs to be greater than 1.";
+ }
+}
+
+template <int AmbientSpaceDimension>
+bool LineManifold<AmbientSpaceDimension>::Plus(const double* x_ptr,
+ const double* delta_ptr,
+ double* x_plus_delta_ptr) const {
+ // We seek a box plus operator of the form
+ //
+ // [o*, d*] = Plus([o, d], [delta_o, delta_d])
+ //
+ // where o is the origin point, d is the direction vector, delta_o is
+ // the delta of the origin point and delta_d the delta of the direction and
+ // o* and d* is the updated origin point and direction.
+ //
+ // We separate the Plus operator into the origin point and directional part
+ // d* = Plus_d(d, delta_d)
+ // o* = Plus_o(o, d, delta_o)
+ //
+ // The direction update function Plus_d is the same as as the SphereManifold:
+ //
+ // d* = H_{v(d)} [sinc(|delta_d|) delta_d, cos(|delta_d|)]^T
+ //
+ // where H is the householder matrix
+ // H_{v} = I - (2 / |v|^2) v v^T
+ // and
+ // v(d) = d - sign(d_n) |d| e_n.
+ //
+ // The origin point update function Plus_o is defined as
+ //
+ // o* = o + H_{v(d)} [delta_o, 0]^T.
+
+ Eigen::Map<const AmbientVector> o(x_ptr, size_);
+ Eigen::Map<const AmbientVector> d(x_ptr + size_, size_);
+
+ Eigen::Map<const TangentVector> delta_o(delta_ptr, size_ - 1);
+ Eigen::Map<const TangentVector> delta_d(delta_ptr + size_ - 1, size_ - 1);
+ Eigen::Map<AmbientVector> o_plus_delta(x_plus_delta_ptr, size_);
+ Eigen::Map<AmbientVector> d_plus_delta(x_plus_delta_ptr + size_, size_);
+
+ const double norm_delta_d = delta_d.norm();
+
+ o_plus_delta = o;
+
+ // Shortcut for zero delta direction.
+ if (norm_delta_d == 0.0) {
+ d_plus_delta = d;
+
+ if (delta_o.isZero(0.0)) {
+ return true;
+ }
+ }
+
+ // Calculate the householder transformation which is needed for f_d and f_o.
+ AmbientVector v(size_);
+ double beta;
+
+ // NOTE: The explicit template arguments are needed here because
+ // ComputeHouseholderVector is templated and some versions of MSVC
+ // have trouble deducing the type of v automatically.
+ internal::ComputeHouseholderVector<Eigen::Map<const AmbientVector>,
+ double,
+ AmbientSpaceDimension>(d, &v, &beta);
+
+ if (norm_delta_d != 0.0) {
+ internal::ComputeSphereManifoldPlus(
+ v, beta, d, delta_d, norm_delta_d, &d_plus_delta);
+ }
+
+ // The null space is in the direction of the line, so the tangent space is
+ // perpendicular to the line direction. This is achieved by using the
+ // householder matrix of the direction and allow only movements
+ // perpendicular to e_n.
+ AmbientVector y(size_);
+ y << delta_o, 0;
+ o_plus_delta += internal::ApplyHouseholderVector(y, v, beta);
+
+ return true;
+}
+
+template <int AmbientSpaceDimension>
+bool LineManifold<AmbientSpaceDimension>::PlusJacobian(
+ const double* x_ptr, double* jacobian_ptr) const {
+ Eigen::Map<const AmbientVector> d(x_ptr + size_, size_);
+ Eigen::Map<MatrixPlusJacobian> jacobian(
+ jacobian_ptr, 2 * size_, 2 * (size_ - 1));
+
+ // Clear the Jacobian as only half of the matrix is not zero.
+ jacobian.setZero();
+
+ auto jacobian_d =
+ jacobian
+ .template topLeftCorner<AmbientSpaceDimension, TangentSpaceDimension>(
+ size_, size_ - 1);
+ auto jacobian_o = jacobian.template bottomRightCorner<AmbientSpaceDimension,
+ TangentSpaceDimension>(
+ size_, size_ - 1);
+ internal::ComputeSphereManifoldPlusJacobian(d, &jacobian_d);
+ jacobian_o = jacobian_d;
+ return true;
+}
+
+template <int AmbientSpaceDimension>
+bool LineManifold<AmbientSpaceDimension>::Minus(const double* y_ptr,
+ const double* x_ptr,
+ double* y_minus_x) const {
+ Eigen::Map<const AmbientVector> y_o(y_ptr, size_);
+ Eigen::Map<const AmbientVector> y_d(y_ptr + size_, size_);
+ Eigen::Map<const AmbientVector> x_o(x_ptr, size_);
+ Eigen::Map<const AmbientVector> x_d(x_ptr + size_, size_);
+
+ Eigen::Map<TangentVector> y_minus_x_o(y_minus_x, size_ - 1);
+ Eigen::Map<TangentVector> y_minus_x_d(y_minus_x + size_ - 1, size_ - 1);
+
+ AmbientVector v(size_);
+ double beta;
+
+ // NOTE: The explicit template arguments are needed here because
+ // ComputeHouseholderVector is templated and some versions of MSVC
+ // have trouble deducing the type of v automatically.
+ internal::ComputeHouseholderVector<Eigen::Map<const AmbientVector>,
+ double,
+ AmbientSpaceDimension>(x_d, &v, &beta);
+
+ internal::ComputeSphereManifoldMinus(v, beta, x_d, y_d, &y_minus_x_d);
+
+ AmbientVector delta_o = y_o - x_o;
+ const AmbientVector h_delta_o =
+ internal::ApplyHouseholderVector(delta_o, v, beta);
+ y_minus_x_o = h_delta_o.template head<TangentSpaceDimension>(size_ - 1);
+
+ return true;
+}
+
+template <int AmbientSpaceDimension>
+bool LineManifold<AmbientSpaceDimension>::MinusJacobian(
+ const double* x_ptr, double* jacobian_ptr) const {
+ Eigen::Map<const AmbientVector> d(x_ptr + size_, size_);
+ Eigen::Map<MatrixMinusJacobian> jacobian(
+ jacobian_ptr, 2 * (size_ - 1), 2 * size_);
+
+ // Clear the Jacobian as only half of the matrix is not zero.
+ jacobian.setZero();
+
+ auto jacobian_d =
+ jacobian
+ .template topLeftCorner<TangentSpaceDimension, AmbientSpaceDimension>(
+ size_ - 1, size_);
+ auto jacobian_o = jacobian.template bottomRightCorner<TangentSpaceDimension,
+ AmbientSpaceDimension>(
+ size_ - 1, size_);
+ internal::ComputeSphereManifoldMinusJacobian(d, &jacobian_d);
+ jacobian_o = jacobian_d;
+
+ return true;
+}
+
+} // namespace ceres
+
+// clang-format off
+#include "ceres/internal/reenable_warnings.h"
+// clang-format on
+
+#endif // CERES_PUBLIC_LINE_MANIFOLD_H_
diff --git a/include/ceres/local_parameterization.h b/include/ceres/local_parameterization.h
deleted file mode 100644
index ba7579d..0000000
--- a/include/ceres/local_parameterization.h
+++ /dev/null
@@ -1,363 +0,0 @@
-// Ceres Solver - A fast non-linear least squares minimizer
-// Copyright 2019 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 <array>
-#include <memory>
-#include <vector>
-
-#include "ceres/internal/disable_warnings.h"
-#include "ceres/internal/port.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 redundant 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 to 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 f 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() {}
- bool Plus(const double* x,
- const double* delta,
- double* x_plus_delta) const override;
- bool ComputeJacobian(const double* x, double* jacobian) const override;
- bool MultiplyByJacobian(const double* x,
- const int num_cols,
- const double* global_matrix,
- double* local_matrix) const override;
- int GlobalSize() const override { return size_; }
- int LocalSize() const override { 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() {}
- bool Plus(const double* x,
- const double* delta,
- double* x_plus_delta) const override;
- bool ComputeJacobian(const double* x, double* jacobian) const override;
- bool MultiplyByJacobian(const double* x,
- const int num_cols,
- const double* global_matrix,
- double* local_matrix) const override;
- int GlobalSize() const override {
- return static_cast<int>(constancy_mask_.size());
- }
- int LocalSize() const override { 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() {}
- bool Plus(const double* x,
- const double* delta,
- double* x_plus_delta) const override;
- bool ComputeJacobian(const double* x, double* jacobian) const override;
- int GlobalSize() const override { return 4; }
- int LocalSize() const override { return 3; }
-};
-
-// Implements the quaternion local parameterization for Eigen's representation
-// of the quaternion. Eigen uses a different internal memory layout for the
-// elements of the quaternion than what is commonly used. Specifically, Eigen
-// stores the elements in memory as [x, y, z, w] where the real part is last
-// whereas it is typically stored first. Note, when creating an Eigen quaternion
-// through the constructor the elements are accepted in w, x, y, z order. Since
-// Ceres operates on parameter blocks which are raw double pointers this
-// difference is important and requires a different parameterization.
-//
-// Plus(x, delta) = [sin(|delta|) delta / |delta|, cos(|delta|)] * x
-// with * being the quaternion multiplication operator.
-class CERES_EXPORT EigenQuaternionParameterization
- : public ceres::LocalParameterization {
- public:
- virtual ~EigenQuaternionParameterization() {}
- bool Plus(const double* x,
- const double* delta,
- double* x_plus_delta) const override;
- bool ComputeJacobian(const double* x, double* jacobian) const override;
- int GlobalSize() const override { return 4; }
- int LocalSize() const override { 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() {}
- bool Plus(const double* x,
- const double* delta,
- double* x_plus_delta) const override;
- bool ComputeJacobian(const double* x, double* jacobian) const override;
- int GlobalSize() const override { return size_; }
- int LocalSize() const override { return size_ - 1; }
-
- private:
- const int size_;
-};
-
-// This provides a parameterization for lines, where the line is
-// over-parameterized by an origin point and a direction vector. So the
-// parameter vector size needs to be two times the ambient space dimension,
-// where the first half is interpreted as the origin point and the second half
-// as the direction.
-//
-// The plus operator for the line direction is the same as for the
-// HomogeneousVectorParameterization. The update of the origin point is
-// perpendicular to the line direction before the update.
-//
-// This local parameterization is a special case of the affine Grassmannian
-// manifold (see https://en.wikipedia.org/wiki/Affine_Grassmannian_(manifold))
-// for the case Graff_1(R^n).
-template <int AmbientSpaceDimension>
-class LineParameterization : public LocalParameterization {
- public:
- static_assert(AmbientSpaceDimension >= 2,
- "The ambient space must be at least 2");
-
- bool Plus(const double* x,
- const double* delta,
- double* x_plus_delta) const override;
- bool ComputeJacobian(const double* x, double* jacobian) const override;
- int GlobalSize() const override { return 2 * AmbientSpaceDimension; }
- int LocalSize() const override { return 2 * (AmbientSpaceDimension - 1); }
-};
-
-// 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.
-//
-// 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:
- ProductParameterization(const ProductParameterization&) = delete;
- ProductParameterization& operator=(const ProductParameterization&) = delete;
- virtual ~ProductParameterization() {}
- //
- // NOTE: The constructor takes ownership of the input local
- // parameterizations.
- //
- template <typename... LocalParams>
- ProductParameterization(LocalParams*... local_params)
- : local_params_(sizeof...(LocalParams)),
- local_size_{0},
- global_size_{0},
- buffer_size_{0} {
- constexpr int kNumLocalParams = sizeof...(LocalParams);
- static_assert(kNumLocalParams >= 2,
- "At least two local parameterizations must be specified.");
-
- using LocalParameterizationPtr = std::unique_ptr<LocalParameterization>;
-
- // Wrap all raw pointers into std::unique_ptr for exception safety.
- std::array<LocalParameterizationPtr, kNumLocalParams> local_params_array{
- LocalParameterizationPtr(local_params)...};
-
- // Initialize internal state.
- for (int i = 0; i < kNumLocalParams; ++i) {
- LocalParameterizationPtr& param = local_params_[i];
- param = std::move(local_params_array[i]);
-
- buffer_size_ =
- std::max(buffer_size_, param->LocalSize() * param->GlobalSize());
- global_size_ += param->GlobalSize();
- local_size_ += param->LocalSize();
- }
- }
-
- bool Plus(const double* x,
- const double* delta,
- double* x_plus_delta) const override;
- bool ComputeJacobian(const double* x,
- double* jacobian) const override;
- int GlobalSize() const override { return global_size_; }
- int LocalSize() const override { return local_size_; }
-
- private:
- std::vector<std::unique_ptr<LocalParameterization>> local_params_;
- int local_size_;
- int global_size_;
- int buffer_size_;
-};
-
-} // namespace ceres
-
-// clang-format off
-#include "ceres/internal/reenable_warnings.h"
-#include "ceres/internal/line_parameterization.h"
-
-#endif // CERES_PUBLIC_LOCAL_PARAMETERIZATION_H_
diff --git a/include/ceres/loss_function.h b/include/ceres/loss_function.h
index 7aabf7d..b8582f8 100644
--- a/include/ceres/loss_function.h
+++ b/include/ceres/loss_function.h
@@ -1,5 +1,5 @@
// Ceres Solver - A fast non-linear least squares minimizer
-// Copyright 2019 Google Inc. All rights reserved.
+// Copyright 2023 Google Inc. All rights reserved.
// http://ceres-solver.org/
//
// Redistribution and use in source and binary forms, with or without
@@ -35,7 +35,7 @@
//
// 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
+// function; instead passing a nullptr to the problem when adding
// residuals implies a standard squared loss.
//
// For least squares problems where the minimization may encounter
@@ -78,6 +78,7 @@
#include <memory>
#include "ceres/internal/disable_warnings.h"
+#include "ceres/internal/export.h"
#include "ceres/types.h"
#include "glog/logging.h"
@@ -85,7 +86,7 @@
class CERES_EXPORT LossFunction {
public:
- virtual ~LossFunction() {}
+ 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
@@ -125,10 +126,10 @@
//
// At s = 0: rho = [0, 1, 0].
//
-// It is not normally necessary to use this, as passing NULL for the
+// It is not normally necessary to use this, as passing nullptr for the
// loss function when building the problem accomplishes the same
// thing.
-class CERES_EXPORT TrivialLoss : public LossFunction {
+class CERES_EXPORT TrivialLoss final : public LossFunction {
public:
void Evaluate(double, double*) const override;
};
@@ -171,7 +172,7 @@
//
// The scaling parameter 'a' corresponds to 'delta' on this page:
// http://en.wikipedia.org/wiki/Huber_Loss_Function
-class CERES_EXPORT HuberLoss : public LossFunction {
+class CERES_EXPORT HuberLoss final : public LossFunction {
public:
explicit HuberLoss(double a) : a_(a), b_(a * a) {}
void Evaluate(double, double*) const override;
@@ -187,7 +188,7 @@
// rho(s) = 2 (sqrt(1 + s) - 1).
//
// At s = 0: rho = [0, 1, -1 / (2 * a^2)].
-class CERES_EXPORT SoftLOneLoss : public LossFunction {
+class CERES_EXPORT SoftLOneLoss final : public LossFunction {
public:
explicit SoftLOneLoss(double a) : b_(a * a), c_(1 / b_) {}
void Evaluate(double, double*) const override;
@@ -204,7 +205,7 @@
// rho(s) = log(1 + s).
//
// At s = 0: rho = [0, 1, -1 / a^2].
-class CERES_EXPORT CauchyLoss : public LossFunction {
+class CERES_EXPORT CauchyLoss final : public LossFunction {
public:
explicit CauchyLoss(double a) : b_(a * a), c_(1 / b_) {}
void Evaluate(double, double*) const override;
@@ -225,7 +226,7 @@
// rho(s) = a atan(s / a).
//
// At s = 0: rho = [0, 1, 0].
-class CERES_EXPORT ArctanLoss : public LossFunction {
+class CERES_EXPORT ArctanLoss final : public LossFunction {
public:
explicit ArctanLoss(double a) : a_(a), b_(1 / (a * a)) {}
void Evaluate(double, double*) const override;
@@ -264,7 +265,7 @@
// concentrated in the range a - b to a + b.
//
// At s = 0: rho = [0, ~0, ~0].
-class CERES_EXPORT TolerantLoss : public LossFunction {
+class CERES_EXPORT TolerantLoss final : public LossFunction {
public:
explicit TolerantLoss(double a, double b);
void Evaluate(double, double*) const override;
@@ -283,7 +284,7 @@
// rho(s) = a^2 / 3 for s > a^2.
//
// At s = 0: rho = [0, 1, -2 / a^2]
-class CERES_EXPORT TukeyLoss : public ceres::LossFunction {
+class CERES_EXPORT TukeyLoss final : public ceres::LossFunction {
public:
explicit TukeyLoss(double a) : a_squared_(a * a) {}
void Evaluate(double, double*) const override;
@@ -294,14 +295,14 @@
// 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 {
+// The loss functions must not be nullptr.
+class CERES_EXPORT ComposedLoss final : public LossFunction {
public:
explicit ComposedLoss(const LossFunction* f,
Ownership ownership_f,
const LossFunction* g,
Ownership ownership_g);
- virtual ~ComposedLoss();
+ ~ComposedLoss() override;
void Evaluate(double, double*) const override;
private:
@@ -322,11 +323,11 @@
// 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
+// Since we treat the a nullptr Loss function as the Identity loss
+// function, rho = nullptr 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 {
+class CERES_EXPORT ScaledLoss final : public LossFunction {
public:
// Constructs a ScaledLoss wrapping another loss function. Takes
// ownership of the wrapped loss function or not depending on the
@@ -336,7 +337,7 @@
ScaledLoss(const ScaledLoss&) = delete;
void operator=(const ScaledLoss&) = delete;
- virtual ~ScaledLoss() {
+ ~ScaledLoss() override {
if (ownership_ == DO_NOT_TAKE_OWNERSHIP) {
rho_.release();
}
@@ -361,8 +362,8 @@
// 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.
+// Since we treat the a nullptr Loss function as the Identity loss
+// function, rho = nullptr is a valid input.
//
// Example usage
//
@@ -374,7 +375,8 @@
// 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);
+// LossFunctionWrapper* loss_function = new LossFunctionWrapper(
+// new HuberLoss(1.0), TAKE_OWNERSHIP);
//
// problem.AddResidualBlock(cost_function, loss_function, parameters);
//
@@ -387,7 +389,7 @@
//
// Solve(options, &problem, &summary)
//
-class CERES_EXPORT LossFunctionWrapper : public LossFunction {
+class CERES_EXPORT LossFunctionWrapper final : public LossFunction {
public:
LossFunctionWrapper(LossFunction* rho, Ownership ownership)
: rho_(rho), ownership_(ownership) {}
@@ -395,14 +397,14 @@
LossFunctionWrapper(const LossFunctionWrapper&) = delete;
void operator=(const LossFunctionWrapper&) = delete;
- virtual ~LossFunctionWrapper() {
+ ~LossFunctionWrapper() override {
if (ownership_ == DO_NOT_TAKE_OWNERSHIP) {
rho_.release();
}
}
void Evaluate(double sq_norm, double out[3]) const override {
- if (rho_.get() == NULL) {
+ if (rho_.get() == nullptr) {
out[0] = sq_norm;
out[1] = 1.0;
out[2] = 0.0;
diff --git a/include/ceres/manifold.h b/include/ceres/manifold.h
new file mode 100644
index 0000000..9bd6459
--- /dev/null
+++ b/include/ceres/manifold.h
@@ -0,0 +1,411 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2023 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_MANIFOLD_H_
+#define CERES_PUBLIC_MANIFOLD_H_
+
+#include <Eigen/Core>
+#include <algorithm>
+#include <array>
+#include <memory>
+#include <utility>
+#include <vector>
+
+#include "ceres/internal/disable_warnings.h"
+#include "ceres/internal/export.h"
+#include "ceres/types.h"
+#include "glog/logging.h"
+
+namespace ceres {
+
+// In sensor fusion problems, often we have to model quantities that live in
+// spaces known as Manifolds, for example the rotation/orientation of a sensor
+// that is represented by a quaternion.
+//
+// Manifolds are spaces which locally look like Euclidean spaces. More
+// precisely, at each point on the manifold there is a linear space that is
+// tangent to the manifold. It has dimension equal to the intrinsic dimension of
+// the manifold itself, which is less than or equal to the ambient space in
+// which the manifold is embedded.
+//
+// For example, the tangent space to a point on a sphere in three dimensions is
+// the two dimensional plane that is tangent to the sphere at that point. There
+// are two reasons tangent spaces are interesting:
+//
+// 1. They are Eucliean spaces so the usual vector space operations apply there,
+// which makes numerical operations easy.
+// 2. Movement in the tangent space translate into movements along the manifold.
+// Movements perpendicular to the tangent space do not translate into
+// movements on the manifold.
+//
+// Returning to our sphere example, moving in the 2 dimensional plane
+// tangent to the sphere and projecting back onto the sphere will move you away
+// from the point you started from but moving along the normal at the same point
+// and the projecting back onto the sphere brings you back to the point.
+//
+// The Manifold interface defines two operations (and their derivatives)
+// involving the tangent space, allowing filtering and optimization to be
+// performed on said manifold:
+//
+// 1. x_plus_delta = Plus(x, delta)
+// 2. delta = Minus(x_plus_delta, x)
+//
+// "Plus" computes the result of moving along delta in the tangent space at x,
+// and then projecting back onto the manifold that x belongs to. In Differential
+// Geometry this is known as a "Retraction". It is a generalization of vector
+// addition in Euclidean spaces.
+//
+// Given two points on the manifold, "Minus" computes the change delta to x in
+// the tangent space at x, that will take it to x_plus_delta.
+//
+// Let us now consider two examples.
+//
+// The Euclidean space R^n is the simplest example of a manifold. It has
+// dimension n (and so does its tangent space) and Plus and Minus are the
+// familiar vector sum and difference operations.
+//
+// Plus(x, delta) = x + delta = y,
+// Minus(y, x) = y - x = delta.
+//
+// A more interesting case is SO(3), the special orthogonal group in three
+// dimensions - the space of 3x3 rotation matrices. SO(3) is a three dimensional
+// manifold embedded in R^9 or R^(3x3). So points on SO(3) are represented using
+// 9 dimensional vectors or 3x3 matrices, and points in its tangent spaces are
+// represented by 3 dimensional vectors.
+//
+// Defining Plus and Minus are defined in terms of the matrix Exp and Log
+// operations as follows:
+//
+// Let Exp(p, q, r) = [cos(theta) + cp^2, -sr + cpq , sq + cpr ]
+// [sr + cpq , cos(theta) + cq^2, -sp + cqr ]
+// [-sq + cpr , sp + cqr , cos(theta) + cr^2]
+//
+// where: theta = sqrt(p^2 + q^2 + r^2)
+// s = sinc(theta)
+// c = (1 - cos(theta))/theta^2
+//
+// and Log(x) = 1/(2 sinc(theta))[x_32 - x_23, x_13 - x_31, x_21 - x_12]
+//
+// where: theta = acos((Trace(x) - 1)/2)
+//
+// Then,
+//
+// Plus(x, delta) = x Exp(delta)
+// Minus(y, x) = Log(x^T y)
+//
+// For Plus and Minus to be mathematically consistent, the following identities
+// must be satisfied at all points x on the manifold:
+//
+// 1. Plus(x, 0) = x.
+// 2. For all y, Plus(x, Minus(y, x)) = y.
+// 3. For all delta, Minus(Plus(x, delta), x) = delta.
+// 4. For all delta_1, delta_2
+// |Minus(Plus(x, delta_1), Plus(x, delta_2)) <= |delta_1 - delta_2|
+//
+// Briefly:
+// (1) Ensures that the tangent space is "centered" at x, and the zero vector is
+// the identity element.
+// (2) Ensures that any y can be reached from x.
+// (3) Ensures that Plus is an injective (one-to-one) map.
+// (4) Allows us to define a metric on the manifold.
+//
+// Additionally we require that Plus and Minus be sufficiently smooth. In
+// particular they need to be differentiable everywhere on the manifold.
+//
+// For more details, please see
+//
+// "Integrating Generic Sensor Fusion Algorithms with Sound State
+// Representations through Encapsulation of Manifolds"
+// By C. Hertzberg, R. Wagner, U. Frese and L. Schroder
+// https://arxiv.org/pdf/1107.1119.pdf
+class CERES_EXPORT Manifold {
+ public:
+ virtual ~Manifold();
+
+ // Dimension of the ambient space in which the manifold is embedded.
+ virtual int AmbientSize() const = 0;
+
+ // Dimension of the manifold/tangent space.
+ virtual int TangentSize() const = 0;
+
+ // x_plus_delta = Plus(x, delta),
+ //
+ // A generalization of vector addition in Euclidean space, Plus computes the
+ // result of moving along delta in the tangent space at x, and then projecting
+ // back onto the manifold that x belongs to.
+ //
+ // x and x_plus_delta are AmbientSize() vectors.
+ // delta is a TangentSize() vector.
+ //
+ // Return value indicates if the operation was successful or not.
+ virtual bool Plus(const double* x,
+ const double* delta,
+ double* x_plus_delta) const = 0;
+
+ // Compute the derivative of Plus(x, delta) w.r.t delta at delta = 0, i.e.
+ //
+ // (D_2 Plus)(x, 0)
+ //
+ // jacobian is a row-major AmbientSize() x TangentSize() matrix.
+ //
+ // Return value indicates whether the operation was successful or not.
+ virtual bool PlusJacobian(const double* x, double* jacobian) const = 0;
+
+ // tangent_matrix = ambient_matrix * (D_2 Plus)(x, 0)
+ //
+ // ambient_matrix is a row-major num_rows x AmbientSize() matrix.
+ // tangent_matrix is a row-major num_rows x TangentSize() matrix.
+ //
+ // Return value indicates whether the operation was successful or not.
+ //
+ // This function is only used by the GradientProblemSolver, where the
+ // dimension of the parameter block can be large and it may be more efficient
+ // to compute this product directly rather than first evaluating the Jacobian
+ // into a matrix and then doing a matrix vector product.
+ //
+ // Because this is not an often used function, we provide a default
+ // implementation for convenience. If performance becomes an issue then the
+ // user should consider implementing a specialization.
+ virtual bool RightMultiplyByPlusJacobian(const double* x,
+ const int num_rows,
+ const double* ambient_matrix,
+ double* tangent_matrix) const;
+
+ // y_minus_x = Minus(y, x)
+ //
+ // Given two points on the manifold, Minus computes the change to x in the
+ // tangent space at x, that will take it to y.
+ //
+ // x and y are AmbientSize() vectors.
+ // y_minus_x is a TangentSize() vector.
+ //
+ // Return value indicates if the operation was successful or not.
+ virtual bool Minus(const double* y,
+ const double* x,
+ double* y_minus_x) const = 0;
+
+ // Compute the derivative of Minus(y, x) w.r.t y at y = x, i.e
+ //
+ // (D_1 Minus) (x, x)
+ //
+ // Jacobian is a row-major TangentSize() x AmbientSize() matrix.
+ //
+ // Return value indicates whether the operation was successful or not.
+ virtual bool MinusJacobian(const double* x, double* jacobian) const = 0;
+};
+
+// The Euclidean manifold is another name for the ordinary vector space R^size,
+// where the plus and minus operations are the usual vector addition and
+// subtraction:
+// Plus(x, delta) = x + delta
+// Minus(y, x) = y - x.
+//
+// The class works with dynamic and static ambient space dimensions. If the
+// ambient space dimensions is know at compile time use
+//
+// EuclideanManifold<3> manifold;
+//
+// If the ambient space dimensions is not known at compile time the template
+// parameter needs to be set to ceres::DYNAMIC and the actual dimension needs
+// to be provided as a constructor argument:
+//
+// EuclideanManifold<ceres::DYNAMIC> manifold(ambient_dim);
+template <int Size>
+class EuclideanManifold final : public Manifold {
+ public:
+ static_assert(Size == ceres::DYNAMIC || Size >= 0,
+ "The size of the manifold needs to be non-negative.");
+ static_assert(ceres::DYNAMIC == Eigen::Dynamic,
+ "ceres::DYNAMIC needs to be the same as Eigen::Dynamic.");
+
+ EuclideanManifold() : size_{Size} {
+ static_assert(
+ Size != ceres::DYNAMIC,
+ "The size is set to dynamic. Please call the constructor with a size.");
+ }
+
+ explicit EuclideanManifold(int size) : size_(size) {
+ if (Size != ceres::DYNAMIC) {
+ CHECK_EQ(Size, size)
+ << "Specified size by template parameter differs from the supplied "
+ "one.";
+ } else {
+ CHECK_GE(size_, 0)
+ << "The size of the manifold needs to be non-negative.";
+ }
+ }
+
+ int AmbientSize() const override { return size_; }
+ int TangentSize() const override { return size_; }
+
+ bool Plus(const double* x_ptr,
+ const double* delta_ptr,
+ double* x_plus_delta_ptr) const override {
+ Eigen::Map<const AmbientVector> x(x_ptr, size_);
+ Eigen::Map<const AmbientVector> delta(delta_ptr, size_);
+ Eigen::Map<AmbientVector> x_plus_delta(x_plus_delta_ptr, size_);
+ x_plus_delta = x + delta;
+ return true;
+ }
+
+ bool PlusJacobian(const double* x_ptr, double* jacobian_ptr) const override {
+ Eigen::Map<MatrixJacobian> jacobian(jacobian_ptr, size_, size_);
+ jacobian.setIdentity();
+ return true;
+ }
+
+ bool RightMultiplyByPlusJacobian(const double* x,
+ const int num_rows,
+ const double* ambient_matrix,
+ double* tangent_matrix) const override {
+ std::copy_n(ambient_matrix, num_rows * size_, tangent_matrix);
+ return true;
+ }
+
+ bool Minus(const double* y_ptr,
+ const double* x_ptr,
+ double* y_minus_x_ptr) const override {
+ Eigen::Map<const AmbientVector> x(x_ptr, size_);
+ Eigen::Map<const AmbientVector> y(y_ptr, size_);
+ Eigen::Map<AmbientVector> y_minus_x(y_minus_x_ptr, size_);
+ y_minus_x = y - x;
+ return true;
+ }
+
+ bool MinusJacobian(const double* x_ptr, double* jacobian_ptr) const override {
+ Eigen::Map<MatrixJacobian> jacobian(jacobian_ptr, size_, size_);
+ jacobian.setIdentity();
+ return true;
+ }
+
+ private:
+ static constexpr bool IsDynamic = (Size == ceres::DYNAMIC);
+ using AmbientVector = Eigen::Matrix<double, Size, 1>;
+ using MatrixJacobian = Eigen::Matrix<double, Size, Size, Eigen::RowMajor>;
+
+ int size_{};
+};
+
+// Hold a subset of the parameters inside a parameter block constant.
+class CERES_EXPORT SubsetManifold final : public Manifold {
+ public:
+ SubsetManifold(int size, const std::vector<int>& constant_parameters);
+ int AmbientSize() const override;
+ int TangentSize() const override;
+
+ bool Plus(const double* x,
+ const double* delta,
+ double* x_plus_delta) const override;
+ bool PlusJacobian(const double* x, double* jacobian) const override;
+ bool RightMultiplyByPlusJacobian(const double* x,
+ const int num_rows,
+ const double* ambient_matrix,
+ double* tangent_matrix) const override;
+ bool Minus(const double* y,
+ const double* x,
+ double* y_minus_x) const override;
+ bool MinusJacobian(const double* x, double* jacobian) const override;
+
+ private:
+ const int tangent_size_ = 0;
+ std::vector<bool> constancy_mask_;
+};
+
+// Implements the manifold for a Hamilton quaternion as defined in
+// https://en.wikipedia.org/wiki/Quaternion. Quaternions are represented as
+// unit norm 4-vectors, i.e.
+//
+// q = [q0; q1; q2; q3], |q| = 1
+//
+// is the ambient space representation.
+//
+// q0 scalar part.
+// q1 coefficient of i.
+// q2 coefficient of j.
+// q3 coefficient of k.
+//
+// where: i*i = j*j = k*k = -1 and i*j = k, j*k = i, k*i = j.
+//
+// The tangent space is R^3, which relates to the ambient space through the
+// Plus and Minus operations defined as:
+//
+// Plus(x, delta) = [cos(|delta|); sin(|delta|) * delta / |delta|] * x
+// Minus(y, x) = to_delta(y * x^{-1})
+//
+// where "*" is the quaternion product and because q is a unit quaternion
+// (|q|=1), q^-1 = [q0; -q1; -q2; -q3]
+//
+// and to_delta( [q0; u_{3x1}] ) = u / |u| * atan2(|u|, q0)
+class CERES_EXPORT QuaternionManifold final : public Manifold {
+ public:
+ int AmbientSize() const override { return 4; }
+ int TangentSize() const override { return 3; }
+
+ bool Plus(const double* x,
+ const double* delta,
+ double* x_plus_delta) const override;
+ bool PlusJacobian(const double* x, double* jacobian) const override;
+ bool Minus(const double* y,
+ const double* x,
+ double* y_minus_x) const override;
+ bool MinusJacobian(const double* x, double* jacobian) const override;
+};
+
+// Implements the quaternion manifold for Eigen's representation of the
+// Hamilton quaternion. Geometrically it is exactly the same as the
+// QuaternionManifold defined above. However, Eigen uses a different internal
+// memory layout for the elements of the quaternion than what is commonly
+// used. It stores the quaternion in memory as [q1, q2, q3, q0] or
+// [x, y, z, w] where the real (scalar) part is last.
+//
+// Since Ceres operates on parameter blocks which are raw double pointers this
+// difference is important and requires a different manifold.
+class CERES_EXPORT EigenQuaternionManifold final : public Manifold {
+ public:
+ int AmbientSize() const override { return 4; }
+ int TangentSize() const override { return 3; }
+
+ bool Plus(const double* x,
+ const double* delta,
+ double* x_plus_delta) const override;
+ bool PlusJacobian(const double* x, double* jacobian) const override;
+ bool Minus(const double* y,
+ const double* x,
+ double* y_minus_x) const override;
+ bool MinusJacobian(const double* x, double* jacobian) const override;
+};
+
+} // namespace ceres
+
+// clang-format off
+#include "ceres/internal/reenable_warnings.h"
+// clang-format on
+
+#endif // CERES_PUBLIC_MANIFOLD_H_
diff --git a/include/ceres/manifold_test_utils.h b/include/ceres/manifold_test_utils.h
new file mode 100644
index 0000000..3e61457
--- /dev/null
+++ b/include/ceres/manifold_test_utils.h
@@ -0,0 +1,345 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2023 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)
+
+#include <cmath>
+#include <limits>
+#include <memory>
+
+#include "ceres/dynamic_numeric_diff_cost_function.h"
+#include "ceres/internal/eigen.h"
+#include "ceres/manifold.h"
+#include "ceres/numeric_diff_options.h"
+#include "ceres/types.h"
+#include "gmock/gmock.h"
+#include "gtest/gtest.h"
+
+namespace ceres {
+
+// Matchers and macros to simplify testing of custom Manifold objects using the
+// gtest testing framework.
+//
+// Testing a Manifold has two parts.
+//
+// 1. Checking that Manifold::Plus() and Manifold::Minus() are correctly
+// defined. This requires per manifold tests.
+//
+// 2. The other methods of the manifold have mathematical properties that make
+// them compatible with Plus() and Minus(), as described in [1].
+//
+// To verify these general requirements for a custom Manifold, use the
+// EXPECT_THAT_MANIFOLD_INVARIANTS_HOLD() macro from within a gtest test. Note
+// that additional domain-specific tests may also be prudent, e.g to verify the
+// behaviour of a Quaternion Manifold about pi.
+//
+// [1] "Integrating Generic Sensor Fusion Algorithms with Sound State
+// Representations through Encapsulation of Manifolds", C. Hertzberg,
+// R. Wagner, U. Frese and L. Schroder, https://arxiv.org/pdf/1107.1119.pdf
+
+// Verifies the general requirements for a custom Manifold are satisfied to
+// within the specified (numerical) tolerance.
+//
+// Example usage for a custom Manifold: ExampleManifold:
+//
+// TEST(ExampleManifold, ManifoldInvariantsHold) {
+// constexpr double kTolerance = 1.0e-9;
+// ExampleManifold manifold;
+// ceres::Vector x = ceres::Vector::Zero(manifold.AmbientSize());
+// ceres::Vector y = ceres::Vector::Zero(manifold.AmbientSize());
+// ceres::Vector delta = ceres::Vector::Zero(manifold.TangentSize());
+// EXPECT_THAT_MANIFOLD_INVARIANTS_HOLD(manifold, x, delta, y, kTolerance);
+// }
+#define EXPECT_THAT_MANIFOLD_INVARIANTS_HOLD(manifold, x, delta, y, tolerance) \
+ ::ceres::Vector zero_tangent = \
+ ::ceres::Vector::Zero(manifold.TangentSize()); \
+ EXPECT_THAT(manifold, ::ceres::XPlusZeroIsXAt(x, tolerance)); \
+ EXPECT_THAT(manifold, ::ceres::XMinusXIsZeroAt(x, tolerance)); \
+ EXPECT_THAT(manifold, ::ceres::MinusPlusIsIdentityAt(x, delta, tolerance)); \
+ EXPECT_THAT(manifold, \
+ ::ceres::MinusPlusIsIdentityAt(x, zero_tangent, tolerance)); \
+ EXPECT_THAT(manifold, ::ceres::PlusMinusIsIdentityAt(x, x, tolerance)); \
+ EXPECT_THAT(manifold, ::ceres::PlusMinusIsIdentityAt(x, y, tolerance)); \
+ EXPECT_THAT(manifold, ::ceres::HasCorrectPlusJacobianAt(x, tolerance)); \
+ EXPECT_THAT(manifold, ::ceres::HasCorrectMinusJacobianAt(x, tolerance)); \
+ EXPECT_THAT(manifold, ::ceres::MinusPlusJacobianIsIdentityAt(x, tolerance)); \
+ EXPECT_THAT(manifold, \
+ ::ceres::HasCorrectRightMultiplyByPlusJacobianAt(x, tolerance));
+
+// Checks that the invariant Plus(x, 0) == x holds.
+MATCHER_P2(XPlusZeroIsXAt, x, tolerance, "") {
+ const int ambient_size = arg.AmbientSize();
+ const int tangent_size = arg.TangentSize();
+
+ Vector actual = Vector::Zero(ambient_size);
+ Vector zero = Vector::Zero(tangent_size);
+ EXPECT_TRUE(arg.Plus(x.data(), zero.data(), actual.data()));
+ const double n = (actual - Vector{x}).norm();
+ const double d = x.norm();
+ const double diffnorm = (d == 0.0) ? n : (n / d);
+ if (diffnorm > tolerance) {
+ *result_listener << "\nexpected (x): " << x.transpose()
+ << "\nactual: " << actual.transpose()
+ << "\ndiffnorm: " << diffnorm;
+ return false;
+ }
+ return true;
+}
+
+// Checks that the invariant Minus(x, x) == 0 holds.
+MATCHER_P2(XMinusXIsZeroAt, x, tolerance, "") {
+ const int tangent_size = arg.TangentSize();
+ Vector actual = Vector::Zero(tangent_size);
+ EXPECT_TRUE(arg.Minus(x.data(), x.data(), actual.data()));
+ const double diffnorm = actual.norm();
+ if (diffnorm > tolerance) {
+ *result_listener << "\nx: " << x.transpose() //
+ << "\nexpected: 0 0 0"
+ << "\nactual: " << actual.transpose()
+ << "\ndiffnorm: " << diffnorm;
+ return false;
+ }
+ return true;
+}
+
+// Helper struct to curry Plus(x, .) so that it can be numerically
+// differentiated.
+struct PlusFunctor {
+ PlusFunctor(const Manifold& manifold, const double* x)
+ : manifold(manifold), x(x) {}
+ bool operator()(double const* const* parameters, double* x_plus_delta) const {
+ return manifold.Plus(x, parameters[0], x_plus_delta);
+ }
+
+ const Manifold& manifold;
+ const double* x;
+};
+
+// Checks that the output of PlusJacobian matches the one obtained by
+// numerically evaluating D_2 Plus(x,0).
+MATCHER_P2(HasCorrectPlusJacobianAt, x, tolerance, "") {
+ const int ambient_size = arg.AmbientSize();
+ const int tangent_size = arg.TangentSize();
+
+ NumericDiffOptions options;
+ options.ridders_relative_initial_step_size = 1e-4;
+
+ DynamicNumericDiffCostFunction<PlusFunctor, RIDDERS> cost_function(
+ new PlusFunctor(arg, x.data()), TAKE_OWNERSHIP, options);
+ cost_function.AddParameterBlock(tangent_size);
+ cost_function.SetNumResiduals(ambient_size);
+
+ Vector zero = Vector::Zero(tangent_size);
+ double* parameters[1] = {zero.data()};
+
+ Vector x_plus_zero = Vector::Zero(ambient_size);
+ Matrix expected = Matrix::Zero(ambient_size, tangent_size);
+ double* jacobians[1] = {expected.data()};
+
+ EXPECT_TRUE(
+ cost_function.Evaluate(parameters, x_plus_zero.data(), jacobians));
+
+ Matrix actual = Matrix::Random(ambient_size, tangent_size);
+ EXPECT_TRUE(arg.PlusJacobian(x.data(), actual.data()));
+
+ const double n = (actual - expected).norm();
+ const double d = expected.norm();
+ const double diffnorm = (d == 0.0) ? n : n / d;
+ if (diffnorm > tolerance) {
+ *result_listener << "\nx: " << x.transpose() << "\nexpected: \n"
+ << expected << "\nactual:\n"
+ << actual << "\ndiff:\n"
+ << expected - actual << "\ndiffnorm : " << diffnorm;
+ return false;
+ }
+ return true;
+}
+
+// Checks that the invariant Minus(Plus(x, delta), x) == delta holds.
+MATCHER_P3(MinusPlusIsIdentityAt, x, delta, tolerance, "") {
+ const int ambient_size = arg.AmbientSize();
+ const int tangent_size = arg.TangentSize();
+ Vector x_plus_delta = Vector::Zero(ambient_size);
+ EXPECT_TRUE(arg.Plus(x.data(), delta.data(), x_plus_delta.data()));
+ Vector actual = Vector::Zero(tangent_size);
+ EXPECT_TRUE(arg.Minus(x_plus_delta.data(), x.data(), actual.data()));
+
+ const double n = (actual - Vector{delta}).norm();
+ const double d = delta.norm();
+ const double diffnorm = (d == 0.0) ? n : (n / d);
+ if (diffnorm > tolerance) {
+ *result_listener << "\nx: " << x.transpose()
+ << "\nexpected: " << delta.transpose()
+ << "\nactual:" << actual.transpose()
+ << "\ndiff:" << (delta - actual).transpose()
+ << "\ndiffnorm: " << diffnorm;
+ return false;
+ }
+ return true;
+}
+
+// Checks that the invariant Plus(Minus(y, x), x) == y holds.
+MATCHER_P3(PlusMinusIsIdentityAt, x, y, tolerance, "") {
+ const int ambient_size = arg.AmbientSize();
+ const int tangent_size = arg.TangentSize();
+
+ Vector y_minus_x = Vector::Zero(tangent_size);
+ EXPECT_TRUE(arg.Minus(y.data(), x.data(), y_minus_x.data()));
+
+ Vector actual = Vector::Zero(ambient_size);
+ EXPECT_TRUE(arg.Plus(x.data(), y_minus_x.data(), actual.data()));
+
+ const double n = (actual - Vector{y}).norm();
+ const double d = y.norm();
+ const double diffnorm = (d == 0.0) ? n : (n / d);
+ if (diffnorm > tolerance) {
+ *result_listener << "\nx: " << x.transpose()
+ << "\nexpected: " << y.transpose()
+ << "\nactual:" << actual.transpose()
+ << "\ndiff:" << (y - actual).transpose()
+ << "\ndiffnorm: " << diffnorm;
+ return false;
+ }
+ return true;
+}
+
+// Helper struct to curry Minus(., x) so that it can be numerically
+// differentiated.
+struct MinusFunctor {
+ MinusFunctor(const Manifold& manifold, const double* x)
+ : manifold(manifold), x(x) {}
+ bool operator()(double const* const* parameters, double* y_minus_x) const {
+ return manifold.Minus(parameters[0], x, y_minus_x);
+ }
+
+ const Manifold& manifold;
+ const double* x;
+};
+
+// Checks that the output of MinusJacobian matches the one obtained by
+// numerically evaluating D_1 Minus(x,x).
+MATCHER_P2(HasCorrectMinusJacobianAt, x, tolerance, "") {
+ const int ambient_size = arg.AmbientSize();
+ const int tangent_size = arg.TangentSize();
+
+ Vector y = x;
+ Vector y_minus_x = Vector::Zero(tangent_size);
+
+ NumericDiffOptions options;
+ options.ridders_relative_initial_step_size = 1e-4;
+ DynamicNumericDiffCostFunction<MinusFunctor, RIDDERS> cost_function(
+ new MinusFunctor(arg, x.data()), TAKE_OWNERSHIP, options);
+ cost_function.AddParameterBlock(ambient_size);
+ cost_function.SetNumResiduals(tangent_size);
+
+ double* parameters[1] = {y.data()};
+
+ Matrix expected = Matrix::Zero(tangent_size, ambient_size);
+ double* jacobians[1] = {expected.data()};
+
+ EXPECT_TRUE(cost_function.Evaluate(parameters, y_minus_x.data(), jacobians));
+
+ Matrix actual = Matrix::Random(tangent_size, ambient_size);
+ EXPECT_TRUE(arg.MinusJacobian(x.data(), actual.data()));
+
+ const double n = (actual - expected).norm();
+ const double d = expected.norm();
+ const double diffnorm = (d == 0.0) ? n : (n / d);
+ if (diffnorm > tolerance) {
+ *result_listener << "\nx: " << x.transpose() << "\nexpected: \n"
+ << expected << "\nactual:\n"
+ << actual << "\ndiff:\n"
+ << expected - actual << "\ndiffnorm: " << diffnorm;
+ return false;
+ }
+ return true;
+}
+
+// Checks that D_delta Minus(Plus(x, delta), x) at delta = 0 is an identity
+// matrix.
+MATCHER_P2(MinusPlusJacobianIsIdentityAt, x, tolerance, "") {
+ const int ambient_size = arg.AmbientSize();
+ const int tangent_size = arg.TangentSize();
+
+ Matrix plus_jacobian(ambient_size, tangent_size);
+ EXPECT_TRUE(arg.PlusJacobian(x.data(), plus_jacobian.data()));
+ Matrix minus_jacobian(tangent_size, ambient_size);
+ EXPECT_TRUE(arg.MinusJacobian(x.data(), minus_jacobian.data()));
+
+ const Matrix actual = minus_jacobian * plus_jacobian;
+ const Matrix expected = Matrix::Identity(tangent_size, tangent_size);
+
+ const double n = (actual - expected).norm();
+ const double d = expected.norm();
+ const double diffnorm = n / d;
+ if (diffnorm > tolerance) {
+ *result_listener << "\nx: " << x.transpose() << "\nexpected: \n"
+ << expected << "\nactual:\n"
+ << actual << "\ndiff:\n"
+ << expected - actual << "\ndiffnorm: " << diffnorm;
+
+ return false;
+ }
+ return true;
+}
+
+// Verify that the output of RightMultiplyByPlusJacobian is ambient_matrix *
+// plus_jacobian.
+MATCHER_P2(HasCorrectRightMultiplyByPlusJacobianAt, x, tolerance, "") {
+ const int ambient_size = arg.AmbientSize();
+ const int tangent_size = arg.TangentSize();
+
+ constexpr int kMinNumRows = 0;
+ constexpr int kMaxNumRows = 3;
+ for (int num_rows = kMinNumRows; num_rows <= kMaxNumRows; ++num_rows) {
+ Matrix plus_jacobian = Matrix::Random(ambient_size, tangent_size);
+ EXPECT_TRUE(arg.PlusJacobian(x.data(), plus_jacobian.data()));
+
+ Matrix ambient_matrix = Matrix::Random(num_rows, ambient_size);
+ Matrix expected = ambient_matrix * plus_jacobian;
+
+ Matrix actual = Matrix::Random(num_rows, tangent_size);
+ EXPECT_TRUE(arg.RightMultiplyByPlusJacobian(
+ x.data(), num_rows, ambient_matrix.data(), actual.data()));
+ const double n = (actual - expected).norm();
+ const double d = expected.norm();
+ const double diffnorm = (d == 0.0) ? n : (n / d);
+ if (diffnorm > tolerance) {
+ *result_listener << "\nx: " << x.transpose() << "\nambient_matrix : \n"
+ << ambient_matrix << "\nplus_jacobian : \n"
+ << plus_jacobian << "\nexpected: \n"
+ << expected << "\nactual:\n"
+ << actual << "\ndiff:\n"
+ << expected - actual << "\ndiffnorm : " << diffnorm;
+ return false;
+ }
+ }
+ return true;
+}
+
+} // namespace ceres
diff --git a/include/ceres/normal_prior.h b/include/ceres/normal_prior.h
index 14ab379..5a26e01 100644
--- a/include/ceres/normal_prior.h
+++ b/include/ceres/normal_prior.h
@@ -1,5 +1,5 @@
// Ceres Solver - A fast non-linear least squares minimizer
-// Copyright 2019 Google Inc. All rights reserved.
+// Copyright 2023 Google Inc. All rights reserved.
// http://ceres-solver.org/
//
// Redistribution and use in source and binary forms, with or without
@@ -57,11 +57,11 @@
// which would be the case if the covariance matrix S is rank
// deficient.
-class CERES_EXPORT NormalPrior : public CostFunction {
+class CERES_EXPORT NormalPrior final : 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);
+ NormalPrior(const Matrix& A, Vector b);
bool Evaluate(double const* const* parameters,
double* residuals,
double** jacobians) const override;
diff --git a/include/ceres/numeric_diff_cost_function.h b/include/ceres/numeric_diff_cost_function.h
index cf7971c..f2a377b 100644
--- a/include/ceres/numeric_diff_cost_function.h
+++ b/include/ceres/numeric_diff_cost_function.h
@@ -1,5 +1,5 @@
// Ceres Solver - A fast non-linear least squares minimizer
-// Copyright 2019 Google Inc. All rights reserved.
+// Copyright 2024 Google Inc. All rights reserved.
// http://ceres-solver.org/
//
// Redistribution and use in source and binary forms, with or without
@@ -149,9 +149,8 @@
// 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);
+// auto* cost_function
+// = new NumericDiffCostFunction<MyCostFunction, CENTRAL, 1, 4, 8>();
//
// where MyCostFunction has 1 residual and 2 parameter blocks with sizes 4 and 8
// respectively. Look at the tests for a more detailed example.
@@ -163,6 +162,7 @@
#include <array>
#include <memory>
+#include <type_traits>
#include "Eigen/Dense"
#include "ceres/cost_function.h"
@@ -171,31 +171,55 @@
#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,
+ NumericDiffMethodType kMethod = CENTRAL,
int kNumResiduals = 0, // Number of residuals, or ceres::DYNAMIC
int... Ns> // Parameters dimensions for each block.
-class NumericDiffCostFunction : public SizedCostFunction<kNumResiduals, Ns...> {
+class NumericDiffCostFunction final
+ : public SizedCostFunction<kNumResiduals, Ns...> {
public:
- NumericDiffCostFunction(
+ explicit 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, Ns...>::set_num_residuals(num_residuals);
- }
- }
+ : NumericDiffCostFunction{std::unique_ptr<CostFunctor>{functor},
+ ownership,
+ num_residuals,
+ options} {}
- explicit NumericDiffCostFunction(NumericDiffCostFunction&& other)
- : functor_(std::move(other.functor_)), ownership_(other.ownership_) {}
+ explicit NumericDiffCostFunction(
+ std::unique_ptr<CostFunctor> functor,
+ int num_residuals = kNumResiduals,
+ const NumericDiffOptions& options = NumericDiffOptions())
+ : NumericDiffCostFunction{
+ std::move(functor), TAKE_OWNERSHIP, num_residuals, options} {}
- virtual ~NumericDiffCostFunction() {
+ // Constructs the CostFunctor on the heap and takes the ownership.
+ // Invocable only if the number of residuals is known at compile-time.
+ template <class... Args,
+ bool kIsDynamic = kNumResiduals == DYNAMIC,
+ std::enable_if_t<!kIsDynamic &&
+ std::is_constructible_v<CostFunctor, Args&&...>>* =
+ nullptr>
+ explicit NumericDiffCostFunction(Args&&... args)
+ // NOTE We explicitly use direct initialization using parentheses instead
+ // of uniform initialization using braces to avoid narrowing conversion
+ // warnings.
+ : NumericDiffCostFunction{
+ std::make_unique<CostFunctor>(std::forward<Args>(args)...),
+ TAKE_OWNERSHIP} {}
+
+ NumericDiffCostFunction(NumericDiffCostFunction&& other) noexcept = default;
+ NumericDiffCostFunction& operator=(NumericDiffCostFunction&& other) noexcept =
+ default;
+ NumericDiffCostFunction(const NumericDiffCostFunction&) = delete;
+ NumericDiffCostFunction& operator=(const NumericDiffCostFunction&) = delete;
+
+ ~NumericDiffCostFunction() override {
if (ownership_ != TAKE_OWNERSHIP) {
functor_.release();
}
@@ -219,7 +243,7 @@
return false;
}
- if (jacobians == NULL) {
+ if (jacobians == nullptr) {
return true;
}
@@ -235,7 +259,7 @@
}
internal::EvaluateJacobianForParameterBlocks<ParameterDims>::
- template Apply<method, kNumResiduals>(
+ template Apply<kMethod, kNumResiduals>(
functor_.get(),
residuals,
options_,
@@ -246,7 +270,19 @@
return true;
}
+ const CostFunctor& functor() const { return *functor_; }
+
private:
+ explicit NumericDiffCostFunction(std::unique_ptr<CostFunctor> functor,
+ Ownership ownership,
+ [[maybe_unused]] int num_residuals,
+ const NumericDiffOptions& options)
+ : functor_(std::move(functor)), ownership_(ownership), options_(options) {
+ if constexpr (kNumResiduals == DYNAMIC) {
+ SizedCostFunction<kNumResiduals, Ns...>::set_num_residuals(num_residuals);
+ }
+ }
+
std::unique_ptr<CostFunctor> functor_;
Ownership ownership_;
NumericDiffOptions options_;
diff --git a/include/ceres/numeric_diff_first_order_function.h b/include/ceres/numeric_diff_first_order_function.h
new file mode 100644
index 0000000..525f197
--- /dev/null
+++ b/include/ceres/numeric_diff_first_order_function.h
@@ -0,0 +1,271 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2023 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_NUMERIC_DIFF_FIRST_ORDER_FUNCTION_H_
+#define CERES_PUBLIC_NUMERIC_DIFF_FIRST_ORDER_FUNCTION_H_
+
+#include <algorithm>
+#include <memory>
+#include <type_traits>
+#include <utility>
+
+#include "ceres/first_order_function.h"
+#include "ceres/internal/eigen.h"
+#include "ceres/internal/fixed_array.h"
+#include "ceres/internal/numeric_diff.h"
+#include "ceres/internal/parameter_dims.h"
+#include "ceres/internal/variadic_evaluate.h"
+#include "ceres/numeric_diff_options.h"
+#include "ceres/types.h"
+#include "glog/logging.h"
+
+namespace ceres {
+
+// Creates FirstOrderFunctions as needed by the GradientProblem
+// framework, with gradients computed via numeric differentiation. For
+// more information on numeric differentiation, see the wikipedia
+// article at https://en.wikipedia.org/wiki/Numerical_differentiation
+//
+// To get an numerically differentiated cost function, you must define
+// a class with an operator() (a functor) that computes the cost.
+//
+// The function must write the computed value in the last argument
+// (the only non-const one) and return true to indicate success.
+//
+// For example, consider a scalar error e = x'y - a, where both x and y are
+// two-dimensional column vector parameters, the prime sign indicates
+// transposition, and a is a constant.
+//
+// To write an numerically-differentiable cost function for the above model,
+// first define the object
+//
+// class QuadraticCostFunctor {
+// public:
+// explicit QuadraticCostFunctor(double a) : a_(a) {}
+// bool operator()(const double* const xy, double* cost) const {
+// constexpr int kInputVectorLength = 2;
+// const double* const x = xy;
+// const double* const y = xy + kInputVectorLength;
+// *cost = x[0] * y[0] + x[1] * y[1] - a_;
+// return true;
+// }
+//
+// private:
+// double a_;
+// };
+//
+//
+// Note that in the declaration of operator() the input parameters xy
+// come first, and are passed as const pointers to array of
+// doubles. The output cost is the last parameter.
+//
+// Then given this class definition, the numerically differentiated
+// first order function with central differences used for computing the
+// derivative can be constructed as follows.
+//
+// FirstOrderFunction* function
+// = new NumericDiffFirstOrderFunction<MyScalarCostFunctor, CENTRAL, 4>(
+// new QuadraticCostFunctor(1.0)); ^ ^ ^
+// | | |
+// Finite Differencing Scheme -+ | |
+// Dimension of xy ------------------------+
+//
+//
+// In the instantiation above, the template parameters following
+// "QuadraticCostFunctor", "CENTRAL, 4", describe the finite
+// differencing scheme as "central differencing" and the functor as
+// computing its cost from a 4 dimensional input.
+//
+// If the size of the parameter vector is not known at compile time, then an
+// alternate construction syntax can be used:
+//
+// FirstOrderFunction* function
+// = new NumericDiffFirstOrderFunction<MyScalarCostFunctor, CENTRAL>(
+// new QuadraticCostFunctor(1.0), 4);
+//
+// Note that instead of passing 4 as a template argument, it is now passed as
+// the second argument to the constructor.
+template <typename FirstOrderFunctor,
+ NumericDiffMethodType kMethod,
+ int kNumParameters = DYNAMIC>
+class NumericDiffFirstOrderFunction final : public FirstOrderFunction {
+ public:
+ template <class... Args,
+ bool kIsDynamic = kNumParameters == DYNAMIC,
+ std::enable_if_t<!kIsDynamic &&
+ std::is_constructible_v<FirstOrderFunctor,
+ Args&&...>>* = nullptr>
+ explicit NumericDiffFirstOrderFunction(Args&&... args)
+ : NumericDiffFirstOrderFunction{std::make_unique<FirstOrderFunction>(
+ std::forward<Args>(args)...)} {}
+
+ NumericDiffFirstOrderFunction(const NumericDiffFirstOrderFunction&) = delete;
+ NumericDiffFirstOrderFunction& operator=(
+ const NumericDiffFirstOrderFunction&) = delete;
+ NumericDiffFirstOrderFunction(
+ NumericDiffFirstOrderFunction&& other) noexcept = default;
+ NumericDiffFirstOrderFunction& operator=(
+ NumericDiffFirstOrderFunction&& other) noexcept = default;
+
+ // Constructor for the case where the parameter size is known at compile time.
+ explicit NumericDiffFirstOrderFunction(
+ FirstOrderFunctor* functor,
+ Ownership ownership = TAKE_OWNERSHIP,
+ const NumericDiffOptions& options = NumericDiffOptions())
+ : NumericDiffFirstOrderFunction{
+ std::unique_ptr<FirstOrderFunctor>{functor},
+ kNumParameters,
+ ownership,
+ options,
+ FIXED_INIT} {}
+
+ // Constructor for the case where the parameter size is known at compile time.
+ explicit NumericDiffFirstOrderFunction(
+ std::unique_ptr<FirstOrderFunctor> functor,
+ const NumericDiffOptions& options = NumericDiffOptions())
+ : NumericDiffFirstOrderFunction{
+ std::move(functor), kNumParameters, TAKE_OWNERSHIP, FIXED_INIT} {}
+
+ // Constructor for the case where the parameter size is specified at run time.
+ explicit NumericDiffFirstOrderFunction(
+ FirstOrderFunctor* functor,
+ int num_parameters,
+ Ownership ownership = TAKE_OWNERSHIP,
+ const NumericDiffOptions& options = NumericDiffOptions())
+ : NumericDiffFirstOrderFunction{
+ std::unique_ptr<FirstOrderFunctor>{functor},
+ num_parameters,
+ ownership,
+ options,
+ DYNAMIC_INIT} {}
+
+ // Constructor for the case where the parameter size is specified at run time.
+ explicit NumericDiffFirstOrderFunction(
+ std::unique_ptr<FirstOrderFunctor> functor,
+ int num_parameters,
+ Ownership ownership = TAKE_OWNERSHIP,
+ const NumericDiffOptions& options = NumericDiffOptions())
+ : NumericDiffFirstOrderFunction{std::move(functor),
+ num_parameters,
+ ownership,
+ options,
+ DYNAMIC_INIT} {}
+
+ ~NumericDiffFirstOrderFunction() override {
+ if (ownership_ != TAKE_OWNERSHIP) {
+ functor_.release();
+ }
+ }
+
+ bool Evaluate(const double* const parameters,
+ double* cost,
+ double* gradient) const override {
+ // Get the function value (cost) at the the point to evaluate.
+ if (!(*functor_)(parameters, cost)) {
+ return false;
+ }
+
+ if (gradient == nullptr) {
+ return true;
+ }
+
+ // Create a copy of the parameters which will get mutated.
+ internal::FixedArray<double, 32> parameters_copy(num_parameters_);
+ std::copy_n(parameters, num_parameters_, parameters_copy.data());
+ double* parameters_ptr = parameters_copy.data();
+ constexpr int kNumResiduals = 1;
+ if constexpr (kNumParameters == DYNAMIC) {
+ internal::FirstOrderFunctorAdapter<FirstOrderFunctor> fofa(*functor_);
+ return internal::NumericDiff<
+ internal::FirstOrderFunctorAdapter<FirstOrderFunctor>,
+ kMethod,
+ kNumResiduals,
+ internal::DynamicParameterDims,
+ 0,
+ DYNAMIC>::EvaluateJacobianForParameterBlock(&fofa,
+ cost,
+ options_,
+ kNumResiduals,
+ 0,
+ num_parameters_,
+ ¶meters_ptr,
+ gradient);
+ } else {
+ return internal::EvaluateJacobianForParameterBlocks<
+ internal::StaticParameterDims<kNumParameters>>::
+ template Apply<kMethod, 1>(functor_.get(),
+ cost,
+ options_,
+ kNumResiduals,
+ ¶meters_ptr,
+ &gradient);
+ }
+ }
+
+ int NumParameters() const override { return num_parameters_; }
+
+ const FirstOrderFunctor& functor() const { return *functor_; }
+
+ private:
+ // Tags used to differentiate between dynamic and fixed size constructor
+ // delegate invocations.
+ static constexpr std::integral_constant<int, DYNAMIC> DYNAMIC_INIT{};
+ static constexpr std::integral_constant<int, kNumParameters> FIXED_INIT{};
+
+ template <class InitTag>
+ explicit NumericDiffFirstOrderFunction(
+ std::unique_ptr<FirstOrderFunctor> functor,
+ int num_parameters,
+ Ownership ownership,
+ const NumericDiffOptions& options,
+ InitTag /*unused*/)
+ : functor_(std::move(functor)),
+ num_parameters_(num_parameters),
+ ownership_(ownership),
+ options_(options) {
+ static_assert(
+ kNumParameters == FIXED_INIT,
+ "Template parameter must be DYNAMIC when using this constructor. If "
+ "you want to provide the number of parameters statically use the other "
+ "constructor.");
+ if constexpr (InitTag::value == DYNAMIC_INIT) {
+ CHECK_GT(num_parameters, 0);
+ }
+ }
+
+ std::unique_ptr<FirstOrderFunctor> functor_;
+ int num_parameters_;
+ Ownership ownership_;
+ NumericDiffOptions options_;
+};
+
+} // namespace ceres
+
+#endif // CERES_PUBLIC_NUMERIC_DIFF_FIRST_ORDER_FUNCTION_H_
diff --git a/include/ceres/numeric_diff_options.h b/include/ceres/numeric_diff_options.h
index 64919ed..eefb7ad 100644
--- a/include/ceres/numeric_diff_options.h
+++ b/include/ceres/numeric_diff_options.h
@@ -1,5 +1,5 @@
// Ceres Solver - A fast non-linear least squares minimizer
-// Copyright 2019 Google Inc. All rights reserved.
+// Copyright 2023 Google Inc. All rights reserved.
// http://ceres-solver.org/
//
// Redistribution and use in source and binary forms, with or without
@@ -32,7 +32,8 @@
#ifndef CERES_PUBLIC_NUMERIC_DIFF_OPTIONS_H_
#define CERES_PUBLIC_NUMERIC_DIFF_OPTIONS_H_
-#include "ceres/internal/port.h"
+#include "ceres/internal/disable_warnings.h"
+#include "ceres/internal/export.h"
namespace ceres {
@@ -70,4 +71,6 @@
} // namespace ceres
+#include "ceres/internal/reenable_warnings.h"
+
#endif // CERES_PUBLIC_NUMERIC_DIFF_OPTIONS_H_
diff --git a/include/ceres/ordered_groups.h b/include/ceres/ordered_groups.h
index 954663c..d15d22d 100644
--- a/include/ceres/ordered_groups.h
+++ b/include/ceres/ordered_groups.h
@@ -1,5 +1,5 @@
// Ceres Solver - A fast non-linear least squares minimizer
-// Copyright 2019 Google Inc. All rights reserved.
+// Copyright 2023 Google Inc. All rights reserved.
// http://ceres-solver.org/
//
// Redistribution and use in source and binary forms, with or without
@@ -36,7 +36,7 @@
#include <unordered_map>
#include <vector>
-#include "ceres/internal/port.h"
+#include "ceres/internal/export.h"
#include "glog/logging.h"
namespace ceres {
@@ -190,7 +190,7 @@
};
// Typedef for the most commonly used version of OrderedGroups.
-typedef OrderedGroups<double*> ParameterBlockOrdering;
+using ParameterBlockOrdering = OrderedGroups<double*>;
} // namespace ceres
diff --git a/include/ceres/problem.h b/include/ceres/problem.h
index add12ea..4c6fd1b 100644
--- a/include/ceres/problem.h
+++ b/include/ceres/problem.h
@@ -1,5 +1,5 @@
// Ceres Solver - A fast non-linear least squares minimizer
-// Copyright 2015 Google Inc. All rights reserved.
+// Copyright 2023 Google Inc. All rights reserved.
// http://ceres-solver.org/
//
// Redistribution and use in source and binary forms, with or without
@@ -43,6 +43,7 @@
#include "ceres/context.h"
#include "ceres/internal/disable_warnings.h"
+#include "ceres/internal/export.h"
#include "ceres/internal/port.h"
#include "ceres/types.h"
#include "glog/logging.h"
@@ -52,7 +53,7 @@
class CostFunction;
class EvaluationCallback;
class LossFunction;
-class LocalParameterization;
+class Manifold;
class Solver;
struct CRSMatrix;
@@ -65,7 +66,7 @@
// A ResidualBlockId is an opaque handle clients can use to remove residual
// blocks from a Problem after adding them.
-typedef internal::ResidualBlock* ResidualBlockId;
+using ResidualBlockId = internal::ResidualBlock*;
// A class to represent non-linear least squares problems. Such
// problems have a cost function that is a sum of error terms (known
@@ -78,31 +79,28 @@
//
// 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.
+// 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.
+// 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.
+// 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).
+// "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
@@ -122,38 +120,37 @@
class CERES_EXPORT Problem {
public:
struct CERES_EXPORT Options {
- // 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.
+ // These flags control whether the Problem object owns the CostFunctions,
+ // LossFunctions, and Manifolds passed into the Problem.
+ //
+ // If set to TAKE_OWNERSHIP, then the problem object will delete the
+ // corresponding object on destruction. The destructor is careful to delete
+ // the pointers only once, since sharing objects is allowed.
Ownership cost_function_ownership = TAKE_OWNERSHIP;
Ownership loss_function_ownership = TAKE_OWNERSHIP;
- Ownership local_parameterization_ownership = TAKE_OWNERSHIP;
+ Ownership manifold_ownership = TAKE_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
+ // proportional to the size of the entire problem. If you only ever remove
// parameters or residuals from the problem occasionally, this might be
- // acceptable. However, if you have memory to spare, enable this option to
+ // 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 additional hash set per
+ // The increase in memory usage is two-fold: an additional 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 = false;
// 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
+ // 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
@@ -167,26 +164,23 @@
// Ceres does NOT take ownership of the pointer.
Context* context = nullptr;
- // Using this callback interface, Ceres can notify you when it is
- // about to evaluate the residuals or jacobians. With the
- // callback, you can share computation between residual blocks by
- // doing the shared computation in
+ // Using this callback interface, Ceres can notify you when it is about to
+ // evaluate the residuals or jacobians. With the callback, you can share
+ // computation between residual blocks by doing the shared computation in
// EvaluationCallback::PrepareForEvaluation() before Ceres calls
- // CostFunction::Evaluate(). It also enables caching results
- // between a pure residual evaluation and a residual & jacobian
- // evaluation.
+ // CostFunction::Evaluate(). It also enables caching results between a pure
+ // residual evaluation and a residual & jacobian evaluation.
//
// Problem DOES NOT take ownership of the callback.
//
- // NOTE: Evaluation callbacks are incompatible with inner
- // iterations. So calling Solve with
- // Solver::Options::use_inner_iterations = true on a Problem with
- // a non-null evaluation callback is an error.
+ // NOTE: Evaluation callbacks are incompatible with inner iterations. So
+ // calling Solve with Solver::Options::use_inner_iterations = true on a
+ // Problem with a non-null evaluation callback is an error.
EvaluationCallback* evaluation_callback = nullptr;
};
- // The default constructor is equivalent to the
- // invocation Problem(Problem::Options()).
+ // The default constructor is equivalent to the invocation
+ // Problem(Problem::Options()).
Problem();
explicit Problem(const Options& options);
Problem(Problem&&);
@@ -197,31 +191,29 @@
~Problem();
- // Add a residual block to the overall cost function. The cost
- // function carries with its 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
- // nullptr, in which case the cost of the term is just the squared norm
- // of the residuals.
+ // Add a residual block to the overall cost function. The cost function
+ // carries with its 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 nullptr, 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 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
+ // The Problem object by default takes ownership of the cost_function and
+ // loss_function pointers (See Problem::Options to override this behaviour).
+ // 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.
+ // 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:
//
@@ -234,8 +226,8 @@
// problem.AddResidualBlock(new MyUnaryCostFunction(...), nullptr, x1);
// problem.AddResidualBlock(new MyBinaryCostFunction(...), nullptr, x2, x1);
//
- // Add a residual block by listing the parameter block pointers
- // directly instead of wapping them in a container.
+ // Add a residual block by listing the parameter block pointers directly
+ // instead of wapping them in a container.
template <typename... Ts>
ResidualBlockId AddResidualBlock(CostFunction* cost_function,
LossFunction* loss_function,
@@ -261,29 +253,32 @@
double* const* const parameter_blocks,
int num_parameter_blocks);
- // 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.
+ // 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 will result in a crash.
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().
+ // Add a parameter block with appropriate size and Manifold to the
+ // problem. It is okay for manifold to be nullptr.
//
- // 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.
+ // Repeated calls with the same arguments are ignored. Repeated calls
+ // with the same double pointer but a different size results in a crash
+ // (unless Solver::Options::disable_all_safety_checks is set to true).
+ //
+ // Repeated calls with the same double pointer and size but different Manifold
+ // is equivalent to calling SetManifold(manifold), i.e., any previously
+ // associated Manifold object will be replaced with the manifold.
+ void AddParameterBlock(double* values, int size, Manifold* manifold);
+
+ // Remove a parameter block from the problem. The Manifold 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
@@ -308,35 +303,41 @@
// Allow the indicated parameter block to vary during optimization.
void SetParameterBlockVariable(double* values);
- // Returns true if a parameter block is set constant, and false
- // otherwise. A parameter block may be set constant in two ways:
- // either by calling SetParameterBlockConstant or by associating a
- // LocalParameterization with a zero dimensional tangent space with
- // it.
+ // Returns true if a parameter block is set constant, and false otherwise. A
+ // parameter block may be set constant in two ways: either by calling
+ // SetParameterBlockConstant or by associating a Manifold with a zero
+ // dimensional tangent space with it.
bool IsParameterBlockConstant(const double* values) const;
- // 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. Calling SetParameterization with
- // nullptr will clear any previously set parameterization.
- void SetParameterization(double* values,
- LocalParameterization* local_parameterization);
+ // Set the Manifold for the parameter block. Calling SetManifold with nullptr
+ // will clear any previously set Manifold for the parameter block.
+ //
+ // Repeated calls will result in any previously associated Manifold object to
+ // be replaced with the manifold.
+ //
+ // The manifold is owned by the Problem by default (See Problem::Options to
+ // override this behaviour).
+ //
+ // It is acceptable to set the same Manifold for multiple parameter blocks.
+ void SetManifold(double* values, Manifold* manifold);
- // Get the local parameterization object associated with this
- // parameter block. If there is no parameterization object
- // associated then nullptr is returned.
- const LocalParameterization* GetParameterization(const double* values) const;
+ // Get the Manifold object associated with this parameter block.
+ //
+ // If there is no Manifold object associated then nullptr is returned.
+ const Manifold* GetManifold(const double* values) const;
+
+ // Returns true if a Manifold is associated with this parameter block, false
+ // otherwise.
+ bool HasManifold(const double* values) const;
// Set the lower/upper bound for the parameter at position "index".
void SetParameterLowerBound(double* values, int index, double lower_bound);
void SetParameterUpperBound(double* values, int index, double upper_bound);
- // Get the lower/upper bound for the parameter at position
- // "index". If the parameter is not bounded by the user, then its
- // lower bound is -std::numeric_limits<double>::max() and upper
- // bound is std::numeric_limits<double>::max().
+ // Get the lower/upper bound for the parameter at position "index". If the
+ // parameter is not bounded by the user, then its lower bound is
+ // -std::numeric_limits<double>::max() and upper bound is
+ // std::numeric_limits<double>::max().
double GetParameterLowerBound(const double* values, int index) const;
double GetParameterUpperBound(const double* values, int index) const;
@@ -344,37 +345,37 @@
// 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.
+ // 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.
+ // 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;
+ // The dimension of the tangent space of the Manifold for the parameter block.
+ // If there is no Manifold associated with this parameter block, then
+ // ParameterBlockTangentSize = ParameterBlockSize.
+ int ParameterBlockTangentSize(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.
+ // 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.
+ // 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.
@@ -393,10 +394,10 @@
// 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.
+ // 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;
@@ -404,49 +405,45 @@
// Options struct to control Problem::Evaluate.
struct EvaluateOptions {
// 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.
+ // 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.
+ // 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.
+ // 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.
+ // 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 = true;
int num_threads = 1;
};
- // Evaluate Problem. Any of the output pointers can be nullptr. Which
- // residual blocks and parameter blocks are used is controlled by
- // the EvaluateOptions struct above.
+ // Evaluate Problem. Any of the output pointers can be nullptr. 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.,
+ // 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;
@@ -456,8 +453,8 @@
// problem.Evaluate(Problem::EvaluateOptions(), &cost,
// nullptr, nullptr, nullptr);
//
- // The cost is evaluated at x = 1. If you wish to evaluate the
- // problem at x = 2, then
+ // 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,
@@ -465,80 +462,74 @@
//
// 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).
+ // Note 2: If no Manifolds 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 Manifold, then it contributes
+ // "TangentSize" entries to the gradient vector (and the number of columns in
+ // the jacobian).
//
- // Note 3: This function cannot be called while the problem is being
- // solved, for example it cannot be called from an IterationCallback
- // at the end of an iteration during a solve.
+ // Note 3: This function cannot be called while the problem is being solved,
+ // for example it cannot be called from an IterationCallback at the end of an
+ // iteration during a solve.
//
- // Note 4: If an EvaluationCallback is associated with the problem,
- // then its PrepareForEvaluation method will be called every time
- // this method is called with new_point = true.
+ // Note 4: If an EvaluationCallback is associated with the problem, then its
+ // PrepareForEvaluation method will be called every time this method is called
+ // with new_point = true.
bool Evaluate(const EvaluateOptions& options,
double* cost,
std::vector<double>* residuals,
std::vector<double>* gradient,
CRSMatrix* jacobian);
- // Evaluates the residual block, storing the scalar cost in *cost,
- // the residual components in *residuals, and the jacobians between
- // the parameters and residuals in jacobians[i], in row-major order.
+ // Evaluates the residual block, storing the scalar cost in *cost, the
+ // residual components in *residuals, and the jacobians between the parameters
+ // and residuals in jacobians[i], in row-major order.
//
// If residuals is nullptr, the residuals are not computed.
//
- // If jacobians is nullptr, no Jacobians are computed. If
- // jacobians[i] is nullptr, then the Jacobian for that parameter
- // block is not computed.
+ // If jacobians is nullptr, no Jacobians are computed. If jacobians[i] is
+ // nullptr, then the Jacobian for that parameter block is not computed.
//
- // It is not okay to request the Jacobian w.r.t a parameter block
- // that is constant.
+ // It is not okay to request the Jacobian w.r.t a parameter block that is
+ // constant.
//
- // The return value indicates the success or failure. Even if the
- // function returns false, the caller should expect the output
- // memory locations to have been modified.
+ // The return value indicates the success or failure. Even if the function
+ // returns false, the caller should expect the output memory locations to have
+ // been modified.
//
- // The returned cost and jacobians have had robustification and
- // local parameterizations applied already; for example, the
- // jacobian for a 4-dimensional quaternion parameter using the
- // "QuaternionParameterization" is num_residuals by 3 instead of
- // num_residuals by 4.
+ // The returned cost and jacobians have had robustification and Manifold
+ // applied already; for example, the jacobian for a 4-dimensional quaternion
+ // parameter using the "QuaternionParameterization" is num_residuals by 3
+ // instead of num_residuals by 4.
//
- // apply_loss_function as the name implies allows the user to switch
- // the application of the loss function on and off.
+ // apply_loss_function as the name implies allows the user to switch the
+ // application of the loss function on and off.
//
// If an EvaluationCallback is associated with the problem, then its
- // PrepareForEvaluation method will be called every time this method
- // is called with new_point = true. This conservatively assumes that
- // the user may have changed the parameter values since the previous
- // call to evaluate / solve. For improved efficiency, and only if
- // you know that the parameter values have not changed between
- // calls, see EvaluateResidualBlockAssumingParametersUnchanged().
+ // PrepareForEvaluation method will be called every time this method is called
+ // with new_point = true. This conservatively assumes that the user may have
+ // changed the parameter values since the previous call to evaluate / solve.
+ // For improved efficiency, and only if you know that the parameter values
+ // have not changed between calls, see
+ // EvaluateResidualBlockAssumingParametersUnchanged().
bool EvaluateResidualBlock(ResidualBlockId residual_block_id,
bool apply_loss_function,
double* cost,
double* residuals,
double** jacobians) const;
- // Same as EvaluateResidualBlock except that if an
- // EvaluationCallback is associated with the problem, then its
- // PrepareForEvaluation method will be called every time this method
- // is called with new_point = false.
+ // Same as EvaluateResidualBlock except that if an EvaluationCallback is
+ // associated with the problem, then its PrepareForEvaluation method will be
+ // called every time this method is called with new_point = false.
//
- // This means, if an EvaluationCallback is associated with the
- // problem then it is the user's responsibility to call
- // PrepareForEvaluation before calling this method if necessary,
- // i.e. iff the parameter values have been changed since the last
- // call to evaluate / solve.'
+ // This means, if an EvaluationCallback is associated with the problem then it
+ // is the user's responsibility to call PrepareForEvaluation before calling
+ // this method if necessary, i.e. iff the parameter values have been changed
+ // since the last call to evaluate / solve.'
//
- // This is because, as the name implies, we assume that the
- // parameter blocks did not change since the last time
- // PrepareForEvaluation was called (via Solve, Evaluate or
- // EvaluateResidualBlock).
+ // This is because, as the name implies, we assume that the parameter blocks
+ // did not change since the last time PrepareForEvaluation was called (via
+ // Solve, Evaluate or EvaluateResidualBlock).
bool EvaluateResidualBlockAssumingParametersUnchanged(
ResidualBlockId residual_block_id,
bool apply_loss_function,
@@ -546,9 +537,13 @@
double* residuals,
double** jacobians) const;
+ // Returns reference to the options with which the Problem was constructed.
+ const Options& options() const;
+
+ // Returns pointer to Problem implementation
+ internal::ProblemImpl* mutable_impl();
+
private:
- friend class Solver;
- friend class Covariance;
std::unique_ptr<internal::ProblemImpl> impl_;
};
diff --git a/include/ceres/product_manifold.h b/include/ceres/product_manifold.h
new file mode 100644
index 0000000..ed2d1f4
--- /dev/null
+++ b/include/ceres/product_manifold.h
@@ -0,0 +1,319 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2023 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)
+// sergiu.deitsch@gmail.com (Sergiu Deitsch)
+//
+
+#ifndef CERES_PUBLIC_PRODUCT_MANIFOLD_H_
+#define CERES_PUBLIC_PRODUCT_MANIFOLD_H_
+
+#include <algorithm>
+#include <array>
+#include <cassert>
+#include <cstddef>
+#include <numeric>
+#include <tuple>
+#include <type_traits>
+#include <utility>
+
+#include "ceres/internal/eigen.h"
+#include "ceres/internal/fixed_array.h"
+#include "ceres/internal/port.h"
+#include "ceres/manifold.h"
+
+namespace ceres {
+
+// Construct a manifold by taking the Cartesian product of a number of other
+// manifolds. 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.
+//
+// Example usage:
+//
+// ProductManifold<QuaternionManifold, EuclideanManifold<3>> se3;
+//
+// is the manifold for a rigid transformation, where the rotation is
+// represented using a quaternion.
+//
+// Manifolds can be copied and moved to ProductManifold:
+//
+// SubsetManifold manifold1(5, {2});
+// SubsetManifold manifold2(3, {0, 1});
+// ProductManifold<SubsetManifold, SubsetManifold> manifold(manifold1,
+// manifold2);
+//
+// In advanced use cases, manifolds can be dynamically allocated and passed as
+// (smart) pointers:
+//
+// ProductManifold<std::unique_ptr<QuaternionManifold>, EuclideanManifold<3>>
+// se3{std::make_unique<QuaternionManifold>(), EuclideanManifold<3>{}};
+//
+// In C++17, the template parameters can be left out as they are automatically
+// deduced making the initialization much simpler:
+//
+// ProductManifold se3{QuaternionManifold{}, EuclideanManifold<3>{}};
+//
+// The manifold implementations must be either default constructible, copyable
+// or moveable to be usable in a ProductManifold.
+template <typename Manifold0, typename Manifold1, typename... ManifoldN>
+class ProductManifold final : public Manifold {
+ public:
+ // ProductManifold constructor perfect forwards arguments to store manifolds.
+ //
+ // Either use default construction or if you need to copy or move-construct a
+ // manifold instance, you need to pass an instance as an argument for all
+ // types given as class template parameters.
+ template <typename... Args,
+ std::enable_if_t<std::is_constructible<
+ std::tuple<Manifold0, Manifold1, ManifoldN...>,
+ Args...>::value>* = nullptr>
+ explicit ProductManifold(Args&&... manifolds)
+ : ProductManifold{std::make_index_sequence<kNumManifolds>{},
+ std::forward<Args>(manifolds)...} {}
+
+ int AmbientSize() const override { return ambient_size_; }
+ int TangentSize() const override { return tangent_size_; }
+
+ bool Plus(const double* x,
+ const double* delta,
+ double* x_plus_delta) const override {
+ return PlusImpl(
+ x, delta, x_plus_delta, std::make_index_sequence<kNumManifolds>{});
+ }
+
+ bool Minus(const double* y,
+ const double* x,
+ double* y_minus_x) const override {
+ return MinusImpl(
+ y, x, y_minus_x, std::make_index_sequence<kNumManifolds>{});
+ }
+
+ bool PlusJacobian(const double* x, double* jacobian_ptr) const override {
+ MatrixRef jacobian(jacobian_ptr, AmbientSize(), TangentSize());
+ jacobian.setZero();
+ internal::FixedArray<double> buffer(buffer_size_);
+
+ return PlusJacobianImpl(
+ x, jacobian, buffer, std::make_index_sequence<kNumManifolds>{});
+ }
+
+ bool MinusJacobian(const double* x, double* jacobian_ptr) const override {
+ MatrixRef jacobian(jacobian_ptr, TangentSize(), AmbientSize());
+ jacobian.setZero();
+ internal::FixedArray<double> buffer(buffer_size_);
+
+ return MinusJacobianImpl(
+ x, jacobian, buffer, std::make_index_sequence<kNumManifolds>{});
+ }
+
+ private:
+ static constexpr std::size_t kNumManifolds = 2 + sizeof...(ManifoldN);
+
+ template <std::size_t... Indices, typename... Args>
+ explicit ProductManifold(std::index_sequence<Indices...>, Args&&... manifolds)
+ : manifolds_{std::forward<Args>(manifolds)...},
+ buffer_size_{(std::max)(
+ {(Dereference(std::get<Indices>(manifolds_)).TangentSize() *
+ Dereference(std::get<Indices>(manifolds_)).AmbientSize())...})},
+ ambient_sizes_{
+ Dereference(std::get<Indices>(manifolds_)).AmbientSize()...},
+ tangent_sizes_{
+ Dereference(std::get<Indices>(manifolds_)).TangentSize()...},
+ ambient_offsets_{ExclusiveScan(ambient_sizes_)},
+ tangent_offsets_{ExclusiveScan(tangent_sizes_)},
+ ambient_size_{
+ std::accumulate(ambient_sizes_.begin(), ambient_sizes_.end(), 0)},
+ tangent_size_{
+ std::accumulate(tangent_sizes_.begin(), tangent_sizes_.end(), 0)} {}
+
+ template <std::size_t Index0, std::size_t... Indices>
+ bool PlusImpl(const double* x,
+ const double* delta,
+ double* x_plus_delta,
+ std::index_sequence<Index0, Indices...>) const {
+ if (!Dereference(std::get<Index0>(manifolds_))
+ .Plus(x + ambient_offsets_[Index0],
+ delta + tangent_offsets_[Index0],
+ x_plus_delta + ambient_offsets_[Index0])) {
+ return false;
+ }
+
+ return PlusImpl(x, delta, x_plus_delta, std::index_sequence<Indices...>{});
+ }
+
+ static constexpr bool PlusImpl(const double* /*x*/,
+ const double* /*delta*/,
+ double* /*x_plus_delta*/,
+ std::index_sequence<>) noexcept {
+ return true;
+ }
+
+ template <std::size_t Index0, std::size_t... Indices>
+ bool MinusImpl(const double* y,
+ const double* x,
+ double* y_minus_x,
+ std::index_sequence<Index0, Indices...>) const {
+ if (!Dereference(std::get<Index0>(manifolds_))
+ .Minus(y + ambient_offsets_[Index0],
+ x + ambient_offsets_[Index0],
+ y_minus_x + tangent_offsets_[Index0])) {
+ return false;
+ }
+
+ return MinusImpl(y, x, y_minus_x, std::index_sequence<Indices...>{});
+ }
+
+ static constexpr bool MinusImpl(const double* /*y*/,
+ const double* /*x*/,
+ double* /*y_minus_x*/,
+ std::index_sequence<>) noexcept {
+ return true;
+ }
+
+ template <std::size_t Index0, std::size_t... Indices>
+ bool PlusJacobianImpl(const double* x,
+ MatrixRef& jacobian,
+ internal::FixedArray<double>& buffer,
+ std::index_sequence<Index0, Indices...>) const {
+ if (!Dereference(std::get<Index0>(manifolds_))
+ .PlusJacobian(x + ambient_offsets_[Index0], buffer.data())) {
+ return false;
+ }
+
+ jacobian.block(ambient_offsets_[Index0],
+ tangent_offsets_[Index0],
+ ambient_sizes_[Index0],
+ tangent_sizes_[Index0]) =
+ MatrixRef(
+ buffer.data(), ambient_sizes_[Index0], tangent_sizes_[Index0]);
+
+ return PlusJacobianImpl(
+ x, jacobian, buffer, std::index_sequence<Indices...>{});
+ }
+
+ static constexpr bool PlusJacobianImpl(
+ const double* /*x*/,
+ MatrixRef& /*jacobian*/,
+ internal::FixedArray<double>& /*buffer*/,
+ std::index_sequence<>) noexcept {
+ return true;
+ }
+
+ template <std::size_t Index0, std::size_t... Indices>
+ bool MinusJacobianImpl(const double* x,
+ MatrixRef& jacobian,
+ internal::FixedArray<double>& buffer,
+ std::index_sequence<Index0, Indices...>) const {
+ if (!Dereference(std::get<Index0>(manifolds_))
+ .MinusJacobian(x + ambient_offsets_[Index0], buffer.data())) {
+ return false;
+ }
+
+ jacobian.block(tangent_offsets_[Index0],
+ ambient_offsets_[Index0],
+ tangent_sizes_[Index0],
+ ambient_sizes_[Index0]) =
+ MatrixRef(
+ buffer.data(), tangent_sizes_[Index0], ambient_sizes_[Index0]);
+
+ return MinusJacobianImpl(
+ x, jacobian, buffer, std::index_sequence<Indices...>{});
+ }
+
+ static constexpr bool MinusJacobianImpl(
+ const double* /*x*/,
+ MatrixRef& /*jacobian*/,
+ internal::FixedArray<double>& /*buffer*/,
+ std::index_sequence<>) noexcept {
+ return true;
+ }
+
+ template <typename T, std::size_t N>
+ static std::array<T, N> ExclusiveScan(const std::array<T, N>& values) {
+ std::array<T, N> result;
+ // TODO Replace with std::exclusive_scan once all platforms have full C++17
+ // STL support.
+ T init = 0;
+ for (std::size_t i = 0; i != N; ++i) {
+ result[i] = init;
+ init += values[i];
+ }
+ return result;
+ }
+
+ template <typename T, typename E = void>
+ struct IsDereferenceable : std::false_type {};
+
+ template <typename T>
+ struct IsDereferenceable<T, std::void_t<decltype(*std::declval<T>())>>
+ : std::true_type {};
+
+ template <typename T,
+ std::enable_if_t<!IsDereferenceable<T>::value>* = nullptr>
+ static constexpr decltype(auto) Dereference(T& value) {
+ return value;
+ }
+
+ // Support dereferenceable types such as std::unique_ptr, std::shared_ptr, raw
+ // pointers etc.
+ template <typename T,
+ std::enable_if_t<IsDereferenceable<T>::value>* = nullptr>
+ static constexpr decltype(auto) Dereference(T& value) {
+ return *value;
+ }
+
+ template <typename T>
+ static constexpr decltype(auto) Dereference(T* p) {
+ assert(p != nullptr);
+ return *p;
+ }
+
+ std::tuple<Manifold0, Manifold1, ManifoldN...> manifolds_;
+ int buffer_size_;
+ std::array<int, kNumManifolds> ambient_sizes_;
+ std::array<int, kNumManifolds> tangent_sizes_;
+ std::array<int, kNumManifolds> ambient_offsets_;
+ std::array<int, kNumManifolds> tangent_offsets_;
+ int ambient_size_;
+ int tangent_size_;
+};
+
+// C++17 deduction guide that allows the user to avoid explicitly specifying
+// the template parameters of ProductManifold. The class can instead be
+// instantiated as follows:
+//
+// ProductManifold manifold{QuaternionManifold{}, EuclideanManifold<3>{}};
+//
+template <typename Manifold0, typename Manifold1, typename... Manifolds>
+ProductManifold(Manifold0&&, Manifold1&&, Manifolds&&...)
+ -> ProductManifold<Manifold0, Manifold1, Manifolds...>;
+
+} // namespace ceres
+
+#endif // CERES_PUBLIC_PRODUCT_MANIFOLD_H_
diff --git a/include/ceres/rotation.h b/include/ceres/rotation.h
index 0c82a41..0cccfa7 100644
--- a/include/ceres/rotation.h
+++ b/include/ceres/rotation.h
@@ -1,5 +1,5 @@
// Ceres Solver - A fast non-linear least squares minimizer
-// Copyright 2019 Google Inc. All rights reserved.
+// Copyright 2023 Google Inc. All rights reserved.
// http://ceres-solver.org/
//
// Redistribution and use in source and binary forms, with or without
@@ -47,8 +47,9 @@
#include <algorithm>
#include <cmath>
-#include <limits>
+#include "ceres/constants.h"
+#include "ceres/internal/euler_angles.h"
#include "glog/logging.h"
namespace ceres {
@@ -60,7 +61,7 @@
//
// the expression M(i, j) is equivalent to
//
-// arrary[i * row_stride + j * col_stride]
+// array[i * row_stride + j * col_stride]
//
// Conversion functions to and from rotation matrices accept
// MatrixAdapters to permit using row-major and column-major layouts,
@@ -136,6 +137,71 @@
void EulerAnglesToRotationMatrix(
const T* euler, const MatrixAdapter<T, row_stride, col_stride>& R);
+// Convert a generic Euler Angle sequence (in radians) to a 3x3 rotation matrix.
+//
+// Euler Angles define a sequence of 3 rotations about a sequence of axes,
+// typically taken to be the X, Y, or Z axes. The last axis may be the same as
+// the first axis (e.g. ZYZ) per Euler's original definition of his angles
+// (proper Euler angles) or not (e.g. ZYX / yaw-pitch-roll), per common usage in
+// the nautical and aerospace fields (Tait-Bryan angles). The three rotations
+// may be in a global frame of reference (Extrinsic) or in a body fixed frame of
+// reference (Intrinsic) that moves with the rotating object.
+//
+// Internally, Euler Axis sequences are classified by Ken Shoemake's scheme from
+// "Euler angle conversion", Graphics Gems IV, where a choice of axis for the
+// first rotation and 3 binary choices:
+// 1. Parity of the axis permutation. The axis sequence has Even parity if the
+// second axis of rotation is 'greater-than' the first axis of rotation
+// according to the order X<Y<Z<X, otherwise it has Odd parity.
+// 2. Proper Euler Angles v.s. Tait-Bryan Angles
+// 3. Extrinsic Rotations v.s. Intrinsic Rotations
+// compactly represent all 24 possible Euler Angle Conventions
+//
+// One template parameter: EulerSystem must be explicitly given. This parameter
+// is a tag named by 'Extrinsic' or 'Intrinsic' followed by three characters in
+// the set '[XYZ]', specifying the axis sequence, e.g. ceres::ExtrinsicYZY
+// (robotic arms), ceres::IntrinsicZYX (for aerospace), etc.
+//
+// The order of elements in the input array 'euler' follows the axis sequence
+template <typename EulerSystem, typename T>
+inline void EulerAnglesToRotation(const T* euler, T* R);
+
+template <typename EulerSystem, typename T, int row_stride, int col_stride>
+void EulerAnglesToRotation(const T* euler,
+ const MatrixAdapter<T, row_stride, col_stride>& R);
+
+// Convert a 3x3 rotation matrix to a generic Euler Angle sequence (in radians)
+//
+// Euler Angles define a sequence of 3 rotations about a sequence of axes,
+// typically taken to be the X, Y, or Z axes. The last axis may be the same as
+// the first axis (e.g. ZYZ) per Euler's original definition of his angles
+// (proper Euler angles) or not (e.g. ZYX / yaw-pitch-roll), per common usage in
+// the nautical and aerospace fields (Tait-Bryan angles). The three rotations
+// may be in a global frame of reference (Extrinsic) or in a body fixed frame of
+// reference (Intrinsic) that moves with the rotating object.
+//
+// Internally, Euler Axis sequences are classified by Ken Shoemake's scheme from
+// "Euler angle conversion", Graphics Gems IV, where a choice of axis for the
+// first rotation and 3 binary choices:
+// 1. Oddness of the axis permutation, that defines whether the second axis is
+// 'greater-than' the first axis according to the order X>Y>Z>X)
+// 2. Proper Euler Angles v.s. Tait-Bryan Angles
+// 3. Extrinsic Rotations v.s. Intrinsic Rotations
+// compactly represent all 24 possible Euler Angle Conventions
+//
+// One template parameter: EulerSystem must be explicitly given. This parameter
+// is a tag named by 'Extrinsic' or 'Intrinsic' followed by three characters in
+// the set '[XYZ]', specifying the axis sequence, e.g. ceres::ExtrinsicYZY
+// (robotic arms), ceres::IntrinsicZYX (for aerospace), etc.
+//
+// The order of elements in the output array 'euler' follows the axis sequence
+template <typename EulerSystem, typename T>
+inline void RotationMatrixToEulerAngles(const T* R, T* euler);
+
+template <typename EulerSystem, typename T, int row_stride, int col_stride>
+void RotationMatrixToEulerAngles(
+ const MatrixAdapter<const T, row_stride, col_stride>& R, T* euler);
+
// 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
@@ -247,14 +313,15 @@
template <typename T>
inline void AngleAxisToQuaternion(const T* angle_axis, T* quaternion) {
+ using std::fpclassify;
+ using std::hypot;
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;
+ const T theta = hypot(a0, a1, 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);
+ if (fpclassify(theta) != FP_ZERO) {
const T half_theta = theta * T(0.5);
const T k = sin(half_theta) / theta;
quaternion[0] = cos(half_theta);
@@ -276,15 +343,16 @@
template <typename T>
inline void QuaternionToAngleAxis(const T* quaternion, T* angle_axis) {
+ using std::fpclassify;
+ using std::hypot;
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;
+ const T sin_theta = hypot(q1, q2, 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);
+ if (fpclassify(sin_theta) != FP_ZERO) {
const T& cos_theta = quaternion[0];
// If cos_theta is negative, theta is greater than pi/2, which
@@ -385,13 +453,14 @@
template <typename T, int row_stride, int col_stride>
void AngleAxisToRotationMatrix(
const T* angle_axis, const MatrixAdapter<T, row_stride, col_stride>& R) {
+ using std::fpclassify;
+ using std::hypot;
static const T kOne = T(1.0);
- const T theta2 = DotProduct(angle_axis, angle_axis);
- if (theta2 > T(std::numeric_limits<double>::epsilon())) {
+ const T theta = hypot(angle_axis[0], angle_axis[1], angle_axis[2]);
+ if (fpclassify(theta) != FP_ZERO) {
// 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;
@@ -411,7 +480,7 @@
R(2, 2) = costheta + wz*wz*(kOne - costheta);
// clang-format on
} else {
- // Near zero, we switch to using the first order Taylor expansion.
+ // At 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];
@@ -424,6 +493,141 @@
}
}
+template <typename EulerSystem, typename T>
+inline void EulerAnglesToRotation(const T* euler, T* R) {
+ EulerAnglesToRotation<EulerSystem>(euler, RowMajorAdapter3x3(R));
+}
+
+template <typename EulerSystem, typename T, int row_stride, int col_stride>
+void EulerAnglesToRotation(const T* euler,
+ const MatrixAdapter<T, row_stride, col_stride>& R) {
+ using std::cos;
+ using std::sin;
+
+ const auto [i, j, k] = EulerSystem::kAxes;
+
+ T ea[3];
+ ea[1] = euler[1];
+ if constexpr (EulerSystem::kIsIntrinsic) {
+ ea[0] = euler[2];
+ ea[2] = euler[0];
+ } else {
+ ea[0] = euler[0];
+ ea[2] = euler[2];
+ }
+ if constexpr (EulerSystem::kIsParityOdd) {
+ ea[0] = -ea[0];
+ ea[1] = -ea[1];
+ ea[2] = -ea[2];
+ }
+
+ const T ci = cos(ea[0]);
+ const T cj = cos(ea[1]);
+ const T ch = cos(ea[2]);
+ const T si = sin(ea[0]);
+ const T sj = sin(ea[1]);
+ const T sh = sin(ea[2]);
+ const T cc = ci * ch;
+ const T cs = ci * sh;
+ const T sc = si * ch;
+ const T ss = si * sh;
+ if constexpr (EulerSystem::kIsProperEuler) {
+ R(i, i) = cj;
+ R(i, j) = sj * si;
+ R(i, k) = sj * ci;
+ R(j, i) = sj * sh;
+ R(j, j) = -cj * ss + cc;
+ R(j, k) = -cj * cs - sc;
+ R(k, i) = -sj * ch;
+ R(k, j) = cj * sc + cs;
+ R(k, k) = cj * cc - ss;
+ } else {
+ R(i, i) = cj * ch;
+ R(i, j) = sj * sc - cs;
+ R(i, k) = sj * cc + ss;
+ R(j, i) = cj * sh;
+ R(j, j) = sj * ss + cc;
+ R(j, k) = sj * cs - sc;
+ R(k, i) = -sj;
+ R(k, j) = cj * si;
+ R(k, k) = cj * ci;
+ }
+}
+
+template <typename EulerSystem, typename T>
+inline void RotationMatrixToEulerAngles(const T* R, T* euler) {
+ RotationMatrixToEulerAngles<EulerSystem>(RowMajorAdapter3x3(R), euler);
+}
+
+template <typename EulerSystem, typename T, int row_stride, int col_stride>
+void RotationMatrixToEulerAngles(
+ const MatrixAdapter<const T, row_stride, col_stride>& R, T* euler) {
+ using std::atan2;
+ using std::fpclassify;
+ using std::hypot;
+
+ const auto [i, j, k] = EulerSystem::kAxes;
+
+ T ea[3];
+ if constexpr (EulerSystem::kIsProperEuler) {
+ const T sy = hypot(R(i, j), R(i, k));
+ if (fpclassify(sy) != FP_ZERO) {
+ ea[0] = atan2(R(i, j), R(i, k));
+ ea[1] = atan2(sy, R(i, i));
+ ea[2] = atan2(R(j, i), -R(k, i));
+ } else {
+ ea[0] = atan2(-R(j, k), R(j, j));
+ ea[1] = atan2(sy, R(i, i));
+ ea[2] = T(0.0);
+ }
+ } else {
+ const T cy = hypot(R(i, i), R(j, i));
+ if (fpclassify(cy) != FP_ZERO) {
+ ea[0] = atan2(R(k, j), R(k, k));
+ ea[1] = atan2(-R(k, i), cy);
+ ea[2] = atan2(R(j, i), R(i, i));
+ } else {
+ ea[0] = atan2(-R(j, k), R(j, j));
+ ea[1] = atan2(-R(k, i), cy);
+ ea[2] = T(0.0);
+ }
+ }
+ if constexpr (EulerSystem::kIsParityOdd) {
+ ea[0] = -ea[0];
+ ea[1] = -ea[1];
+ ea[2] = -ea[2];
+ }
+ euler[1] = ea[1];
+ if constexpr (EulerSystem::kIsIntrinsic) {
+ euler[0] = ea[2];
+ euler[2] = ea[0];
+ } else {
+ euler[0] = ea[0];
+ euler[2] = ea[2];
+ }
+
+ // Proper euler angles are defined for angles in
+ // [-pi, pi) x [0, pi / 2) x [-pi, pi)
+ // which is enforced here
+ if constexpr (EulerSystem::kIsProperEuler) {
+ const T kPi(constants::pi);
+ const T kTwoPi(2.0 * kPi);
+ if (euler[1] < T(0.0) || ea[1] > kPi) {
+ euler[0] += kPi;
+ euler[1] = -euler[1];
+ euler[2] -= kPi;
+ }
+
+ for (int i = 0; i < 3; ++i) {
+ if (euler[i] < -kPi) {
+ euler[i] += kTwoPi;
+ } else if (euler[i] > kPi) {
+ euler[i] -= kTwoPi;
+ }
+ }
+ }
+}
+
template <typename T>
inline void EulerAnglesToRotationMatrix(const T* euler,
const int row_stride_parameter,
@@ -521,18 +725,18 @@
DCHECK_NE(pt, result) << "Inplace rotation is not supported.";
// clang-format off
- 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
+ T uv0 = q[2] * pt[2] - q[3] * pt[1];
+ T uv1 = q[3] * pt[0] - q[1] * pt[2];
+ T uv2 = q[1] * pt[1] - q[2] * pt[0];
+ uv0 += uv0;
+ uv1 += uv1;
+ uv2 += uv2;
+ result[0] = pt[0] + q[0] * uv0;
+ result[1] = pt[1] + q[0] * uv1;
+ result[2] = pt[2] + q[0] * uv2;
+ result[0] += q[2] * uv2 - q[3] * uv1;
+ result[1] += q[3] * uv0 - q[1] * uv2;
+ result[2] += q[1] * uv1 - q[2] * uv0;
// clang-format on
}
@@ -589,9 +793,12 @@
const T pt[3],
T result[3]) {
DCHECK_NE(pt, result) << "Inplace rotation is not supported.";
+ using std::fpclassify;
+ using std::hypot;
- const T theta2 = DotProduct(angle_axis, angle_axis);
- if (theta2 > T(std::numeric_limits<double>::epsilon())) {
+ const T theta = hypot(angle_axis[0], angle_axis[1], angle_axis[2]);
+
+ if (fpclassify(theta) != FP_ZERO) {
// Away from zero, use the rodriguez formula
//
// result = pt costheta +
@@ -602,7 +809,6 @@
// 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 = T(1.0) / theta;
@@ -623,19 +829,19 @@
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
+ // At zero, the first order Taylor approximation of the rotation
+ // matrix R corresponding to a vector w and angle theta is
//
// R = I + hat(w) * sin(theta)
//
// But sintheta ~ theta and theta * w = angle_axis, which gives us
//
- // R = I + hat(w)
+ // R = I + hat(angle_axis)
//
// and actually performing multiplication with the point pt, gives us
- // R * pt = pt + w x pt.
+ // R * pt = pt + angle_axis x pt.
//
- // Switching to the Taylor expansion near zero provides meaningful
+ // Switching to the Taylor expansion at zero provides meaningful
// derivatives when evaluated using Jets.
//
// Explicitly inlined evaluation of the cross product for
diff --git a/include/ceres/sized_cost_function.h b/include/ceres/sized_cost_function.h
index 8e92f1b..8928c19 100644
--- a/include/ceres/sized_cost_function.h
+++ b/include/ceres/sized_cost_function.h
@@ -1,5 +1,5 @@
// Ceres Solver - A fast non-linear least squares minimizer
-// Copyright 2019 Google Inc. All rights reserved.
+// Copyright 2024 Google Inc. All rights reserved.
// http://ceres-solver.org/
//
// Redistribution and use in source and binary forms, with or without
@@ -38,9 +38,10 @@
#ifndef CERES_PUBLIC_SIZED_COST_FUNCTION_H_
#define CERES_PUBLIC_SIZED_COST_FUNCTION_H_
+#include <initializer_list>
+
#include "ceres/cost_function.h"
#include "ceres/types.h"
-#include "glog/logging.h"
#include "internal/parameter_dims.h"
namespace ceres {
@@ -58,11 +59,9 @@
SizedCostFunction() {
set_num_residuals(kNumResiduals);
- *mutable_parameter_block_sizes() = std::vector<int32_t>{Ns...};
+ *mutable_parameter_block_sizes() = std::initializer_list<int32_t>{Ns...};
}
- virtual ~SizedCostFunction() {}
-
// Subclasses must implement Evaluate().
};
diff --git a/include/ceres/solver.h b/include/ceres/solver.h
index 61b8dd5..68438a1 100644
--- a/include/ceres/solver.h
+++ b/include/ceres/solver.h
@@ -1,5 +1,5 @@
// Ceres Solver - A fast non-linear least squares minimizer
-// Copyright 2019 Google Inc. All rights reserved.
+// Copyright 2023 Google Inc. All rights reserved.
// http://ceres-solver.org/
//
// Redistribution and use in source and binary forms, with or without
@@ -38,8 +38,9 @@
#include <vector>
#include "ceres/crs_matrix.h"
+#include "ceres/internal/config.h"
#include "ceres/internal/disable_warnings.h"
-#include "ceres/internal/port.h"
+#include "ceres/internal/export.h"
#include "ceres/iteration_callback.h"
#include "ceres/ordered_groups.h"
#include "ceres/problem.h"
@@ -63,8 +64,6 @@
// with a message describing the problem.
bool IsValid(std::string* error) const;
- // Minimizer options ----------------------------------------
-
// Ceres supports the two major families of optimization strategies -
// Trust Region and Line Search.
//
@@ -363,102 +362,158 @@
std::unordered_set<ResidualBlockId>
residual_blocks_for_subset_preconditioner;
- // Ceres supports using multiple dense linear algebra libraries
- // for dense matrix factorizations. Currently EIGEN and LAPACK are
- // the valid choices. EIGEN is always available, LAPACK refers to
- // the system BLAS + LAPACK library which may or may not be
+ // Ceres supports using multiple dense linear algebra libraries for dense
+ // matrix factorizations. Currently EIGEN, LAPACK and CUDA are the valid
+ // choices. EIGEN is always available, LAPACK refers to the system BLAS +
+ // LAPACK library which may or may not be available. CUDA refers to Nvidia's
+ // GPU based dense linear algebra library, which may or may not be
// available.
//
- // This setting affects the DENSE_QR, DENSE_NORMAL_CHOLESKY and
- // DENSE_SCHUR solvers. For small to moderate sized problem EIGEN
- // is a fine choice but for large problems, an optimized LAPACK +
- // BLAS implementation can make a substantial difference in
- // performance.
+ // This setting affects the DENSE_QR, DENSE_NORMAL_CHOLESKY and DENSE_SCHUR
+ // solvers. For small to moderate sized problem EIGEN is a fine choice but
+ // for large problems, an optimized LAPACK + BLAS or CUDA implementation can
+ // make a substantial difference in performance.
DenseLinearAlgebraLibraryType dense_linear_algebra_library_type = EIGEN;
- // Ceres supports using multiple sparse linear algebra libraries
- // for sparse matrix ordering and factorizations. Currently,
- // SUITE_SPARSE and CX_SPARSE are the valid choices, depending on
- // whether they are linked into Ceres at build time.
+ // Ceres supports using multiple sparse linear algebra libraries for sparse
+ // matrix ordering and factorizations.
SparseLinearAlgebraLibraryType sparse_linear_algebra_library_type =
#if !defined(CERES_NO_SUITESPARSE)
SUITE_SPARSE;
-#elif defined(CERES_USE_EIGEN_SPARSE)
- EIGEN_SPARSE;
-#elif !defined(CERES_NO_CXSPARSE)
- CX_SPARSE;
#elif !defined(CERES_NO_ACCELERATE_SPARSE)
ACCELERATE_SPARSE;
+#elif defined(CERES_USE_EIGEN_SPARSE)
+ EIGEN_SPARSE;
#else
NO_SPARSE;
#endif
// The order in which variables are eliminated in a linear solver
- // can have a significant of impact on the efficiency and accuracy
- // of the method. e.g., when doing sparse Cholesky factorization,
+ // can have a significant impact on the efficiency and accuracy of
+ // the method. e.g., when doing sparse Cholesky factorization,
// there are matrices for which a good ordering will give a
// Cholesky factor with O(n) storage, where as a bad ordering will
// result in an completely dense factor.
//
- // Ceres allows the user to provide varying amounts of hints to
- // the solver about the variable elimination ordering to use. This
- // can range from no hints, where the solver is free to decide the
- // best possible ordering based on the user's choices like the
- // linear solver being used, to an exact order in which the
- // variables should be eliminated, and a variety of possibilities
- // in between.
+ // Sparse direct solvers like SPARSE_NORMAL_CHOLESKY and
+ // SPARSE_SCHUR use a fill reducing ordering of the columns and
+ // rows of the matrix being factorized before computing the
+ // numeric factorization.
//
- // Instances of the ParameterBlockOrdering class are used to
- // communicate this information to Ceres.
+ // This enum controls the type of algorithm used to compute
+ // this fill reducing ordering. There is no single algorithm
+ // that works on all matrices, so determining which algorithm
+ // works better is a matter of empirical experimentation.
//
- // Formally an ordering is an ordered partitioning of the
- // parameter blocks, i.e, each parameter block belongs to exactly
- // one group, and each group has a unique non-negative integer
- // associated with it, that determines its order in the set of
- // groups.
+ // The exact behaviour of this setting is affected by the value of
+ // linear_solver_ordering as described below.
+ LinearSolverOrderingType linear_solver_ordering_type = AMD;
+
+ // Besides specifying the fill reducing ordering via
+ // linear_solver_ordering_type, Ceres allows the user to provide varying
+ // amounts of hints to the linear solver about the variable elimination
+ // ordering to use. This can range from no hints, where the solver is free
+ // to decide the best possible ordering based on the user's choices like the
+ // linear solver being used, to an exact order in which the variables should
+ // be eliminated, and a variety of possibilities in between.
//
- // Given such an ordering, Ceres ensures that the parameter blocks in
- // the lowest numbered group are eliminated first, and then the
- // parameter blocks in the next lowest numbered group and so on. Within
- // each group, Ceres is free to order the parameter blocks as it
- // chooses.
+ // Instances of the ParameterBlockOrdering class are used to communicate
+ // this information to Ceres.
//
- // If NULL, then all parameter blocks are assumed to be in the
- // same group and the solver is free to decide the best
- // ordering.
+ // Formally an ordering is an ordered partitioning of the parameter blocks,
+ // i.e, each parameter block belongs to exactly one group, and each group
+ // has a unique non-negative integer associated with it, that determines its
+ // order in the set of groups.
//
// e.g. Consider the linear system
//
// x + y = 3
// 2x + 3y = 7
//
- // There are two ways in which it can be solved. First eliminating x
- // from the two equations, solving for y and then back substituting
- // for x, or first eliminating y, solving for x and back substituting
- // for y. The user can construct three orderings here.
+ // There are two ways in which it can be solved. First eliminating x from
+ // the two equations, solving for y and then back substituting for x, or
+ // first eliminating y, solving for x and back substituting for y. The user
+ // can construct three orderings here.
//
// {0: x}, {1: y} - eliminate x first.
// {0: y}, {1: x} - eliminate y first.
// {0: x, y} - Solver gets to decide the elimination order.
//
- // Thus, to have Ceres determine the ordering automatically using
- // heuristics, put all the variables in group 0 and to control the
- // ordering for every variable, create groups 0..N-1, one per
- // variable, in the desired order.
+ // Thus, to have Ceres determine the ordering automatically, put all the
+ // variables in group 0 and to control the ordering for every variable
+ // create groups 0 ... N-1, one per variable, in the desired
+ // order.
+ //
+ // linear_solver_ordering == nullptr and an ordering where all the parameter
+ // blocks are in one elimination group mean the same thing - the solver is
+ // free to choose what it thinks is the best elimination ordering. Therefore
+ // in the following we will only consider the case where
+ // linear_solver_ordering is nullptr.
+ //
+ // The exact interpretation of this information depends on the values of
+ // linear_solver_ordering_type and linear_solver_type/preconditioner_type
+ // and sparse_linear_algebra_type.
//
// Bundle Adjustment
- // -----------------
+ // =================
//
- // A particular case of interest is bundle adjustment, where the user
- // has two options. The default is to not specify an ordering at all,
- // the solver will see that the user wants to use a Schur type solver
- // and figure out the right elimination ordering.
+ // If the user is using one of the Schur solvers (DENSE_SCHUR,
+ // SPARSE_SCHUR, ITERATIVE_SCHUR) and chooses to specify an
+ // ordering, it must have one important property. The lowest
+ // numbered elimination group must form an independent set in the
+ // graph corresponding to the Hessian, or in other words, no two
+ // parameter blocks in in the first elimination group should
+ // co-occur in the same residual block. For the best performance,
+ // this elimination group should be as large as possible. For
+ // standard bundle adjustment problems, this corresponds to the
+ // first elimination group containing all the 3d points, and the
+ // second containing the all the cameras parameter blocks.
//
- // But if the user already knows what parameter blocks are points and
- // what are cameras, they can save preprocessing time by partitioning
- // the parameter blocks into two groups, one for the points and one
- // for the cameras, where the group containing the points has an id
- // smaller than the group containing cameras.
+ // If the user leaves the choice to Ceres, then the solver uses an
+ // approximate maximum independent set algorithm to identify the first
+ // elimination group.
+ //
+ // sparse_linear_algebra_library_type = SUITE_SPARSE
+ // =================================================
+ //
+ // linear_solver_ordering_type = AMD
+ // ---------------------------------
+ //
+ // A Constrained Approximate Minimum Degree (CAMD) ordering used where the
+ // parameter blocks in the lowest numbered group are eliminated first, and
+ // then the parameter blocks in the next lowest numbered group and so
+ // on. Within each group, CAMD free to order the parameter blocks as it
+ // chooses.
+ //
+ // linear_solver_ordering_type = NESDIS
+ // -------------------------------------
+ //
+ // a. linear_solver_type = SPARSE_NORMAL_CHOLESKY or
+ // linear_solver_type = CGNR and preconditioner_type = SUBSET
+ //
+ // The value of linear_solver_ordering is ignored and a Nested Dissection
+ // algorithm is used to compute a fill reducing ordering.
+ //
+ // b. linear_solver_type = SPARSE_SCHUR/DENSE_SCHUR/ITERATIVE_SCHUR
+ //
+ // ONLY the lowest group are used to compute the Schur complement, and
+ // Nested Dissection is used to compute a fill reducing ordering for the
+ // Schur Complement (or its preconditioner).
+ //
+ // sparse_linear_algebra_library_type = EIGEN_SPARSE or ACCELERATE_SPARSE
+ // ======================================================================
+ //
+ // a. linear_solver_type = SPARSE_NORMAL_CHOLESKY or
+ // linear_solver_type = CGNR and preconditioner_type = SUBSET
+ //
+ // then the value of linear_solver_ordering is ignored and AMD or NESDIS is
+ // used to compute a fill reducing ordering as requested by the user.
+ //
+ // b. linear_solver_type = SPARSE_SCHUR/DENSE_SCHUR/ITERATIVE_SCHUR
+ //
+ // ONLY the lowest group are used to compute the Schur complement, and AMD
+ // or NESDIS is used to compute a fill reducing ordering for the Schur
+ // Complement (or its preconditioner).
std::shared_ptr<ParameterBlockOrdering> linear_solver_ordering;
// Use an explicitly computed Schur complement matrix with
@@ -499,12 +554,6 @@
// Jacobian matrix and generally speaking, there is no performance
// penalty for doing so.
- // In some rare cases, it is worth using a more complicated
- // reordering algorithm which has slightly better runtime
- // performance at the expense of an extra copy of the Jacobian
- // matrix. Setting use_postordering to true enables this tradeoff.
- bool use_postordering = false;
-
// Some non-linear least squares problems are symbolically dense but
// numerically sparse. i.e. at any given state only a small number
// of jacobian entries are non-zero, but the position and number of
@@ -520,11 +569,6 @@
// This settings only affects the SPARSE_NORMAL_CHOLESKY solver.
bool dynamic_sparsity = false;
- // TODO(sameeragarwal): Further expand the documentation for the
- // following two options.
-
- // NOTE1: EXPERIMENTAL FEATURE, UNDER DEVELOPMENT, USE AT YOUR OWN RISK.
- //
// If use_mixed_precision_solves is true, the Gauss-Newton matrix
// is computed in double precision, but its factorization is
// computed in single precision. This can result in significant
@@ -535,15 +579,57 @@
// If use_mixed_precision_solves is true, we recommend setting
// max_num_refinement_iterations to 2-3.
//
- // NOTE2: The following two options are currently only applicable
- // if sparse_linear_algebra_library_type is EIGEN_SPARSE and
- // linear_solver_type is SPARSE_NORMAL_CHOLESKY, or SPARSE_SCHUR.
+ // This options is available when linear solver uses sparse or dense
+ // cholesky factorization, except when sparse_linear_algebra_library_type =
+ // SUITE_SPARSE.
bool use_mixed_precision_solves = false;
// Number steps of the iterative refinement process to run when
// computing the Gauss-Newton step.
int max_num_refinement_iterations = 0;
+ // Minimum number of iterations for which the linear solver should
+ // run, even if the convergence criterion is satisfied.
+ int min_linear_solver_iterations = 0;
+
+ // Maximum number of iterations for which the linear solver should
+ // run. If the solver does not converge in less than
+ // max_linear_solver_iterations, then it returns MAX_ITERATIONS,
+ // as its termination type.
+ int max_linear_solver_iterations = 500;
+
+ // Maximum number of iterations performed by SCHUR_POWER_SERIES_EXPANSION.
+ // Each iteration corresponds to one more term in the power series expansion
+ // od the inverse of the Schur complement. This value controls the maximum
+ // number of iterations whether it is used as a preconditioner or just to
+ // initialize the solution for ITERATIVE_SCHUR.
+ int max_num_spse_iterations = 5;
+
+ // Use SCHUR_POWER_SERIES_EXPANSION to initialize the solution for
+ // ITERATIVE_SCHUR. This option can be set true regardless of what
+ // preconditioner is being used.
+ bool use_spse_initialization = false;
+
+ // When use_spse_initialization is true, this parameter along with
+ // max_num_spse_iterations controls the number of
+ // SCHUR_POWER_SERIES_EXPANSION iterations performed for initialization. It
+ // is not used to control the preconditioner.
+ double spse_tolerance = 0.1;
+
+ // Forcing sequence parameter. The truncated Newton solver uses
+ // this number to control the relative accuracy with which the
+ // Newton step is computed.
+ //
+ // This constant is passed to ConjugateGradientsSolver which uses
+ // it to terminate the iterations when
+ //
+ // (Q_i - Q_{i-1})/Q_i < eta/i
+ double eta = 1e-1;
+
+ // Normalize the jacobian using Jacobi scaling before calling
+ // the linear least squares solver.
+ bool jacobi_scaling = true;
+
// Some non-linear least squares problems have additional
// structure in the way the parameter blocks interact that it is
// beneficial to modify the way the trust region step is computed.
@@ -627,32 +713,6 @@
// iterations is disabled.
double inner_iteration_tolerance = 1e-3;
- // Minimum number of iterations for which the linear solver should
- // run, even if the convergence criterion is satisfied.
- int min_linear_solver_iterations = 0;
-
- // Maximum number of iterations for which the linear solver should
- // run. If the solver does not converge in less than
- // max_linear_solver_iterations, then it returns MAX_ITERATIONS,
- // as its termination type.
- int max_linear_solver_iterations = 500;
-
- // Forcing sequence parameter. The truncated Newton solver uses
- // this number to control the relative accuracy with which the
- // Newton step is computed.
- //
- // This constant is passed to ConjugateGradientsSolver which uses
- // it to terminate the iterations when
- //
- // (Q_i - Q_{i-1})/Q_i < eta/i
- double eta = 1e-1;
-
- // Normalize the jacobian using Jacobi scaling before calling
- // the linear least squares solver.
- bool jacobi_scaling = true;
-
- // Logging options ---------------------------------------------------------
-
LoggingType logging_type = PER_MINIMIZER_ITERATION;
// By default the Minimizer progress is logged to VLOG(1), which
@@ -789,10 +849,9 @@
// IterationSummary for each minimizer iteration in order.
std::vector<IterationSummary> iterations;
- // Number of minimizer iterations in which the step was
- // accepted. Unless use_non_monotonic_steps is true this is also
- // the number of steps in which the objective function value/cost
- // went down.
+ // Number of minimizer iterations in which the step was accepted. Unless
+ // use_nonmonotonic_steps is true this is also the number of steps in which
+ // the objective function value/cost went down.
int num_successful_steps = -1;
// Number of minimizer iterations in which the step was rejected
@@ -882,7 +941,7 @@
// Dimension of the tangent space of the problem (or the number of
// columns in the Jacobian for the problem). This is different
// from num_parameters if a parameter block is associated with a
- // LocalParameterization
+ // Manifold.
int num_effective_parameters = -1;
// Number of residual blocks in the problem.
@@ -903,7 +962,7 @@
// number of columns in the Jacobian for the reduced
// problem). This is different from num_parameters_reduced if a
// parameter block in the reduced problem is associated with a
- // LocalParameterization.
+ // Manifold.
int num_effective_parameters_reduced = -1;
// Number of residual blocks in the reduced problem.
@@ -920,8 +979,7 @@
int num_threads_given = -1;
// Number of threads actually used by the solver for Jacobian and
- // residual evaluation. This number is not equal to
- // num_threads_given if OpenMP is not available.
+ // residual evaluation.
int num_threads_used = -1;
// Type of the linear solver requested by the user.
@@ -944,6 +1002,10 @@
SPARSE_NORMAL_CHOLESKY;
#endif
+ bool mixed_precision_solves_used = false;
+
+ LinearSolverOrderingType linear_solver_ordering_type = AMD;
+
// Size of the elimination groups given by the user as hints to
// the linear solver.
std::vector<int> linear_solver_ordering_given;
@@ -1003,7 +1065,7 @@
PreconditionerType preconditioner_type_used = IDENTITY;
// Type of clustering algorithm used for visibility based
- // preconditioning. Only meaningful when the preconditioner_type
+ // preconditioning. Only meaningful when the preconditioner_type_used
// is CLUSTER_JACOBI or CLUSTER_TRIDIAGONAL.
VisibilityClusteringType visibility_clustering_type = CANONICAL_VIEWS;
diff --git a/include/ceres/sphere_manifold.h b/include/ceres/sphere_manifold.h
new file mode 100644
index 0000000..1c7458b
--- /dev/null
+++ b/include/ceres/sphere_manifold.h
@@ -0,0 +1,236 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2023 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: vitus@google.com (Mike Vitus)
+// jodebo_beck@gmx.de (Johannes Beck)
+
+#ifndef CERES_PUBLIC_SPHERE_MANIFOLD_H_
+#define CERES_PUBLIC_SPHERE_MANIFOLD_H_
+
+#include <Eigen/Core>
+#include <algorithm>
+#include <array>
+#include <memory>
+#include <vector>
+
+#include "ceres/internal/disable_warnings.h"
+#include "ceres/internal/export.h"
+#include "ceres/internal/householder_vector.h"
+#include "ceres/internal/sphere_manifold_functions.h"
+#include "ceres/manifold.h"
+#include "ceres/types.h"
+#include "glog/logging.h"
+
+namespace ceres {
+
+// This provides a manifold on a sphere meaning that the norm of the vector
+// stays the same. Such cases often arises 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
+//
+// The minus operator is defined as
+// Minus(x, y) = 2 atan2(nhy, y[-1]) / nhy * hy[0 : size_ - 1]
+// with nhy = norm(hy[0 : size_ - 1])
+//
+// with * defined as an operator which applies the update orthogonal to x to
+// remain on the sphere. The ambient space dimension is required to be greater
+// than 1.
+//
+// The class works with dynamic and static ambient space dimensions. If the
+// ambient space dimensions is known at compile time use
+//
+// SphereManifold<3> manifold;
+//
+// If the ambient space dimensions is not known at compile time the template
+// parameter needs to be set to ceres::DYNAMIC and the actual dimension needs
+// to be provided as a constructor argument:
+//
+// SphereManifold<ceres::DYNAMIC> manifold(ambient_dim);
+//
+// See section B.2 (p.25) in "Integrating Generic Sensor Fusion Algorithms
+// with Sound State Representations through Encapsulation of Manifolds" by C.
+// Hertzberg, R. Wagner, U. Frese and L. Schroder for more details
+// (https://arxiv.org/pdf/1107.1119.pdf)
+template <int AmbientSpaceDimension>
+class SphereManifold final : public Manifold {
+ public:
+ static_assert(
+ AmbientSpaceDimension == ceres::DYNAMIC || AmbientSpaceDimension > 1,
+ "The size of the homogeneous vector needs to be greater than 1.");
+ static_assert(ceres::DYNAMIC == Eigen::Dynamic,
+ "ceres::DYNAMIC needs to be the same as Eigen::Dynamic.");
+
+ SphereManifold();
+ explicit SphereManifold(int size);
+
+ int AmbientSize() const override {
+ return AmbientSpaceDimension == ceres::DYNAMIC ? size_
+ : AmbientSpaceDimension;
+ }
+ int TangentSize() const override { return AmbientSize() - 1; }
+
+ bool Plus(const double* x,
+ const double* delta,
+ double* x_plus_delta) const override;
+ bool PlusJacobian(const double* x, double* jacobian) const override;
+
+ bool Minus(const double* y,
+ const double* x,
+ double* y_minus_x) const override;
+ bool MinusJacobian(const double* x, double* jacobian) const override;
+
+ private:
+ static constexpr int TangentSpaceDimension =
+ AmbientSpaceDimension > 0 ? AmbientSpaceDimension - 1 : Eigen::Dynamic;
+
+ // NOTE: Eigen does not allow to have a RowMajor column vector.
+ // In that case, change the storage order
+ static constexpr int SafeRowMajor =
+ TangentSpaceDimension == 1 ? Eigen::ColMajor : Eigen::RowMajor;
+
+ using AmbientVector = Eigen::Matrix<double, AmbientSpaceDimension, 1>;
+ using TangentVector = Eigen::Matrix<double, TangentSpaceDimension, 1>;
+ using MatrixPlusJacobian = Eigen::Matrix<double,
+ AmbientSpaceDimension,
+ TangentSpaceDimension,
+ SafeRowMajor>;
+ using MatrixMinusJacobian = Eigen::Matrix<double,
+ TangentSpaceDimension,
+ AmbientSpaceDimension,
+ Eigen::RowMajor>;
+
+ const int size_{};
+};
+
+template <int AmbientSpaceDimension>
+SphereManifold<AmbientSpaceDimension>::SphereManifold()
+ : size_{AmbientSpaceDimension} {
+ static_assert(
+ AmbientSpaceDimension != Eigen::Dynamic,
+ "The size is set to dynamic. Please call the constructor with a size.");
+}
+
+template <int AmbientSpaceDimension>
+SphereManifold<AmbientSpaceDimension>::SphereManifold(int size) : size_{size} {
+ if (AmbientSpaceDimension != Eigen::Dynamic) {
+ CHECK_EQ(AmbientSpaceDimension, size)
+ << "Specified size by template parameter differs from the supplied "
+ "one.";
+ } else {
+ CHECK_GT(size_, 1)
+ << "The size of the manifold needs to be greater than 1.";
+ }
+}
+
+template <int AmbientSpaceDimension>
+bool SphereManifold<AmbientSpaceDimension>::Plus(
+ const double* x_ptr,
+ const double* delta_ptr,
+ double* x_plus_delta_ptr) const {
+ Eigen::Map<const AmbientVector> x(x_ptr, size_);
+ Eigen::Map<const TangentVector> delta(delta_ptr, size_ - 1);
+ Eigen::Map<AmbientVector> x_plus_delta(x_plus_delta_ptr, size_);
+
+ const double norm_delta = delta.norm();
+
+ if (norm_delta == 0.0) {
+ x_plus_delta = x;
+ return true;
+ }
+
+ AmbientVector v(size_);
+ double beta;
+
+ // NOTE: The explicit template arguments are needed here because
+ // ComputeHouseholderVector is templated and some versions of MSVC
+ // have trouble deducing the type of v automatically.
+ internal::ComputeHouseholderVector<Eigen::Map<const AmbientVector>,
+ double,
+ AmbientSpaceDimension>(x, &v, &beta);
+
+ internal::ComputeSphereManifoldPlus(
+ v, beta, x, delta, norm_delta, &x_plus_delta);
+
+ return true;
+}
+
+template <int AmbientSpaceDimension>
+bool SphereManifold<AmbientSpaceDimension>::PlusJacobian(
+ const double* x_ptr, double* jacobian_ptr) const {
+ Eigen::Map<const AmbientVector> x(x_ptr, size_);
+ Eigen::Map<MatrixPlusJacobian> jacobian(jacobian_ptr, size_, size_ - 1);
+ internal::ComputeSphereManifoldPlusJacobian(x, &jacobian);
+
+ return true;
+}
+
+template <int AmbientSpaceDimension>
+bool SphereManifold<AmbientSpaceDimension>::Minus(const double* y_ptr,
+ const double* x_ptr,
+ double* y_minus_x_ptr) const {
+ AmbientVector y = Eigen::Map<const AmbientVector>(y_ptr, size_);
+ Eigen::Map<const AmbientVector> x(x_ptr, size_);
+ Eigen::Map<TangentVector> y_minus_x(y_minus_x_ptr, size_ - 1);
+
+ // Apply hoseholder transformation.
+ AmbientVector v(size_);
+ double beta;
+
+ // NOTE: The explicit template arguments are needed here because
+ // ComputeHouseholderVector is templated and some versions of MSVC
+ // have trouble deducing the type of v automatically.
+ internal::ComputeHouseholderVector<Eigen::Map<const AmbientVector>,
+ double,
+ AmbientSpaceDimension>(x, &v, &beta);
+ internal::ComputeSphereManifoldMinus(v, beta, x, y, &y_minus_x);
+ return true;
+}
+
+template <int AmbientSpaceDimension>
+bool SphereManifold<AmbientSpaceDimension>::MinusJacobian(
+ const double* x_ptr, double* jacobian_ptr) const {
+ Eigen::Map<const AmbientVector> x(x_ptr, size_);
+ Eigen::Map<MatrixMinusJacobian> jacobian(jacobian_ptr, size_ - 1, size_);
+
+ internal::ComputeSphereManifoldMinusJacobian(x, &jacobian);
+ return true;
+}
+
+} // namespace ceres
+
+// clang-format off
+#include "ceres/internal/reenable_warnings.h"
+// clang-format on
+
+#endif // CERES_PUBLIC_SPHERE_MANIFOLD_H_
diff --git a/include/ceres/tiny_solver.h b/include/ceres/tiny_solver.h
index 47db582..9242cd0 100644
--- a/include/ceres/tiny_solver.h
+++ b/include/ceres/tiny_solver.h
@@ -1,5 +1,5 @@
// Ceres Solver - A fast non-linear least squares minimizer
-// Copyright 2019 Google Inc. All rights reserved.
+// Copyright 2023 Google Inc. All rights reserved.
// http://ceres-solver.org/
//
// Redistribution and use in source and binary forms, with or without
@@ -84,7 +84,8 @@
// double* parameters -- NUM_PARAMETERS or NumParameters()
// double* residuals -- NUM_RESIDUALS or NumResiduals()
// double* jacobian -- NUM_RESIDUALS * NUM_PARAMETERS in column-major format
-// (Eigen's default); or NULL if no jacobian requested.
+// (Eigen's default); or nullptr if no jacobian
+// requested.
//
// An example (fully statically sized):
//
@@ -126,8 +127,8 @@
//
template <typename Function,
typename LinearSolver =
- Eigen::LDLT<Eigen::Matrix<typename Function::Scalar,
- Function::NUM_PARAMETERS,
+ Eigen::LDLT<Eigen::Matrix<typename Function::Scalar, //
+ Function::NUM_PARAMETERS, //
Function::NUM_PARAMETERS>>>
class TinySolver {
public:
@@ -139,41 +140,59 @@
NUM_RESIDUALS = Function::NUM_RESIDUALS,
NUM_PARAMETERS = Function::NUM_PARAMETERS
};
- typedef typename Function::Scalar Scalar;
- typedef typename Eigen::Matrix<Scalar, NUM_PARAMETERS, 1> Parameters;
+ using Scalar = typename Function::Scalar;
+ using Parameters = typename Eigen::Matrix<Scalar, NUM_PARAMETERS, 1>;
enum Status {
- GRADIENT_TOO_SMALL, // eps > max(J'*f(x))
- RELATIVE_STEP_SIZE_TOO_SMALL, // eps > ||dx|| / (||x|| + eps)
- COST_TOO_SMALL, // eps > ||f(x)||^2 / 2
+ // max_norm |J'(x) * f(x)| < gradient_tolerance
+ GRADIENT_TOO_SMALL,
+ // ||dx|| <= parameter_tolerance * (||x|| + parameter_tolerance)
+ RELATIVE_STEP_SIZE_TOO_SMALL,
+ // cost_threshold > ||f(x)||^2 / 2
+ COST_TOO_SMALL,
+ // num_iterations >= max_num_iterations
HIT_MAX_ITERATIONS,
+ // (new_cost - old_cost) < function_tolerance * old_cost
+ COST_CHANGE_TOO_SMALL,
// TODO(sameeragarwal): Deal with numerical failures.
};
struct Options {
- Scalar gradient_tolerance = 1e-10; // eps > max(J'*f(x))
- Scalar parameter_tolerance = 1e-8; // eps > ||dx|| / ||x||
- Scalar cost_threshold = // eps > ||f(x)||
- std::numeric_limits<Scalar>::epsilon();
- Scalar initial_trust_region_radius = 1e4;
int max_num_iterations = 50;
+
+ // max_norm |J'(x) * f(x)| < gradient_tolerance
+ Scalar gradient_tolerance = 1e-10;
+
+ // ||dx|| <= parameter_tolerance * (||x|| + parameter_tolerance)
+ Scalar parameter_tolerance = 1e-8;
+
+ // (new_cost - old_cost) < function_tolerance * old_cost
+ Scalar function_tolerance = 1e-6;
+
+ // cost_threshold > ||f(x)||^2 / 2
+ Scalar cost_threshold = std::numeric_limits<Scalar>::epsilon();
+
+ Scalar initial_trust_region_radius = 1e4;
};
struct Summary {
- Scalar initial_cost = -1; // 1/2 ||f(x)||^2
- Scalar final_cost = -1; // 1/2 ||f(x)||^2
- Scalar gradient_max_norm = -1; // max(J'f(x))
+ // 1/2 ||f(x_0)||^2
+ Scalar initial_cost = -1;
+ // 1/2 ||f(x)||^2
+ Scalar final_cost = -1;
+ // max_norm(J'f(x))
+ Scalar gradient_max_norm = -1;
int iterations = -1;
Status status = HIT_MAX_ITERATIONS;
};
bool Update(const Function& function, const Parameters& x) {
- if (!function(x.data(), error_.data(), jacobian_.data())) {
+ if (!function(x.data(), residuals_.data(), jacobian_.data())) {
return false;
}
- error_ = -error_;
+ residuals_ = -residuals_;
// On the first iteration, compute a diagonal (Jacobi) scaling
// matrix, which we store as a vector.
@@ -192,9 +211,9 @@
// factorization.
jacobian_ = jacobian_ * jacobi_scaling_.asDiagonal();
jtj_ = jacobian_.transpose() * jacobian_;
- g_ = jacobian_.transpose() * error_;
+ g_ = jacobian_.transpose() * residuals_;
summary.gradient_max_norm = g_.array().abs().maxCoeff();
- cost_ = error_.squaredNorm() / 2;
+ cost_ = residuals_.squaredNorm() / 2;
return true;
}
@@ -229,10 +248,9 @@
jtj_regularized_ = jtj_;
const Scalar min_diagonal = 1e-6;
const Scalar max_diagonal = 1e32;
- for (int i = 0; i < lm_diagonal_.rows(); ++i) {
- lm_diagonal_[i] = std::sqrt(
- u * std::min(std::max(jtj_(i, i), min_diagonal), max_diagonal));
- jtj_regularized_(i, i) += lm_diagonal_[i] * lm_diagonal_[i];
+ for (int i = 0; i < dx_.rows(); ++i) {
+ jtj_regularized_(i, i) +=
+ u * (std::min)((std::max)(jtj_(i, i), min_diagonal), max_diagonal);
}
// TODO(sameeragarwal): Check for failure and deal with it.
@@ -253,10 +271,9 @@
// TODO(keir): Add proper handling of errors from user eval of cost
// functions.
- function(&x_new_[0], &f_x_new_[0], NULL);
+ function(&x_new_[0], &f_x_new_[0], nullptr);
const Scalar cost_change = (2 * cost_ - f_x_new_.squaredNorm());
-
// TODO(sameeragarwal): Better more numerically stable evaluation.
const Scalar model_cost_change = lm_step_.dot(2 * g_ - jtj_ * lm_step_);
@@ -269,6 +286,12 @@
// model fits well.
x = x_new_;
+ if (std::abs(cost_change) < options.function_tolerance) {
+ cost_ = f_x_new_.squaredNorm() / 2;
+ summary.status = COST_CHANGE_TOO_SMALL;
+ break;
+ }
+
// TODO(sameeragarwal): Deal with failure.
Update(function, x);
if (summary.gradient_max_norm < options.gradient_tolerance) {
@@ -282,16 +305,24 @@
}
Scalar tmp = Scalar(2 * rho - 1);
- u = u * std::max(1 / 3., 1 - tmp * tmp * tmp);
+ u = u * (std::max)(Scalar(1 / 3.), Scalar(1) - tmp * tmp * tmp);
v = 2;
- continue;
- }
- // Reject the update because either the normal equations failed to solve
- // or the local linear model was not good (rho < 0). Instead, increase u
- // to move closer to gradient descent.
- u *= v;
- v *= 2;
+ } else {
+ // Reject the update because either the normal equations failed to solve
+ // or the local linear model was not good (rho < 0).
+
+ // Additionally if the cost change is too small, then terminate.
+ if (std::abs(cost_change) < options.function_tolerance) {
+ // Terminate
+ summary.status = COST_CHANGE_TOO_SMALL;
+ break;
+ }
+
+ // Reduce the size of the trust region.
+ u *= v;
+ v *= 2;
+ }
}
summary.final_cost = cost_;
@@ -306,8 +337,8 @@
// linear system. This allows reusing the intermediate storage across solves.
LinearSolver linear_solver_;
Scalar cost_;
- Parameters dx_, x_new_, g_, jacobi_scaling_, lm_diagonal_, lm_step_;
- Eigen::Matrix<Scalar, NUM_RESIDUALS, 1> error_, f_x_new_;
+ Parameters dx_, x_new_, g_, jacobi_scaling_, lm_step_;
+ Eigen::Matrix<Scalar, NUM_RESIDUALS, 1> residuals_, f_x_new_;
Eigen::Matrix<Scalar, NUM_RESIDUALS, NUM_PARAMETERS> jacobian_;
Eigen::Matrix<Scalar, NUM_PARAMETERS, NUM_PARAMETERS> jtj_, jtj_regularized_;
@@ -317,7 +348,7 @@
template <typename T>
struct enable_if<true, T> {
- typedef T type;
+ using type = T;
};
// The number of parameters and residuals are dynamically sized.
@@ -353,9 +384,8 @@
x_new_.resize(num_parameters);
g_.resize(num_parameters);
jacobi_scaling_.resize(num_parameters);
- lm_diagonal_.resize(num_parameters);
lm_step_.resize(num_parameters);
- error_.resize(num_residuals);
+ residuals_.resize(num_residuals);
f_x_new_.resize(num_residuals);
jacobian_.resize(num_residuals, num_parameters);
jtj_.resize(num_parameters, num_parameters);
diff --git a/include/ceres/tiny_solver_autodiff_function.h b/include/ceres/tiny_solver_autodiff_function.h
index b782f54..1b9bd96 100644
--- a/include/ceres/tiny_solver_autodiff_function.h
+++ b/include/ceres/tiny_solver_autodiff_function.h
@@ -1,5 +1,5 @@
// Ceres Solver - A fast non-linear least squares minimizer
-// Copyright 2019 Google Inc. All rights reserved.
+// Copyright 2023 Google Inc. All rights reserved.
// http://ceres-solver.org/
//
// Redistribution and use in source and binary forms, with or without
@@ -113,12 +113,12 @@
// as a member a Jet type, which itself has a fixed-size Eigen type as member.
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
- TinySolverAutoDiffFunction(const CostFunctor& cost_functor)
+ explicit TinySolverAutoDiffFunction(const CostFunctor& cost_functor)
: cost_functor_(cost_functor) {
Initialize<kNumResiduals>(cost_functor);
}
- typedef T Scalar;
+ using Scalar = T;
enum {
NUM_PARAMETERS = kNumParameters,
NUM_RESIDUALS = kNumResiduals,
@@ -127,7 +127,7 @@
// This is similar to AutoDifferentiate(), but since there is only one
// parameter block it is easier to inline to avoid overhead.
bool operator()(const T* parameters, T* residuals, T* jacobian) const {
- if (jacobian == NULL) {
+ if (jacobian == nullptr) {
// No jacobian requested, so just directly call the cost function with
// doubles, skipping jets and derivatives.
return cost_functor_(parameters, residuals);
@@ -171,7 +171,7 @@
const CostFunctor& cost_functor_;
// The number of residuals at runtime.
- // This will be overriden if NUM_RESIDUALS == Eigen::Dynamic.
+ // This will be overridden if NUM_RESIDUALS == Eigen::Dynamic.
int num_residuals_ = kNumResiduals;
// To evaluate the cost function with jets, temporary storage is needed. These
diff --git a/include/ceres/tiny_solver_cost_function_adapter.h b/include/ceres/tiny_solver_cost_function_adapter.h
index 18ccb39..166f03f 100644
--- a/include/ceres/tiny_solver_cost_function_adapter.h
+++ b/include/ceres/tiny_solver_cost_function_adapter.h
@@ -1,5 +1,5 @@
// Ceres Solver - A fast non-linear least squares minimizer
-// Copyright 2019 Google Inc. All rights reserved.
+// Copyright 2023 Google Inc. All rights reserved.
// http://ceres-solver.org/
//
// Redistribution and use in source and binary forms, with or without
@@ -75,7 +75,7 @@
int kNumParameters = Eigen::Dynamic>
class TinySolverCostFunctionAdapter {
public:
- typedef double Scalar;
+ using Scalar = double;
enum ComponentSizeType {
NUM_PARAMETERS = kNumParameters,
NUM_RESIDUALS = kNumResiduals
@@ -85,7 +85,7 @@
// fixed-size Eigen types.
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
- TinySolverCostFunctionAdapter(const CostFunction& cost_function)
+ explicit TinySolverCostFunctionAdapter(const CostFunction& cost_function)
: cost_function_(cost_function) {
CHECK_EQ(cost_function_.parameter_block_sizes().size(), 1)
<< "Only CostFunctions with exactly one parameter blocks are allowed.";
@@ -108,7 +108,7 @@
double* residuals,
double* jacobian) const {
if (!jacobian) {
- return cost_function_.Evaluate(¶meters, residuals, NULL);
+ return cost_function_.Evaluate(¶meters, residuals, nullptr);
}
double* jacobians[1] = {row_major_jacobian_.data()};
diff --git a/include/ceres/types.h b/include/ceres/types.h
index 5ee6fdc..6e19c51 100644
--- a/include/ceres/types.h
+++ b/include/ceres/types.h
@@ -1,5 +1,5 @@
// Ceres Solver - A fast non-linear least squares minimizer
-// Copyright 2019 Google Inc. All rights reserved.
+// Copyright 2023 Google Inc. All rights reserved.
// http://ceres-solver.org/
//
// Redistribution and use in source and binary forms, with or without
@@ -40,7 +40,7 @@
#include <string>
#include "ceres/internal/disable_warnings.h"
-#include "ceres/internal/port.h"
+#include "ceres/internal/export.h"
namespace ceres {
@@ -67,8 +67,7 @@
// Eigen.
DENSE_QR,
- // Solve the normal equations using a sparse cholesky solver; requires
- // SuiteSparse or CXSparse.
+ // Solve the normal equations using a sparse cholesky solver;
SPARSE_NORMAL_CHOLESKY,
// Specialized solvers, specific to problems with a generalized
@@ -98,7 +97,7 @@
// Block diagonal of the Gauss-Newton Hessian.
JACOBI,
- // Note: The following three preconditioners can only be used with
+ // Note: The following four preconditioners can only be used with
// the ITERATIVE_SCHUR solver. They are well suited for Structure
// from Motion problems.
@@ -106,6 +105,10 @@
// only be used with the ITERATIVE_SCHUR solver.
SCHUR_JACOBI,
+ // Use power series expansion to approximate the inversion of Schur complement
+ // as a preconditioner.
+ SCHUR_POWER_SERIES_EXPANSION,
+
// Visibility clustering based preconditioners.
//
// The following two preconditioners use the visibility structure of
@@ -134,7 +137,7 @@
// well the matrix Q approximates J'J, or how well the chosen
// residual blocks approximate the non-linear least squares
// problem.
- SUBSET,
+ SUBSET
};
enum VisibilityClusteringType {
@@ -165,11 +168,6 @@
// minimum degree ordering.
SUITE_SPARSE,
- // A lightweight replacement 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,
@@ -177,15 +175,43 @@
// Apple's Accelerate framework sparse linear algebra routines.
ACCELERATE_SPARSE,
+ // Nvidia's cuSPARSE library.
+ CUDA_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
};
+// The order in which variables are eliminated in a linear solver
+// can have a significant of impact on the efficiency and accuracy
+// of the method. e.g., when doing sparse Cholesky factorization,
+// there are matrices for which a good ordering will give a
+// Cholesky factor with O(n) storage, where as a bad ordering will
+// result in an completely dense factor.
+//
+// So sparse direct solvers like SPARSE_NORMAL_CHOLESKY and
+// SPARSE_SCHUR and preconditioners like SUBSET, CLUSTER_JACOBI &
+// CLUSTER_TRIDIAGONAL use a fill reducing ordering of the columns and
+// rows of the matrix being factorized before actually the numeric
+// factorization.
+//
+// This enum controls the class of algorithm used to compute this
+// fill reducing ordering. There is no single algorithm that works
+// on all matrices, so determining which algorithm works better is a
+// matter of empirical experimentation.
+enum LinearSolverOrderingType {
+ // Approximate Minimum Degree.
+ AMD,
+ // Nested Dissection.
+ NESDIS
+};
+
enum DenseLinearAlgebraLibraryType {
EIGEN,
LAPACK,
+ CUDA,
};
// Logging options
@@ -466,6 +492,11 @@
CERES_EXPORT bool StringToSparseLinearAlgebraLibraryType(
std::string value, SparseLinearAlgebraLibraryType* type);
+CERES_EXPORT const char* LinearSolverOrderingTypeToString(
+ LinearSolverOrderingType type);
+CERES_EXPORT bool StringToLinearSolverOrderingType(
+ std::string value, LinearSolverOrderingType* type);
+
CERES_EXPORT const char* DenseLinearAlgebraLibraryTypeToString(
DenseLinearAlgebraLibraryType type);
CERES_EXPORT bool StringToDenseLinearAlgebraLibraryType(
diff --git a/include/ceres/version.h b/include/ceres/version.h
index a76cc10..fe6c288 100644
--- a/include/ceres/version.h
+++ b/include/ceres/version.h
@@ -1,5 +1,5 @@
// Ceres Solver - A fast non-linear least squares minimizer
-// Copyright 2019 Google Inc. All rights reserved.
+// Copyright 2023 Google Inc. All rights reserved.
// http://ceres-solver.org/
//
// Redistribution and use in source and binary forms, with or without
@@ -32,7 +32,7 @@
#define CERES_PUBLIC_VERSION_H_
#define CERES_VERSION_MAJOR 2
-#define CERES_VERSION_MINOR 0
+#define CERES_VERSION_MINOR 2
#define CERES_VERSION_REVISION 0
// Classic CPP stringifcation; the extra level of indirection allows the
@@ -40,10 +40,16 @@
#define CERES_TO_STRING_HELPER(x) #x
#define CERES_TO_STRING(x) CERES_TO_STRING_HELPER(x)
+// clang-format off
+#define CERES_SEMVER_VERSION(MAJOR, MINOR, PATCH) \
+ CERES_TO_STRING(MAJOR) "." \
+ CERES_TO_STRING(MINOR) "." \
+ CERES_TO_STRING(PATCH)
+// clang-format on
+
// 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)
+#define CERES_VERSION_STRING \
+ CERES_SEMVER_VERSION( \
+ CERES_VERSION_MAJOR, CERES_VERSION_MINOR, CERES_VERSION_REVISION)
#endif // CERES_PUBLIC_VERSION_H_