diff options
| author | Sergey Sharybin <sergey@blender.org> | 2022-05-10 17:36:22 +0300 |
|---|---|---|
| committer | Sergey Sharybin <sergey@blender.org> | 2022-05-10 18:01:20 +0300 |
| commit | 3ad2597a4eca5091031c213445c6583e21097d5f (patch) | |
| tree | f909af8ad783d1adea67911ddaf1633ad7f570a9 | |
| parent | b4b85c5ce2752ea9241cbcfa1ddc3f639ad64262 (diff) | |
Update Ceres to latest upstream version 2.1.0temp-ceres_update
This release deprecated the Parameterization API and the new Manifolds
API is to be used instead. This is what was done in the Libmv as part
of this change.
Additionally, remove the bundling scripts. Nowadays those are only
leading to a duplicated work to maintain.
333 files changed, 9012 insertions, 5529 deletions
diff --git a/CMakeLists.txt b/CMakeLists.txt index f48334a298b..0351840ad0d 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1456,14 +1456,6 @@ if(WITH_LIBMV OR WITH_GTESTS OR (WITH_CYCLES AND WITH_CYCLES_LOGGING)) endif() #----------------------------------------------------------------------------- -# Configure Ceres - -if(WITH_LIBMV) - # We always have C++11 which includes unordered_map. - set(CERES_DEFINES "-DCERES_STD_UNORDERED_MAP;-DCERES_USE_CXX_THREADS") -endif() - -#----------------------------------------------------------------------------- # Extra limits to number of jobs running in parallel for some kind os tasks. # Only supported by Ninja build system currently. if("${CMAKE_GENERATOR}" MATCHES "Ninja" AND WITH_NINJA_POOL_JOBS) diff --git a/extern/ceres/CMakeLists.txt b/extern/ceres/CMakeLists.txt index 1d8a611b107..b723a4466fb 100644 --- a/extern/ceres/CMakeLists.txt +++ b/extern/ceres/CMakeLists.txt @@ -1,16 +1,11 @@ # SPDX-License-Identifier: GPL-2.0-or-later # Copyright 2012 Blender Foundation. All rights reserved. -# NOTE: This file is automatically generated by bundle.sh script -# If you're doing changes in this file, please update template -# in that script too - set(INC . include internal config - ../gflags/src ) set(INC_SYS @@ -20,279 +15,296 @@ set(INC_SYS ) set(SRC - internal/ceres/accelerate_sparse.cc - internal/ceres/array_utils.cc - internal/ceres/blas.cc - internal/ceres/block_evaluate_preparer.cc - internal/ceres/block_jacobian_writer.cc - internal/ceres/block_jacobi_preconditioner.cc - internal/ceres/block_random_access_dense_matrix.cc - internal/ceres/block_random_access_diagonal_matrix.cc - internal/ceres/block_random_access_matrix.cc - internal/ceres/block_random_access_sparse_matrix.cc - internal/ceres/block_sparse_matrix.cc - internal/ceres/block_structure.cc - internal/ceres/callbacks.cc - internal/ceres/canonical_views_clustering.cc - internal/ceres/c_api.cc - internal/ceres/cgnr_solver.cc - internal/ceres/compressed_col_sparse_matrix_utils.cc - internal/ceres/compressed_row_jacobian_writer.cc - internal/ceres/compressed_row_sparse_matrix.cc - internal/ceres/conditioned_cost_function.cc - internal/ceres/conjugate_gradients_solver.cc - internal/ceres/context.cc - internal/ceres/context_impl.cc - internal/ceres/coordinate_descent_minimizer.cc - internal/ceres/corrector.cc - internal/ceres/covariance.cc - internal/ceres/covariance_impl.cc - internal/ceres/cxsparse.cc - internal/ceres/dense_normal_cholesky_solver.cc - internal/ceres/dense_qr_solver.cc - internal/ceres/dense_sparse_matrix.cc - internal/ceres/detect_structure.cc - internal/ceres/dogleg_strategy.cc - internal/ceres/dynamic_compressed_row_jacobian_writer.cc - internal/ceres/dynamic_compressed_row_sparse_matrix.cc - internal/ceres/dynamic_sparse_normal_cholesky_solver.cc - internal/ceres/eigensparse.cc - internal/ceres/evaluator.cc - internal/ceres/file.cc - internal/ceres/float_cxsparse.cc - internal/ceres/float_suitesparse.cc - internal/ceres/function_sample.cc - internal/ceres/generated/partitioned_matrix_view_d_d_d.cc - internal/ceres/generated/schur_eliminator_d_d_d.cc - internal/ceres/gradient_checker.cc - internal/ceres/gradient_checking_cost_function.cc - internal/ceres/gradient_problem.cc - internal/ceres/gradient_problem_solver.cc - internal/ceres/implicit_schur_complement.cc - internal/ceres/inner_product_computer.cc - internal/ceres/is_close.cc - internal/ceres/iterative_refiner.cc - internal/ceres/iterative_schur_complement_solver.cc - internal/ceres/lapack.cc - internal/ceres/levenberg_marquardt_strategy.cc - internal/ceres/linear_least_squares_problems.cc - internal/ceres/linear_operator.cc - internal/ceres/linear_solver.cc - internal/ceres/line_search.cc - internal/ceres/line_search_direction.cc - internal/ceres/line_search_minimizer.cc - internal/ceres/line_search_preprocessor.cc - internal/ceres/local_parameterization.cc - internal/ceres/loss_function.cc - internal/ceres/low_rank_inverse_hessian.cc - internal/ceres/minimizer.cc - internal/ceres/normal_prior.cc - internal/ceres/parallel_for_cxx.cc - internal/ceres/parallel_for_nothreads.cc - internal/ceres/parallel_for_openmp.cc - internal/ceres/parallel_utils.cc - internal/ceres/parameter_block_ordering.cc - internal/ceres/partitioned_matrix_view.cc - internal/ceres/polynomial.cc - internal/ceres/preconditioner.cc - internal/ceres/preprocessor.cc - internal/ceres/problem.cc - internal/ceres/problem_impl.cc - internal/ceres/program.cc - internal/ceres/reorder_program.cc - internal/ceres/residual_block.cc - internal/ceres/residual_block_utils.cc - internal/ceres/schur_complement_solver.cc - internal/ceres/schur_eliminator.cc - internal/ceres/schur_jacobi_preconditioner.cc - internal/ceres/schur_templates.cc - internal/ceres/scratch_evaluate_preparer.cc - internal/ceres/single_linkage_clustering.cc - internal/ceres/solver.cc - internal/ceres/solver_utils.cc - internal/ceres/sparse_cholesky.cc - internal/ceres/sparse_matrix.cc - internal/ceres/sparse_normal_cholesky_solver.cc - internal/ceres/split.cc - internal/ceres/stringprintf.cc - internal/ceres/subset_preconditioner.cc - internal/ceres/suitesparse.cc - internal/ceres/thread_pool.cc - internal/ceres/thread_token_provider.cc - internal/ceres/triplet_sparse_matrix.cc - internal/ceres/trust_region_minimizer.cc - internal/ceres/trust_region_preprocessor.cc - internal/ceres/trust_region_step_evaluator.cc - internal/ceres/trust_region_strategy.cc - internal/ceres/types.cc - internal/ceres/visibility_based_preconditioner.cc - internal/ceres/visibility.cc - internal/ceres/wall_time.cc + include/ceres/autodiff_cost_function.h + include/ceres/autodiff_first_order_function.h + include/ceres/autodiff_local_parameterization.h + include/ceres/autodiff_manifold.h + include/ceres/c_api.h + include/ceres/ceres.h + include/ceres/conditioned_cost_function.h + include/ceres/context.h + include/ceres/cost_function.h + include/ceres/cost_function_to_functor.h + include/ceres/covariance.h + include/ceres/crs_matrix.h + include/ceres/cubic_interpolation.h + include/ceres/dynamic_autodiff_cost_function.h + include/ceres/dynamic_cost_function.h + 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/jet_fwd.h + include/ceres/line_manifold.h + include/ceres/local_parameterization.h + include/ceres/loss_function.h + include/ceres/manifold.h + include/ceres/manifold_test_utils.h + include/ceres/normal_prior.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/rotation.h + include/ceres/sized_cost_function.h + include/ceres/solver.h + include/ceres/sphere_manifold.h + include/ceres/tiny_solver.h + include/ceres/tiny_solver_autodiff_function.h + include/ceres/tiny_solver_cost_function_adapter.h + include/ceres/types.h + include/ceres/version.h + + include/ceres/internal/array_selector.h + include/ceres/internal/autodiff.h + include/ceres/internal/disable_warnings.h + include/ceres/internal/eigen.h + include/ceres/internal/fixed_array.h + include/ceres/internal/householder_vector.h + include/ceres/internal/integer_sequence_algorithm.h + include/ceres/internal/jet_traits.h + include/ceres/internal/line_parameterization.h + include/ceres/internal/memory.h + include/ceres/internal/numeric_diff.h + include/ceres/internal/parameter_dims.h + include/ceres/internal/port.h + include/ceres/internal/reenable_warnings.h + include/ceres/internal/sphere_manifold_functions.h + include/ceres/internal/variadic_evaluate.h + + internal/ceres/accelerate_sparse.cc + internal/ceres/accelerate_sparse.h + internal/ceres/array_utils.cc + internal/ceres/array_utils.h + internal/ceres/block_evaluate_preparer.cc + internal/ceres/block_evaluate_preparer.h + internal/ceres/block_jacobi_preconditioner.cc + internal/ceres/block_jacobi_preconditioner.h + internal/ceres/block_jacobian_writer.cc + internal/ceres/block_jacobian_writer.h + internal/ceres/block_random_access_dense_matrix.cc + internal/ceres/block_random_access_dense_matrix.h + internal/ceres/block_random_access_diagonal_matrix.cc + internal/ceres/block_random_access_diagonal_matrix.h + internal/ceres/block_random_access_matrix.cc + internal/ceres/block_random_access_matrix.h + internal/ceres/block_random_access_sparse_matrix.cc + internal/ceres/block_random_access_sparse_matrix.h + internal/ceres/block_sparse_matrix.cc + internal/ceres/block_sparse_matrix.h + internal/ceres/block_structure.cc + internal/ceres/block_structure.h + internal/ceres/c_api.cc + internal/ceres/callbacks.cc + internal/ceres/callbacks.h + internal/ceres/canonical_views_clustering.cc + internal/ceres/canonical_views_clustering.h + internal/ceres/casts.h + internal/ceres/cgnr_linear_operator.h + internal/ceres/cgnr_solver.cc + internal/ceres/cgnr_solver.h + internal/ceres/compressed_col_sparse_matrix_utils.cc + internal/ceres/compressed_col_sparse_matrix_utils.h + internal/ceres/compressed_row_jacobian_writer.cc + internal/ceres/compressed_row_jacobian_writer.h + internal/ceres/compressed_row_sparse_matrix.cc + internal/ceres/compressed_row_sparse_matrix.h + internal/ceres/concurrent_queue.h + internal/ceres/conditioned_cost_function.cc + internal/ceres/conjugate_gradients_solver.cc + internal/ceres/conjugate_gradients_solver.h + internal/ceres/context.cc + internal/ceres/context_impl.cc + internal/ceres/context_impl.h + internal/ceres/coordinate_descent_minimizer.cc + internal/ceres/coordinate_descent_minimizer.h + internal/ceres/corrector.cc + internal/ceres/corrector.h + internal/ceres/cost_function.cc + internal/ceres/covariance.cc + internal/ceres/covariance_impl.cc + internal/ceres/covariance_impl.h + internal/ceres/cuda_buffer.h + internal/ceres/cxsparse.cc + internal/ceres/cxsparse.h + internal/ceres/dense_cholesky.cc + internal/ceres/dense_cholesky.h + internal/ceres/dense_jacobian_writer.h + internal/ceres/dense_normal_cholesky_solver.cc + internal/ceres/dense_normal_cholesky_solver.h + internal/ceres/dense_qr.cc + internal/ceres/dense_qr.h + internal/ceres/dense_qr_solver.cc + internal/ceres/dense_qr_solver.h + internal/ceres/dense_sparse_matrix.cc + internal/ceres/dense_sparse_matrix.h + internal/ceres/detect_structure.cc + internal/ceres/detect_structure.h + internal/ceres/dogleg_strategy.cc + internal/ceres/dogleg_strategy.h + internal/ceres/dynamic_compressed_row_finalizer.h + internal/ceres/dynamic_compressed_row_jacobian_writer.cc + internal/ceres/dynamic_compressed_row_jacobian_writer.h + internal/ceres/dynamic_compressed_row_sparse_matrix.cc + internal/ceres/dynamic_compressed_row_sparse_matrix.h + internal/ceres/dynamic_sparse_normal_cholesky_solver.cc + internal/ceres/dynamic_sparse_normal_cholesky_solver.h + internal/ceres/eigensparse.cc + internal/ceres/eigensparse.h + internal/ceres/evaluation_callback.cc + internal/ceres/evaluator.cc + internal/ceres/evaluator.h + internal/ceres/execution_summary.h + internal/ceres/file.cc + internal/ceres/file.h + internal/ceres/first_order_function.cc + internal/ceres/float_cxsparse.cc + internal/ceres/float_cxsparse.h + internal/ceres/float_suitesparse.cc + internal/ceres/float_suitesparse.h + internal/ceres/function_sample.cc + internal/ceres/function_sample.h + internal/ceres/gradient_checker.cc + internal/ceres/gradient_checking_cost_function.cc + internal/ceres/gradient_checking_cost_function.h + internal/ceres/gradient_problem.cc + internal/ceres/gradient_problem_evaluator.h + internal/ceres/gradient_problem_solver.cc + internal/ceres/graph.h + internal/ceres/graph_algorithms.h + internal/ceres/implicit_schur_complement.cc + internal/ceres/implicit_schur_complement.h + internal/ceres/inner_product_computer.cc + internal/ceres/inner_product_computer.h + internal/ceres/invert_psd_matrix.h + internal/ceres/is_close.cc + internal/ceres/is_close.h + internal/ceres/iteration_callback.cc + internal/ceres/iterative_refiner.cc + internal/ceres/iterative_refiner.h + internal/ceres/iterative_schur_complement_solver.cc + internal/ceres/iterative_schur_complement_solver.h + internal/ceres/levenberg_marquardt_strategy.cc + internal/ceres/levenberg_marquardt_strategy.h + internal/ceres/line_search.cc + internal/ceres/line_search.h + internal/ceres/line_search_direction.cc + internal/ceres/line_search_direction.h + internal/ceres/line_search_minimizer.cc + internal/ceres/line_search_minimizer.h + internal/ceres/line_search_preprocessor.cc + internal/ceres/line_search_preprocessor.h + internal/ceres/linear_least_squares_problems.cc + internal/ceres/linear_least_squares_problems.h + internal/ceres/linear_operator.cc + internal/ceres/linear_operator.h + internal/ceres/linear_solver.cc + internal/ceres/linear_solver.h + internal/ceres/local_parameterization.cc + internal/ceres/loss_function.cc + internal/ceres/low_rank_inverse_hessian.cc + internal/ceres/low_rank_inverse_hessian.h + internal/ceres/manifold.cc + internal/ceres/manifold_adapter.h + internal/ceres/map_util.h + internal/ceres/minimizer.cc + internal/ceres/minimizer.h + internal/ceres/normal_prior.cc + internal/ceres/pair_hash.h + internal/ceres/parallel_for.h + internal/ceres/parallel_for_cxx.cc + internal/ceres/parallel_for_nothreads.cc + internal/ceres/parallel_for_openmp.cc + internal/ceres/parallel_utils.cc + internal/ceres/parallel_utils.h + internal/ceres/parameter_block.h + internal/ceres/parameter_block_ordering.cc + internal/ceres/parameter_block_ordering.h + internal/ceres/partitioned_matrix_view.cc + internal/ceres/partitioned_matrix_view.h + internal/ceres/partitioned_matrix_view_impl.h + internal/ceres/polynomial.cc + internal/ceres/polynomial.h + internal/ceres/preconditioner.cc + internal/ceres/preconditioner.h + internal/ceres/preprocessor.cc + internal/ceres/preprocessor.h + internal/ceres/problem.cc + internal/ceres/problem_impl.cc + internal/ceres/problem_impl.h + internal/ceres/program.cc + internal/ceres/program.h + internal/ceres/program_evaluator.h + internal/ceres/random.h + internal/ceres/reorder_program.cc + internal/ceres/reorder_program.h + internal/ceres/residual_block.cc + internal/ceres/residual_block.h + internal/ceres/residual_block_utils.cc + internal/ceres/residual_block_utils.h + internal/ceres/schur_complement_solver.cc + internal/ceres/schur_complement_solver.h + internal/ceres/schur_eliminator.cc + internal/ceres/schur_eliminator.h + internal/ceres/schur_eliminator_impl.h + internal/ceres/schur_jacobi_preconditioner.cc + internal/ceres/schur_jacobi_preconditioner.h + internal/ceres/schur_templates.cc + internal/ceres/schur_templates.h + internal/ceres/scoped_thread_token.h + internal/ceres/scratch_evaluate_preparer.cc + internal/ceres/scratch_evaluate_preparer.h + internal/ceres/single_linkage_clustering.cc + internal/ceres/single_linkage_clustering.h + internal/ceres/small_blas.h + internal/ceres/small_blas_generic.h + internal/ceres/solver.cc + internal/ceres/solver_utils.cc + internal/ceres/solver_utils.h + internal/ceres/sparse_cholesky.cc + internal/ceres/sparse_cholesky.h + internal/ceres/sparse_matrix.cc + internal/ceres/sparse_matrix.h + internal/ceres/sparse_normal_cholesky_solver.cc + internal/ceres/sparse_normal_cholesky_solver.h + internal/ceres/stl_util.h + internal/ceres/stringprintf.cc + internal/ceres/stringprintf.h + internal/ceres/subset_preconditioner.cc + internal/ceres/subset_preconditioner.h + internal/ceres/suitesparse.cc + internal/ceres/suitesparse.h + internal/ceres/thread_pool.cc + internal/ceres/thread_pool.h + internal/ceres/thread_token_provider.cc + internal/ceres/thread_token_provider.h + internal/ceres/triplet_sparse_matrix.cc + internal/ceres/triplet_sparse_matrix.h + internal/ceres/trust_region_minimizer.cc + internal/ceres/trust_region_minimizer.h + internal/ceres/trust_region_preprocessor.cc + internal/ceres/trust_region_preprocessor.h + internal/ceres/trust_region_step_evaluator.cc + internal/ceres/trust_region_step_evaluator.h + internal/ceres/trust_region_strategy.cc + internal/ceres/trust_region_strategy.h + internal/ceres/types.cc + internal/ceres/visibility.cc + internal/ceres/visibility.h + internal/ceres/visibility_based_preconditioner.cc + internal/ceres/visibility_based_preconditioner.h + internal/ceres/wall_time.cc + internal/ceres/wall_time.h - include/ceres/autodiff_cost_function.h - include/ceres/autodiff_first_order_function.h - include/ceres/autodiff_local_parameterization.h - include/ceres/c_api.h - include/ceres/ceres.h - include/ceres/conditioned_cost_function.h - include/ceres/context.h - include/ceres/cost_function.h - include/ceres/cost_function_to_functor.h - include/ceres/covariance.h - include/ceres/crs_matrix.h - include/ceres/cubic_interpolation.h - include/ceres/dynamic_autodiff_cost_function.h - include/ceres/dynamic_cost_function.h - 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/internal/array_selector.h - include/ceres/internal/autodiff.h - include/ceres/internal/disable_warnings.h - include/ceres/internal/eigen.h - include/ceres/internal/fixed_array.h - include/ceres/internal/householder_vector.h - include/ceres/internal/integer_sequence_algorithm.h - include/ceres/internal/line_parameterization.h - include/ceres/internal/memory.h - include/ceres/internal/numeric_diff.h - include/ceres/internal/parameter_dims.h - include/ceres/internal/port.h - include/ceres/internal/reenable_warnings.h - include/ceres/internal/variadic_evaluate.h - include/ceres/iteration_callback.h - include/ceres/jet.h - include/ceres/local_parameterization.h - include/ceres/loss_function.h - include/ceres/normal_prior.h - include/ceres/numeric_diff_cost_function.h - include/ceres/numeric_diff_options.h - include/ceres/ordered_groups.h - include/ceres/problem.h - include/ceres/rotation.h - include/ceres/sized_cost_function.h - include/ceres/solver.h - include/ceres/tiny_solver_autodiff_function.h - include/ceres/tiny_solver_cost_function_adapter.h - include/ceres/tiny_solver.h - include/ceres/types.h - include/ceres/version.h - internal/ceres/accelerate_sparse.h - internal/ceres/array_utils.h - internal/ceres/blas.h - internal/ceres/block_evaluate_preparer.h - internal/ceres/block_jacobian_writer.h - internal/ceres/block_jacobi_preconditioner.h - internal/ceres/block_random_access_dense_matrix.h - internal/ceres/block_random_access_diagonal_matrix.h - internal/ceres/block_random_access_matrix.h - internal/ceres/block_random_access_sparse_matrix.h - internal/ceres/block_sparse_matrix.h - internal/ceres/block_structure.h - internal/ceres/callbacks.h - internal/ceres/canonical_views_clustering.h - internal/ceres/casts.h - internal/ceres/cgnr_linear_operator.h - internal/ceres/cgnr_solver.h - internal/ceres/compressed_col_sparse_matrix_utils.h - internal/ceres/compressed_row_jacobian_writer.h - internal/ceres/compressed_row_sparse_matrix.h - internal/ceres/concurrent_queue.h - internal/ceres/conjugate_gradients_solver.h - internal/ceres/context_impl.h - internal/ceres/coordinate_descent_minimizer.h - internal/ceres/corrector.h - internal/ceres/covariance_impl.h - internal/ceres/cxsparse.h - internal/ceres/dense_jacobian_writer.h - internal/ceres/dense_normal_cholesky_solver.h - internal/ceres/dense_qr_solver.h - internal/ceres/dense_sparse_matrix.h - internal/ceres/detect_structure.h - internal/ceres/dogleg_strategy.h - internal/ceres/dynamic_compressed_row_finalizer.h - internal/ceres/dynamic_compressed_row_jacobian_writer.h - internal/ceres/dynamic_compressed_row_sparse_matrix.h - internal/ceres/dynamic_sparse_normal_cholesky_solver.h - internal/ceres/eigensparse.h - internal/ceres/evaluator.h - internal/ceres/execution_summary.h - internal/ceres/file.h - internal/ceres/float_cxsparse.h - internal/ceres/float_suitesparse.h - internal/ceres/function_sample.h - internal/ceres/gradient_checking_cost_function.h - internal/ceres/gradient_problem_evaluator.h - internal/ceres/graph_algorithms.h - internal/ceres/graph.h - internal/ceres/implicit_schur_complement.h - internal/ceres/inner_product_computer.h - internal/ceres/invert_psd_matrix.h - internal/ceres/is_close.h - internal/ceres/iterative_refiner.h - internal/ceres/iterative_schur_complement_solver.h - internal/ceres/lapack.h - internal/ceres/levenberg_marquardt_strategy.h - internal/ceres/linear_least_squares_problems.h - internal/ceres/linear_operator.h - internal/ceres/linear_solver.h - internal/ceres/line_search_direction.h - internal/ceres/line_search.h - internal/ceres/line_search_minimizer.h - internal/ceres/line_search_preprocessor.h - internal/ceres/low_rank_inverse_hessian.h - internal/ceres/map_util.h - internal/ceres/minimizer.h - internal/ceres/pair_hash.h - internal/ceres/parallel_for.h - internal/ceres/parallel_utils.h - internal/ceres/parameter_block.h - internal/ceres/parameter_block_ordering.h - internal/ceres/partitioned_matrix_view.h - internal/ceres/partitioned_matrix_view_impl.h - internal/ceres/polynomial.h - internal/ceres/preconditioner.h - internal/ceres/preprocessor.h - internal/ceres/problem_impl.h - internal/ceres/program_evaluator.h - internal/ceres/program.h - internal/ceres/random.h - internal/ceres/reorder_program.h - internal/ceres/residual_block.h - internal/ceres/residual_block_utils.h - internal/ceres/schur_complement_solver.h - internal/ceres/schur_eliminator.h - internal/ceres/schur_eliminator_impl.h - internal/ceres/schur_jacobi_preconditioner.h - internal/ceres/schur_templates.h - internal/ceres/scoped_thread_token.h - internal/ceres/scratch_evaluate_preparer.h - internal/ceres/single_linkage_clustering.h - internal/ceres/small_blas_generic.h - internal/ceres/small_blas.h - internal/ceres/solver_utils.h - internal/ceres/sparse_cholesky.h - internal/ceres/sparse_matrix.h - internal/ceres/sparse_normal_cholesky_solver.h - internal/ceres/split.h - internal/ceres/stl_util.h - internal/ceres/stringprintf.h - internal/ceres/subset_preconditioner.h - internal/ceres/suitesparse.h - internal/ceres/thread_pool.h - internal/ceres/thread_token_provider.h - internal/ceres/triplet_sparse_matrix.h - internal/ceres/trust_region_minimizer.h - internal/ceres/trust_region_preprocessor.h - internal/ceres/trust_region_step_evaluator.h - internal/ceres/trust_region_strategy.h - internal/ceres/visibility_based_preconditioner.h - internal/ceres/visibility.h - internal/ceres/wall_time.h + internal/ceres/generated/partitioned_matrix_view_d_d_d.cc + internal/ceres/generated/schur_eliminator_d_d_d.cc ) set(LIB @@ -302,48 +314,48 @@ set(LIB if(WITH_LIBMV_SCHUR_SPECIALIZATIONS) list(APPEND SRC - internal/ceres/generated/partitioned_matrix_view_2_2_2.cc - internal/ceres/generated/partitioned_matrix_view_2_2_3.cc - internal/ceres/generated/partitioned_matrix_view_2_2_4.cc - internal/ceres/generated/partitioned_matrix_view_2_2_d.cc - internal/ceres/generated/partitioned_matrix_view_2_3_3.cc - internal/ceres/generated/partitioned_matrix_view_2_3_4.cc - internal/ceres/generated/partitioned_matrix_view_2_3_6.cc - internal/ceres/generated/partitioned_matrix_view_2_3_9.cc - internal/ceres/generated/partitioned_matrix_view_2_3_d.cc - internal/ceres/generated/partitioned_matrix_view_2_4_3.cc - internal/ceres/generated/partitioned_matrix_view_2_4_4.cc - internal/ceres/generated/partitioned_matrix_view_2_4_6.cc - internal/ceres/generated/partitioned_matrix_view_2_4_8.cc - internal/ceres/generated/partitioned_matrix_view_2_4_9.cc - internal/ceres/generated/partitioned_matrix_view_2_4_d.cc - internal/ceres/generated/partitioned_matrix_view_2_d_d.cc - internal/ceres/generated/partitioned_matrix_view_3_3_3.cc - internal/ceres/generated/partitioned_matrix_view_4_4_2.cc - internal/ceres/generated/partitioned_matrix_view_4_4_3.cc - internal/ceres/generated/partitioned_matrix_view_4_4_4.cc - internal/ceres/generated/partitioned_matrix_view_4_4_d.cc - internal/ceres/generated/schur_eliminator_2_2_2.cc - internal/ceres/generated/schur_eliminator_2_2_3.cc - internal/ceres/generated/schur_eliminator_2_2_4.cc - internal/ceres/generated/schur_eliminator_2_2_d.cc - internal/ceres/generated/schur_eliminator_2_3_3.cc - internal/ceres/generated/schur_eliminator_2_3_4.cc - internal/ceres/generated/schur_eliminator_2_3_6.cc - internal/ceres/generated/schur_eliminator_2_3_9.cc - internal/ceres/generated/schur_eliminator_2_3_d.cc - internal/ceres/generated/schur_eliminator_2_4_3.cc - internal/ceres/generated/schur_eliminator_2_4_4.cc - internal/ceres/generated/schur_eliminator_2_4_6.cc - internal/ceres/generated/schur_eliminator_2_4_8.cc - internal/ceres/generated/schur_eliminator_2_4_9.cc - internal/ceres/generated/schur_eliminator_2_4_d.cc - internal/ceres/generated/schur_eliminator_2_d_d.cc - internal/ceres/generated/schur_eliminator_3_3_3.cc - internal/ceres/generated/schur_eliminator_4_4_2.cc - internal/ceres/generated/schur_eliminator_4_4_3.cc - internal/ceres/generated/schur_eliminator_4_4_4.cc - internal/ceres/generated/schur_eliminator_4_4_d.cc + internal/ceres/generated/partitioned_matrix_view_2_2_2.cc + internal/ceres/generated/partitioned_matrix_view_2_2_3.cc + internal/ceres/generated/partitioned_matrix_view_2_2_4.cc + internal/ceres/generated/partitioned_matrix_view_2_2_d.cc + internal/ceres/generated/partitioned_matrix_view_2_3_3.cc + internal/ceres/generated/partitioned_matrix_view_2_3_4.cc + internal/ceres/generated/partitioned_matrix_view_2_3_6.cc + internal/ceres/generated/partitioned_matrix_view_2_3_9.cc + internal/ceres/generated/partitioned_matrix_view_2_3_d.cc + internal/ceres/generated/partitioned_matrix_view_2_4_3.cc + internal/ceres/generated/partitioned_matrix_view_2_4_4.cc + internal/ceres/generated/partitioned_matrix_view_2_4_6.cc + internal/ceres/generated/partitioned_matrix_view_2_4_8.cc + internal/ceres/generated/partitioned_matrix_view_2_4_9.cc + internal/ceres/generated/partitioned_matrix_view_2_4_d.cc + internal/ceres/generated/partitioned_matrix_view_2_d_d.cc + internal/ceres/generated/partitioned_matrix_view_3_3_3.cc + internal/ceres/generated/partitioned_matrix_view_4_4_2.cc + internal/ceres/generated/partitioned_matrix_view_4_4_3.cc + internal/ceres/generated/partitioned_matrix_view_4_4_4.cc + internal/ceres/generated/partitioned_matrix_view_4_4_d.cc + internal/ceres/generated/schur_eliminator_2_2_2.cc + internal/ceres/generated/schur_eliminator_2_2_3.cc + internal/ceres/generated/schur_eliminator_2_2_4.cc + internal/ceres/generated/schur_eliminator_2_2_d.cc + internal/ceres/generated/schur_eliminator_2_3_3.cc + internal/ceres/generated/schur_eliminator_2_3_4.cc + internal/ceres/generated/schur_eliminator_2_3_6.cc + internal/ceres/generated/schur_eliminator_2_3_9.cc + internal/ceres/generated/schur_eliminator_2_3_d.cc + internal/ceres/generated/schur_eliminator_2_4_3.cc + internal/ceres/generated/schur_eliminator_2_4_4.cc + internal/ceres/generated/schur_eliminator_2_4_6.cc + internal/ceres/generated/schur_eliminator_2_4_8.cc + internal/ceres/generated/schur_eliminator_2_4_9.cc + internal/ceres/generated/schur_eliminator_2_4_d.cc + internal/ceres/generated/schur_eliminator_2_d_d.cc + internal/ceres/generated/schur_eliminator_3_3_3.cc + internal/ceres/generated/schur_eliminator_4_4_2.cc + internal/ceres/generated/schur_eliminator_4_4_3.cc + internal/ceres/generated/schur_eliminator_4_4_4.cc + internal/ceres/generated/schur_eliminator_4_4_d.cc ) else() add_definitions(-DCERES_RESTRICT_SCHUR_SPECIALIZATION) @@ -351,16 +363,5 @@ endif() add_definitions(${GFLAGS_DEFINES}) add_definitions(${GLOG_DEFINES}) -add_definitions(${CERES_DEFINES}) - -add_definitions( - -DCERES_HAVE_PTHREAD - -DCERES_NO_SUITESPARSE - -DCERES_NO_CXSPARSE - -DCERES_NO_LAPACK - -DCERES_NO_ACCELERATE_SPARSE - -DCERES_HAVE_RWLOCK - -DCERES_USE_CXX_THREADS -) blender_add_lib(extern_ceres "${SRC}" "${INC}" "${INC_SYS}" "${LIB}") diff --git a/extern/ceres/ChangeLog b/extern/ceres/ChangeLog deleted file mode 100644 index 40fe3f16bac..00000000000 --- a/extern/ceres/ChangeLog +++ /dev/null @@ -1,596 +0,0 @@ -commit 399cda773035d99eaf1f4a129a666b3c4df9d1b1 -Author: Alex Stewart <alexs.mac@gmail.com> -Date: Fri Oct 23 19:36:08 2020 +0100 - - Update build documentation to reflect detection of Eigen via config mode - - Change-Id: I18d5f0fc1eb51ea630164c911d935e9bffea35ce - -commit bb127272f9b57672bca48424f2d83bc430a46eb8 -Author: Sameer Agarwal <sameeragarwal@google.com> -Date: Mon Oct 19 09:28:34 2020 -0700 - - Fix typos. - - Contributed by Ishamis@, IanBoyanZhang@, gkrobner@ & mithunjacob@. - - Change-Id: Iab3c19a07a6f3db2486e3557dcb55bfe5de2aee5 - -commit a0ec5c32af5c5f5a52168dc2748be910dba14810 -Author: Sameer Agarwal <sameeragarwal@google.com> -Date: Sun Oct 18 15:20:36 2020 -0700 - - Update version history for 2.0.0RC2 - - Change-Id: I75b7515fbf9880bd8eaea6ecd5e72ce1ae4a3a86 - -commit 3f6d2736769044e7c08c873c41a184849eea73ab -Author: Taylor Braun-Jones <taylor@braun-jones.org> -Date: Tue Jan 28 12:09:30 2020 -0500 - - Unify symbol visibility configuration for all compilers - - This makes it possible to build unit tests with shared libraries on MSVC. - - Change-Id: I1db66a80b2c78c4f3d354e35235244d17bac9809 - -commit 29c2912ee635c77f3ddf2e382a5d6a9cf9805a3d -Author: Sameer Agarwal <sameeragarwal@google.com> -Date: Tue Oct 13 12:07:06 2020 -0700 - - Unbreak the bazel build some more - - Change-Id: I6bbf3df977a473b9b5e16a9e59da5f535f8cdc24 - -commit bf47e1a36829f62697b930241d0a353932f34090 -Author: Sameer Agarwal <sameeragarwal@google.com> -Date: Tue Oct 13 10:00:22 2020 -0700 - - Fix the Bazel build. - - 1. Fix the path to eigen, now that it uses gitlab instead of bitbucket. - 2. Remove an unrecognized compiler option. - 3. Remove an obsolete benchmark. - - This CL only unbreaks the build, it is likely that it is still not - at par with the cmake build. - - https://github.com/ceres-solver/ceres-solver/issues/628 - - Change-Id: I470209cbb48b6a4f499564a86b52436e0c8d98ef - -commit 600e8c529ebbb4bb89d5baefa3d5ab6ad923706a -Author: Nikolaus Demmel <nikolaus@nikolaus-demmel.de> -Date: Mon Oct 12 23:00:39 2020 +0200 - - fix minor typos - - all timing values in the summary are initialized to -1, so the one - +1 is likely an oversight. - - Change-Id: Ie355f3b7da08a56d49d19ca9a5bc48fe5581dee3 - -commit bdcdcc78af61a0cb85317ebee52dc804bf4ea975 -Author: Nikolaus Demmel <nikolaus@nikolaus-demmel.de> -Date: Mon Sep 7 01:48:50 2020 +0200 - - update docs for changed cmake usage - - - update links to cmake docs to version 3.5 - - highlight difference between dependencies with and without custom - find modules - - point out removal of CERES_INCLUDE_DIRS - - point out that TBB might be linked if SuiteSparseQR is found - - added 'Migration' section - - fixed typos - - Change-Id: Icbcc0e723d11f12246fb3cf09b9d7c6206195a82 - -commit 3f69e5b36a49b44344e96a26b39693a914ba80c6 -Author: Sameer Agarwal <sameeragarwal@google.com> -Date: Mon Oct 12 11:46:40 2020 -0700 - - Corrections from William Rucklidge - - Change-Id: I0b5d4808be48f68df7829c70ec93ffa67d81315d - -commit 8bfdb02fb18551bbd5f222c5472e45eddecd42b9 -Author: Sameer Agarwal <sameeragarwal@google.com> -Date: Mon Oct 12 10:07:13 2020 -0700 - - Rewrite uses of VLOG_IF and LOG_IF. - - VLOG_IF's evaluation order is ambiguous - does it mean - `if (cond) VLOG(lvl)` or `if (VLOG_IS_ON(lvl) && cond) LOG(INFO)`? - In particular, the way it works now is inconsistent with the way the - rest of the LOG macros evaluate their arguments. - Fixing this would be hard, and the macro's behavior would still surprise - some people. Replacing it with an if statement is simple, clear, and unambiguous. - - Change-Id: I97a92d17a932c0a5344a1bf98d676308793ba877 - -commit d1b35ffc161fd857c7c433574ca82aa9b2db7f98 -Author: Sameer Agarwal <sameeragarwal@google.com> -Date: Mon Oct 12 10:58:05 2020 -0700 - - Corrections from William Rucklidge - - Change-Id: Ifb50e87aa915d00f9861fe1a6da0acee11bc0a94 - -commit f34e80e91f600014a3030915cf9ea28bcbc576e7 -Author: Sameer Agarwal <sameeragarwal@google.com> -Date: Thu Oct 8 12:34:53 2020 -0700 - - Add dividers between licenses. - - Change-Id: I4e4aaa15e0621c5648550cfa622fe0a79f1f4f9f - -commit 65c397daeca77da53d16e73720b9a17edd6757ab -Author: Sameer Agarwal <sameeragarwal@google.com> -Date: Wed Oct 7 14:34:52 2020 -0700 - - Fix formatting - - Change-Id: Ib4ca8a097059dbb8d2f3a6a888222c0188cb126e - -commit f63b1fea9cfa48ae4530c327b10efa4985e69631 -Author: Sameer Agarwal <sameeragarwal@google.com> -Date: Wed Oct 7 14:30:54 2020 -0700 - - Add the MIT license text corresponding to the libmv derived files. - - Change-Id: Ie72fb45ae96a7892c00411eee6873db7f0e365a8 - -commit 542613c13d8b7469822aff5eec076f2cad4507ec -Author: Nikolaus Demmel <nikolaus@nikolaus-demmel.de> -Date: Tue Oct 6 22:48:59 2020 +0200 - - minor formatting fix for trust_region_minimizer.cc - - Change-Id: I18ba27825fc23dd0e9e3e15dc13fc0833db01b5b - -commit 6d9e9843d8c61cfb04cc55b9def9518f823a592a -Author: Sameer Agarwal <sameeragarwal@google.com> -Date: Mon Sep 28 11:35:37 2020 -0700 - - Remove inclusion of ceres/eigen.h - - The initial reason for this is because of a previous reformatting CL - triggered a macro redefinition warning in the schur eliminator. But - actually it was worse because the reordering had caused the macro - definition to be ignored and caused a performance regression. - - This simplifies the generated files, fixes some formatting errors - and recovers the performance. - - Change-Id: I9dbeffc38743b3f24b25843feec2e26a73188413 - -commit eafeca5dcb7af8688d40a9c14b0d2fcb856c96fc -Author: Sameer Agarwal <sameeragarwal@google.com> -Date: Mon Sep 28 11:12:59 2020 -0700 - - Fix a logging bug in TrustRegionMinimizer. - - Upon encountering an unsuccessful step (one where the cost goes up) - the the trust region minimizer failed to populate the gradient norm - in the IterationSummary. This would cause the gradient norm to be - logged as zero which is incorrect. Instead it should be the gradient - norm at the current point. - - This CL fixes this issue. - - Before: - iter cost cost_change |gradient| |step| tr_ratio tr_radius ls_iter iter_time total_time - 0 1.115206e+07 0.00e+00 1.90e+07 0.00e+00 0.00e+00 1.00e+04 0 2.72e-01 1.33e+00 - 1 3.687552e+06 7.46e+06 1.84e+08 2.86e+03 6.91e-01 1.06e+04 1 1.32e+00 2.65e+00 - 2 3.670266e+10 -3.67e+10 0.00e+00 3.27e+03 -1.07e+04 5.30e+03 1 7.52e-01 3.40e+00 - 3 4.335397e+07 -3.97e+07 0.00e+00 2.74e+03 -1.16e+01 1.32e+03 1 7.28e-01 4.13e+00 - 4 1.345488e+06 2.34e+06 4.12e+07 1.55e+03 6.87e-01 1.40e+03 1 9.31e-01 5.06e+00 - 5 5.376653e+05 8.08e+05 9.99e+06 6.64e+02 7.46e-01 1.59e+03 1 9.64e-01 6.03e+00 - - After: - iter cost cost_change |gradient| |step| tr_ratio tr_radius ls_iter iter_time total_time - 0 1.115206e+07 0.00e+00 1.90e+07 0.00e+00 0.00e+00 1.00e+04 0 2.37e-01 1.13e+00 - 1 3.687552e+06 7.46e+06 1.84e+08 2.86e+03 6.91e-01 1.06e+04 1 1.08e+00 2.21e+00 - 2 3.670266e+10 -3.67e+10 1.84e+08 3.27e+03 -1.07e+04 5.30e+03 1 7.50e-01 2.96e+00 - 3 4.335397e+07 -3.97e+07 1.84e+08 2.74e+03 -1.16e+01 1.32e+03 1 7.13e-01 3.67e+00 - 4 1.345488e+06 2.34e+06 4.12e+07 1.55e+03 6.87e-01 1.40e+03 1 9.01e-01 4.57e+00 - 5 5.376653e+05 8.08e+05 9.99e+06 6.64e+02 7.46e-01 1.59e+03 1 9.36e-01 5.51e+00 - - Change-Id: Iae538fe089be07c7bb219337a6f1392f7213acfe - -commit 1fd0be916dd4ff4241bd52264b9e9170bc7e4339 -Author: Alex Stewart <alexs.mac@gmail.com> -Date: Mon Sep 28 18:54:33 2020 +0100 - - Fix default initialisation of IterationCallback::cost - - Change-Id: I9f529093fc09424c90dbff8e9648b90b16990623 - -commit 137bbe845577929a87f8eef979196df6a8b30ee4 -Author: Nikolaus Demmel <nikolaus@nikolaus-demmel.de> -Date: Mon Sep 28 02:17:32 2020 +0200 - - add info about clang-format to contributing docs - - Change-Id: I2f4dcbda2e4f36096df217d76de370103ffaa43e - -commit d3f66d77f45482b90d01af47938289c32dd2cc08 -Author: Nikolaus Demmel <nikolaus@nikolaus-demmel.de> -Date: Mon Sep 28 02:01:43 2020 +0200 - - fix formatting generated files (best effort) - - - update file generator scripts / templates so generated files adhere - to clang-format - - A few exceptions are not fixed, where the file generation results in - lines of different width. To properly fix this would make the code - more complicated and it's not that important for generated files - anyway. - - note that generated files are excluded in ./scripts/format_all.sh - - Change-Id: I4f42c83d1fec01242eada5e7ce6c1a5192234d37 - -commit a9c7361c8dc1d37e78d216754a4c03e8a8f1e74f -Author: Nikolaus Demmel <nikolaus@nikolaus-demmel.de> -Date: Mon Sep 28 02:14:29 2020 +0200 - - minor formatting fix (wrongly updated in earlier commit) - - Change-Id: I544635fd936cb5b7f7bd9255876641cd5a9590c6 - -commit 7b8f675bfdb1d924af6a2dcc1f79bda5ace7e886 -Author: Nikolaus Demmel <nikolaus@nikolaus-demmel.de> -Date: Sun Sep 20 21:45:24 2020 +0200 - - fix formatting for (non-generated) internal source files - - - Change formatting standard to Cpp11. Main difference is not having - the space between two closing >> for nested templates. We don't - choose c++14, because older versions of clang-format (version 9 - and earlier) don't know this value yet, and it doesn't make a - difference in the formatting. - - Apply clang-format to all (non generated) internal source files. - - Manually fix some code sections (clang-format on/off) and c-strings - - Exclude some embedded external files with very different formatting - (gtest/gmock) - - Add script to format all source files - - Change-Id: Ic6cea41575ad6e37c9e136dbce176b0d505dc44d - -commit 921368ce31c42ee793cf131860abba291a7e39ad -Author: Sameer Agarwal <sameeragarwal@google.com> -Date: Wed Sep 9 09:15:37 2020 -0700 - - Fix a number of typos in covariance.h - - Also some minor cleanups in covariance_impl.h - - Thanks to Lorenzo Lamia for pointing these out. - - Change-Id: Icb4012a367fdd1f249bc1e7019e0114c868e45b6 - -commit 7b6b2491cc1be0b3abb67338366d8d69bef3a402 -Author: Nikolaus Demmel <nikolaus@nikolaus-demmel.de> -Date: Tue Sep 8 17:51:32 2020 +0200 - - fix formatting for examples - - This is mostly just applying the existing clang format config, except: - - Use NOLINT on overlong comment lines. - - Wrap some sections in 'clang-format off' / 'clang format on'. - - Manually split or join some multi-line strings. - - Change-Id: Ia1a40eeb92112e12c3a169309afe087af55b2f4f - -commit 82275d8a4eac4fc0bd07e17c3a41a6e429e72bfb -Author: Nikolaus Demmel <nikolaus@nikolaus-demmel.de> -Date: Tue Sep 8 02:00:21 2020 +0200 - - some fixes for Linux and macOS install docs - - Linux: - - Remove workaround for Ubuntu 14.04, which is EOL. libsuitesparse-dev - seems to come with a shared library on 16.04 and later, so linking - to a shared build of ceres doesn't seem to be an issue any more. - - Add missing libgflags-dev. - - macOS: - - OS X is now called macOS. - - Update homebrew link. - - Mac homebrew the preferred method of installation. - - Fix OpenMP instructions. - - Remove reference to homebrew/science. Everything is in core. - - Add missing gflags. - - Change-Id: I633b3c7ea84a87886bfd823f8187fdd0a84737c9 - -commit 9d762d74f06b946bbd2f098de7216032d0e7b51d -Author: Nikolaus Demmel <nikolaus@nikolaus-demmel.de> -Date: Sun Sep 6 21:04:24 2020 +0200 - - fix formatting for public header files - - - ensure all public headers files adhere to clang-format - - preserve one-per-line for enums by adding trailing comma - - preserve include order for en/disable_warning.h - - Change-Id: I78dbd0527a294ab2ec5f074fb426e48b20c393e6 - -commit c76478c4898f3af11a6a826ac89c261205f4dd96 -Author: Nikolaus Demmel <nikolaus@nikolaus-demmel.de> -Date: Sun Sep 6 23:29:56 2020 +0200 - - gitignore *.pyc - - Change-Id: Ic6238a617a3c7ce92df7dcefcc44bae20c32b30b - -commit 4e69a475cd7d7cbed983f5aebf79ae13a46e5415 -Author: Alex Stewart <alexs.mac@gmail.com> -Date: Tue Sep 1 10:15:23 2020 +0100 - - Fix potential for mismatched release/debug TBB libraries - - - Protect against the case when the user has multiple installs of TBB - in their search paths and the first install does not contain debug - libraries. In this case it is possible to get mismatched versions - of TBB inserted into TBB_LIBRARIES. - - Also suppresses warning about use of TBB_ROOT on modern versions of - CMake due to CMP0074. - - Change-Id: I2eaafdde4a028cbf6c500c63771973d85bc4723d - -commit 8e1d8e32ad0d28c0d4d1d7b2b1ce7fc01d90b7b0 -Author: Sameer Agarwal <sameeragarwal@google.com> -Date: Thu Sep 3 10:49:20 2020 -0700 - - A number of small changes. - - 1. Add a move constructor to NumericDiffCostFunction, DynamicAutoDiffCostfunction - and DynamicNumericDiffCostFunction. - 2. Add optional ownership of the underlying functor. - 3. Update docs to reflect this as well as the variadic templates that allow an - arbitrary number of parameter blocks. - - Change-Id: I57bbb51fb9e75f36ec2a661b603beda270f30a19 - -commit 368a738e5281039f19587545806b7bc6f35e78f9 -Author: Julian Kent <jkflying@gmail.com> -Date: Thu May 7 12:54:35 2020 +0200 - - AutoDiffCostFunction: optional ownership - - Add Ownership semantics to the AutoDiffCostFunction - - This allows several benefits, such as pointer ordering always being the - same for numerical repeatability (due to blocks being ordered by - pointer address), memory adjacency for better cache performance, and - reduced allocator pressure / overhead. - - This is then made use of in libmv by preallocating the errors and - cost functions into vectors - - Change-Id: Ia5b97e7249b55a463264b6e26f7a02291927c9f2 - -commit 8cbd721c199c69f127af6ef7c187ddf7e8f116f9 -Author: Morten Hannemose <morten@hannemose.dk> -Date: Thu Sep 3 17:54:20 2020 +0200 - - Add erf and erfc to jet.h, including tests in jet_test.cc - - erf is necessary for evaluating Gaussian functions. - erfc was added because it is so similar to erf. - - Change-Id: I5e470dbe013cc938fabb87cde3b0ebf26a90fff4 - -commit 31366cff299cf2a8d97b43a7533d953ff28fdc29 -Author: Sameer Agarwal <sameeragarwal@google.com> -Date: Tue Sep 1 09:23:34 2020 -0700 - - Benchmarks for dynamic autodiff. - - This patch is from Clement Courbet. courbet@google.com - - Change-Id: I886390663644733bfa5b7b52b0c883079e793726 - -commit 29fb08aeae1ce691851724af7209fea6127523a9 -Author: Alex Stewart <alexs.mac@gmail.com> -Date: Tue Sep 1 10:23:31 2020 +0100 - - Use CMAKE_PREFIX_PATH to pass Homebrew install location - - - Passing HINTS disables the MODULE mode of find_package() which - precludes users from creating their own find modules to provide - Ceres' dependencies. - - Change-Id: I6f2edf429331d13fe67bf61ac4b79d17579d9a57 - -commit 242c703b501ffd64d645f4016d63c8b41c381038 -Author: Sameer Agarwal <sameeragarwal@google.com> -Date: Tue Aug 4 21:02:11 2020 -0700 - - Minor fixes to the documentation - - Change-Id: I65e6f648d963b8aa640078684ce02dcde6acb87d - -commit 79bbf95103672fa4b5485e055ff7692ee4a1f9da -Author: Sameer Agarwal <sameeragarwal@google.com> -Date: Tue Aug 4 18:26:02 2020 -0700 - - Add changelog for 2.0.0 - - Change-Id: I8acad62bfe629454ae5032732693e43fe37b97ff - -commit 41d05f13d0ffb230d7a5a9d67ed31b0cfb35d669 -Author: Sameer Agarwal <sameeragarwal@google.com> -Date: Tue Aug 4 14:56:33 2020 -0700 - - Fix lint errors in evaluation_callback_test.cc - - Change-Id: I63eb069544ad0d8f495490fe4caa07b9f04f7ec2 - -commit 4b67903c1f96037048c83a723028c5d0991c09cf -Author: Sameer Agarwal <sameeragarwal@google.com> -Date: Tue Aug 4 14:40:50 2020 -0700 - - Remove unused variables from problem_test.cc - - Change-Id: Ia1a13cfc6e462f6d249dcbf169ad34831dd93ec2 - -commit 10449fc3664c96d4b5454c092195432df79412f8 -Author: Sameer Agarwal <sameeragarwal@google.com> -Date: Tue Aug 4 14:30:25 2020 -0700 - - Add Apache license to the LICENSE file for FixedArray - - FixedArray implementation comes from ABSL which is Apache - licensed. - - Change-Id: I566dbe9d236814c95945732c6347d3bf7b508283 - -commit 8c3ecec6db26d7a66f5de8dc654475ec7aa0df14 -Author: Sameer Agarwal <sameeragarwal@google.com> -Date: Tue May 26 04:44:11 2020 -0700 - - Fix some minor errors in IterationCallback docs - - Change-Id: Id3d7f21a523ff8466868cdec542921c566bbbfa9 - -commit 7d3ffcb4234632dc51ee84c8a509d9428263070b -Author: Alex Stewart <alexs.mac@gmail.com> -Date: Sun Jul 26 19:42:16 2020 +0100 - - Remove forced CONFIG from find_package(Eigen3) - - - Ceres will fail to configure if Eigen3::Eigen target is not found, and - the minimum required Eigen version specified (3.3) exports Eigen as - a CMake package and this is reflected in the default Ubuntu 18.04 - packages. - - This permits users to specify their own Eigen3 detection should they - choose to do so, but they must do so via an imported target. - - Change-Id: I5edff117c8001770004f49012ac1ae63b66ec9c1 - -commit a029fc0f93817f20b387b707bc578dc1f1a269ae -Author: Alex Stewart <alexs.mac@gmail.com> -Date: Sun Jul 26 18:44:59 2020 +0100 - - Use latest FindTBB.cmake from VTK project - - - Retrieved from [1], SHA: 0d9bbf9beb97f8f696c43a9edf1e52c082b3639b on - 2020-07-26 - - [1]: https://gitlab.kitware.com/vtk/vtk/blob/master/CMake/FindTBB.cmake - - Change-Id: I953a8c87802a974d30ccc7c80f5229683826efbd - -commit aa1abbc578797c6b17ee7221db31535dc249ae66 -Author: Alex Stewart <alexs.mac@gmail.com> -Date: Sun Jul 26 19:57:31 2020 +0100 - - Replace use of GFLAGS_LIBRARIES with export gflags target - - - As our minimum required version of gflags (2.2) exports itself as - a CMake package and this is the case for the default 18.04 package - we can use the gflags target directly. - - Replaces forced use of CONFIG in find_package(gflags) with a check - that the gflags imported target exists to avoid ambiguity with - libgflags if installed in a default location. This permits users to - override the gflags detection should they so choose, provided that - they do so via an imported target. - - Also removes some previously removed legacy GLAGS_ vars from the - installation docs. - - Change-Id: I015f5a751e5b22f956bbf9df692e63a6825c9f0d - -commit db2af1be8780bbe88944775400baa2dbd3592b7d -Author: Sameer Agarwal <sameeragarwal@google.com> -Date: Mon Aug 3 04:57:08 2020 -0700 - - Add Problem::EvaluateResidualBlockAssumingParametersUnchanged - - Simplify the semantics for Problem::EvaluateResidualBlock to - not ignore the presence of EvaluationCallback and add another method - EvaluateResidualBlockAssumingParametersUnchanged to handle the case - where the user has an EvaluationCallback but knows that the parameter - blocks do not change between calls. - - Updated the documentation for the methods and EvaluationCallback to - reflect these semantics. - - Also added tests for Evaluation related methods calling i - EvaluationCallback when its present. - - https://github.com/ceres-solver/ceres-solver/issues/483 - - Change-Id: If0a0c95c2f1f92e9183a90df240104a69a71c46d - -commit ab4ed32cda004befd29a0b4b02f1d907e0c4dab7 -Author: Sameer Agarwal <sameeragarwal@google.com> -Date: Mon Aug 3 04:17:33 2020 -0700 - - Replace NULL with nullptr in the documentation. - - Change-Id: I995f68770e2a4b6027c0a1d3edf5eb5132b081d7 - -commit ee280e27a6140295ef6258d24c92305628f3d508 -Author: Sameer Agarwal <sameeragarwal@google.com> -Date: Fri Jul 31 16:48:06 2020 -0700 - - Allow SubsetParameterization to accept an empty vector of constant parameters. - - Thanks to Frédéric Devernay for reporting this and providing an initial fix. - - Change-Id: Id86a2051ab7841ecafdcfb00f4634b353a7ef3b4 - -commit 4b8c731d8a4f3fda53c642ff14a25fab6c233918 -Author: Sameer Agarwal <sameeragarwal@google.com> -Date: Fri Jul 31 10:05:52 2020 -0700 - - Fix a bug in DynamicAutoDiffCostFunction - - DynamicAutoDiffCostFunction::Evaluate when provided with a jacobians - array that was non-empty but all its entries are nullptr, would - compute num_active_parameters = 0, and then skip over all the loops - that evaluated the CostFunctor. - - The fix is to check if num_active_parameters == 0, and then treat - it as the case where jacobians array is null. - - Thanks to Ky Waegel for reporting and providing a reproduction for this. - - Change-Id: Ib86930c2c3f722724d249f662bf88238679bbf98 - -commit 5cb5b35a930c1702278083c75769dbb4e5801045 -Author: Alex Stewart <alexs.mac@gmail.com> -Date: Sun Jul 26 20:42:12 2020 +0100 - - Fixed incorrect argument name in RotationMatrixToQuaternion() - - - Raised as: https://github.com/ceres-solver/ceres-solver/pull/607 by - Frank Dellaert - - Change-Id: Id3e9f190e814cf18206e2f8c3b1b67b995c21dd5 - -commit e39d9ed1d60dfeb58dd2a0df4622c683f87b28e3 -Author: Carl Dehlin <carl@dehlin.com> -Date: Tue Jun 16 09:02:05 2020 +0200 - - Add a missing term and remove a superfluous word - - Change-Id: I25f40f0bf241302b975e6fc14690aa863c0728b0 - -commit 27cab77b699a1a2b5354820c57a91c92eaeb21e3 -Author: Carl Dehlin <carl@dehlin.com> -Date: Mon Jun 15 20:01:18 2020 +0200 - - Reformulate some sentences - - Change-Id: I4841aa8e8522008dd816261d9ad98e5fb8ad1758 - -commit 8ac6655ce85a4462f2882fcb9e9118a7057ebe09 -Author: Carl Dehlin <carl@dehlin.com> -Date: Mon Jun 15 19:10:12 2020 +0200 - - Fix documentation formatting issues - - Change-Id: Iea3a6e75dc3a7376eda866ab24e535a6df84f8ea diff --git a/extern/ceres/LICENSE b/extern/ceres/LICENSE index 2e3ead5ed45..cf69df2e02f 100644 --- a/extern/ceres/LICENSE +++ b/extern/ceres/LICENSE @@ -1,6 +1,6 @@ Ceres Solver - A fast non-linear least squares minimizer -Copyright 2010, 2011, 2012 Google Inc. All rights reserved. -http://code.google.com/p/ceres-solver/ +Copyright 2015 Google Inc. All rights reserved. +http://ceres-solver.org/ Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: @@ -25,3 +25,233 @@ 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. + +---------------------------------------------------------------------------------------------------------------------------- + + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "[]" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright [yyyy] [name of copyright owner] + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. + +------------------------------------------------------------------------------------------------------------------------------- + +Some of the code in the examples directory derives from libmv, which is +distributed under the MIT license as described below + + +Copyright (c) 2007-2011 libmv authors. + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to +deal in the Software without restriction, including without limitation the +rights to use, copy, modify, merge, publish, distribute, sublicense, and/or +sell copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING +FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS +IN THE SOFTWARE. diff --git a/extern/ceres/README b/extern/ceres/README deleted file mode 100644 index 8dd8ccf91a1..00000000000 --- a/extern/ceres/README +++ /dev/null @@ -1,3 +0,0 @@ -Ceres Solver - A non-linear least squares minimizer -================================================== -Please see ceres.pdf in docs/ for a tutorial and reference. diff --git a/extern/ceres/README.blender b/extern/ceres/README.blender index 20e5dda6f57..8d43f8d16c5 100644 --- a/extern/ceres/README.blender +++ b/extern/ceres/README.blender @@ -1,5 +1,5 @@ Project: Ceres Solver URL: http://ceres-solver.org/ License: BSD 3-Clause -Upstream version 2.0.0 +Upstream version 2.1.0 Local modifications: None diff --git a/extern/ceres/README.md b/extern/ceres/README.md new file mode 100644 index 00000000000..7de8f0deed0 --- /dev/null +++ b/extern/ceres/README.md @@ -0,0 +1,18 @@ +[](https://github.com/ceres-solver/ceres-solver/actions/workflows/android.yml) +[](https://github.com/ceres-solver/ceres-solver/actions/workflows/linux.yml) +[](https://github.com/ceres-solver/ceres-solver/actions/workflows/macos.yml) +[](https://github.com/ceres-solver/ceres-solver/actions/workflows/windows.yml) + +Ceres Solver +============ + +Ceres Solver is an open source C++ library for modeling and solving +large, complicated optimization problems. It is a feature rich, mature +and performant library which has been used in production at Google +since 2010. Ceres Solver can solve two kinds of problems. + +1. Non-linear Least Squares problems with bounds constraints. +2. General unconstrained optimization problems. + +Please see [ceres-solver.org](http://ceres-solver.org/) for more +information. diff --git a/extern/ceres/bundle.sh b/extern/ceres/bundle.sh deleted file mode 100755 index e2d2dce8779..00000000000 --- a/extern/ceres/bundle.sh +++ /dev/null @@ -1,165 +0,0 @@ -#!/bin/sh - -if [ "x$1" = "x--i-really-know-what-im-doing" ] ; then - echo Proceeding as requested by command line ... -else - echo "*** Please run again with --i-really-know-what-im-doing ..." - exit 1 -fi - -repo="https://ceres-solver.googlesource.com/ceres-solver" -#branch="master" -tag="2.0.0" -tmp=`mktemp -d` -checkout="$tmp/ceres" - -GIT="git --git-dir $tmp/ceres/.git --work-tree $checkout" - -git clone $repo $checkout - -if [ $branch != "master" ]; then - $GIT checkout -t remotes/origin/$branch -else - if [ "x$tag" != "x" ]; then - $GIT checkout $tag - fi -fi - -$GIT log -n 50 > ChangeLog - -for p in `cat ./patches/series`; do - echo "Applying patch $p..." - cat ./patches/$p | patch -d $tmp/ceres -p1 -done - -find include -type f -not -iwholename '*.svn*' -exec rm -rf {} \; -find internal -type f -not -iwholename '*.svn*' -exec rm -rf {} \; - -cat "files.txt" | while read f; do - mkdir -p `dirname $f` - cp $tmp/ceres/$f $f -done - -rm -rf $tmp - -sources=`find ./include ./internal -type f -iname '*.cc' -or -iname '*.cpp' -or -iname '*.c' | sed -r 's/^\.\//\t/' | \ - grep -v -E 'schur_eliminator_[0-9]_[0-9d]_[0-9d].cc' | \ - grep -v -E 'partitioned_matrix_view_[0-9]_[0-9d]_[0-9d].cc' | sort -d` -generated_sources=`find ./include ./internal -type f -iname '*.cc' -or -iname '*.cpp' -or -iname '*.c' | sed -r 's/^\.\//\t\t/' | \ - grep -E 'schur_eliminator_[0-9]_[0-9d]_[0-9d].cc|partitioned_matrix_view_[0-9]_[0-9d]_[0-9d].cc' | sort -d` -headers=`find ./include ./internal -type f -iname '*.h' | sed -r 's/^\.\//\t/' | sort -d` - -src_dir=`find ./internal -type f -iname '*.cc' -exec dirname {} \; -or -iname '*.cpp' -exec dirname {} \; -or -iname '*.c' -exec dirname {} \; | sed -r 's/^\.\//\t/' | sort -d | uniq` -src="" -for x in $src_dir $src_third_dir; do - t="" - - if test `echo "$x" | grep -c glog ` -eq 1; then - continue; - fi - - if test `echo "$x" | grep -c generated` -eq 1; then - continue; - fi - - if stat $x/*.cpp > /dev/null 2>&1; then - t="src += env.Glob('`echo $x'/*.cpp'`')" - fi - - if stat $x/*.c > /dev/null 2>&1; then - if [ -z "$t" ]; then - t="src += env.Glob('`echo $x'/*.c'`')" - else - t="$t + env.Glob('`echo $x'/*.c'`')" - fi - fi - - if stat $x/*.cc > /dev/null 2>&1; then - if [ -z "$t" ]; then - t="src += env.Glob('`echo $x'/*.cc'`')" - else - t="$t + env.Glob('`echo $x'/*.cc'`')" - fi - fi - - if [ -z "$src" ]; then - src=$t - else - src=`echo "$src\n$t"` - fi -done - -cat > CMakeLists.txt << EOF -# ***** BEGIN GPL LICENSE BLOCK ***** -# -# This program is free software; you can redistribute it and/or -# modify it under the terms of the GNU General Public License -# as published by the Free Software Foundation; either version 2 -# of the License, or (at your option) any later version. -# -# This program is distributed in the hope that it will be useful, -# but WITHOUT ANY WARRANTY; without even the implied warranty of -# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -# GNU General Public License for more details. -# -# You should have received a copy of the GNU General Public License -# along with this program; if not, write to the Free Software Foundation, -# Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. -# -# The Original Code is Copyright (C) 2012, Blender Foundation -# All rights reserved. -# ***** END GPL LICENSE BLOCK ***** - -# NOTE: This file is automatically generated by bundle.sh script -# If you're doing changes in this file, please update template -# in that script too - -set(INC - . - include - internal - config - ../gflags/src -) - -set(INC_SYS - \${EIGEN3_INCLUDE_DIRS} - \${GFLAGS_INCLUDE_DIRS} - \${GLOG_INCLUDE_DIRS} -) - -set(SRC -${sources} - -${headers} -) - -set(LIB - \${GLOG_LIBRARIES} - \${GFLAGS_LIBRARIES} -) - -if(WITH_LIBMV_SCHUR_SPECIALIZATIONS) - list(APPEND SRC -${generated_sources} - ) -else() - add_definitions(-DCERES_RESTRICT_SCHUR_SPECIALIZATION) -endif() - -add_definitions(\${GFLAGS_DEFINES}) -add_definitions(\${GLOG_DEFINES}) -add_definitions(\${CERES_DEFINES}) - -add_definitions( - -DCERES_HAVE_PTHREAD - -DCERES_NO_SUITESPARSE - -DCERES_NO_CXSPARSE - -DCERES_NO_LAPACK - -DCERES_NO_ACCELERATE_SPARSE - -DCERES_HAVE_RWLOCK - -DCERES_USE_CXX_THREADS -) - -blender_add_lib(extern_ceres "\${SRC}" "\${INC}" "\${INC_SYS}" "\${LIB}") -EOF diff --git a/extern/ceres/config/ceres/internal/config.h b/extern/ceres/config/ceres/internal/config.h index 1cf034ded5f..2566945e084 100644 --- a/extern/ceres/config/ceres/internal/config.h +++ b/extern/ceres/config/ceres/internal/config.h @@ -1,5 +1,5 @@ // Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2015 Google Inc. All rights reserved. +// Copyright 2022 Google Inc. All rights reserved. // http://ceres-solver.org/ // // Redistribution and use in source and binary forms, with or without @@ -28,21 +28,98 @@ // // Author: alexs.mac@gmail.com (Alex Stewart) -// Default (empty) configuration options for Ceres. +// Configuration options for Ceres. // -// IMPORTANT: Most users of Ceres will not use this file, when -// compiling Ceres with CMake, CMake will configure a new -// config.h with the currently selected Ceres compile -// options in <BUILD_DIR>/config, which will be added to -// the include path for compilation, and installed with the -// public Ceres headers. However, for some users of Ceres -// who compile without CMake (Android), this file ensures -// that Ceres will compile, with the user either specifying -// manually the Ceres compile options, or passing them -// directly through the compiler. +// Do not edit this file, it was automatically configured by CMake when +// Ceres was compiled with the relevant configuration for the machine +// on which Ceres was compiled. +// +// Ceres Developers: All options should have the same name as their mapped +// CMake options, in the preconfigured version of this file +// all options should be enclosed in '@'. #ifndef CERES_PUBLIC_INTERNAL_CONFIG_H_ #define CERES_PUBLIC_INTERNAL_CONFIG_H_ +// If defined, use the LGPL code in Eigen. +#define CERES_USE_EIGEN_SPARSE + +// If defined, Ceres was compiled without LAPACK. +#define CERES_NO_LAPACK + +// If defined, Ceres was compiled without SuiteSparse. +#define CERES_NO_SUITESPARSE + +// If defined, Ceres was compiled without CXSparse. +#define CERES_NO_CXSPARSE + +// If defined, Ceres was compiled without CUDA. +#define CERES_NO_CUDA + +// If defined, Ceres was compiled without Apple's Accelerate framework solvers. +#define CERES_NO_ACCELERATE_SPARSE + +#if defined(CERES_NO_SUITESPARSE) && \ + defined(CERES_NO_ACCELERATE_SPARSE) && \ + defined(CERES_NO_CXSPARSE) && \ + !defined(CERES_USE_EIGEN_SPARSE) // NOLINT +// If defined Ceres was compiled without any sparse linear algebra support. +#define CERES_NO_SPARSE +#endif + +// If defined, Ceres was compiled without Schur specializations. +// #define CERES_RESTRICT_SCHUR_SPECIALIZATION + +// If defined, Ceres was compiled to use Eigen instead of hardcoded BLAS +// routines. +// #define CERES_NO_CUSTOM_BLAS + +// If defined, Ceres was compiled without multithreading support. +// #define CERES_NO_THREADS +// If defined Ceres was compiled with OpenMP multithreading. +// #define CERES_USE_OPENMP +// If defined Ceres was compiled with modern C++ multithreading. +#define CERES_USE_CXX_THREADS + +// If defined, Ceres was compiled with a version MSVC >= 2005 which +// deprecated the standard POSIX names for bessel functions, replacing them +// with underscore prefixed versions (e.g. j0() -> _j0()). +#ifdef _MSC_VER +#define CERES_MSVC_USE_UNDERSCORE_PREFIXED_BESSEL_FUNCTIONS +#endif + +#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 #endif // CERES_PUBLIC_INTERNAL_CONFIG_H_ diff --git a/extern/ceres/config/ceres/internal/export.h b/extern/ceres/config/ceres/internal/export.h new file mode 100644 index 00000000000..c85bc5ca65d --- /dev/null +++ b/extern/ceres/config/ceres/internal/export.h @@ -0,0 +1,42 @@ + +#ifndef CERES_EXPORT_H +#define CERES_EXPORT_H + +#ifdef CERES_STATIC_DEFINE +# define CERES_EXPORT +# define CERES_NO_EXPORT +#else +# ifndef CERES_EXPORT +# ifdef ceres_EXPORTS + /* We are building this library */ +# define CERES_EXPORT +# else + /* We are using this library */ +# define CERES_EXPORT +# endif +# endif + +# ifndef CERES_NO_EXPORT +# define CERES_NO_EXPORT +# endif +#endif + +#ifndef CERES_DEPRECATED +# define CERES_DEPRECATED __attribute__ ((__deprecated__)) +#endif + +#ifndef CERES_DEPRECATED_EXPORT +# define CERES_DEPRECATED_EXPORT CERES_EXPORT CERES_DEPRECATED +#endif + +#ifndef CERES_DEPRECATED_NO_EXPORT +# define CERES_DEPRECATED_NO_EXPORT CERES_NO_EXPORT CERES_DEPRECATED +#endif + +#if 0 /* DEFINE_NO_DEPRECATED */ +# ifndef CERES_NO_DEPRECATED +# define CERES_NO_DEPRECATED +# endif +#endif + +#endif /* CERES_EXPORT_H */ diff --git a/extern/ceres/files.txt b/extern/ceres/files.txt deleted file mode 100644 index bfbd57090c9..00000000000 --- a/extern/ceres/files.txt +++ /dev/null @@ -1,318 +0,0 @@ -include/ceres/autodiff_cost_function.h -include/ceres/autodiff_first_order_function.h -include/ceres/autodiff_local_parameterization.h -include/ceres/c_api.h -include/ceres/ceres.h -include/ceres/conditioned_cost_function.h -include/ceres/context.h -include/ceres/cost_function.h -include/ceres/cost_function_to_functor.h -include/ceres/covariance.h -include/ceres/crs_matrix.h -include/ceres/cubic_interpolation.h -include/ceres/dynamic_autodiff_cost_function.h -include/ceres/dynamic_cost_function.h -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/internal/array_selector.h -include/ceres/internal/autodiff.h -include/ceres/internal/disable_warnings.h -include/ceres/internal/eigen.h -include/ceres/internal/fixed_array.h -include/ceres/internal/householder_vector.h -include/ceres/internal/integer_sequence_algorithm.h -include/ceres/internal/line_parameterization.h -include/ceres/internal/memory.h -include/ceres/internal/numeric_diff.h -include/ceres/internal/parameter_dims.h -include/ceres/internal/port.h -include/ceres/internal/reenable_warnings.h -include/ceres/internal/variadic_evaluate.h -include/ceres/iteration_callback.h -include/ceres/jet.h -include/ceres/local_parameterization.h -include/ceres/loss_function.h -include/ceres/normal_prior.h -include/ceres/numeric_diff_cost_function.h -include/ceres/numeric_diff_options.h -include/ceres/ordered_groups.h -include/ceres/problem.h -include/ceres/rotation.h -include/ceres/sized_cost_function.h -include/ceres/solver.h -include/ceres/tiny_solver_autodiff_function.h -include/ceres/tiny_solver_cost_function_adapter.h -include/ceres/tiny_solver.h -include/ceres/types.h -include/ceres/version.h -internal/ceres/accelerate_sparse.cc -internal/ceres/accelerate_sparse.h -internal/ceres/array_utils.cc -internal/ceres/array_utils.h -internal/ceres/blas.cc -internal/ceres/blas.h -internal/ceres/block_evaluate_preparer.cc -internal/ceres/block_evaluate_preparer.h -internal/ceres/block_jacobian_writer.cc -internal/ceres/block_jacobian_writer.h -internal/ceres/block_jacobi_preconditioner.cc -internal/ceres/block_jacobi_preconditioner.h -internal/ceres/block_random_access_dense_matrix.cc -internal/ceres/block_random_access_dense_matrix.h -internal/ceres/block_random_access_diagonal_matrix.cc -internal/ceres/block_random_access_diagonal_matrix.h -internal/ceres/block_random_access_matrix.cc -internal/ceres/block_random_access_matrix.h -internal/ceres/block_random_access_sparse_matrix.cc -internal/ceres/block_random_access_sparse_matrix.h -internal/ceres/block_sparse_matrix.cc -internal/ceres/block_sparse_matrix.h -internal/ceres/block_structure.cc -internal/ceres/block_structure.h -internal/ceres/callbacks.cc -internal/ceres/callbacks.h -internal/ceres/canonical_views_clustering.cc -internal/ceres/canonical_views_clustering.h -internal/ceres/c_api.cc -internal/ceres/casts.h -internal/ceres/cgnr_linear_operator.h -internal/ceres/cgnr_solver.cc -internal/ceres/cgnr_solver.h -internal/ceres/compressed_col_sparse_matrix_utils.cc -internal/ceres/compressed_col_sparse_matrix_utils.h -internal/ceres/compressed_row_jacobian_writer.cc -internal/ceres/compressed_row_jacobian_writer.h -internal/ceres/compressed_row_sparse_matrix.cc -internal/ceres/compressed_row_sparse_matrix.h -internal/ceres/concurrent_queue.h -internal/ceres/conditioned_cost_function.cc -internal/ceres/conjugate_gradients_solver.cc -internal/ceres/conjugate_gradients_solver.h -internal/ceres/context.cc -internal/ceres/context_impl.cc -internal/ceres/context_impl.h -internal/ceres/coordinate_descent_minimizer.cc -internal/ceres/coordinate_descent_minimizer.h -internal/ceres/corrector.cc -internal/ceres/corrector.h -internal/ceres/covariance.cc -internal/ceres/covariance_impl.cc -internal/ceres/covariance_impl.h -internal/ceres/cxsparse.cc -internal/ceres/cxsparse.h -internal/ceres/dense_jacobian_writer.h -internal/ceres/dense_normal_cholesky_solver.cc -internal/ceres/dense_normal_cholesky_solver.h -internal/ceres/dense_qr_solver.cc -internal/ceres/dense_qr_solver.h -internal/ceres/dense_sparse_matrix.cc -internal/ceres/dense_sparse_matrix.h -internal/ceres/detect_structure.cc -internal/ceres/detect_structure.h -internal/ceres/dogleg_strategy.cc -internal/ceres/dogleg_strategy.h -internal/ceres/dynamic_compressed_row_finalizer.h -internal/ceres/dynamic_compressed_row_jacobian_writer.cc -internal/ceres/dynamic_compressed_row_jacobian_writer.h -internal/ceres/dynamic_compressed_row_sparse_matrix.cc -internal/ceres/dynamic_compressed_row_sparse_matrix.h -internal/ceres/dynamic_sparse_normal_cholesky_solver.cc -internal/ceres/dynamic_sparse_normal_cholesky_solver.h -internal/ceres/eigensparse.cc -internal/ceres/eigensparse.h -internal/ceres/evaluator.cc -internal/ceres/evaluator.h -internal/ceres/execution_summary.h -internal/ceres/file.cc -internal/ceres/file.h -internal/ceres/float_cxsparse.cc -internal/ceres/float_cxsparse.h -internal/ceres/float_suitesparse.cc -internal/ceres/float_suitesparse.h -internal/ceres/function_sample.cc -internal/ceres/function_sample.h -internal/ceres/generated/partitioned_matrix_view_2_2_2.cc -internal/ceres/generated/partitioned_matrix_view_2_2_3.cc -internal/ceres/generated/partitioned_matrix_view_2_2_4.cc -internal/ceres/generated/partitioned_matrix_view_2_2_d.cc -internal/ceres/generated/partitioned_matrix_view_2_3_3.cc -internal/ceres/generated/partitioned_matrix_view_2_3_4.cc -internal/ceres/generated/partitioned_matrix_view_2_3_6.cc -internal/ceres/generated/partitioned_matrix_view_2_3_9.cc -internal/ceres/generated/partitioned_matrix_view_2_3_d.cc -internal/ceres/generated/partitioned_matrix_view_2_4_3.cc -internal/ceres/generated/partitioned_matrix_view_2_4_4.cc -internal/ceres/generated/partitioned_matrix_view_2_4_6.cc -internal/ceres/generated/partitioned_matrix_view_2_4_8.cc -internal/ceres/generated/partitioned_matrix_view_2_4_9.cc -internal/ceres/generated/partitioned_matrix_view_2_4_d.cc -internal/ceres/generated/partitioned_matrix_view_2_d_d.cc -internal/ceres/generated/partitioned_matrix_view_3_3_3.cc -internal/ceres/generated/partitioned_matrix_view_4_4_2.cc -internal/ceres/generated/partitioned_matrix_view_4_4_3.cc -internal/ceres/generated/partitioned_matrix_view_4_4_4.cc -internal/ceres/generated/partitioned_matrix_view_4_4_d.cc -internal/ceres/generated/partitioned_matrix_view_d_d_d.cc -internal/ceres/generated/schur_eliminator_2_2_2.cc -internal/ceres/generated/schur_eliminator_2_2_3.cc -internal/ceres/generated/schur_eliminator_2_2_4.cc -internal/ceres/generated/schur_eliminator_2_2_d.cc -internal/ceres/generated/schur_eliminator_2_3_3.cc -internal/ceres/generated/schur_eliminator_2_3_4.cc -internal/ceres/generated/schur_eliminator_2_3_6.cc -internal/ceres/generated/schur_eliminator_2_3_9.cc -internal/ceres/generated/schur_eliminator_2_3_d.cc -internal/ceres/generated/schur_eliminator_2_4_3.cc -internal/ceres/generated/schur_eliminator_2_4_4.cc -internal/ceres/generated/schur_eliminator_2_4_6.cc -internal/ceres/generated/schur_eliminator_2_4_8.cc -internal/ceres/generated/schur_eliminator_2_4_9.cc -internal/ceres/generated/schur_eliminator_2_4_d.cc -internal/ceres/generated/schur_eliminator_2_d_d.cc -internal/ceres/generated/schur_eliminator_3_3_3.cc -internal/ceres/generated/schur_eliminator_4_4_2.cc -internal/ceres/generated/schur_eliminator_4_4_3.cc -internal/ceres/generated/schur_eliminator_4_4_4.cc -internal/ceres/generated/schur_eliminator_4_4_d.cc -internal/ceres/generated/schur_eliminator_d_d_d.cc -internal/ceres/generate_template_specializations.py -internal/ceres/gradient_checker.cc -internal/ceres/gradient_checking_cost_function.cc -internal/ceres/gradient_checking_cost_function.h -internal/ceres/gradient_problem.cc -internal/ceres/gradient_problem_evaluator.h -internal/ceres/gradient_problem_solver.cc -internal/ceres/graph_algorithms.h -internal/ceres/graph.h -internal/ceres/implicit_schur_complement.cc -internal/ceres/implicit_schur_complement.h -internal/ceres/inner_product_computer.cc -internal/ceres/inner_product_computer.h -internal/ceres/invert_psd_matrix.h -internal/ceres/is_close.cc -internal/ceres/is_close.h -internal/ceres/iterative_refiner.cc -internal/ceres/iterative_refiner.h -internal/ceres/iterative_schur_complement_solver.cc -internal/ceres/iterative_schur_complement_solver.h -internal/ceres/lapack.cc -internal/ceres/lapack.h -internal/ceres/levenberg_marquardt_strategy.cc -internal/ceres/levenberg_marquardt_strategy.h -internal/ceres/linear_least_squares_problems.cc -internal/ceres/linear_least_squares_problems.h -internal/ceres/linear_operator.cc -internal/ceres/linear_operator.h -internal/ceres/linear_solver.cc -internal/ceres/linear_solver.h -internal/ceres/line_search.cc -internal/ceres/line_search_direction.cc -internal/ceres/line_search_direction.h -internal/ceres/line_search.h -internal/ceres/line_search_minimizer.cc -internal/ceres/line_search_minimizer.h -internal/ceres/line_search_preprocessor.cc -internal/ceres/line_search_preprocessor.h -internal/ceres/local_parameterization.cc -internal/ceres/loss_function.cc -internal/ceres/low_rank_inverse_hessian.cc -internal/ceres/low_rank_inverse_hessian.h -internal/ceres/map_util.h -internal/ceres/minimizer.cc -internal/ceres/minimizer.h -internal/ceres/normal_prior.cc -internal/ceres/pair_hash.h -internal/ceres/parallel_for_cxx.cc -internal/ceres/parallel_for.h -internal/ceres/parallel_for_nothreads.cc -internal/ceres/parallel_for_openmp.cc -internal/ceres/parallel_utils.cc -internal/ceres/parallel_utils.h -internal/ceres/parameter_block.h -internal/ceres/parameter_block_ordering.cc -internal/ceres/parameter_block_ordering.h -internal/ceres/partitioned_matrix_view.cc -internal/ceres/partitioned_matrix_view.h -internal/ceres/partitioned_matrix_view_impl.h -internal/ceres/partitioned_matrix_view_template.py -internal/ceres/polynomial.cc -internal/ceres/polynomial.h -internal/ceres/preconditioner.cc -internal/ceres/preconditioner.h -internal/ceres/preprocessor.cc -internal/ceres/preprocessor.h -internal/ceres/problem.cc -internal/ceres/problem_impl.cc -internal/ceres/problem_impl.h -internal/ceres/program.cc -internal/ceres/program_evaluator.h -internal/ceres/program.h -internal/ceres/random.h -internal/ceres/reorder_program.cc -internal/ceres/reorder_program.h -internal/ceres/residual_block.cc -internal/ceres/residual_block.h -internal/ceres/residual_block_utils.cc -internal/ceres/residual_block_utils.h -internal/ceres/schur_complement_solver.cc -internal/ceres/schur_complement_solver.h -internal/ceres/schur_eliminator.cc -internal/ceres/schur_eliminator.h -internal/ceres/schur_eliminator_impl.h -internal/ceres/schur_eliminator_template.py -internal/ceres/schur_jacobi_preconditioner.cc -internal/ceres/schur_jacobi_preconditioner.h -internal/ceres/schur_templates.cc -internal/ceres/schur_templates.h -internal/ceres/scoped_thread_token.h -internal/ceres/scratch_evaluate_preparer.cc -internal/ceres/scratch_evaluate_preparer.h -internal/ceres/single_linkage_clustering.cc -internal/ceres/single_linkage_clustering.h -internal/ceres/small_blas_generic.h -internal/ceres/small_blas.h -internal/ceres/solver.cc -internal/ceres/solver_utils.cc -internal/ceres/solver_utils.h -internal/ceres/sparse_cholesky.cc -internal/ceres/sparse_cholesky.h -internal/ceres/sparse_matrix.cc -internal/ceres/sparse_matrix.h -internal/ceres/sparse_normal_cholesky_solver.cc -internal/ceres/sparse_normal_cholesky_solver.h -internal/ceres/split.cc -internal/ceres/split.h -internal/ceres/stl_util.h -internal/ceres/stringprintf.cc -internal/ceres/stringprintf.h -internal/ceres/subset_preconditioner.cc -internal/ceres/subset_preconditioner.h -internal/ceres/suitesparse.cc -internal/ceres/suitesparse.h -internal/ceres/thread_pool.cc -internal/ceres/thread_pool.h -internal/ceres/thread_token_provider.cc -internal/ceres/thread_token_provider.h -internal/ceres/triplet_sparse_matrix.cc -internal/ceres/triplet_sparse_matrix.h -internal/ceres/trust_region_minimizer.cc -internal/ceres/trust_region_minimizer.h -internal/ceres/trust_region_preprocessor.cc -internal/ceres/trust_region_preprocessor.h -internal/ceres/trust_region_step_evaluator.cc -internal/ceres/trust_region_step_evaluator.h -internal/ceres/trust_region_strategy.cc -internal/ceres/trust_region_strategy.h -internal/ceres/types.cc -internal/ceres/visibility_based_preconditioner.cc -internal/ceres/visibility_based_preconditioner.h -internal/ceres/visibility.cc -internal/ceres/visibility.h -internal/ceres/wall_time.cc -internal/ceres/wall_time.h -config/ceres/internal/config.h diff --git a/extern/ceres/include/ceres/autodiff_cost_function.h b/extern/ceres/include/ceres/autodiff_cost_function.h index 207f0a41688..cd256432a98 100644 --- a/extern/ceres/include/ceres/autodiff_cost_function.h +++ b/extern/ceres/include/ceres/autodiff_cost_function.h @@ -151,7 +151,8 @@ namespace ceres { 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"). @@ -178,7 +179,7 @@ class AutoDiffCostFunction : public SizedCostFunction<kNumResiduals, Ns...> { SizedCostFunction<kNumResiduals, Ns...>::set_num_residuals(num_residuals); } - explicit AutoDiffCostFunction(AutoDiffCostFunction&& other) + AutoDiffCostFunction(AutoDiffCostFunction&& other) : functor_(std::move(other.functor_)), ownership_(other.ownership_) {} virtual ~AutoDiffCostFunction() { @@ -215,6 +216,8 @@ class AutoDiffCostFunction : public SizedCostFunction<kNumResiduals, Ns...> { jacobians); }; + const CostFunctor& functor() const { return *functor_; } + private: std::unique_ptr<CostFunctor> functor_; Ownership ownership_; diff --git a/extern/ceres/include/ceres/autodiff_first_order_function.h b/extern/ceres/include/ceres/autodiff_first_order_function.h index b98d845655b..7c13f4239a6 100644 --- a/extern/ceres/include/ceres/autodiff_first_order_function.h +++ b/extern/ceres/include/ceres/autodiff_first_order_function.h @@ -102,7 +102,7 @@ namespace ceres { // 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) @@ -110,8 +110,6 @@ class AutoDiffFirstOrderFunction : public FirstOrderFunction { static_assert(kNumParameters > 0, "kNumParameters must be positive"); } - virtual ~AutoDiffFirstOrderFunction() {} - bool Evaluate(const double* const parameters, double* cost, double* gradient) const override { @@ -119,7 +117,7 @@ class AutoDiffFirstOrderFunction : public FirstOrderFunction { 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 +140,8 @@ class AutoDiffFirstOrderFunction : public FirstOrderFunction { int NumParameters() const override { return kNumParameters; } + const FirstOrderFunctor& functor() const { return *functor_; } + private: std::unique_ptr<FirstOrderFunctor> functor_; }; diff --git a/extern/ceres/include/ceres/autodiff_local_parameterization.h b/extern/ceres/include/ceres/autodiff_local_parameterization.h index d694376fdd1..5f9b04d0670 100644 --- a/extern/ceres/include/ceres/autodiff_local_parameterization.h +++ b/extern/ceres/include/ceres/autodiff_local_parameterization.h @@ -40,6 +40,10 @@ namespace ceres { +// WARNING: LocalParameterizations are deprecated, so is +// AutoDiffLocalParameterization. They will be removed from Ceres Solver in +// version 2.2.0. Please use Manifolds and AutoDiffManifold instead. + // Create local parameterization with Jacobians computed via automatic // differentiation. For more information on local parameterizations, // see include/ceres/local_parameterization.h @@ -106,7 +110,8 @@ namespace ceres { // seen where instead of using k_ directly, k_ is wrapped with T(k_). template <typename Functor, int kGlobalSize, int kLocalSize> -class AutoDiffLocalParameterization : public LocalParameterization { +class CERES_DEPRECATED_WITH_MSG("Use AutoDiffManifold instead.") + AutoDiffLocalParameterization : public LocalParameterization { public: AutoDiffLocalParameterization() : functor_(new Functor()) {} @@ -114,7 +119,6 @@ class AutoDiffLocalParameterization : public LocalParameterization { explicit AutoDiffLocalParameterization(Functor* functor) : functor_(functor) {} - virtual ~AutoDiffLocalParameterization() {} bool Plus(const double* x, const double* delta, double* x_plus_delta) const override { @@ -133,7 +137,7 @@ class AutoDiffLocalParameterization : public LocalParameterization { } const double* parameter_ptrs[2] = {x, zero_delta}; - double* jacobian_ptrs[2] = {NULL, jacobian}; + double* jacobian_ptrs[2] = {nullptr, jacobian}; return internal::AutoDifferentiate< kGlobalSize, internal::StaticParameterDims<kGlobalSize, kLocalSize>>( @@ -143,6 +147,8 @@ class AutoDiffLocalParameterization : public LocalParameterization { int GlobalSize() const override { return kGlobalSize; } int LocalSize() const override { return kLocalSize; } + const Functor& functor() const { return *functor_; } + private: std::unique_ptr<Functor> functor_; }; diff --git a/extern/ceres/include/ceres/autodiff_manifold.h b/extern/ceres/include/ceres/autodiff_manifold.h new file mode 100644 index 00000000000..3063e19e802 --- /dev/null +++ b/extern/ceres/include/ceres/autodiff_manifold.h @@ -0,0 +1,259 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2022 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_y 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/extern/ceres/include/ceres/c_api.h b/extern/ceres/include/ceres/c_api.h index 91b82bf995f..1be8ca2e077 100644 --- a/extern/ceres/include/ceres/c_api.h +++ b/extern/ceres/include/ceres/c_api.h @@ -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/extern/ceres/include/ceres/ceres.h b/extern/ceres/include/ceres/ceres.h index d249351694c..c32477d4254 100644 --- a/extern/ceres/include/ceres/ceres.h +++ b/extern/ceres/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 2022 Google Inc. All rights reserved. // http://ceres-solver.org/ // // Redistribution and use in source and binary forms, with or without @@ -35,7 +35,9 @@ #define CERES_PUBLIC_CERES_H_ #include "ceres/autodiff_cost_function.h" +#include "ceres/autodiff_first_order_function.h" #include "ceres/autodiff_local_parameterization.h" +#include "ceres/autodiff_manifold.h" #include "ceres/conditioned_cost_function.h" #include "ceres/context.h" #include "ceres/cost_function.h" @@ -47,19 +49,25 @@ #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/line_manifold.h" #include "ceres/local_parameterization.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" diff --git a/extern/ceres/include/ceres/conditioned_cost_function.h b/extern/ceres/include/ceres/conditioned_cost_function.h index a57ee209b80..e4c3decbfd5 100644 --- a/extern/ceres/include/ceres/conditioned_cost_function.h +++ b/extern/ceres/include/ceres/conditioned_cost_function.h @@ -71,18 +71,18 @@ namespace ceres { // 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/extern/ceres/include/ceres/context.h b/extern/ceres/include/ceres/context.h index d08e32b31a8..6c6e8f4c953 100644 --- a/extern/ceres/include/ceres/context.h +++ b/extern/ceres/include/ceres/context.h @@ -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 @@ namespace ceres { // 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/extern/ceres/include/ceres/cost_function.h b/extern/ceres/include/ceres/cost_function.h index d1550c119e8..fef972b75af 100644 --- a/extern/ceres/include/ceres/cost_function.h +++ b/extern/ceres/include/ceres/cost_function.h @@ -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 @@ namespace ceres { // when added with AddResidualBlock(). class CERES_EXPORT CostFunction { public: - CostFunction() : num_residuals_(0) {} + CostFunction(); CostFunction(const CostFunction&) = delete; void operator=(const CostFunction&) = delete; - virtual ~CostFunction() {} + virtual ~CostFunction(); // Inputs: // @@ -92,8 +92,8 @@ class CERES_EXPORT CostFunction { // 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. // diff --git a/extern/ceres/include/ceres/cost_function_to_functor.h b/extern/ceres/include/ceres/cost_function_to_functor.h index 9364293afc5..08a8050c5f8 100644 --- a/extern/ceres/include/ceres/cost_function_to_functor.h +++ b/extern/ceres/include/ceres/cost_function_to_functor.h @@ -94,10 +94,11 @@ #include "ceres/cost_function.h" #include "ceres/dynamic_cost_function_to_functor.h" +#include "ceres/internal/export.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 { diff --git a/extern/ceres/include/ceres/covariance.h b/extern/ceres/include/ceres/covariance.h index 2fe025df3ce..60bcc80b80f 100644 --- a/extern/ceres/include/ceres/covariance.h +++ b/extern/ceres/include/ceres/covariance.h @@ -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 @@ class CovarianceImpl; // 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 LocalParameterization/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. diff --git a/extern/ceres/include/ceres/crs_matrix.h b/extern/ceres/include/ceres/crs_matrix.h index bc618fa0905..faa0f988528 100644 --- a/extern/ceres/include/ceres/crs_matrix.h +++ b/extern/ceres/include/ceres/crs_matrix.h @@ -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/extern/ceres/include/ceres/cubic_interpolation.h b/extern/ceres/include/ceres/cubic_interpolation.h index 9b9ea4a942c..3ca6b11b407 100644 --- a/extern/ceres/include/ceres/cubic_interpolation.h +++ b/extern/ceres/include/ceres/cubic_interpolation.h @@ -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 @@ namespace ceres { // 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 @@ void CubicHermiteSpline(const Eigen::Matrix<double, kDataDimension, 1>& p0, 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 @@ void CubicHermiteSpline(const Eigen::Matrix<double, kDataDimension, 1>& p0, // 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 @@ class CubicInterpolator { // 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 @@ struct Grid1D { } 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 @@ class BiCubicInterpolator { // 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 @@ class BiCubicInterpolator { // 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> @@ -402,9 +402,9 @@ struct Grid2D { 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/extern/ceres/include/ceres/dynamic_autodiff_cost_function.h b/extern/ceres/include/ceres/dynamic_autodiff_cost_function.h index 7ccf6a88c32..c21d0517f27 100644 --- a/extern/ceres/include/ceres/dynamic_autodiff_cost_function.h +++ b/extern/ceres/include/ceres/dynamic_autodiff_cost_function.h @@ -77,17 +77,17 @@ namespace ceres { // 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: // Takes ownership by default. - DynamicAutoDiffCostFunction(CostFunctor* functor, - Ownership ownership = TAKE_OWNERSHIP) + explicit DynamicAutoDiffCostFunction(CostFunctor* functor, + Ownership ownership = TAKE_OWNERSHIP) : functor_(functor), ownership_(ownership) {} - explicit DynamicAutoDiffCostFunction(DynamicAutoDiffCostFunction&& other) + DynamicAutoDiffCostFunction(DynamicAutoDiffCostFunction&& other) : functor_(std::move(other.functor_)), ownership_(other.ownership_) {} - virtual ~DynamicAutoDiffCostFunction() { + ~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 +105,7 @@ class DynamicAutoDiffCostFunction : public DynamicCostFunction { << "You must call DynamicAutoDiffCostFunction::SetNumResiduals() " << "before DynamicAutoDiffCostFunction::Evaluate()."; - if (jacobians == NULL) { + if (jacobians == nullptr) { return (*functor_)(parameters, residuals); } @@ -150,7 +150,7 @@ class DynamicAutoDiffCostFunction : public DynamicCostFunction { 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 +209,7 @@ class DynamicAutoDiffCostFunction : public DynamicCostFunction { 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 +238,7 @@ class DynamicAutoDiffCostFunction : public DynamicCostFunction { 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]; diff --git a/extern/ceres/include/ceres/dynamic_cost_function.h b/extern/ceres/include/ceres/dynamic_cost_function.h index 6e8a076ecd0..c84a366dafb 100644 --- a/extern/ceres/include/ceres/dynamic_cost_function.h +++ b/extern/ceres/include/ceres/dynamic_cost_function.h @@ -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 @@ namespace ceres { // 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 @@ class CERES_EXPORT DynamicCostFunction : public CostFunction { } // namespace ceres +#include "ceres/internal/reenable_warnings.h" + #endif // CERES_PUBLIC_DYNAMIC_COST_FUNCTION_H_ diff --git a/extern/ceres/include/ceres/dynamic_cost_function_to_functor.h b/extern/ceres/include/ceres/dynamic_cost_function_to_functor.h index 8d174d8ecc2..5b5feaaf58e 100644 --- a/extern/ceres/include/ceres/dynamic_cost_function_to_functor.h +++ b/extern/ceres/include/ceres/dynamic_cost_function_to_functor.h @@ -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,7 +102,7 @@ namespace ceres { // private: // DynamicCostFunctionToFunctor intrinsic_projection_; // }; -class DynamicCostFunctionToFunctor { +class CERES_EXPORT DynamicCostFunctionToFunctor { public: // Takes ownership of cost_function. explicit DynamicCostFunctionToFunctor(CostFunction* cost_function) @@ -109,7 +111,7 @@ class DynamicCostFunctionToFunctor { } 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> @@ -187,4 +189,6 @@ class DynamicCostFunctionToFunctor { } // namespace ceres +#include "ceres/internal/reenable_warnings.h" + #endif // CERES_PUBLIC_DYNAMIC_COST_FUNCTION_TO_FUNCTOR_H_ diff --git a/extern/ceres/include/ceres/dynamic_numeric_diff_cost_function.h b/extern/ceres/include/ceres/dynamic_numeric_diff_cost_function.h index ccc8f66db43..e1892e8ba4a 100644 --- a/extern/ceres/include/ceres/dynamic_numeric_diff_cost_function.h +++ b/extern/ceres/include/ceres/dynamic_numeric_diff_cost_function.h @@ -77,7 +77,7 @@ namespace ceres { // cost_function.AddParameterBlock(10); // cost_function.SetNumResiduals(21); template <typename CostFunctor, NumericDiffMethodType method = CENTRAL> -class DynamicNumericDiffCostFunction : public DynamicCostFunction { +class DynamicNumericDiffCostFunction final : public DynamicCostFunction { public: explicit DynamicNumericDiffCostFunction( const CostFunctor* functor, @@ -85,11 +85,10 @@ class DynamicNumericDiffCostFunction : public DynamicCostFunction { const NumericDiffOptions& options = NumericDiffOptions()) : functor_(functor), ownership_(ownership), options_(options) {} - explicit DynamicNumericDiffCostFunction( - DynamicNumericDiffCostFunction&& other) + DynamicNumericDiffCostFunction(DynamicNumericDiffCostFunction&& other) : functor_(std::move(other.functor_)), ownership_(other.ownership_) {} - virtual ~DynamicNumericDiffCostFunction() { + ~DynamicNumericDiffCostFunction() override { if (ownership_ != TAKE_OWNERSHIP) { functor_.release(); } @@ -111,7 +110,7 @@ class DynamicNumericDiffCostFunction : public DynamicCostFunction { const bool status = internal::VariadicEvaluate<internal::DynamicParameterDims>( *functor_.get(), parameters, residuals); - if (jacobians == NULL || !status) { + if (jacobians == nullptr || !status) { return status; } @@ -133,7 +132,7 @@ class DynamicNumericDiffCostFunction : public DynamicCostFunction { } for (size_t block = 0; block < block_sizes.size(); ++block) { - if (jacobians[block] != NULL && + if (jacobians[block] != nullptr && !NumericDiff<CostFunctor, method, ceres::DYNAMIC, diff --git a/extern/ceres/include/ceres/evaluation_callback.h b/extern/ceres/include/ceres/evaluation_callback.h index b9f5bbb5194..495d565047a 100644 --- a/extern/ceres/include/ceres/evaluation_callback.h +++ b/extern/ceres/include/ceres/evaluation_callback.h @@ -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,7 +62,7 @@ namespace ceres { // 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 diff --git a/extern/ceres/include/ceres/first_order_function.h b/extern/ceres/include/ceres/first_order_function.h index 1420153b2aa..d718b6679ce 100644 --- a/extern/ceres/include/ceres/first_order_function.h +++ b/extern/ceres/include/ceres/first_order_function.h @@ -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 @@ namespace ceres { // 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/extern/ceres/include/ceres/gradient_checker.h b/extern/ceres/include/ceres/gradient_checker.h index b79cf86b314..178fa2b0dd2 100644 --- a/extern/ceres/include/ceres/gradient_checker.h +++ b/extern/ceres/include/ceres/gradient_checker.h @@ -40,9 +40,12 @@ #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 { @@ -65,19 +68,42 @@ namespace ceres { // 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 + // This constructor will not take ownership of the cost function or local // parameterizations. // // 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 + // + // local_parameterizations: A vector of local parameterizations, one for each + // parameter block. May be nullptr or contain nullptrs to indicate that the // respective parameter does not have a local parameterization. + // // options: Options to use for numerical differentiation. + // + // NOTE: This constructor is deprecated and will be removed in the next public + // release of Ceres Solver. Please transition to using the Manifold based + // version. + CERES_DEPRECATED_WITH_MSG( + "Local Parameterizations are deprecated. Use the constructor that uses " + "Manifolds instead.") GradientChecker( const CostFunction* function, const std::vector<const LocalParameterization*>* local_parameterizations, const NumericDiffOptions& options); + // This will not take ownership of the cost function or manifolds. + // + // function: The cost function to probe. + // + // 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 Manifold*>* manifolds, + const NumericDiffOptions& options); + ~GradientChecker(); + // Contains results from a call to Probe for later inspection. struct CERES_EXPORT ProbeResults { // The return value of the cost function. @@ -87,11 +113,11 @@ class CERES_EXPORT GradientChecker { 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 +140,20 @@ class CERES_EXPORT GradientChecker { }; // 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 +166,24 @@ class CERES_EXPORT GradientChecker { GradientChecker(const GradientChecker&) = delete; void operator=(const GradientChecker&) = delete; - std::vector<const LocalParameterization*> local_parameterizations_; + // This bool is used to determine whether the constructor with the + // LocalParameterizations is called or the one with Manifolds is called. If + // the former, then the vector of manifolds is a vector of ManifoldAdapter + // objects which we own and should be deleted. If the latter then they are + // real Manifold objects owned by the caller and will not be deleted. + // + // This bool is only needed during the LocalParameterization to Manifold + // transition, once this transition is complete the LocalParameterization + // based constructor and this bool will be removed. + const bool delete_manifolds_ = false; + + 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/extern/ceres/include/ceres/gradient_problem.h b/extern/ceres/include/ceres/gradient_problem.h index 49d605ea2d6..b6a8b867421 100644 --- a/extern/ceres/include/ceres/gradient_problem.h +++ b/extern/ceres/include/ceres/gradient_problem.h @@ -34,8 +34,10 @@ #include <memory> #include "ceres/first_order_function.h" -#include "ceres/internal/port.h" +#include "ceres/internal/disable_warnings.h" +#include "ceres/internal/export.h" #include "ceres/local_parameterization.h" +#include "ceres/manifold.h" namespace ceres { @@ -43,23 +45,22 @@ class FirstOrderFunction; // Instances of GradientProblem represent general non-linear // optimization problems that must be solved using just the value of -// the objective function and its gradient. Unlike the Problem class, -// which can only be used to model non-linear least squares problems, -// instances of GradientProblem not restricted in the form of the -// objective function. +// 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 +79,7 @@ class FirstOrderFunction; // 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); // } @@ -89,28 +90,96 @@ class FirstOrderFunction; // }; // // ceres::GradientProblem problem(new Rosenbrock()); +// +// NOTE: We are currently in the process of transitioning from +// LocalParameterization to Manifolds in the Ceres API. During this period, +// GradientProblem will support using both Manifold and LocalParameterization +// objects interchangably. For methods in the API affected by this change, see +// their documentation below. class CERES_EXPORT GradientProblem { public: // Takes ownership of the function. explicit GradientProblem(FirstOrderFunction* function); // Takes ownership of the function and the parameterization. + // + // NOTE: This constructor is deprecated and will be removed in the next public + // release of Ceres Solver. Please move to using the Manifold based + // constructor. + CERES_DEPRECATED_WITH_MSG( + "LocalParameterizations are deprecated. Please use the constructor that " + "uses Manifold instead.") 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). + // + // During the transition from LocalParameterization to Manifold, this method + // reports the LocalSize of the LocalParameterization or the TangentSize of + // the Manifold object associated with this problem. + int NumTangentParameters() const; + + // Dimension of the manifold (and its tangent space). + // + // NOTE: This method is deprecated and will be removed in the next public + // release of Ceres Solver. Please move to using NumTangentParameters() + // instead. + int NumLocalParameters() const { return NumTangentParameters(); } // 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(); } + + // NOTE: During the transition from LocalParameterization to Manifold we need + // to support both The LocalParameterization and Manifold based constructors. + // + // When the user uses the LocalParameterization, internally the solver will + // wrap it in a ManifoldAdapter object and return it when manifold or + // mutable_manifold are called. + // + // As a result this method will return a non-nullptr result if a Manifold or a + // LocalParameterization was used when constructing the GradientProblem. + const Manifold* manifold() const { return manifold_.get(); } + Manifold* mutable_manifold() { return manifold_.get(); } + + // If the problem is constructed without a LocalParameterization or with a + // Manifold this method will return a nullptr. + // + // NOTE: This method is deprecated and will be removed in the next public + // release of Ceres Solver. + CERES_DEPRECATED_WITH_MSG("Use Manifolds instead.") + const LocalParameterization* parameterization() const { + return parameterization_.get(); + } + + // If the problem is constructed without a LocalParameterization or with a + // Manifold this method will return a nullptr. + // + // NOTE: This method is deprecated and will be removed in the next public + // release of Ceres Solver. + CERES_DEPRECATED_WITH_MSG("Use Manifolds instead.") + LocalParameterization* mutable_parameterization() { + return parameterization_.get(); + } + private: std::unique_ptr<FirstOrderFunction> function_; + CERES_DEPRECATED_WITH_MSG("") 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/extern/ceres/include/ceres/gradient_problem_solver.h b/extern/ceres/include/ceres/gradient_problem_solver.h index 9fab62e6d94..b6290c80c28 100644 --- a/extern/ceres/include/ceres/gradient_problem_solver.h +++ b/extern/ceres/include/ceres/gradient_problem_solver.h @@ -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,8 +306,12 @@ class CERES_EXPORT GradientProblemSolver { int num_parameters = -1; // Dimension of the tangent space of the problem. + CERES_DEPRECATED_WITH_MSG("Use num_tangent_parameters.") int num_local_parameters = -1; + // Dimension of the tangent space of the problem. + int num_tangent_parameters = -1; + // Type of line search direction used. LineSearchDirectionType line_search_direction_type = LBFGS; diff --git a/extern/ceres/include/ceres/internal/array_selector.h b/extern/ceres/include/ceres/internal/array_selector.h index 841797f4c69..b4db012f00b 100644 --- a/extern/ceres/include/ceres/internal/array_selector.h +++ b/extern/ceres/include/ceres/internal/array_selector.h @@ -73,20 +73,22 @@ struct ArraySelector<T, 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 diff --git a/extern/ceres/include/ceres/internal/autodiff.h b/extern/ceres/include/ceres/internal/autodiff.h index 9d7de758508..c796618cd2d 100644 --- a/extern/ceres/include/ceres/internal/autodiff.h +++ b/extern/ceres/include/ceres/internal/autodiff.h @@ -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" @@ -198,7 +197,7 @@ struct Make1stOrderPerturbation { 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 +310,7 @@ inline bool AutoDifferentiate(const Functor& functor, 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) { diff --git a/extern/ceres/include/ceres/internal/eigen.h b/extern/ceres/include/ceres/internal/eigen.h index b6d0b7f610c..111cc7a07bb 100644 --- a/extern/ceres/include/ceres/internal/eigen.h +++ b/extern/ceres/include/ceres/internal/eigen.h @@ -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/extern/ceres/include/ceres/internal/householder_vector.h b/extern/ceres/include/ceres/internal/householder_vector.h index 55f68e526a0..7700208be22 100644 --- a/extern/ceres/include/ceres/internal/householder_vector.h +++ b/extern/ceres/include/ceres/internal/householder_vector.h @@ -82,6 +82,14 @@ void ComputeHouseholderVector(const XVectorType& x, v->head(v->rows() - 1) /= v_pivot; } +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 internal } // namespace ceres diff --git a/extern/ceres/include/ceres/internal/integer_sequence_algorithm.h b/extern/ceres/include/ceres/internal/integer_sequence_algorithm.h index 8c0f3bc8ac4..777c119a77f 100644 --- a/extern/ceres/include/ceres/internal/integer_sequence_algorithm.h +++ b/extern/ceres/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 2022 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,6 +38,8 @@ #include <utility> +#include "ceres/jet_fwd.h" + namespace ceres { namespace internal { @@ -164,6 +167,124 @@ class ExclusiveScanT { template <typename Seq> using ExclusiveScan = typename ExclusiveScanT<Seq>::Type; +// 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; + +// Determines whether the values of an integer sequence are all the same. +// +// The integer sequence must contain at least one value. The predicate is +// undefined for empty sequences. The evaluation result of the predicate for a +// sequence containing only one value is defined to be true. +template <typename... Sequence> +struct AreAllEqual; + +// The predicate result for a sequence containing one element is defined to be +// true. +template <typename T, T Value> +struct AreAllEqual<std::integer_sequence<T, Value>> : std::true_type {}; + +// Recursion end. +template <typename T, T Value1, T Value2> +struct AreAllEqual<std::integer_sequence<T, Value1, Value2>> + : std::integral_constant<bool, Value1 == Value2> {}; + +// Recursion for sequences containing at least two elements. +template <typename T, T Value1, T Value2, T... Values> +// clang-format off +struct AreAllEqual<std::integer_sequence<T, Value1, Value2, Values...> > + : std::integral_constant +< + bool, + AreAllEqual<std::integer_sequence<T, Value1, Value2> >::value && + AreAllEqual<std::integer_sequence<T, Value2, Values...> >::value +> +// clang-format on +{}; + +// Convenience variable template for AreAllEqual. +template <class Sequence> +constexpr bool AreAllEqual_v = AreAllEqual<Sequence>::value; + +// 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...>> + : AreAllEqual<std::integer_sequence<T, HeadValue, Values...>> {}; + +// Convenience variable template for IsEmptyOrAreAllEqual. +template <class Sequence> +constexpr bool IsEmptyOrAreAllEqual_v = IsEmptyOrAreAllEqual<Sequence>::value; + } // namespace internal } // namespace ceres diff --git a/extern/ceres/include/ceres/internal/jet_traits.h b/extern/ceres/include/ceres/internal/jet_traits.h new file mode 100644 index 00000000000..2a38c05b7da --- /dev/null +++ b/extern/ceres/include/ceres/internal/jet_traits.h @@ -0,0 +1,223 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2022 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 T is a Jet. +template <typename T, typename E = void> +struct IsJet : std::false_type {}; + +template <typename T, int N> +struct IsJet<Jet<T, N>> : std::true_type {}; + +// Convenience variable template for IsJet. +template <typename T> +constexpr bool IsJet_v = IsJet<T>::value; + +// 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> +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. +// +// The predicate is defined only for template packs containing at least two +// types. +template <typename T1, typename T2, typename... Types> +// clang-format off +struct AreAllSame : std::integral_constant +< + bool, + AreAllSame<T1, T2>::value && + AreAllSame<T2, Types...>::value +> +// clang-format on +{}; + +// AreAllSame pairwise test. +template <typename T1, typename T2> +struct AreAllSame<T1, T2> : std::is_same<T1, T2> {}; + +// Convenience variable template for AreAllSame. +template <typename... Types> +constexpr bool AreAllSame_v = AreAllSame<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> +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> +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> +constexpr bool PromotableJetOperands_v = PromotableJetOperands<Types...>::value; + +} // namespace ceres + +#endif // CERES_PUBLIC_INTERNAL_JET_TRAITS_H_ diff --git a/extern/ceres/include/ceres/internal/numeric_diff.h b/extern/ceres/include/ceres/internal/numeric_diff.h index ff7a2c345e4..351845c05fb 100644 --- a/extern/ceres/include/ceres/internal/numeric_diff.h +++ b/extern/ceres/include/ceres/internal/numeric_diff.h @@ -86,18 +86,18 @@ struct NumericDiff { (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 +121,7 @@ struct NumericDiff { // 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 +132,7 @@ struct NumericDiff { 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 +184,8 @@ struct NumericDiff { 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 +260,10 @@ struct NumericDiff { 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 +296,7 @@ struct NumericDiff { // 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 +342,7 @@ struct NumericDiff { 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)) diff --git a/extern/ceres/include/ceres/internal/port.h b/extern/ceres/include/ceres/internal/port.h index 040a1efba02..4275b0e15c3 100644 --- a/extern/ceres/include/ceres/internal/port.h +++ b/extern/ceres/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 2022 Google Inc. All rights reserved. // http://ceres-solver.org/ // // Redistribution and use in source and binary forms, with or without @@ -31,80 +31,58 @@ #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" - -#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 -#endif +// Indicates whether C++17 is currently active +#ifndef CERES_HAS_CPP17 +#if __cplusplus >= 201703L || (defined(_MSVC_LANG) && _MSVC_LANG >= 201703L) +#define CERES_HAS_CPP17 +#endif // __cplusplus >= 201703L || (defined(_MSVC_LANG) && _MSVC_LANG >= + // 201703L) +#endif // !defined(CERES_HAS_CPP17) + +// 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 #endif // CERES_PUBLIC_INTERNAL_PORT_H_ diff --git a/extern/ceres/include/ceres/internal/sphere_manifold_functions.h b/extern/ceres/include/ceres/internal/sphere_manifold_functions.h new file mode 100644 index 00000000000..5be3321a579 --- /dev/null +++ b/extern/ceres/include/ceres/internal/sphere_manifold_functions.h @@ -0,0 +1,162 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2022 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/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 { +namespace internal { + +template <typename VT, typename XT, typename DeltaT, typename XPlusDeltaT> +inline void ComputeSphereManifoldPlus(const VT& v, + double beta, + const XT& x, + const DeltaT& delta, + 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 norm_delta_div_2 = 0.5 * norm_delta; + const double sin_delta_by_delta = + std::sin(norm_delta_div_2) / norm_delta_div_2; + + Eigen::Matrix<double, AmbientDim, 1> y(v.size()); + y << 0.5 * sin_delta_by_delta * delta, std::cos(norm_delta_div_2); + + // 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 = 0.5 * 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) = -0.5 * beta * v(i) * v; + (*jacobian)(i, i) += 0.5; + } + (*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 tanget_size = v.size() - 1; + + const AmbientVector hy = ApplyHouseholderVector(y, v, beta) / x.norm(); + + // Calculate y - x. See B.2 p.25 equation (108). + double y_last = hy[tanget_size]; + double hy_norm = hy.template head<TangentSpaceDim>(tanget_size).norm(); + if (hy_norm == 0.0) { + y_minus_x->setZero(); + } else { + *y_minus_x = 2.0 * std::atan2(hy_norm, y_last) / hy_norm * + hy.template head<TangentSpaceDim>(tanget_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 = 2.0 * H.leftCols(size_ - 1) where H is the + // Householder matrix (H = I - beta * v * v'). + for (int i = 0; i < tangent_size; ++i) { + (*jacobian).row(i) = -2.0 * beta * v(i) * v; + (*jacobian)(i, i) += 2.0; + } + (*jacobian) /= x.norm(); +} + +} // namespace internal +} // namespace ceres + +#endif diff --git a/extern/ceres/include/ceres/internal/variadic_evaluate.h b/extern/ceres/include/ceres/internal/variadic_evaluate.h index 47ff6b18fa0..b8408237cc3 100644 --- a/extern/ceres/include/ceres/internal/variadic_evaluate.h +++ b/extern/ceres/include/ceres/internal/variadic_evaluate.h @@ -33,8 +33,7 @@ #ifndef CERES_PUBLIC_INTERNAL_VARIADIC_EVALUATE_H_ #define CERES_PUBLIC_INTERNAL_VARIADIC_EVALUATE_H_ -#include <stddef.h> - +#include <cstddef> #include <type_traits> #include <utility> diff --git a/extern/ceres/include/ceres/iteration_callback.h b/extern/ceres/include/ceres/iteration_callback.h index 4507fdf748c..3d7e8e94f30 100644 --- a/extern/ceres/include/ceres/iteration_callback.h +++ b/extern/ceres/include/ceres/iteration_callback.h @@ -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 @@ struct CERES_EXPORT IterationSummary { // 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 @@ struct CERES_EXPORT IterationSummary { // class CERES_EXPORT IterationCallback { public: - virtual ~IterationCallback() {} + virtual ~IterationCallback(); virtual CallbackReturnType operator()(const IterationSummary& summary) = 0; }; diff --git a/extern/ceres/include/ceres/jet.h b/extern/ceres/include/ceres/jet.h index da49f32019f..fba1e2ab6e0 100644 --- a/extern/ceres/include/ceres/jet.h +++ b/extern/ceres/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 2022 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 @@ inline Jet<T, N> operator/(const Jet<T, N>& f, T s) { return Jet<T, N>(f.a * s_inverse, f.v * s_inverse); } -// Binary comparison operators for both scalars and jets. -#define CERES_DEFINE_JET_COMPARISON_OPERATOR(op) \ - template <typename T, int N> \ - inline bool operator op(const Jet<T, N>& f, const Jet<T, N>& g) { \ - return f.a op g.a; \ - } \ - template <typename T, int N> \ - inline bool operator op(const T& s, const Jet<T, N>& g) { \ - return s op g.a; \ - } \ - template <typename T, int N> \ - inline bool operator op(const Jet<T, N>& f, const T& s) { \ - return f.a op s; \ +// 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,138 @@ using std::atan; using std::atan2; using std::cbrt; using std::ceil; +using std::copysign; using std::cos; using std::cosh; 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 +568,21 @@ inline Jet<T, N> log(const Jet<T, N>& f) { 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 +590,14 @@ inline Jet<T, N> exp(const Jet<T, N>& f) { 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,29 +724,101 @@ inline Jet<T, N> hypot(const Jet<T, N>& x, const Jet<T, N>& y) { return Jet<T, N>(tmp, x.a / tmp * x.v + y.a / tmp * y.v); } +#ifdef CERES_HAS_CPP17 +// 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); } +#endif // defined(CERES_HAS_CPP17) +// 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); +} + +// Returns the larger of the two arguments. 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& f, const Rhs& g) { + using J = std::common_type_t<Lhs, Rhs>; + return (isnan(g) || isgreater(f, g)) ? J{f} : J{g}; +} + +// Returns the smaller of the two arguments. 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& f, const Rhs& g) { + using J = std::common_type_t<Lhs, Rhs>; + return (isnan(f) || isless(g, f)) ? J{g} : J{f}; +} + +// 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 analyticaly +// 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 sytems 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))))); } // Bessel functions of the first kind with integer order equal to 0, 1, n. @@ -648,80 +879,210 @@ inline Jet<T, N> BesselJn(int n, const Jet<T, N>& f) { T(0.5) * (BesselJn(n - 1, f.a) - BesselJn(n + 1, f.a)) * f.v); } -// 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. - -// The jet is finite if all parts of the jet are finite. +// 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 +1098,22 @@ inline Jet<T, N> atan2(const Jet<T, N>& g, const Jet<T, N>& f) { 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 +1137,14 @@ template <typename T, int N> 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 +1199,21 @@ template <typename T, int N> 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()); } @@ -904,8 +1281,9 @@ struct numeric_limits<ceres::Jet<T, N>> { 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 +1307,9 @@ struct numeric_limits<ceres::Jet<T, N>> { 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 +1321,10 @@ namespace Eigen { // 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); @@ -984,8 +1363,8 @@ struct NumTraits<ceres::Jet<T, N>> { }; }; - 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 +1375,11 @@ struct NumTraits<ceres::Jet<T, N>> { // 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/extern/ceres/internal/ceres/split.h b/extern/ceres/include/ceres/jet_fwd.h index f513023ec69..fbb6286958c 100644 --- a/extern/ceres/internal/ceres/split.h +++ b/extern/ceres/include/ceres/jet_fwd.h @@ -1,5 +1,5 @@ // Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2015 Google Inc. All rights reserved. +// Copyright 2022 Google Inc. All rights reserved. // http://ceres-solver.org/ // // Redistribution and use in source and binary forms, with or without @@ -26,27 +26,19 @@ // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE // POSSIBILITY OF SUCH DAMAGE. // -// Author: keir@google.com (Keir Mierle) - -#ifndef CERES_INTERNAL_SPLIT_H_ -#define CERES_INTERNAL_SPLIT_H_ - -#include <string> -#include <vector> +// Author: sergiu.deitsch@gmail.com (Sergiu Deitsch) +// -#include "ceres/internal/port.h" +#ifndef CERES_PUBLIC_JET_FWD_H_ +#define CERES_PUBLIC_JET_FWD_H_ namespace ceres { -namespace internal { -// Split a string using one or more character delimiters, presented as a -// nul-terminated c string. Append the components to 'result'. If there are -// consecutive delimiters, this function skips over all of them. -void SplitStringUsing(const std::string& full, - const char* delim, - std::vector<std::string>* res); +// Jet forward declaration necessary for the following partial specialization of +// std::common_type and type traits. +template <typename T, int N> +struct Jet; -} // namespace internal } // namespace ceres -#endif // CERES_INTERNAL_SPLIT_H_ +#endif // CERES_PUBLIC_JET_FWD_H_ diff --git a/extern/ceres/include/ceres/line_manifold.h b/extern/ceres/include/ceres/line_manifold.h new file mode 100644 index 00000000000..f8f1b235220 --- /dev/null +++ b/extern/ceres/include/ceres/line_manifold.h @@ -0,0 +1,304 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2022 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)} [0.5 sinc(0.5 |delta_d|) delta_d, cos(0.5 |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)} [0.5 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. + // + // The factor of 0.5 is used to be consistent with the line direction + // update. + AmbientVector y(size_); + y << 0.5 * 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 = + 2.0 * 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/extern/ceres/include/ceres/local_parameterization.h b/extern/ceres/include/ceres/local_parameterization.h index ba7579deca0..5815dd17d15 100644 --- a/extern/ceres/include/ceres/local_parameterization.h +++ b/extern/ceres/include/ceres/local_parameterization.h @@ -37,10 +37,14 @@ #include <vector> #include "ceres/internal/disable_warnings.h" +#include "ceres/internal/export.h" #include "ceres/internal/port.h" namespace ceres { +// WARNING: LocalParameterizations are deprecated. They will be removed from +// Ceres Solver in version 2.2.0. Please use Manifolds instead. + // Purpose: Sometimes parameter blocks x can overparameterize a problem // // min f(x) @@ -111,7 +115,10 @@ namespace ceres { // // 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 { +class CERES_DEPRECATED_WITH_MSG( + "LocalParameterizations will be removed from the Ceres Solver API in " + "version 2.2.0. Use Manifolds instead.") + CERES_EXPORT LocalParameterization { public: virtual ~LocalParameterization(); @@ -120,6 +127,7 @@ class CERES_EXPORT LocalParameterization { // 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; @@ -152,10 +160,10 @@ class CERES_EXPORT LocalParameterization { // Some basic parameterizations // Identity Parameterization: Plus(x, delta) = x + delta -class CERES_EXPORT IdentityParameterization : public LocalParameterization { +class CERES_DEPRECATED_WITH_MSG("Use EuclideanManifold instead.") + 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; @@ -172,11 +180,11 @@ class CERES_EXPORT IdentityParameterization : public LocalParameterization { }; // Hold a subset of the parameters inside a parameter block constant. -class CERES_EXPORT SubsetParameterization : public LocalParameterization { +class CERES_DEPRECATED_WITH_MSG("Use SubsetManifold instead.") + 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; @@ -199,9 +207,9 @@ class CERES_EXPORT SubsetParameterization : public LocalParameterization { // 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 { +class CERES_DEPRECATED_WITH_MSG("Use QuaternionManifold instead.") + CERES_EXPORT QuaternionParameterization : public LocalParameterization { public: - virtual ~QuaternionParameterization() {} bool Plus(const double* x, const double* delta, double* x_plus_delta) const override; @@ -221,10 +229,10 @@ class CERES_EXPORT QuaternionParameterization : public LocalParameterization { // // Plus(x, delta) = [sin(|delta|) delta / |delta|, cos(|delta|)] * x // with * being the quaternion multiplication operator. -class CERES_EXPORT EigenQuaternionParameterization +class CERES_DEPRECATED_WITH_MSG("Use EigenQuaternionManifold instead.") + CERES_EXPORT EigenQuaternionParameterization : public ceres::LocalParameterization { public: - virtual ~EigenQuaternionParameterization() {} bool Plus(const double* x, const double* delta, double* x_plus_delta) const override; @@ -234,23 +242,23 @@ class CERES_EXPORT EigenQuaternionParameterization }; // 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. +// used in Structure from 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 { +class CERES_DEPRECATED_WITH_MSG("Use SphereManifold instead.") 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; @@ -276,7 +284,8 @@ class CERES_EXPORT HomogeneousVectorParameterization // manifold (see https://en.wikipedia.org/wiki/Affine_Grassmannian_(manifold)) // for the case Graff_1(R^n). template <int AmbientSpaceDimension> -class LineParameterization : public LocalParameterization { +class CERES_DEPRECATED_WITH_MSG("Use LineManifold instead.") + LineParameterization : public LocalParameterization { public: static_assert(AmbientSpaceDimension >= 2, "The ambient space must be at least 2"); @@ -302,21 +311,19 @@ class LineParameterization : public LocalParameterization { // // is the local parameterization for a rigid transformation, where the // rotation is represented using a quaternion. -class CERES_EXPORT ProductParameterization : public LocalParameterization { +// +class CERES_DEPRECATED_WITH_MSG("Use ProductManifold instead.") + 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} { + explicit ProductParameterization(LocalParams*... local_params) + : local_params_(sizeof...(LocalParams)) { constexpr int kNumLocalParams = sizeof...(LocalParams); static_assert(kNumLocalParams >= 2, "At least two local parameterizations must be specified."); @@ -342,22 +349,23 @@ class CERES_EXPORT ProductParameterization : public LocalParameterization { bool Plus(const double* x, const double* delta, double* x_plus_delta) const override; - bool ComputeJacobian(const double* x, - double* jacobian) 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_; + int local_size_{0}; + int global_size_{0}; + int buffer_size_{0}; }; } // namespace ceres // clang-format off #include "ceres/internal/reenable_warnings.h" +// clang-format on + #include "ceres/internal/line_parameterization.h" #endif // CERES_PUBLIC_LOCAL_PARAMETERIZATION_H_ diff --git a/extern/ceres/include/ceres/loss_function.h b/extern/ceres/include/ceres/loss_function.h index 7aabf7dfce1..8a5a37ff665 100644 --- a/extern/ceres/include/ceres/loss_function.h +++ b/extern/ceres/include/ceres/loss_function.h @@ -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 @@ namespace ceres { 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 @@ class CERES_EXPORT LossFunction { // // 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 @@ class CERES_EXPORT TrivialLoss : public LossFunction { // // 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 @@ class CERES_EXPORT HuberLoss : public LossFunction { // 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 @@ class CERES_EXPORT SoftLOneLoss : public LossFunction { // 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 @@ class CERES_EXPORT CauchyLoss : public LossFunction { // 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 @@ class CERES_EXPORT ArctanLoss : public LossFunction { // 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 @@ class CERES_EXPORT TolerantLoss : public LossFunction { // 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 @@ class CERES_EXPORT TukeyLoss : public ceres::LossFunction { // 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 @@ class CERES_EXPORT ComposedLoss : public LossFunction { // 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 @@ class CERES_EXPORT ScaledLoss : public LossFunction { 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 @@ class CERES_EXPORT ScaledLoss : public LossFunction { // 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 @@ class CERES_EXPORT ScaledLoss : public LossFunction { // 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 @@ class CERES_EXPORT ScaledLoss : public LossFunction { // // 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 @@ class CERES_EXPORT LossFunctionWrapper : public LossFunction { 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/extern/ceres/include/ceres/manifold.h b/extern/ceres/include/ceres/manifold.h new file mode 100644 index 00000000000..4d6e9fa0f59 --- /dev/null +++ b/extern/ceres/include/ceres/manifold.h @@ -0,0 +1,411 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2022 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/extern/ceres/include/ceres/manifold_test_utils.h b/extern/ceres/include/ceres/manifold_test_utils.h new file mode 100644 index 00000000000..3f9fb21e8f3 --- /dev/null +++ b/extern/ceres/include/ceres/manifold_test_utils.h @@ -0,0 +1,328 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2022 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 for help with testing Manifold objects. +// +// Testing a Manifold has two parts. +// +// 1. Checking that Manifold::Plus is correctly defined. This requires per +// manifold tests. +// +// 2. The other methods of the manifold have mathematical properties that make +// it compatible with Plus, as described in: +// +// "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 +// +// These tests are implemented using generic matchers defined below which can +// all be called by the macro EXPECT_THAT_MANIFOLD_INVARIANTS_HOLD(manifold, x, +// delta, y, tolerance). See manifold_test.cc for example usage. + +// 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 - 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 - 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 - 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; +} + +#define EXPECT_THAT_MANIFOLD_INVARIANTS_HOLD(manifold, x, delta, y, tolerance) \ + Vector zero_tangent = Vector::Zero(manifold.TangentSize()); \ + EXPECT_THAT(manifold, XPlusZeroIsXAt(x, tolerance)); \ + EXPECT_THAT(manifold, XMinusXIsZeroAt(x, tolerance)); \ + EXPECT_THAT(manifold, MinusPlusIsIdentityAt(x, delta, tolerance)); \ + EXPECT_THAT(manifold, MinusPlusIsIdentityAt(x, zero_tangent, tolerance)); \ + EXPECT_THAT(manifold, PlusMinusIsIdentityAt(x, x, tolerance)); \ + EXPECT_THAT(manifold, PlusMinusIsIdentityAt(x, y, tolerance)); \ + EXPECT_THAT(manifold, HasCorrectPlusJacobianAt(x, tolerance)); \ + EXPECT_THAT(manifold, HasCorrectMinusJacobianAt(x, tolerance)); \ + EXPECT_THAT(manifold, MinusPlusJacobianIsIdentityAt(x, tolerance)); \ + EXPECT_THAT(manifold, HasCorrectRightMultiplyByPlusJacobianAt(x, tolerance)); + +} // namespace ceres diff --git a/extern/ceres/include/ceres/normal_prior.h b/extern/ceres/include/ceres/normal_prior.h index 14ab379f4af..c5c7f3e623e 100644 --- a/extern/ceres/include/ceres/normal_prior.h +++ b/extern/ceres/include/ceres/normal_prior.h @@ -57,7 +57,7 @@ namespace ceres { // 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. diff --git a/extern/ceres/include/ceres/numeric_diff_cost_function.h b/extern/ceres/include/ceres/numeric_diff_cost_function.h index cf7971cde79..6ec53175030 100644 --- a/extern/ceres/include/ceres/numeric_diff_cost_function.h +++ b/extern/ceres/include/ceres/numeric_diff_cost_function.h @@ -179,9 +179,10 @@ template <typename CostFunctor, NumericDiffMethodType method = 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, @@ -192,7 +193,7 @@ class NumericDiffCostFunction : public SizedCostFunction<kNumResiduals, Ns...> { } } - explicit NumericDiffCostFunction(NumericDiffCostFunction&& other) + NumericDiffCostFunction(NumericDiffCostFunction&& other) : functor_(std::move(other.functor_)), ownership_(other.ownership_) {} virtual ~NumericDiffCostFunction() { @@ -219,7 +220,7 @@ class NumericDiffCostFunction : public SizedCostFunction<kNumResiduals, Ns...> { return false; } - if (jacobians == NULL) { + if (jacobians == nullptr) { return true; } @@ -246,6 +247,8 @@ class NumericDiffCostFunction : public SizedCostFunction<kNumResiduals, Ns...> { return true; } + const CostFunctor& functor() const { return *functor_; } + private: std::unique_ptr<CostFunctor> functor_; Ownership ownership_; diff --git a/extern/ceres/include/ceres/numeric_diff_first_order_function.h b/extern/ceres/include/ceres/numeric_diff_first_order_function.h new file mode 100644 index 00000000000..f5bb005be58 --- /dev/null +++ b/extern/ceres/include/ceres/numeric_diff_first_order_function.h @@ -0,0 +1,163 @@ +// 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: 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 "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" + +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. +template <typename FirstOrderFunctor, + NumericDiffMethodType method, + int kNumParameters> +class NumericDiffFirstOrderFunction final : public FirstOrderFunction { + public: + explicit NumericDiffFirstOrderFunction( + FirstOrderFunctor* functor, + Ownership ownership = TAKE_OWNERSHIP, + const NumericDiffOptions& options = NumericDiffOptions()) + : functor_(functor), ownership_(ownership), options_(options) { + static_assert(kNumParameters > 0, "kNumParameters must be positive"); + } + + ~NumericDiffFirstOrderFunction() override { + if (ownership_ != TAKE_OWNERSHIP) { + functor_.release(); + } + } + + bool Evaluate(const double* const parameters, + double* cost, + double* gradient) const override { + using ParameterDims = internal::StaticParameterDims<kNumParameters>; + constexpr int kNumResiduals = 1; + + // Get the function value (cost) at the the point to evaluate. + if (!internal::VariadicEvaluate<ParameterDims>( + *functor_, ¶meters, cost)) { + return false; + } + + if (gradient == nullptr) { + return true; + } + + // Create a copy of the parameters which will get mutated. + internal::FixedArray<double, 32> parameters_copy(kNumParameters); + std::copy_n(parameters, kNumParameters, parameters_copy.data()); + double* parameters_ptr = parameters_copy.data(); + internal::EvaluateJacobianForParameterBlocks< + ParameterDims>::template Apply<method, kNumResiduals>(functor_.get(), + cost, + options_, + kNumResiduals, + ¶meters_ptr, + &gradient); + return true; + } + + int NumParameters() const override { return kNumParameters; } + + const FirstOrderFunctor& functor() const { return *functor_; } + + private: + std::unique_ptr<FirstOrderFunctor> functor_; + Ownership ownership_; + NumericDiffOptions options_; +}; + +} // namespace ceres + +#endif // CERES_PUBLIC_NUMERIC_DIFF_FIRST_ORDER_FUNCTION_H_ diff --git a/extern/ceres/include/ceres/numeric_diff_options.h b/extern/ceres/include/ceres/numeric_diff_options.h index 64919ed5ab1..b025b51d938 100644 --- a/extern/ceres/include/ceres/numeric_diff_options.h +++ b/extern/ceres/include/ceres/numeric_diff_options.h @@ -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 @@ struct CERES_EXPORT NumericDiffOptions { } // namespace ceres +#include "ceres/internal/reenable_warnings.h" + #endif // CERES_PUBLIC_NUMERIC_DIFF_OPTIONS_H_ diff --git a/extern/ceres/include/ceres/ordered_groups.h b/extern/ceres/include/ceres/ordered_groups.h index 954663c97e6..c1531cce65f 100644 --- a/extern/ceres/include/ceres/ordered_groups.h +++ b/extern/ceres/include/ceres/ordered_groups.h @@ -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 @@ class OrderedGroups { }; // Typedef for the most commonly used version of OrderedGroups. -typedef OrderedGroups<double*> ParameterBlockOrdering; +using ParameterBlockOrdering = OrderedGroups<double*>; } // namespace ceres diff --git a/extern/ceres/include/ceres/problem.h b/extern/ceres/include/ceres/problem.h index add12ea401d..819fa454b21 100644 --- a/extern/ceres/include/ceres/problem.h +++ b/extern/ceres/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 2021 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" @@ -53,6 +54,7 @@ class CostFunction; class EvaluationCallback; class LossFunction; class LocalParameterization; +class Manifold; class Solver; struct CRSMatrix; @@ -65,7 +67,7 @@ class ResidualBlock; // 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 +80,28 @@ typedef internal::ResidualBlock* ResidualBlockId; // // 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 @@ -119,41 +118,52 @@ typedef internal::ResidualBlock* ResidualBlockId; // problem.AddResidualBlock(new MyBinaryCostFunction(...), nullptr, x2, x3); // // Please see cost_function.h for details of the CostFunction object. +// +// NOTE: We are currently in the process of transitioning from +// LocalParameterization to Manifolds in the Ceres API. During this period, +// Problem will support using both Manifold and LocalParameterization objects +// interchangably. In particular, adding a LocalParameterization to a parameter +// block is the same as adding a Manifold to that parameter block. For methods +// in the API affected by this change, see their documentation below. 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, LocalParameterizations, 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; + CERES_DEPRECATED_WITH_MSG( + "Local Parameterizations are deprecated. Use Manifold and " + "manifold_ownership instead.") 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 +177,23 @@ class CERES_EXPORT Problem { // 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 +204,29 @@ class CERES_EXPORT Problem { ~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. - // - // 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 + // 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 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 +239,8 @@ class CERES_EXPORT Problem { // 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 +266,75 @@ class CERES_EXPORT Problem { 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. + // Add a parameter block with appropriate size and parameterization to the + // problem. It is okay for local_parameterization to be nullptr. + // + // 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::diable_all_safety_checks is set to true). + // + // Repeated calls with the same double pointer and size but different + // LocalParameterization is equivalent to calling + // SetParameterization(local_parameterization), i.e., any previously + // associated LocalParameterization or Manifold object will be replaced with + // the local_parameterization. + // + // NOTE: + // ---- + // + // This method is deprecated and will be removed in the next public + // release of Ceres Solver. Please move to using the Manifold based version of + // AddParameterBlock. + // + // During the transition from LocalParameterization to Manifold, internally + // the LocalParameterization is treated as a Manifold by wrapping it using a + // ManifoldAdapter object. So HasManifold() will return true, GetManifold() + // will return the wrapped object and ParameterBlockTangentSize() will return + // the LocalSize of the LocalParameterization. + CERES_DEPRECATED_WITH_MSG( + "LocalParameterizations are deprecated. Use the version with Manifolds " + "instead.") 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::diable_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 LocalParameterization or Manifold object will be replaced with + // the manifold. + // + // Note: + // ---- + // + // During the transition from LocalParameterization to Manifold, calling + // AddParameterBlock with a Manifold when a LocalParameterization is already + // associated with the parameter block is okay. It is equivalent to calling + // SetManifold(manifold), i.e., any previously associated + // LocalParameterization or Manifold object will be replaced with the + // manifold. + void AddParameterBlock(double* values, int size, Manifold* manifold); + + // Remove a parameter block from the problem. The LocalParameterization or + // 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 +359,109 @@ class CERES_EXPORT Problem { // 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 LocalParameterization or + // 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. + // Set the LocalParameterization for the parameter block. Calling + // SetParameterization with nullptr will clear any previously set + // LocalParameterization or Manifold for the parameter block. + // + // Repeated calls will cause any previously associated LocalParameterization + // or Manifold object to be replaced with the local_parameterization. + // + // The local_parameterization is owned by the Problem by default (See + // Problem::Options to override this behaviour). + // + // It is acceptable to set the same LocalParameterization for multiple + // parameter blocks; the destructor is careful to delete + // LocalParamaterizations only once. + // + // NOTE: + // ---- + // + // This method is deprecated and will be removed in the next public + // release of Ceres Solver. Please move to using the SetManifold instead. + // + // During the transition from LocalParameterization to Manifold, internally + // the LocalParameterization is treated as a Manifold by wrapping it using a + // ManifoldAdapter object. So HasManifold() will return true, GetManifold() + // will return the wrapped object and ParameterBlockTangentSize will return + // the same value of ParameterBlockLocalSize. + CERES_DEPRECATED_WITH_MSG( + "LocalParameterizations are deprecated. Use SetManifold instead.") void SetParameterization(double* values, LocalParameterization* local_parameterization); - // Get the local parameterization object associated with this - // parameter block. If there is no parameterization object - // associated then nullptr is returned. + // Get the LocalParameterization object associated with this parameter block. + // If there is no LocalParameterization associated then nullptr is returned. + // + // NOTE: This method is deprecated and will be removed in the next public + // release of Ceres Solver. Use GetManifold instead. + // + // Note also that if a LocalParameterization is associated with a parameter + // block, HasManifold will return true and GetManifold will return the + // LocalParameterization wrapped in a ManifoldAdapter. + // + // The converse is NOT true, i.e., if a Manifold is associated with a + // parameter block, HasParameterization will return false and + // GetParameterization will return a nullptr. + CERES_DEPRECATED_WITH_MSG( + "LocalParameterizations are deprecated. Use GetManifold " + "instead.") const LocalParameterization* GetParameterization(const double* values) const; + // Returns true if a LocalParameterization is associated with this parameter + // block, false otherwise. + // + // NOTE: This method is deprecated and will be removed in the next public + // release of Ceres Solver. Use HasManifold instead. + // + // Note also that if a Manifold is associated with the parameter block, this + // method will return false. + CERES_DEPRECATED_WITH_MSG( + "LocalParameterizations are deprecated. Use HasManifold instead.") + bool HasParameterization(const double* values) const; + + // Set the Manifold for the parameter block. Calling SetManifold with nullptr + // will clear any previously set LocalParameterization or Manifold for the + // parameter block. + // + // Repeated calls will result in any previously associated + // LocalParameterization or 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 Manifold object associated with this parameter block. + // + // If there is no Manifold Or LocalParameterization object associated then + // nullptr is returned. + // + // NOTE: During the transition from LocalParameterization to Manifold, + // internally the LocalParameterization is treated as a Manifold by wrapping + // it using a ManifoldAdapter object. So calling GetManifold on a parameter + // block with a LocalParameterization associated with it will return the + // LocalParameterization wrapped in a ManifoldAdapter + const Manifold* GetManifold(const double* values) const; + + // Returns true if a Manifold or a LocalParameterization 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 +469,47 @@ class CERES_EXPORT Problem { // 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. + // The dimension of the tangent space of the LocalParameterization or Manifold + // for the parameter block. If there is no LocalParameterization or Manifold + // associated with this parameter block, then ParameterBlockLocalSize = + // ParameterBlockSize. + CERES_DEPRECATED_WITH_MSG( + "LocalParameterizations are deprecated. Use ParameterBlockTangentSize " + "instead.") int ParameterBlockLocalSize(const double* values) const; + // The dimenion of the tangent space of the LocalParameterization or Manifold + // for the parameter block. If there is no LocalParameterization or 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 +528,10 @@ class CERES_EXPORT Problem { // 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 +539,45 @@ class CERES_EXPORT Problem { // 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 +587,8 @@ class CERES_EXPORT Problem { // 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 +596,75 @@ class CERES_EXPORT Problem { // // 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 LocalParameterizations or 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 + // LocalParameterization or 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. + // LocalParameterization/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. - // - // 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). + // 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 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, diff --git a/extern/ceres/include/ceres/product_manifold.h b/extern/ceres/include/ceres/product_manifold.h new file mode 100644 index 00000000000..33f046da24e --- /dev/null +++ b/extern/ceres/include/ceres/product_manifold.h @@ -0,0 +1,328 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2022 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; + T init = 0; + + // TODO Replace by std::exclusive_scan once C++17 is available + for (std::size_t i = 0; i != N; ++i) { + result[i] = init; + init += values[i]; + } + + return result; + } + + // TODO Replace by std::void_t once C++17 is available + template <typename... Types> + struct Void { + using type = void; + }; + + template <typename T, typename E = void> + struct IsDereferenceable : std::false_type {}; + + template <typename T> + struct IsDereferenceable<T, typename Void<decltype(*std::declval<T>())>::type> + : 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_; +}; + +#ifdef CERES_HAS_CPP17 +// 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...>; +#endif + +} // namespace ceres + +#endif // CERES_PUBLIC_PRODUCT_MANIFOLD_H_ diff --git a/extern/ceres/include/ceres/rotation.h b/extern/ceres/include/ceres/rotation.h index 0c82a417a2c..51079901aaf 100644 --- a/extern/ceres/include/ceres/rotation.h +++ b/extern/ceres/include/ceres/rotation.h @@ -521,18 +521,18 @@ inline void UnitQuaternionRotatePoint(const T q[4], 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 } @@ -624,16 +624,16 @@ inline void AngleAxisRotatePoint(const T angle_axis[3], 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 + // 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 // derivatives when evaluated using Jets. diff --git a/extern/ceres/include/ceres/sized_cost_function.h b/extern/ceres/include/ceres/sized_cost_function.h index 8e92f1b796c..d76b5c26b4c 100644 --- a/extern/ceres/include/ceres/sized_cost_function.h +++ b/extern/ceres/include/ceres/sized_cost_function.h @@ -61,8 +61,6 @@ class SizedCostFunction : public CostFunction { *mutable_parameter_block_sizes() = std::vector<int32_t>{Ns...}; } - virtual ~SizedCostFunction() {} - // Subclasses must implement Evaluate(). }; diff --git a/extern/ceres/include/ceres/solver.h b/extern/ceres/include/ceres/solver.h index 61b8dd53eb3..026fc1c0830 100644 --- a/extern/ceres/include/ceres/solver.h +++ b/extern/ceres/include/ceres/solver.h @@ -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" @@ -363,23 +364,23 @@ class CERES_EXPORT Solver { 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. Currently, SUITE_SPARSE and CX_SPARSE + // are the valid choices, depending on whether they are linked into Ceres at + // build time. SparseLinearAlgebraLibraryType sparse_linear_algebra_library_type = #if !defined(CERES_NO_SUITESPARSE) SUITE_SPARSE; @@ -423,7 +424,7 @@ class CERES_EXPORT Solver { // each group, Ceres is free to order the parameter blocks as it // chooses. // - // If NULL, then all parameter blocks are assumed to be in the + // If nullptr, then all parameter blocks are assumed to be in the // same group and the solver is free to decide the best // ordering. // @@ -536,8 +537,9 @@ class CERES_EXPORT Solver { // 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. + // if sparse_linear_algebra_library_type is EIGEN_SPARSE or + // ACCELERATE_SPARSE, and linear_solver_type is SPARSE_NORMAL_CHOLESKY + // or SPARSE_SCHUR. bool use_mixed_precision_solves = false; // Number steps of the iterative refinement process to run when @@ -882,7 +884,7 @@ class CERES_EXPORT Solver { // 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 + // LocalParameterization/Manifold. int num_effective_parameters = -1; // Number of residual blocks in the problem. @@ -903,7 +905,7 @@ class CERES_EXPORT Solver { // 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. + // LocalParameterization/Manifold. int num_effective_parameters_reduced = -1; // Number of residual blocks in the reduced problem. diff --git a/extern/ceres/include/ceres/sphere_manifold.h b/extern/ceres/include/ceres/sphere_manifold.h new file mode 100644 index 00000000000..5d71cbbca9a --- /dev/null +++ b/extern/ceres/include/ceres/sphere_manifold.h @@ -0,0 +1,231 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2022 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; + + using AmbientVector = Eigen::Matrix<double, AmbientSpaceDimension, 1>; + using TangentVector = Eigen::Matrix<double, TangentSpaceDimension, 1>; + using MatrixPlusJacobian = Eigen::Matrix<double, + AmbientSpaceDimension, + TangentSpaceDimension, + Eigen::RowMajor>; + 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/extern/ceres/include/ceres/tiny_solver.h b/extern/ceres/include/ceres/tiny_solver.h index 47db5824dc5..86a655dc07d 100644 --- a/extern/ceres/include/ceres/tiny_solver.h +++ b/extern/ceres/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 2021 Google Inc. All rights reserved. // http://ceres-solver.org/ // // Redistribution and use in source and binary forms, with or without @@ -84,7 +84,8 @@ namespace ceres { // 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 @@ namespace ceres { // 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 @@ class TinySolver { 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 @@ class TinySolver { // 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; } @@ -231,7 +250,7 @@ class TinySolver { 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)); + u * (std::min)((std::max)(jtj_(i, i), min_diagonal), max_diagonal)); jtj_regularized_(i, i) += lm_diagonal_[i] * lm_diagonal_[i]; } @@ -253,10 +272,9 @@ class TinySolver { // 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 +287,12 @@ class TinySolver { // 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 +306,24 @@ class TinySolver { } 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_; @@ -307,7 +339,7 @@ class TinySolver { 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_; + 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 +349,7 @@ class TinySolver { template <typename T> struct enable_if<true, T> { - typedef T type; + using type = T; }; // The number of parameters and residuals are dynamically sized. @@ -355,7 +387,7 @@ class TinySolver { 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/extern/ceres/include/ceres/tiny_solver_autodiff_function.h b/extern/ceres/include/ceres/tiny_solver_autodiff_function.h index b782f549cc1..3e3675ff070 100644 --- a/extern/ceres/include/ceres/tiny_solver_autodiff_function.h +++ b/extern/ceres/include/ceres/tiny_solver_autodiff_function.h @@ -113,12 +113,12 @@ class TinySolverAutoDiffFunction { // 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 @@ class TinySolverAutoDiffFunction { // 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); diff --git a/extern/ceres/include/ceres/tiny_solver_cost_function_adapter.h b/extern/ceres/include/ceres/tiny_solver_cost_function_adapter.h index 18ccb398f90..cc5ca16af5d 100644 --- a/extern/ceres/include/ceres/tiny_solver_cost_function_adapter.h +++ b/extern/ceres/include/ceres/tiny_solver_cost_function_adapter.h @@ -75,7 +75,7 @@ template <int kNumResiduals = Eigen::Dynamic, 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 @@ class TinySolverCostFunctionAdapter { // 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 @@ class TinySolverCostFunctionAdapter { 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/extern/ceres/include/ceres/types.h b/extern/ceres/include/ceres/types.h index 5ee6fdca576..e5224238129 100644 --- a/extern/ceres/include/ceres/types.h +++ b/extern/ceres/include/ceres/types.h @@ -40,7 +40,7 @@ #include <string> #include "ceres/internal/disable_warnings.h" -#include "ceres/internal/port.h" +#include "ceres/internal/export.h" namespace ceres { @@ -186,6 +186,7 @@ enum SparseLinearAlgebraLibraryType { enum DenseLinearAlgebraLibraryType { EIGEN, LAPACK, + CUDA, }; // Logging options diff --git a/extern/ceres/include/ceres/version.h b/extern/ceres/include/ceres/version.h index a76cc1099c5..e0d61972896 100644 --- a/extern/ceres/include/ceres/version.h +++ b/extern/ceres/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 2021 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 1 #define CERES_VERSION_REVISION 0 // Classic CPP stringifcation; the extra level of indirection allows the diff --git a/extern/ceres/internal/ceres/accelerate_sparse.cc b/extern/ceres/internal/ceres/accelerate_sparse.cc index d2b642bf5dc..74adfaf9afc 100644 --- a/extern/ceres/internal/ceres/accelerate_sparse.cc +++ b/extern/ceres/internal/ceres/accelerate_sparse.cc @@ -29,11 +29,12 @@ // Author: alexs.mac@gmail.com (Alex Stewart) // This include must come before any #ifndef check on Ceres compile options. -#include "ceres/internal/port.h" +#include "ceres/internal/config.h" #ifndef CERES_NO_ACCELERATE_SPARSE #include <algorithm> +#include <memory> #include <string> #include <vector> @@ -196,17 +197,17 @@ template <typename Scalar> LinearSolverTerminationType AppleAccelerateCholesky<Scalar>::Factorize( CompressedRowSparseMatrix* lhs, std::string* message) { CHECK_EQ(lhs->storage_type(), StorageType()); - if (lhs == NULL) { - *message = "Failure: Input lhs is NULL."; + if (lhs == nullptr) { + *message = "Failure: Input lhs is nullptr."; return LINEAR_SOLVER_FATAL_ERROR; } typename SparseTypesTrait<Scalar>::SparseMatrix as_lhs = as_.CreateSparseMatrixTransposeView(lhs); if (!symbolic_factor_) { - symbolic_factor_.reset( - new typename SparseTypesTrait<Scalar>::SymbolicFactorization( - as_.AnalyzeCholesky(&as_lhs))); + symbolic_factor_ = std::make_unique< + typename SparseTypesTrait<Scalar>::SymbolicFactorization>( + as_.AnalyzeCholesky(&as_lhs)); if (symbolic_factor_->status != SparseStatusOK) { *message = StringPrintf( "Apple Accelerate Failure : Symbolic factorisation failed: %s", @@ -217,9 +218,9 @@ LinearSolverTerminationType AppleAccelerateCholesky<Scalar>::Factorize( } if (!numeric_factor_) { - numeric_factor_.reset( - new typename SparseTypesTrait<Scalar>::NumericFactorization( - as_.Cholesky(&as_lhs, symbolic_factor_.get()))); + numeric_factor_ = std::make_unique< + typename SparseTypesTrait<Scalar>::NumericFactorization>( + as_.Cholesky(&as_lhs, symbolic_factor_.get())); } else { // Recycle memory from previous numeric factorization. as_.Cholesky(&as_lhs, numeric_factor_.get()); @@ -265,7 +266,7 @@ template <typename Scalar> void AppleAccelerateCholesky<Scalar>::FreeSymbolicFactorization() { if (symbolic_factor_) { SparseCleanup(*symbolic_factor_); - symbolic_factor_.reset(); + symbolic_factor_ = nullptr; } } @@ -273,7 +274,7 @@ template <typename Scalar> void AppleAccelerateCholesky<Scalar>::FreeNumericFactorization() { if (numeric_factor_) { SparseCleanup(*numeric_factor_); - numeric_factor_.reset(); + numeric_factor_ = nullptr; } } diff --git a/extern/ceres/internal/ceres/accelerate_sparse.h b/extern/ceres/internal/ceres/accelerate_sparse.h index e53758dfa15..29d78e8c261 100644 --- a/extern/ceres/internal/ceres/accelerate_sparse.h +++ b/extern/ceres/internal/ceres/accelerate_sparse.h @@ -32,7 +32,7 @@ #define CERES_INTERNAL_ACCELERATE_SPARSE_H_ // This include must come before any #ifndef check on Ceres compile options. -#include "ceres/internal/port.h" +#include "ceres/internal/config.h" #ifndef CERES_NO_ACCELERATE_SPARSE @@ -111,7 +111,7 @@ class AccelerateSparse { // An implementation of SparseCholesky interface using Apple's Accelerate // framework. template <typename Scalar> -class AppleAccelerateCholesky : public SparseCholesky { +class AppleAccelerateCholesky final : public SparseCholesky { public: // Factory static std::unique_ptr<SparseCholesky> Create(OrderingType ordering_type); diff --git a/extern/ceres/internal/ceres/array_utils.cc b/extern/ceres/internal/ceres/array_utils.cc index 6bffd840f4b..113d41c927e 100644 --- a/extern/ceres/internal/ceres/array_utils.cc +++ b/extern/ceres/internal/ceres/array_utils.cc @@ -44,7 +44,7 @@ namespace internal { using std::string; bool IsArrayValid(const int size, const double* x) { - if (x != NULL) { + if (x != nullptr) { for (int i = 0; i < size; ++i) { if (!std::isfinite(x[i]) || (x[i] == kImpossibleValue)) { return false; @@ -55,7 +55,7 @@ bool IsArrayValid(const int size, const double* x) { } int FindInvalidValue(const int size, const double* x) { - if (x == NULL) { + if (x == nullptr) { return size; } @@ -69,7 +69,7 @@ int FindInvalidValue(const int size, const double* x) { } void InvalidateArray(const int size, double* x) { - if (x != NULL) { + if (x != nullptr) { for (int i = 0; i < size; ++i) { x[i] = kImpossibleValue; } @@ -78,7 +78,7 @@ void InvalidateArray(const int size, double* x) { void AppendArrayToString(const int size, const double* x, string* result) { for (int i = 0; i < size; ++i) { - if (x == NULL) { + if (x == nullptr) { StringAppendF(result, "Not Computed "); } else { if (x[i] == kImpossibleValue) { diff --git a/extern/ceres/internal/ceres/array_utils.h b/extern/ceres/internal/ceres/array_utils.h index 68feca5e792..d2fc7914e1b 100644 --- a/extern/ceres/internal/ceres/array_utils.h +++ b/extern/ceres/internal/ceres/array_utils.h @@ -45,29 +45,30 @@ #include <string> -#include "ceres/internal/port.h" +#include "ceres/internal/disable_warnings.h" +#include "ceres/internal/export.h" namespace ceres { namespace internal { // Fill the array x with an impossible value that the user code is // never expected to compute. -CERES_EXPORT_INTERNAL void InvalidateArray(int size, double* x); +CERES_NO_EXPORT void InvalidateArray(int size, double* x); // Check if all the entries of the array x are valid, i.e. all the // values in the array should be finite and none of them should be // equal to the "impossible" value used by InvalidateArray. -CERES_EXPORT_INTERNAL bool IsArrayValid(int size, const double* x); +CERES_NO_EXPORT bool IsArrayValid(int size, const double* x); // If the array contains an invalid value, return the index for it, // otherwise return size. -CERES_EXPORT_INTERNAL int FindInvalidValue(const int size, const double* x); +CERES_NO_EXPORT int FindInvalidValue(const int size, const double* x); // Utility routine to print an array of doubles to a string. If the -// array pointer is NULL, it is treated as an array of zeros. -CERES_EXPORT_INTERNAL void AppendArrayToString(const int size, - const double* x, - std::string* result); +// array pointer is nullptr, it is treated as an array of zeros. +CERES_NO_EXPORT void AppendArrayToString(const int size, + const double* x, + std::string* result); // This routine takes an array of integer values, sorts and uniques // them and then maps each value in the array to its position in the @@ -82,9 +83,11 @@ CERES_EXPORT_INTERNAL void AppendArrayToString(const int size, // gets mapped to // // [1 0 2 3 0 1 3] -CERES_EXPORT_INTERNAL void MapValuesToContiguousRange(int size, int* array); +CERES_NO_EXPORT void MapValuesToContiguousRange(int size, int* array); } // namespace internal } // namespace ceres +#include "ceres/internal/reenable_warnings.h" + #endif // CERES_INTERNAL_ARRAY_UTILS_H_ diff --git a/extern/ceres/internal/ceres/block_evaluate_preparer.cc b/extern/ceres/internal/ceres/block_evaluate_preparer.cc index 7db96d94e0a..56c97b60cc4 100644 --- a/extern/ceres/internal/ceres/block_evaluate_preparer.cc +++ b/extern/ceres/internal/ceres/block_evaluate_preparer.cc @@ -53,7 +53,7 @@ void BlockEvaluatePreparer::Prepare(const ResidualBlock* residual_block, SparseMatrix* jacobian, double** jacobians) { // If the overall jacobian is not available, use the scratch space. - if (jacobian == NULL) { + if (jacobian == nullptr) { scratch_evaluate_preparer_.Prepare( residual_block, residual_block_index, jacobian, jacobians); return; @@ -73,7 +73,7 @@ void BlockEvaluatePreparer::Prepare(const ResidualBlock* residual_block, // parameters. Instead, bump the pointer for active parameters only. jacobian_block_offset++; } else { - jacobians[j] = NULL; + jacobians[j] = nullptr; } } } diff --git a/extern/ceres/internal/ceres/block_evaluate_preparer.h b/extern/ceres/internal/ceres/block_evaluate_preparer.h index 4378689729f..d72e41ba3e4 100644 --- a/extern/ceres/internal/ceres/block_evaluate_preparer.h +++ b/extern/ceres/internal/ceres/block_evaluate_preparer.h @@ -36,6 +36,7 @@ #ifndef CERES_INTERNAL_BLOCK_EVALUATE_PREPARER_H_ #define CERES_INTERNAL_BLOCK_EVALUATE_PREPARER_H_ +#include "ceres/internal/export.h" #include "ceres/scratch_evaluate_preparer.h" namespace ceres { @@ -44,7 +45,7 @@ namespace internal { class ResidualBlock; class SparseMatrix; -class BlockEvaluatePreparer { +class CERES_NO_EXPORT BlockEvaluatePreparer { public: // Using Init() instead of a constructor allows for allocating this structure // with new[]. This is because C++ doesn't allow passing arguments to objects diff --git a/extern/ceres/internal/ceres/block_jacobi_preconditioner.cc b/extern/ceres/internal/ceres/block_jacobi_preconditioner.cc index 6f37aca553c..6e979dea93b 100644 --- a/extern/ceres/internal/ceres/block_jacobi_preconditioner.cc +++ b/extern/ceres/internal/ceres/block_jacobi_preconditioner.cc @@ -1,5 +1,5 @@ // Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2015 Google Inc. All rights reserved. +// Copyright 2022 Google Inc. All rights reserved. // http://ceres-solver.org/ // // Redistribution and use in source and binary forms, with or without @@ -47,10 +47,10 @@ BlockJacobiPreconditioner::BlockJacobiPreconditioner( blocks[i] = bs->cols[i].size; } - m_.reset(new BlockRandomAccessDiagonalMatrix(blocks)); + m_ = std::make_unique<BlockRandomAccessDiagonalMatrix>(blocks); } -BlockJacobiPreconditioner::~BlockJacobiPreconditioner() {} +BlockJacobiPreconditioner::~BlockJacobiPreconditioner() = default; bool BlockJacobiPreconditioner::UpdateImpl(const BlockSparseMatrix& A, const double* D) { @@ -60,21 +60,20 @@ bool BlockJacobiPreconditioner::UpdateImpl(const BlockSparseMatrix& A, for (int i = 0; i < bs->rows.size(); ++i) { const int row_block_size = bs->rows[i].block.size; const std::vector<Cell>& cells = bs->rows[i].cells; - for (int j = 0; j < cells.size(); ++j) { - const int block_id = cells[j].block_id; + for (const auto& cell : cells) { + const int block_id = cell.block_id; const int col_block_size = bs->cols[block_id].size; int r, c, row_stride, col_stride; CellInfo* cell_info = m_->GetCell(block_id, block_id, &r, &c, &row_stride, &col_stride); MatrixRef m(cell_info->values, row_stride, col_stride); - ConstMatrixRef b( - values + cells[j].position, row_block_size, col_block_size); + ConstMatrixRef b(values + cell.position, row_block_size, col_block_size); m.block(r, c, col_block_size, col_block_size) += b.transpose() * b; } } - if (D != NULL) { + if (D != nullptr) { // Add the diagonal. int position = 0; for (int i = 0; i < bs->cols.size(); ++i) { diff --git a/extern/ceres/internal/ceres/block_jacobi_preconditioner.h b/extern/ceres/internal/ceres/block_jacobi_preconditioner.h index 18f749533e0..e0a512a1469 100644 --- a/extern/ceres/internal/ceres/block_jacobi_preconditioner.h +++ b/extern/ceres/internal/ceres/block_jacobi_preconditioner.h @@ -34,7 +34,8 @@ #include <memory> #include "ceres/block_random_access_diagonal_matrix.h" -#include "ceres/internal/port.h" +#include "ceres/internal/disable_warnings.h" +#include "ceres/internal/export.h" #include "ceres/preconditioner.h" namespace ceres { @@ -53,7 +54,7 @@ struct CompressedRowBlockStructure; // update the matrix by running Update(A, D). The values of the matrix A are // inspected to construct the preconditioner. The vector D is applied as the // D^TD diagonal term. -class CERES_EXPORT_INTERNAL BlockJacobiPreconditioner +class CERES_NO_EXPORT BlockJacobiPreconditioner : public BlockSparseMatrixPreconditioner { public: // A must remain valid while the BlockJacobiPreconditioner is. @@ -61,7 +62,7 @@ class CERES_EXPORT_INTERNAL BlockJacobiPreconditioner BlockJacobiPreconditioner(const BlockJacobiPreconditioner&) = delete; void operator=(const BlockJacobiPreconditioner&) = delete; - virtual ~BlockJacobiPreconditioner(); + ~BlockJacobiPreconditioner() override; // Preconditioner interface void RightMultiply(const double* x, double* y) const final; @@ -78,4 +79,6 @@ class CERES_EXPORT_INTERNAL BlockJacobiPreconditioner } // namespace internal } // namespace ceres +#include "ceres/internal/reenable_warnings.h" + #endif // CERES_INTERNAL_BLOCK_JACOBI_PRECONDITIONER_H_ diff --git a/extern/ceres/internal/ceres/block_jacobian_writer.cc b/extern/ceres/internal/ceres/block_jacobian_writer.cc index 17c157b47f9..a70660f860a 100644 --- a/extern/ceres/internal/ceres/block_jacobian_writer.cc +++ b/extern/ceres/internal/ceres/block_jacobian_writer.cc @@ -1,5 +1,5 @@ // Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2015 Google Inc. All rights reserved. +// Copyright 2022 Google Inc. All rights reserved. // http://ceres-solver.org/ // // Redistribution and use in source and binary forms, with or without @@ -30,10 +30,13 @@ #include "ceres/block_jacobian_writer.h" +#include <algorithm> +#include <memory> + #include "ceres/block_evaluate_preparer.h" #include "ceres/block_sparse_matrix.h" #include "ceres/internal/eigen.h" -#include "ceres/internal/port.h" +#include "ceres/internal/export.h" #include "ceres/parameter_block.h" #include "ceres/program.h" #include "ceres/residual_block.h" @@ -66,8 +69,7 @@ void BuildJacobianLayout(const Program& program, // matrix. Also compute the number of jacobian blocks. int f_block_pos = 0; int num_jacobian_blocks = 0; - for (int i = 0; i < residual_blocks.size(); ++i) { - ResidualBlock* residual_block = residual_blocks[i]; + for (auto* residual_block : residual_blocks) { const int num_residuals = residual_block->NumResiduals(); const int num_parameter_blocks = residual_block->NumParameterBlocks(); @@ -78,7 +80,7 @@ void BuildJacobianLayout(const Program& program, // Only count blocks for active parameters. num_jacobian_blocks++; if (parameter_block->index() < num_eliminate_blocks) { - f_block_pos += num_residuals * parameter_block->LocalSize(); + f_block_pos += num_residuals * parameter_block->TangentSize(); } } } @@ -107,7 +109,7 @@ void BuildJacobianLayout(const Program& program, continue; } const int jacobian_block_size = - num_residuals * parameter_block->LocalSize(); + num_residuals * parameter_block->TangentSize(); if (parameter_block_index < num_eliminate_blocks) { *jacobian_pos = e_block_pos; e_block_pos += jacobian_block_size; @@ -136,20 +138,20 @@ BlockJacobianWriter::BlockJacobianWriter(const Evaluator::Options& options, // Create evaluate prepareres that point directly into the final jacobian. This // makes the final Write() a nop. -BlockEvaluatePreparer* BlockJacobianWriter::CreateEvaluatePreparers( - int num_threads) { +std::unique_ptr<BlockEvaluatePreparer[]> +BlockJacobianWriter::CreateEvaluatePreparers(int num_threads) { int max_derivatives_per_residual_block = program_->MaxDerivativesPerResidualBlock(); - BlockEvaluatePreparer* preparers = new BlockEvaluatePreparer[num_threads]; + auto preparers = std::make_unique<BlockEvaluatePreparer[]>(num_threads); for (int i = 0; i < num_threads; i++) { preparers[i].Init(&jacobian_layout_[0], max_derivatives_per_residual_block); } return preparers; } -SparseMatrix* BlockJacobianWriter::CreateJacobian() const { - CompressedRowBlockStructure* bs = new CompressedRowBlockStructure; +std::unique_ptr<SparseMatrix> BlockJacobianWriter::CreateJacobian() const { + auto* bs = new CompressedRowBlockStructure; const vector<ParameterBlock*>& parameter_blocks = program_->parameter_blocks(); @@ -159,7 +161,7 @@ SparseMatrix* BlockJacobianWriter::CreateJacobian() const { for (int i = 0, cursor = 0; i < parameter_blocks.size(); ++i) { CHECK_NE(parameter_blocks[i]->index(), -1); CHECK(!parameter_blocks[i]->IsConstant()); - bs->cols[i].size = parameter_blocks[i]->LocalSize(); + bs->cols[i].size = parameter_blocks[i]->TangentSize(); bs->cols[i].position = cursor; cursor += bs->cols[i].size; } @@ -201,12 +203,10 @@ SparseMatrix* BlockJacobianWriter::CreateJacobian() const { } } - sort(row->cells.begin(), row->cells.end(), CellLessThan); + std::sort(row->cells.begin(), row->cells.end(), CellLessThan); } - BlockSparseMatrix* jacobian = new BlockSparseMatrix(bs); - CHECK(jacobian != nullptr); - return jacobian; + return std::make_unique<BlockSparseMatrix>(bs); } } // namespace internal diff --git a/extern/ceres/internal/ceres/block_jacobian_writer.h b/extern/ceres/internal/ceres/block_jacobian_writer.h index 8054d7b33aa..b2d0aaa3b73 100644 --- a/extern/ceres/internal/ceres/block_jacobian_writer.h +++ b/extern/ceres/internal/ceres/block_jacobian_writer.h @@ -38,10 +38,11 @@ #ifndef CERES_INTERNAL_BLOCK_JACOBIAN_WRITER_H_ #define CERES_INTERNAL_BLOCK_JACOBIAN_WRITER_H_ +#include <memory> #include <vector> #include "ceres/evaluator.h" -#include "ceres/internal/port.h" +#include "ceres/internal/export.h" namespace ceres { namespace internal { @@ -51,7 +52,7 @@ class Program; class SparseMatrix; // TODO(sameeragarwal): This class needs documemtation. -class BlockJacobianWriter { +class CERES_NO_EXPORT BlockJacobianWriter { public: BlockJacobianWriter(const Evaluator::Options& options, Program* program); @@ -59,9 +60,10 @@ class BlockJacobianWriter { // Create evaluate prepareres that point directly into the final jacobian. // This makes the final Write() a nop. - BlockEvaluatePreparer* CreateEvaluatePreparers(int num_threads); + std::unique_ptr<BlockEvaluatePreparer[]> CreateEvaluatePreparers( + int num_threads); - SparseMatrix* CreateJacobian() const; + std::unique_ptr<SparseMatrix> CreateJacobian() const; void Write(int /* residual_id */, int /* residual_offset */, diff --git a/extern/ceres/internal/ceres/block_random_access_dense_matrix.cc b/extern/ceres/internal/ceres/block_random_access_dense_matrix.cc index 386f81eae77..ed172de1d82 100644 --- a/extern/ceres/internal/ceres/block_random_access_dense_matrix.cc +++ b/extern/ceres/internal/ceres/block_random_access_dense_matrix.cc @@ -48,9 +48,9 @@ BlockRandomAccessDenseMatrix::BlockRandomAccessDenseMatrix( num_rows_ += blocks[i]; } - values_.reset(new double[num_rows_ * num_rows_]); + values_ = std::make_unique<double[]>(num_rows_ * num_rows_); - cell_infos_.reset(new CellInfo[num_blocks * num_blocks]); + cell_infos_ = std::make_unique<CellInfo[]>(num_blocks * num_blocks); for (int i = 0; i < num_blocks * num_blocks; ++i) { cell_infos_[i].values = values_.get(); } @@ -60,7 +60,7 @@ BlockRandomAccessDenseMatrix::BlockRandomAccessDenseMatrix( // Assume that the user does not hold any locks on any cell blocks // when they are calling SetZero. -BlockRandomAccessDenseMatrix::~BlockRandomAccessDenseMatrix() {} +BlockRandomAccessDenseMatrix::~BlockRandomAccessDenseMatrix() = default; CellInfo* BlockRandomAccessDenseMatrix::GetCell(const int row_block_id, const int col_block_id, diff --git a/extern/ceres/internal/ceres/block_random_access_dense_matrix.h b/extern/ceres/internal/ceres/block_random_access_dense_matrix.h index 9e555242994..171a6d694b5 100644 --- a/extern/ceres/internal/ceres/block_random_access_dense_matrix.h +++ b/extern/ceres/internal/ceres/block_random_access_dense_matrix.h @@ -35,7 +35,8 @@ #include <vector> #include "ceres/block_random_access_matrix.h" -#include "ceres/internal/port.h" +#include "ceres/internal/disable_warnings.h" +#include "ceres/internal/export.h" namespace ceres { namespace internal { @@ -46,11 +47,11 @@ namespace internal { // num_rows x num_cols. // // This class is NOT thread safe. Since all n^2 cells are stored, -// GetCell never returns NULL for any (row_block_id, col_block_id) +// GetCell never returns nullptr for any (row_block_id, col_block_id) // pair. // // ReturnCell is a nop. -class CERES_EXPORT_INTERNAL BlockRandomAccessDenseMatrix +class CERES_NO_EXPORT BlockRandomAccessDenseMatrix : public BlockRandomAccessMatrix { public: // blocks is a vector of block sizes. The resulting matrix has @@ -61,7 +62,7 @@ class CERES_EXPORT_INTERNAL BlockRandomAccessDenseMatrix // The destructor is not thread safe. It assumes that no one is // modifying any cells when the matrix is being destroyed. - virtual ~BlockRandomAccessDenseMatrix(); + ~BlockRandomAccessDenseMatrix() override; // BlockRandomAccessMatrix interface. CellInfo* GetCell(int row_block_id, @@ -94,4 +95,6 @@ class CERES_EXPORT_INTERNAL BlockRandomAccessDenseMatrix } // namespace internal } // namespace ceres +#include "ceres/internal/reenable_warnings.h" + #endif // CERES_INTERNAL_BLOCK_RANDOM_ACCESS_DENSE_MATRIX_H_ diff --git a/extern/ceres/internal/ceres/block_random_access_diagonal_matrix.cc b/extern/ceres/internal/ceres/block_random_access_diagonal_matrix.cc index 08f6d7f1750..f55f3b30c61 100644 --- a/extern/ceres/internal/ceres/block_random_access_diagonal_matrix.cc +++ b/extern/ceres/internal/ceres/block_random_access_diagonal_matrix.cc @@ -1,5 +1,5 @@ // Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2015 Google Inc. All rights reserved. +// Copyright 2022 Google Inc. All rights reserved. // http://ceres-solver.org/ // // Redistribution and use in source and binary forms, with or without @@ -31,12 +31,13 @@ #include "ceres/block_random_access_diagonal_matrix.h" #include <algorithm> +#include <memory> #include <set> #include <utility> #include <vector> #include "Eigen/Dense" -#include "ceres/internal/port.h" +#include "ceres/internal/export.h" #include "ceres/stl_util.h" #include "ceres/triplet_sparse_matrix.h" #include "ceres/types.h" @@ -57,16 +58,17 @@ BlockRandomAccessDiagonalMatrix::BlockRandomAccessDiagonalMatrix( int num_cols = 0; int num_nonzeros = 0; vector<int> block_positions; - for (int i = 0; i < blocks_.size(); ++i) { + for (int block_size : blocks_) { block_positions.push_back(num_cols); - num_cols += blocks_[i]; - num_nonzeros += blocks_[i] * blocks_[i]; + num_cols += block_size; + num_nonzeros += block_size * block_size; } VLOG(1) << "Matrix Size [" << num_cols << "," << num_cols << "] " << num_nonzeros; - tsm_.reset(new TripletSparseMatrix(num_cols, num_cols, num_nonzeros)); + tsm_ = + std::make_unique<TripletSparseMatrix>(num_cols, num_cols, num_nonzeros); tsm_->set_num_nonzeros(num_nonzeros); int* rows = tsm_->mutable_rows(); int* cols = tsm_->mutable_cols(); @@ -99,7 +101,7 @@ CellInfo* BlockRandomAccessDiagonalMatrix::GetCell(int row_block_id, int* row_stride, int* col_stride) { if (row_block_id != col_block_id) { - return NULL; + return nullptr; } const int stride = blocks_[row_block_id]; @@ -121,8 +123,7 @@ void BlockRandomAccessDiagonalMatrix::SetZero() { void BlockRandomAccessDiagonalMatrix::Invert() { double* values = tsm_->mutable_values(); - for (int i = 0; i < blocks_.size(); ++i) { - const int block_size = blocks_[i]; + for (int block_size : blocks_) { MatrixRef block(values, block_size, block_size); block = block.selfadjointView<Eigen::Upper>().llt().solve( Matrix::Identity(block_size, block_size)); @@ -135,8 +136,7 @@ void BlockRandomAccessDiagonalMatrix::RightMultiply(const double* x, CHECK(x != nullptr); CHECK(y != nullptr); const double* values = tsm_->values(); - for (int i = 0; i < blocks_.size(); ++i) { - const int block_size = blocks_[i]; + for (int block_size : blocks_) { ConstMatrixRef block(values, block_size, block_size); VectorRef(y, block_size).noalias() += block * ConstVectorRef(x, block_size); x += block_size; diff --git a/extern/ceres/internal/ceres/block_random_access_diagonal_matrix.h b/extern/ceres/internal/ceres/block_random_access_diagonal_matrix.h index 3fe7c1e5b22..3d36c378320 100644 --- a/extern/ceres/internal/ceres/block_random_access_diagonal_matrix.h +++ b/extern/ceres/internal/ceres/block_random_access_diagonal_matrix.h @@ -37,7 +37,8 @@ #include <vector> #include "ceres/block_random_access_matrix.h" -#include "ceres/internal/port.h" +#include "ceres/internal/disable_warnings.h" +#include "ceres/internal/export.h" #include "ceres/triplet_sparse_matrix.h" #include "ceres/types.h" @@ -46,7 +47,7 @@ namespace internal { // A thread safe block diagonal matrix implementation of // BlockRandomAccessMatrix. -class CERES_EXPORT_INTERNAL BlockRandomAccessDiagonalMatrix +class CERES_NO_EXPORT BlockRandomAccessDiagonalMatrix : public BlockRandomAccessMatrix { public: // blocks is an array of block sizes. @@ -57,7 +58,7 @@ class CERES_EXPORT_INTERNAL BlockRandomAccessDiagonalMatrix // The destructor is not thread safe. It assumes that no one is // modifying any cells when the matrix is being destroyed. - virtual ~BlockRandomAccessDiagonalMatrix(); + ~BlockRandomAccessDiagonalMatrix() override; // BlockRandomAccessMatrix Interface. CellInfo* GetCell(int row_block_id, @@ -98,4 +99,6 @@ class CERES_EXPORT_INTERNAL BlockRandomAccessDiagonalMatrix } // namespace internal } // namespace ceres +#include "ceres/internal/reenable_warnings.h" + #endif // CERES_INTERNAL_BLOCK_RANDOM_ACCESS_DIAGONAL_MATRIX_H_ diff --git a/extern/ceres/internal/ceres/block_random_access_matrix.cc b/extern/ceres/internal/ceres/block_random_access_matrix.cc index ea88855b59a..8e70c049796 100644 --- a/extern/ceres/internal/ceres/block_random_access_matrix.cc +++ b/extern/ceres/internal/ceres/block_random_access_matrix.cc @@ -33,7 +33,7 @@ namespace ceres { namespace internal { -BlockRandomAccessMatrix::~BlockRandomAccessMatrix() {} +BlockRandomAccessMatrix::~BlockRandomAccessMatrix() = default; } // namespace internal } // namespace ceres diff --git a/extern/ceres/internal/ceres/block_random_access_matrix.h b/extern/ceres/internal/ceres/block_random_access_matrix.h index f190622eafe..48759b79a18 100644 --- a/extern/ceres/internal/ceres/block_random_access_matrix.h +++ b/extern/ceres/internal/ceres/block_random_access_matrix.h @@ -35,7 +35,7 @@ #include <mutex> -#include "ceres/internal/port.h" +#include "ceres/internal/export.h" namespace ceres { namespace internal { @@ -62,7 +62,7 @@ namespace internal { // // There is no requirement that all cells be present, i.e. the matrix // itself can be block sparse. When a cell is not present, the GetCell -// method will return a NULL pointer. +// method will return a nullptr pointer. // // There is no requirement about how the cells are stored beyond that // form a dense submatrix of a larger dense matrix. Like everywhere @@ -77,7 +77,7 @@ namespace internal { // &row, &col, // &row_stride, &col_stride); // -// if (cell != NULL) { +// if (cell != nullptr) { // MatrixRef m(cell->values, row_stride, col_stride); // std::lock_guard<std::mutex> l(&cell->m); // m.block(row, col, row_block_size, col_block_size) = ... @@ -85,21 +85,21 @@ namespace internal { // Structure to carry a pointer to the array containing a cell and the // mutex guarding it. -struct CellInfo { - CellInfo() : values(nullptr) {} +struct CERES_NO_EXPORT CellInfo { + CellInfo() = default; explicit CellInfo(double* values) : values(values) {} - double* values; + double* values{nullptr}; std::mutex m; }; -class CERES_EXPORT_INTERNAL BlockRandomAccessMatrix { +class CERES_NO_EXPORT BlockRandomAccessMatrix { public: virtual ~BlockRandomAccessMatrix(); // If the cell (row_block_id, col_block_id) is present, then return // a CellInfo with a pointer to the dense matrix containing it, - // otherwise return NULL. The dense matrix containing this cell has + // otherwise return nullptr. The dense matrix containing this cell has // size row_stride, col_stride and the cell is located at position // (row, col) within this matrix. // diff --git a/extern/ceres/internal/ceres/block_random_access_sparse_matrix.cc b/extern/ceres/internal/ceres/block_random_access_sparse_matrix.cc index c28b7cef3f4..a026daa5dac 100644 --- a/extern/ceres/internal/ceres/block_random_access_sparse_matrix.cc +++ b/extern/ceres/internal/ceres/block_random_access_sparse_matrix.cc @@ -1,5 +1,5 @@ // Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2015 Google Inc. All rights reserved. +// Copyright 2022 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 <utility> #include <vector> -#include "ceres/internal/port.h" +#include "ceres/internal/export.h" #include "ceres/triplet_sparse_matrix.h" #include "ceres/types.h" #include "glog/logging.h" @@ -58,9 +58,9 @@ BlockRandomAccessSparseMatrix::BlockRandomAccessSparseMatrix( // rows/columns. int num_cols = 0; block_positions_.reserve(blocks_.size()); - for (int i = 0; i < blocks_.size(); ++i) { + for (int block_size : blocks_) { block_positions_.push_back(num_cols); - num_cols += blocks_[i]; + num_cols += block_size; } // Count the number of scalar non-zero entries and build the layout @@ -76,7 +76,8 @@ BlockRandomAccessSparseMatrix::BlockRandomAccessSparseMatrix( VLOG(1) << "Matrix Size [" << num_cols << "," << num_cols << "] " << num_nonzeros; - tsm_.reset(new TripletSparseMatrix(num_cols, num_cols, num_nonzeros)); + tsm_ = + std::make_unique<TripletSparseMatrix>(num_cols, num_cols, num_nonzeros); tsm_->set_num_nonzeros(num_nonzeros); int* rows = tsm_->mutable_rows(); int* cols = tsm_->mutable_cols(); @@ -86,7 +87,7 @@ BlockRandomAccessSparseMatrix::BlockRandomAccessSparseMatrix( for (const auto& block_pair : block_pairs) { const int row_block_size = blocks_[block_pair.first]; const int col_block_size = blocks_[block_pair.second]; - cell_values_.push_back(make_pair(block_pair, values + pos)); + cell_values_.emplace_back(block_pair, values + pos); layout_[IntPairToLong(block_pair.first, block_pair.second)] = new CellInfo(values + pos); pos += row_block_size * col_block_size; @@ -129,7 +130,7 @@ CellInfo* BlockRandomAccessSparseMatrix::GetCell(int row_block_id, const LayoutType::iterator it = layout_.find(IntPairToLong(row_block_id, col_block_id)); if (it == layout_.end()) { - return NULL; + return nullptr; } // Each cell is stored contiguously as its own little dense matrix. diff --git a/extern/ceres/internal/ceres/block_random_access_sparse_matrix.h b/extern/ceres/internal/ceres/block_random_access_sparse_matrix.h index 0e58bbb6b42..b31a2ade843 100644 --- a/extern/ceres/internal/ceres/block_random_access_sparse_matrix.h +++ b/extern/ceres/internal/ceres/block_random_access_sparse_matrix.h @@ -39,7 +39,8 @@ #include <vector> #include "ceres/block_random_access_matrix.h" -#include "ceres/internal/port.h" +#include "ceres/internal/disable_warnings.h" +#include "ceres/internal/export.h" #include "ceres/small_blas.h" #include "ceres/triplet_sparse_matrix.h" #include "ceres/types.h" @@ -51,7 +52,7 @@ namespace internal { // BlockRandomAccessMatrix. Internally a TripletSparseMatrix is used // for doing the actual storage. This class augments this matrix with // an unordered_map that allows random read/write access. -class CERES_EXPORT_INTERNAL BlockRandomAccessSparseMatrix +class CERES_NO_EXPORT BlockRandomAccessSparseMatrix : public BlockRandomAccessMatrix { public: // blocks is an array of block sizes. block_pairs is a set of @@ -65,7 +66,7 @@ class CERES_EXPORT_INTERNAL BlockRandomAccessSparseMatrix // The destructor is not thread safe. It assumes that no one is // modifying any cells when the matrix is being destroyed. - virtual ~BlockRandomAccessSparseMatrix(); + ~BlockRandomAccessSparseMatrix() override; // BlockRandomAccessMatrix Interface. CellInfo* GetCell(int row_block_id, @@ -111,7 +112,7 @@ class CERES_EXPORT_INTERNAL BlockRandomAccessSparseMatrix // A mapping from <row_block_id, col_block_id> to the position in // the values array of tsm_ where the block is stored. - typedef std::unordered_map<long int, CellInfo*> LayoutType; + using LayoutType = std::unordered_map<long, CellInfo*>; LayoutType layout_; // In order traversal of contents of the matrix. This allows us to @@ -127,4 +128,6 @@ class CERES_EXPORT_INTERNAL BlockRandomAccessSparseMatrix } // namespace internal } // namespace ceres +#include "ceres/internal/reenable_warnings.h" + #endif // CERES_INTERNAL_BLOCK_RANDOM_ACCESS_SPARSE_MATRIX_H_ diff --git a/extern/ceres/internal/ceres/block_sparse_matrix.cc b/extern/ceres/internal/ceres/block_sparse_matrix.cc index 5efd2e1ecfb..31ea39daeea 100644 --- a/extern/ceres/internal/ceres/block_sparse_matrix.cc +++ b/extern/ceres/internal/ceres/block_sparse_matrix.cc @@ -1,5 +1,5 @@ // Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2015 Google Inc. All rights reserved. +// Copyright 2022 Google Inc. All rights reserved. // http://ceres-solver.org/ // // Redistribution and use in source and binary forms, with or without @@ -32,6 +32,7 @@ #include <algorithm> #include <cstddef> +#include <memory> #include <vector> #include "ceres/block_structure.h" @@ -46,8 +47,6 @@ namespace internal { using std::vector; -BlockSparseMatrix::~BlockSparseMatrix() {} - BlockSparseMatrix::BlockSparseMatrix( CompressedRowBlockStructure* block_structure) : num_rows_(0), @@ -57,8 +56,8 @@ BlockSparseMatrix::BlockSparseMatrix( CHECK(block_structure_ != nullptr); // Count the number of columns in the matrix. - for (int i = 0; i < block_structure_->cols.size(); ++i) { - num_cols_ += block_structure_->cols[i].size; + for (auto& col : block_structure_->cols) { + num_cols_ += col.size; } // Count the number of non-zero entries and the number of rows in @@ -68,8 +67,8 @@ BlockSparseMatrix::BlockSparseMatrix( num_rows_ += row_block_size; const vector<Cell>& cells = block_structure_->rows[i].cells; - for (int j = 0; j < cells.size(); ++j) { - int col_block_id = cells[j].block_id; + for (const auto& cell : cells) { + int col_block_id = cell.block_id; int col_block_size = block_structure_->cols[col_block_id].size; num_nonzeros_ += col_block_size * row_block_size; } @@ -80,7 +79,7 @@ BlockSparseMatrix::BlockSparseMatrix( CHECK_GE(num_nonzeros_, 0); VLOG(2) << "Allocating values array with " << num_nonzeros_ * sizeof(double) << " bytes."; // NOLINT - values_.reset(new double[num_nonzeros_]); + values_ = std::make_unique<double[]>(num_nonzeros_); max_num_nonzeros_ = num_nonzeros_; CHECK(values_ != nullptr); } @@ -97,12 +96,12 @@ void BlockSparseMatrix::RightMultiply(const double* x, double* y) const { int row_block_pos = block_structure_->rows[i].block.position; int row_block_size = block_structure_->rows[i].block.size; const vector<Cell>& cells = block_structure_->rows[i].cells; - for (int j = 0; j < cells.size(); ++j) { - int col_block_id = cells[j].block_id; + for (const auto& cell : cells) { + int col_block_id = cell.block_id; int col_block_size = block_structure_->cols[col_block_id].size; int col_block_pos = block_structure_->cols[col_block_id].position; MatrixVectorMultiply<Eigen::Dynamic, Eigen::Dynamic, 1>( - values_.get() + cells[j].position, + values_.get() + cell.position, row_block_size, col_block_size, x + col_block_pos, @@ -119,12 +118,12 @@ void BlockSparseMatrix::LeftMultiply(const double* x, double* y) const { int row_block_pos = block_structure_->rows[i].block.position; int row_block_size = block_structure_->rows[i].block.size; const vector<Cell>& cells = block_structure_->rows[i].cells; - for (int j = 0; j < cells.size(); ++j) { - int col_block_id = cells[j].block_id; + for (const auto& cell : cells) { + int col_block_id = cell.block_id; int col_block_size = block_structure_->cols[col_block_id].size; int col_block_pos = block_structure_->cols[col_block_id].position; MatrixTransposeVectorMultiply<Eigen::Dynamic, Eigen::Dynamic, 1>( - values_.get() + cells[j].position, + values_.get() + cell.position, row_block_size, col_block_size, x + row_block_pos, @@ -139,12 +138,12 @@ void BlockSparseMatrix::SquaredColumnNorm(double* x) const { for (int i = 0; i < block_structure_->rows.size(); ++i) { int row_block_size = block_structure_->rows[i].block.size; const vector<Cell>& cells = block_structure_->rows[i].cells; - for (int j = 0; j < cells.size(); ++j) { - int col_block_id = cells[j].block_id; + for (const auto& cell : cells) { + int col_block_id = cell.block_id; int col_block_size = block_structure_->cols[col_block_id].size; int col_block_pos = block_structure_->cols[col_block_id].position; const MatrixRef m( - values_.get() + cells[j].position, row_block_size, col_block_size); + values_.get() + cell.position, row_block_size, col_block_size); VectorRef(x + col_block_pos, col_block_size) += m.colwise().squaredNorm(); } } @@ -156,12 +155,12 @@ void BlockSparseMatrix::ScaleColumns(const double* scale) { for (int i = 0; i < block_structure_->rows.size(); ++i) { int row_block_size = block_structure_->rows[i].block.size; const vector<Cell>& cells = block_structure_->rows[i].cells; - for (int j = 0; j < cells.size(); ++j) { - int col_block_id = cells[j].block_id; + for (const auto& cell : cells) { + int col_block_id = cell.block_id; int col_block_size = block_structure_->cols[col_block_id].size; int col_block_pos = block_structure_->cols[col_block_id].position; MatrixRef m( - values_.get() + cells[j].position, row_block_size, col_block_size); + values_.get() + cell.position, row_block_size, col_block_size); m *= ConstVectorRef(scale + col_block_pos, col_block_size).asDiagonal(); } } @@ -178,11 +177,11 @@ void BlockSparseMatrix::ToDenseMatrix(Matrix* dense_matrix) const { int row_block_pos = block_structure_->rows[i].block.position; int row_block_size = block_structure_->rows[i].block.size; const vector<Cell>& cells = block_structure_->rows[i].cells; - for (int j = 0; j < cells.size(); ++j) { - int col_block_id = cells[j].block_id; + for (const auto& cell : cells) { + int col_block_id = cell.block_id; int col_block_size = block_structure_->cols[col_block_id].size; int col_block_pos = block_structure_->cols[col_block_id].position; - int jac_pos = cells[j].position; + int jac_pos = cell.position; m.block(row_block_pos, col_block_pos, row_block_size, col_block_size) += MatrixRef(values_.get() + jac_pos, row_block_size, col_block_size); } @@ -201,11 +200,11 @@ void BlockSparseMatrix::ToTripletSparseMatrix( int row_block_pos = block_structure_->rows[i].block.position; int row_block_size = block_structure_->rows[i].block.size; const vector<Cell>& cells = block_structure_->rows[i].cells; - for (int j = 0; j < cells.size(); ++j) { - int col_block_id = cells[j].block_id; + for (const auto& cell : cells) { + int col_block_id = cell.block_id; int col_block_size = block_structure_->cols[col_block_id].size; int col_block_pos = block_structure_->cols[col_block_id].position; - int jac_pos = cells[j].position; + int jac_pos = cell.position; for (int r = 0; r < row_block_size; ++r) { for (int c = 0; c < col_block_size; ++c, ++jac_pos) { matrix->mutable_rows()[jac_pos] = row_block_pos + r; @@ -230,11 +229,11 @@ void BlockSparseMatrix::ToTextFile(FILE* file) const { const int row_block_pos = block_structure_->rows[i].block.position; const int row_block_size = block_structure_->rows[i].block.size; const vector<Cell>& cells = block_structure_->rows[i].cells; - for (int j = 0; j < cells.size(); ++j) { - const int col_block_id = cells[j].block_id; + for (const auto& cell : cells) { + const int col_block_id = cell.block_id; const int col_block_size = block_structure_->cols[col_block_id].size; const int col_block_pos = block_structure_->cols[col_block_id].position; - int jac_pos = cells[j].position; + int jac_pos = cell.position; for (int r = 0; r < row_block_size; ++r) { for (int c = 0; c < col_block_size; ++c) { fprintf(file, @@ -248,10 +247,10 @@ void BlockSparseMatrix::ToTextFile(FILE* file) const { } } -BlockSparseMatrix* BlockSparseMatrix::CreateDiagonalMatrix( +std::unique_ptr<BlockSparseMatrix> BlockSparseMatrix::CreateDiagonalMatrix( const double* diagonal, const std::vector<Block>& column_blocks) { // Create the block structure for the diagonal matrix. - CompressedRowBlockStructure* bs = new CompressedRowBlockStructure(); + auto* bs = new CompressedRowBlockStructure(); bs->cols = column_blocks; int position = 0; bs->rows.resize(column_blocks.size(), CompressedRow(1)); @@ -265,13 +264,13 @@ BlockSparseMatrix* BlockSparseMatrix::CreateDiagonalMatrix( } // Create the BlockSparseMatrix with the given block structure. - BlockSparseMatrix* matrix = new BlockSparseMatrix(bs); + auto matrix = std::make_unique<BlockSparseMatrix>(bs); matrix->SetZero(); // Fill the values array of the block sparse matrix. double* values = matrix->mutable_values(); - for (int i = 0; i < column_blocks.size(); ++i) { - const int size = column_blocks[i].size; + for (const auto& column_block : column_blocks) { + const int size = column_block.size; for (int j = 0; j < size; ++j) { // (j + 1) * size is compact way of accessing the (j,j) entry. values[j * (size + 1)] = diagonal[j]; @@ -308,9 +307,10 @@ void BlockSparseMatrix::AppendRows(const BlockSparseMatrix& m) { } if (num_nonzeros_ > max_num_nonzeros_) { - double* new_values = new double[num_nonzeros_]; - std::copy(values_.get(), values_.get() + old_num_nonzeros, new_values); - values_.reset(new_values); + std::unique_ptr<double[]> new_values = + std::make_unique<double[]>(num_nonzeros_); + std::copy_n(values_.get(), old_num_nonzeros, new_values.get()); + values_ = std::move(new_values); max_num_nonzeros_ = num_nonzeros_; } @@ -337,7 +337,7 @@ void BlockSparseMatrix::DeleteRowBlocks(const int delta_row_blocks) { block_structure_->rows.resize(num_row_blocks - delta_row_blocks); } -BlockSparseMatrix* BlockSparseMatrix::CreateRandomMatrix( +std::unique_ptr<BlockSparseMatrix> BlockSparseMatrix::CreateRandomMatrix( const BlockSparseMatrix::RandomMatrixOptions& options) { CHECK_GT(options.num_row_blocks, 0); CHECK_GT(options.min_row_block_size, 0); @@ -346,7 +346,7 @@ BlockSparseMatrix* BlockSparseMatrix::CreateRandomMatrix( CHECK_GT(options.block_density, 0.0); CHECK_LE(options.block_density, 1.0); - CompressedRowBlockStructure* bs = new CompressedRowBlockStructure(); + auto* bs = new CompressedRowBlockStructure(); if (options.col_blocks.empty()) { CHECK_GT(options.num_col_blocks, 0); CHECK_GT(options.min_col_block_size, 0); @@ -360,7 +360,7 @@ BlockSparseMatrix* BlockSparseMatrix::CreateRandomMatrix( const int delta_block_size = Uniform(options.max_col_block_size - options.min_col_block_size); const int col_block_size = options.min_col_block_size + delta_block_size; - bs->cols.push_back(Block(col_block_size, col_block_position)); + bs->cols.emplace_back(col_block_size, col_block_position); col_block_position += col_block_size; } } else { @@ -377,7 +377,7 @@ BlockSparseMatrix* BlockSparseMatrix::CreateRandomMatrix( const int delta_block_size = Uniform(options.max_row_block_size - options.min_row_block_size); const int row_block_size = options.min_row_block_size + delta_block_size; - bs->rows.push_back(CompressedRow()); + bs->rows.emplace_back(); CompressedRow& row = bs->rows.back(); row.block.size = row_block_size; row.block.position = row_block_position; @@ -385,7 +385,7 @@ BlockSparseMatrix* BlockSparseMatrix::CreateRandomMatrix( for (int c = 0; c < bs->cols.size(); ++c) { if (RandDouble() > options.block_density) continue; - row.cells.push_back(Cell()); + row.cells.emplace_back(); Cell& cell = row.cells.back(); cell.block_id = c; cell.position = value_position; @@ -395,7 +395,7 @@ BlockSparseMatrix* BlockSparseMatrix::CreateRandomMatrix( } } - BlockSparseMatrix* matrix = new BlockSparseMatrix(bs); + auto matrix = std::make_unique<BlockSparseMatrix>(bs); double* values = matrix->mutable_values(); for (int i = 0; i < matrix->num_nonzeros(); ++i) { values[i] = RandNormal(); diff --git a/extern/ceres/internal/ceres/block_sparse_matrix.h b/extern/ceres/internal/ceres/block_sparse_matrix.h index e5b3634c3cc..75b0deb59e6 100644 --- a/extern/ceres/internal/ceres/block_sparse_matrix.h +++ b/extern/ceres/internal/ceres/block_sparse_matrix.h @@ -37,8 +37,9 @@ #include <memory> #include "ceres/block_structure.h" +#include "ceres/internal/disable_warnings.h" #include "ceres/internal/eigen.h" -#include "ceres/internal/port.h" +#include "ceres/internal/export.h" #include "ceres/sparse_matrix.h" namespace ceres { @@ -54,7 +55,7 @@ class TripletSparseMatrix; // // internal/ceres/block_structure.h // -class CERES_EXPORT_INTERNAL BlockSparseMatrix : public SparseMatrix { +class CERES_NO_EXPORT BlockSparseMatrix final : public SparseMatrix { public: // Construct a block sparse matrix with a fully initialized // CompressedRowBlockStructure objected. The matrix takes over @@ -68,8 +69,6 @@ class CERES_EXPORT_INTERNAL BlockSparseMatrix : public SparseMatrix { BlockSparseMatrix(const BlockSparseMatrix&) = delete; void operator=(const BlockSparseMatrix&) = delete; - virtual ~BlockSparseMatrix(); - // Implementation of SparseMatrix interface. void SetZero() final; void RightMultiply(const double* x, double* y) const final; @@ -97,7 +96,7 @@ class CERES_EXPORT_INTERNAL BlockSparseMatrix : public SparseMatrix { // Delete the bottom delta_rows_blocks. void DeleteRowBlocks(int delta_row_blocks); - static BlockSparseMatrix* CreateDiagonalMatrix( + static std::unique_ptr<BlockSparseMatrix> CreateDiagonalMatrix( const double* diagonal, const std::vector<Block>& column_blocks); struct RandomMatrixOptions { @@ -122,9 +121,7 @@ class CERES_EXPORT_INTERNAL BlockSparseMatrix : public SparseMatrix { // Create a random BlockSparseMatrix whose entries are normally // distributed and whose structure is determined by // RandomMatrixOptions. - // - // Caller owns the result. - static BlockSparseMatrix* CreateRandomMatrix( + static std::unique_ptr<BlockSparseMatrix> CreateRandomMatrix( const RandomMatrixOptions& options); private: @@ -142,9 +139,9 @@ class CERES_EXPORT_INTERNAL BlockSparseMatrix : public SparseMatrix { // // BlockSparseDataMatrix a struct that carries these two bits of // information -class BlockSparseMatrixData { +class CERES_NO_EXPORT BlockSparseMatrixData { public: - BlockSparseMatrixData(const BlockSparseMatrix& m) + explicit BlockSparseMatrixData(const BlockSparseMatrix& m) : block_structure_(m.block_structure()), values_(m.values()){}; BlockSparseMatrixData(const CompressedRowBlockStructure* block_structure, @@ -164,4 +161,6 @@ class BlockSparseMatrixData { } // namespace internal } // namespace ceres +#include "ceres/internal/reenable_warnings.h" + #endif // CERES_INTERNAL_BLOCK_SPARSE_MATRIX_H_ diff --git a/extern/ceres/internal/ceres/block_structure.h b/extern/ceres/internal/ceres/block_structure.h index d49d7d3f3a4..fe7574c6817 100644 --- a/extern/ceres/internal/ceres/block_structure.h +++ b/extern/ceres/internal/ceres/block_structure.h @@ -41,54 +41,54 @@ #include <cstdint> #include <vector> -#include "ceres/internal/port.h" +#include "ceres/internal/export.h" namespace ceres { namespace internal { -typedef int32_t BlockSize; +using BlockSize = int32_t; -struct Block { - Block() : size(-1), position(-1) {} +struct CERES_NO_EXPORT Block { + Block() = default; Block(int size_, int position_) : size(size_), position(position_) {} - BlockSize size; - int position; // Position along the row/column. + BlockSize size{-1}; + int position{-1}; // Position along the row/column. }; -struct Cell { - Cell() : block_id(-1), position(-1) {} +struct CERES_NO_EXPORT Cell { + Cell() = default; Cell(int block_id_, int position_) : block_id(block_id_), position(position_) {} // Column or row block id as the case maybe. - int block_id; + int block_id{-1}; // Where in the values array of the jacobian is this cell located. - int position; + int position{-1}; }; // Order cell by their block_id; -bool CellLessThan(const Cell& lhs, const Cell& rhs); +CERES_NO_EXPORT bool CellLessThan(const Cell& lhs, const Cell& rhs); -struct CompressedList { - CompressedList() {} +struct CERES_NO_EXPORT CompressedList { + CompressedList() = default; // Construct a CompressedList with the cells containing num_cells // entries. - CompressedList(int num_cells) : cells(num_cells) {} + explicit CompressedList(int num_cells) : cells(num_cells) {} Block block; std::vector<Cell> cells; }; -typedef CompressedList CompressedRow; -typedef CompressedList CompressedColumn; +using CompressedRow = CompressedList; +using CompressedColumn = CompressedList; -struct CompressedRowBlockStructure { +struct CERES_NO_EXPORT CompressedRowBlockStructure { std::vector<Block> cols; std::vector<CompressedRow> rows; }; -struct CompressedColumnBlockStructure { +struct CERES_NO_EXPORT CompressedColumnBlockStructure { std::vector<Block> rows; std::vector<CompressedColumn> cols; }; diff --git a/extern/ceres/internal/ceres/c_api.cc b/extern/ceres/internal/ceres/c_api.cc index 251cde42101..8ea344dd54a 100644 --- a/extern/ceres/internal/ceres/c_api.cc +++ b/extern/ceres/internal/ceres/c_api.cc @@ -1,5 +1,5 @@ // Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2015 Google Inc. All rights reserved. +// Copyright 2022 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 "ceres/c_api.h" #include <iostream> +#include <memory> #include <string> #include <vector> @@ -64,7 +65,7 @@ void ceres_free_problem(ceres_problem_t* problem) { // This cost function wraps a C-level function pointer from the user, to bridge // between C and C++. -class CallbackCostFunction : public ceres::CostFunction { +class CERES_NO_EXPORT CallbackCostFunction final : public ceres::CostFunction { public: CallbackCostFunction(ceres_cost_function_t cost_function, void* user_data, @@ -78,8 +79,6 @@ class CallbackCostFunction : public ceres::CostFunction { } } - virtual ~CallbackCostFunction() {} - bool Evaluate(double const* const* parameters, double* residuals, double** jacobians) const final { @@ -94,7 +93,7 @@ class CallbackCostFunction : public ceres::CostFunction { // This loss function wraps a C-level function pointer from the user, to bridge // between C and C++. -class CallbackLossFunction : public ceres::LossFunction { +class CallbackLossFunction final : public ceres::LossFunction { public: explicit CallbackLossFunction(ceres_loss_function_t loss_function, void* user_data) @@ -146,30 +145,31 @@ ceres_residual_block_id_t* ceres_problem_add_residual_block( int num_parameter_blocks, int* parameter_block_sizes, double** parameters) { - Problem* ceres_problem = reinterpret_cast<Problem*>(problem); - - ceres::CostFunction* callback_cost_function = - new CallbackCostFunction(cost_function, - cost_function_data, - num_residuals, - num_parameter_blocks, - parameter_block_sizes); - - ceres::LossFunction* callback_loss_function = NULL; - if (loss_function != NULL) { - callback_loss_function = - new CallbackLossFunction(loss_function, loss_function_data); + auto* ceres_problem = reinterpret_cast<Problem*>(problem); + + auto callback_cost_function = + std::make_unique<CallbackCostFunction>(cost_function, + cost_function_data, + num_residuals, + num_parameter_blocks, + parameter_block_sizes); + + std::unique_ptr<ceres::LossFunction> callback_loss_function; + if (loss_function != nullptr) { + callback_loss_function = std::make_unique<CallbackLossFunction>( + loss_function, loss_function_data); } std::vector<double*> parameter_blocks(parameters, parameters + num_parameter_blocks); return reinterpret_cast<ceres_residual_block_id_t*>( - ceres_problem->AddResidualBlock( - callback_cost_function, callback_loss_function, parameter_blocks)); + ceres_problem->AddResidualBlock(callback_cost_function.release(), + callback_loss_function.release(), + parameter_blocks)); } void ceres_solve(ceres_problem_t* c_problem) { - Problem* problem = reinterpret_cast<Problem*>(c_problem); + auto* problem = reinterpret_cast<Problem*>(c_problem); // TODO(keir): Obviously, this way of setting options won't scale or last. // Instead, figure out a way to specify some of the options without diff --git a/extern/ceres/internal/ceres/callbacks.cc b/extern/ceres/internal/ceres/callbacks.cc index 0e0df9d91b1..7a4381c293f 100644 --- a/extern/ceres/internal/ceres/callbacks.cc +++ b/extern/ceres/internal/ceres/callbacks.cc @@ -30,6 +30,7 @@ #include "ceres/callbacks.h" +#include <algorithm> #include <iostream> // NO LINT #include "ceres/program.h" @@ -45,7 +46,7 @@ StateUpdatingCallback::StateUpdatingCallback(Program* program, double* parameters) : program_(program), parameters_(parameters) {} -StateUpdatingCallback::~StateUpdatingCallback() {} +StateUpdatingCallback::~StateUpdatingCallback() = default; CallbackReturnType StateUpdatingCallback::operator()( const IterationSummary& summary) { @@ -64,14 +65,12 @@ GradientProblemSolverStateUpdatingCallback:: user_parameters_(user_parameters) {} GradientProblemSolverStateUpdatingCallback:: - ~GradientProblemSolverStateUpdatingCallback() {} + ~GradientProblemSolverStateUpdatingCallback() = default; CallbackReturnType GradientProblemSolverStateUpdatingCallback::operator()( const IterationSummary& summary) { if (summary.step_is_successful) { - std::copy(internal_parameters_, - internal_parameters_ + num_parameters_, - user_parameters_); + std::copy_n(internal_parameters_, num_parameters_, user_parameters_); } return SOLVER_CONTINUE; } @@ -80,44 +79,42 @@ LoggingCallback::LoggingCallback(const MinimizerType minimizer_type, const bool log_to_stdout) : minimizer_type(minimizer_type), log_to_stdout_(log_to_stdout) {} -LoggingCallback::~LoggingCallback() {} +LoggingCallback::~LoggingCallback() = default; CallbackReturnType LoggingCallback::operator()( const IterationSummary& summary) { string output; if (minimizer_type == LINE_SEARCH) { - const char* kReportRowFormat = - "% 4d: f:% 8e d:% 3.2e g:% 3.2e h:% 3.2e " - "s:% 3.2e e:% 3d it:% 3.2e tt:% 3.2e"; - output = StringPrintf(kReportRowFormat, - summary.iteration, - summary.cost, - summary.cost_change, - summary.gradient_max_norm, - summary.step_norm, - summary.step_size, - summary.line_search_function_evaluations, - summary.iteration_time_in_seconds, - summary.cumulative_time_in_seconds); + output = StringPrintf( + "% 4d: f:% 8e d:% 3.2e g:% 3.2e h:% 3.2e s:% 3.2e e:% 3d it:% 3.2e " + "tt:% 3.2e", + summary.iteration, + summary.cost, + summary.cost_change, + summary.gradient_max_norm, + summary.step_norm, + summary.step_size, + summary.line_search_function_evaluations, + summary.iteration_time_in_seconds, + summary.cumulative_time_in_seconds); } else if (minimizer_type == TRUST_REGION) { // clang-format off if (summary.iteration == 0) { output = "iter cost cost_change |gradient| |step| tr_ratio tr_radius ls_iter iter_time total_time\n"; // NOLINT } - const char* kReportRowFormat = - "% 4d % 8e % 3.2e % 3.2e % 3.2e % 3.2e % 3.2e % 4d % 3.2e % 3.2e"; // NOLINT - // clang-format on - output += StringPrintf(kReportRowFormat, - summary.iteration, - summary.cost, - summary.cost_change, - summary.gradient_max_norm, - summary.step_norm, - summary.relative_decrease, - summary.trust_region_radius, - summary.linear_solver_iterations, - summary.iteration_time_in_seconds, - summary.cumulative_time_in_seconds); + output += StringPrintf( + "% 4d % 8e % 3.2e % 3.2e % 3.2e % 3.2e % 3.2e % 4d % 3.2e % 3.2e", // NOLINT + // clang-format on + summary.iteration, + summary.cost, + summary.cost_change, + summary.gradient_max_norm, + summary.step_norm, + summary.relative_decrease, + summary.trust_region_radius, + summary.linear_solver_iterations, + summary.iteration_time_in_seconds, + summary.cumulative_time_in_seconds); } else { LOG(FATAL) << "Unknown minimizer type."; } diff --git a/extern/ceres/internal/ceres/callbacks.h b/extern/ceres/internal/ceres/callbacks.h index 47112b88fd8..3b1d10cfa7f 100644 --- a/extern/ceres/internal/ceres/callbacks.h +++ b/extern/ceres/internal/ceres/callbacks.h @@ -33,7 +33,7 @@ #include <string> -#include "ceres/internal/port.h" +#include "ceres/internal/export.h" #include "ceres/iteration_callback.h" namespace ceres { @@ -43,10 +43,10 @@ class Program; // Callback for updating the externally visible state of parameter // blocks. -class StateUpdatingCallback : public IterationCallback { +class CERES_NO_EXPORT StateUpdatingCallback final : public IterationCallback { public: StateUpdatingCallback(Program* program, double* parameters); - virtual ~StateUpdatingCallback(); + ~StateUpdatingCallback() override; CallbackReturnType operator()(const IterationSummary& summary) final; private: @@ -56,12 +56,13 @@ class StateUpdatingCallback : public IterationCallback { // Callback for updating the externally visible state of the // parameters vector for GradientProblemSolver. -class GradientProblemSolverStateUpdatingCallback : public IterationCallback { +class CERES_NO_EXPORT GradientProblemSolverStateUpdatingCallback final + : public IterationCallback { public: GradientProblemSolverStateUpdatingCallback(int num_parameters, const double* internal_parameters, double* user_parameters); - virtual ~GradientProblemSolverStateUpdatingCallback(); + ~GradientProblemSolverStateUpdatingCallback() override; CallbackReturnType operator()(const IterationSummary& summary) final; private: @@ -72,10 +73,10 @@ class GradientProblemSolverStateUpdatingCallback : public IterationCallback { // Callback for logging the state of the minimizer to STDERR or // STDOUT depending on the user's preferences and logging level. -class LoggingCallback : public IterationCallback { +class CERES_NO_EXPORT LoggingCallback final : public IterationCallback { public: LoggingCallback(MinimizerType minimizer_type, bool log_to_stdout); - virtual ~LoggingCallback(); + ~LoggingCallback() override; CallbackReturnType operator()(const IterationSummary& summary) final; private: diff --git a/extern/ceres/internal/ceres/canonical_views_clustering.cc b/extern/ceres/internal/ceres/canonical_views_clustering.cc index c193735f106..01b8ad38ae0 100644 --- a/extern/ceres/internal/ceres/canonical_views_clustering.cc +++ b/extern/ceres/internal/ceres/canonical_views_clustering.cc @@ -35,6 +35,7 @@ #include <unordered_set> #include "ceres/graph.h" +#include "ceres/internal/export.h" #include "ceres/map_util.h" #include "glog/logging.h" @@ -43,13 +44,11 @@ namespace internal { using std::vector; -typedef std::unordered_map<int, int> IntMap; -typedef std::unordered_set<int> IntSet; +using IntMap = std::unordered_map<int, int>; +using IntSet = std::unordered_set<int>; -class CanonicalViewsClustering { +class CERES_NO_EXPORT CanonicalViewsClustering { public: - CanonicalViewsClustering() {} - // Compute the canonical views clustering of the vertices of the // graph. centers will contain the vertices that are the identified // as the canonical views/cluster centers, and membership is a map @@ -85,11 +84,11 @@ void ComputeCanonicalViewsClustering( const WeightedGraph<int>& graph, vector<int>* centers, IntMap* membership) { - time_t start_time = time(NULL); + time_t start_time = time(nullptr); CanonicalViewsClustering cv; cv.ComputeClustering(options, graph, centers, membership); VLOG(2) << "Canonical views clustering time (secs): " - << time(NULL) - start_time; + << time(nullptr) - start_time; } // Implementation of CanonicalViewsClustering @@ -107,7 +106,7 @@ void CanonicalViewsClustering::ComputeClustering( IntSet valid_views; FindValidViews(&valid_views); - while (valid_views.size() > 0) { + while (!valid_views.empty()) { // Find the next best canonical view. double best_difference = -std::numeric_limits<double>::max(); int best_view = 0; @@ -174,9 +173,9 @@ double CanonicalViewsClustering::ComputeClusteringQualityDifference( difference -= options_.size_penalty_weight; // Orthogonality. - for (int i = 0; i < centers.size(); ++i) { + for (int center : centers) { difference -= options_.similarity_penalty_weight * - graph_->EdgeWeight(centers[i], candidate); + graph_->EdgeWeight(center, candidate); } return difference; diff --git a/extern/ceres/internal/ceres/canonical_views_clustering.h b/extern/ceres/internal/ceres/canonical_views_clustering.h index 465233ddfcd..00a6a739d29 100644 --- a/extern/ceres/internal/ceres/canonical_views_clustering.h +++ b/extern/ceres/internal/ceres/canonical_views_clustering.h @@ -45,7 +45,8 @@ #include <vector> #include "ceres/graph.h" -#include "ceres/internal/port.h" +#include "ceres/internal/disable_warnings.h" +#include "ceres/internal/export.h" namespace ceres { namespace internal { @@ -95,13 +96,13 @@ struct CanonicalViewsClusteringOptions; // It is possible depending on the configuration of the clustering // algorithm that some of the vertices may not be assigned to any // cluster. In this case they are assigned to a cluster with id = -1; -CERES_EXPORT_INTERNAL void ComputeCanonicalViewsClustering( +CERES_NO_EXPORT void ComputeCanonicalViewsClustering( const CanonicalViewsClusteringOptions& options, const WeightedGraph<int>& graph, std::vector<int>* centers, std::unordered_map<int, int>* membership); -struct CERES_EXPORT_INTERNAL CanonicalViewsClusteringOptions { +struct CERES_NO_EXPORT CanonicalViewsClusteringOptions { // The minimum number of canonical views to compute. int min_views = 3; @@ -122,4 +123,6 @@ struct CERES_EXPORT_INTERNAL CanonicalViewsClusteringOptions { } // namespace internal } // namespace ceres +#include "ceres/internal/reenable_warnings.h" + #endif // CERES_INTERNAL_CANONICAL_VIEWS_CLUSTERING_H_ diff --git a/extern/ceres/internal/ceres/casts.h b/extern/ceres/internal/ceres/casts.h index d13707131c2..04d8ba4fe33 100644 --- a/extern/ceres/internal/ceres/casts.h +++ b/extern/ceres/internal/ceres/casts.h @@ -32,14 +32,13 @@ #define CERES_INTERNAL_CASTS_H_ #include <cassert> -#include <cstddef> // For NULL. namespace ceres { // Identity metafunction. template <class T> struct identity_ { - typedef T type; + using type = T; }; // Use implicit_cast as a safe version of static_cast or const_cast @@ -86,6 +85,7 @@ inline To implicit_cast(typename identity_<From>::type const& f) { // if (dynamic_cast<Subclass2>(foo)) HandleASubclass2Object(foo); // You should design the code some other way not to need this. +// TODO(sameeragarwal): Modernize this. template <typename To, typename From> // use like this: down_cast<T*>(foo); inline To down_cast(From* f) { // so we only accept pointers // Ensures that To is a sub-type of From *. This test is here only @@ -95,11 +95,11 @@ inline To down_cast(From* f) { // so we only accept pointers // TODO(csilvers): This should use COMPILE_ASSERT. if (false) { - implicit_cast<From*, To>(NULL); + implicit_cast<From*, To>(nullptr); } // uses RTTI in dbg and fastbuild. asserts are disabled in opt builds. - assert(f == NULL || dynamic_cast<To>(f) != NULL); // NOLINT + assert(f == nullptr || dynamic_cast<To>(f) != nullptr); // NOLINT return static_cast<To>(f); } diff --git a/extern/ceres/internal/ceres/cgnr_linear_operator.h b/extern/ceres/internal/ceres/cgnr_linear_operator.h index beb8bbc2c2a..d708efca24c 100644 --- a/extern/ceres/internal/ceres/cgnr_linear_operator.h +++ b/extern/ceres/internal/ceres/cgnr_linear_operator.h @@ -34,7 +34,9 @@ #include <algorithm> #include <memory> +#include "ceres/internal/disable_warnings.h" #include "ceres/internal/eigen.h" +#include "ceres/internal/export.h" #include "ceres/linear_operator.h" namespace ceres { @@ -78,11 +80,10 @@ class SparseMatrix; // and z = A^T b // // Note: This class is not thread safe, since it uses some temporary storage. -class CgnrLinearOperator : public LinearOperator { +class CERES_NO_EXPORT CgnrLinearOperator final : public LinearOperator { public: CgnrLinearOperator(const LinearOperator& A, const double* D) : A_(A), D_(D), z_(new double[A.num_rows()]) {} - virtual ~CgnrLinearOperator() {} void RightMultiply(const double* x, double* y) const final { std::fill(z_.get(), z_.get() + A_.num_rows(), 0.0); @@ -94,7 +95,7 @@ class CgnrLinearOperator : public LinearOperator { A_.LeftMultiply(z_.get(), y); // y = y + DtDx - if (D_ != NULL) { + if (D_ != nullptr) { int n = A_.num_cols(); VectorRef(y, n).array() += ConstVectorRef(D_, n).array().square() * ConstVectorRef(x, n).array(); @@ -117,4 +118,6 @@ class CgnrLinearOperator : public LinearOperator { } // namespace internal } // namespace ceres +#include "ceres/internal/reenable_warnings.h" + #endif // CERES_INTERNAL_CGNR_LINEAR_OPERATOR_H_ diff --git a/extern/ceres/internal/ceres/cgnr_solver.cc b/extern/ceres/internal/ceres/cgnr_solver.cc index 9dba1cfb4a8..cca72bca988 100644 --- a/extern/ceres/internal/ceres/cgnr_solver.cc +++ b/extern/ceres/internal/ceres/cgnr_solver.cc @@ -30,6 +30,9 @@ #include "ceres/cgnr_solver.h" +#include <memory> +#include <utility> + #include "ceres/block_jacobi_preconditioner.h" #include "ceres/cgnr_linear_operator.h" #include "ceres/conjugate_gradients_solver.h" @@ -42,8 +45,8 @@ namespace ceres { namespace internal { -CgnrSolver::CgnrSolver(const LinearSolver::Options& options) - : options_(options) { +CgnrSolver::CgnrSolver(LinearSolver::Options options) + : options_(std::move(options)) { if (options_.preconditioner_type != JACOBI && options_.preconditioner_type != IDENTITY && options_.preconditioner_type != SUBSET) { @@ -54,7 +57,7 @@ CgnrSolver::CgnrSolver(const LinearSolver::Options& options) } } -CgnrSolver::~CgnrSolver() {} +CgnrSolver::~CgnrSolver() = default; LinearSolver::Summary CgnrSolver::SolveImpl( BlockSparseMatrix* A, @@ -70,7 +73,7 @@ LinearSolver::Summary CgnrSolver::SolveImpl( if (!preconditioner_) { if (options_.preconditioner_type == JACOBI) { - preconditioner_.reset(new BlockJacobiPreconditioner(*A)); + preconditioner_ = std::make_unique<BlockJacobiPreconditioner>(*A); } else if (options_.preconditioner_type == SUBSET) { Preconditioner::Options preconditioner_options; preconditioner_options.type = SUBSET; @@ -81,8 +84,8 @@ LinearSolver::Summary CgnrSolver::SolveImpl( preconditioner_options.use_postordering = options_.use_postordering; preconditioner_options.num_threads = options_.num_threads; preconditioner_options.context = options_.context; - preconditioner_.reset( - new SubsetPreconditioner(preconditioner_options, *A)); + preconditioner_ = + std::make_unique<SubsetPreconditioner>(preconditioner_options, *A); } } diff --git a/extern/ceres/internal/ceres/cgnr_solver.h b/extern/ceres/internal/ceres/cgnr_solver.h index bc701c0e9ed..25e62e9abd9 100644 --- a/extern/ceres/internal/ceres/cgnr_solver.h +++ b/extern/ceres/internal/ceres/cgnr_solver.h @@ -33,6 +33,7 @@ #include <memory> +#include "ceres/internal/export.h" #include "ceres/linear_solver.h" namespace ceres { @@ -49,12 +50,12 @@ class BlockJacobiPreconditioner; // // as required for solving for x in the least squares sense. Currently only // block diagonal preconditioning is supported. -class CgnrSolver : public BlockSparseMatrixSolver { +class CERES_NO_EXPORT CgnrSolver final : public BlockSparseMatrixSolver { public: - explicit CgnrSolver(const LinearSolver::Options& options); + explicit CgnrSolver(LinearSolver::Options options); CgnrSolver(const CgnrSolver&) = delete; void operator=(const CgnrSolver&) = delete; - virtual ~CgnrSolver(); + ~CgnrSolver() override; Summary SolveImpl(BlockSparseMatrix* A, const double* b, diff --git a/extern/ceres/internal/ceres/compressed_col_sparse_matrix_utils.cc b/extern/ceres/internal/ceres/compressed_col_sparse_matrix_utils.cc index e1f6bb8ff9d..94e7e9aa446 100644 --- a/extern/ceres/internal/ceres/compressed_col_sparse_matrix_utils.cc +++ b/extern/ceres/internal/ceres/compressed_col_sparse_matrix_utils.cc @@ -33,7 +33,7 @@ #include <algorithm> #include <vector> -#include "ceres/internal/port.h" +#include "ceres/internal/export.h" #include "glog/logging.h" namespace ceres { diff --git a/extern/ceres/internal/ceres/compressed_col_sparse_matrix_utils.h b/extern/ceres/internal/ceres/compressed_col_sparse_matrix_utils.h index d442e1a9bb8..f88a5bd9588 100644 --- a/extern/ceres/internal/ceres/compressed_col_sparse_matrix_utils.h +++ b/extern/ceres/internal/ceres/compressed_col_sparse_matrix_utils.h @@ -31,9 +31,11 @@ #ifndef CERES_INTERNAL_COMPRESSED_COL_SPARSE_MATRIX_UTILS_H_ #define CERES_INTERNAL_COMPRESSED_COL_SPARSE_MATRIX_UTILS_H_ +#include <algorithm> #include <vector> -#include "ceres/internal/port.h" +#include "ceres/internal/disable_warnings.h" +#include "ceres/internal/export.h" namespace ceres { namespace internal { @@ -48,7 +50,7 @@ namespace internal { // and column block j, then it is expected that A contains at least // one non-zero entry corresponding to the top left entry of c_ij, // as that entry is used to detect the presence of a non-zero c_ij. -CERES_EXPORT_INTERNAL void CompressedColumnScalarMatrixToBlockMatrix( +CERES_NO_EXPORT void CompressedColumnScalarMatrixToBlockMatrix( const int* scalar_rows, const int* scalar_cols, const std::vector<int>& row_blocks, @@ -59,7 +61,7 @@ CERES_EXPORT_INTERNAL void CompressedColumnScalarMatrixToBlockMatrix( // Given a set of blocks and a permutation of these blocks, compute // the corresponding "scalar" ordering, where the scalar ordering of // size sum(blocks). -CERES_EXPORT_INTERNAL void BlockOrderingToScalarOrdering( +CERES_NO_EXPORT void BlockOrderingToScalarOrdering( const std::vector<int>& blocks, const std::vector<int>& block_ordering, std::vector<int>* scalar_ordering); @@ -142,4 +144,6 @@ void SolveRTRWithSparseRHS(IntegerType num_cols, } // namespace internal } // namespace ceres +#include "ceres/internal/reenable_warnings.h" + #endif // CERES_INTERNAL_COMPRESSED_COL_SPARSE_MATRIX_UTILS_H_ diff --git a/extern/ceres/internal/ceres/compressed_row_jacobian_writer.cc b/extern/ceres/internal/ceres/compressed_row_jacobian_writer.cc index 8e7e3e7e7e6..55b30a290f9 100644 --- a/extern/ceres/internal/ceres/compressed_row_jacobian_writer.cc +++ b/extern/ceres/internal/ceres/compressed_row_jacobian_writer.cc @@ -1,5 +1,5 @@ // Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2015 Google Inc. All rights reserved. +// Copyright 2022 Google Inc. All rights reserved. // http://ceres-solver.org/ // // Redistribution and use in source and binary forms, with or without @@ -30,7 +30,10 @@ #include "ceres/compressed_row_jacobian_writer.h" +#include <algorithm> #include <iterator> +#include <memory> +#include <string> #include <utility> #include <vector> @@ -55,7 +58,7 @@ void CompressedRowJacobianWriter::PopulateJacobianRowAndColumnBlockVectors( vector<int>& col_blocks = *(jacobian->mutable_col_blocks()); col_blocks.resize(parameter_blocks.size()); for (int i = 0; i < parameter_blocks.size(); ++i) { - col_blocks[i] = parameter_blocks[i]->LocalSize(); + col_blocks[i] = parameter_blocks[i]->TangentSize(); } const vector<ResidualBlock*>& residual_blocks = program->residual_blocks(); @@ -81,10 +84,12 @@ void CompressedRowJacobianWriter::GetOrderedParameterBlocks( make_pair(parameter_block->index(), j)); } } - sort(evaluated_jacobian_blocks->begin(), evaluated_jacobian_blocks->end()); + std::sort(evaluated_jacobian_blocks->begin(), + evaluated_jacobian_blocks->end()); } -SparseMatrix* CompressedRowJacobianWriter::CreateJacobian() const { +std::unique_ptr<SparseMatrix> CompressedRowJacobianWriter::CreateJacobian() + const { const vector<ResidualBlock*>& residual_blocks = program_->residual_blocks(); int total_num_residuals = program_->NumResiduals(); @@ -92,14 +97,13 @@ SparseMatrix* CompressedRowJacobianWriter::CreateJacobian() const { // Count the number of jacobian nonzeros. int num_jacobian_nonzeros = 0; - for (int i = 0; i < residual_blocks.size(); ++i) { - ResidualBlock* residual_block = residual_blocks[i]; + for (auto* residual_block : residual_blocks) { const int num_residuals = residual_block->NumResiduals(); const int num_parameter_blocks = residual_block->NumParameterBlocks(); for (int j = 0; j < num_parameter_blocks; ++j) { ParameterBlock* parameter_block = residual_block->parameter_blocks()[j]; if (!parameter_block->IsConstant()) { - num_jacobian_nonzeros += num_residuals * parameter_block->LocalSize(); + num_jacobian_nonzeros += num_residuals * parameter_block->TangentSize(); } } } @@ -108,10 +112,11 @@ SparseMatrix* CompressedRowJacobianWriter::CreateJacobian() const { // Allocate more space than needed to store the jacobian so that when the LM // algorithm adds the diagonal, no reallocation is necessary. This reduces // peak memory usage significantly. - CompressedRowSparseMatrix* jacobian = new CompressedRowSparseMatrix( - total_num_residuals, - total_num_effective_parameters, - num_jacobian_nonzeros + total_num_effective_parameters); + std::unique_ptr<CompressedRowSparseMatrix> jacobian = + std::make_unique<CompressedRowSparseMatrix>( + total_num_residuals, + total_num_effective_parameters, + num_jacobian_nonzeros + total_num_effective_parameters); // At this stage, the CompressedRowSparseMatrix is an invalid state. But this // seems to be the only way to construct it without doing a memory copy. @@ -120,8 +125,7 @@ SparseMatrix* CompressedRowJacobianWriter::CreateJacobian() const { int row_pos = 0; rows[0] = 0; - for (int i = 0; i < residual_blocks.size(); ++i) { - const ResidualBlock* residual_block = residual_blocks[i]; + for (auto* residual_block : residual_blocks) { const int num_parameter_blocks = residual_block->NumParameterBlocks(); // Count the number of derivatives for a row of this residual block and @@ -132,7 +136,7 @@ SparseMatrix* CompressedRowJacobianWriter::CreateJacobian() const { ParameterBlock* parameter_block = residual_block->parameter_blocks()[j]; if (!parameter_block->IsConstant()) { parameter_indices.push_back(parameter_block->index()); - num_derivatives += parameter_block->LocalSize(); + num_derivatives += parameter_block->TangentSize(); } } @@ -163,10 +167,10 @@ SparseMatrix* CompressedRowJacobianWriter::CreateJacobian() const { // parameter vector. This code mirrors that in Write(), where jacobian // values are updated. int col_pos = 0; - for (int j = 0; j < parameter_indices.size(); ++j) { + for (int parameter_index : parameter_indices) { ParameterBlock* parameter_block = - program_->parameter_blocks()[parameter_indices[j]]; - const int parameter_block_size = parameter_block->LocalSize(); + program_->parameter_blocks()[parameter_index]; + const int parameter_block_size = parameter_block->TangentSize(); for (int r = 0; r < num_residuals; ++r) { // This is the position in the values array of the jacobian where this @@ -183,7 +187,7 @@ SparseMatrix* CompressedRowJacobianWriter::CreateJacobian() const { } CHECK_EQ(num_jacobian_nonzeros, rows[total_num_residuals]); - PopulateJacobianRowAndColumnBlockVectors(program_, jacobian); + PopulateJacobianRowAndColumnBlockVectors(program_, jacobian.get()); return jacobian; } @@ -192,8 +196,7 @@ void CompressedRowJacobianWriter::Write(int residual_id, int residual_offset, double** jacobians, SparseMatrix* base_jacobian) { - CompressedRowSparseMatrix* jacobian = - down_cast<CompressedRowSparseMatrix*>(base_jacobian); + auto* jacobian = down_cast<CompressedRowSparseMatrix*>(base_jacobian); double* jacobian_values = jacobian->mutable_values(); const int* jacobian_rows = jacobian->rows(); @@ -210,11 +213,11 @@ void CompressedRowJacobianWriter::Write(int residual_id, // Iterate over the jacobian blocks in increasing order of their // positions in the reduced parameter vector. - for (int i = 0; i < evaluated_jacobian_blocks.size(); ++i) { + for (auto& evaluated_jacobian_block : evaluated_jacobian_blocks) { const ParameterBlock* parameter_block = - program_->parameter_blocks()[evaluated_jacobian_blocks[i].first]; - const int argument = evaluated_jacobian_blocks[i].second; - const int parameter_block_size = parameter_block->LocalSize(); + program_->parameter_blocks()[evaluated_jacobian_block.first]; + const int argument = evaluated_jacobian_block.second; + const int parameter_block_size = parameter_block->TangentSize(); // Copy one row of the jacobian block at a time. for (int r = 0; r < num_residuals; ++r) { diff --git a/extern/ceres/internal/ceres/compressed_row_jacobian_writer.h b/extern/ceres/internal/ceres/compressed_row_jacobian_writer.h index b1251ca5cf5..7badab71b04 100644 --- a/extern/ceres/internal/ceres/compressed_row_jacobian_writer.h +++ b/extern/ceres/internal/ceres/compressed_row_jacobian_writer.h @@ -33,10 +33,12 @@ #ifndef CERES_INTERNAL_COMPRESSED_ROW_JACOBIAN_WRITER_H_ #define CERES_INTERNAL_COMPRESSED_ROW_JACOBIAN_WRITER_H_ +#include <memory> #include <utility> #include <vector> #include "ceres/evaluator.h" +#include "ceres/internal/export.h" #include "ceres/scratch_evaluate_preparer.h" namespace ceres { @@ -46,7 +48,7 @@ class CompressedRowSparseMatrix; class Program; class SparseMatrix; -class CompressedRowJacobianWriter { +class CERES_NO_EXPORT CompressedRowJacobianWriter { public: CompressedRowJacobianWriter(Evaluator::Options /* ignored */, Program* program) @@ -89,11 +91,12 @@ class CompressedRowJacobianWriter { // assumed by the cost functions, use scratch space to store the // jacobians temporarily then copy them over to the larger jacobian // in the Write() function. - ScratchEvaluatePreparer* CreateEvaluatePreparers(int num_threads) { + std::unique_ptr<ScratchEvaluatePreparer[]> CreateEvaluatePreparers( + int num_threads) { return ScratchEvaluatePreparer::Create(*program_, num_threads); } - SparseMatrix* CreateJacobian() const; + std::unique_ptr<SparseMatrix> CreateJacobian() const; void Write(int residual_id, int residual_offset, diff --git a/extern/ceres/internal/ceres/compressed_row_sparse_matrix.cc b/extern/ceres/internal/ceres/compressed_row_sparse_matrix.cc index 900586c2c45..db103d9c0fa 100644 --- a/extern/ceres/internal/ceres/compressed_row_sparse_matrix.cc +++ b/extern/ceres/internal/ceres/compressed_row_sparse_matrix.cc @@ -1,5 +1,5 @@ // Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2017 Google Inc. All rights reserved. +// Copyright 2022 Google Inc. All rights reserved. // http://ceres-solver.org/ // // Redistribution and use in source and binary forms, with or without @@ -31,11 +31,12 @@ #include "ceres/compressed_row_sparse_matrix.h" #include <algorithm> +#include <memory> #include <numeric> #include <vector> #include "ceres/crs_matrix.h" -#include "ceres/internal/port.h" +#include "ceres/internal/export.h" #include "ceres/random.h" #include "ceres/triplet_sparse_matrix.h" #include "glog/logging.h" @@ -104,7 +105,7 @@ void TransposeForCompressedRowSparseStructure(const int num_rows, const int c = cols[idx]; const int transpose_idx = transpose_rows[c]++; transpose_cols[transpose_idx] = r; - if (values != NULL && transpose_values != NULL) { + if (values != nullptr && transpose_values != nullptr) { transpose_values[transpose_idx] = values[idx]; } } @@ -174,18 +175,20 @@ CompressedRowSparseMatrix::CompressedRowSparseMatrix(int num_rows, cols_.size() * sizeof(double); // NOLINT } -CompressedRowSparseMatrix* CompressedRowSparseMatrix::FromTripletSparseMatrix( +std::unique_ptr<CompressedRowSparseMatrix> +CompressedRowSparseMatrix::FromTripletSparseMatrix( const TripletSparseMatrix& input) { return CompressedRowSparseMatrix::FromTripletSparseMatrix(input, false); } -CompressedRowSparseMatrix* +std::unique_ptr<CompressedRowSparseMatrix> CompressedRowSparseMatrix::FromTripletSparseMatrixTransposed( const TripletSparseMatrix& input) { return CompressedRowSparseMatrix::FromTripletSparseMatrix(input, true); } -CompressedRowSparseMatrix* CompressedRowSparseMatrix::FromTripletSparseMatrix( +std::unique_ptr<CompressedRowSparseMatrix> +CompressedRowSparseMatrix::FromTripletSparseMatrix( const TripletSparseMatrix& input, bool transpose) { int num_rows = input.num_rows(); int num_cols = input.num_cols(); @@ -214,8 +217,9 @@ CompressedRowSparseMatrix* CompressedRowSparseMatrix::FromTripletSparseMatrix( input.num_nonzeros() * sizeof(int) + // NOLINT input.num_nonzeros() * sizeof(double)); // NOLINT - CompressedRowSparseMatrix* output = - new CompressedRowSparseMatrix(num_rows, num_cols, input.num_nonzeros()); + std::unique_ptr<CompressedRowSparseMatrix> output = + std::make_unique<CompressedRowSparseMatrix>( + num_rows, num_cols, input.num_nonzeros()); if (num_rows == 0) { // No data to copy. @@ -266,7 +270,7 @@ CompressedRowSparseMatrix::CompressedRowSparseMatrix(const double* diagonal, CHECK_EQ(num_nonzeros(), num_rows); } -CompressedRowSparseMatrix::~CompressedRowSparseMatrix() {} +CompressedRowSparseMatrix::~CompressedRowSparseMatrix() = default; void CompressedRowSparseMatrix::SetZero() { std::fill(values_.begin(), values_.end(), 0); @@ -533,17 +537,19 @@ void CompressedRowSparseMatrix::SetMaxNumNonZeros(int num_nonzeros) { values_.resize(num_nonzeros); } -CompressedRowSparseMatrix* CompressedRowSparseMatrix::CreateBlockDiagonalMatrix( +std::unique_ptr<CompressedRowSparseMatrix> +CompressedRowSparseMatrix::CreateBlockDiagonalMatrix( const double* diagonal, const vector<int>& blocks) { int num_rows = 0; int num_nonzeros = 0; - for (int i = 0; i < blocks.size(); ++i) { - num_rows += blocks[i]; - num_nonzeros += blocks[i] * blocks[i]; + for (int block_size : blocks) { + num_rows += block_size; + num_nonzeros += block_size * block_size; } - CompressedRowSparseMatrix* matrix = - new CompressedRowSparseMatrix(num_rows, num_rows, num_nonzeros); + std::unique_ptr<CompressedRowSparseMatrix> matrix = + std::make_unique<CompressedRowSparseMatrix>( + num_rows, num_rows, num_nonzeros); int* rows = matrix->mutable_rows(); int* cols = matrix->mutable_cols(); @@ -552,8 +558,7 @@ CompressedRowSparseMatrix* CompressedRowSparseMatrix::CreateBlockDiagonalMatrix( int idx_cursor = 0; int col_cursor = 0; - for (int i = 0; i < blocks.size(); ++i) { - const int block_size = blocks[i]; + for (int block_size : blocks) { for (int r = 0; r < block_size; ++r) { *(rows++) = idx_cursor; values[idx_cursor + r] = diagonal[col_cursor + r]; @@ -573,9 +578,11 @@ CompressedRowSparseMatrix* CompressedRowSparseMatrix::CreateBlockDiagonalMatrix( return matrix; } -CompressedRowSparseMatrix* CompressedRowSparseMatrix::Transpose() const { - CompressedRowSparseMatrix* transpose = - new CompressedRowSparseMatrix(num_cols_, num_rows_, num_nonzeros()); +std::unique_ptr<CompressedRowSparseMatrix> +CompressedRowSparseMatrix::Transpose() const { + std::unique_ptr<CompressedRowSparseMatrix> transpose = + std::make_unique<CompressedRowSparseMatrix>( + num_cols_, num_rows_, num_nonzeros()); switch (storage_type_) { case UNSYMMETRIC: @@ -612,7 +619,8 @@ CompressedRowSparseMatrix* CompressedRowSparseMatrix::Transpose() const { return transpose; } -CompressedRowSparseMatrix* CompressedRowSparseMatrix::CreateRandomMatrix( +std::unique_ptr<CompressedRowSparseMatrix> +CompressedRowSparseMatrix::CreateRandomMatrix( CompressedRowSparseMatrix::RandomMatrixOptions options) { CHECK_GT(options.num_row_blocks, 0); CHECK_GT(options.min_row_block_size, 0); @@ -714,7 +722,7 @@ CompressedRowSparseMatrix* CompressedRowSparseMatrix::CreateRandomMatrix( const int num_rows = std::accumulate(row_blocks.begin(), row_blocks.end(), 0); const int num_cols = std::accumulate(col_blocks.begin(), col_blocks.end(), 0); const bool kDoNotTranspose = false; - CompressedRowSparseMatrix* matrix = + std::unique_ptr<CompressedRowSparseMatrix> matrix = CompressedRowSparseMatrix::FromTripletSparseMatrix( TripletSparseMatrix( num_rows, num_cols, tsm_rows, tsm_cols, tsm_values), diff --git a/extern/ceres/internal/ceres/compressed_row_sparse_matrix.h b/extern/ceres/internal/ceres/compressed_row_sparse_matrix.h index 0a1b945193d..3d7d385b185 100644 --- a/extern/ceres/internal/ceres/compressed_row_sparse_matrix.h +++ b/extern/ceres/internal/ceres/compressed_row_sparse_matrix.h @@ -31,9 +31,11 @@ #ifndef CERES_INTERNAL_COMPRESSED_ROW_SPARSE_MATRIX_H_ #define CERES_INTERNAL_COMPRESSED_ROW_SPARSE_MATRIX_H_ +#include <memory> #include <vector> -#include "ceres/internal/port.h" +#include "ceres/internal/disable_warnings.h" +#include "ceres/internal/export.h" #include "ceres/sparse_matrix.h" #include "ceres/types.h" #include "glog/logging.h" @@ -46,7 +48,7 @@ namespace internal { class TripletSparseMatrix; -class CERES_EXPORT_INTERNAL CompressedRowSparseMatrix : public SparseMatrix { +class CERES_NO_EXPORT CompressedRowSparseMatrix : public SparseMatrix { public: enum StorageType { UNSYMMETRIC, @@ -63,9 +65,7 @@ class CERES_EXPORT_INTERNAL CompressedRowSparseMatrix : public SparseMatrix { // entries. // // The storage type of the matrix is set to UNSYMMETRIC. - // - // Caller owns the result. - static CompressedRowSparseMatrix* FromTripletSparseMatrix( + static std::unique_ptr<CompressedRowSparseMatrix> FromTripletSparseMatrix( const TripletSparseMatrix& input); // Create a matrix with the same content as the TripletSparseMatrix @@ -73,10 +73,8 @@ class CERES_EXPORT_INTERNAL CompressedRowSparseMatrix : public SparseMatrix { // entries. // // The storage type of the matrix is set to UNSYMMETRIC. - // - // Caller owns the result. - static CompressedRowSparseMatrix* FromTripletSparseMatrixTransposed( - const TripletSparseMatrix& input); + static std::unique_ptr<CompressedRowSparseMatrix> + FromTripletSparseMatrixTransposed(const TripletSparseMatrix& input); // Use this constructor only if you know what you are doing. This // creates a "blank" matrix with the appropriate amount of memory @@ -100,7 +98,7 @@ class CERES_EXPORT_INTERNAL CompressedRowSparseMatrix : public SparseMatrix { CompressedRowSparseMatrix(const double* diagonal, int num_rows); // SparseMatrix interface. - virtual ~CompressedRowSparseMatrix(); + ~CompressedRowSparseMatrix() override; void SetZero() final; void RightMultiply(const double* x, double* y) const final; void LeftMultiply(const double* x, double* y) const final; @@ -124,7 +122,7 @@ class CERES_EXPORT_INTERNAL CompressedRowSparseMatrix : public SparseMatrix { void ToCRSMatrix(CRSMatrix* matrix) const; - CompressedRowSparseMatrix* Transpose() const; + std::unique_ptr<CompressedRowSparseMatrix> Transpose() const; // Destructive array resizing method. void SetMaxNumNonZeros(int num_nonzeros); @@ -140,7 +138,7 @@ class CERES_EXPORT_INTERNAL CompressedRowSparseMatrix : public SparseMatrix { const int* rows() const { return &rows_[0]; } int* mutable_rows() { return &rows_[0]; } - const StorageType storage_type() const { return storage_type_; } + StorageType storage_type() const { return storage_type_; } void set_storage_type(const StorageType storage_type) { storage_type_ = storage_type; } @@ -154,9 +152,7 @@ class CERES_EXPORT_INTERNAL CompressedRowSparseMatrix : public SparseMatrix { // Create a block diagonal CompressedRowSparseMatrix with the given // block structure. The individual blocks are assumed to be laid out // contiguously in the diagonal array, one block at a time. - // - // Caller owns the result. - static CompressedRowSparseMatrix* CreateBlockDiagonalMatrix( + static std::unique_ptr<CompressedRowSparseMatrix> CreateBlockDiagonalMatrix( const double* diagonal, const std::vector<int>& blocks); // Options struct to control the generation of random block sparse @@ -198,13 +194,11 @@ class CERES_EXPORT_INTERNAL CompressedRowSparseMatrix : public SparseMatrix { // Create a random CompressedRowSparseMatrix whose entries are // normally distributed and whose structure is determined by // RandomMatrixOptions. - // - // Caller owns the result. - static CompressedRowSparseMatrix* CreateRandomMatrix( + static std::unique_ptr<CompressedRowSparseMatrix> CreateRandomMatrix( RandomMatrixOptions options); private: - static CompressedRowSparseMatrix* FromTripletSparseMatrix( + static std::unique_ptr<CompressedRowSparseMatrix> FromTripletSparseMatrix( const TripletSparseMatrix& input, bool transpose); int num_rows_; @@ -226,4 +220,6 @@ class CERES_EXPORT_INTERNAL CompressedRowSparseMatrix : public SparseMatrix { } // namespace internal } // namespace ceres +#include "ceres/internal/reenable_warnings.h" + #endif // CERES_INTERNAL_COMPRESSED_ROW_SPARSE_MATRIX_H_ diff --git a/extern/ceres/internal/ceres/concurrent_queue.h b/extern/ceres/internal/ceres/concurrent_queue.h index a04d147c5c0..1e74153566a 100644 --- a/extern/ceres/internal/ceres/concurrent_queue.h +++ b/extern/ceres/internal/ceres/concurrent_queue.h @@ -78,7 +78,7 @@ template <typename T> class ConcurrentQueue { public: // Defaults the queue to blocking on Wait calls. - ConcurrentQueue() : wait_(true) {} + ConcurrentQueue() = default; // Atomically push an element onto the queue. If a thread was waiting for an // element, wake it up. @@ -149,7 +149,7 @@ class ConcurrentQueue { std::queue<T> queue_; // If true, signals that callers of Wait will block waiting to pop an // element off the queue. - bool wait_; + bool wait_{true}; }; } // namespace internal diff --git a/extern/ceres/internal/ceres/conditioned_cost_function.cc b/extern/ceres/internal/ceres/conditioned_cost_function.cc index fb4c52af084..a9013a23d0a 100644 --- a/extern/ceres/internal/ceres/conditioned_cost_function.cc +++ b/extern/ceres/internal/ceres/conditioned_cost_function.cc @@ -98,7 +98,7 @@ bool ConditionedCostFunction::Evaluate(double const* const* parameters, double** conditioner_derivative_pointer2 = &conditioner_derivative_pointer; if (!jacobians) { - conditioner_derivative_pointer2 = NULL; + conditioner_derivative_pointer2 = nullptr; } double unconditioned_residual = residuals[r]; diff --git a/extern/ceres/internal/ceres/conjugate_gradients_solver.cc b/extern/ceres/internal/ceres/conjugate_gradients_solver.cc index 3019628a16c..62ae9201cb5 100644 --- a/extern/ceres/internal/ceres/conjugate_gradients_solver.cc +++ b/extern/ceres/internal/ceres/conjugate_gradients_solver.cc @@ -41,6 +41,7 @@ #include <cmath> #include <cstddef> +#include <utility> #include "ceres/internal/eigen.h" #include "ceres/linear_operator.h" @@ -57,8 +58,8 @@ bool IsZeroOrInfinity(double x) { return ((x == 0.0) || std::isinf(x)); } } // namespace ConjugateGradientsSolver::ConjugateGradientsSolver( - const LinearSolver::Options& options) - : options_(options) {} + LinearSolver::Options options) + : options_(std::move(options)) {} LinearSolver::Summary ConjugateGradientsSolver::Solve( LinearOperator* A, @@ -112,7 +113,7 @@ LinearSolver::Summary ConjugateGradientsSolver::Solve( for (summary.num_iterations = 1;; ++summary.num_iterations) { // Apply preconditioner - if (per_solve_options.preconditioner != NULL) { + if (per_solve_options.preconditioner != nullptr) { z.setZero(); per_solve_options.preconditioner->RightMultiply(r.data(), z.data()); } else { diff --git a/extern/ceres/internal/ceres/conjugate_gradients_solver.h b/extern/ceres/internal/ceres/conjugate_gradients_solver.h index f79ca496531..99ddb5d485b 100644 --- a/extern/ceres/internal/ceres/conjugate_gradients_solver.h +++ b/extern/ceres/internal/ceres/conjugate_gradients_solver.h @@ -34,7 +34,8 @@ #ifndef CERES_INTERNAL_CONJUGATE_GRADIENTS_SOLVER_H_ #define CERES_INTERNAL_CONJUGATE_GRADIENTS_SOLVER_H_ -#include "ceres/internal/port.h" +#include "ceres/internal/disable_warnings.h" +#include "ceres/internal/export.h" #include "ceres/linear_solver.h" namespace ceres { @@ -55,9 +56,9 @@ class LinearOperator; // For more details see the documentation for // LinearSolver::PerSolveOptions::r_tolerance and // LinearSolver::PerSolveOptions::q_tolerance in linear_solver.h. -class CERES_EXPORT_INTERNAL ConjugateGradientsSolver : public LinearSolver { +class CERES_NO_EXPORT ConjugateGradientsSolver final : public LinearSolver { public: - explicit ConjugateGradientsSolver(const LinearSolver::Options& options); + explicit ConjugateGradientsSolver(LinearSolver::Options options); Summary Solve(LinearOperator* A, const double* b, const LinearSolver::PerSolveOptions& per_solve_options, @@ -70,4 +71,6 @@ class CERES_EXPORT_INTERNAL ConjugateGradientsSolver : public LinearSolver { } // namespace internal } // namespace ceres +#include "ceres/internal/reenable_warnings.h" + #endif // CERES_INTERNAL_CONJUGATE_GRADIENTS_SOLVER_H_ diff --git a/extern/ceres/internal/ceres/context.cc b/extern/ceres/internal/ceres/context.cc index 55e76351219..fde16b84bca 100644 --- a/extern/ceres/internal/ceres/context.cc +++ b/extern/ceres/internal/ceres/context.cc @@ -34,6 +34,8 @@ namespace ceres { +Context::Context() = default; Context* Context::Create() { return new internal::ContextImpl(); } +Context::~Context() = default; } // namespace ceres diff --git a/extern/ceres/internal/ceres/context_impl.cc b/extern/ceres/internal/ceres/context_impl.cc index 20fe5cbab2a..a4b3c842da1 100644 --- a/extern/ceres/internal/ceres/context_impl.cc +++ b/extern/ceres/internal/ceres/context_impl.cc @@ -30,9 +30,75 @@ #include "ceres/context_impl.h" +#include <string> + +#include "ceres/internal/config.h" + +#ifndef CERES_NO_CUDA +#include "cublas_v2.h" +#include "cuda_runtime.h" +#include "cusolverDn.h" +#endif // CERES_NO_CUDA + namespace ceres { namespace internal { +ContextImpl::ContextImpl() = default; + +#ifndef CERES_NO_CUDA +bool ContextImpl::InitCUDA(std::string* message) { + if (cuda_initialized_) { + return true; + } + if (cublasCreate(&cublas_handle_) != CUBLAS_STATUS_SUCCESS) { + *message = "cuBLAS::cublasCreate failed."; + cublas_handle_ = nullptr; + return false; + } + if (cusolverDnCreate(&cusolver_handle_) != CUSOLVER_STATUS_SUCCESS) { + *message = "cuSolverDN::cusolverDnCreate failed."; + cusolver_handle_ = nullptr; + cublasDestroy(cublas_handle_); + cublas_handle_ = nullptr; + return false; + } + if (cudaStreamCreateWithFlags(&stream_, cudaStreamNonBlocking) != + cudaSuccess) { + *message = "CUDA::cudaStreamCreateWithFlags failed."; + cusolverDnDestroy(cusolver_handle_); + cublasDestroy(cublas_handle_); + cusolver_handle_ = nullptr; + cublas_handle_ = nullptr; + stream_ = nullptr; + return false; + } + if (cusolverDnSetStream(cusolver_handle_, stream_) != + CUSOLVER_STATUS_SUCCESS || + cublasSetStream(cublas_handle_, stream_) != CUBLAS_STATUS_SUCCESS) { + *message = + "cuSolverDN::cusolverDnSetStream or cuBLAS::cublasSetStream failed."; + cusolverDnDestroy(cusolver_handle_); + cublasDestroy(cublas_handle_); + cudaStreamDestroy(stream_); + cusolver_handle_ = nullptr; + cublas_handle_ = nullptr; + stream_ = nullptr; + return false; + } + cuda_initialized_ = true; + return true; +} +#endif // CERES_NO_CUDA + +ContextImpl::~ContextImpl() { +#ifndef CERES_NO_CUDA + if (cuda_initialized_) { + cusolverDnDestroy(cusolver_handle_); + cublasDestroy(cublas_handle_); + cudaStreamDestroy(stream_); + } +#endif // CERES_NO_CUDA +} void ContextImpl::EnsureMinimumThreads(int num_threads) { #ifdef CERES_USE_CXX_THREADS thread_pool.Resize(num_threads); diff --git a/extern/ceres/internal/ceres/context_impl.h b/extern/ceres/internal/ceres/context_impl.h index 574d1efcc6d..8e9a03fb4ae 100644 --- a/extern/ceres/internal/ceres/context_impl.h +++ b/extern/ceres/internal/ceres/context_impl.h @@ -33,10 +33,20 @@ // This include must come before any #ifndef check on Ceres compile options. // clang-format off -#include "ceres/internal/port.h" -// clanf-format on +#include "ceres/internal/config.h" +// clang-format on + +#include <string> #include "ceres/context.h" +#include "ceres/internal/disable_warnings.h" +#include "ceres/internal/export.h" + +#ifndef CERES_NO_CUDA +#include "cublas_v2.h" +#include "cuda_runtime.h" +#include "cusolverDn.h" +#endif // CERES_NO_CUDA #ifdef CERES_USE_CXX_THREADS #include "ceres/thread_pool.h" @@ -45,14 +55,13 @@ namespace ceres { namespace internal { -class CERES_EXPORT_INTERNAL ContextImpl : public Context { +class CERES_NO_EXPORT ContextImpl final : public Context { public: - ContextImpl() {} + ContextImpl(); + ~ContextImpl() override; ContextImpl(const ContextImpl&) = delete; void operator=(const ContextImpl&) = delete; - virtual ~ContextImpl() {} - // When compiled with C++ threading support, resize the thread pool to have // at min(num_thread, num_hardware_threads) where num_hardware_threads is // defined by the hardware. Otherwise this call is a no-op. @@ -61,9 +70,28 @@ class CERES_EXPORT_INTERNAL ContextImpl : public Context { #ifdef CERES_USE_CXX_THREADS ThreadPool thread_pool; #endif // CERES_USE_CXX_THREADS + +#ifndef CERES_NO_CUDA + // Initializes the cuSolverDN context, creates an asynchronous stream, and + // associates the stream with cuSolverDN. Returns true iff initialization was + // successful, else it returns false and a human-readable error message is + // returned. + bool InitCUDA(std::string* message); + + // Handle to the cuSOLVER context. + cusolverDnHandle_t cusolver_handle_ = nullptr; + // Handle to cuBLAS context. + cublasHandle_t cublas_handle_ = nullptr; + // CUDA device stream. + cudaStream_t stream_ = nullptr; + // Indicates whether all the CUDA resources have been initialized. + bool cuda_initialized_ = false; +#endif // CERES_NO_CUDA }; } // namespace internal } // namespace ceres +#include "ceres/internal/reenable_warnings.h" + #endif // CERES_INTERNAL_CONTEXT_IMPL_H_ diff --git a/extern/ceres/internal/ceres/coordinate_descent_minimizer.cc b/extern/ceres/internal/ceres/coordinate_descent_minimizer.cc index 93096ac0728..a6e149d1cee 100644 --- a/extern/ceres/internal/ceres/coordinate_descent_minimizer.cc +++ b/extern/ceres/internal/ceres/coordinate_descent_minimizer.cc @@ -1,5 +1,5 @@ // Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2015 Google Inc. All rights reserved. +// Copyright 2022 Google Inc. All rights reserved. // http://ceres-solver.org/ // // Redistribution and use in source and binary forms, with or without @@ -64,7 +64,7 @@ CoordinateDescentMinimizer::CoordinateDescentMinimizer(ContextImpl* context) CHECK(context_ != nullptr); } -CoordinateDescentMinimizer::~CoordinateDescentMinimizer() {} +CoordinateDescentMinimizer::~CoordinateDescentMinimizer() = default; bool CoordinateDescentMinimizer::Init( const Program& program, @@ -94,9 +94,9 @@ bool CoordinateDescentMinimizer::Init( // assign zero offsets/empty independent sets to these parameter // blocks. const vector<ParameterBlock*>& parameter_blocks = program.parameter_blocks(); - for (int i = 0; i < parameter_blocks.size(); ++i) { - if (!ordering.IsMember(parameter_blocks[i]->mutable_user_state())) { - parameter_blocks_.push_back(parameter_blocks[i]); + for (auto* parameter_block : parameter_blocks) { + if (!ordering.IsMember(parameter_block->mutable_user_state())) { + parameter_blocks_.push_back(parameter_block); independent_set_offsets_.push_back(independent_set_offsets_.back()); } } @@ -105,8 +105,7 @@ bool CoordinateDescentMinimizer::Init( // block. residual_blocks_.resize(parameter_block_index.size()); const vector<ResidualBlock*>& residual_blocks = program.residual_blocks(); - for (int i = 0; i < residual_blocks.size(); ++i) { - ResidualBlock* residual_block = residual_blocks[i]; + for (auto* residual_block : residual_blocks) { const int num_parameter_blocks = residual_block->NumParameterBlocks(); for (int j = 0; j < num_parameter_blocks; ++j) { ParameterBlock* parameter_block = residual_block->parameter_blocks()[j]; @@ -129,14 +128,15 @@ void CoordinateDescentMinimizer::Minimize(const Minimizer::Options& options, double* parameters, Solver::Summary* summary) { // Set the state and mark all parameter blocks constant. - for (int i = 0; i < parameter_blocks_.size(); ++i) { - ParameterBlock* parameter_block = parameter_blocks_[i]; + for (auto* parameter_block : parameter_blocks_) { parameter_block->SetState(parameters + parameter_block->state_offset()); parameter_block->SetConstant(); } - std::unique_ptr<LinearSolver*[]> linear_solvers( - new LinearSolver*[options.num_threads]); + std::vector<std::unique_ptr<LinearSolver>> linear_solvers( + options.num_threads); + // std::unique_ptr<LinearSolver*[]> linear_solvers( + // new LinearSolver*[options.num_threads]); LinearSolver::Options linear_solver_options; linear_solver_options.type = DENSE_QR; @@ -188,7 +188,7 @@ void CoordinateDescentMinimizer::Minimize(const Minimizer::Options& options, // we are fine. Solver::Summary inner_summary; Solve(&inner_program, - linear_solvers[thread_id], + linear_solvers[thread_id].get(), parameters + parameter_block->state_offset(), &inner_summary); @@ -200,13 +200,13 @@ void CoordinateDescentMinimizer::Minimize(const Minimizer::Options& options, }); } - for (int i = 0; i < parameter_blocks_.size(); ++i) { - parameter_blocks_[i]->SetVarying(); + for (auto* parameter_block : parameter_blocks_) { + parameter_block->SetVarying(); } - for (int i = 0; i < options.num_threads; ++i) { - delete linear_solvers[i]; - } + // for (int i = 0; i < options.num_threads; ++i) { + // delete linear_solvers[i]; + //} } // Solve the optimization problem for one parameter block. @@ -221,17 +221,16 @@ void CoordinateDescentMinimizer::Solve(Program* program, string error; Minimizer::Options minimizer_options; - minimizer_options.evaluator.reset( - Evaluator::Create(evaluator_options_, program, &error)); + minimizer_options.evaluator = + Evaluator::Create(evaluator_options_, program, &error); CHECK(minimizer_options.evaluator != nullptr); - minimizer_options.jacobian.reset( - minimizer_options.evaluator->CreateJacobian()); + minimizer_options.jacobian = minimizer_options.evaluator->CreateJacobian(); CHECK(minimizer_options.jacobian != nullptr); TrustRegionStrategy::Options trs_options; trs_options.linear_solver = linear_solver; - minimizer_options.trust_region_strategy.reset( - TrustRegionStrategy::Create(trs_options)); + minimizer_options.trust_region_strategy = + TrustRegionStrategy::Create(trs_options); CHECK(minimizer_options.trust_region_strategy != nullptr); minimizer_options.is_silent = true; @@ -263,12 +262,12 @@ bool CoordinateDescentMinimizer::IsOrderingValid( // of independent sets of decreasing size and invert it. This // seems to work better in practice, i.e., Cameras before // points. -ParameterBlockOrdering* CoordinateDescentMinimizer::CreateOrdering( - const Program& program) { - std::unique_ptr<ParameterBlockOrdering> ordering(new ParameterBlockOrdering); +std::shared_ptr<ParameterBlockOrdering> +CoordinateDescentMinimizer::CreateOrdering(const Program& program) { + auto ordering = std::make_shared<ParameterBlockOrdering>(); ComputeRecursiveIndependentSetOrdering(program, ordering.get()); ordering->Reverse(); - return ordering.release(); + return ordering; } } // namespace internal diff --git a/extern/ceres/internal/ceres/coordinate_descent_minimizer.h b/extern/ceres/internal/ceres/coordinate_descent_minimizer.h index 7d17d53eb0f..75f26480c88 100644 --- a/extern/ceres/internal/ceres/coordinate_descent_minimizer.h +++ b/extern/ceres/internal/ceres/coordinate_descent_minimizer.h @@ -56,7 +56,7 @@ class LinearSolver; // // The minimizer assumes that none of the parameter blocks in the // program are constant. -class CoordinateDescentMinimizer : public Minimizer { +class CERES_NO_EXPORT CoordinateDescentMinimizer final : public Minimizer { public: explicit CoordinateDescentMinimizer(ContextImpl* context); @@ -66,7 +66,7 @@ class CoordinateDescentMinimizer : public Minimizer { std::string* error); // Minimizer interface. - virtual ~CoordinateDescentMinimizer(); + ~CoordinateDescentMinimizer() override; void Minimize(const Minimizer::Options& options, double* parameters, @@ -81,7 +81,8 @@ class CoordinateDescentMinimizer : public Minimizer { // of independent sets of decreasing size and invert it. This // seems to work better in practice, i.e., Cameras before // points. - static ParameterBlockOrdering* CreateOrdering(const Program& program); + static std::shared_ptr<ParameterBlockOrdering> CreateOrdering( + const Program& program); private: void Solve(Program* program, diff --git a/extern/ceres/internal/ceres/corrector.cc b/extern/ceres/internal/ceres/corrector.cc index 6a79a06a544..bf3ba9c5714 100644 --- a/extern/ceres/internal/ceres/corrector.cc +++ b/extern/ceres/internal/ceres/corrector.cc @@ -111,7 +111,7 @@ Corrector::Corrector(const double sq_norm, const double rho[3]) { } void Corrector::CorrectResiduals(const int num_rows, double* residuals) { - DCHECK(residuals != NULL); + DCHECK(residuals != nullptr); // Equation 11 in BANS. VectorRef(residuals, num_rows) *= residual_scaling_; } @@ -120,8 +120,8 @@ void Corrector::CorrectJacobian(const int num_rows, const int num_cols, double* residuals, double* jacobian) { - DCHECK(residuals != NULL); - DCHECK(jacobian != NULL); + DCHECK(residuals != nullptr); + DCHECK(jacobian != nullptr); // The common case (rho[2] <= 0). if (alpha_sq_norm_ == 0.0) { diff --git a/extern/ceres/internal/ceres/corrector.h b/extern/ceres/internal/ceres/corrector.h index 3e11cdce1ae..44379a3ea7a 100644 --- a/extern/ceres/internal/ceres/corrector.h +++ b/extern/ceres/internal/ceres/corrector.h @@ -35,7 +35,8 @@ #ifndef CERES_INTERNAL_CORRECTOR_H_ #define CERES_INTERNAL_CORRECTOR_H_ -#include "ceres/internal/port.h" +#include "ceres/internal/disable_warnings.h" +#include "ceres/internal/export.h" namespace ceres { namespace internal { @@ -48,7 +49,7 @@ namespace internal { // gauss newton approximation and then take its square root to get the // corresponding corrections to the residual and jacobian. For the // full expressions see Eq. 10 and 11 in BANS by Triggs et al. -class CERES_EXPORT_INTERNAL Corrector { +class CERES_NO_EXPORT Corrector { public: // The constructor takes the squared norm, the value, the first and // second derivatives of the LossFunction. It precalculates some of @@ -89,4 +90,6 @@ class CERES_EXPORT_INTERNAL Corrector { } // namespace internal } // namespace ceres +#include "ceres/internal/reenable_warnings.h" + #endif // CERES_INTERNAL_CORRECTOR_H_ diff --git a/extern/ceres/internal/ceres/blas.h b/extern/ceres/internal/ceres/cost_function.cc index a43301c5d18..7597b431ec9 100644 --- a/extern/ceres/internal/ceres/blas.h +++ b/extern/ceres/internal/ceres/cost_function.cc @@ -1,5 +1,5 @@ // Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2015 Google Inc. All rights reserved. +// Copyright 2022 Google Inc. All rights reserved. // http://ceres-solver.org/ // // Redistribution and use in source and binary forms, with or without @@ -27,31 +27,13 @@ // POSSIBILITY OF SUCH DAMAGE. // // Author: sameeragarwal@google.com (Sameer Agarwal) -// -// Wrapper functions around BLAS functions. +// keir@google.m (Keir Mierle) -#ifndef CERES_INTERNAL_BLAS_H_ -#define CERES_INTERNAL_BLAS_H_ +#include "ceres/cost_function.h" namespace ceres { -namespace internal { -class BLAS { - public: - // transpose = true : c = alpha * a'a + beta * c; - // transpose = false : c = alpha * aa' + beta * c; - // - // Assumes column major matrices. - static void SymmetricRankKUpdate(int num_rows, - int num_cols, - const double* a, - bool transpose, - double alpha, - double beta, - double* c); -}; +CostFunction::CostFunction() : num_residuals_(0) {} +CostFunction::~CostFunction() = default; -} // namespace internal } // namespace ceres - -#endif // CERES_INTERNAL_BLAS_H_ diff --git a/extern/ceres/internal/ceres/covariance.cc b/extern/ceres/internal/ceres/covariance.cc index 8e240ff317c..d63dd3789c3 100644 --- a/extern/ceres/internal/ceres/covariance.cc +++ b/extern/ceres/internal/ceres/covariance.cc @@ -39,15 +39,14 @@ namespace ceres { -using std::make_pair; using std::pair; using std::vector; Covariance::Covariance(const Covariance::Options& options) { - impl_.reset(new internal::CovarianceImpl(options)); + impl_ = std::make_unique<internal::CovarianceImpl>(options); } -Covariance::~Covariance() {} +Covariance::~Covariance() = default; bool Covariance::Compute( const vector<pair<const double*, const double*>>& covariance_blocks, diff --git a/extern/ceres/internal/ceres/covariance_impl.cc b/extern/ceres/internal/ceres/covariance_impl.cc index 1f86707f5a7..324b5531a04 100644 --- a/extern/ceres/internal/ceres/covariance_impl.cc +++ b/extern/ceres/internal/ceres/covariance_impl.cc @@ -1,5 +1,5 @@ // Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2015 Google Inc. All rights reserved. +// Copyright 2022 Google Inc. All rights reserved. // http://ceres-solver.org/ // // Redistribution and use in source and binary forms, with or without @@ -79,13 +79,12 @@ CovarianceImpl::CovarianceImpl(const Covariance::Options& options) evaluate_options_.apply_loss_function = options_.apply_loss_function; } -CovarianceImpl::~CovarianceImpl() {} +CovarianceImpl::~CovarianceImpl() = default; template <typename T> void CheckForDuplicates(std::vector<T> blocks) { - sort(blocks.begin(), blocks.end()); - typename std::vector<T>::iterator it = - std::adjacent_find(blocks.begin(), blocks.end()); + std::sort(blocks.begin(), blocks.end()); + auto it = std::adjacent_find(blocks.begin(), blocks.end()); if (it != blocks.end()) { // In case there are duplicates, we search for their location. std::map<T, std::vector<int>> blocks_map; @@ -117,7 +116,7 @@ bool CovarianceImpl::Compute(const CovarianceBlocks& covariance_blocks, covariance_blocks); problem_ = problem; parameter_block_to_row_index_.clear(); - covariance_matrix_.reset(NULL); + covariance_matrix_ = nullptr; is_valid_ = (ComputeCovarianceSparsity(covariance_blocks, problem) && ComputeCovarianceValues()); is_computed_ = true; @@ -162,10 +161,10 @@ bool CovarianceImpl::GetCovarianceBlockInTangentOrAmbientSpace( const int block1_size = block1->Size(); const int block2_size = block2->Size(); - const int block1_local_size = block1->LocalSize(); - const int block2_local_size = block2->LocalSize(); + const int block1_tangent_size = block1->TangentSize(); + const int block2_tangent_size = block2->TangentSize(); if (!lift_covariance_to_ambient_space) { - MatrixRef(covariance_block, block1_local_size, block2_local_size) + MatrixRef(covariance_block, block1_tangent_size, block2_tangent_size) .setZero(); } else { MatrixRef(covariance_block, block1_size, block2_size).setZero(); @@ -209,34 +208,34 @@ bool CovarianceImpl::GetCovarianceBlockInTangentOrAmbientSpace( FindOrDie(parameter_map, const_cast<double*>(parameter_block1)); ParameterBlock* block2 = FindOrDie(parameter_map, const_cast<double*>(parameter_block2)); - const LocalParameterization* local_param1 = block1->local_parameterization(); - const LocalParameterization* local_param2 = block2->local_parameterization(); + const Manifold* manifold1 = block1->manifold(); + const Manifold* manifold2 = block2->manifold(); const int block1_size = block1->Size(); - const int block1_local_size = block1->LocalSize(); + const int block1_tangent_size = block1->TangentSize(); const int block2_size = block2->Size(); - const int block2_local_size = block2->LocalSize(); + const int block2_tangent_size = block2->TangentSize(); - ConstMatrixRef cov( - covariance_matrix_->values() + rows[row_begin], block1_size, row_size); + ConstMatrixRef cov(covariance_matrix_->values() + rows[row_begin], + block1_tangent_size, + row_size); - // Fast path when there are no local parameterizations or if the - // user does not want it lifted to the ambient space. - if ((local_param1 == NULL && local_param2 == NULL) || + // Fast path when there are no manifolds or if the user does not want it + // lifted to the ambient space. + if ((manifold1 == nullptr && manifold2 == nullptr) || !lift_covariance_to_ambient_space) { if (transpose) { - MatrixRef(covariance_block, block2_local_size, block1_local_size) = - cov.block(0, offset, block1_local_size, block2_local_size) + MatrixRef(covariance_block, block2_tangent_size, block1_tangent_size) = + cov.block(0, offset, block1_tangent_size, block2_tangent_size) .transpose(); } else { - MatrixRef(covariance_block, block1_local_size, block2_local_size) = - cov.block(0, offset, block1_local_size, block2_local_size); + MatrixRef(covariance_block, block1_tangent_size, block2_tangent_size) = + cov.block(0, offset, block1_tangent_size, block2_tangent_size); } return true; } - // If local parameterizations are used then the covariance that has - // been computed is in the tangent space and it needs to be lifted - // back to the ambient space. + // If manifolds are used then the covariance that has been computed is in the + // tangent space and it needs to be lifted back to the ambient space. // // This is given by the formula // @@ -249,36 +248,37 @@ bool CovarianceImpl::GetCovarianceBlockInTangentOrAmbientSpace( // See Result 5.11 on page 142 of Hartley & Zisserman (2nd Edition) // for a proof. // - // TODO(sameeragarwal): Add caching of local parameterization, so - // that they are computed just once per parameter block. - Matrix block1_jacobian(block1_size, block1_local_size); - if (local_param1 == NULL) { + // TODO(sameeragarwal): Add caching the manifold plus_jacobian, so that they + // are computed just once per parameter block. + Matrix block1_jacobian(block1_size, block1_tangent_size); + if (manifold1 == nullptr) { block1_jacobian.setIdentity(); } else { - local_param1->ComputeJacobian(parameter_block1, block1_jacobian.data()); + manifold1->PlusJacobian(parameter_block1, block1_jacobian.data()); } - Matrix block2_jacobian(block2_size, block2_local_size); + Matrix block2_jacobian(block2_size, block2_tangent_size); // Fast path if the user is requesting a diagonal block. if (parameter_block1 == parameter_block2) { block2_jacobian = block1_jacobian; } else { - if (local_param2 == NULL) { + if (manifold2 == nullptr) { block2_jacobian.setIdentity(); } else { - local_param2->ComputeJacobian(parameter_block2, block2_jacobian.data()); + manifold2->PlusJacobian(parameter_block2, block2_jacobian.data()); } } if (transpose) { MatrixRef(covariance_block, block2_size, block1_size) = block2_jacobian * - cov.block(0, offset, block1_local_size, block2_local_size).transpose() * + cov.block(0, offset, block1_tangent_size, block2_tangent_size) + .transpose() * block1_jacobian.transpose(); } else { MatrixRef(covariance_block, block1_size, block2_size) = block1_jacobian * - cov.block(0, offset, block1_local_size, block2_local_size) * + cov.block(0, offset, block1_tangent_size, block2_tangent_size) * block2_jacobian.transpose(); } @@ -309,7 +309,7 @@ bool CovarianceImpl::GetCovarianceMatrixInTangentOrAmbientSpace( if (lift_covariance_to_ambient_space) { parameter_sizes.push_back(block->Size()); } else { - parameter_sizes.push_back(block->LocalSize()); + parameter_sizes.push_back(block->TangentSize()); } } std::partial_sum(parameter_sizes.begin(), @@ -383,8 +383,7 @@ bool CovarianceImpl::ComputeCovarianceSparsity( std::vector<ResidualBlock*> residual_blocks; problem->GetResidualBlocks(&residual_blocks); - for (int i = 0; i < residual_blocks.size(); ++i) { - ResidualBlock* residual_block = residual_blocks[i]; + for (auto* residual_block : residual_blocks) { parameter_blocks_in_use.insert(residual_block->parameter_blocks(), residual_block->parameter_blocks() + residual_block->NumParameterBlocks()); @@ -394,8 +393,7 @@ bool CovarianceImpl::ComputeCovarianceSparsity( std::vector<double*>& active_parameter_blocks = evaluate_options_.parameter_blocks; active_parameter_blocks.clear(); - for (int i = 0; i < all_parameter_blocks.size(); ++i) { - double* parameter_block = all_parameter_blocks[i]; + for (auto* parameter_block : all_parameter_blocks) { ParameterBlock* block = FindOrDie(parameter_map, parameter_block); if (!block->IsConstant() && (parameter_blocks_in_use.count(block) > 0)) { active_parameter_blocks.push_back(parameter_block); @@ -411,10 +409,9 @@ bool CovarianceImpl::ComputeCovarianceSparsity( // ordering of parameter blocks just constructed. int num_rows = 0; parameter_block_to_row_index_.clear(); - for (int i = 0; i < active_parameter_blocks.size(); ++i) { - double* parameter_block = active_parameter_blocks[i]; + for (auto* parameter_block : active_parameter_blocks) { const int parameter_block_size = - problem->ParameterBlockLocalSize(parameter_block); + problem->ParameterBlockTangentSize(parameter_block); parameter_block_to_row_index_[parameter_block] = num_rows; num_rows += parameter_block_size; } @@ -424,9 +421,7 @@ bool CovarianceImpl::ComputeCovarianceSparsity( // triangular part of the matrix. int num_nonzeros = 0; CovarianceBlocks covariance_blocks; - for (int i = 0; i < original_covariance_blocks.size(); ++i) { - const std::pair<const double*, const double*>& block_pair = - original_covariance_blocks[i]; + for (const auto& block_pair : original_covariance_blocks) { if (constant_parameter_blocks_.count(block_pair.first) > 0 || constant_parameter_blocks_.count(block_pair.second) > 0) { continue; @@ -434,8 +429,8 @@ bool CovarianceImpl::ComputeCovarianceSparsity( int index1 = FindOrDie(parameter_block_to_row_index_, block_pair.first); int index2 = FindOrDie(parameter_block_to_row_index_, block_pair.second); - const int size1 = problem->ParameterBlockLocalSize(block_pair.first); - const int size2 = problem->ParameterBlockLocalSize(block_pair.second); + const int size1 = problem->ParameterBlockTangentSize(block_pair.first); + const int size2 = problem->ParameterBlockTangentSize(block_pair.second); num_nonzeros += size1 * size2; // Make sure we are constructing a block upper triangular matrix. @@ -447,9 +442,9 @@ bool CovarianceImpl::ComputeCovarianceSparsity( } } - if (covariance_blocks.size() == 0) { + if (covariance_blocks.empty()) { VLOG(2) << "No non-zero covariance blocks found"; - covariance_matrix_.reset(NULL); + covariance_matrix_ = nullptr; return true; } @@ -459,8 +454,8 @@ bool CovarianceImpl::ComputeCovarianceSparsity( std::sort(covariance_blocks.begin(), covariance_blocks.end()); // Fill the sparsity pattern of the covariance matrix. - covariance_matrix_.reset( - new CompressedRowSparseMatrix(num_rows, num_rows, num_nonzeros)); + covariance_matrix_ = std::make_unique<CompressedRowSparseMatrix>( + num_rows, num_rows, num_nonzeros); int* rows = covariance_matrix_->mutable_rows(); int* cols = covariance_matrix_->mutable_cols(); @@ -480,7 +475,7 @@ bool CovarianceImpl::ComputeCovarianceSparsity( int cursor = 0; // index into the covariance matrix. for (const auto& entry : parameter_block_to_row_index_) { const double* row_block = entry.first; - const int row_block_size = problem->ParameterBlockLocalSize(row_block); + const int row_block_size = problem->ParameterBlockTangentSize(row_block); int row_begin = entry.second; // Iterate over the covariance blocks contained in this row block @@ -493,7 +488,7 @@ bool CovarianceImpl::ComputeCovarianceSparsity( if (block_pair.first != row_block) { break; } - num_columns += problem->ParameterBlockLocalSize(block_pair.second); + num_columns += problem->ParameterBlockTangentSize(block_pair.second); } // Fill out all the compressed rows for this parameter block. @@ -501,7 +496,8 @@ bool CovarianceImpl::ComputeCovarianceSparsity( rows[row_begin + r] = cursor; for (int c = 0; c < num_col_blocks; ++c) { const double* col_block = covariance_blocks[i + c].second; - const int col_block_size = problem->ParameterBlockLocalSize(col_block); + const int col_block_size = + problem->ParameterBlockTangentSize(col_block); int col_begin = FindOrDie(parameter_block_to_row_index_, col_block); for (int k = 0; k < col_block_size; ++k) { cols[cursor++] = col_begin++; @@ -556,13 +552,13 @@ bool CovarianceImpl::ComputeCovarianceValuesUsingSuiteSparseQR() { "CovarianceImpl::ComputeCovarianceValuesUsingSparseQR"); #ifndef CERES_NO_SUITESPARSE - if (covariance_matrix_.get() == NULL) { + if (covariance_matrix_ == nullptr) { // Nothing to do, all zeros covariance matrix. return true; } CRSMatrix jacobian; - problem_->Evaluate(evaluate_options_, NULL, NULL, NULL, &jacobian); + problem_->Evaluate(evaluate_options_, nullptr, nullptr, nullptr, &jacobian); event_logger.AddEvent("Evaluate"); // Construct a compressed column form of the Jacobian. @@ -601,11 +597,11 @@ bool CovarianceImpl::ComputeCovarianceValuesUsingSuiteSparseQR() { cholmod_jacobian.nrow = num_rows; cholmod_jacobian.ncol = num_cols; cholmod_jacobian.nzmax = num_nonzeros; - cholmod_jacobian.nz = NULL; + cholmod_jacobian.nz = nullptr; cholmod_jacobian.p = reinterpret_cast<void*>(&transpose_rows[0]); cholmod_jacobian.i = reinterpret_cast<void*>(&transpose_cols[0]); cholmod_jacobian.x = reinterpret_cast<void*>(&transpose_values[0]); - cholmod_jacobian.z = NULL; + cholmod_jacobian.z = nullptr; cholmod_jacobian.stype = 0; // Matrix is not symmetric. cholmod_jacobian.itype = CHOLMOD_LONG; cholmod_jacobian.xtype = CHOLMOD_REAL; @@ -616,8 +612,8 @@ bool CovarianceImpl::ComputeCovarianceValuesUsingSuiteSparseQR() { cholmod_common cc; cholmod_l_start(&cc); - cholmod_sparse* R = NULL; - SuiteSparse_long* permutation = NULL; + cholmod_sparse* R = nullptr; + SuiteSparse_long* permutation = nullptr; // Compute a Q-less QR factorization of the Jacobian. Since we are // only interested in inverting J'J = R'R, we do not need Q. This @@ -648,9 +644,9 @@ bool CovarianceImpl::ComputeCovarianceValuesUsingSuiteSparseQR() { } if (rank < cholmod_jacobian.ncol) { - LOG(ERROR) << "Jacobian matrix is rank deficient. " - << "Number of columns: " << cholmod_jacobian.ncol - << " rank: " << rank; + LOG(WARNING) << "Jacobian matrix is rank deficient. " + << "Number of columns: " << cholmod_jacobian.ncol + << " rank: " << rank; free(permutation); cholmod_l_free_sparse(&R, &cc); cholmod_l_finish(&cc); @@ -721,13 +717,13 @@ bool CovarianceImpl::ComputeCovarianceValuesUsingSuiteSparseQR() { bool CovarianceImpl::ComputeCovarianceValuesUsingDenseSVD() { EventLogger event_logger( "CovarianceImpl::ComputeCovarianceValuesUsingDenseSVD"); - if (covariance_matrix_.get() == NULL) { + if (covariance_matrix_ == nullptr) { // Nothing to do, all zeros covariance matrix. return true; } CRSMatrix jacobian; - problem_->Evaluate(evaluate_options_, NULL, NULL, NULL, &jacobian); + problem_->Evaluate(evaluate_options_, nullptr, nullptr, nullptr, &jacobian); event_logger.AddEvent("Evaluate"); Matrix dense_jacobian(jacobian.num_rows, jacobian.num_cols); @@ -812,20 +808,20 @@ bool CovarianceImpl::ComputeCovarianceValuesUsingDenseSVD() { bool CovarianceImpl::ComputeCovarianceValuesUsingEigenSparseQR() { EventLogger event_logger( "CovarianceImpl::ComputeCovarianceValuesUsingEigenSparseQR"); - if (covariance_matrix_.get() == NULL) { + if (covariance_matrix_ == nullptr) { // Nothing to do, all zeros covariance matrix. return true; } CRSMatrix jacobian; - problem_->Evaluate(evaluate_options_, NULL, NULL, NULL, &jacobian); + problem_->Evaluate(evaluate_options_, nullptr, nullptr, nullptr, &jacobian); event_logger.AddEvent("Evaluate"); - typedef Eigen::SparseMatrix<double, Eigen::ColMajor> EigenSparseMatrix; + using EigenSparseMatrix = Eigen::SparseMatrix<double, Eigen::ColMajor>; // Convert the matrix to column major order as required by SparseQR. EigenSparseMatrix sparse_jacobian = - Eigen::MappedSparseMatrix<double, Eigen::RowMajor>( + Eigen::Map<Eigen::SparseMatrix<double, Eigen::RowMajor>>( jacobian.num_rows, jacobian.num_cols, static_cast<int>(jacobian.values.size()), diff --git a/extern/ceres/internal/ceres/covariance_impl.h b/extern/ceres/internal/ceres/covariance_impl.h index 394a04bbc78..fc029ce25b7 100644 --- a/extern/ceres/internal/ceres/covariance_impl.h +++ b/extern/ceres/internal/ceres/covariance_impl.h @@ -38,7 +38,8 @@ #include <vector> #include "ceres/covariance.h" -#include "ceres/internal/port.h" +#include "ceres/internal/disable_warnings.h" +#include "ceres/internal/export.h" #include "ceres/problem_impl.h" #include "ceres/suitesparse.h" @@ -47,7 +48,7 @@ namespace internal { class CompressedRowSparseMatrix; -class CERES_EXPORT_INTERNAL CovarianceImpl { +class CERES_NO_EXPORT CovarianceImpl { public: explicit CovarianceImpl(const Covariance::Options& options); ~CovarianceImpl(); @@ -98,4 +99,6 @@ class CERES_EXPORT_INTERNAL CovarianceImpl { } // namespace internal } // namespace ceres +#include "ceres/internal/reenable_warnings.h" + #endif // CERES_INTERNAL_COVARIANCE_IMPL_H_ diff --git a/extern/ceres/internal/ceres/cuda_buffer.h b/extern/ceres/internal/ceres/cuda_buffer.h new file mode 100644 index 00000000000..a1cf78420d5 --- /dev/null +++ b/extern/ceres/internal/ceres/cuda_buffer.h @@ -0,0 +1,107 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2022 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: joydeepb@cs.utexas.edu (Joydeep Biswas) + +#ifndef CERES_INTERNAL_CUDA_BUFFER_H_ +#define CERES_INTERNAL_CUDA_BUFFER_H_ + +#include "ceres/internal/config.h" + +#ifndef CERES_NO_CUDA + +#include <vector> + +#include "cuda_runtime.h" +#include "glog/logging.h" + +// An encapsulated buffer to maintain GPU memory, and handle transfers between +// GPU and system memory. It is the responsibility of the user to ensure that +// the appropriate GPU device is selected before each subroutine is called. This +// is particularly important when using multiple GPU devices on different CPU +// threads, since active Cuda devices are determined by the cuda runtime on a +// per-thread basis. Note that unless otherwise specified, all methods use the +// default stream, and are synchronous. +template <typename T> +class CudaBuffer { + public: + CudaBuffer() = default; + CudaBuffer(const CudaBuffer&) = delete; + CudaBuffer& operator=(const CudaBuffer&) = delete; + + ~CudaBuffer() { + if (data_ != nullptr) { + CHECK_EQ(cudaFree(data_), cudaSuccess); + } + } + + // Grow the GPU memory buffer if needed to accommodate data of the specified + // size + void Reserve(const size_t size) { + if (size > size_) { + if (data_ != nullptr) { + CHECK_EQ(cudaFree(data_), cudaSuccess); + } + CHECK_EQ(cudaMalloc(&data_, size * sizeof(T)), cudaSuccess); + size_ = size; + } + } + + // Perform an asynchronous copy from CPU memory to GPU memory using the stream + // provided. + void CopyToGpuAsync(const T* data, const size_t size, cudaStream_t stream) { + Reserve(size); + CHECK_EQ(cudaMemcpyAsync( + data_, data, size * sizeof(T), cudaMemcpyHostToDevice, stream), + cudaSuccess); + } + + // Copy data from the GPU to CPU memory. This is necessarily synchronous since + // any potential GPU kernels that may be writing to the buffer must finish + // before the transfer happens. + void CopyToHost(T* data, const size_t size) { + CHECK(data_ != nullptr); + CHECK_EQ(cudaMemcpy(data, data_, size * sizeof(T), cudaMemcpyDeviceToHost), + cudaSuccess); + } + + void CopyToGpu(const std::vector<T>& data) { + CopyToGpu(data.data(), data.size()); + } + + T* data() { return data_; } + size_t size() const { return size_; } + + private: + T* data_ = nullptr; + size_t size_ = 0; +}; + +#endif // CERES_NO_CUDA + +#endif // CERES_INTERNAL_CUDA_BUFFER_H_
\ No newline at end of file diff --git a/extern/ceres/internal/ceres/cxsparse.cc b/extern/ceres/internal/ceres/cxsparse.cc index 0167f988648..b1eb2055e35 100644 --- a/extern/ceres/internal/ceres/cxsparse.cc +++ b/extern/ceres/internal/ceres/cxsparse.cc @@ -29,10 +29,11 @@ // Author: strandmark@google.com (Petter Strandmark) // This include must come before any #ifndef check on Ceres compile options. -#include "ceres/internal/port.h" +#include "ceres/internal/config.h" #ifndef CERES_NO_CXSPARSE +#include <memory> #include <string> #include <vector> @@ -47,7 +48,7 @@ namespace internal { using std::vector; -CXSparse::CXSparse() : scratch_(NULL), scratch_size_(0) {} +CXSparse::CXSparse() : scratch_(nullptr), scratch_size_(0) {} CXSparse::~CXSparse() { if (scratch_size_ > 0) { @@ -116,7 +117,7 @@ cs_dis* CXSparse::BlockAnalyzeCholesky(cs_di* A, block_matrix.nzmax = block_rows.size(); block_matrix.p = &block_cols[0]; block_matrix.i = &block_rows[0]; - block_matrix.x = NULL; + block_matrix.x = nullptr; int* ordering = cs_amd(1, &block_matrix); vector<int> block_ordering(num_row_blocks, -1); @@ -126,7 +127,7 @@ cs_dis* CXSparse::BlockAnalyzeCholesky(cs_di* A, vector<int> scalar_ordering; BlockOrderingToScalarOrdering(row_blocks, block_ordering, &scalar_ordering); - cs_dis* symbolic_factor = + auto* symbolic_factor = reinterpret_cast<cs_dis*>(cs_calloc(1, sizeof(cs_dis))); symbolic_factor->pinv = cs_pinv(&scalar_ordering[0], A->n); cs* permuted_A = cs_symperm(A, symbolic_factor->pinv, 0); @@ -138,7 +139,7 @@ cs_dis* CXSparse::BlockAnalyzeCholesky(cs_di* A, cs_free(postordering); cs_spfree(permuted_A); - symbolic_factor->cp = (int*)cs_malloc(A->n + 1, sizeof(int)); + symbolic_factor->cp = static_cast<int*>(cs_malloc(A->n + 1, sizeof(int))); symbolic_factor->lnz = cs_cumsum(symbolic_factor->cp, column_counts, A->n); symbolic_factor->unz = symbolic_factor->lnz; @@ -146,7 +147,7 @@ cs_dis* CXSparse::BlockAnalyzeCholesky(cs_di* A, if (symbolic_factor->lnz < 0) { cs_sfree(symbolic_factor); - symbolic_factor = NULL; + symbolic_factor = nullptr; } return symbolic_factor; @@ -206,8 +207,8 @@ CompressedRowSparseMatrix::StorageType CXSparseCholesky::StorageType() const { CXSparseCholesky::CXSparseCholesky(const OrderingType ordering_type) : ordering_type_(ordering_type), - symbolic_factor_(NULL), - numeric_factor_(NULL) {} + symbolic_factor_(nullptr), + numeric_factor_(nullptr) {} CXSparseCholesky::~CXSparseCholesky() { FreeSymbolicFactorization(); @@ -217,14 +218,14 @@ CXSparseCholesky::~CXSparseCholesky() { LinearSolverTerminationType CXSparseCholesky::Factorize( CompressedRowSparseMatrix* lhs, std::string* message) { CHECK_EQ(lhs->storage_type(), StorageType()); - if (lhs == NULL) { - *message = "Failure: Input lhs is NULL."; + if (lhs == nullptr) { + *message = "Failure: Input lhs is nullptr."; return LINEAR_SOLVER_FATAL_ERROR; } cs_di cs_lhs = cs_.CreateSparseMatrixTransposeView(lhs); - if (symbolic_factor_ == NULL) { + if (symbolic_factor_ == nullptr) { if (ordering_type_ == NATURAL) { symbolic_factor_ = cs_.AnalyzeCholeskyWithNaturalOrdering(&cs_lhs); } else { @@ -236,7 +237,7 @@ LinearSolverTerminationType CXSparseCholesky::Factorize( } } - if (symbolic_factor_ == NULL) { + if (symbolic_factor_ == nullptr) { *message = "CXSparse Failure : Symbolic factorization failed."; return LINEAR_SOLVER_FATAL_ERROR; } @@ -244,7 +245,7 @@ LinearSolverTerminationType CXSparseCholesky::Factorize( FreeNumericFactorization(); numeric_factor_ = cs_.Cholesky(&cs_lhs, symbolic_factor_); - if (numeric_factor_ == NULL) { + if (numeric_factor_ == nullptr) { *message = "CXSparse Failure : Numeric factorization failed."; return LINEAR_SOLVER_FAILURE; } @@ -255,7 +256,7 @@ LinearSolverTerminationType CXSparseCholesky::Factorize( LinearSolverTerminationType CXSparseCholesky::Solve(const double* rhs, double* solution, std::string* message) { - CHECK(numeric_factor_ != NULL) + CHECK(numeric_factor_ != nullptr) << "Solve called without a call to Factorize first."; const int num_cols = numeric_factor_->L->n; memcpy(solution, rhs, num_cols * sizeof(*solution)); @@ -264,16 +265,16 @@ LinearSolverTerminationType CXSparseCholesky::Solve(const double* rhs, } void CXSparseCholesky::FreeSymbolicFactorization() { - if (symbolic_factor_ != NULL) { + if (symbolic_factor_ != nullptr) { cs_.Free(symbolic_factor_); - symbolic_factor_ = NULL; + symbolic_factor_ = nullptr; } } void CXSparseCholesky::FreeNumericFactorization() { - if (numeric_factor_ != NULL) { + if (numeric_factor_ != nullptr) { cs_.Free(numeric_factor_); - numeric_factor_ = NULL; + numeric_factor_ = nullptr; } } diff --git a/extern/ceres/internal/ceres/cxsparse.h b/extern/ceres/internal/ceres/cxsparse.h index d3f76e0575e..97fc0459464 100644 --- a/extern/ceres/internal/ceres/cxsparse.h +++ b/extern/ceres/internal/ceres/cxsparse.h @@ -32,7 +32,7 @@ #define CERES_INTERNAL_CXSPARSE_H_ // This include must come before any #ifndef check on Ceres compile options. -#include "ceres/internal/port.h" +#include "ceres/internal/config.h" #ifndef CERES_NO_CXSPARSE @@ -40,6 +40,7 @@ #include <string> #include <vector> +#include "ceres/internal/disable_warnings.h" #include "ceres/linear_solver.h" #include "ceres/sparse_cholesky.h" #include "cs.h" @@ -54,7 +55,7 @@ class TripletSparseMatrix; // factorization with a known symbolic factorization. This features does not // explicitly exist in CXSparse. The methods in the class are nonstatic because // the class manages internal scratch space. -class CXSparse { +class CERES_NO_EXPORT CXSparse { public: CXSparse(); ~CXSparse(); @@ -80,7 +81,7 @@ class CXSparse { cs_di CreateSparseMatrixTransposeView(CompressedRowSparseMatrix* A); // Creates a new matrix from a triplet form. Deallocate the returned matrix - // with Free. May return NULL if the compression or allocation fails. + // with Free. May return nullptr if the compression or allocation fails. cs_di* CreateSparseMatrix(TripletSparseMatrix* A); // B = A' @@ -122,7 +123,7 @@ class CXSparse { const std::vector<int>& col_blocks); // Compute an fill-reducing approximate minimum degree ordering of - // the matrix A. ordering should be non-NULL and should point to + // the matrix A. ordering should be non-nullptr and should point to // enough memory to hold the ordering for the rows of A. void ApproximateMinimumDegreeOrdering(cs_di* A, int* ordering); @@ -138,13 +139,13 @@ class CXSparse { // An implementation of SparseCholesky interface using the CXSparse // library. -class CXSparseCholesky : public SparseCholesky { +class CERES_NO_EXPORT CXSparseCholesky final : public SparseCholesky { public: // Factory static std::unique_ptr<SparseCholesky> Create(OrderingType ordering_type); // SparseCholesky interface. - virtual ~CXSparseCholesky(); + ~CXSparseCholesky() override; CompressedRowSparseMatrix::StorageType StorageType() const final; LinearSolverTerminationType Factorize(CompressedRowSparseMatrix* lhs, std::string* message) final; @@ -153,7 +154,7 @@ class CXSparseCholesky : public SparseCholesky { std::string* message) final; private: - CXSparseCholesky(const OrderingType ordering_type); + explicit CXSparseCholesky(const OrderingType ordering_type); void FreeSymbolicFactorization(); void FreeNumericFactorization(); @@ -166,6 +167,8 @@ class CXSparseCholesky : public SparseCholesky { } // namespace internal } // namespace ceres +#include "ceres/internal/reenable_warnings.h" + #else typedef void cs_dis; diff --git a/extern/ceres/internal/ceres/dense_cholesky.cc b/extern/ceres/internal/ceres/dense_cholesky.cc new file mode 100644 index 00000000000..0e0bba7873b --- /dev/null +++ b/extern/ceres/internal/ceres/dense_cholesky.cc @@ -0,0 +1,327 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2022 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 "ceres/dense_cholesky.h" + +#include <algorithm> +#include <memory> +#include <string> +#include <vector> + +#include "ceres/internal/config.h" + +#ifndef CERES_NO_CUDA +#include "ceres/context_impl.h" +#include "cuda_runtime.h" +#include "cusolverDn.h" +#endif // CERES_NO_CUDA + +#ifndef CERES_NO_LAPACK + +// C interface to the LAPACK Cholesky factorization and triangular solve. +extern "C" void dpotrf_( + const char* uplo, const int* n, double* a, const int* lda, int* info); + +extern "C" void dpotrs_(const char* uplo, + const int* n, + const int* nrhs, + const double* a, + const int* lda, + double* b, + const int* ldb, + int* info); +#endif + +namespace ceres { +namespace internal { + +DenseCholesky::~DenseCholesky() = default; + +std::unique_ptr<DenseCholesky> DenseCholesky::Create( + const LinearSolver::Options& options) { + std::unique_ptr<DenseCholesky> dense_cholesky; + + switch (options.dense_linear_algebra_library_type) { + case EIGEN: + dense_cholesky = std::make_unique<EigenDenseCholesky>(); + break; + + case LAPACK: +#ifndef CERES_NO_LAPACK + dense_cholesky = std::make_unique<LAPACKDenseCholesky>(); + break; +#else + LOG(FATAL) << "Ceres was compiled without support for LAPACK."; +#endif + + case CUDA: +#ifndef CERES_NO_CUDA + dense_cholesky = CUDADenseCholesky::Create(options); + break; +#else + LOG(FATAL) << "Ceres was compiled without support for CUDA."; +#endif + + default: + LOG(FATAL) << "Unknown dense linear algebra library type : " + << DenseLinearAlgebraLibraryTypeToString( + options.dense_linear_algebra_library_type); + } + return dense_cholesky; +} + +LinearSolverTerminationType DenseCholesky::FactorAndSolve( + int num_cols, + double* lhs, + const double* rhs, + double* solution, + std::string* message) { + LinearSolverTerminationType termination_type = + Factorize(num_cols, lhs, message); + if (termination_type == LINEAR_SOLVER_SUCCESS) { + termination_type = Solve(rhs, solution, message); + } + return termination_type; +} + +LinearSolverTerminationType EigenDenseCholesky::Factorize( + int num_cols, double* lhs, std::string* message) { + Eigen::Map<Eigen::MatrixXd> m(lhs, num_cols, num_cols); + llt_ = std::make_unique<LLTType>(m); + if (llt_->info() != Eigen::Success) { + *message = "Eigen failure. Unable to perform dense Cholesky factorization."; + return LINEAR_SOLVER_FAILURE; + } + + *message = "Success."; + return LINEAR_SOLVER_SUCCESS; +} + +LinearSolverTerminationType EigenDenseCholesky::Solve(const double* rhs, + double* solution, + std::string* message) { + if (llt_->info() != Eigen::Success) { + *message = "Eigen failure. Unable to perform dense Cholesky factorization."; + return LINEAR_SOLVER_FAILURE; + } + + VectorRef(solution, llt_->cols()) = + llt_->solve(ConstVectorRef(rhs, llt_->cols())); + *message = "Success."; + return LINEAR_SOLVER_SUCCESS; +} + +#ifndef CERES_NO_LAPACK +LinearSolverTerminationType LAPACKDenseCholesky::Factorize( + int num_cols, double* lhs, std::string* message) { + lhs_ = lhs; + num_cols_ = num_cols; + + const char uplo = 'L'; + int info = 0; + dpotrf_(&uplo, &num_cols_, lhs_, &num_cols_, &info); + + if (info < 0) { + termination_type_ = LINEAR_SOLVER_FATAL_ERROR; + LOG(FATAL) << "Congratulations, you found a bug in Ceres. " + << "Please report it. " + << "LAPACK::dpotrf fatal error. " + << "Argument: " << -info << " is invalid."; + } else if (info > 0) { + termination_type_ = LINEAR_SOLVER_FAILURE; + *message = StringPrintf( + "LAPACK::dpotrf numerical failure. " + "The leading minor of order %d is not positive definite.", + info); + } else { + termination_type_ = LINEAR_SOLVER_SUCCESS; + *message = "Success."; + } + return termination_type_; +} + +LinearSolverTerminationType LAPACKDenseCholesky::Solve(const double* rhs, + double* solution, + std::string* message) { + const char uplo = 'L'; + const int nrhs = 1; + int info = 0; + + std::copy_n(rhs, num_cols_, solution); + dpotrs_( + &uplo, &num_cols_, &nrhs, lhs_, &num_cols_, solution, &num_cols_, &info); + + if (info < 0) { + termination_type_ = LINEAR_SOLVER_FATAL_ERROR; + LOG(FATAL) << "Congratulations, you found a bug in Ceres. " + << "Please report it. " + << "LAPACK::dpotrs fatal error. " + << "Argument: " << -info << " is invalid."; + } + + *message = "Success"; + termination_type_ = LINEAR_SOLVER_SUCCESS; + + return termination_type_; +} + +#endif // CERES_NO_LAPACK + +#ifndef CERES_NO_CUDA + +bool CUDADenseCholesky::Init(ContextImpl* context, std::string* message) { + if (!context->InitCUDA(message)) { + return false; + } + cusolver_handle_ = context->cusolver_handle_; + stream_ = context->stream_; + error_.Reserve(1); + *message = "CUDADenseCholesky::Init Success."; + return true; +} + +LinearSolverTerminationType CUDADenseCholesky::Factorize(int num_cols, + double* lhs, + std::string* message) { + factorize_result_ = LinearSolverTerminationType::LINEAR_SOLVER_FATAL_ERROR; + lhs_.Reserve(num_cols * num_cols); + num_cols_ = num_cols; + lhs_.CopyToGpuAsync(lhs, num_cols * num_cols, stream_); + int device_workspace_size = 0; + if (cusolverDnDpotrf_bufferSize(cusolver_handle_, + CUBLAS_FILL_MODE_LOWER, + num_cols, + lhs_.data(), + num_cols, + &device_workspace_size) != + CUSOLVER_STATUS_SUCCESS) { + *message = "cuSolverDN::cusolverDnDpotrf_bufferSize failed."; + return LinearSolverTerminationType::LINEAR_SOLVER_FATAL_ERROR; + } + device_workspace_.Reserve(device_workspace_size); + if (cusolverDnDpotrf(cusolver_handle_, + CUBLAS_FILL_MODE_LOWER, + num_cols, + lhs_.data(), + num_cols, + reinterpret_cast<double*>(device_workspace_.data()), + device_workspace_.size(), + error_.data()) != CUSOLVER_STATUS_SUCCESS) { + *message = "cuSolverDN::cusolverDnDpotrf failed."; + return LinearSolverTerminationType::LINEAR_SOLVER_FATAL_ERROR; + } + if (cudaDeviceSynchronize() != cudaSuccess || + cudaStreamSynchronize(stream_) != cudaSuccess) { + *message = "Cuda device synchronization failed."; + return LinearSolverTerminationType::LINEAR_SOLVER_FATAL_ERROR; + } + int error = 0; + error_.CopyToHost(&error, 1); + if (error < 0) { + LOG(FATAL) << "Congratulations, you found a bug in Ceres - " + << "please report it. " + << "cuSolverDN::cusolverDnXpotrf fatal error. " + << "Argument: " << -error << " is invalid."; + // The following line is unreachable, but return failure just to be + // pedantic, since the compiler does not know that. + return LinearSolverTerminationType::LINEAR_SOLVER_FATAL_ERROR; + } else if (error > 0) { + *message = StringPrintf( + "cuSolverDN::cusolverDnDpotrf numerical failure. " + "The leading minor of order %d is not positive definite.", + error); + factorize_result_ = LinearSolverTerminationType::LINEAR_SOLVER_FAILURE; + return LinearSolverTerminationType::LINEAR_SOLVER_FAILURE; + } + *message = "Success"; + factorize_result_ = LinearSolverTerminationType::LINEAR_SOLVER_SUCCESS; + return LinearSolverTerminationType::LINEAR_SOLVER_SUCCESS; +} + +LinearSolverTerminationType CUDADenseCholesky::Solve(const double* rhs, + double* solution, + std::string* message) { + if (factorize_result_ != LinearSolverTerminationType::LINEAR_SOLVER_SUCCESS) { + *message = "Factorize did not complete succesfully previously."; + return factorize_result_; + } + rhs_.CopyToGpuAsync(rhs, num_cols_, stream_); + if (cusolverDnDpotrs(cusolver_handle_, + CUBLAS_FILL_MODE_LOWER, + num_cols_, + 1, + lhs_.data(), + num_cols_, + rhs_.data(), + num_cols_, + error_.data()) != CUSOLVER_STATUS_SUCCESS) { + *message = "cuSolverDN::cusolverDnDpotrs failed."; + return LinearSolverTerminationType::LINEAR_SOLVER_FATAL_ERROR; + } + if (cudaDeviceSynchronize() != cudaSuccess || + cudaStreamSynchronize(stream_) != cudaSuccess) { + *message = "Cuda device synchronization failed."; + return LinearSolverTerminationType::LINEAR_SOLVER_FATAL_ERROR; + } + int error = 0; + error_.CopyToHost(&error, 1); + if (error != 0) { + LOG(FATAL) << "Congratulations, you found a bug in Ceres. " + << "Please report it." + << "cuSolverDN::cusolverDnDpotrs fatal error. " + << "Argument: " << -error << " is invalid."; + } + rhs_.CopyToHost(solution, num_cols_); + *message = "Success"; + return LinearSolverTerminationType::LINEAR_SOLVER_SUCCESS; +} + +std::unique_ptr<CUDADenseCholesky> CUDADenseCholesky::Create( + const LinearSolver::Options& options) { + if (options.dense_linear_algebra_library_type != CUDA) { + // The user called the wrong factory method. + return nullptr; + } + auto cuda_dense_cholesky = + std::unique_ptr<CUDADenseCholesky>(new CUDADenseCholesky()); + std::string cuda_error; + if (cuda_dense_cholesky->Init(options.context, &cuda_error)) { + return cuda_dense_cholesky; + } + // Initialization failed, destroy the object (done automatically) and return a + // nullptr. + LOG(ERROR) << "CUDADenseCholesky::Init failed: " << cuda_error; + return nullptr; +} + +#endif // CERES_NO_CUDA + +} // namespace internal +} // namespace ceres diff --git a/extern/ceres/internal/ceres/dense_cholesky.h b/extern/ceres/internal/ceres/dense_cholesky.h new file mode 100644 index 00000000000..655a2f815ee --- /dev/null +++ b/extern/ceres/internal/ceres/dense_cholesky.h @@ -0,0 +1,183 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2022 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#ifndef CERES_INTERNAL_DENSE_CHOLESKY_H_ +#define CERES_INTERNAL_DENSE_CHOLESKY_H_ + +// This include must come before any #ifndef check on Ceres compile options. +// clang-format off +#include "ceres/internal/config.h" +// clang-format on + +#include <memory> +#include <vector> + +#include "Eigen/Dense" +#include "ceres/cuda_buffer.h" +#include "ceres/linear_solver.h" +#include "glog/logging.h" +#ifndef CERES_NO_CUDA +#include "ceres/context_impl.h" +#include "cuda_runtime.h" +#include "cusolverDn.h" +#endif // CERES_NO_CUDA + +namespace ceres { +namespace internal { + +// An interface that abstracts away the internal details of various dense linear +// algebra libraries and offers a simple API for solving dense symmetric +// positive definite linear systems using a Cholesky factorization. +class CERES_NO_EXPORT DenseCholesky { + public: + static std::unique_ptr<DenseCholesky> Create( + const LinearSolver::Options& options); + + virtual ~DenseCholesky(); + + // Computes the Cholesky factorization of the given matrix. + // + // The input matrix lhs is assumed to be a column-major num_cols x num_cols + // matrix, that is symmetric positive definite with its lower triangular part + // containing the left hand side of the linear system being solved. + // + // The input matrix lhs may be modified by the implementation to store the + // factorization, irrespective of whether the factorization succeeds or not. + // As a result it is the user's responsibility to ensure that lhs is valid + // when Solve is called. + virtual LinearSolverTerminationType Factorize(int num_cols, + double* lhs, + std::string* message) = 0; + + // Computes the solution to the equation + // + // lhs * solution = rhs + // + // Calling Solve without calling Factorize is undefined behaviour. It is the + // user's responsibility to ensure that the input matrix lhs passed to + // Factorize has not been freed/modified when Solve is called. + virtual LinearSolverTerminationType Solve(const double* rhs, + double* solution, + std::string* message) = 0; + + // Convenience method which combines a call to Factorize and Solve. Solve is + // only called if Factorize returns LINEAR_SOLVER_SUCCESS. + // + // The input matrix lhs may be modified by the implementation to store the + // factorization, irrespective of whether the method succeeds or not. It is + // the user's responsibility to ensure that lhs is valid if and when Solve is + // called again after this call. + LinearSolverTerminationType FactorAndSolve(int num_cols, + double* lhs, + const double* rhs, + double* solution, + std::string* message); +}; + +class CERES_NO_EXPORT EigenDenseCholesky final : public DenseCholesky { + public: + LinearSolverTerminationType Factorize(int num_cols, + double* lhs, + std::string* message) override; + LinearSolverTerminationType Solve(const double* rhs, + double* solution, + std::string* message) override; + + private: + using LLTType = Eigen::LLT<Eigen::Ref<Eigen::MatrixXd>, Eigen::Lower>; + std::unique_ptr<LLTType> llt_; +}; + +#ifndef CERES_NO_LAPACK +class CERES_NO_EXPORT LAPACKDenseCholesky final : public DenseCholesky { + public: + LinearSolverTerminationType Factorize(int num_cols, + double* lhs, + std::string* message) override; + LinearSolverTerminationType Solve(const double* rhs, + double* solution, + std::string* message) override; + + private: + double* lhs_ = nullptr; + int num_cols_ = -1; + LinearSolverTerminationType termination_type_ = LINEAR_SOLVER_FATAL_ERROR; +}; +#endif // CERES_NO_LAPACK + +#ifndef CERES_NO_CUDA +// CUDA implementation of DenseCholesky using the cuSolverDN library using the +// 32-bit legacy interface for maximum compatibility. +class CERES_NO_EXPORT CUDADenseCholesky final : public DenseCholesky { + public: + static std::unique_ptr<CUDADenseCholesky> Create( + const LinearSolver::Options& options); + CUDADenseCholesky(const CUDADenseCholesky&) = delete; + CUDADenseCholesky& operator=(const CUDADenseCholesky&) = delete; + LinearSolverTerminationType Factorize(int num_cols, + double* lhs, + std::string* message) override; + LinearSolverTerminationType Solve(const double* rhs, + double* solution, + std::string* message) override; + + private: + CUDADenseCholesky() = default; + // Picks up the cuSolverDN and cuStream handles from the context. If + // the context is unable to initialize CUDA, returns false with a + // human-readable message indicating the reason. + bool Init(ContextImpl* context, std::string* message); + + // Handle to the cuSOLVER context. + cusolverDnHandle_t cusolver_handle_ = nullptr; + // CUDA device stream. + cudaStream_t stream_ = nullptr; + // Number of columns in the A matrix, to be cached between calls to *Factorize + // and *Solve. + size_t num_cols_ = 0; + // GPU memory allocated for the A matrix (lhs matrix). + CudaBuffer<double> lhs_; + // GPU memory allocated for the B matrix (rhs vector). + CudaBuffer<double> rhs_; + // Scratch space for cuSOLVER on the GPU. + CudaBuffer<double> device_workspace_; + // Required for error handling with cuSOLVER. + CudaBuffer<int> error_; + // Cache the result of Factorize to ensure that when Solve is called, the + // factiorization of lhs is valid. + LinearSolverTerminationType factorize_result_ = LINEAR_SOLVER_FATAL_ERROR; +}; + +#endif // CERES_NO_CUDA + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_DENSE_CHOLESKY_H_ diff --git a/extern/ceres/internal/ceres/dense_jacobian_writer.h b/extern/ceres/internal/ceres/dense_jacobian_writer.h index 28c60e20a1b..0020937124e 100644 --- a/extern/ceres/internal/ceres/dense_jacobian_writer.h +++ b/extern/ceres/internal/ceres/dense_jacobian_writer.h @@ -33,9 +33,13 @@ #ifndef CERES_INTERNAL_DENSE_JACOBIAN_WRITER_H_ #define CERES_INTERNAL_DENSE_JACOBIAN_WRITER_H_ +#include <memory> + #include "ceres/casts.h" #include "ceres/dense_sparse_matrix.h" +#include "ceres/internal/disable_warnings.h" #include "ceres/internal/eigen.h" +#include "ceres/internal/export.h" #include "ceres/parameter_block.h" #include "ceres/program.h" #include "ceres/residual_block.h" @@ -44,7 +48,7 @@ namespace ceres { namespace internal { -class DenseJacobianWriter { +class CERES_NO_EXPORT DenseJacobianWriter { public: DenseJacobianWriter(Evaluator::Options /* ignored */, Program* program) : program_(program) {} @@ -54,13 +58,14 @@ class DenseJacobianWriter { // Since the dense matrix has different layout than that assumed by the cost // functions, use scratch space to store the jacobians temporarily then copy // them over to the larger jacobian later. - ScratchEvaluatePreparer* CreateEvaluatePreparers(int num_threads) { + std::unique_ptr<ScratchEvaluatePreparer[]> CreateEvaluatePreparers( + int num_threads) { return ScratchEvaluatePreparer::Create(*program_, num_threads); } - SparseMatrix* CreateJacobian() const { - return new DenseSparseMatrix( - program_->NumResiduals(), program_->NumEffectiveParameters(), true); + std::unique_ptr<SparseMatrix> CreateJacobian() const { + return std::make_unique<DenseSparseMatrix>( + program_->NumResiduals(), program_->NumEffectiveParameters()); } void Write(int residual_id, @@ -82,14 +87,14 @@ class DenseJacobianWriter { continue; } - const int parameter_block_size = parameter_block->LocalSize(); + const int parameter_block_size = parameter_block->TangentSize(); ConstMatrixRef parameter_jacobian( jacobians[j], num_residuals, parameter_block_size); - dense_jacobian->mutable_matrix().block(residual_offset, - parameter_block->delta_offset(), - num_residuals, - parameter_block_size) = + dense_jacobian->mutable_matrix()->block(residual_offset, + parameter_block->delta_offset(), + num_residuals, + parameter_block_size) = parameter_jacobian; } } @@ -101,4 +106,6 @@ class DenseJacobianWriter { } // namespace internal } // namespace ceres +#include "ceres/internal/reenable_warnings.h" + #endif // CERES_INTERNAL_DENSE_JACOBIAN_WRITER_H_ diff --git a/extern/ceres/internal/ceres/dense_normal_cholesky_solver.cc b/extern/ceres/internal/ceres/dense_normal_cholesky_solver.cc index 51c639097b6..30a0c023f51 100644 --- a/extern/ceres/internal/ceres/dense_normal_cholesky_solver.cc +++ b/extern/ceres/internal/ceres/dense_normal_cholesky_solver.cc @@ -30,13 +30,11 @@ #include "ceres/dense_normal_cholesky_solver.h" -#include <cstddef> +#include <utility> #include "Eigen/Dense" -#include "ceres/blas.h" #include "ceres/dense_sparse_matrix.h" #include "ceres/internal/eigen.h" -#include "ceres/lapack.h" #include "ceres/linear_solver.h" #include "ceres/types.h" #include "ceres/wall_time.h" @@ -45,32 +43,20 @@ namespace ceres { namespace internal { DenseNormalCholeskySolver::DenseNormalCholeskySolver( - const LinearSolver::Options& options) - : options_(options) {} + LinearSolver::Options options) + : options_(std::move(options)), + cholesky_(DenseCholesky::Create(options_)) {} LinearSolver::Summary DenseNormalCholeskySolver::SolveImpl( DenseSparseMatrix* A, const double* b, const LinearSolver::PerSolveOptions& per_solve_options, double* x) { - if (options_.dense_linear_algebra_library_type == EIGEN) { - return SolveUsingEigen(A, b, per_solve_options, x); - } else { - return SolveUsingLAPACK(A, b, per_solve_options, x); - } -} - -LinearSolver::Summary DenseNormalCholeskySolver::SolveUsingEigen( - DenseSparseMatrix* A, - const double* b, - const LinearSolver::PerSolveOptions& per_solve_options, - double* x) { EventLogger event_logger("DenseNormalCholeskySolver::Solve"); const int num_rows = A->num_rows(); const int num_cols = A->num_cols(); - ConstColMajorMatrixRef Aref = A->matrix(); Matrix lhs(num_cols, num_cols); lhs.setZero(); @@ -81,12 +67,12 @@ LinearSolver::Summary DenseNormalCholeskySolver::SolveUsingEigen( // Using rankUpdate instead of GEMM, exposes the fact that its the // same matrix being multiplied with itself and that the product is // symmetric. - lhs.selfadjointView<Eigen::Upper>().rankUpdate(Aref.transpose()); + lhs.selfadjointView<Eigen::Upper>().rankUpdate(A->matrix().transpose()); // rhs = A'b - Vector rhs = Aref.transpose() * ConstVectorRef(b, num_rows); + Vector rhs = A->matrix().transpose() * ConstVectorRef(b, num_rows); - if (per_solve_options.D != NULL) { + if (per_solve_options.D != nullptr) { ConstVectorRef D(per_solve_options.D, num_cols); lhs += D.array().square().matrix().asDiagonal(); } @@ -94,64 +80,12 @@ LinearSolver::Summary DenseNormalCholeskySolver::SolveUsingEigen( LinearSolver::Summary summary; summary.num_iterations = 1; - summary.termination_type = LINEAR_SOLVER_SUCCESS; - Eigen::LLT<Matrix, Eigen::Upper> llt = - lhs.selfadjointView<Eigen::Upper>().llt(); - - if (llt.info() != Eigen::Success) { - summary.termination_type = LINEAR_SOLVER_FAILURE; - summary.message = "Eigen LLT decomposition failed."; - } else { - summary.termination_type = LINEAR_SOLVER_SUCCESS; - summary.message = "Success."; - } + summary.termination_type = cholesky_->FactorAndSolve( + num_cols, lhs.data(), rhs.data(), x, &summary.message); + event_logger.AddEvent("FactorAndSolve"); - VectorRef(x, num_cols) = llt.solve(rhs); - event_logger.AddEvent("Solve"); return summary; } -LinearSolver::Summary DenseNormalCholeskySolver::SolveUsingLAPACK( - DenseSparseMatrix* A, - const double* b, - const LinearSolver::PerSolveOptions& per_solve_options, - double* x) { - EventLogger event_logger("DenseNormalCholeskySolver::Solve"); - - if (per_solve_options.D != NULL) { - // Temporarily append a diagonal block to the A matrix, but undo - // it before returning the matrix to the user. - A->AppendDiagonal(per_solve_options.D); - } - - const int num_cols = A->num_cols(); - Matrix lhs(num_cols, num_cols); - event_logger.AddEvent("Setup"); - - // lhs = A'A - // - // Note: This is a bit delicate, it assumes that the stride on this - // matrix is the same as the number of rows. - BLAS::SymmetricRankKUpdate( - A->num_rows(), num_cols, A->values(), true, 1.0, 0.0, lhs.data()); - - if (per_solve_options.D != NULL) { - // Undo the modifications to the matrix A. - A->RemoveDiagonal(); - } - - // TODO(sameeragarwal): Replace this with a gemv call for true blasness. - // rhs = A'b - VectorRef(x, num_cols) = - A->matrix().transpose() * ConstVectorRef(b, A->num_rows()); - event_logger.AddEvent("Product"); - - LinearSolver::Summary summary; - summary.num_iterations = 1; - summary.termination_type = LAPACK::SolveInPlaceUsingCholesky( - num_cols, lhs.data(), x, &summary.message); - event_logger.AddEvent("Solve"); - return summary; -} } // namespace internal } // namespace ceres diff --git a/extern/ceres/internal/ceres/dense_normal_cholesky_solver.h b/extern/ceres/internal/ceres/dense_normal_cholesky_solver.h index 68ea611299f..5b3c74069f0 100644 --- a/extern/ceres/internal/ceres/dense_normal_cholesky_solver.h +++ b/extern/ceres/internal/ceres/dense_normal_cholesky_solver.h @@ -34,6 +34,11 @@ #ifndef CERES_INTERNAL_DENSE_NORMAL_CHOLESKY_SOLVER_H_ #define CERES_INTERNAL_DENSE_NORMAL_CHOLESKY_SOLVER_H_ +#include <memory> + +#include "ceres/dense_cholesky.h" +#include "ceres/internal/disable_warnings.h" +#include "ceres/internal/export.h" #include "ceres/linear_solver.h" namespace ceres { @@ -73,9 +78,10 @@ class DenseSparseMatrix; // library. This solver always returns a solution, it is the user's // responsibility to judge if the solution is good enough for their // purposes. -class DenseNormalCholeskySolver : public DenseSparseMatrixSolver { +class CERES_NO_EXPORT DenseNormalCholeskySolver + : public DenseSparseMatrixSolver { public: - explicit DenseNormalCholeskySolver(const LinearSolver::Options& options); + explicit DenseNormalCholeskySolver(LinearSolver::Options options); private: LinearSolver::Summary SolveImpl( @@ -84,22 +90,13 @@ class DenseNormalCholeskySolver : public DenseSparseMatrixSolver { const LinearSolver::PerSolveOptions& per_solve_options, double* x) final; - LinearSolver::Summary SolveUsingLAPACK( - DenseSparseMatrix* A, - const double* b, - const LinearSolver::PerSolveOptions& per_solve_options, - double* x); - - LinearSolver::Summary SolveUsingEigen( - DenseSparseMatrix* A, - const double* b, - const LinearSolver::PerSolveOptions& per_solve_options, - double* x); - const LinearSolver::Options options_; + std::unique_ptr<DenseCholesky> cholesky_; }; } // namespace internal } // namespace ceres +#include "ceres/internal/reenable_warnings.h" + #endif // CERES_INTERNAL_DENSE_NORMAL_CHOLESKY_SOLVER_H_ diff --git a/extern/ceres/internal/ceres/dense_qr.cc b/extern/ceres/internal/ceres/dense_qr.cc new file mode 100644 index 00000000000..4b9c8a4a035 --- /dev/null +++ b/extern/ceres/internal/ceres/dense_qr.cc @@ -0,0 +1,481 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2022 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 "ceres/dense_qr.h" + +#include <algorithm> +#include <memory> +#include <string> +#ifndef CERES_NO_CUDA +#include "ceres/context_impl.h" +#include "cublas_v2.h" +#include "cusolverDn.h" +#endif // CERES_NO_CUDA + +#ifndef CERES_NO_LAPACK + +// LAPACK routines for solving a linear least squares problem using QR +// factorization. This is done in three stages: +// +// A * x = b +// Q * R * x = b (dgeqrf) +// R * x = Q' * b (dormqr) +// x = R^{-1} * Q'* b (dtrtrs) + +// clang-format off + +// Compute the QR factorization of a. +// +// a is an m x n column major matrix (Denoted by "A" in the above description) +// lda is the leading dimension of a. lda >= max(1, num_rows) +// tau is an array of size min(m,n). It contains the scalar factors of the +// elementary reflectors. +// work is an array of size max(1,lwork). On exit, if info=0, work[0] contains +// the optimal size of work. +// +// if lwork >= 1 it is the size of work. If lwork = -1, then a workspace query is assumed. +// dgeqrf computes the optimal size of the work array and returns it as work[0]. +// +// info = 0, successful exit. +// info < 0, if info = -i, then the i^th argument had illegal value. +extern "C" void dgeqrf_(const int* m, const int* n, double* a, const int* lda, + double* tau, double* work, const int* lwork, int* info); + +// Apply Q or Q' to b. +// +// b is a m times n column major matrix. +// size = 'L' applies Q or Q' on the left, size = 'R' applies Q or Q' on the right. +// trans = 'N', applies Q, trans = 'T', applies Q'. +// k is the number of elementary reflectors whose product defines the matrix Q. +// If size = 'L', m >= k >= 0 and if side = 'R', n >= k >= 0. +// a is an lda x k column major matrix containing the reflectors as returned by dgeqrf. +// ldb is the leading dimension of b. +// work is an array of size max(1, lwork) +// lwork if positive is the size of work. If lwork = -1, then a +// workspace query is assumed. +// +// info = 0, successful exit. +// info < 0, if info = -i, then the i^th argument had illegal value. +extern "C" void dormqr_(const char* side, const char* trans, const int* m, + const int* n ,const int* k, double* a, const int* lda, + double* tau, double* b, const int* ldb, double* work, + const int* lwork, int* info); + +// Solve a triangular system of the form A * x = b +// +// uplo = 'U', A is upper triangular. uplo = 'L' is lower triangular. +// trans = 'N', 'T', 'C' specifies the form - A, A^T, A^H. +// DIAG = 'N', A is not unit triangular. 'U' is unit triangular. +// n is the order of the matrix A. +// nrhs number of columns of b. +// a is a column major lda x n. +// b is a column major matrix of ldb x nrhs +// +// info = 0 succesful. +// = -i < 0 i^th argument is an illegal value. +// = i > 0, i^th diagonal element of A is zero. +extern "C" void dtrtrs_(const char* uplo, const char* trans, const char* diag, + const int* n, const int* nrhs, double* a, const int* lda, + double* b, const int* ldb, int* info); +// clang-format on + +#endif + +namespace ceres { +namespace internal { + +DenseQR::~DenseQR() = default; + +std::unique_ptr<DenseQR> DenseQR::Create(const LinearSolver::Options& options) { + std::unique_ptr<DenseQR> dense_qr; + + switch (options.dense_linear_algebra_library_type) { + case EIGEN: + dense_qr = std::make_unique<EigenDenseQR>(); + break; + + case LAPACK: +#ifndef CERES_NO_LAPACK + dense_qr = std::make_unique<LAPACKDenseQR>(); + break; +#else + LOG(FATAL) << "Ceres was compiled without support for LAPACK."; +#endif + + case CUDA: +#ifndef CERES_NO_CUDA + dense_qr = CUDADenseQR::Create(options); + break; +#else + LOG(FATAL) << "Ceres was compiled without support for CUDA."; +#endif + + default: + LOG(FATAL) << "Unknown dense linear algebra library type : " + << DenseLinearAlgebraLibraryTypeToString( + options.dense_linear_algebra_library_type); + } + return dense_qr; +} + +LinearSolverTerminationType DenseQR::FactorAndSolve(int num_rows, + int num_cols, + double* lhs, + const double* rhs, + double* solution, + std::string* message) { + LinearSolverTerminationType termination_type = + Factorize(num_rows, num_cols, lhs, message); + if (termination_type == LINEAR_SOLVER_SUCCESS) { + termination_type = Solve(rhs, solution, message); + } + return termination_type; +} + +LinearSolverTerminationType EigenDenseQR::Factorize(int num_rows, + int num_cols, + double* lhs, + std::string* message) { + Eigen::Map<ColMajorMatrix> m(lhs, num_rows, num_cols); + qr_ = std::make_unique<QRType>(m); + *message = "Success."; + return LINEAR_SOLVER_SUCCESS; +} + +LinearSolverTerminationType EigenDenseQR::Solve(const double* rhs, + double* solution, + std::string* message) { + VectorRef(solution, qr_->cols()) = + qr_->solve(ConstVectorRef(rhs, qr_->rows())); + *message = "Success."; + return LINEAR_SOLVER_SUCCESS; +} + +#ifndef CERES_NO_LAPACK +LinearSolverTerminationType LAPACKDenseQR::Factorize(int num_rows, + int num_cols, + double* lhs, + std::string* message) { + int lwork = -1; + double work_size; + int info = 0; + + // Compute the size of the temporary workspace needed to compute the QR + // factorization in the dgeqrf call below. + dgeqrf_(&num_rows, + &num_cols, + lhs_, + &num_rows, + tau_.data(), + &work_size, + &lwork, + &info); + if (info < 0) { + LOG(FATAL) << "Congratulations, you found a bug in Ceres." + << "Please report it." + << "LAPACK::dgels fatal error." + << "Argument: " << -info << " is invalid."; + } + + lhs_ = lhs; + num_rows_ = num_rows; + num_cols_ = num_cols; + + lwork = static_cast<int>(work_size); + + if (work_.size() < lwork) { + work_.resize(lwork); + } + if (tau_.size() < num_cols) { + tau_.resize(num_cols); + } + + if (q_transpose_rhs_.size() < num_rows) { + q_transpose_rhs_.resize(num_rows); + } + + // Factorize the lhs_ using the workspace that we just constructed above. + dgeqrf_(&num_rows, + &num_cols, + lhs_, + &num_rows, + tau_.data(), + work_.data(), + &lwork, + &info); + + if (info < 0) { + LOG(FATAL) << "Congratulations, you found a bug in Ceres." + << "Please report it. dgeqrf fatal error." + << "Argument: " << -info << " is invalid."; + } + + termination_type_ = LINEAR_SOLVER_SUCCESS; + *message = "Success."; + return termination_type_; +} + +LinearSolverTerminationType LAPACKDenseQR::Solve(const double* rhs, + double* solution, + std::string* message) { + if (termination_type_ != LINEAR_SOLVER_SUCCESS) { + *message = "QR factorization failed and solve called."; + return termination_type_; + } + + std::copy_n(rhs, num_rows_, q_transpose_rhs_.data()); + + const char side = 'L'; + char trans = 'T'; + const int num_c_cols = 1; + const int lwork = work_.size(); + int info = 0; + dormqr_(&side, + &trans, + &num_rows_, + &num_c_cols, + &num_cols_, + lhs_, + &num_rows_, + tau_.data(), + q_transpose_rhs_.data(), + &num_rows_, + work_.data(), + &lwork, + &info); + if (info < 0) { + LOG(FATAL) << "Congratulations, you found a bug in Ceres." + << "Please report it. dormr fatal error." + << "Argument: " << -info << " is invalid."; + } + + const char uplo = 'U'; + trans = 'N'; + const char diag = 'N'; + dtrtrs_(&uplo, + &trans, + &diag, + &num_cols_, + &num_c_cols, + lhs_, + &num_rows_, + q_transpose_rhs_.data(), + &num_rows_, + &info); + + if (info < 0) { + LOG(FATAL) << "Congratulations, you found a bug in Ceres." + << "Please report it. dormr fatal error." + << "Argument: " << -info << " is invalid."; + } else if (info > 0) { + *message = + "QR factorization failure. The factorization is not full rank. R has " + "zeros on the diagonal."; + termination_type_ = LINEAR_SOLVER_FAILURE; + } else { + std::copy_n(q_transpose_rhs_.data(), num_cols_, solution); + termination_type_ = LINEAR_SOLVER_SUCCESS; + } + + return termination_type_; +} + +#endif // CERES_NO_LAPACK + +#ifndef CERES_NO_CUDA + +bool CUDADenseQR::Init(ContextImpl* context, std::string* message) { + if (!context->InitCUDA(message)) { + return false; + } + cublas_handle_ = context->cublas_handle_; + cusolver_handle_ = context->cusolver_handle_; + stream_ = context->stream_; + error_.Reserve(1); + *message = "CUDADenseQR::Init Success."; + return true; +} + +LinearSolverTerminationType CUDADenseQR::Factorize(int num_rows, + int num_cols, + double* lhs, + std::string* message) { + factorize_result_ = LinearSolverTerminationType::LINEAR_SOLVER_FATAL_ERROR; + lhs_.Reserve(num_rows * num_cols); + tau_.Reserve(std::min(num_rows, num_cols)); + num_rows_ = num_rows; + num_cols_ = num_cols; + lhs_.CopyToGpuAsync(lhs, num_rows * num_cols, stream_); + int device_workspace_size = 0; + if (cusolverDnDgeqrf_bufferSize(cusolver_handle_, + num_rows, + num_cols, + lhs_.data(), + num_rows, + &device_workspace_size) != + CUSOLVER_STATUS_SUCCESS) { + *message = "cuSolverDN::cusolverDnDgeqrf_bufferSize failed."; + return LinearSolverTerminationType::LINEAR_SOLVER_FATAL_ERROR; + } + device_workspace_.Reserve(device_workspace_size); + if (cusolverDnDgeqrf(cusolver_handle_, + num_rows, + num_cols, + lhs_.data(), + num_rows, + tau_.data(), + reinterpret_cast<double*>(device_workspace_.data()), + device_workspace_.size(), + error_.data()) != CUSOLVER_STATUS_SUCCESS) { + *message = "cuSolverDN::cusolverDnDgeqrf failed."; + return LinearSolverTerminationType::LINEAR_SOLVER_FATAL_ERROR; + } + if (cudaDeviceSynchronize() != cudaSuccess || + cudaStreamSynchronize(stream_) != cudaSuccess) { + *message = "Cuda device synchronization failed."; + return LinearSolverTerminationType::LINEAR_SOLVER_FATAL_ERROR; + } + int error = 0; + error_.CopyToHost(&error, 1); + if (error < 0) { + LOG(FATAL) << "Congratulations, you found a bug in Ceres - " + << "please report it. " + << "cuSolverDN::cusolverDnDgeqrf fatal error. " + << "Argument: " << -error << " is invalid."; + // The following line is unreachable, but return failure just to be + // pedantic, since the compiler does not know that. + return LinearSolverTerminationType::LINEAR_SOLVER_FATAL_ERROR; + } + + *message = "Success"; + factorize_result_ = LinearSolverTerminationType::LINEAR_SOLVER_SUCCESS; + return LinearSolverTerminationType::LINEAR_SOLVER_SUCCESS; +} + +LinearSolverTerminationType CUDADenseQR::Solve(const double* rhs, + double* solution, + std::string* message) { + if (factorize_result_ != LinearSolverTerminationType::LINEAR_SOLVER_SUCCESS) { + *message = "Factorize did not complete succesfully previously."; + return factorize_result_; + } + rhs_.CopyToGpuAsync(rhs, num_rows_, stream_); + int device_workspace_size = 0; + if (cusolverDnDormqr_bufferSize(cusolver_handle_, + CUBLAS_SIDE_LEFT, + CUBLAS_OP_T, + num_rows_, + 1, + num_cols_, + lhs_.data(), + num_rows_, + tau_.data(), + rhs_.data(), + num_rows_, + &device_workspace_size) != + CUSOLVER_STATUS_SUCCESS) { + *message = "cuSolverDN::cusolverDnDormqr_bufferSize failed."; + return LinearSolverTerminationType::LINEAR_SOLVER_FATAL_ERROR; + } + device_workspace_.Reserve(device_workspace_size); + // Compute rhs = Q^T * rhs, assuming that lhs has already been factorized. + // The result of factorization would have stored Q in a packed form in lhs_. + if (cusolverDnDormqr(cusolver_handle_, + CUBLAS_SIDE_LEFT, + CUBLAS_OP_T, + num_rows_, + 1, + num_cols_, + lhs_.data(), + num_rows_, + tau_.data(), + rhs_.data(), + num_rows_, + reinterpret_cast<double*>(device_workspace_.data()), + device_workspace_.size(), + error_.data()) != CUSOLVER_STATUS_SUCCESS) { + *message = "cuSolverDN::cusolverDnDormqr failed."; + return LinearSolverTerminationType::LINEAR_SOLVER_FATAL_ERROR; + } + int error = 0; + error_.CopyToHost(&error, 1); + if (error < 0) { + LOG(FATAL) << "Congratulations, you found a bug in Ceres. " + << "Please report it." + << "cuSolverDN::cusolverDnDormqr fatal error. " + << "Argument: " << -error << " is invalid."; + } + // Compute the solution vector as x = R \ (Q^T * rhs). Since the previous step + // replaced rhs by (Q^T * rhs), this is just x = R \ rhs. + if (cublasDtrsv(cublas_handle_, + CUBLAS_FILL_MODE_UPPER, + CUBLAS_OP_N, + CUBLAS_DIAG_NON_UNIT, + num_cols_, + lhs_.data(), + num_rows_, + rhs_.data(), + 1) != CUBLAS_STATUS_SUCCESS) { + *message = "cuBLAS::cublasDtrsv failed."; + return LinearSolverTerminationType::LINEAR_SOLVER_FATAL_ERROR; + } + if (cudaDeviceSynchronize() != cudaSuccess || + cudaStreamSynchronize(stream_) != cudaSuccess) { + *message = "Cuda device synchronization failed."; + return LinearSolverTerminationType::LINEAR_SOLVER_FATAL_ERROR; + } + rhs_.CopyToHost(solution, num_cols_); + *message = "Success"; + return LinearSolverTerminationType::LINEAR_SOLVER_SUCCESS; +} + +std::unique_ptr<CUDADenseQR> CUDADenseQR::Create( + const LinearSolver::Options& options) { + if (options.dense_linear_algebra_library_type != CUDA) { + // The user called the wrong factory method. + return nullptr; + } + auto cuda_dense_qr = std::unique_ptr<CUDADenseQR>(new CUDADenseQR()); + std::string cuda_error; + if (cuda_dense_qr->Init(options.context, &cuda_error)) { + return cuda_dense_qr; + } + // Initialization failed, destroy the object (done automatically) and return a + // nullptr. + LOG(ERROR) << "CUDADenseQR::Init failed: " << cuda_error; + return nullptr; +} + +CUDADenseQR::CUDADenseQR() = default; + +#endif // CERES_NO_CUDA + +} // namespace internal +} // namespace ceres diff --git a/extern/ceres/internal/ceres/dense_qr.h b/extern/ceres/internal/ceres/dense_qr.h new file mode 100644 index 00000000000..7a2ffb52ae6 --- /dev/null +++ b/extern/ceres/internal/ceres/dense_qr.h @@ -0,0 +1,207 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2022 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#ifndef CERES_INTERNAL_DENSE_QR_H_ +#define CERES_INTERNAL_DENSE_QR_H_ + +// This include must come before any #ifndef check on Ceres compile options. +// clang-format off +#include "ceres/internal/config.h" +// clang-format on + +#include <memory> +#include <vector> + +#include "Eigen/Dense" +#include "ceres/internal/disable_warnings.h" +#include "ceres/internal/eigen.h" +#include "ceres/internal/export.h" +#include "ceres/linear_solver.h" +#include "glog/logging.h" + +#ifndef CERES_NO_CUDA +#include "ceres/context_impl.h" +#include "ceres/cuda_buffer.h" +#include "cublas_v2.h" +#include "cuda_runtime.h" +#include "cusolverDn.h" +#endif // CERES_NO_CUDA + +namespace ceres { +namespace internal { + +// An interface that abstracts away the internal details of various dense linear +// algebra libraries and offers a simple API for solving dense linear systems +// using a QR factorization. +class CERES_NO_EXPORT DenseQR { + public: + static std::unique_ptr<DenseQR> Create(const LinearSolver::Options& options); + + virtual ~DenseQR(); + + // Computes the QR factorization of the given matrix. + // + // The input matrix lhs is assumed to be a column-major num_rows x num_cols + // matrix. + // + // The input matrix lhs may be modified by the implementation to store the + // factorization, irrespective of whether the factorization succeeds or not. + // As a result it is the user's responsibility to ensure that lhs is valid + // when Solve is called. + virtual LinearSolverTerminationType Factorize(int num_rows, + int num_cols, + double* lhs, + std::string* message) = 0; + + // Computes the solution to the equation + // + // lhs * solution = rhs + // + // Calling Solve without calling Factorize is undefined behaviour. It is the + // user's responsibility to ensure that the input matrix lhs passed to + // Factorize has not been freed/modified when Solve is called. + virtual LinearSolverTerminationType Solve(const double* rhs, + double* solution, + std::string* message) = 0; + + // Convenience method which combines a call to Factorize and Solve. Solve is + // only called if Factorize returns LINEAR_SOLVER_SUCCESS. + // + // The input matrix lhs may be modified by the implementation to store the + // factorization, irrespective of whether the method succeeds or not. It is + // the user's responsibility to ensure that lhs is valid if and when Solve is + // called again after this call. + LinearSolverTerminationType FactorAndSolve(int num_rows, + int num_cols, + double* lhs, + const double* rhs, + double* solution, + std::string* message); +}; + +class CERES_NO_EXPORT EigenDenseQR final : public DenseQR { + public: + LinearSolverTerminationType Factorize(int num_rows, + int num_cols, + double* lhs, + std::string* message) override; + LinearSolverTerminationType Solve(const double* rhs, + double* solution, + std::string* message) override; + + private: + using QRType = Eigen::HouseholderQR<Eigen::Ref<ColMajorMatrix>>; + std::unique_ptr<QRType> qr_; +}; + +#ifndef CERES_NO_LAPACK +class CERES_NO_EXPORT LAPACKDenseQR final : public DenseQR { + public: + LinearSolverTerminationType Factorize(int num_rows, + int num_cols, + double* lhs, + std::string* message) override; + LinearSolverTerminationType Solve(const double* rhs, + double* solution, + std::string* message) override; + + private: + double* lhs_ = nullptr; + int num_rows_; + int num_cols_; + LinearSolverTerminationType termination_type_ = LINEAR_SOLVER_FATAL_ERROR; + Vector work_; + Vector tau_; + Vector q_transpose_rhs_; +}; +#endif // CERES_NO_LAPACK + +#ifndef CERES_NO_CUDA +// Implementation of DenseQR using the 32-bit cuSolverDn interface. A +// requirement for using this solver is that the lhs must not be rank deficient. +// This is because cuSolverDn does not implement the singularity-checking +// wrapper trtrs, hence this solver directly uses trsv from CUBLAS for the +// backsubstitution. +class CERES_NO_EXPORT CUDADenseQR final : public DenseQR { + public: + static std::unique_ptr<CUDADenseQR> Create( + const LinearSolver::Options& options); + CUDADenseQR(const CUDADenseQR&) = delete; + CUDADenseQR& operator=(const CUDADenseQR&) = delete; + LinearSolverTerminationType Factorize(int num_rows, + int num_cols, + double* lhs, + std::string* message) override; + LinearSolverTerminationType Solve(const double* rhs, + double* solution, + std::string* message) override; + + private: + CUDADenseQR(); + // Picks up the cuSolverDN, cuBLAS, and cuStream handles from the context. If + // the context is unable to initialize CUDA, returns false with a + // human-readable message indicating the reason. + bool Init(ContextImpl* context, std::string* message); + + // Handle to the cuSOLVER context. + cusolverDnHandle_t cusolver_handle_ = nullptr; + // Handle to cuBLAS context. + cublasHandle_t cublas_handle_ = nullptr; + // CUDA device stream. + cudaStream_t stream_ = nullptr; + // Number of rowns in the A matrix, to be cached between calls to *Factorize + // and *Solve. + size_t num_rows_ = 0; + // Number of columns in the A matrix, to be cached between calls to *Factorize + // and *Solve. + size_t num_cols_ = 0; + // GPU memory allocated for the A matrix (lhs matrix). + CudaBuffer<double> lhs_; + // GPU memory allocated for the B matrix (rhs vector). + CudaBuffer<double> rhs_; + // GPU memory allocated for the TAU matrix (scaling of householder vectors). + CudaBuffer<double> tau_; + // Scratch space for cuSOLVER on the GPU. + CudaBuffer<double> device_workspace_; + // Required for error handling with cuSOLVER. + CudaBuffer<int> error_; + // Cache the result of Factorize to ensure that when Solve is called, the + // factiorization of lhs is valid. + LinearSolverTerminationType factorize_result_ = LINEAR_SOLVER_FATAL_ERROR; +}; + +#endif // CERES_NO_CUDA + +} // namespace internal +} // namespace ceres + +#include "ceres/internal/reenable_warnings.h" + +#endif // CERES_INTERNAL_DENSE_QR_H_ diff --git a/extern/ceres/internal/ceres/dense_qr_solver.cc b/extern/ceres/internal/ceres/dense_qr_solver.cc index 44388f30aee..24cb25abd8e 100644 --- a/extern/ceres/internal/ceres/dense_qr_solver.cc +++ b/extern/ceres/internal/ceres/dense_qr_solver.cc @@ -33,9 +33,9 @@ #include <cstddef> #include "Eigen/Dense" +#include "ceres/dense_qr.h" #include "ceres/dense_sparse_matrix.h" #include "ceres/internal/eigen.h" -#include "ceres/lapack.h" #include "ceres/linear_solver.h" #include "ceres/types.h" #include "ceres/wall_time.h" @@ -44,124 +44,40 @@ namespace ceres { namespace internal { DenseQRSolver::DenseQRSolver(const LinearSolver::Options& options) - : options_(options) { - work_.resize(1); -} + : options_(options), dense_qr_(DenseQR::Create(options)) {} LinearSolver::Summary DenseQRSolver::SolveImpl( DenseSparseMatrix* A, const double* b, const LinearSolver::PerSolveOptions& per_solve_options, double* x) { - if (options_.dense_linear_algebra_library_type == EIGEN) { - return SolveUsingEigen(A, b, per_solve_options, x); - } else { - return SolveUsingLAPACK(A, b, per_solve_options, x); - } -} - -LinearSolver::Summary DenseQRSolver::SolveUsingLAPACK( - DenseSparseMatrix* A, - const double* b, - const LinearSolver::PerSolveOptions& per_solve_options, - double* x) { EventLogger event_logger("DenseQRSolver::Solve"); const int num_rows = A->num_rows(); const int num_cols = A->num_cols(); + const int num_augmented_rows = + num_rows + ((per_solve_options.D != nullptr) ? num_cols : 0); - if (per_solve_options.D != NULL) { - // Temporarily append a diagonal block to the A matrix, but undo - // it before returning the matrix to the user. - A->AppendDiagonal(per_solve_options.D); + if (lhs_.rows() != num_augmented_rows || lhs_.cols() != num_cols) { + lhs_.resize(num_augmented_rows, num_cols); + rhs_.resize(num_augmented_rows); } - // TODO(sameeragarwal): Since we are copying anyways, the diagonal - // can be appended to the matrix instead of doing it on A. - lhs_ = A->matrix(); - - if (per_solve_options.D != NULL) { - // Undo the modifications to the matrix A. - A->RemoveDiagonal(); - } - - // rhs = [b;0] to account for the additional rows in the lhs. - if (rhs_.rows() != lhs_.rows()) { - rhs_.resize(lhs_.rows()); - } - rhs_.setZero(); + lhs_.topRows(num_rows) = A->matrix(); rhs_.head(num_rows) = ConstVectorRef(b, num_rows); - if (work_.rows() == 1) { - const int work_size = - LAPACK::EstimateWorkSizeForQR(lhs_.rows(), lhs_.cols()); - VLOG(3) << "Working memory for Dense QR factorization: " - << work_size * sizeof(double); - work_.resize(work_size); + if (num_rows != num_augmented_rows) { + lhs_.bottomRows(num_cols) = + ConstVectorRef(per_solve_options.D, num_cols).asDiagonal(); + rhs_.tail(num_cols).setZero(); } LinearSolver::Summary summary; + summary.termination_type = dense_qr_->FactorAndSolve( + lhs_.rows(), lhs_.cols(), lhs_.data(), rhs_.data(), x, &summary.message); summary.num_iterations = 1; - summary.termination_type = LAPACK::SolveInPlaceUsingQR(lhs_.rows(), - lhs_.cols(), - lhs_.data(), - work_.rows(), - work_.data(), - rhs_.data(), - &summary.message); event_logger.AddEvent("Solve"); - if (summary.termination_type == LINEAR_SOLVER_SUCCESS) { - VectorRef(x, num_cols) = rhs_.head(num_cols); - } - - event_logger.AddEvent("TearDown"); - return summary; -} - -LinearSolver::Summary DenseQRSolver::SolveUsingEigen( - DenseSparseMatrix* A, - const double* b, - const LinearSolver::PerSolveOptions& per_solve_options, - double* x) { - EventLogger event_logger("DenseQRSolver::Solve"); - - const int num_rows = A->num_rows(); - const int num_cols = A->num_cols(); - - if (per_solve_options.D != NULL) { - // Temporarily append a diagonal block to the A matrix, but undo - // it before returning the matrix to the user. - A->AppendDiagonal(per_solve_options.D); - } - - // rhs = [b;0] to account for the additional rows in the lhs. - const int augmented_num_rows = - num_rows + ((per_solve_options.D != NULL) ? num_cols : 0); - if (rhs_.rows() != augmented_num_rows) { - rhs_.resize(augmented_num_rows); - rhs_.setZero(); - } - rhs_.head(num_rows) = ConstVectorRef(b, num_rows); - event_logger.AddEvent("Setup"); - - // Solve the system. - VectorRef(x, num_cols) = A->matrix().householderQr().solve(rhs_); - event_logger.AddEvent("Solve"); - - if (per_solve_options.D != NULL) { - // Undo the modifications to the matrix A. - A->RemoveDiagonal(); - } - - // We always succeed, since the QR solver returns the best solution - // it can. It is the job of the caller to determine if the solution - // is good enough or not. - LinearSolver::Summary summary; - summary.num_iterations = 1; - summary.termination_type = LINEAR_SOLVER_SUCCESS; - summary.message = "Success."; - event_logger.AddEvent("TearDown"); return summary; } diff --git a/extern/ceres/internal/ceres/dense_qr_solver.h b/extern/ceres/internal/ceres/dense_qr_solver.h index 980243bd6c6..39922a2692b 100644 --- a/extern/ceres/internal/ceres/dense_qr_solver.h +++ b/extern/ceres/internal/ceres/dense_qr_solver.h @@ -32,8 +32,12 @@ #ifndef CERES_INTERNAL_DENSE_QR_SOLVER_H_ #define CERES_INTERNAL_DENSE_QR_SOLVER_H_ +#include <memory> + +#include "ceres/dense_qr.h" +#include "ceres/internal/disable_warnings.h" #include "ceres/internal/eigen.h" -#include "ceres/internal/port.h" +#include "ceres/internal/export.h" #include "ceres/linear_solver.h" namespace ceres { @@ -79,7 +83,7 @@ class DenseSparseMatrix; // library. This solver always returns a solution, it is the user's // responsibility to judge if the solution is good enough for their // purposes. -class CERES_EXPORT_INTERNAL DenseQRSolver : public DenseSparseMatrixSolver { +class CERES_NO_EXPORT DenseQRSolver final : public DenseSparseMatrixSolver { public: explicit DenseQRSolver(const LinearSolver::Options& options); @@ -105,10 +109,12 @@ class CERES_EXPORT_INTERNAL DenseQRSolver : public DenseSparseMatrixSolver { const LinearSolver::Options options_; ColMajorMatrix lhs_; Vector rhs_; - Vector work_; + std::unique_ptr<DenseQR> dense_qr_; }; } // namespace internal } // namespace ceres +#include "ceres/internal/reenable_warnings.h" + #endif // CERES_INTERNAL_DENSE_QR_SOLVER_H_ diff --git a/extern/ceres/internal/ceres/dense_sparse_matrix.cc b/extern/ceres/internal/ceres/dense_sparse_matrix.cc index 53207fe300e..8b967f2ade7 100644 --- a/extern/ceres/internal/ceres/dense_sparse_matrix.cc +++ b/extern/ceres/internal/ceres/dense_sparse_matrix.cc @@ -31,9 +31,10 @@ #include "ceres/dense_sparse_matrix.h" #include <algorithm> +#include <utility> #include "ceres/internal/eigen.h" -#include "ceres/internal/port.h" +#include "ceres/internal/export.h" #include "ceres/triplet_sparse_matrix.h" #include "glog/logging.h" @@ -41,28 +42,10 @@ namespace ceres { namespace internal { DenseSparseMatrix::DenseSparseMatrix(int num_rows, int num_cols) - : has_diagonal_appended_(false), has_diagonal_reserved_(false) { - m_.resize(num_rows, num_cols); - m_.setZero(); -} - -DenseSparseMatrix::DenseSparseMatrix(int num_rows, - int num_cols, - bool reserve_diagonal) - : has_diagonal_appended_(false), has_diagonal_reserved_(reserve_diagonal) { - if (reserve_diagonal) { - // Allocate enough space for the diagonal. - m_.resize(num_rows + num_cols, num_cols); - } else { - m_.resize(num_rows, num_cols); - } - m_.setZero(); -} + : m_(Matrix(num_rows, num_cols)) {} DenseSparseMatrix::DenseSparseMatrix(const TripletSparseMatrix& m) - : m_(Eigen::MatrixXd::Zero(m.num_rows(), m.num_cols())), - has_diagonal_appended_(false), - has_diagonal_reserved_(false) { + : m_(Matrix::Zero(m.num_rows(), m.num_cols())) { const double* values = m.values(); const int* rows = m.rows(); const int* cols = m.cols(); @@ -73,8 +56,7 @@ DenseSparseMatrix::DenseSparseMatrix(const TripletSparseMatrix& m) } } -DenseSparseMatrix::DenseSparseMatrix(const ColMajorMatrix& m) - : m_(m), has_diagonal_appended_(false), has_diagonal_reserved_(false) {} +DenseSparseMatrix::DenseSparseMatrix(Matrix m) : m_(std::move(m)) {} void DenseSparseMatrix::SetZero() { m_.setZero(); } @@ -96,72 +78,22 @@ void DenseSparseMatrix::ScaleColumns(const double* scale) { } void DenseSparseMatrix::ToDenseMatrix(Matrix* dense_matrix) const { - *dense_matrix = m_.block(0, 0, num_rows(), num_cols()); + *dense_matrix = m_; } -void DenseSparseMatrix::AppendDiagonal(double* d) { - CHECK(!has_diagonal_appended_); - if (!has_diagonal_reserved_) { - ColMajorMatrix tmp = m_; - m_.resize(m_.rows() + m_.cols(), m_.cols()); - m_.setZero(); - m_.block(0, 0, tmp.rows(), tmp.cols()) = tmp; - has_diagonal_reserved_ = true; - } - - m_.bottomLeftCorner(m_.cols(), m_.cols()) = - ConstVectorRef(d, m_.cols()).asDiagonal(); - has_diagonal_appended_ = true; -} - -void DenseSparseMatrix::RemoveDiagonal() { - CHECK(has_diagonal_appended_); - has_diagonal_appended_ = false; - // Leave the diagonal reserved. -} - -int DenseSparseMatrix::num_rows() const { - if (has_diagonal_reserved_ && !has_diagonal_appended_) { - return m_.rows() - m_.cols(); - } - return m_.rows(); -} +int DenseSparseMatrix::num_rows() const { return m_.rows(); } int DenseSparseMatrix::num_cols() const { return m_.cols(); } -int DenseSparseMatrix::num_nonzeros() const { - if (has_diagonal_reserved_ && !has_diagonal_appended_) { - return (m_.rows() - m_.cols()) * m_.cols(); - } - return m_.rows() * m_.cols(); -} +int DenseSparseMatrix::num_nonzeros() const { return m_.rows() * m_.cols(); } -ConstColMajorMatrixRef DenseSparseMatrix::matrix() const { - return ConstColMajorMatrixRef( - m_.data(), - ((has_diagonal_reserved_ && !has_diagonal_appended_) - ? m_.rows() - m_.cols() - : m_.rows()), - m_.cols(), - Eigen::Stride<Eigen::Dynamic, 1>(m_.rows(), 1)); -} +const Matrix& DenseSparseMatrix::matrix() const { return m_; } -ColMajorMatrixRef DenseSparseMatrix::mutable_matrix() { - return ColMajorMatrixRef(m_.data(), - ((has_diagonal_reserved_ && !has_diagonal_appended_) - ? m_.rows() - m_.cols() - : m_.rows()), - m_.cols(), - Eigen::Stride<Eigen::Dynamic, 1>(m_.rows(), 1)); -} +Matrix* DenseSparseMatrix::mutable_matrix() { return &m_; } void DenseSparseMatrix::ToTextFile(FILE* file) const { CHECK(file != nullptr); - const int active_rows = (has_diagonal_reserved_ && !has_diagonal_appended_) - ? (m_.rows() - m_.cols()) - : m_.rows(); - - for (int r = 0; r < active_rows; ++r) { + for (int r = 0; r < m_.rows(); ++r) { for (int c = 0; c < m_.cols(); ++c) { fprintf(file, "% 10d % 10d %17f\n", r, c, m_(r, c)); } diff --git a/extern/ceres/internal/ceres/dense_sparse_matrix.h b/extern/ceres/internal/ceres/dense_sparse_matrix.h index 94064b3eddc..655cbb8a3db 100644 --- a/extern/ceres/internal/ceres/dense_sparse_matrix.h +++ b/extern/ceres/internal/ceres/dense_sparse_matrix.h @@ -1,5 +1,5 @@ // Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2015 Google Inc. All rights reserved. +// Copyright 2022 Google Inc. All rights reserved. // http://ceres-solver.org/ // // Redistribution and use in source and binary forms, with or without @@ -33,8 +33,9 @@ #ifndef CERES_INTERNAL_DENSE_SPARSE_MATRIX_H_ #define CERES_INTERNAL_DENSE_SPARSE_MATRIX_H_ +#include "ceres/internal/disable_warnings.h" #include "ceres/internal/eigen.h" -#include "ceres/internal/port.h" +#include "ceres/internal/export.h" #include "ceres/sparse_matrix.h" #include "ceres/types.h" @@ -43,17 +44,13 @@ namespace internal { class TripletSparseMatrix; -class CERES_EXPORT_INTERNAL DenseSparseMatrix : public SparseMatrix { +class CERES_NO_EXPORT DenseSparseMatrix final : public SparseMatrix { public: // Build a matrix with the same content as the TripletSparseMatrix // m. This assumes that m does not have any repeated entries. explicit DenseSparseMatrix(const TripletSparseMatrix& m); - explicit DenseSparseMatrix(const ColMajorMatrix& m); - + explicit DenseSparseMatrix(Matrix m); DenseSparseMatrix(int num_rows, int num_cols); - DenseSparseMatrix(int num_rows, int num_cols, bool reserve_diagonal); - - virtual ~DenseSparseMatrix() {} // SparseMatrix interface. void SetZero() final; @@ -69,40 +66,16 @@ class CERES_EXPORT_INTERNAL DenseSparseMatrix : public SparseMatrix { const double* values() const final { return m_.data(); } double* mutable_values() final { return m_.data(); } - ConstColMajorMatrixRef matrix() const; - ColMajorMatrixRef mutable_matrix(); - - // Only one diagonal can be appended at a time. The diagonal is appended to - // as a new set of rows, e.g. - // - // Original matrix: - // - // x x x - // x x x - // x x x - // - // After append diagonal (1, 2, 3): - // - // x x x - // x x x - // x x x - // 1 0 0 - // 0 2 0 - // 0 0 3 - // - // Calling RemoveDiagonal removes the block. It is a fatal error to append a - // diagonal to a matrix that already has an appended diagonal, and it is also - // a fatal error to remove a diagonal from a matrix that has none. - void AppendDiagonal(double* d); - void RemoveDiagonal(); + const Matrix& matrix() const; + Matrix* mutable_matrix(); private: - ColMajorMatrix m_; - bool has_diagonal_appended_; - bool has_diagonal_reserved_; + Matrix m_; }; } // namespace internal } // namespace ceres +#include "ceres/internal/reenable_warnings.h" + #endif // CERES_INTERNAL_DENSE_SPARSE_MATRIX_H_ diff --git a/extern/ceres/internal/ceres/detect_structure.h b/extern/ceres/internal/ceres/detect_structure.h index 06242307ca8..6151c042256 100644 --- a/extern/ceres/internal/ceres/detect_structure.h +++ b/extern/ceres/internal/ceres/detect_structure.h @@ -32,7 +32,8 @@ #define CERES_INTERNAL_DETECT_STRUCTURE_H_ #include "ceres/block_structure.h" -#include "ceres/internal/port.h" +#include "ceres/internal/disable_warnings.h" +#include "ceres/internal/export.h" namespace ceres { namespace internal { @@ -56,13 +57,15 @@ namespace internal { // Note: The structure of rows without any e-blocks has no effect on // the values returned by this function. It is entirely possible that // the f_block_size and row_blocks_size is not constant in such rows. -void CERES_EXPORT DetectStructure(const CompressedRowBlockStructure& bs, - const int num_eliminate_blocks, - int* row_block_size, - int* e_block_size, - int* f_block_size); +void CERES_NO_EXPORT DetectStructure(const CompressedRowBlockStructure& bs, + const int num_eliminate_blocks, + int* row_block_size, + int* e_block_size, + int* f_block_size); } // namespace internal } // namespace ceres +#include "ceres/internal/reenable_warnings.h" + #endif // CERES_INTERNAL_DETECT_STRUCTURE_H_ diff --git a/extern/ceres/internal/ceres/dogleg_strategy.cc b/extern/ceres/internal/ceres/dogleg_strategy.cc index 03ae22f7e57..65f7ccd8480 100644 --- a/extern/ceres/internal/ceres/dogleg_strategy.cc +++ b/extern/ceres/internal/ceres/dogleg_strategy.cc @@ -480,7 +480,7 @@ bool DoglegStrategy::FindMinimumOnTrustRegionBoundary(Vector2d* minimum) const { // Find the real parts y_i of its roots (not only the real roots). Vector roots_real; - if (!FindPolynomialRoots(polynomial, &roots_real, NULL)) { + if (!FindPolynomialRoots(polynomial, &roots_real, nullptr)) { // Failed to find the roots of the polynomial, i.e. the candidate // solutions of the constrained problem. Report this back to the caller. return false; diff --git a/extern/ceres/internal/ceres/dogleg_strategy.h b/extern/ceres/internal/ceres/dogleg_strategy.h index cc3778ea2a0..1d219afe8bc 100644 --- a/extern/ceres/internal/ceres/dogleg_strategy.h +++ b/extern/ceres/internal/ceres/dogleg_strategy.h @@ -31,7 +31,8 @@ #ifndef CERES_INTERNAL_DOGLEG_STRATEGY_H_ #define CERES_INTERNAL_DOGLEG_STRATEGY_H_ -#include "ceres/internal/port.h" +#include "ceres/internal/disable_warnings.h" +#include "ceres/internal/export.h" #include "ceres/linear_solver.h" #include "ceres/trust_region_strategy.h" @@ -53,10 +54,9 @@ namespace internal { // DoglegStrategy follows the approach by Shultz, Schnabel, Byrd. // This finds the exact optimum over the two-dimensional subspace // spanned by the two Dogleg vectors. -class CERES_EXPORT_INTERNAL DoglegStrategy : public TrustRegionStrategy { +class CERES_NO_EXPORT DoglegStrategy final : public TrustRegionStrategy { public: explicit DoglegStrategy(const TrustRegionStrategy::Options& options); - virtual ~DoglegStrategy() {} // TrustRegionStrategy interface Summary ComputeStep(const PerSolveOptions& per_solve_options, @@ -65,7 +65,7 @@ class CERES_EXPORT_INTERNAL DoglegStrategy : public TrustRegionStrategy { double* step) final; void StepAccepted(double step_quality) final; void StepRejected(double step_quality) final; - void StepIsInvalid(); + void StepIsInvalid() override; double Radius() const final; // These functions are predominantly for testing. @@ -76,8 +76,8 @@ class CERES_EXPORT_INTERNAL DoglegStrategy : public TrustRegionStrategy { Matrix subspace_B() const { return subspace_B_; } private: - typedef Eigen::Matrix<double, 2, 1, Eigen::DontAlign> Vector2d; - typedef Eigen::Matrix<double, 2, 2, Eigen::DontAlign> Matrix2d; + using Vector2d = Eigen::Matrix<double, 2, 1, Eigen::DontAlign>; + using Matrix2d = Eigen::Matrix<double, 2, 2, Eigen::DontAlign>; LinearSolver::Summary ComputeGaussNewtonStep( const PerSolveOptions& per_solve_options, @@ -162,4 +162,6 @@ class CERES_EXPORT_INTERNAL DoglegStrategy : public TrustRegionStrategy { } // namespace internal } // namespace ceres +#include "ceres/internal/reenable_warnings.h" + #endif // CERES_INTERNAL_DOGLEG_STRATEGY_H_ diff --git a/extern/ceres/internal/ceres/dynamic_compressed_row_finalizer.h b/extern/ceres/internal/ceres/dynamic_compressed_row_finalizer.h index 30c98d86b6f..fedee3b7a83 100644 --- a/extern/ceres/internal/ceres/dynamic_compressed_row_finalizer.h +++ b/extern/ceres/internal/ceres/dynamic_compressed_row_finalizer.h @@ -33,13 +33,14 @@ #include "ceres/casts.h" #include "ceres/dynamic_compressed_row_sparse_matrix.h" +#include "ceres/internal/export.h" namespace ceres { namespace internal { -struct DynamicCompressedRowJacobianFinalizer { +struct CERES_NO_EXPORT DynamicCompressedRowJacobianFinalizer { void operator()(SparseMatrix* base_jacobian, int num_parameters) { - DynamicCompressedRowSparseMatrix* jacobian = + auto* jacobian = down_cast<DynamicCompressedRowSparseMatrix*>(base_jacobian); jacobian->Finalize(num_parameters); } diff --git a/extern/ceres/internal/ceres/dynamic_compressed_row_jacobian_writer.cc b/extern/ceres/internal/ceres/dynamic_compressed_row_jacobian_writer.cc index 1749449043e..8c254e98f46 100644 --- a/extern/ceres/internal/ceres/dynamic_compressed_row_jacobian_writer.cc +++ b/extern/ceres/internal/ceres/dynamic_compressed_row_jacobian_writer.cc @@ -1,5 +1,5 @@ // Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2015 Google Inc. All rights reserved. +// Copyright 2022 Google Inc. All rights reserved. // http://ceres-solver.org/ // // Redistribution and use in source and binary forms, with or without @@ -30,6 +30,8 @@ #include "ceres/dynamic_compressed_row_jacobian_writer.h" +#include <memory> + #include "ceres/casts.h" #include "ceres/compressed_row_jacobian_writer.h" #include "ceres/dynamic_compressed_row_sparse_matrix.h" @@ -43,25 +45,24 @@ namespace internal { using std::pair; using std::vector; -ScratchEvaluatePreparer* +std::unique_ptr<ScratchEvaluatePreparer[]> DynamicCompressedRowJacobianWriter::CreateEvaluatePreparers(int num_threads) { return ScratchEvaluatePreparer::Create(*program_, num_threads); } -SparseMatrix* DynamicCompressedRowJacobianWriter::CreateJacobian() const { - DynamicCompressedRowSparseMatrix* jacobian = - new DynamicCompressedRowSparseMatrix(program_->NumResiduals(), - program_->NumEffectiveParameters(), - 0 /* max_num_nonzeros */); - return jacobian; +std::unique_ptr<SparseMatrix> +DynamicCompressedRowJacobianWriter::CreateJacobian() const { + return std::make_unique<DynamicCompressedRowSparseMatrix>( + program_->NumResiduals(), + program_->NumEffectiveParameters(), + 0 /* max_num_nonzeros */); } void DynamicCompressedRowJacobianWriter::Write(int residual_id, int residual_offset, double** jacobians, SparseMatrix* base_jacobian) { - DynamicCompressedRowSparseMatrix* jacobian = - down_cast<DynamicCompressedRowSparseMatrix*>(base_jacobian); + auto* jacobian = down_cast<DynamicCompressedRowSparseMatrix*>(base_jacobian); // Get the `residual_block` of interest. const ResidualBlock* residual_block = @@ -77,12 +78,11 @@ void DynamicCompressedRowJacobianWriter::Write(int residual_id, jacobian->ClearRows(residual_offset, num_residuals); // Iterate over each parameter block. - for (int i = 0; i < evaluated_jacobian_blocks.size(); ++i) { + for (const auto& evaluated_jacobian_block : evaluated_jacobian_blocks) { const ParameterBlock* parameter_block = - program_->parameter_blocks()[evaluated_jacobian_blocks[i].first]; - const int parameter_block_jacobian_index = - evaluated_jacobian_blocks[i].second; - const int parameter_block_size = parameter_block->LocalSize(); + program_->parameter_blocks()[evaluated_jacobian_block.first]; + const int parameter_block_jacobian_index = evaluated_jacobian_block.second; + const int parameter_block_size = parameter_block->TangentSize(); const double* parameter_jacobian = jacobians[parameter_block_jacobian_index]; diff --git a/extern/ceres/internal/ceres/dynamic_compressed_row_jacobian_writer.h b/extern/ceres/internal/ceres/dynamic_compressed_row_jacobian_writer.h index ef8fa25d7d4..794a9b4c1e6 100644 --- a/extern/ceres/internal/ceres/dynamic_compressed_row_jacobian_writer.h +++ b/extern/ceres/internal/ceres/dynamic_compressed_row_jacobian_writer.h @@ -34,7 +34,10 @@ #ifndef CERES_INTERNAL_DYNAMIC_COMPRESSED_ROW_JACOBIAN_WRITER_H_ #define CERES_INTERNAL_DYNAMIC_COMPRESSED_ROW_JACOBIAN_WRITER_H_ +#include <memory> + #include "ceres/evaluator.h" +#include "ceres/internal/export.h" #include "ceres/scratch_evaluate_preparer.h" namespace ceres { @@ -43,7 +46,7 @@ namespace internal { class Program; class SparseMatrix; -class DynamicCompressedRowJacobianWriter { +class CERES_NO_EXPORT DynamicCompressedRowJacobianWriter { public: DynamicCompressedRowJacobianWriter(Evaluator::Options /* ignored */, Program* program) @@ -55,12 +58,13 @@ class DynamicCompressedRowJacobianWriter { // the cost functions. The scratch space is therefore used to store // the jacobians (including zeros) temporarily before only the non-zero // entries are copied over to the larger jacobian in `Write`. - ScratchEvaluatePreparer* CreateEvaluatePreparers(int num_threads); + std::unique_ptr<ScratchEvaluatePreparer[]> CreateEvaluatePreparers( + int num_threads); // Return a `DynamicCompressedRowSparseMatrix` which is filled by // `Write`. Note that `Finalize` must be called to make the // `CompressedRowSparseMatrix` interface valid. - SparseMatrix* CreateJacobian() const; + std::unique_ptr<SparseMatrix> CreateJacobian() const; // Write only the non-zero jacobian entries for a residual block // (specified by `residual_id`) into `base_jacobian`, starting at the row diff --git a/extern/ceres/internal/ceres/dynamic_compressed_row_sparse_matrix.cc b/extern/ceres/internal/ceres/dynamic_compressed_row_sparse_matrix.cc index 936e682b763..7185e14a411 100644 --- a/extern/ceres/internal/ceres/dynamic_compressed_row_sparse_matrix.cc +++ b/extern/ceres/internal/ceres/dynamic_compressed_row_sparse_matrix.cc @@ -1,5 +1,5 @@ // Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2015 Google Inc. All rights reserved. +// Copyright 2022 Google Inc. All rights reserved. // http://ceres-solver.org/ // // Redistribution and use in source and binary forms, with or without @@ -70,8 +70,8 @@ void DynamicCompressedRowSparseMatrix::Finalize(int num_additional_elements) { // Count the number of non-zeros and resize `cols_` and `values_`. int num_jacobian_nonzeros = 0; - for (int i = 0; i < dynamic_cols_.size(); ++i) { - num_jacobian_nonzeros += dynamic_cols_[i].size(); + for (const auto& dynamic_col : dynamic_cols_) { + num_jacobian_nonzeros += dynamic_col.size(); } SetMaxNumNonZeros(num_jacobian_nonzeros + num_additional_elements); diff --git a/extern/ceres/internal/ceres/dynamic_compressed_row_sparse_matrix.h b/extern/ceres/internal/ceres/dynamic_compressed_row_sparse_matrix.h index d06c36ebb94..5b4c402f830 100644 --- a/extern/ceres/internal/ceres/dynamic_compressed_row_sparse_matrix.h +++ b/extern/ceres/internal/ceres/dynamic_compressed_row_sparse_matrix.h @@ -44,12 +44,13 @@ #include <vector> #include "ceres/compressed_row_sparse_matrix.h" -#include "ceres/internal/port.h" +#include "ceres/internal/disable_warnings.h" +#include "ceres/internal/export.h" namespace ceres { namespace internal { -class CERES_EXPORT_INTERNAL DynamicCompressedRowSparseMatrix +class CERES_NO_EXPORT DynamicCompressedRowSparseMatrix final : public CompressedRowSparseMatrix { public: // Set the number of rows and columns for the underlyig @@ -100,4 +101,6 @@ class CERES_EXPORT_INTERNAL DynamicCompressedRowSparseMatrix } // namespace internal } // namespace ceres +#include "ceres/internal/reenable_warnings.h" + #endif // CERES_INTERNAL_DYNAMIC_COMPRESSED_ROW_SPARSE_MATRIX_H_ diff --git a/extern/ceres/internal/ceres/dynamic_sparse_normal_cholesky_solver.cc b/extern/ceres/internal/ceres/dynamic_sparse_normal_cholesky_solver.cc index d31c4228f05..5e907e18d51 100644 --- a/extern/ceres/internal/ceres/dynamic_sparse_normal_cholesky_solver.cc +++ b/extern/ceres/internal/ceres/dynamic_sparse_normal_cholesky_solver.cc @@ -35,10 +35,12 @@ #include <ctime> #include <memory> #include <sstream> +#include <utility> #include "Eigen/SparseCore" #include "ceres/compressed_row_sparse_matrix.h" #include "ceres/cxsparse.h" +#include "ceres/internal/config.h" #include "ceres/internal/eigen.h" #include "ceres/linear_solver.h" #include "ceres/suitesparse.h" @@ -54,8 +56,8 @@ namespace ceres { namespace internal { DynamicSparseNormalCholeskySolver::DynamicSparseNormalCholeskySolver( - const LinearSolver::Options& options) - : options_(options) {} + LinearSolver::Options options) + : options_(std::move(options)) {} LinearSolver::Summary DynamicSparseNormalCholeskySolver::SolveImpl( CompressedRowSparseMatrix* A, @@ -71,11 +73,11 @@ LinearSolver::Summary DynamicSparseNormalCholeskySolver::SolveImpl( // it before returning the matrix to the user. std::unique_ptr<CompressedRowSparseMatrix> regularizer; if (!A->col_blocks().empty()) { - regularizer.reset(CompressedRowSparseMatrix::CreateBlockDiagonalMatrix( - per_solve_options.D, A->col_blocks())); + regularizer = CompressedRowSparseMatrix::CreateBlockDiagonalMatrix( + per_solve_options.D, A->col_blocks()); } else { - regularizer.reset( - new CompressedRowSparseMatrix(per_solve_options.D, num_cols)); + regularizer = std::make_unique<CompressedRowSparseMatrix>( + per_solve_options.D, num_cols); } A->AppendRows(*regularizer); } @@ -123,12 +125,13 @@ LinearSolver::Summary DynamicSparseNormalCholeskySolver::SolveImplUsingEigen( EventLogger event_logger("DynamicSparseNormalCholeskySolver::Eigen::Solve"); - Eigen::MappedSparseMatrix<double, Eigen::RowMajor> a(A->num_rows(), - A->num_cols(), - A->num_nonzeros(), - A->mutable_rows(), - A->mutable_cols(), - A->mutable_values()); + Eigen::Map<Eigen::SparseMatrix<double, Eigen::RowMajor>> a( + A->num_rows(), + A->num_cols(), + A->num_nonzeros(), + A->mutable_rows(), + A->mutable_cols(), + A->mutable_values()); Eigen::SparseMatrix<double> lhs = a.transpose() * a; Eigen::SimplicialLDLT<Eigen::SparseMatrix<double>> solver; diff --git a/extern/ceres/internal/ceres/dynamic_sparse_normal_cholesky_solver.h b/extern/ceres/internal/ceres/dynamic_sparse_normal_cholesky_solver.h index 36118bab1a5..6f73c961212 100644 --- a/extern/ceres/internal/ceres/dynamic_sparse_normal_cholesky_solver.h +++ b/extern/ceres/internal/ceres/dynamic_sparse_normal_cholesky_solver.h @@ -36,9 +36,10 @@ // This include must come before any #ifndef check on Ceres compile options. // clang-format off -#include "ceres/internal/port.h" +#include "ceres/internal/config.h" // clang-format on +#include "ceres/internal/export.h" #include "ceres/linear_solver.h" namespace ceres { @@ -53,12 +54,10 @@ class CompressedRowSparseMatrix; // // TODO(alex): Add support for Accelerate sparse solvers: // https://github.com/ceres-solver/ceres-solver/issues/397 -class DynamicSparseNormalCholeskySolver +class CERES_NO_EXPORT DynamicSparseNormalCholeskySolver : public CompressedRowSparseMatrixSolver { public: - explicit DynamicSparseNormalCholeskySolver( - const LinearSolver::Options& options); - virtual ~DynamicSparseNormalCholeskySolver() {} + explicit DynamicSparseNormalCholeskySolver(LinearSolver::Options options); private: LinearSolver::Summary SolveImpl(CompressedRowSparseMatrix* A, diff --git a/extern/ceres/internal/ceres/eigensparse.cc b/extern/ceres/internal/ceres/eigensparse.cc index 22ed2c43b5d..81668c82e67 100644 --- a/extern/ceres/internal/ceres/eigensparse.cc +++ b/extern/ceres/internal/ceres/eigensparse.cc @@ -30,6 +30,8 @@ #include "ceres/eigensparse.h" +#include <memory> + #ifdef CERES_USE_EIGEN_SPARSE #include <sstream> @@ -45,10 +47,9 @@ namespace internal { // TODO(sameeragarwal): Use enable_if to clean up the implementations // for when Scalar == double. template <typename Solver> -class EigenSparseCholeskyTemplate : public SparseCholesky { +class EigenSparseCholeskyTemplate final : public SparseCholesky { public: - EigenSparseCholeskyTemplate() : analyzed_(false) {} - virtual ~EigenSparseCholeskyTemplate() {} + EigenSparseCholeskyTemplate() = default; CompressedRowSparseMatrix::StorageType StorageType() const final { return CompressedRowSparseMatrix::LOWER_TRIANGULAR; } @@ -83,7 +84,7 @@ class EigenSparseCholeskyTemplate : public SparseCholesky { LinearSolverTerminationType Solve(const double* rhs_ptr, double* solution_ptr, - std::string* message) { + std::string* message) override { CHECK(analyzed_) << "Solve called without a call to Factorize first."; scalar_rhs_ = ConstVectorRef(rhs_ptr, solver_.cols()) @@ -109,7 +110,7 @@ class EigenSparseCholeskyTemplate : public SparseCholesky { std::string* message) final { CHECK_EQ(lhs->storage_type(), StorageType()); - typename Solver::Scalar* values_ptr = NULL; + typename Solver::Scalar* values_ptr = nullptr; if (std::is_same<typename Solver::Scalar, double>::value) { values_ptr = reinterpret_cast<typename Solver::Scalar*>(lhs->mutable_values()); @@ -122,7 +123,7 @@ class EigenSparseCholeskyTemplate : public SparseCholesky { values_ptr = values_.data(); } - Eigen::MappedSparseMatrix<typename Solver::Scalar, Eigen::ColMajor> + Eigen::Map<Eigen::SparseMatrix<typename Solver::Scalar, Eigen::ColMajor>> eigen_lhs(lhs->num_rows(), lhs->num_rows(), lhs->num_nonzeros(), @@ -135,54 +136,46 @@ class EigenSparseCholeskyTemplate : public SparseCholesky { private: Eigen::Matrix<typename Solver::Scalar, Eigen::Dynamic, 1> values_, scalar_rhs_, scalar_solution_; - bool analyzed_; + bool analyzed_{false}; Solver solver_; }; std::unique_ptr<SparseCholesky> EigenSparseCholesky::Create( const OrderingType ordering_type) { - std::unique_ptr<SparseCholesky> sparse_cholesky; - - typedef Eigen::SimplicialLDLT<Eigen::SparseMatrix<double>, - Eigen::Upper, - Eigen::AMDOrdering<int>> - WithAMDOrdering; - typedef Eigen::SimplicialLDLT<Eigen::SparseMatrix<double>, - Eigen::Upper, - Eigen::NaturalOrdering<int>> - WithNaturalOrdering; + using WithAMDOrdering = Eigen::SimplicialLDLT<Eigen::SparseMatrix<double>, + Eigen::Upper, + Eigen::AMDOrdering<int>>; + using WithNaturalOrdering = + Eigen::SimplicialLDLT<Eigen::SparseMatrix<double>, + Eigen::Upper, + Eigen::NaturalOrdering<int>>; + if (ordering_type == AMD) { - sparse_cholesky.reset(new EigenSparseCholeskyTemplate<WithAMDOrdering>()); + return std::make_unique<EigenSparseCholeskyTemplate<WithAMDOrdering>>(); } else { - sparse_cholesky.reset( - new EigenSparseCholeskyTemplate<WithNaturalOrdering>()); + return std::make_unique<EigenSparseCholeskyTemplate<WithNaturalOrdering>>(); } - return sparse_cholesky; } -EigenSparseCholesky::~EigenSparseCholesky() {} +EigenSparseCholesky::~EigenSparseCholesky() = default; std::unique_ptr<SparseCholesky> FloatEigenSparseCholesky::Create( const OrderingType ordering_type) { - std::unique_ptr<SparseCholesky> sparse_cholesky; - typedef Eigen::SimplicialLDLT<Eigen::SparseMatrix<float>, - Eigen::Upper, - Eigen::AMDOrdering<int>> - WithAMDOrdering; - typedef Eigen::SimplicialLDLT<Eigen::SparseMatrix<float>, - Eigen::Upper, - Eigen::NaturalOrdering<int>> - WithNaturalOrdering; + using WithAMDOrdering = Eigen::SimplicialLDLT<Eigen::SparseMatrix<float>, + Eigen::Upper, + Eigen::AMDOrdering<int>>; + using WithNaturalOrdering = + Eigen::SimplicialLDLT<Eigen::SparseMatrix<float>, + Eigen::Upper, + Eigen::NaturalOrdering<int>>; if (ordering_type == AMD) { - sparse_cholesky.reset(new EigenSparseCholeskyTemplate<WithAMDOrdering>()); + return std::make_unique<EigenSparseCholeskyTemplate<WithAMDOrdering>>(); } else { - sparse_cholesky.reset( - new EigenSparseCholeskyTemplate<WithNaturalOrdering>()); + return std::make_unique<EigenSparseCholeskyTemplate<WithNaturalOrdering>>(); } - return sparse_cholesky; } -FloatEigenSparseCholesky::~FloatEigenSparseCholesky() {} +FloatEigenSparseCholesky::~FloatEigenSparseCholesky() = default; } // namespace internal } // namespace ceres diff --git a/extern/ceres/internal/ceres/eigensparse.h b/extern/ceres/internal/ceres/eigensparse.h index bb89c2c11ac..c4a4142e586 100644 --- a/extern/ceres/internal/ceres/eigensparse.h +++ b/extern/ceres/internal/ceres/eigensparse.h @@ -34,7 +34,7 @@ #define CERES_INTERNAL_EIGENSPARSE_H_ // This include must come before any #ifndef check on Ceres compile options. -#include "ceres/internal/port.h" +#include "ceres/internal/config.h" #ifdef CERES_USE_EIGEN_SPARSE @@ -42,44 +42,45 @@ #include <string> #include "Eigen/SparseCore" +#include "ceres/internal/export.h" #include "ceres/linear_solver.h" #include "ceres/sparse_cholesky.h" namespace ceres { namespace internal { -class EigenSparseCholesky : public SparseCholesky { +class CERES_NO_EXPORT EigenSparseCholesky : public SparseCholesky { public: // Factory static std::unique_ptr<SparseCholesky> Create( const OrderingType ordering_type); // SparseCholesky interface. - virtual ~EigenSparseCholesky(); - virtual LinearSolverTerminationType Factorize(CompressedRowSparseMatrix* lhs, - std::string* message) = 0; - virtual CompressedRowSparseMatrix::StorageType StorageType() const = 0; - virtual LinearSolverTerminationType Solve(const double* rhs, - double* solution, - std::string* message) = 0; + ~EigenSparseCholesky() override; + LinearSolverTerminationType Factorize(CompressedRowSparseMatrix* lhs, + std::string* message) override = 0; + CompressedRowSparseMatrix::StorageType StorageType() const override = 0; + LinearSolverTerminationType Solve(const double* rhs, + double* solution, + std::string* message) override = 0; }; // Even though the input is double precision linear system, this class // solves it by computing a single precision Cholesky factorization. -class FloatEigenSparseCholesky : public SparseCholesky { +class CERES_NO_EXPORT FloatEigenSparseCholesky : public SparseCholesky { public: // Factory static std::unique_ptr<SparseCholesky> Create( const OrderingType ordering_type); // SparseCholesky interface. - virtual ~FloatEigenSparseCholesky(); - virtual LinearSolverTerminationType Factorize(CompressedRowSparseMatrix* lhs, - std::string* message) = 0; - virtual CompressedRowSparseMatrix::StorageType StorageType() const = 0; - virtual LinearSolverTerminationType Solve(const double* rhs, - double* solution, - std::string* message) = 0; + ~FloatEigenSparseCholesky() override; + LinearSolverTerminationType Factorize(CompressedRowSparseMatrix* lhs, + std::string* message) override = 0; + CompressedRowSparseMatrix::StorageType StorageType() const override = 0; + LinearSolverTerminationType Solve(const double* rhs, + double* solution, + std::string* message) override = 0; }; } // namespace internal diff --git a/extern/ceres/internal/ceres/evaluation_callback.cc b/extern/ceres/internal/ceres/evaluation_callback.cc new file mode 100644 index 00000000000..77591a8c621 --- /dev/null +++ b/extern/ceres/internal/ceres/evaluation_callback.cc @@ -0,0 +1,37 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2022 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: mierle@gmail.com (Keir Mierle) + +#include "ceres/evaluation_callback.h" + +namespace ceres { + +EvaluationCallback::~EvaluationCallback() = default; + +} // namespace ceres diff --git a/extern/ceres/internal/ceres/evaluator.cc b/extern/ceres/internal/ceres/evaluator.cc index 516874184d9..52d0f09e5df 100644 --- a/extern/ceres/internal/ceres/evaluator.cc +++ b/extern/ceres/internal/ceres/evaluator.cc @@ -30,6 +30,7 @@ #include "ceres/evaluator.h" +#include <memory> #include <vector> #include "ceres/block_evaluate_preparer.h" @@ -40,7 +41,7 @@ #include "ceres/dense_jacobian_writer.h" #include "ceres/dynamic_compressed_row_finalizer.h" #include "ceres/dynamic_compressed_row_jacobian_writer.h" -#include "ceres/internal/port.h" +#include "ceres/internal/export.h" #include "ceres/program_evaluator.h" #include "ceres/scratch_evaluate_preparer.h" #include "glog/logging.h" @@ -48,38 +49,42 @@ namespace ceres { namespace internal { -Evaluator::~Evaluator() {} +Evaluator::~Evaluator() = default; -Evaluator* Evaluator::Create(const Evaluator::Options& options, - Program* program, - std::string* error) { - CHECK(options.context != NULL); +std::unique_ptr<Evaluator> Evaluator::Create(const Evaluator::Options& options, + Program* program, + std::string* error) { + CHECK(options.context != nullptr); switch (options.linear_solver_type) { case DENSE_QR: case DENSE_NORMAL_CHOLESKY: - return new ProgramEvaluator<ScratchEvaluatePreparer, DenseJacobianWriter>( + return std::make_unique< + ProgramEvaluator<ScratchEvaluatePreparer, DenseJacobianWriter>>( options, program); case DENSE_SCHUR: case SPARSE_SCHUR: case ITERATIVE_SCHUR: case CGNR: - return new ProgramEvaluator<BlockEvaluatePreparer, BlockJacobianWriter>( + return std::make_unique< + ProgramEvaluator<BlockEvaluatePreparer, BlockJacobianWriter>>( options, program); case SPARSE_NORMAL_CHOLESKY: if (options.dynamic_sparsity) { - return new ProgramEvaluator<ScratchEvaluatePreparer, - DynamicCompressedRowJacobianWriter, - DynamicCompressedRowJacobianFinalizer>( - options, program); + return std::make_unique< + ProgramEvaluator<ScratchEvaluatePreparer, + DynamicCompressedRowJacobianWriter, + DynamicCompressedRowJacobianFinalizer>>(options, + program); } else { - return new ProgramEvaluator<BlockEvaluatePreparer, BlockJacobianWriter>( + return std::make_unique< + ProgramEvaluator<BlockEvaluatePreparer, BlockJacobianWriter>>( options, program); } default: *error = "Invalid Linear Solver Type. Unable to create evaluator."; - return NULL; + return nullptr; } } diff --git a/extern/ceres/internal/ceres/evaluator.h b/extern/ceres/internal/ceres/evaluator.h index 9cf42593e95..68a4fb28a55 100644 --- a/extern/ceres/internal/ceres/evaluator.h +++ b/extern/ceres/internal/ceres/evaluator.h @@ -33,12 +33,14 @@ #define CERES_INTERNAL_EVALUATOR_H_ #include <map> +#include <memory> #include <string> #include <vector> #include "ceres/context_impl.h" #include "ceres/execution_summary.h" -#include "ceres/internal/port.h" +#include "ceres/internal/disable_warnings.h" +#include "ceres/internal/export.h" #include "ceres/types.h" namespace ceres { @@ -54,8 +56,8 @@ class SparseMatrix; // The Evaluator interface offers a way to interact with a least squares cost // function that is useful for an optimizer that wants to minimize the least // squares objective. This insulates the optimizer from issues like Jacobian -// storage, parameterization, etc. -class CERES_EXPORT_INTERNAL Evaluator { +// storage, manifolds, etc. +class CERES_NO_EXPORT Evaluator { public: virtual ~Evaluator(); @@ -68,9 +70,9 @@ class CERES_EXPORT_INTERNAL Evaluator { EvaluationCallback* evaluation_callback = nullptr; }; - static Evaluator* Create(const Options& options, - Program* program, - std::string* error); + static std::unique_ptr<Evaluator> Create(const Options& options, + Program* program, + std::string* error); // Build and return a sparse matrix for storing and working with the Jacobian // of the objective function. The jacobian has dimensions @@ -88,7 +90,7 @@ class CERES_EXPORT_INTERNAL Evaluator { // the jacobian for use with CHOLMOD, where as BlockOptimizationProblem // creates a BlockSparseMatrix representation of the jacobian for use in the // Schur complement based methods. - virtual SparseMatrix* CreateJacobian() const = 0; + virtual std::unique_ptr<SparseMatrix> CreateJacobian() const = 0; // Options struct to control Evaluator::Evaluate; struct EvaluateOptions { @@ -102,10 +104,10 @@ class CERES_EXPORT_INTERNAL Evaluator { // Evaluate the cost function for the given state. Returns the cost, // residuals, and jacobian in the corresponding arguments. Both residuals and - // jacobian are optional; to avoid computing them, pass NULL. + // jacobian are optional; to avoid computing them, pass nullptr. // - // If non-NULL, the Jacobian must have a suitable sparsity pattern; only the - // values array of the jacobian is modified. + // If non-nullptr, the Jacobian must have a suitable sparsity pattern; only + // the values array of the jacobian is modified. // // state is an array of size NumParameters(), cost is a pointer to a single // double, and residuals is an array of doubles of size NumResiduals(). @@ -131,13 +133,13 @@ class CERES_EXPORT_INTERNAL Evaluator { // Make a change delta (of size NumEffectiveParameters()) to state (of size // NumParameters()) and store the result in state_plus_delta. // - // In the case that there are no parameterizations used, this is equivalent to + // In the case that there are no manifolds used, this is equivalent to // // state_plus_delta[i] = state[i] + delta[i] ; // - // however, the mapping is more complicated in the case of parameterizations + // however, the mapping is more complicated in the case of manifolds // like quaternions. This is the same as the "Plus()" operation in - // local_parameterization.h, but operating over the entire state vector for a + // manifold.h, but operating over the entire state vector for a // problem. virtual bool Plus(const double* state, const double* delta, @@ -147,7 +149,7 @@ class CERES_EXPORT_INTERNAL Evaluator { virtual int NumParameters() const = 0; // This is the effective number of parameters that the optimizer may adjust. - // This applies when there are parameterizations on some of the parameters. + // This applies when there are manifolds on some of the parameters. virtual int NumEffectiveParameters() const = 0; // The number of residuals in the optimization problem. @@ -158,11 +160,13 @@ class CERES_EXPORT_INTERNAL Evaluator { // life time issues. Further, these calls are not expected to be // frequent or performance sensitive. virtual std::map<std::string, CallStatistics> Statistics() const { - return std::map<std::string, CallStatistics>(); + return {}; } }; } // namespace internal } // namespace ceres +#include "ceres/internal/reenable_warnings.h" + #endif // CERES_INTERNAL_EVALUATOR_H_ diff --git a/extern/ceres/internal/ceres/execution_summary.h b/extern/ceres/internal/ceres/execution_summary.h index 17fd882af03..fbee75fc0cb 100644 --- a/extern/ceres/internal/ceres/execution_summary.h +++ b/extern/ceres/internal/ceres/execution_summary.h @@ -34,17 +34,18 @@ #include <map> #include <mutex> #include <string> +#include <utility> -#include "ceres/internal/port.h" +#include "ceres/internal/export.h" #include "ceres/wall_time.h" namespace ceres { namespace internal { struct CallStatistics { - CallStatistics() : time(0.), calls(0) {} - double time; - int calls; + CallStatistics() = default; + double time{0.}; + int calls{0}; }; // Struct used by various objects to report statistics about their @@ -69,8 +70,10 @@ class ExecutionSummary { class ScopedExecutionTimer { public: - ScopedExecutionTimer(const std::string& name, ExecutionSummary* summary) - : start_time_(WallTimeInSeconds()), name_(name), summary_(summary) {} + ScopedExecutionTimer(std::string name, ExecutionSummary* summary) + : start_time_(WallTimeInSeconds()), + name_(std::move(name)), + summary_(summary) {} ~ScopedExecutionTimer() { summary_->IncrementTimeBy(name_, WallTimeInSeconds() - start_time_); diff --git a/extern/ceres/internal/ceres/file.h b/extern/ceres/internal/ceres/file.h index c0015df60f0..bd13128aedf 100644 --- a/extern/ceres/internal/ceres/file.h +++ b/extern/ceres/internal/ceres/file.h @@ -35,21 +35,26 @@ #include <string> -#include "ceres/internal/port.h" +#include "ceres/internal/disable_warnings.h" +#include "ceres/internal/export.h" namespace ceres { namespace internal { +CERES_NO_EXPORT void WriteStringToFileOrDie(const std::string& data, const std::string& filename); +CERES_NO_EXPORT void ReadFileToStringOrDie(const std::string& filename, std::string* data); // Join two path components, adding a slash if necessary. If basename is an // absolute path then JoinPath ignores dirname and simply returns basename. -CERES_EXPORT_INTERNAL std::string JoinPath(const std::string& dirname, - const std::string& basename); +CERES_NO_EXPORT +std::string JoinPath(const std::string& dirname, const std::string& basename); } // namespace internal } // namespace ceres +#include "ceres/internal/reenable_warnings.h" + #endif // CERES_INTERNAL_FILE_H_ diff --git a/extern/ceres/internal/ceres/blas.cc b/extern/ceres/internal/ceres/first_order_function.cc index f8d006e3069..26f13488a1d 100644 --- a/extern/ceres/internal/ceres/blas.cc +++ b/extern/ceres/internal/ceres/first_order_function.cc @@ -1,5 +1,5 @@ // Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2015 Google Inc. All rights reserved. +// Copyright 2022 Google Inc. All rights reserved. // http://ceres-solver.org/ // // Redistribution and use in source and binary forms, with or without @@ -28,55 +28,10 @@ // // Author: sameeragarwal@google.com (Sameer Agarwal) -#include "ceres/blas.h" - -#include "ceres/internal/port.h" -#include "glog/logging.h" - -#ifndef CERES_NO_LAPACK -extern "C" void dsyrk_(char* uplo, - char* trans, - int* n, - int* k, - double* alpha, - double* a, - int* lda, - double* beta, - double* c, - int* ldc); -#endif +#include "ceres/first_order_function.h" namespace ceres { -namespace internal { -void BLAS::SymmetricRankKUpdate(int num_rows, - int num_cols, - const double* a, - bool transpose, - double alpha, - double beta, - double* c) { -#ifdef CERES_NO_LAPACK - LOG(FATAL) << "Ceres was built without a BLAS library."; -#else - char uplo = 'L'; - char trans = transpose ? 'T' : 'N'; - int n = transpose ? num_cols : num_rows; - int k = transpose ? num_rows : num_cols; - int lda = k; - int ldc = n; - dsyrk_(&uplo, - &trans, - &n, - &k, - &alpha, - const_cast<double*>(a), - &lda, - &beta, - c, - &ldc); -#endif -} +FirstOrderFunction::~FirstOrderFunction() = default; -} // namespace internal } // namespace ceres diff --git a/extern/ceres/internal/ceres/float_cxsparse.cc b/extern/ceres/internal/ceres/float_cxsparse.cc index 6c688303444..a6d5e811efd 100644 --- a/extern/ceres/internal/ceres/float_cxsparse.cc +++ b/extern/ceres/internal/ceres/float_cxsparse.cc @@ -30,6 +30,8 @@ #include "ceres/float_cxsparse.h" +#include <memory> + #if !defined(CERES_NO_CXSPARSE) namespace ceres { @@ -38,7 +40,7 @@ namespace internal { std::unique_ptr<SparseCholesky> FloatCXSparseCholesky::Create( OrderingType ordering_type) { LOG(FATAL) << "FloatCXSparseCholesky is not available."; - return std::unique_ptr<SparseCholesky>(); + return {}; } } // namespace internal diff --git a/extern/ceres/internal/ceres/float_cxsparse.h b/extern/ceres/internal/ceres/float_cxsparse.h index 9a274c23636..8b4514acb18 100644 --- a/extern/ceres/internal/ceres/float_cxsparse.h +++ b/extern/ceres/internal/ceres/float_cxsparse.h @@ -32,12 +32,13 @@ #define CERES_INTERNAL_FLOAT_CXSPARSE_H_ // This include must come before any #ifndef check on Ceres compile options. -#include "ceres/internal/port.h" +#include "ceres/internal/config.h" #if !defined(CERES_NO_CXSPARSE) #include <memory> +#include "ceres/internal/export.h" #include "ceres/sparse_cholesky.h" namespace ceres { @@ -45,7 +46,7 @@ namespace internal { // Fake implementation of a single precision Sparse Cholesky using // CXSparse. -class FloatCXSparseCholesky : public SparseCholesky { +class CERES_NO_EXPORT FloatCXSparseCholesky : public SparseCholesky { public: static std::unique_ptr<SparseCholesky> Create(OrderingType ordering_type); }; diff --git a/extern/ceres/internal/ceres/float_suitesparse.cc b/extern/ceres/internal/ceres/float_suitesparse.cc index 03604572b5c..dc1d0e45bdb 100644 --- a/extern/ceres/internal/ceres/float_suitesparse.cc +++ b/extern/ceres/internal/ceres/float_suitesparse.cc @@ -30,6 +30,8 @@ #include "ceres/float_suitesparse.h" +#include <memory> + #if !defined(CERES_NO_SUITESPARSE) namespace ceres { @@ -38,7 +40,7 @@ namespace internal { std::unique_ptr<SparseCholesky> FloatSuiteSparseCholesky::Create( OrderingType ordering_type) { LOG(FATAL) << "FloatSuiteSparseCholesky is not available."; - return std::unique_ptr<SparseCholesky>(); + return {}; } } // namespace internal diff --git a/extern/ceres/internal/ceres/float_suitesparse.h b/extern/ceres/internal/ceres/float_suitesparse.h index c436da43f86..7e76799e241 100644 --- a/extern/ceres/internal/ceres/float_suitesparse.h +++ b/extern/ceres/internal/ceres/float_suitesparse.h @@ -33,11 +33,12 @@ // This include must come before any #ifndef check on Ceres compile options. // clang-format off -#include "ceres/internal/port.h" +#include "ceres/internal/config.h" // clang-format on #include <memory> +#include "ceres/internal/export.h" #include "ceres/sparse_cholesky.h" #if !defined(CERES_NO_SUITESPARSE) @@ -47,7 +48,7 @@ namespace internal { // Fake implementation of a single precision Sparse Cholesky using // SuiteSparse. -class FloatSuiteSparseCholesky : public SparseCholesky { +class CERES_NO_EXPORT FloatSuiteSparseCholesky : public SparseCholesky { public: static std::unique_ptr<SparseCholesky> Create(OrderingType ordering_type); }; diff --git a/extern/ceres/internal/ceres/function_sample.h b/extern/ceres/internal/ceres/function_sample.h index 3bcea1bc5ff..63ffc8ff8fc 100644 --- a/extern/ceres/internal/ceres/function_sample.h +++ b/extern/ceres/internal/ceres/function_sample.h @@ -33,8 +33,9 @@ #include <string> +#include "ceres/internal/disable_warnings.h" #include "ceres/internal/eigen.h" -#include "ceres/internal/port.h" +#include "ceres/internal/export.h" namespace ceres { namespace internal { @@ -47,7 +48,7 @@ namespace internal { // line/direction. FunctionSample contains the information in two // ways. Information in the ambient space and information along the // direction of search. -struct CERES_EXPORT_INTERNAL FunctionSample { +struct CERES_NO_EXPORT FunctionSample { FunctionSample(); FunctionSample(double x, double value); FunctionSample(double x, double value, double gradient); @@ -90,4 +91,6 @@ struct CERES_EXPORT_INTERNAL FunctionSample { } // namespace internal } // namespace ceres +#include "ceres/internal/reenable_warnings.h" + #endif // CERES_INTERNAL_FUNCTION_SAMPLE_H_ diff --git a/extern/ceres/internal/ceres/generate_template_specializations.py b/extern/ceres/internal/ceres/generate_template_specializations.py deleted file mode 100644 index 74e46c28b78..00000000000 --- a/extern/ceres/internal/ceres/generate_template_specializations.py +++ /dev/null @@ -1,246 +0,0 @@ -# Ceres Solver - A fast non-linear least squares minimizer -# Copyright 2015 Google Inc. All rights reserved. -# http://ceres-solver.org/ -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions are met: -# -# * Redistributions of source code must retain the above copyright notice, -# this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above copyright notice, -# this list of conditions and the following disclaimer in the documentation -# and/or other materials provided with the distribution. -# * Neither the name of Google Inc. nor the names of its contributors may be -# used to endorse or promote products derived from this software without -# specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE -# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -# Author: sameeragarwal@google.com (Sameer Agarwal) -# -# Script for explicitly generating template specialization of the -# SchurEliminator class. It is a rather large class -# and the number of explicit instantiations is also large. Explicitly -# generating these instantiations in separate .cc files breaks the -# compilation into separate compilation unit rather than one large cc -# file which takes 2+GB of RAM to compile. -# -# This script creates three sets of files. -# -# 1. schur_eliminator_x_x_x.cc and partitioned_matrix_view_x_x_x.cc -# where, the x indicates the template parameters and -# -# 2. schur_eliminator.cc & partitioned_matrix_view.cc -# -# that contains a factory function for instantiating these classes -# based on runtime parameters. -# -# 3. schur_templates.cc -# -# that contains a function which can be queried to determine what -# template specializations are available. -# -# The following list of tuples, specializations indicates the set of -# specializations that is generated. -SPECIALIZATIONS = [(2, 2, 2), - (2, 2, 3), - (2, 2, 4), - (2, 2, "Eigen::Dynamic"), - (2, 3, 3), - (2, 3, 4), - (2, 3, 6), - (2, 3, 9), - (2, 3, "Eigen::Dynamic"), - (2, 4, 3), - (2, 4, 4), - (2, 4, 6), - (2, 4, 8), - (2, 4, 9), - (2, 4, "Eigen::Dynamic"), - (2, "Eigen::Dynamic", "Eigen::Dynamic"), - (3, 3, 3), - (4, 4, 2), - (4, 4, 3), - (4, 4, 4), - (4, 4, "Eigen::Dynamic")] - -import schur_eliminator_template -import partitioned_matrix_view_template -import os -import glob - -def SuffixForSize(size): - if size == "Eigen::Dynamic": - return "d" - return str(size) - -def SpecializationFilename(prefix, row_block_size, e_block_size, f_block_size): - return "_".join([prefix] + map(SuffixForSize, (row_block_size, - e_block_size, - f_block_size))) - -def GenerateFactoryConditional(row_block_size, e_block_size, f_block_size): - conditionals = [] - if (row_block_size != "Eigen::Dynamic"): - conditionals.append("(options.row_block_size == %s)" % row_block_size) - if (e_block_size != "Eigen::Dynamic"): - conditionals.append("(options.e_block_size == %s)" % e_block_size) - if (f_block_size != "Eigen::Dynamic"): - conditionals.append("(options.f_block_size == %s)" % f_block_size) - if (len(conditionals) == 0): - return "%s" - - if (len(conditionals) == 1): - return " if " + conditionals[0] + " {\n %s\n }\n" - - return " if (" + " &&\n ".join(conditionals) + ") {\n %s\n }\n" - -def Specialize(name, data): - """ - Generate specialization code and the conditionals to instantiate it. - """ - - # Specialization files - for row_block_size, e_block_size, f_block_size in SPECIALIZATIONS: - output = SpecializationFilename("generated/" + name, - row_block_size, - e_block_size, - f_block_size) + ".cc" - - with open(output, "w") as f: - f.write(data["HEADER"]) - f.write(data["SPECIALIZATION_FILE"] % - (row_block_size, e_block_size, f_block_size)) - - # Generate the _d_d_d specialization. - output = SpecializationFilename("generated/" + name, - "Eigen::Dynamic", - "Eigen::Dynamic", - "Eigen::Dynamic") + ".cc" - with open(output, "w") as f: - f.write(data["HEADER"]) - f.write(data["DYNAMIC_FILE"] % - ("Eigen::Dynamic", "Eigen::Dynamic", "Eigen::Dynamic")) - - # Factory - with open(name + ".cc", "w") as f: - f.write(data["HEADER"]) - f.write(data["FACTORY_FILE_HEADER"]) - for row_block_size, e_block_size, f_block_size in SPECIALIZATIONS: - factory_conditional = GenerateFactoryConditional( - row_block_size, e_block_size, f_block_size) - factory = data["FACTORY"] % (row_block_size, e_block_size, f_block_size) - f.write(factory_conditional % factory); - f.write(data["FACTORY_FOOTER"]) - -QUERY_HEADER = """// Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2017 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) -// -// What template specializations are available. -// -// ======================================== -// THIS FILE IS AUTOGENERATED. DO NOT EDIT. -// THIS FILE IS AUTOGENERATED. DO NOT EDIT. -// THIS FILE IS AUTOGENERATED. DO NOT EDIT. -// THIS FILE IS AUTOGENERATED. DO NOT EDIT. -//========================================= -// -// This file is generated using generate_template_specializations.py. -""" - -QUERY_FILE_HEADER = """ -#include "ceres/internal/eigen.h" -#include "ceres/schur_templates.h" - -namespace ceres { -namespace internal { - -void GetBestSchurTemplateSpecialization(int* row_block_size, - int* e_block_size, - int* f_block_size) { - LinearSolver::Options options; - options.row_block_size = *row_block_size; - options.e_block_size = *e_block_size; - options.f_block_size = *f_block_size; - *row_block_size = Eigen::Dynamic; - *e_block_size = Eigen::Dynamic; - *f_block_size = Eigen::Dynamic; -#ifndef CERES_RESTRICT_SCHUR_SPECIALIZATION -""" - -QUERY_FOOTER = """ -#endif - return; -} - -} // namespace internal -} // namespace ceres -""" - -QUERY_ACTION = """ *row_block_size = %s; - *e_block_size = %s; - *f_block_size = %s; - return;""" - -def GenerateQueryFile(): - """ - Generate file that allows querying for available template specializations. - """ - - with open("schur_templates.cc", "w") as f: - f.write(QUERY_HEADER) - f.write(QUERY_FILE_HEADER) - for row_block_size, e_block_size, f_block_size in SPECIALIZATIONS: - factory_conditional = GenerateFactoryConditional( - row_block_size, e_block_size, f_block_size) - action = QUERY_ACTION % (row_block_size, e_block_size, f_block_size) - f.write(factory_conditional % action) - f.write(QUERY_FOOTER) - - -if __name__ == "__main__": - for f in glob.glob("generated/*"): - os.remove(f) - - Specialize("schur_eliminator", - schur_eliminator_template.__dict__) - Specialize("partitioned_matrix_view", - partitioned_matrix_view_template.__dict__) - GenerateQueryFile() diff --git a/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_2_2.cc b/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_2_2.cc index f5753bef544..7b4ed167d05 100644 --- a/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_2_2.cc +++ b/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_2_2.cc @@ -40,7 +40,7 @@ // This file is generated using generate_template_specializations.py. // This include must come before any #ifndef check on Ceres compile options. -#include "ceres/internal/port.h" +#include "ceres/internal/config.h" #ifndef CERES_RESTRICT_SCHUR_SPECIALIZATION diff --git a/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_2_3.cc b/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_2_3.cc index a7a9b5231cf..0f012515a95 100644 --- a/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_2_3.cc +++ b/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_2_3.cc @@ -40,7 +40,7 @@ // This file is generated using generate_template_specializations.py. // This include must come before any #ifndef check on Ceres compile options. -#include "ceres/internal/port.h" +#include "ceres/internal/config.h" #ifndef CERES_RESTRICT_SCHUR_SPECIALIZATION diff --git a/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_2_4.cc b/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_2_4.cc index faf6c4a754a..bdbe91c43f6 100644 --- a/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_2_4.cc +++ b/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_2_4.cc @@ -40,7 +40,7 @@ // This file is generated using generate_template_specializations.py. // This include must come before any #ifndef check on Ceres compile options. -#include "ceres/internal/port.h" +#include "ceres/internal/config.h" #ifndef CERES_RESTRICT_SCHUR_SPECIALIZATION diff --git a/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_2_d.cc b/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_2_d.cc index 92fd4cddf43..71f293b5512 100644 --- a/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_2_d.cc +++ b/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_2_d.cc @@ -40,7 +40,7 @@ // This file is generated using generate_template_specializations.py. // This include must come before any #ifndef check on Ceres compile options. -#include "ceres/internal/port.h" +#include "ceres/internal/config.h" #ifndef CERES_RESTRICT_SCHUR_SPECIALIZATION diff --git a/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_3_3.cc b/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_3_3.cc index 2df314f137a..a6ea7761c9a 100644 --- a/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_3_3.cc +++ b/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_3_3.cc @@ -40,7 +40,7 @@ // This file is generated using generate_template_specializations.py. // This include must come before any #ifndef check on Ceres compile options. -#include "ceres/internal/port.h" +#include "ceres/internal/config.h" #ifndef CERES_RESTRICT_SCHUR_SPECIALIZATION diff --git a/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_3_4.cc b/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_3_4.cc index ff1ca3e7f1f..e712678a28a 100644 --- a/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_3_4.cc +++ b/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_3_4.cc @@ -40,7 +40,7 @@ // This file is generated using generate_template_specializations.py. // This include must come before any #ifndef check on Ceres compile options. -#include "ceres/internal/port.h" +#include "ceres/internal/config.h" #ifndef CERES_RESTRICT_SCHUR_SPECIALIZATION diff --git a/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_3_6.cc b/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_3_6.cc index 5041df9152d..3aff26e657b 100644 --- a/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_3_6.cc +++ b/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_3_6.cc @@ -40,7 +40,7 @@ // This file is generated using generate_template_specializations.py. // This include must come before any #ifndef check on Ceres compile options. -#include "ceres/internal/port.h" +#include "ceres/internal/config.h" #ifndef CERES_RESTRICT_SCHUR_SPECIALIZATION diff --git a/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_3_9.cc b/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_3_9.cc index c0b72fec8b8..6cd239bfd9a 100644 --- a/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_3_9.cc +++ b/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_3_9.cc @@ -40,7 +40,7 @@ // This file is generated using generate_template_specializations.py. // This include must come before any #ifndef check on Ceres compile options. -#include "ceres/internal/port.h" +#include "ceres/internal/config.h" #ifndef CERES_RESTRICT_SCHUR_SPECIALIZATION diff --git a/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_3_d.cc b/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_3_d.cc index 8a3c162ab7e..68c50552d42 100644 --- a/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_3_d.cc +++ b/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_3_d.cc @@ -40,7 +40,7 @@ // This file is generated using generate_template_specializations.py. // This include must come before any #ifndef check on Ceres compile options. -#include "ceres/internal/port.h" +#include "ceres/internal/config.h" #ifndef CERES_RESTRICT_SCHUR_SPECIALIZATION diff --git a/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_4_3.cc b/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_4_3.cc index 0e69ca6404d..88c5e29c6f8 100644 --- a/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_4_3.cc +++ b/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_4_3.cc @@ -40,7 +40,7 @@ // This file is generated using generate_template_specializations.py. // This include must come before any #ifndef check on Ceres compile options. -#include "ceres/internal/port.h" +#include "ceres/internal/config.h" #ifndef CERES_RESTRICT_SCHUR_SPECIALIZATION diff --git a/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_4_4.cc b/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_4_4.cc index ba9bb615291..b9487834441 100644 --- a/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_4_4.cc +++ b/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_4_4.cc @@ -40,7 +40,7 @@ // This file is generated using generate_template_specializations.py. // This include must come before any #ifndef check on Ceres compile options. -#include "ceres/internal/port.h" +#include "ceres/internal/config.h" #ifndef CERES_RESTRICT_SCHUR_SPECIALIZATION diff --git a/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_4_6.cc b/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_4_6.cc index 1acdb9b21d5..7f044ef628b 100644 --- a/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_4_6.cc +++ b/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_4_6.cc @@ -40,7 +40,7 @@ // This file is generated using generate_template_specializations.py. // This include must come before any #ifndef check on Ceres compile options. -#include "ceres/internal/port.h" +#include "ceres/internal/config.h" #ifndef CERES_RESTRICT_SCHUR_SPECIALIZATION diff --git a/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_4_8.cc b/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_4_8.cc index 888ff99557d..7394e7998e7 100644 --- a/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_4_8.cc +++ b/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_4_8.cc @@ -40,7 +40,7 @@ // This file is generated using generate_template_specializations.py. // This include must come before any #ifndef check on Ceres compile options. -#include "ceres/internal/port.h" +#include "ceres/internal/config.h" #ifndef CERES_RESTRICT_SCHUR_SPECIALIZATION diff --git a/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_4_9.cc b/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_4_9.cc index bd4dde3d207..263f1fb36f1 100644 --- a/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_4_9.cc +++ b/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_4_9.cc @@ -40,7 +40,7 @@ // This file is generated using generate_template_specializations.py. // This include must come before any #ifndef check on Ceres compile options. -#include "ceres/internal/port.h" +#include "ceres/internal/config.h" #ifndef CERES_RESTRICT_SCHUR_SPECIALIZATION diff --git a/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_4_d.cc b/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_4_d.cc index 6d3516fc6d5..d47634e0f40 100644 --- a/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_4_d.cc +++ b/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_4_d.cc @@ -40,7 +40,7 @@ // This file is generated using generate_template_specializations.py. // This include must come before any #ifndef check on Ceres compile options. -#include "ceres/internal/port.h" +#include "ceres/internal/config.h" #ifndef CERES_RESTRICT_SCHUR_SPECIALIZATION diff --git a/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_d_d.cc b/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_d_d.cc index 77d22ed6bc2..0944cdcbfda 100644 --- a/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_d_d.cc +++ b/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_d_d.cc @@ -40,7 +40,7 @@ // This file is generated using generate_template_specializations.py. // This include must come before any #ifndef check on Ceres compile options. -#include "ceres/internal/port.h" +#include "ceres/internal/config.h" #ifndef CERES_RESTRICT_SCHUR_SPECIALIZATION diff --git a/extern/ceres/internal/ceres/generated/partitioned_matrix_view_3_3_3.cc b/extern/ceres/internal/ceres/generated/partitioned_matrix_view_3_3_3.cc index aeb456c6e2a..23674031bb9 100644 --- a/extern/ceres/internal/ceres/generated/partitioned_matrix_view_3_3_3.cc +++ b/extern/ceres/internal/ceres/generated/partitioned_matrix_view_3_3_3.cc @@ -40,7 +40,7 @@ // This file is generated using generate_template_specializations.py. // This include must come before any #ifndef check on Ceres compile options. -#include "ceres/internal/port.h" +#include "ceres/internal/config.h" #ifndef CERES_RESTRICT_SCHUR_SPECIALIZATION diff --git a/extern/ceres/internal/ceres/generated/partitioned_matrix_view_4_4_2.cc b/extern/ceres/internal/ceres/generated/partitioned_matrix_view_4_4_2.cc index bb240b9e3f6..d5268cac481 100644 --- a/extern/ceres/internal/ceres/generated/partitioned_matrix_view_4_4_2.cc +++ b/extern/ceres/internal/ceres/generated/partitioned_matrix_view_4_4_2.cc @@ -40,7 +40,7 @@ // This file is generated using generate_template_specializations.py. // This include must come before any #ifndef check on Ceres compile options. -#include "ceres/internal/port.h" +#include "ceres/internal/config.h" #ifndef CERES_RESTRICT_SCHUR_SPECIALIZATION diff --git a/extern/ceres/internal/ceres/generated/partitioned_matrix_view_4_4_3.cc b/extern/ceres/internal/ceres/generated/partitioned_matrix_view_4_4_3.cc index 5d47543644d..67e098fc6f3 100644 --- a/extern/ceres/internal/ceres/generated/partitioned_matrix_view_4_4_3.cc +++ b/extern/ceres/internal/ceres/generated/partitioned_matrix_view_4_4_3.cc @@ -40,7 +40,7 @@ // This file is generated using generate_template_specializations.py. // This include must come before any #ifndef check on Ceres compile options. -#include "ceres/internal/port.h" +#include "ceres/internal/config.h" #ifndef CERES_RESTRICT_SCHUR_SPECIALIZATION diff --git a/extern/ceres/internal/ceres/generated/partitioned_matrix_view_4_4_4.cc b/extern/ceres/internal/ceres/generated/partitioned_matrix_view_4_4_4.cc index e14f980933d..5fe28caee8c 100644 --- a/extern/ceres/internal/ceres/generated/partitioned_matrix_view_4_4_4.cc +++ b/extern/ceres/internal/ceres/generated/partitioned_matrix_view_4_4_4.cc @@ -40,7 +40,7 @@ // This file is generated using generate_template_specializations.py. // This include must come before any #ifndef check on Ceres compile options. -#include "ceres/internal/port.h" +#include "ceres/internal/config.h" #ifndef CERES_RESTRICT_SCHUR_SPECIALIZATION diff --git a/extern/ceres/internal/ceres/generated/partitioned_matrix_view_4_4_d.cc b/extern/ceres/internal/ceres/generated/partitioned_matrix_view_4_4_d.cc index 9ec50563ac8..d87c76d0aa4 100644 --- a/extern/ceres/internal/ceres/generated/partitioned_matrix_view_4_4_d.cc +++ b/extern/ceres/internal/ceres/generated/partitioned_matrix_view_4_4_d.cc @@ -40,7 +40,7 @@ // This file is generated using generate_template_specializations.py. // This include must come before any #ifndef check on Ceres compile options. -#include "ceres/internal/port.h" +#include "ceres/internal/config.h" #ifndef CERES_RESTRICT_SCHUR_SPECIALIZATION diff --git a/extern/ceres/internal/ceres/generated/schur_eliminator_2_2_2.cc b/extern/ceres/internal/ceres/generated/schur_eliminator_2_2_2.cc index 289a809acb7..dc47a2e6d8e 100644 --- a/extern/ceres/internal/ceres/generated/schur_eliminator_2_2_2.cc +++ b/extern/ceres/internal/ceres/generated/schur_eliminator_2_2_2.cc @@ -40,7 +40,7 @@ // This file is generated using generate_template_specializations.py. // This include must come before any #ifndef check on Ceres compile options. -#include "ceres/internal/port.h" +#include "ceres/internal/config.h" #ifndef CERES_RESTRICT_SCHUR_SPECIALIZATION diff --git a/extern/ceres/internal/ceres/generated/schur_eliminator_2_2_3.cc b/extern/ceres/internal/ceres/generated/schur_eliminator_2_2_3.cc index 20311ba843d..e2df6f63d2f 100644 --- a/extern/ceres/internal/ceres/generated/schur_eliminator_2_2_3.cc +++ b/extern/ceres/internal/ceres/generated/schur_eliminator_2_2_3.cc @@ -40,7 +40,7 @@ // This file is generated using generate_template_specializations.py. // This include must come before any #ifndef check on Ceres compile options. -#include "ceres/internal/port.h" +#include "ceres/internal/config.h" #ifndef CERES_RESTRICT_SCHUR_SPECIALIZATION diff --git a/extern/ceres/internal/ceres/generated/schur_eliminator_2_2_4.cc b/extern/ceres/internal/ceres/generated/schur_eliminator_2_2_4.cc index 1f6a8ae4a0e..0b1ae949a09 100644 --- a/extern/ceres/internal/ceres/generated/schur_eliminator_2_2_4.cc +++ b/extern/ceres/internal/ceres/generated/schur_eliminator_2_2_4.cc @@ -40,7 +40,7 @@ // This file is generated using generate_template_specializations.py. // This include must come before any #ifndef check on Ceres compile options. -#include "ceres/internal/port.h" +#include "ceres/internal/config.h" #ifndef CERES_RESTRICT_SCHUR_SPECIALIZATION diff --git a/extern/ceres/internal/ceres/generated/schur_eliminator_2_2_d.cc b/extern/ceres/internal/ceres/generated/schur_eliminator_2_2_d.cc index 08b18d357bd..0f7b6d78c7f 100644 --- a/extern/ceres/internal/ceres/generated/schur_eliminator_2_2_d.cc +++ b/extern/ceres/internal/ceres/generated/schur_eliminator_2_2_d.cc @@ -40,7 +40,7 @@ // This file is generated using generate_template_specializations.py. // This include must come before any #ifndef check on Ceres compile options. -#include "ceres/internal/port.h" +#include "ceres/internal/config.h" #ifndef CERES_RESTRICT_SCHUR_SPECIALIZATION diff --git a/extern/ceres/internal/ceres/generated/schur_eliminator_2_3_3.cc b/extern/ceres/internal/ceres/generated/schur_eliminator_2_3_3.cc index 115b4c8cce1..e4ab8eb19bf 100644 --- a/extern/ceres/internal/ceres/generated/schur_eliminator_2_3_3.cc +++ b/extern/ceres/internal/ceres/generated/schur_eliminator_2_3_3.cc @@ -40,7 +40,7 @@ // This file is generated using generate_template_specializations.py. // This include must come before any #ifndef check on Ceres compile options. -#include "ceres/internal/port.h" +#include "ceres/internal/config.h" #ifndef CERES_RESTRICT_SCHUR_SPECIALIZATION diff --git a/extern/ceres/internal/ceres/generated/schur_eliminator_2_3_4.cc b/extern/ceres/internal/ceres/generated/schur_eliminator_2_3_4.cc index c7035370424..d73d466b04c 100644 --- a/extern/ceres/internal/ceres/generated/schur_eliminator_2_3_4.cc +++ b/extern/ceres/internal/ceres/generated/schur_eliminator_2_3_4.cc @@ -40,7 +40,7 @@ // This file is generated using generate_template_specializations.py. // This include must come before any #ifndef check on Ceres compile options. -#include "ceres/internal/port.h" +#include "ceres/internal/config.h" #ifndef CERES_RESTRICT_SCHUR_SPECIALIZATION diff --git a/extern/ceres/internal/ceres/generated/schur_eliminator_2_3_6.cc b/extern/ceres/internal/ceres/generated/schur_eliminator_2_3_6.cc index edb9afea969..800ee536bbf 100644 --- a/extern/ceres/internal/ceres/generated/schur_eliminator_2_3_6.cc +++ b/extern/ceres/internal/ceres/generated/schur_eliminator_2_3_6.cc @@ -40,7 +40,7 @@ // This file is generated using generate_template_specializations.py. // This include must come before any #ifndef check on Ceres compile options. -#include "ceres/internal/port.h" +#include "ceres/internal/config.h" #ifndef CERES_RESTRICT_SCHUR_SPECIALIZATION diff --git a/extern/ceres/internal/ceres/generated/schur_eliminator_2_3_9.cc b/extern/ceres/internal/ceres/generated/schur_eliminator_2_3_9.cc index faa5c19f5c0..d38cd566082 100644 --- a/extern/ceres/internal/ceres/generated/schur_eliminator_2_3_9.cc +++ b/extern/ceres/internal/ceres/generated/schur_eliminator_2_3_9.cc @@ -40,7 +40,7 @@ // This file is generated using generate_template_specializations.py. // This include must come before any #ifndef check on Ceres compile options. -#include "ceres/internal/port.h" +#include "ceres/internal/config.h" #ifndef CERES_RESTRICT_SCHUR_SPECIALIZATION diff --git a/extern/ceres/internal/ceres/generated/schur_eliminator_2_3_d.cc b/extern/ceres/internal/ceres/generated/schur_eliminator_2_3_d.cc index 81b6f975e7f..4ac4b8ac8b7 100644 --- a/extern/ceres/internal/ceres/generated/schur_eliminator_2_3_d.cc +++ b/extern/ceres/internal/ceres/generated/schur_eliminator_2_3_d.cc @@ -40,7 +40,7 @@ // This file is generated using generate_template_specializations.py. // This include must come before any #ifndef check on Ceres compile options. -#include "ceres/internal/port.h" +#include "ceres/internal/config.h" #ifndef CERES_RESTRICT_SCHUR_SPECIALIZATION diff --git a/extern/ceres/internal/ceres/generated/schur_eliminator_2_4_3.cc b/extern/ceres/internal/ceres/generated/schur_eliminator_2_4_3.cc index 2cb2d15ac93..d5f5dbea4b4 100644 --- a/extern/ceres/internal/ceres/generated/schur_eliminator_2_4_3.cc +++ b/extern/ceres/internal/ceres/generated/schur_eliminator_2_4_3.cc @@ -40,7 +40,7 @@ // This file is generated using generate_template_specializations.py. // This include must come before any #ifndef check on Ceres compile options. -#include "ceres/internal/port.h" +#include "ceres/internal/config.h" #ifndef CERES_RESTRICT_SCHUR_SPECIALIZATION diff --git a/extern/ceres/internal/ceres/generated/schur_eliminator_2_4_4.cc b/extern/ceres/internal/ceres/generated/schur_eliminator_2_4_4.cc index a78eff3aa02..d50a6d4002b 100644 --- a/extern/ceres/internal/ceres/generated/schur_eliminator_2_4_4.cc +++ b/extern/ceres/internal/ceres/generated/schur_eliminator_2_4_4.cc @@ -40,7 +40,7 @@ // This file is generated using generate_template_specializations.py. // This include must come before any #ifndef check on Ceres compile options. -#include "ceres/internal/port.h" +#include "ceres/internal/config.h" #ifndef CERES_RESTRICT_SCHUR_SPECIALIZATION diff --git a/extern/ceres/internal/ceres/generated/schur_eliminator_2_4_6.cc b/extern/ceres/internal/ceres/generated/schur_eliminator_2_4_6.cc index e2534f235b6..f79fa4dd2f0 100644 --- a/extern/ceres/internal/ceres/generated/schur_eliminator_2_4_6.cc +++ b/extern/ceres/internal/ceres/generated/schur_eliminator_2_4_6.cc @@ -40,7 +40,7 @@ // This file is generated using generate_template_specializations.py. // This include must come before any #ifndef check on Ceres compile options. -#include "ceres/internal/port.h" +#include "ceres/internal/config.h" #ifndef CERES_RESTRICT_SCHUR_SPECIALIZATION diff --git a/extern/ceres/internal/ceres/generated/schur_eliminator_2_4_8.cc b/extern/ceres/internal/ceres/generated/schur_eliminator_2_4_8.cc index 296a46273bc..972b000f1ba 100644 --- a/extern/ceres/internal/ceres/generated/schur_eliminator_2_4_8.cc +++ b/extern/ceres/internal/ceres/generated/schur_eliminator_2_4_8.cc @@ -40,7 +40,7 @@ // This file is generated using generate_template_specializations.py. // This include must come before any #ifndef check on Ceres compile options. -#include "ceres/internal/port.h" +#include "ceres/internal/config.h" #ifndef CERES_RESTRICT_SCHUR_SPECIALIZATION diff --git a/extern/ceres/internal/ceres/generated/schur_eliminator_2_4_9.cc b/extern/ceres/internal/ceres/generated/schur_eliminator_2_4_9.cc index 0d0b04e686c..aa33e479bc5 100644 --- a/extern/ceres/internal/ceres/generated/schur_eliminator_2_4_9.cc +++ b/extern/ceres/internal/ceres/generated/schur_eliminator_2_4_9.cc @@ -40,7 +40,7 @@ // This file is generated using generate_template_specializations.py. // This include must come before any #ifndef check on Ceres compile options. -#include "ceres/internal/port.h" +#include "ceres/internal/config.h" #ifndef CERES_RESTRICT_SCHUR_SPECIALIZATION diff --git a/extern/ceres/internal/ceres/generated/schur_eliminator_2_4_d.cc b/extern/ceres/internal/ceres/generated/schur_eliminator_2_4_d.cc index 797992660d7..a28ef15a522 100644 --- a/extern/ceres/internal/ceres/generated/schur_eliminator_2_4_d.cc +++ b/extern/ceres/internal/ceres/generated/schur_eliminator_2_4_d.cc @@ -40,7 +40,7 @@ // This file is generated using generate_template_specializations.py. // This include must come before any #ifndef check on Ceres compile options. -#include "ceres/internal/port.h" +#include "ceres/internal/config.h" #ifndef CERES_RESTRICT_SCHUR_SPECIALIZATION diff --git a/extern/ceres/internal/ceres/generated/schur_eliminator_2_d_d.cc b/extern/ceres/internal/ceres/generated/schur_eliminator_2_d_d.cc index 189be043af8..43924279a39 100644 --- a/extern/ceres/internal/ceres/generated/schur_eliminator_2_d_d.cc +++ b/extern/ceres/internal/ceres/generated/schur_eliminator_2_d_d.cc @@ -40,7 +40,7 @@ // This file is generated using generate_template_specializations.py. // This include must come before any #ifndef check on Ceres compile options. -#include "ceres/internal/port.h" +#include "ceres/internal/config.h" #ifndef CERES_RESTRICT_SCHUR_SPECIALIZATION diff --git a/extern/ceres/internal/ceres/generated/schur_eliminator_3_3_3.cc b/extern/ceres/internal/ceres/generated/schur_eliminator_3_3_3.cc index 35c14a8f4bd..7ff2a62341c 100644 --- a/extern/ceres/internal/ceres/generated/schur_eliminator_3_3_3.cc +++ b/extern/ceres/internal/ceres/generated/schur_eliminator_3_3_3.cc @@ -40,7 +40,7 @@ // This file is generated using generate_template_specializations.py. // This include must come before any #ifndef check on Ceres compile options. -#include "ceres/internal/port.h" +#include "ceres/internal/config.h" #ifndef CERES_RESTRICT_SCHUR_SPECIALIZATION diff --git a/extern/ceres/internal/ceres/generated/schur_eliminator_4_4_2.cc b/extern/ceres/internal/ceres/generated/schur_eliminator_4_4_2.cc index 878500a2100..9008b816843 100644 --- a/extern/ceres/internal/ceres/generated/schur_eliminator_4_4_2.cc +++ b/extern/ceres/internal/ceres/generated/schur_eliminator_4_4_2.cc @@ -40,7 +40,7 @@ // This file is generated using generate_template_specializations.py. // This include must come before any #ifndef check on Ceres compile options. -#include "ceres/internal/port.h" +#include "ceres/internal/config.h" #ifndef CERES_RESTRICT_SCHUR_SPECIALIZATION diff --git a/extern/ceres/internal/ceres/generated/schur_eliminator_4_4_3.cc b/extern/ceres/internal/ceres/generated/schur_eliminator_4_4_3.cc index c4b0959db6a..8e37df51bee 100644 --- a/extern/ceres/internal/ceres/generated/schur_eliminator_4_4_3.cc +++ b/extern/ceres/internal/ceres/generated/schur_eliminator_4_4_3.cc @@ -40,7 +40,7 @@ // This file is generated using generate_template_specializations.py. // This include must come before any #ifndef check on Ceres compile options. -#include "ceres/internal/port.h" +#include "ceres/internal/config.h" #ifndef CERES_RESTRICT_SCHUR_SPECIALIZATION diff --git a/extern/ceres/internal/ceres/generated/schur_eliminator_4_4_4.cc b/extern/ceres/internal/ceres/generated/schur_eliminator_4_4_4.cc index 20df5343335..09d50813a8a 100644 --- a/extern/ceres/internal/ceres/generated/schur_eliminator_4_4_4.cc +++ b/extern/ceres/internal/ceres/generated/schur_eliminator_4_4_4.cc @@ -40,7 +40,7 @@ // This file is generated using generate_template_specializations.py. // This include must come before any #ifndef check on Ceres compile options. -#include "ceres/internal/port.h" +#include "ceres/internal/config.h" #ifndef CERES_RESTRICT_SCHUR_SPECIALIZATION diff --git a/extern/ceres/internal/ceres/generated/schur_eliminator_4_4_d.cc b/extern/ceres/internal/ceres/generated/schur_eliminator_4_4_d.cc index 17368dca4f6..089df2d7e3e 100644 --- a/extern/ceres/internal/ceres/generated/schur_eliminator_4_4_d.cc +++ b/extern/ceres/internal/ceres/generated/schur_eliminator_4_4_d.cc @@ -40,7 +40,7 @@ // This file is generated using generate_template_specializations.py. // This include must come before any #ifndef check on Ceres compile options. -#include "ceres/internal/port.h" +#include "ceres/internal/config.h" #ifndef CERES_RESTRICT_SCHUR_SPECIALIZATION diff --git a/extern/ceres/internal/ceres/gradient_checker.cc b/extern/ceres/internal/ceres/gradient_checker.cc index dadaaa08734..777001e013c 100644 --- a/extern/ceres/internal/ceres/gradient_checker.cc +++ b/extern/ceres/internal/ceres/gradient_checker.cc @@ -40,6 +40,7 @@ #include <vector> #include "ceres/is_close.h" +#include "ceres/manifold_adapter.h" #include "ceres/stringprintf.h" #include "ceres/types.h" @@ -53,15 +54,13 @@ using std::vector; namespace { // Evaluate the cost function and transform the returned Jacobians to -// the local space of the respective local parameterizations. -bool EvaluateCostFunction( - const ceres::CostFunction* function, - double const* const* parameters, - const std::vector<const ceres::LocalParameterization*>& - local_parameterizations, - Vector* residuals, - std::vector<Matrix>* jacobians, - std::vector<Matrix>* local_jacobians) { +// the tangent space of the respective local parameterizations. +bool EvaluateCostFunction(const CostFunction* function, + double const* const* parameters, + const std::vector<const Manifold*>& manifolds, + Vector* residuals, + std::vector<Matrix>* jacobians, + std::vector<Matrix>* local_jacobians) { CHECK(residuals != nullptr); CHECK(jacobians != nullptr); CHECK(local_jacobians != nullptr); @@ -69,20 +68,20 @@ bool EvaluateCostFunction( const vector<int32_t>& block_sizes = function->parameter_block_sizes(); const int num_parameter_blocks = block_sizes.size(); - // Allocate Jacobian matrices in local space. + // Allocate Jacobian matrices in tangent space. local_jacobians->resize(num_parameter_blocks); vector<double*> local_jacobian_data(num_parameter_blocks); for (int i = 0; i < num_parameter_blocks; ++i) { int block_size = block_sizes.at(i); - if (local_parameterizations.at(i) != NULL) { - block_size = local_parameterizations.at(i)->LocalSize(); + if (manifolds.at(i) != nullptr) { + block_size = manifolds.at(i)->TangentSize(); } local_jacobians->at(i).resize(function->num_residuals(), block_size); local_jacobians->at(i).setZero(); local_jacobian_data.at(i) = local_jacobians->at(i).data(); } - // Allocate Jacobian matrices in global space. + // Allocate Jacobian matrices in ambient space. jacobians->resize(num_parameter_blocks); vector<double*> jacobian_data(num_parameter_blocks); for (int i = 0; i < num_parameter_blocks; ++i) { @@ -100,18 +99,17 @@ bool EvaluateCostFunction( return false; } - // Convert Jacobians from global to local space. + // Convert Jacobians from ambient to local space. for (size_t i = 0; i < local_jacobians->size(); ++i) { - if (local_parameterizations.at(i) == NULL) { + if (manifolds.at(i) == nullptr) { local_jacobians->at(i) = jacobians->at(i); } else { - int global_size = local_parameterizations.at(i)->GlobalSize(); - int local_size = local_parameterizations.at(i)->LocalSize(); - CHECK_EQ(jacobians->at(i).cols(), global_size); - Matrix global_J_local(global_size, local_size); - local_parameterizations.at(i)->ComputeJacobian(parameters[i], - global_J_local.data()); - local_jacobians->at(i).noalias() = jacobians->at(i) * global_J_local; + int ambient_size = manifolds.at(i)->AmbientSize(); + int tangent_size = manifolds.at(i)->TangentSize(); + CHECK_EQ(jacobians->at(i).cols(), ambient_size); + Matrix ambient_J_tangent(ambient_size, tangent_size); + manifolds.at(i)->PlusJacobian(parameters[i], ambient_J_tangent.data()); + local_jacobians->at(i).noalias() = jacobians->at(i) * ambient_J_tangent; } } return true; @@ -122,20 +120,47 @@ GradientChecker::GradientChecker( const CostFunction* function, const vector<const LocalParameterization*>* local_parameterizations, const NumericDiffOptions& options) + : delete_manifolds_(true), function_(function) { + CHECK(function != nullptr); + manifolds_.resize(function->parameter_block_sizes().size(), nullptr); + + // Wrap the local parameterization into manifold objects using + // ManifoldAdapter. + for (int i = 0; i < manifolds_.size(); ++i) { + const LocalParameterization* local_param = local_parameterizations->at(i); + if (local_param == nullptr) { + continue; + } + manifolds_[i] = new internal::ManifoldAdapter(local_param); + } + + auto finite_diff_cost_function = + std::make_unique<DynamicNumericDiffCostFunction<CostFunction, RIDDERS>>( + function, DO_NOT_TAKE_OWNERSHIP, options); + const vector<int32_t>& parameter_block_sizes = + function->parameter_block_sizes(); + for (int32_t parameter_block_size : parameter_block_sizes) { + finite_diff_cost_function->AddParameterBlock(parameter_block_size); + } + finite_diff_cost_function->SetNumResiduals(function->num_residuals()); + + finite_diff_cost_function_ = std::move(finite_diff_cost_function); +} + +GradientChecker::GradientChecker(const CostFunction* function, + const vector<const Manifold*>* manifolds, + const NumericDiffOptions& options) : function_(function) { CHECK(function != nullptr); - if (local_parameterizations != NULL) { - local_parameterizations_ = *local_parameterizations; + if (manifolds != nullptr) { + manifolds_ = *manifolds; } else { - local_parameterizations_.resize(function->parameter_block_sizes().size(), - NULL); + manifolds_.resize(function->parameter_block_sizes().size(), nullptr); } - DynamicNumericDiffCostFunction<CostFunction, RIDDERS>* - finite_diff_cost_function = - new DynamicNumericDiffCostFunction<CostFunction, RIDDERS>( - function, DO_NOT_TAKE_OWNERSHIP, options); - finite_diff_cost_function_.reset(finite_diff_cost_function); + auto finite_diff_cost_function = + std::make_unique<DynamicNumericDiffCostFunction<CostFunction, RIDDERS>>( + function, DO_NOT_TAKE_OWNERSHIP, options); const vector<int32_t>& parameter_block_sizes = function->parameter_block_sizes(); const int num_parameter_blocks = parameter_block_sizes.size(); @@ -143,6 +168,16 @@ GradientChecker::GradientChecker( finite_diff_cost_function->AddParameterBlock(parameter_block_sizes[i]); } finite_diff_cost_function->SetNumResiduals(function->num_residuals()); + + finite_diff_cost_function_ = std::move(finite_diff_cost_function); +} + +GradientChecker::~GradientChecker() { + if (delete_manifolds_) { + for (const auto m : manifolds_) { + delete m; + } + } } bool GradientChecker::Probe(double const* const* parameters, @@ -154,7 +189,7 @@ bool GradientChecker::Probe(double const* const* parameters, // provided an output argument. ProbeResults* results; ProbeResults results_local; - if (results_param != NULL) { + if (results_param != nullptr) { results = results_param; results->residuals.resize(0); results->jacobians.clear(); @@ -173,7 +208,7 @@ bool GradientChecker::Probe(double const* const* parameters, vector<Matrix>& local_jacobians = results->local_jacobians; if (!EvaluateCostFunction(function_, parameters, - local_parameterizations_, + manifolds_, &results->residuals, &jacobians, &local_jacobians)) { @@ -187,7 +222,7 @@ bool GradientChecker::Probe(double const* const* parameters, Vector finite_diff_residuals; if (!EvaluateCostFunction(finite_diff_cost_function_.get(), parameters, - local_parameterizations_, + manifolds_, &finite_diff_residuals, &numeric_jacobians, &local_numeric_jacobians)) { @@ -205,8 +240,8 @@ bool GradientChecker::Probe(double const* const* parameters, if (!IsClose(results->residuals[i], finite_diff_residuals[i], relative_precision, - NULL, - NULL)) { + nullptr, + nullptr)) { results->error_log = "Function evaluation with and without Jacobians " "resulted in different residuals."; diff --git a/extern/ceres/internal/ceres/gradient_checking_cost_function.cc b/extern/ceres/internal/ceres/gradient_checking_cost_function.cc index 2eb6d627167..1c3b318ed04 100644 --- a/extern/ceres/internal/ceres/gradient_checking_cost_function.cc +++ b/extern/ceres/internal/ceres/gradient_checking_cost_function.cc @@ -1,5 +1,5 @@ // Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2015 Google Inc. All rights reserved. +// Copyright 2022 Google Inc. All rights reserved. // http://ceres-solver.org/ // // Redistribution and use in source and binary forms, with or without @@ -34,8 +34,10 @@ #include <algorithm> #include <cmath> #include <cstdint> +#include <memory> #include <numeric> #include <string> +#include <utility> #include <vector> #include "ceres/dynamic_numeric_diff_cost_function.h" @@ -60,19 +62,18 @@ using std::vector; namespace { -class GradientCheckingCostFunction : public CostFunction { +class GradientCheckingCostFunction final : public CostFunction { public: - GradientCheckingCostFunction( - const CostFunction* function, - const std::vector<const LocalParameterization*>* local_parameterizations, - const NumericDiffOptions& options, - double relative_precision, - const string& extra_info, - GradientCheckingIterationCallback* callback) + GradientCheckingCostFunction(const CostFunction* function, + const std::vector<const Manifold*>* manifolds, + const NumericDiffOptions& options, + double relative_precision, + string extra_info, + GradientCheckingIterationCallback* callback) : function_(function), - gradient_checker_(function, local_parameterizations, options), + gradient_checker_(function, manifolds, options), relative_precision_(relative_precision), - extra_info_(extra_info), + extra_info_(std::move(extra_info)), callback_(callback) { CHECK(callback_ != nullptr); const vector<int32_t>& parameter_block_sizes = @@ -81,14 +82,12 @@ class GradientCheckingCostFunction : public CostFunction { set_num_residuals(function->num_residuals()); } - virtual ~GradientCheckingCostFunction() {} - bool Evaluate(double const* const* parameters, double* residuals, double** jacobians) const final { if (!jacobians) { // Nothing to check in this case; just forward. - return function_->Evaluate(parameters, residuals, NULL); + return function_->Evaluate(parameters, residuals, nullptr); } GradientChecker::ProbeResults results; @@ -108,7 +107,7 @@ class GradientCheckingCostFunction : public CostFunction { // Copy the original jacobian blocks into the jacobians array. const vector<int32_t>& block_sizes = function_->parameter_block_sizes(); for (int k = 0; k < block_sizes.size(); k++) { - if (jacobians[k] != NULL) { + if (jacobians[k] != nullptr) { MatrixRef(jacobians[k], results.jacobians[k].rows(), results.jacobians[k].cols()) = results.jacobians[k]; @@ -145,6 +144,7 @@ CallbackReturnType GradientCheckingIterationCallback::operator()( } return SOLVER_CONTINUE; } + void GradientCheckingIterationCallback::SetGradientErrorDetected( std::string& error_log) { std::lock_guard<std::mutex> l(mutex_); @@ -152,9 +152,9 @@ void GradientCheckingIterationCallback::SetGradientErrorDetected( error_log_ += "\n" + error_log; } -CostFunction* CreateGradientCheckingCostFunction( +std::unique_ptr<CostFunction> CreateGradientCheckingCostFunction( const CostFunction* cost_function, - const std::vector<const LocalParameterization*>* local_parameterizations, + const std::vector<const Manifold*>* manifolds, double relative_step_size, double relative_precision, const std::string& extra_info, @@ -162,51 +162,48 @@ CostFunction* CreateGradientCheckingCostFunction( NumericDiffOptions numeric_diff_options; numeric_diff_options.relative_step_size = relative_step_size; - return new GradientCheckingCostFunction(cost_function, - local_parameterizations, - numeric_diff_options, - relative_precision, - extra_info, - callback); + return std::make_unique<GradientCheckingCostFunction>(cost_function, + manifolds, + numeric_diff_options, + relative_precision, + extra_info, + callback); } -ProblemImpl* CreateGradientCheckingProblemImpl( +std::unique_ptr<ProblemImpl> CreateGradientCheckingProblemImpl( ProblemImpl* problem_impl, double relative_step_size, double relative_precision, GradientCheckingIterationCallback* callback) { CHECK(callback != nullptr); - // We create new CostFunctions by wrapping the original CostFunction - // in a gradient checking CostFunction. So its okay for the - // ProblemImpl to take ownership of it and destroy it. The - // LossFunctions and LocalParameterizations are reused and since - // they are owned by problem_impl, gradient_checking_problem_impl + // We create new CostFunctions by wrapping the original CostFunction in a + // gradient checking CostFunction. So its okay for the ProblemImpl to take + // ownership of it and destroy it. The LossFunctions and Manifolds are reused + // and since they are owned by problem_impl, gradient_checking_problem_impl // should not take ownership of it. Problem::Options gradient_checking_problem_options; gradient_checking_problem_options.cost_function_ownership = TAKE_OWNERSHIP; gradient_checking_problem_options.loss_function_ownership = DO_NOT_TAKE_OWNERSHIP; - gradient_checking_problem_options.local_parameterization_ownership = - DO_NOT_TAKE_OWNERSHIP; + gradient_checking_problem_options.manifold_ownership = DO_NOT_TAKE_OWNERSHIP; gradient_checking_problem_options.context = problem_impl->context(); NumericDiffOptions numeric_diff_options; numeric_diff_options.relative_step_size = relative_step_size; - ProblemImpl* gradient_checking_problem_impl = - new ProblemImpl(gradient_checking_problem_options); + auto gradient_checking_problem_impl = + std::make_unique<ProblemImpl>(gradient_checking_problem_options); Program* program = problem_impl->mutable_program(); - // For every ParameterBlock in problem_impl, create a new parameter - // block with the same local parameterization and constancy. + // For every ParameterBlock in problem_impl, create a new parameter block with + // the same manifold and constancy. const vector<ParameterBlock*>& parameter_blocks = program->parameter_blocks(); - for (int i = 0; i < parameter_blocks.size(); ++i) { - ParameterBlock* parameter_block = parameter_blocks[i]; + for (auto* parameter_block : parameter_blocks) { gradient_checking_problem_impl->AddParameterBlock( parameter_block->mutable_user_state(), parameter_block->Size(), - parameter_block->mutable_local_parameterization()); + parameter_block->mutable_manifold()); if (parameter_block->IsConstant()) { gradient_checking_problem_impl->SetParameterBlockConstant( @@ -238,22 +235,22 @@ ProblemImpl* CreateGradientCheckingProblemImpl( string extra_info = StringPrintf("Residual block id %d; depends on parameters [", i); vector<double*> parameter_blocks; - vector<const LocalParameterization*> local_parameterizations; + vector<const Manifold*> manifolds; parameter_blocks.reserve(residual_block->NumParameterBlocks()); - local_parameterizations.reserve(residual_block->NumParameterBlocks()); + manifolds.reserve(residual_block->NumParameterBlocks()); for (int j = 0; j < residual_block->NumParameterBlocks(); ++j) { ParameterBlock* parameter_block = residual_block->parameter_blocks()[j]; parameter_blocks.push_back(parameter_block->mutable_user_state()); StringAppendF(&extra_info, "%p", parameter_block->mutable_user_state()); extra_info += (j < residual_block->NumParameterBlocks() - 1) ? ", " : "]"; - local_parameterizations.push_back(problem_impl->GetParameterization( - parameter_block->mutable_user_state())); + manifolds.push_back( + problem_impl->GetManifold(parameter_block->mutable_user_state())); } // Wrap the original CostFunction in a GradientCheckingCostFunction. CostFunction* gradient_checking_cost_function = new GradientCheckingCostFunction(residual_block->cost_function(), - &local_parameterizations, + &manifolds, numeric_diff_options, relative_precision, extra_info, diff --git a/extern/ceres/internal/ceres/gradient_checking_cost_function.h b/extern/ceres/internal/ceres/gradient_checking_cost_function.h index ea6e9b31c8c..0caafafa8fa 100644 --- a/extern/ceres/internal/ceres/gradient_checking_cost_function.h +++ b/extern/ceres/internal/ceres/gradient_checking_cost_function.h @@ -32,13 +32,15 @@ #ifndef CERES_INTERNAL_GRADIENT_CHECKING_COST_FUNCTION_H_ #define CERES_INTERNAL_GRADIENT_CHECKING_COST_FUNCTION_H_ +#include <memory> #include <mutex> #include <string> #include "ceres/cost_function.h" -#include "ceres/internal/port.h" +#include "ceres/internal/disable_warnings.h" +#include "ceres/internal/export.h" #include "ceres/iteration_callback.h" -#include "ceres/local_parameterization.h" +#include "ceres/manifold.h" namespace ceres { namespace internal { @@ -47,7 +49,7 @@ class ProblemImpl; // Callback that collects information about gradient checking errors, and // will abort the solve as soon as an error occurs. -class CERES_EXPORT_INTERNAL GradientCheckingIterationCallback +class CERES_NO_EXPORT GradientCheckingIterationCallback : public IterationCallback { public: GradientCheckingIterationCallback(); @@ -73,9 +75,10 @@ class CERES_EXPORT_INTERNAL GradientCheckingIterationCallback // with finite differences. This API is only intended for unit tests that intend // to check the functionality of the GradientCheckingCostFunction // implementation directly. -CERES_EXPORT_INTERNAL CostFunction* CreateGradientCheckingCostFunction( +CERES_NO_EXPORT std::unique_ptr<CostFunction> +CreateGradientCheckingCostFunction( const CostFunction* cost_function, - const std::vector<const LocalParameterization*>* local_parameterizations, + const std::vector<const Manifold*>* manifolds, double relative_step_size, double relative_precision, const std::string& extra_info, @@ -92,8 +95,6 @@ CERES_EXPORT_INTERNAL CostFunction* CreateGradientCheckingCostFunction( // iteration, the respective cost function will notify the // GradientCheckingIterationCallback. // -// The caller owns the returned ProblemImpl object. -// // Note: This is quite inefficient and is intended only for debugging. // // relative_step_size and relative_precision are parameters to control @@ -102,7 +103,7 @@ CERES_EXPORT_INTERNAL CostFunction* CreateGradientCheckingCostFunction( // jacobians obtained by numerically differentiating them. See the // documentation of 'numeric_derivative_relative_step_size' in solver.h for a // better explanation. -CERES_EXPORT_INTERNAL ProblemImpl* CreateGradientCheckingProblemImpl( +CERES_NO_EXPORT std::unique_ptr<ProblemImpl> CreateGradientCheckingProblemImpl( ProblemImpl* problem_impl, double relative_step_size, double relative_precision, @@ -111,4 +112,6 @@ CERES_EXPORT_INTERNAL ProblemImpl* CreateGradientCheckingProblemImpl( } // namespace internal } // namespace ceres +#include "ceres/internal/reenable_warnings.h" + #endif // CERES_INTERNAL_GRADIENT_CHECKING_COST_FUNCTION_H_ diff --git a/extern/ceres/internal/ceres/gradient_problem.cc b/extern/ceres/internal/ceres/gradient_problem.cc index ba33fbc90f8..cdd472fe87f 100644 --- a/extern/ceres/internal/ceres/gradient_problem.cc +++ b/extern/ceres/internal/ceres/gradient_problem.cc @@ -30,49 +30,75 @@ #include "ceres/gradient_problem.h" +#include <memory> + #include "ceres/local_parameterization.h" +#include "ceres/manifold_adapter.h" #include "glog/logging.h" namespace ceres { GradientProblem::GradientProblem(FirstOrderFunction* function) : function_(function), - parameterization_( - new IdentityParameterization(function_->NumParameters())), - scratch_(new double[function_->NumParameters()]) {} + manifold_(std::make_unique<EuclideanManifold<DYNAMIC>>( + function_->NumParameters())), + scratch_(new double[function_->NumParameters()]) { + CHECK(function != nullptr); +} GradientProblem::GradientProblem(FirstOrderFunction* function, LocalParameterization* parameterization) : function_(function), parameterization_(parameterization), scratch_(new double[function_->NumParameters()]) { - CHECK_EQ(function_->NumParameters(), parameterization_->GlobalSize()); + CHECK(function != nullptr); + if (parameterization != nullptr) { + manifold_ = + std::make_unique<internal::ManifoldAdapter>(parameterization_.get()); + } else { + manifold_ = std::make_unique<EuclideanManifold<DYNAMIC>>( + function_->NumParameters()); + } + CHECK_EQ(function_->NumParameters(), manifold_->AmbientSize()); +} + +GradientProblem::GradientProblem(FirstOrderFunction* function, + Manifold* manifold) + : function_(function), scratch_(new double[function_->NumParameters()]) { + CHECK(function != nullptr); + if (manifold != nullptr) { + manifold_.reset(manifold); + } else { + manifold_ = std::make_unique<EuclideanManifold<DYNAMIC>>( + function_->NumParameters()); + } + CHECK_EQ(function_->NumParameters(), manifold_->AmbientSize()); } int GradientProblem::NumParameters() const { return function_->NumParameters(); } -int GradientProblem::NumLocalParameters() const { - return parameterization_->LocalSize(); +int GradientProblem::NumTangentParameters() const { + return manifold_->TangentSize(); } bool GradientProblem::Evaluate(const double* parameters, double* cost, double* gradient) const { - if (gradient == NULL) { - return function_->Evaluate(parameters, cost, NULL); + if (gradient == nullptr) { + return function_->Evaluate(parameters, cost, nullptr); } return (function_->Evaluate(parameters, cost, scratch_.get()) && - parameterization_->MultiplyByJacobian( + manifold_->RightMultiplyByPlusJacobian( parameters, 1, scratch_.get(), gradient)); } bool GradientProblem::Plus(const double* x, const double* delta, double* x_plus_delta) const { - return parameterization_->Plus(x, delta, x_plus_delta); + return manifold_->Plus(x, delta, x_plus_delta); } } // namespace ceres diff --git a/extern/ceres/internal/ceres/gradient_problem_evaluator.h b/extern/ceres/internal/ceres/gradient_problem_evaluator.h index d224dbed0ae..efbb257ec75 100644 --- a/extern/ceres/internal/ceres/gradient_problem_evaluator.h +++ b/extern/ceres/internal/ceres/gradient_problem_evaluator.h @@ -32,30 +32,34 @@ #define CERES_INTERNAL_GRADIENT_PROBLEM_EVALUATOR_H_ #include <map> +#include <memory> #include <string> #include "ceres/evaluator.h" #include "ceres/execution_summary.h" #include "ceres/gradient_problem.h" -#include "ceres/internal/port.h" +#include "ceres/internal/disable_warnings.h" +#include "ceres/internal/export.h" +#include "ceres/sparse_matrix.h" #include "ceres/wall_time.h" namespace ceres { namespace internal { -class GradientProblemEvaluator : public Evaluator { +class CERES_NO_EXPORT GradientProblemEvaluator final : public Evaluator { public: explicit GradientProblemEvaluator(const GradientProblem& problem) : problem_(problem) {} - virtual ~GradientProblemEvaluator() {} - SparseMatrix* CreateJacobian() const final { return nullptr; } + + std::unique_ptr<SparseMatrix> CreateJacobian() const final { return nullptr; } + bool Evaluate(const EvaluateOptions& evaluate_options, const double* state, double* cost, double* residuals, double* gradient, SparseMatrix* jacobian) final { - CHECK(jacobian == NULL); + CHECK(jacobian == nullptr); ScopedExecutionTimer total_timer("Evaluator::Total", &execution_summary_); // The reason we use Residual and Jacobian here even when we are // only computing the cost and gradient has to do with the fact @@ -65,7 +69,7 @@ class GradientProblemEvaluator : public Evaluator { // to be consistent across the code base for the time accounting // to work. ScopedExecutionTimer call_type_timer( - gradient == NULL ? "Evaluator::Residual" : "Evaluator::Jacobian", + gradient == nullptr ? "Evaluator::Residual" : "Evaluator::Jacobian", &execution_summary_); return problem_.Evaluate(state, cost, gradient); } @@ -96,4 +100,6 @@ class GradientProblemEvaluator : public Evaluator { } // namespace internal } // namespace ceres +#include "ceres/internal/reenable_warnings.h" + #endif // CERES_INTERNAL_GRADIENT_PROBLEM_EVALUATOR_H_ diff --git a/extern/ceres/internal/ceres/gradient_problem_solver.cc b/extern/ceres/internal/ceres/gradient_problem_solver.cc index b72fad91542..9382556d292 100644 --- a/extern/ceres/internal/ceres/gradient_problem_solver.cc +++ b/extern/ceres/internal/ceres/gradient_problem_solver.cc @@ -36,7 +36,7 @@ #include "ceres/gradient_problem.h" #include "ceres/gradient_problem_evaluator.h" #include "ceres/internal/eigen.h" -#include "ceres/internal/port.h" +#include "ceres/internal/export.h" #include "ceres/map_util.h" #include "ceres/minimizer.h" #include "ceres/solver.h" @@ -92,7 +92,7 @@ bool GradientProblemSolver::Options::IsValid(std::string* error) const { return solver_options.IsValid(error); } -GradientProblemSolver::~GradientProblemSolver() {} +GradientProblemSolver::~GradientProblemSolver() = default; void GradientProblemSolver::Solve(const GradientProblemSolver::Options& options, const GradientProblem& problem, @@ -135,21 +135,22 @@ void GradientProblemSolver::Solve(const GradientProblemSolver::Options& options, // now. Minimizer::Options minimizer_options = Minimizer::Options(GradientProblemSolverOptionsToSolverOptions(options)); - minimizer_options.evaluator.reset(new GradientProblemEvaluator(problem)); + minimizer_options.evaluator = + std::make_unique<GradientProblemEvaluator>(problem); std::unique_ptr<IterationCallback> logging_callback; if (options.logging_type != SILENT) { - logging_callback.reset( - new LoggingCallback(LINE_SEARCH, options.minimizer_progress_to_stdout)); + logging_callback = std::make_unique<LoggingCallback>( + LINE_SEARCH, options.minimizer_progress_to_stdout); minimizer_options.callbacks.insert(minimizer_options.callbacks.begin(), logging_callback.get()); } std::unique_ptr<IterationCallback> state_updating_callback; if (options.update_state_every_iteration) { - state_updating_callback.reset( - new GradientProblemSolverStateUpdatingCallback( - problem.NumParameters(), solution.data(), parameters_ptr)); + state_updating_callback = + std::make_unique<GradientProblemSolverStateUpdatingCallback>( + problem.NumParameters(), solution.data(), parameters_ptr); minimizer_options.callbacks.insert(minimizer_options.callbacks.begin(), state_updating_callback.get()); } diff --git a/extern/ceres/internal/ceres/graph.h b/extern/ceres/internal/ceres/graph.h index 9b26158753f..6a6f8f01c00 100644 --- a/extern/ceres/internal/ceres/graph.h +++ b/extern/ceres/internal/ceres/graph.h @@ -36,6 +36,7 @@ #include <unordered_set> #include <utility> +#include "ceres/internal/export.h" #include "ceres/map_util.h" #include "ceres/pair_hash.h" #include "ceres/types.h" @@ -47,10 +48,8 @@ namespace internal { // A unweighted undirected graph templated over the vertex ids. Vertex // should be hashable. template <typename Vertex> -class Graph { +class CERES_NO_EXPORT Graph { public: - Graph() {} - // Add a vertex. void AddVertex(const Vertex& vertex) { if (vertices_.insert(vertex).second) { @@ -106,8 +105,6 @@ class Graph { template <typename Vertex> class WeightedGraph { public: - WeightedGraph() {} - // Add a weighted vertex. If the vertex already exists in the graph, // its weight is set to the new weight. void AddVertex(const Vertex& vertex, double weight) { diff --git a/extern/ceres/internal/ceres/graph_algorithms.h b/extern/ceres/internal/ceres/graph_algorithms.h index 7d63b337f68..5299f80d963 100644 --- a/extern/ceres/internal/ceres/graph_algorithms.h +++ b/extern/ceres/internal/ceres/graph_algorithms.h @@ -34,12 +34,14 @@ #define CERES_INTERNAL_GRAPH_ALGORITHMS_H_ #include <algorithm> +#include <memory> #include <unordered_map> #include <unordered_set> #include <utility> #include <vector> #include "ceres/graph.h" +#include "ceres/internal/export.h" #include "ceres/wall_time.h" #include "glog/logging.h" @@ -49,7 +51,7 @@ namespace internal { // Compare two vertices of a graph by their degrees, if the degrees // are equal then order them by their ids. template <typename Vertex> -class VertexTotalOrdering { +class CERES_NO_EXPORT VertexTotalOrdering { public: explicit VertexTotalOrdering(const Graph<Vertex>& graph) : graph_(graph) {} @@ -257,11 +259,11 @@ Vertex FindConnectedComponent(const Vertex& vertex, // spanning forest, or a collection of linear paths that span the // graph G. template <typename Vertex> -WeightedGraph<Vertex>* Degree2MaximumSpanningForest( +std::unique_ptr<WeightedGraph<Vertex>> Degree2MaximumSpanningForest( const WeightedGraph<Vertex>& graph) { // Array of edges sorted in decreasing order of their weights. std::vector<std::pair<double, std::pair<Vertex, Vertex>>> weighted_edges; - WeightedGraph<Vertex>* forest = new WeightedGraph<Vertex>(); + auto forest = std::make_unique<WeightedGraph<Vertex>>(); // Disjoint-set to keep track of the connected components in the // maximum spanning tree. diff --git a/extern/ceres/internal/ceres/implicit_schur_complement.cc b/extern/ceres/internal/ceres/implicit_schur_complement.cc index f2196d4ef9c..677d767fa93 100644 --- a/extern/ceres/internal/ceres/implicit_schur_complement.cc +++ b/extern/ceres/internal/ceres/implicit_schur_complement.cc @@ -43,17 +43,15 @@ namespace internal { ImplicitSchurComplement::ImplicitSchurComplement( const LinearSolver::Options& options) - : options_(options), D_(NULL), b_(NULL) {} - -ImplicitSchurComplement::~ImplicitSchurComplement() {} + : options_(options), D_(nullptr), b_(nullptr) {} void ImplicitSchurComplement::Init(const BlockSparseMatrix& A, const double* D, const double* b) { // Since initialization is reasonably heavy, perhaps we can save on // constructing a new object everytime. - if (A_ == NULL) { - A_.reset(PartitionedMatrixViewBase::Create(options_, A)); + if (A_ == nullptr) { + A_ = PartitionedMatrixViewBase::Create(options_, A); } D_ = D; @@ -61,10 +59,10 @@ void ImplicitSchurComplement::Init(const BlockSparseMatrix& A, // Initialize temporary storage and compute the block diagonals of // E'E and F'E. - if (block_diagonal_EtE_inverse_ == NULL) { - block_diagonal_EtE_inverse_.reset(A_->CreateBlockDiagonalEtE()); + if (block_diagonal_EtE_inverse_ == nullptr) { + block_diagonal_EtE_inverse_ = A_->CreateBlockDiagonalEtE(); if (options_.preconditioner_type == JACOBI) { - block_diagonal_FtF_inverse_.reset(A_->CreateBlockDiagonalFtF()); + block_diagonal_FtF_inverse_ = A_->CreateBlockDiagonalFtF(); } rhs_.resize(A_->num_cols_f()); rhs_.setZero(); @@ -84,7 +82,7 @@ void ImplicitSchurComplement::Init(const BlockSparseMatrix& A, // the block diagonals and invert them. AddDiagonalAndInvert(D_, block_diagonal_EtE_inverse_.get()); if (options_.preconditioner_type == JACOBI) { - AddDiagonalAndInvert((D_ == NULL) ? NULL : D_ + A_->num_cols_e(), + AddDiagonalAndInvert((D_ == nullptr) ? nullptr : D_ + A_->num_cols_e(), block_diagonal_FtF_inverse_.get()); } @@ -118,7 +116,7 @@ void ImplicitSchurComplement::RightMultiply(const double* x, double* y) const { A_->RightMultiplyE(tmp_e_cols_2_.data(), tmp_rows_.data()); // y5 = D * x - if (D_ != NULL) { + if (D_ != nullptr) { ConstVectorRef Dref(D_ + A_->num_cols_e(), num_cols()); VectorRef(y, num_cols()) = (Dref.array().square() * ConstVectorRef(x, num_cols()).array()) @@ -138,15 +136,15 @@ void ImplicitSchurComplement::AddDiagonalAndInvert( const double* D, BlockSparseMatrix* block_diagonal) { const CompressedRowBlockStructure* block_diagonal_structure = block_diagonal->block_structure(); - for (int r = 0; r < block_diagonal_structure->rows.size(); ++r) { - const int row_block_pos = block_diagonal_structure->rows[r].block.position; - const int row_block_size = block_diagonal_structure->rows[r].block.size; - const Cell& cell = block_diagonal_structure->rows[r].cells[0]; + for (const auto& row : block_diagonal_structure->rows) { + const int row_block_pos = row.block.position; + const int row_block_size = row.block.size; + const Cell& cell = row.cells[0]; MatrixRef m(block_diagonal->mutable_values() + cell.position, row_block_size, row_block_size); - if (D != NULL) { + if (D != nullptr) { ConstVectorRef d(D + row_block_pos, row_block_size); m += d.array().square().matrix().asDiagonal(); } diff --git a/extern/ceres/internal/ceres/implicit_schur_complement.h b/extern/ceres/internal/ceres/implicit_schur_complement.h index e83892af017..598d48411aa 100644 --- a/extern/ceres/internal/ceres/implicit_schur_complement.h +++ b/extern/ceres/internal/ceres/implicit_schur_complement.h @@ -36,8 +36,9 @@ #include <memory> +#include "ceres/internal/disable_warnings.h" #include "ceres/internal/eigen.h" -#include "ceres/internal/port.h" +#include "ceres/internal/export.h" #include "ceres/linear_operator.h" #include "ceres/linear_solver.h" #include "ceres/partitioned_matrix_view.h" @@ -88,7 +89,7 @@ class BlockSparseMatrix; // RightMultiply (and the LeftMultiply) methods are not thread safe as // they depend on mutable arrays used for the temporaries needed to // compute the product y += Sx; -class CERES_EXPORT_INTERNAL ImplicitSchurComplement : public LinearOperator { +class CERES_NO_EXPORT ImplicitSchurComplement final : public LinearOperator { public: // num_eliminate_blocks is the number of E blocks in the matrix // A. @@ -100,7 +101,6 @@ class CERES_EXPORT_INTERNAL ImplicitSchurComplement : public LinearOperator { // TODO(sameeragarwal): Get rid of the two bools below and replace // them with enums. explicit ImplicitSchurComplement(const LinearSolver::Options& options); - virtual ~ImplicitSchurComplement(); // Initialize the Schur complement for a linear least squares // problem of the form @@ -166,4 +166,6 @@ class CERES_EXPORT_INTERNAL ImplicitSchurComplement : public LinearOperator { } // namespace internal } // namespace ceres +#include "ceres/internal/reenable_warnings.h" + #endif // CERES_INTERNAL_IMPLICIT_SCHUR_COMPLEMENT_H_ diff --git a/extern/ceres/internal/ceres/inner_product_computer.cc b/extern/ceres/internal/ceres/inner_product_computer.cc index ef38b7b3ad4..fbc43bfed8b 100644 --- a/extern/ceres/internal/ceres/inner_product_computer.cc +++ b/extern/ceres/internal/ceres/inner_product_computer.cc @@ -31,6 +31,7 @@ #include "ceres/inner_product_computer.h" #include <algorithm> +#include <memory> #include "ceres/small_blas.h" @@ -44,11 +45,12 @@ namespace internal { // or the lower triangular part of the product. // // num_nonzeros is the number of non-zeros in the result matrix. -CompressedRowSparseMatrix* InnerProductComputer::CreateResultMatrix( +std::unique_ptr<CompressedRowSparseMatrix> +InnerProductComputer::CreateResultMatrix( const CompressedRowSparseMatrix::StorageType storage_type, const int num_nonzeros) { - CompressedRowSparseMatrix* matrix = - new CompressedRowSparseMatrix(m_.num_cols(), m_.num_cols(), num_nonzeros); + auto matrix = std::make_unique<CompressedRowSparseMatrix>( + m_.num_cols(), m_.num_cols(), num_nonzeros); matrix->set_storage_type(storage_type); const CompressedRowBlockStructure* bs = m_.block_structure(); @@ -116,14 +118,14 @@ InnerProductComputer::InnerProductComputer(const BlockSparseMatrix& m, // // product_storage_type controls the form of the output matrix. It // can be LOWER_TRIANGULAR or UPPER_TRIANGULAR. -InnerProductComputer* InnerProductComputer::Create( +std::unique_ptr<InnerProductComputer> InnerProductComputer::Create( const BlockSparseMatrix& m, CompressedRowSparseMatrix::StorageType product_storage_type) { return InnerProductComputer::Create( m, 0, m.block_structure()->rows.size(), product_storage_type); } -InnerProductComputer* InnerProductComputer::Create( +std::unique_ptr<InnerProductComputer> InnerProductComputer::Create( const BlockSparseMatrix& m, const int start_row_block, const int end_row_block, @@ -132,8 +134,8 @@ InnerProductComputer* InnerProductComputer::Create( product_storage_type == CompressedRowSparseMatrix::UPPER_TRIANGULAR); CHECK_GT(m.num_nonzeros(), 0) << "Congratulations, you found a bug in Ceres. Please report it."; - InnerProductComputer* inner_product_computer = - new InnerProductComputer(m, start_row_block, end_row_block); + std::unique_ptr<InnerProductComputer> inner_product_computer( + new InnerProductComputer(m, start_row_block, end_row_block)); inner_product_computer->Init(product_storage_type); return inner_product_computer; } @@ -165,8 +167,8 @@ void InnerProductComputer::Init( for (int c2 = c2_begin; c2 < c2_end; ++c2) { const Cell& cell2 = row.cells[c2]; - product_terms.push_back(InnerProductComputer::ProductTerm( - cell1.block_id, cell2.block_id, product_terms.size())); + product_terms.emplace_back( + cell1.block_id, cell2.block_id, product_terms.size()); } } } @@ -183,7 +185,7 @@ void InnerProductComputer::ComputeOffsetsAndCreateResultMatrix( std::vector<int> row_block_nnz; const int num_nonzeros = ComputeNonzeros(product_terms, &row_block_nnz); - result_.reset(CreateResultMatrix(product_storage_type, num_nonzeros)); + result_ = CreateResultMatrix(product_storage_type, num_nonzeros); // Populate the row non-zero counts in the result matrix. int* crsm_rows = result_->mutable_rows(); diff --git a/extern/ceres/internal/ceres/inner_product_computer.h b/extern/ceres/internal/ceres/inner_product_computer.h index 04ec1d18316..c6ed0b23e87 100644 --- a/extern/ceres/internal/ceres/inner_product_computer.h +++ b/extern/ceres/internal/ceres/inner_product_computer.h @@ -36,7 +36,8 @@ #include "ceres/block_sparse_matrix.h" #include "ceres/compressed_row_sparse_matrix.h" -#include "ceres/internal/port.h" +#include "ceres/internal/disable_warnings.h" +#include "ceres/internal/export.h" namespace ceres { namespace internal { @@ -61,7 +62,7 @@ namespace internal { // This is not a problem as sparse linear algebra libraries can ignore // these entries with ease and the space used is minimal/linear in the // size of the matrices. -class CERES_EXPORT_INTERNAL InnerProductComputer { +class CERES_NO_EXPORT InnerProductComputer { public: // Factory // @@ -74,7 +75,7 @@ class CERES_EXPORT_INTERNAL InnerProductComputer { // // The user must ensure that the matrix m is valid for the life time // of this object. - static InnerProductComputer* Create( + static std::unique_ptr<InnerProductComputer> Create( const BlockSparseMatrix& m, CompressedRowSparseMatrix::StorageType storage_type); @@ -83,7 +84,7 @@ class CERES_EXPORT_INTERNAL InnerProductComputer { // // a = m(start_row_block : end_row_block, :); // result = a' * a; - static InnerProductComputer* Create( + static std::unique_ptr<InnerProductComputer> Create( const BlockSparseMatrix& m, int start_row_block, int end_row_block, @@ -127,7 +128,7 @@ class CERES_EXPORT_INTERNAL InnerProductComputer { void Init(CompressedRowSparseMatrix::StorageType storage_type); - CompressedRowSparseMatrix* CreateResultMatrix( + std::unique_ptr<CompressedRowSparseMatrix> CreateResultMatrix( const CompressedRowSparseMatrix::StorageType storage_type, int num_nonzeros); @@ -155,4 +156,6 @@ class CERES_EXPORT_INTERNAL InnerProductComputer { } // namespace internal } // namespace ceres +#include "ceres/internal/reenable_warnings.h" + #endif // CERES_INTERNAL_INNER_PRODUCT_COMPUTER_H_ diff --git a/extern/ceres/internal/ceres/is_close.h b/extern/ceres/internal/ceres/is_close.h index b781a4493ff..a1e4e2f6721 100644 --- a/extern/ceres/internal/ceres/is_close.h +++ b/extern/ceres/internal/ceres/is_close.h @@ -33,21 +33,24 @@ #ifndef CERES_INTERNAL_IS_CLOSE_H_ #define CERES_INTERNAL_IS_CLOSE_H_ -#include "ceres/internal/port.h" +#include "ceres/internal/disable_warnings.h" +#include "ceres/internal/export.h" namespace ceres { namespace internal { // Returns true if x and y have a relative (unsigned) difference less than // relative_precision and false otherwise. Stores the relative and absolute -// difference in relative/absolute_error if non-NULL. If one of the two values -// is exactly zero, the absolute difference will be compared, and relative_error -// will be set to the absolute difference. -CERES_EXPORT_INTERNAL bool IsClose(double x, - double y, - double relative_precision, - double* relative_error, - double* absolute_error); +// difference in relative/absolute_error if non-nullptr. If one of the two +// values is exactly zero, the absolute difference will be compared, and +// relative_error will be set to the absolute difference. +CERES_NO_EXPORT bool IsClose(double x, + double y, + double relative_precision, + double* relative_error, + double* absolute_error); } // namespace internal } // namespace ceres +#include "ceres/internal/reenable_warnings.h" + #endif // CERES_INTERNAL_IS_CLOSE_H_ diff --git a/extern/ceres/internal/ceres/iteration_callback.cc b/extern/ceres/internal/ceres/iteration_callback.cc new file mode 100644 index 00000000000..804811d2807 --- /dev/null +++ b/extern/ceres/internal/ceres/iteration_callback.cc @@ -0,0 +1,37 @@ +// 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: sameeragarwal@google.com (Sameer Agarwal) + +#include "ceres/iteration_callback.h" + +namespace ceres { + +IterationCallback::~IterationCallback() = default; + +} // namespace ceres diff --git a/extern/ceres/internal/ceres/iterative_refiner.cc b/extern/ceres/internal/ceres/iterative_refiner.cc index 5f0bfdd250d..18154690597 100644 --- a/extern/ceres/internal/ceres/iterative_refiner.cc +++ b/extern/ceres/internal/ceres/iterative_refiner.cc @@ -42,7 +42,7 @@ namespace internal { IterativeRefiner::IterativeRefiner(const int max_num_iterations) : max_num_iterations_(max_num_iterations) {} -IterativeRefiner::~IterativeRefiner() {} +IterativeRefiner::~IterativeRefiner() = default; void IterativeRefiner::Allocate(int num_cols) { residual_.resize(num_cols); diff --git a/extern/ceres/internal/ceres/iterative_refiner.h b/extern/ceres/internal/ceres/iterative_refiner.h index 08f8d6762cf..837af178ab4 100644 --- a/extern/ceres/internal/ceres/iterative_refiner.h +++ b/extern/ceres/internal/ceres/iterative_refiner.h @@ -33,10 +33,11 @@ // This include must come before any #ifndef check on Ceres compile options. // clang-format off -#include "ceres/internal/port.h" +#include "ceres/internal/config.h" // clang-format on #include "ceres/internal/eigen.h" +#include "ceres/internal/export.h" namespace ceres { namespace internal { @@ -57,11 +58,11 @@ class SparseMatrix; // Definite linear systems. // // The above iterative loop is run until max_num_iterations is reached. -class CERES_EXPORT_INTERNAL IterativeRefiner { +class CERES_NO_EXPORT IterativeRefiner { public: // max_num_iterations is the number of refinement iterations to // perform. - IterativeRefiner(int max_num_iterations); + explicit IterativeRefiner(int max_num_iterations); // Needed for mocking. virtual ~IterativeRefiner(); diff --git a/extern/ceres/internal/ceres/iterative_schur_complement_solver.cc b/extern/ceres/internal/ceres/iterative_schur_complement_solver.cc index 143df5e5814..bc22d68bc55 100644 --- a/extern/ceres/internal/ceres/iterative_schur_complement_solver.cc +++ b/extern/ceres/internal/ceres/iterative_schur_complement_solver.cc @@ -32,6 +32,7 @@ #include <algorithm> #include <cstring> +#include <utility> #include <vector> #include "Eigen/Dense" @@ -54,10 +55,10 @@ namespace ceres { namespace internal { IterativeSchurComplementSolver::IterativeSchurComplementSolver( - const LinearSolver::Options& options) - : options_(options) {} + LinearSolver::Options options) + : options_(std::move(options)) {} -IterativeSchurComplementSolver::~IterativeSchurComplementSolver() {} +IterativeSchurComplementSolver::~IterativeSchurComplementSolver() = default; LinearSolver::Summary IterativeSchurComplementSolver::SolveImpl( BlockSparseMatrix* A, @@ -69,13 +70,13 @@ LinearSolver::Summary IterativeSchurComplementSolver::SolveImpl( CHECK(A->block_structure() != nullptr); const int num_eliminate_blocks = options_.elimination_groups[0]; // Initialize a ImplicitSchurComplement object. - if (schur_complement_ == NULL) { + if (schur_complement_ == nullptr) { DetectStructure(*(A->block_structure()), num_eliminate_blocks, &options_.row_block_size, &options_.e_block_size, &options_.f_block_size); - schur_complement_.reset(new ImplicitSchurComplement(options_)); + schur_complement_ = std::make_unique<ImplicitSchurComplement>(options_); } schur_complement_->Init(*A, per_solve_options.D, b); @@ -86,7 +87,7 @@ LinearSolver::Summary IterativeSchurComplementSolver::SolveImpl( LinearSolver::Summary summary; summary.num_iterations = 0; summary.termination_type = LINEAR_SOLVER_SUCCESS; - schur_complement_->BackSubstitute(NULL, x); + schur_complement_->BackSubstitute(nullptr, x); return summary; } @@ -104,7 +105,7 @@ LinearSolver::Summary IterativeSchurComplementSolver::SolveImpl( cg_per_solve_options.q_tolerance = per_solve_options.q_tolerance; CreatePreconditioner(A); - if (preconditioner_.get() != NULL) { + if (preconditioner_.get() != nullptr) { if (!preconditioner_->Update(*A, per_solve_options.D)) { LinearSolver::Summary summary; summary.num_iterations = 0; @@ -134,7 +135,7 @@ LinearSolver::Summary IterativeSchurComplementSolver::SolveImpl( void IterativeSchurComplementSolver::CreatePreconditioner( BlockSparseMatrix* A) { if (options_.preconditioner_type == IDENTITY || - preconditioner_.get() != NULL) { + preconditioner_.get() != nullptr) { return; } @@ -149,22 +150,22 @@ void IterativeSchurComplementSolver::CreatePreconditioner( preconditioner_options.e_block_size = options_.e_block_size; preconditioner_options.f_block_size = options_.f_block_size; preconditioner_options.elimination_groups = options_.elimination_groups; - CHECK(options_.context != NULL); + CHECK(options_.context != nullptr); preconditioner_options.context = options_.context; switch (options_.preconditioner_type) { case JACOBI: - preconditioner_.reset(new SparseMatrixPreconditionerWrapper( - schur_complement_->block_diagonal_FtF_inverse())); + preconditioner_ = std::make_unique<SparseMatrixPreconditionerWrapper>( + schur_complement_->block_diagonal_FtF_inverse()); break; case SCHUR_JACOBI: - preconditioner_.reset(new SchurJacobiPreconditioner( - *A->block_structure(), preconditioner_options)); + preconditioner_ = std::make_unique<SchurJacobiPreconditioner>( + *A->block_structure(), preconditioner_options); break; case CLUSTER_JACOBI: case CLUSTER_TRIDIAGONAL: - preconditioner_.reset(new VisibilityBasedPreconditioner( - *A->block_structure(), preconditioner_options)); + preconditioner_ = std::make_unique<VisibilityBasedPreconditioner>( + *A->block_structure(), preconditioner_options); break; default: LOG(FATAL) << "Unknown Preconditioner Type"; diff --git a/extern/ceres/internal/ceres/iterative_schur_complement_solver.h b/extern/ceres/internal/ceres/iterative_schur_complement_solver.h index 37606b32d3a..50f469484f8 100644 --- a/extern/ceres/internal/ceres/iterative_schur_complement_solver.h +++ b/extern/ceres/internal/ceres/iterative_schur_complement_solver.h @@ -33,8 +33,9 @@ #include <memory> +#include "ceres/internal/disable_warnings.h" #include "ceres/internal/eigen.h" -#include "ceres/internal/port.h" +#include "ceres/internal/export.h" #include "ceres/linear_solver.h" #include "ceres/types.h" @@ -69,15 +70,15 @@ class Preconditioner; // a proof of this fact and others related to this solver please see // the section on Domain Decomposition Methods in Saad's book // "Iterative Methods for Sparse Linear Systems". -class CERES_EXPORT_INTERNAL IterativeSchurComplementSolver +class CERES_NO_EXPORT IterativeSchurComplementSolver final : public BlockSparseMatrixSolver { public: - explicit IterativeSchurComplementSolver(const LinearSolver::Options& options); + explicit IterativeSchurComplementSolver(LinearSolver::Options options); IterativeSchurComplementSolver(const IterativeSchurComplementSolver&) = delete; void operator=(const IterativeSchurComplementSolver&) = delete; - virtual ~IterativeSchurComplementSolver(); + ~IterativeSchurComplementSolver() override; private: LinearSolver::Summary SolveImpl(BlockSparseMatrix* A, @@ -96,4 +97,6 @@ class CERES_EXPORT_INTERNAL IterativeSchurComplementSolver } // namespace internal } // namespace ceres +#include "ceres/internal/reenable_warnings.h" + #endif // CERES_INTERNAL_ITERATIVE_SCHUR_COMPLEMENT_SOLVER_H_ diff --git a/extern/ceres/internal/ceres/lapack.cc b/extern/ceres/internal/ceres/lapack.cc deleted file mode 100644 index a159ec70696..00000000000 --- a/extern/ceres/internal/ceres/lapack.cc +++ /dev/null @@ -1,190 +0,0 @@ -// Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2015 Google Inc. All rights reserved. -// http://ceres-solver.org/ -// -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions are met: -// -// * Redistributions of source code must retain the above copyright notice, -// this list of conditions and the following disclaimer. -// * Redistributions in binary form must reproduce the above copyright notice, -// this list of conditions and the following disclaimer in the documentation -// and/or other materials provided with the distribution. -// * Neither the name of Google Inc. nor the names of its contributors may be -// used to endorse or promote products derived from this software without -// specific prior written permission. -// -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE -// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -// POSSIBILITY OF SUCH DAMAGE. -// -// Author: sameeragarwal@google.com (Sameer Agarwal) - -#include "ceres/lapack.h" - -#include "ceres/internal/port.h" -#include "ceres/linear_solver.h" -#include "glog/logging.h" - -#ifndef CERES_NO_LAPACK -// C interface to the LAPACK Cholesky factorization and triangular solve. -extern "C" void dpotrf_(char* uplo, int* n, double* a, int* lda, int* info); - -extern "C" void dpotrs_(char* uplo, - int* n, - int* nrhs, - double* a, - int* lda, - double* b, - int* ldb, - int* info); - -extern "C" void dgels_(char* uplo, - int* m, - int* n, - int* nrhs, - double* a, - int* lda, - double* b, - int* ldb, - double* work, - int* lwork, - int* info); -#endif - -namespace ceres { -namespace internal { - -LinearSolverTerminationType LAPACK::SolveInPlaceUsingCholesky( - int num_rows, - const double* in_lhs, - double* rhs_and_solution, - std::string* message) { -#ifdef CERES_NO_LAPACK - LOG(FATAL) << "Ceres was built without a BLAS library."; - return LINEAR_SOLVER_FATAL_ERROR; -#else - char uplo = 'L'; - int n = num_rows; - int info = 0; - int nrhs = 1; - double* lhs = const_cast<double*>(in_lhs); - - dpotrf_(&uplo, &n, lhs, &n, &info); - if (info < 0) { - LOG(FATAL) << "Congratulations, you found a bug in Ceres." - << "Please report it." - << "LAPACK::dpotrf fatal error." - << "Argument: " << -info << " is invalid."; - return LINEAR_SOLVER_FATAL_ERROR; - } - - if (info > 0) { - *message = StringPrintf( - "LAPACK::dpotrf numerical failure. " - "The leading minor of order %d is not positive definite.", - info); - return LINEAR_SOLVER_FAILURE; - } - - dpotrs_(&uplo, &n, &nrhs, lhs, &n, rhs_and_solution, &n, &info); - if (info < 0) { - LOG(FATAL) << "Congratulations, you found a bug in Ceres." - << "Please report it." - << "LAPACK::dpotrs fatal error." - << "Argument: " << -info << " is invalid."; - return LINEAR_SOLVER_FATAL_ERROR; - } - - *message = "Success"; - return LINEAR_SOLVER_SUCCESS; -#endif -} - -int LAPACK::EstimateWorkSizeForQR(int num_rows, int num_cols) { -#ifdef CERES_NO_LAPACK - LOG(FATAL) << "Ceres was built without a LAPACK library."; - return -1; -#else - char trans = 'N'; - int nrhs = 1; - int lwork = -1; - double work; - int info = 0; - dgels_(&trans, - &num_rows, - &num_cols, - &nrhs, - NULL, - &num_rows, - NULL, - &num_rows, - &work, - &lwork, - &info); - - if (info < 0) { - LOG(FATAL) << "Congratulations, you found a bug in Ceres." - << "Please report it." - << "LAPACK::dgels fatal error." - << "Argument: " << -info << " is invalid."; - } - return static_cast<int>(work); -#endif -} - -LinearSolverTerminationType LAPACK::SolveInPlaceUsingQR( - int num_rows, - int num_cols, - const double* in_lhs, - int work_size, - double* work, - double* rhs_and_solution, - std::string* message) { -#ifdef CERES_NO_LAPACK - LOG(FATAL) << "Ceres was built without a LAPACK library."; - return LINEAR_SOLVER_FATAL_ERROR; -#else - char trans = 'N'; - int m = num_rows; - int n = num_cols; - int nrhs = 1; - int lda = num_rows; - int ldb = num_rows; - int info = 0; - double* lhs = const_cast<double*>(in_lhs); - - dgels_(&trans, - &m, - &n, - &nrhs, - lhs, - &lda, - rhs_and_solution, - &ldb, - work, - &work_size, - &info); - - if (info < 0) { - LOG(FATAL) << "Congratulations, you found a bug in Ceres." - << "Please report it." - << "LAPACK::dgels fatal error." - << "Argument: " << -info << " is invalid."; - } - - *message = "Success."; - return LINEAR_SOLVER_SUCCESS; -#endif -} - -} // namespace internal -} // namespace ceres diff --git a/extern/ceres/internal/ceres/lapack.h b/extern/ceres/internal/ceres/lapack.h deleted file mode 100644 index 5c5bf8bf8b8..00000000000 --- a/extern/ceres/internal/ceres/lapack.h +++ /dev/null @@ -1,101 +0,0 @@ -// Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2015 Google Inc. All rights reserved. -// http://ceres-solver.org/ -// -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions are met: -// -// * Redistributions of source code must retain the above copyright notice, -// this list of conditions and the following disclaimer. -// * Redistributions in binary form must reproduce the above copyright notice, -// this list of conditions and the following disclaimer in the documentation -// and/or other materials provided with the distribution. -// * Neither the name of Google Inc. nor the names of its contributors may be -// used to endorse or promote products derived from this software without -// specific prior written permission. -// -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE -// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -// POSSIBILITY OF SUCH DAMAGE. -// -// Author: sameeragarwal@google.com (Sameer Agarwal) - -#ifndef CERES_INTERNAL_LAPACK_H_ -#define CERES_INTERNAL_LAPACK_H_ - -#include <string> - -#include "ceres/internal/port.h" -#include "ceres/linear_solver.h" - -namespace ceres { -namespace internal { - -class LAPACK { - public: - // Solve - // - // lhs * solution = rhs - // - // using a Cholesky factorization. Here - // lhs is a symmetric positive definite matrix. It is assumed to be - // column major and only the lower triangular part of the matrix is - // referenced. - // - // This function uses the LAPACK dpotrf and dpotrs routines. - // - // The return value and the message string together describe whether - // the solver terminated successfully or not and if so, what was the - // reason for failure. - static LinearSolverTerminationType SolveInPlaceUsingCholesky( - int num_rows, - const double* lhs, - double* rhs_and_solution, - std::string* message); - - // The SolveUsingQR function requires a buffer for its temporary - // computation. This function given the size of the lhs matrix will - // return the size of the buffer needed. - static int EstimateWorkSizeForQR(int num_rows, int num_cols); - - // Solve - // - // lhs * solution = rhs - // - // using a dense QR factorization. lhs is an arbitrary (possibly - // rectangular) matrix with full column rank. - // - // work is an array of size work_size that this routine uses for its - // temporary storage. The optimal size of this array can be obtained - // by calling EstimateWorkSizeForQR. - // - // When calling, rhs_and_solution contains the rhs, and upon return - // the first num_col entries are the solution. - // - // This function uses the LAPACK dgels routine. - // - // The return value and the message string together describe whether - // the solver terminated successfully or not and if so, what was the - // reason for failure. - static LinearSolverTerminationType SolveInPlaceUsingQR( - int num_rows, - int num_cols, - const double* lhs, - int work_size, - double* work, - double* rhs_and_solution, - std::string* message); -}; - -} // namespace internal -} // namespace ceres - -#endif // CERES_INTERNAL_LAPACK_H_ diff --git a/extern/ceres/internal/ceres/levenberg_marquardt_strategy.cc b/extern/ceres/internal/ceres/levenberg_marquardt_strategy.cc index cb0e9371b75..2445f5bb99a 100644 --- a/extern/ceres/internal/ceres/levenberg_marquardt_strategy.cc +++ b/extern/ceres/internal/ceres/levenberg_marquardt_strategy.cc @@ -61,7 +61,7 @@ LevenbergMarquardtStrategy::LevenbergMarquardtStrategy( CHECK_GT(max_radius_, 0.0); } -LevenbergMarquardtStrategy::~LevenbergMarquardtStrategy() {} +LevenbergMarquardtStrategy::~LevenbergMarquardtStrategy() = default; TrustRegionStrategy::Summary LevenbergMarquardtStrategy::ComputeStep( const TrustRegionStrategy::PerSolveOptions& per_solve_options, diff --git a/extern/ceres/internal/ceres/levenberg_marquardt_strategy.h b/extern/ceres/internal/ceres/levenberg_marquardt_strategy.h index 12cd463c152..4383a493cde 100644 --- a/extern/ceres/internal/ceres/levenberg_marquardt_strategy.h +++ b/extern/ceres/internal/ceres/levenberg_marquardt_strategy.h @@ -31,8 +31,9 @@ #ifndef CERES_INTERNAL_LEVENBERG_MARQUARDT_STRATEGY_H_ #define CERES_INTERNAL_LEVENBERG_MARQUARDT_STRATEGY_H_ +#include "ceres/internal/disable_warnings.h" #include "ceres/internal/eigen.h" -#include "ceres/internal/port.h" +#include "ceres/internal/export.h" #include "ceres/trust_region_strategy.h" namespace ceres { @@ -43,12 +44,12 @@ namespace internal { // K. Madsen, H.B. Nielsen and O. Tingleff. Available to download from // // http://www2.imm.dtu.dk/pubdb/views/edoc_download.php/3215/pdf/imm3215.pdf -class CERES_EXPORT_INTERNAL LevenbergMarquardtStrategy +class CERES_NO_EXPORT LevenbergMarquardtStrategy final : public TrustRegionStrategy { public: explicit LevenbergMarquardtStrategy( const TrustRegionStrategy::Options& options); - virtual ~LevenbergMarquardtStrategy(); + ~LevenbergMarquardtStrategy() override; // TrustRegionStrategy interface TrustRegionStrategy::Summary ComputeStep( @@ -86,4 +87,6 @@ class CERES_EXPORT_INTERNAL LevenbergMarquardtStrategy } // namespace internal } // namespace ceres +#include "ceres/internal/reenable_warnings.h" + #endif // CERES_INTERNAL_LEVENBERG_MARQUARDT_STRATEGY_H_ diff --git a/extern/ceres/internal/ceres/line_search.cc b/extern/ceres/internal/ceres/line_search.cc index 7e871a20a53..7e7d97f6d93 100644 --- a/extern/ceres/internal/ceres/line_search.cc +++ b/extern/ceres/internal/ceres/line_search.cc @@ -34,6 +34,7 @@ #include <cmath> #include <iomanip> #include <iostream> // NOLINT +#include <memory> #include "ceres/evaluator.h" #include "ceres/function_sample.h" @@ -65,27 +66,26 @@ ostream& operator<<(ostream& os, const FunctionSample& sample) { return os; } +LineSearch::~LineSearch() = default; + LineSearch::LineSearch(const LineSearch::Options& options) : options_(options) {} -LineSearch* LineSearch::Create(const LineSearchType line_search_type, - const LineSearch::Options& options, - string* error) { - LineSearch* line_search = NULL; +std::unique_ptr<LineSearch> LineSearch::Create( + const LineSearchType line_search_type, + const LineSearch::Options& options, + string* error) { switch (line_search_type) { case ceres::ARMIJO: - line_search = new ArmijoLineSearch(options); - break; + return std::make_unique<ArmijoLineSearch>(options); case ceres::WOLFE: - line_search = new WolfeLineSearch(options); - break; + return std::make_unique<WolfeLineSearch>(options); default: *error = string("Invalid line search algorithm type: ") + LineSearchTypeToString(line_search_type) + string(", unable to create line search."); - return NULL; } - return line_search; + return nullptr; } LineSearchFunction::LineSearchFunction(Evaluator* evaluator) @@ -119,13 +119,13 @@ void LineSearchFunction::Evaluate(const double x, } output->vector_x_is_valid = true; - double* gradient = NULL; + double* gradient = nullptr; if (evaluate_gradient) { output->vector_gradient.resize(direction_.rows(), 1); gradient = output->vector_gradient.data(); } const bool eval_status = evaluator_->Evaluate( - output->vector_x.data(), &(output->value), NULL, gradient, NULL); + output->vector_x.data(), &(output->value), nullptr, gradient, nullptr); if (!eval_status || !std::isfinite(output->value)) { return; @@ -249,12 +249,12 @@ double LineSearch::InterpolatingPolynomialMinimizingStepSize( if (interpolation_type == QUADRATIC) { // Two point interpolation using function values and the // gradient at the lower bound. - samples.push_back(FunctionSample(current.x, current.value)); + samples.emplace_back(current.x, current.value); if (previous.value_is_valid) { // Three point interpolation, using function values and the // gradient at the lower bound. - samples.push_back(FunctionSample(previous.x, previous.value)); + samples.emplace_back(previous.x, previous.value); } } else if (interpolation_type == CUBIC) { // Two point interpolation using the function values and the gradients. diff --git a/extern/ceres/internal/ceres/line_search.h b/extern/ceres/internal/ceres/line_search.h index 634c9717532..c2c744afe00 100644 --- a/extern/ceres/internal/ceres/line_search.h +++ b/extern/ceres/internal/ceres/line_search.h @@ -33,12 +33,13 @@ #ifndef CERES_INTERNAL_LINE_SEARCH_H_ #define CERES_INTERNAL_LINE_SEARCH_H_ +#include <memory> #include <string> #include <vector> #include "ceres/function_sample.h" #include "ceres/internal/eigen.h" -#include "ceres/internal/port.h" +#include "ceres/internal/export.h" #include "ceres/types.h" namespace ceres { @@ -57,11 +58,11 @@ class LineSearchFunction; // sufficient decrease condition. Depending on the particular // condition used, we get a variety of different line search // algorithms, e.g., Armijo, Wolfe etc. -class LineSearch { +class CERES_NO_EXPORT LineSearch { public: struct Summary; - struct Options { + struct CERES_NO_EXPORT Options { // Degree of the polynomial used to approximate the objective // function. LineSearchInterpolationType interpolation_type = CUBIC; @@ -161,11 +162,12 @@ class LineSearch { }; explicit LineSearch(const LineSearch::Options& options); - virtual ~LineSearch() {} + virtual ~LineSearch(); - static LineSearch* Create(const LineSearchType line_search_type, - const LineSearch::Options& options, - std::string* error); + static std::unique_ptr<LineSearch> Create( + const LineSearchType line_search_type, + const LineSearch::Options& options, + std::string* error); // Perform the line search. // @@ -208,7 +210,7 @@ class LineSearch { // In practice, this object provides access to the objective // function value and the directional derivative of the underlying // optimization problem along a specific search direction. -class LineSearchFunction { +class CERES_NO_EXPORT LineSearchFunction { public: explicit LineSearchFunction(Evaluator* evaluator); void Init(const Vector& position, const Vector& direction); @@ -257,10 +259,9 @@ class LineSearchFunction { // minFunc package by Mark Schmidt. // // For more details: http://www.di.ens.fr/~mschmidt/Software/minFunc.html -class ArmijoLineSearch : public LineSearch { +class CERES_NO_EXPORT ArmijoLineSearch final : public LineSearch { public: explicit ArmijoLineSearch(const LineSearch::Options& options); - virtual ~ArmijoLineSearch() {} private: void DoSearch(double step_size_estimate, @@ -276,10 +277,9 @@ class ArmijoLineSearch : public LineSearch { // // [1] Nocedal J., Wright S., Numerical Optimization, 2nd Ed., Springer, 1999. // [2] http://www.di.ens.fr/~mschmidt/Software/minFunc.html. -class WolfeLineSearch : public LineSearch { +class CERES_NO_EXPORT WolfeLineSearch final : public LineSearch { public: explicit WolfeLineSearch(const LineSearch::Options& options); - virtual ~WolfeLineSearch() {} // Returns true iff either a valid point, or valid bracket are found. bool BracketingPhase(const FunctionSample& initial_position, diff --git a/extern/ceres/internal/ceres/line_search_direction.cc b/extern/ceres/internal/ceres/line_search_direction.cc index 48e6c9812e9..98e335a8029 100644 --- a/extern/ceres/internal/ceres/line_search_direction.cc +++ b/extern/ceres/internal/ceres/line_search_direction.cc @@ -30,7 +30,10 @@ #include "ceres/line_search_direction.h" +#include <memory> + #include "ceres/internal/eigen.h" +#include "ceres/internal/export.h" #include "ceres/line_search_minimizer.h" #include "ceres/low_rank_inverse_hessian.h" #include "glog/logging.h" @@ -38,18 +41,18 @@ namespace ceres { namespace internal { -class SteepestDescent : public LineSearchDirection { +class CERES_NO_EXPORT SteepestDescent final : public LineSearchDirection { public: - virtual ~SteepestDescent() {} bool NextDirection(const LineSearchMinimizer::State& previous, const LineSearchMinimizer::State& current, - Vector* search_direction) { + Vector* search_direction) override { *search_direction = -current.gradient; return true; } }; -class NonlinearConjugateGradient : public LineSearchDirection { +class CERES_NO_EXPORT NonlinearConjugateGradient final + : public LineSearchDirection { public: NonlinearConjugateGradient(const NonlinearConjugateGradientType type, const double function_tolerance) @@ -57,7 +60,7 @@ class NonlinearConjugateGradient : public LineSearchDirection { bool NextDirection(const LineSearchMinimizer::State& previous, const LineSearchMinimizer::State& current, - Vector* search_direction) { + Vector* search_direction) override { double beta = 0.0; Vector gradient_change; switch (type_) { @@ -95,7 +98,7 @@ class NonlinearConjugateGradient : public LineSearchDirection { const double function_tolerance_; }; -class LBFGS : public LineSearchDirection { +class CERES_NO_EXPORT LBFGS final : public LineSearchDirection { public: LBFGS(const int num_parameters, const int max_lbfgs_rank, @@ -105,11 +108,9 @@ class LBFGS : public LineSearchDirection { use_approximate_eigenvalue_bfgs_scaling), is_positive_definite_(true) {} - virtual ~LBFGS() {} - bool NextDirection(const LineSearchMinimizer::State& previous, const LineSearchMinimizer::State& current, - Vector* search_direction) { + Vector* search_direction) override { CHECK(is_positive_definite_) << "Ceres bug: NextDirection() called on L-BFGS after inverse Hessian " << "approximation has become indefinite, please contact the " @@ -141,7 +142,7 @@ class LBFGS : public LineSearchDirection { bool is_positive_definite_; }; -class BFGS : public LineSearchDirection { +class CERES_NO_EXPORT BFGS final : public LineSearchDirection { public: BFGS(const int num_parameters, const bool use_approximate_eigenvalue_scaling) : num_parameters_(num_parameters), @@ -161,11 +162,9 @@ class BFGS : public LineSearchDirection { inverse_hessian_ = Matrix::Identity(num_parameters, num_parameters); } - virtual ~BFGS() {} - bool NextDirection(const LineSearchMinimizer::State& previous, const LineSearchMinimizer::State& current, - Vector* search_direction) { + Vector* search_direction) override { CHECK(is_positive_definite_) << "Ceres bug: NextDirection() called on BFGS after inverse Hessian " << "approximation has become indefinite, please contact the " @@ -338,32 +337,34 @@ class BFGS : public LineSearchDirection { bool is_positive_definite_; }; -LineSearchDirection* LineSearchDirection::Create( +LineSearchDirection::~LineSearchDirection() = default; + +std::unique_ptr<LineSearchDirection> LineSearchDirection::Create( const LineSearchDirection::Options& options) { if (options.type == STEEPEST_DESCENT) { - return new SteepestDescent; + return std::make_unique<SteepestDescent>(); } if (options.type == NONLINEAR_CONJUGATE_GRADIENT) { - return new NonlinearConjugateGradient( + return std::make_unique<NonlinearConjugateGradient>( options.nonlinear_conjugate_gradient_type, options.function_tolerance); } if (options.type == ceres::LBFGS) { - return new ceres::internal::LBFGS( + return std::make_unique<ceres::internal::LBFGS>( options.num_parameters, options.max_lbfgs_rank, options.use_approximate_eigenvalue_bfgs_scaling); } if (options.type == ceres::BFGS) { - return new ceres::internal::BFGS( + return std::make_unique<ceres::internal::BFGS>( options.num_parameters, options.use_approximate_eigenvalue_bfgs_scaling); } LOG(ERROR) << "Unknown line search direction type: " << options.type; - return NULL; + return nullptr; } } // namespace internal diff --git a/extern/ceres/internal/ceres/line_search_direction.h b/extern/ceres/internal/ceres/line_search_direction.h index 2fcf4729ca5..47b256d7133 100644 --- a/extern/ceres/internal/ceres/line_search_direction.h +++ b/extern/ceres/internal/ceres/line_search_direction.h @@ -1,5 +1,5 @@ // Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2015 Google Inc. All rights reserved. +// Copyright 2022 Google Inc. All rights reserved. // http://ceres-solver.org/ // // Redistribution and use in source and binary forms, with or without @@ -31,35 +31,31 @@ #ifndef CERES_INTERNAL_LINE_SEARCH_DIRECTION_H_ #define CERES_INTERNAL_LINE_SEARCH_DIRECTION_H_ +#include <memory> + #include "ceres/internal/eigen.h" +#include "ceres/internal/export.h" #include "ceres/line_search_minimizer.h" #include "ceres/types.h" namespace ceres { namespace internal { -class LineSearchDirection { +class CERES_NO_EXPORT LineSearchDirection { public: struct Options { - Options() - : num_parameters(0), - type(LBFGS), - nonlinear_conjugate_gradient_type(FLETCHER_REEVES), - function_tolerance(1e-12), - max_lbfgs_rank(20), - use_approximate_eigenvalue_bfgs_scaling(true) {} - - int num_parameters; - LineSearchDirectionType type; - NonlinearConjugateGradientType nonlinear_conjugate_gradient_type; - double function_tolerance; - int max_lbfgs_rank; - bool use_approximate_eigenvalue_bfgs_scaling; + int num_parameters{0}; + LineSearchDirectionType type{LBFGS}; + NonlinearConjugateGradientType nonlinear_conjugate_gradient_type{ + FLETCHER_REEVES}; + double function_tolerance{1e-12}; + int max_lbfgs_rank{20}; + bool use_approximate_eigenvalue_bfgs_scaling{true}; }; - static LineSearchDirection* Create(const Options& options); + static std::unique_ptr<LineSearchDirection> Create(const Options& options); - virtual ~LineSearchDirection() {} + virtual ~LineSearchDirection(); virtual bool NextDirection(const LineSearchMinimizer::State& previous, const LineSearchMinimizer::State& current, Vector* search_direction) = 0; diff --git a/extern/ceres/internal/ceres/line_search_minimizer.cc b/extern/ceres/internal/ceres/line_search_minimizer.cc index ea1c5072a14..ad1e1852386 100644 --- a/extern/ceres/internal/ceres/line_search_minimizer.cc +++ b/extern/ceres/internal/ceres/line_search_minimizer.cc @@ -51,7 +51,7 @@ #include "ceres/array_utils.h" #include "ceres/evaluator.h" #include "ceres/internal/eigen.h" -#include "ceres/internal/port.h" +#include "ceres/internal/export.h" #include "ceres/line_search.h" #include "ceres/line_search_direction.h" #include "ceres/stringprintf.h" @@ -171,8 +171,8 @@ void LineSearchMinimizer::Minimize(const Minimizer::Options& options, line_search_direction_options.max_lbfgs_rank = options.max_lbfgs_rank; line_search_direction_options.use_approximate_eigenvalue_bfgs_scaling = options.use_approximate_eigenvalue_bfgs_scaling; - std::unique_ptr<LineSearchDirection> line_search_direction( - LineSearchDirection::Create(line_search_direction_options)); + std::unique_ptr<LineSearchDirection> line_search_direction = + LineSearchDirection::Create(line_search_direction_options); LineSearchFunction line_search_function(evaluator); @@ -280,8 +280,8 @@ void LineSearchMinimizer::Minimize(const Minimizer::Options& options, << options.max_num_line_search_direction_restarts << " [max]."; } - line_search_direction.reset( - LineSearchDirection::Create(line_search_direction_options)); + line_search_direction = + LineSearchDirection::Create(line_search_direction_options); current_state.search_direction = -current_state.gradient; } diff --git a/extern/ceres/internal/ceres/line_search_minimizer.h b/extern/ceres/internal/ceres/line_search_minimizer.h index 79e8dc9e49a..9a0e994dcfc 100644 --- a/extern/ceres/internal/ceres/line_search_minimizer.h +++ b/extern/ceres/internal/ceres/line_search_minimizer.h @@ -32,6 +32,7 @@ #define CERES_INTERNAL_LINE_SEARCH_MINIMIZER_H_ #include "ceres/internal/eigen.h" +#include "ceres/internal/export.h" #include "ceres/minimizer.h" #include "ceres/solver.h" #include "ceres/types.h" @@ -43,7 +44,7 @@ namespace internal { // Generic line search minimization algorithm. // // For example usage, see SolverImpl::Minimize. -class LineSearchMinimizer : public Minimizer { +class CERES_NO_EXPORT LineSearchMinimizer final : public Minimizer { public: struct State { State(int num_parameters, int num_effective_parameters) @@ -63,7 +64,6 @@ class LineSearchMinimizer : public Minimizer { double step_size; }; - ~LineSearchMinimizer() {} void Minimize(const Minimizer::Options& options, double* parameters, Solver::Summary* summary) final; diff --git a/extern/ceres/internal/ceres/line_search_preprocessor.cc b/extern/ceres/internal/ceres/line_search_preprocessor.cc index 6a69425e764..26b8d99a4c7 100644 --- a/extern/ceres/internal/ceres/line_search_preprocessor.cc +++ b/extern/ceres/internal/ceres/line_search_preprocessor.cc @@ -63,15 +63,13 @@ bool SetupEvaluator(PreprocessedProblem* pp) { pp->evaluator_options.context = pp->problem->context(); pp->evaluator_options.evaluation_callback = pp->reduced_program->mutable_evaluation_callback(); - pp->evaluator.reset(Evaluator::Create( - pp->evaluator_options, pp->reduced_program.get(), &pp->error)); - return (pp->evaluator.get() != NULL); + pp->evaluator = Evaluator::Create( + pp->evaluator_options, pp->reduced_program.get(), &pp->error); + return (pp->evaluator.get() != nullptr); } } // namespace -LineSearchPreprocessor::~LineSearchPreprocessor() {} - bool LineSearchPreprocessor::Preprocess(const Solver::Options& options, ProblemImpl* problem, PreprocessedProblem* pp) { @@ -85,10 +83,10 @@ bool LineSearchPreprocessor::Preprocess(const Solver::Options& options, return false; } - pp->reduced_program.reset(program->CreateReducedProgram( - &pp->removed_parameter_blocks, &pp->fixed_cost, &pp->error)); + pp->reduced_program = program->CreateReducedProgram( + &pp->removed_parameter_blocks, &pp->fixed_cost, &pp->error); - if (pp->reduced_program.get() == NULL) { + if (pp->reduced_program.get() == nullptr) { return false; } diff --git a/extern/ceres/internal/ceres/line_search_preprocessor.h b/extern/ceres/internal/ceres/line_search_preprocessor.h index bd426c7f2f6..27e9c2db9b9 100644 --- a/extern/ceres/internal/ceres/line_search_preprocessor.h +++ b/extern/ceres/internal/ceres/line_search_preprocessor.h @@ -31,15 +31,15 @@ #ifndef CERES_INTERNAL_LINE_SEARCH_PREPROCESSOR_H_ #define CERES_INTERNAL_LINE_SEARCH_PREPROCESSOR_H_ -#include "ceres/internal/port.h" +#include "ceres/internal/disable_warnings.h" +#include "ceres/internal/export.h" #include "ceres/preprocessor.h" namespace ceres { namespace internal { -class CERES_EXPORT_INTERNAL LineSearchPreprocessor : public Preprocessor { +class CERES_NO_EXPORT LineSearchPreprocessor final : public Preprocessor { public: - virtual ~LineSearchPreprocessor(); bool Preprocess(const Solver::Options& options, ProblemImpl* problem, PreprocessedProblem* preprocessed_problem) final; @@ -48,4 +48,6 @@ class CERES_EXPORT_INTERNAL LineSearchPreprocessor : public Preprocessor { } // namespace internal } // namespace ceres +#include "ceres/internal/reenable_warnings.h" + #endif // CERES_INTERNAL_LINE_SEARCH_PREPROCESSOR_H_ diff --git a/extern/ceres/internal/ceres/linear_least_squares_problems.cc b/extern/ceres/internal/ceres/linear_least_squares_problems.cc index 299051c5bcf..2d415af338f 100644 --- a/extern/ceres/internal/ceres/linear_least_squares_problems.cc +++ b/extern/ceres/internal/ceres/linear_least_squares_problems.cc @@ -49,7 +49,8 @@ namespace internal { using std::string; -LinearLeastSquaresProblem* CreateLinearLeastSquaresProblemFromId(int id) { +std::unique_ptr<LinearLeastSquaresProblem> +CreateLinearLeastSquaresProblemFromId(int id) { switch (id) { case 0: return LinearLeastSquaresProblem0(); @@ -64,7 +65,7 @@ LinearLeastSquaresProblem* CreateLinearLeastSquaresProblemFromId(int id) { default: LOG(FATAL) << "Unknown problem id requested " << id; } - return NULL; + return nullptr; } /* @@ -85,15 +86,16 @@ D = [1 x_D = [1.78448275; 2.82327586;] */ -LinearLeastSquaresProblem* LinearLeastSquaresProblem0() { - LinearLeastSquaresProblem* problem = new LinearLeastSquaresProblem; +std::unique_ptr<LinearLeastSquaresProblem> LinearLeastSquaresProblem0() { + std::unique_ptr<LinearLeastSquaresProblem> problem = + std::make_unique<LinearLeastSquaresProblem>(); - TripletSparseMatrix* A = new TripletSparseMatrix(3, 2, 6); - problem->b.reset(new double[3]); - problem->D.reset(new double[2]); + auto A = std::make_unique<TripletSparseMatrix>(3, 2, 6); + problem->b = std::make_unique<double[]>(3); + problem->D = std::make_unique<double[]>(2); - problem->x.reset(new double[2]); - problem->x_D.reset(new double[2]); + problem->x = std::make_unique<double[]>(2); + problem->x_D = std::make_unique<double[]>(2); int* Ai = A->mutable_rows(); int* Aj = A->mutable_cols(); @@ -115,7 +117,7 @@ LinearLeastSquaresProblem* LinearLeastSquaresProblem0() { Ax[4] = 6; Ax[5] = -10; A->set_num_nonzeros(6); - problem->A.reset(A); + problem->A = std::move(A); problem->b[0] = 8; problem->b[1] = 18; @@ -161,7 +163,7 @@ LinearLeastSquaresProblem* LinearLeastSquaresProblem0() { S = [ 42.3419 -1.4000 -11.5806 -1.4000 2.6000 1.0000 - 11.5806 1.0000 31.1935] + -11.5806 1.0000 31.1935] r = [ 4.3032 5.4000 @@ -181,15 +183,16 @@ LinearLeastSquaresProblem* LinearLeastSquaresProblem0() { // BlockSparseMatrix version of this problem. // TripletSparseMatrix version. -LinearLeastSquaresProblem* LinearLeastSquaresProblem1() { +std::unique_ptr<LinearLeastSquaresProblem> LinearLeastSquaresProblem1() { int num_rows = 6; int num_cols = 5; - LinearLeastSquaresProblem* problem = new LinearLeastSquaresProblem; - TripletSparseMatrix* A = - new TripletSparseMatrix(num_rows, num_cols, num_rows * num_cols); - problem->b.reset(new double[num_rows]); - problem->D.reset(new double[num_cols]); + std::unique_ptr<LinearLeastSquaresProblem> problem = + std::make_unique<LinearLeastSquaresProblem>(); + auto A = std::make_unique<TripletSparseMatrix>( + num_rows, num_cols, num_rows * num_cols); + problem->b = std::make_unique<double[]>(num_rows); + problem->D = std::make_unique<double[]>(num_cols); problem->num_eliminate_blocks = 2; int* rows = A->mutable_rows(); @@ -271,7 +274,7 @@ LinearLeastSquaresProblem* LinearLeastSquaresProblem1() { A->set_num_nonzeros(nnz); CHECK(A->IsValid()); - problem->A.reset(A); + problem->A = std::move(A); for (int i = 0; i < num_cols; ++i) { problem->D.get()[i] = 1; @@ -285,21 +288,23 @@ LinearLeastSquaresProblem* LinearLeastSquaresProblem1() { } // BlockSparseMatrix version -LinearLeastSquaresProblem* LinearLeastSquaresProblem2() { +std::unique_ptr<LinearLeastSquaresProblem> LinearLeastSquaresProblem2() { int num_rows = 6; int num_cols = 5; - LinearLeastSquaresProblem* problem = new LinearLeastSquaresProblem; + std::unique_ptr<LinearLeastSquaresProblem> problem = + std::make_unique<LinearLeastSquaresProblem>(); - problem->b.reset(new double[num_rows]); - problem->D.reset(new double[num_cols]); + problem->b = std::make_unique<double[]>(num_rows); + problem->D = std::make_unique<double[]>(num_cols); problem->num_eliminate_blocks = 2; - CompressedRowBlockStructure* bs = new CompressedRowBlockStructure; - std::unique_ptr<double[]> values(new double[num_rows * num_cols]); + auto* bs = new CompressedRowBlockStructure; + std::unique_ptr<double[]> values = + std::make_unique<double[]>(num_rows * num_cols); for (int c = 0; c < num_cols; ++c) { - bs->cols.push_back(Block()); + bs->cols.emplace_back(); bs->cols.back().size = 1; bs->cols.back().position = c; } @@ -311,12 +316,12 @@ LinearLeastSquaresProblem* LinearLeastSquaresProblem2() { values[nnz++] = 1; values[nnz++] = 2; - bs->rows.push_back(CompressedRow()); + bs->rows.emplace_back(); CompressedRow& row = bs->rows.back(); row.block.size = 1; row.block.position = 0; - row.cells.push_back(Cell(0, 0)); - row.cells.push_back(Cell(2, 1)); + row.cells.emplace_back(0, 0); + row.cells.emplace_back(2, 1); } // Row 2 @@ -324,12 +329,12 @@ LinearLeastSquaresProblem* LinearLeastSquaresProblem2() { values[nnz++] = 3; values[nnz++] = 4; - bs->rows.push_back(CompressedRow()); + bs->rows.emplace_back(); CompressedRow& row = bs->rows.back(); row.block.size = 1; row.block.position = 1; - row.cells.push_back(Cell(0, 2)); - row.cells.push_back(Cell(3, 3)); + row.cells.emplace_back(0, 2); + row.cells.emplace_back(3, 3); } // Row 3 @@ -337,12 +342,12 @@ LinearLeastSquaresProblem* LinearLeastSquaresProblem2() { values[nnz++] = 5; values[nnz++] = 6; - bs->rows.push_back(CompressedRow()); + bs->rows.emplace_back(); CompressedRow& row = bs->rows.back(); row.block.size = 1; row.block.position = 2; - row.cells.push_back(Cell(1, 4)); - row.cells.push_back(Cell(4, 5)); + row.cells.emplace_back(1, 4); + row.cells.emplace_back(4, 5); } // Row 4 @@ -350,12 +355,12 @@ LinearLeastSquaresProblem* LinearLeastSquaresProblem2() { values[nnz++] = 7; values[nnz++] = 8; - bs->rows.push_back(CompressedRow()); + bs->rows.emplace_back(); CompressedRow& row = bs->rows.back(); row.block.size = 1; row.block.position = 3; - row.cells.push_back(Cell(1, 6)); - row.cells.push_back(Cell(2, 7)); + row.cells.emplace_back(1, 6); + row.cells.emplace_back(2, 7); } // Row 5 @@ -363,12 +368,12 @@ LinearLeastSquaresProblem* LinearLeastSquaresProblem2() { values[nnz++] = 9; values[nnz++] = 1; - bs->rows.push_back(CompressedRow()); + bs->rows.emplace_back(); CompressedRow& row = bs->rows.back(); row.block.size = 1; row.block.position = 4; - row.cells.push_back(Cell(1, 8)); - row.cells.push_back(Cell(2, 9)); + row.cells.emplace_back(1, 8); + row.cells.emplace_back(2, 9); } // Row 6 @@ -377,16 +382,16 @@ LinearLeastSquaresProblem* LinearLeastSquaresProblem2() { values[nnz++] = 1; values[nnz++] = 1; - bs->rows.push_back(CompressedRow()); + bs->rows.emplace_back(); CompressedRow& row = bs->rows.back(); row.block.size = 1; row.block.position = 5; - row.cells.push_back(Cell(2, 10)); - row.cells.push_back(Cell(3, 11)); - row.cells.push_back(Cell(4, 12)); + row.cells.emplace_back(2, 10); + row.cells.emplace_back(3, 11); + row.cells.emplace_back(4, 12); } - BlockSparseMatrix* A = new BlockSparseMatrix(bs); + auto A = std::make_unique<BlockSparseMatrix>(bs); memcpy(A->mutable_values(), values.get(), nnz * sizeof(*A->values())); for (int i = 0; i < num_cols; ++i) { @@ -397,7 +402,7 @@ LinearLeastSquaresProblem* LinearLeastSquaresProblem2() { problem->b.get()[i] = i; } - problem->A.reset(A); + problem->A = std::move(A); return problem; } @@ -418,21 +423,23 @@ LinearLeastSquaresProblem* LinearLeastSquaresProblem2() { 5] */ // BlockSparseMatrix version -LinearLeastSquaresProblem* LinearLeastSquaresProblem3() { +std::unique_ptr<LinearLeastSquaresProblem> LinearLeastSquaresProblem3() { int num_rows = 5; int num_cols = 2; - LinearLeastSquaresProblem* problem = new LinearLeastSquaresProblem; + std::unique_ptr<LinearLeastSquaresProblem> problem = + std::make_unique<LinearLeastSquaresProblem>(); - problem->b.reset(new double[num_rows]); - problem->D.reset(new double[num_cols]); + problem->b = std::make_unique<double[]>(num_rows); + problem->D = std::make_unique<double[]>(num_cols); problem->num_eliminate_blocks = 2; - CompressedRowBlockStructure* bs = new CompressedRowBlockStructure; - std::unique_ptr<double[]> values(new double[num_rows * num_cols]); + auto* bs = new CompressedRowBlockStructure; + std::unique_ptr<double[]> values = + std::make_unique<double[]>(num_rows * num_cols); for (int c = 0; c < num_cols; ++c) { - bs->cols.push_back(Block()); + bs->cols.emplace_back(); bs->cols.back().size = 1; bs->cols.back().position = c; } @@ -442,54 +449,54 @@ LinearLeastSquaresProblem* LinearLeastSquaresProblem3() { // Row 1 { values[nnz++] = 1; - bs->rows.push_back(CompressedRow()); + bs->rows.emplace_back(); CompressedRow& row = bs->rows.back(); row.block.size = 1; row.block.position = 0; - row.cells.push_back(Cell(0, 0)); + row.cells.emplace_back(0, 0); } // Row 2 { values[nnz++] = 3; - bs->rows.push_back(CompressedRow()); + bs->rows.emplace_back(); CompressedRow& row = bs->rows.back(); row.block.size = 1; row.block.position = 1; - row.cells.push_back(Cell(0, 1)); + row.cells.emplace_back(0, 1); } // Row 3 { values[nnz++] = 5; - bs->rows.push_back(CompressedRow()); + bs->rows.emplace_back(); CompressedRow& row = bs->rows.back(); row.block.size = 1; row.block.position = 2; - row.cells.push_back(Cell(1, 2)); + row.cells.emplace_back(1, 2); } // Row 4 { values[nnz++] = 7; - bs->rows.push_back(CompressedRow()); + bs->rows.emplace_back(); CompressedRow& row = bs->rows.back(); row.block.size = 1; row.block.position = 3; - row.cells.push_back(Cell(1, 3)); + row.cells.emplace_back(1, 3); } // Row 5 { values[nnz++] = 9; - bs->rows.push_back(CompressedRow()); + bs->rows.emplace_back(); CompressedRow& row = bs->rows.back(); row.block.size = 1; row.block.position = 4; - row.cells.push_back(Cell(1, 4)); + row.cells.emplace_back(1, 4); } - BlockSparseMatrix* A = new BlockSparseMatrix(bs); + auto A = std::make_unique<BlockSparseMatrix>(bs); memcpy(A->mutable_values(), values.get(), nnz * sizeof(*A->values())); for (int i = 0; i < num_cols; ++i) { @@ -500,7 +507,7 @@ LinearLeastSquaresProblem* LinearLeastSquaresProblem3() { problem->b.get()[i] = i; } - problem->A.reset(A); + problem->A = std::move(A); return problem; } @@ -525,29 +532,31 @@ LinearLeastSquaresProblem* LinearLeastSquaresProblem3() { // // NOTE: This problem is too small and rank deficient to be solved without // the diagonal regularization. -LinearLeastSquaresProblem* LinearLeastSquaresProblem4() { +std::unique_ptr<LinearLeastSquaresProblem> LinearLeastSquaresProblem4() { int num_rows = 3; int num_cols = 7; - LinearLeastSquaresProblem* problem = new LinearLeastSquaresProblem; + std::unique_ptr<LinearLeastSquaresProblem> problem = + std::make_unique<LinearLeastSquaresProblem>(); - problem->b.reset(new double[num_rows]); - problem->D.reset(new double[num_cols]); + problem->b = std::make_unique<double[]>(num_rows); + problem->D = std::make_unique<double[]>(num_cols); problem->num_eliminate_blocks = 1; - CompressedRowBlockStructure* bs = new CompressedRowBlockStructure; - std::unique_ptr<double[]> values(new double[num_rows * num_cols]); + auto* bs = new CompressedRowBlockStructure; + std::unique_ptr<double[]> values = + std::make_unique<double[]>(num_rows * num_cols); // Column block structure - bs->cols.push_back(Block()); + bs->cols.emplace_back(); bs->cols.back().size = 2; bs->cols.back().position = 0; - bs->cols.push_back(Block()); + bs->cols.emplace_back(); bs->cols.back().size = 3; bs->cols.back().position = 2; - bs->cols.push_back(Block()); + bs->cols.emplace_back(); bs->cols.back().size = 2; bs->cols.back().position = 5; @@ -555,18 +564,18 @@ LinearLeastSquaresProblem* LinearLeastSquaresProblem4() { // Row 1 & 2 { - bs->rows.push_back(CompressedRow()); + bs->rows.emplace_back(); CompressedRow& row = bs->rows.back(); row.block.size = 2; row.block.position = 0; - row.cells.push_back(Cell(0, nnz)); + row.cells.emplace_back(0, nnz); values[nnz++] = 1; values[nnz++] = 2; values[nnz++] = 1; values[nnz++] = 4; - row.cells.push_back(Cell(2, nnz)); + row.cells.emplace_back(2, nnz); values[nnz++] = 1; values[nnz++] = 1; values[nnz++] = 5; @@ -575,22 +584,22 @@ LinearLeastSquaresProblem* LinearLeastSquaresProblem4() { // Row 3 { - bs->rows.push_back(CompressedRow()); + bs->rows.emplace_back(); CompressedRow& row = bs->rows.back(); row.block.size = 1; row.block.position = 2; - row.cells.push_back(Cell(1, nnz)); + row.cells.emplace_back(1, nnz); values[nnz++] = 9; values[nnz++] = 0; values[nnz++] = 0; - row.cells.push_back(Cell(2, nnz)); + row.cells.emplace_back(2, nnz); values[nnz++] = 3; values[nnz++] = 1; } - BlockSparseMatrix* A = new BlockSparseMatrix(bs); + auto A = std::make_unique<BlockSparseMatrix>(bs); memcpy(A->mutable_values(), values.get(), nnz * sizeof(*A->values())); for (int i = 0; i < num_cols; ++i) { @@ -601,7 +610,7 @@ LinearLeastSquaresProblem* LinearLeastSquaresProblem4() { problem->b.get()[i] = i; } - problem->A.reset(A); + problem->A = std::move(A); return problem; } @@ -616,15 +625,15 @@ bool DumpLinearLeastSquaresProblemToConsole(const SparseMatrix* A, A->ToDenseMatrix(&AA); LOG(INFO) << "A^T: \n" << AA.transpose(); - if (D != NULL) { + if (D != nullptr) { LOG(INFO) << "A's appended diagonal:\n" << ConstVectorRef(D, A->num_cols()); } - if (b != NULL) { + if (b != nullptr) { LOG(INFO) << "b: \n" << ConstVectorRef(b, A->num_rows()); } - if (x != NULL) { + if (x != nullptr) { LOG(INFO) << "x: \n" << ConstVectorRef(x, A->num_cols()); } return true; @@ -673,21 +682,21 @@ bool DumpLinearLeastSquaresProblemToTextFile(const string& filename_base, A->num_cols()); } - if (D != NULL) { + if (D != nullptr) { string filename = filename_base + "_D.txt"; WriteArrayToFileOrDie(filename, D, A->num_cols()); StringAppendF( &matlab_script, "lsqp.D = load('%s', '-ascii');\n", filename.c_str()); } - if (b != NULL) { + if (b != nullptr) { string filename = filename_base + "_b.txt"; WriteArrayToFileOrDie(filename, b, A->num_rows()); StringAppendF( &matlab_script, "lsqp.b = load('%s', '-ascii');\n", filename.c_str()); } - if (x != NULL) { + if (x != nullptr) { string filename = filename_base + "_x.txt"; WriteArrayToFileOrDie(filename, x, A->num_cols()); StringAppendF( diff --git a/extern/ceres/internal/ceres/linear_least_squares_problems.h b/extern/ceres/internal/ceres/linear_least_squares_problems.h index cddaa9fd4d7..a1f67eb306e 100644 --- a/extern/ceres/internal/ceres/linear_least_squares_problems.h +++ b/extern/ceres/internal/ceres/linear_least_squares_problems.h @@ -35,7 +35,8 @@ #include <string> #include <vector> -#include "ceres/internal/port.h" +#include "ceres/internal/disable_warnings.h" +#include "ceres/internal/export.h" #include "ceres/sparse_matrix.h" namespace ceres { @@ -43,15 +44,15 @@ namespace internal { // Structure defining a linear least squares problem and if possible // ground truth solutions. To be used by various LinearSolver tests. -struct CERES_EXPORT_INTERNAL LinearLeastSquaresProblem { - LinearLeastSquaresProblem() : num_eliminate_blocks(0) {} +struct CERES_NO_EXPORT LinearLeastSquaresProblem { + LinearLeastSquaresProblem() = default; std::unique_ptr<SparseMatrix> A; std::unique_ptr<double[]> b; std::unique_ptr<double[]> D; // If using the schur eliminator then how many of the variable // blocks are e_type blocks. - int num_eliminate_blocks; + int num_eliminate_blocks{0}; // Solution to min_x |Ax - b|^2 std::unique_ptr<double[]> x; @@ -60,17 +61,23 @@ struct CERES_EXPORT_INTERNAL LinearLeastSquaresProblem { }; // Factories for linear least squares problem. -CERES_EXPORT_INTERNAL LinearLeastSquaresProblem* +CERES_NO_EXPORT std::unique_ptr<LinearLeastSquaresProblem> CreateLinearLeastSquaresProblemFromId(int id); -LinearLeastSquaresProblem* LinearLeastSquaresProblem0(); -LinearLeastSquaresProblem* LinearLeastSquaresProblem1(); -LinearLeastSquaresProblem* LinearLeastSquaresProblem2(); -LinearLeastSquaresProblem* LinearLeastSquaresProblem3(); -LinearLeastSquaresProblem* LinearLeastSquaresProblem4(); +CERES_NO_EXPORT +std::unique_ptr<LinearLeastSquaresProblem> LinearLeastSquaresProblem0(); +CERES_NO_EXPORT +std::unique_ptr<LinearLeastSquaresProblem> LinearLeastSquaresProblem1(); +CERES_NO_EXPORT +std::unique_ptr<LinearLeastSquaresProblem> LinearLeastSquaresProblem2(); +CERES_NO_EXPORT +std::unique_ptr<LinearLeastSquaresProblem> LinearLeastSquaresProblem3(); +CERES_NO_EXPORT +std::unique_ptr<LinearLeastSquaresProblem> LinearLeastSquaresProblem4(); // Write the linear least squares problem to disk. The exact format // depends on dump_format_type. +CERES_NO_EXPORT bool DumpLinearLeastSquaresProblem(const std::string& filename_base, DumpFormatType dump_format_type, const SparseMatrix* A, @@ -81,4 +88,6 @@ bool DumpLinearLeastSquaresProblem(const std::string& filename_base, } // namespace internal } // namespace ceres +#include "ceres/internal/reenable_warnings.h" + #endif // CERES_INTERNAL_LINEAR_LEAST_SQUARES_PROBLEMS_H_ diff --git a/extern/ceres/internal/ceres/linear_operator.cc b/extern/ceres/internal/ceres/linear_operator.cc index 548c724267e..88b7cc752d4 100644 --- a/extern/ceres/internal/ceres/linear_operator.cc +++ b/extern/ceres/internal/ceres/linear_operator.cc @@ -33,7 +33,7 @@ namespace ceres { namespace internal { -LinearOperator::~LinearOperator() {} +LinearOperator::~LinearOperator() = default; } // namespace internal } // namespace ceres diff --git a/extern/ceres/internal/ceres/linear_operator.h b/extern/ceres/internal/ceres/linear_operator.h index 9c59fc39c13..c9e6188e2e8 100644 --- a/extern/ceres/internal/ceres/linear_operator.h +++ b/extern/ceres/internal/ceres/linear_operator.h @@ -33,7 +33,7 @@ #ifndef CERES_INTERNAL_LINEAR_OPERATOR_H_ #define CERES_INTERNAL_LINEAR_OPERATOR_H_ -#include "ceres/internal/port.h" +#include "ceres/internal/export.h" #include "ceres/types.h" namespace ceres { @@ -41,7 +41,7 @@ namespace internal { // This is an abstract base class for linear operators. It supports // access to size information and left and right multiply operators. -class CERES_EXPORT_INTERNAL LinearOperator { +class CERES_NO_EXPORT LinearOperator { public: virtual ~LinearOperator(); diff --git a/extern/ceres/internal/ceres/linear_solver.cc b/extern/ceres/internal/ceres/linear_solver.cc index 6cae2488f07..fe324f81301 100644 --- a/extern/ceres/internal/ceres/linear_solver.cc +++ b/extern/ceres/internal/ceres/linear_solver.cc @@ -30,10 +30,13 @@ #include "ceres/linear_solver.h" +#include <memory> + #include "ceres/cgnr_solver.h" #include "ceres/dense_normal_cholesky_solver.h" #include "ceres/dense_qr_solver.h" #include "ceres/dynamic_sparse_normal_cholesky_solver.h" +#include "ceres/internal/config.h" #include "ceres/iterative_schur_complement_solver.h" #include "ceres/schur_complement_solver.h" #include "ceres/sparse_normal_cholesky_solver.h" @@ -43,7 +46,7 @@ namespace ceres { namespace internal { -LinearSolver::~LinearSolver() {} +LinearSolver::~LinearSolver() = default; LinearSolverType LinearSolver::LinearSolverForZeroEBlocks( LinearSolverType linear_solver_type) { @@ -69,50 +72,51 @@ LinearSolverType LinearSolver::LinearSolverForZeroEBlocks( return linear_solver_type; } -LinearSolver* LinearSolver::Create(const LinearSolver::Options& options) { - CHECK(options.context != NULL); +std::unique_ptr<LinearSolver> LinearSolver::Create( + const LinearSolver::Options& options) { + CHECK(options.context != nullptr); switch (options.type) { case CGNR: - return new CgnrSolver(options); + return std::make_unique<CgnrSolver>(options); case SPARSE_NORMAL_CHOLESKY: #if defined(CERES_NO_SPARSE) - return NULL; + return nullptr; #else if (options.dynamic_sparsity) { - return new DynamicSparseNormalCholeskySolver(options); + return std::make_unique<DynamicSparseNormalCholeskySolver>(options); } - return new SparseNormalCholeskySolver(options); + return std::make_unique<SparseNormalCholeskySolver>(options); #endif case SPARSE_SCHUR: #if defined(CERES_NO_SPARSE) - return NULL; + return nullptr; #else - return new SparseSchurComplementSolver(options); + return std::make_unique<SparseSchurComplementSolver>(options); #endif case DENSE_SCHUR: - return new DenseSchurComplementSolver(options); + return std::make_unique<DenseSchurComplementSolver>(options); case ITERATIVE_SCHUR: if (options.use_explicit_schur_complement) { - return new SparseSchurComplementSolver(options); + return std::make_unique<SparseSchurComplementSolver>(options); } else { - return new IterativeSchurComplementSolver(options); + return std::make_unique<IterativeSchurComplementSolver>(options); } case DENSE_QR: - return new DenseQRSolver(options); + return std::make_unique<DenseQRSolver>(options); case DENSE_NORMAL_CHOLESKY: - return new DenseNormalCholeskySolver(options); + return std::make_unique<DenseNormalCholeskySolver>(options); default: LOG(FATAL) << "Unknown linear solver type :" << options.type; - return NULL; // MSVC doesn't understand that LOG(FATAL) never returns. + return nullptr; // MSVC doesn't understand that LOG(FATAL) never returns. } } diff --git a/extern/ceres/internal/ceres/linear_solver.h b/extern/ceres/internal/ceres/linear_solver.h index 49c6527acc9..2f709c297e5 100644 --- a/extern/ceres/internal/ceres/linear_solver.h +++ b/extern/ceres/internal/ceres/linear_solver.h @@ -36,6 +36,7 @@ #include <cstddef> #include <map> +#include <memory> #include <string> #include <vector> @@ -45,7 +46,8 @@ #include "ceres/context_impl.h" #include "ceres/dense_sparse_matrix.h" #include "ceres/execution_summary.h" -#include "ceres/internal/port.h" +#include "ceres/internal/disable_warnings.h" +#include "ceres/internal/export.h" #include "ceres/triplet_sparse_matrix.h" #include "ceres/types.h" #include "glog/logging.h" @@ -101,7 +103,7 @@ class LinearOperator; // The Options struct configures the LinearSolver object for its // lifetime. The PerSolveOptions struct is used to specify options for // a particular Solve call. -class CERES_EXPORT_INTERNAL LinearSolver { +class CERES_NO_EXPORT LinearSolver { public: struct Options { LinearSolverType type = SPARSE_NORMAL_CHOLESKY; @@ -284,11 +286,11 @@ class CERES_EXPORT_INTERNAL LinearSolver { // issues. Further, this calls are not expected to be frequent or // performance sensitive. virtual std::map<std::string, CallStatistics> Statistics() const { - return std::map<std::string, CallStatistics>(); + return {}; } // Factory - static LinearSolver* Create(const Options& options); + static std::unique_ptr<LinearSolver> Create(const Options& options); }; // This templated subclass of LinearSolver serves as a base class for @@ -301,12 +303,11 @@ class CERES_EXPORT_INTERNAL LinearSolver { template <typename MatrixType> class TypedLinearSolver : public LinearSolver { public: - virtual ~TypedLinearSolver() {} - virtual LinearSolver::Summary Solve( + LinearSolver::Summary Solve( LinearOperator* A, const double* b, const LinearSolver::PerSolveOptions& per_solve_options, - double* x) { + double* x) override { ScopedExecutionTimer total_time("LinearSolver::Solve", &execution_summary_); CHECK(A != nullptr); CHECK(b != nullptr); @@ -314,7 +315,7 @@ class TypedLinearSolver : public LinearSolver { return SolveImpl(down_cast<MatrixType*>(A), b, per_solve_options, x); } - virtual std::map<std::string, CallStatistics> Statistics() const { + std::map<std::string, CallStatistics> Statistics() const override { return execution_summary_.statistics(); } @@ -340,4 +341,6 @@ typedef TypedLinearSolver<TripletSparseMatrix> TripletSparseMatrixSolver; } // namespace internal } // namespace ceres +#include "ceres/internal/reenable_warnings.h" + #endif // CERES_INTERNAL_LINEAR_SOLVER_H_ diff --git a/extern/ceres/internal/ceres/local_parameterization.cc b/extern/ceres/internal/ceres/local_parameterization.cc index 62947f06fcc..db6f95a1984 100644 --- a/extern/ceres/internal/ceres/local_parameterization.cc +++ b/extern/ceres/internal/ceres/local_parameterization.cc @@ -1,5 +1,5 @@ // Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2015 Google Inc. All rights reserved. +// Copyright 2022 Google Inc. All rights reserved. // http://ceres-solver.org/ // // Redistribution and use in source and binary forms, with or without @@ -43,7 +43,7 @@ namespace ceres { using std::vector; -LocalParameterization::~LocalParameterization() {} +LocalParameterization::~LocalParameterization() = default; bool LocalParameterization::MultiplyByJacobian(const double* x, const int num_rows, @@ -107,8 +107,8 @@ SubsetParameterization::SubsetParameterization( << "of the parameter block."; CHECK(std::adjacent_find(constant.begin(), constant.end()) == constant.end()) << "The set of constant parameters cannot contain duplicates"; - for (int i = 0; i < constant_parameters.size(); ++i) { - constancy_mask_[constant_parameters[i]] = 1; + for (int parameter : constant_parameters) { + constancy_mask_[parameter] = 1; } } diff --git a/extern/ceres/internal/ceres/loss_function.cc b/extern/ceres/internal/ceres/loss_function.cc index 353f29a5bf5..3392b3b7f81 100644 --- a/extern/ceres/internal/ceres/loss_function.cc +++ b/extern/ceres/internal/ceres/loss_function.cc @@ -39,6 +39,8 @@ namespace ceres { +LossFunction::~LossFunction() = default; + void TrivialLoss::Evaluate(double s, double rho[3]) const { rho[0] = s; rho[1] = 1.0; @@ -161,7 +163,7 @@ void ComposedLoss::Evaluate(double s, double rho[3]) const { } void ScaledLoss::Evaluate(double s, double rho[3]) const { - if (rho_.get() == NULL) { + if (rho_.get() == nullptr) { rho[0] = a_ * s; rho[1] = a_; rho[2] = 0.0; diff --git a/extern/ceres/internal/ceres/low_rank_inverse_hessian.cc b/extern/ceres/internal/ceres/low_rank_inverse_hessian.cc index c73e5dbf88d..2fd1ac83f00 100644 --- a/extern/ceres/internal/ceres/low_rank_inverse_hessian.cc +++ b/extern/ceres/internal/ceres/low_rank_inverse_hessian.cc @@ -127,9 +127,7 @@ void LowRankInverseHessian::RightMultiply(const double* x_ptr, const int num_corrections = indices_.size(); Vector alpha(num_corrections); - for (list<int>::const_reverse_iterator it = indices_.rbegin(); - it != indices_.rend(); - ++it) { + for (auto it = indices_.rbegin(); it != indices_.rend(); ++it) { const double alpha_i = delta_x_history_.col(*it).dot(search_direction) / delta_x_dot_delta_gradient_(*it); search_direction -= alpha_i * delta_gradient_history_.col(*it); diff --git a/extern/ceres/internal/ceres/low_rank_inverse_hessian.h b/extern/ceres/internal/ceres/low_rank_inverse_hessian.h index 0028a988923..36519360262 100644 --- a/extern/ceres/internal/ceres/low_rank_inverse_hessian.h +++ b/extern/ceres/internal/ceres/low_rank_inverse_hessian.h @@ -37,6 +37,7 @@ #include <list> #include "ceres/internal/eigen.h" +#include "ceres/internal/export.h" #include "ceres/linear_operator.h" namespace ceres { @@ -59,7 +60,7 @@ namespace internal { // Byrd, R. H.; Nocedal, J.; Schnabel, R. B. (1994). // "Representations of Quasi-Newton Matrices and their use in // Limited Memory Methods". Mathematical Programming 63 (4): -class LowRankInverseHessian : public LinearOperator { +class CERES_NO_EXPORT LowRankInverseHessian final : public LinearOperator { public: // num_parameters is the row/column size of the Hessian. // max_num_corrections is the rank of the Hessian approximation. @@ -73,7 +74,6 @@ class LowRankInverseHessian : public LinearOperator { LowRankInverseHessian(int num_parameters, int max_num_corrections, bool use_approximate_eigenvalue_scaling); - virtual ~LowRankInverseHessian() {} // Update the low rank approximation. delta_x is the change in the // domain of Hessian, and delta_gradient is the change in the diff --git a/extern/ceres/internal/ceres/manifold.cc b/extern/ceres/internal/ceres/manifold.cc new file mode 100644 index 00000000000..f412a793f93 --- /dev/null +++ b/extern/ceres/internal/ceres/manifold.cc @@ -0,0 +1,321 @@ +#include "ceres/manifold.h" + +#include <algorithm> +#include <cmath> + +#include "ceres/internal/eigen.h" +#include "ceres/internal/fixed_array.h" +#include "glog/logging.h" + +namespace ceres { +namespace { + +struct CeresQuaternionOrder { + static constexpr int kW = 0; + static constexpr int kX = 1; + static constexpr int kY = 2; + static constexpr int kZ = 3; +}; + +struct EigenQuaternionOrder { + static constexpr int kW = 3; + static constexpr int kX = 0; + static constexpr int kY = 1; + static constexpr int kZ = 2; +}; + +template <typename Order> +inline void QuaternionPlusImpl(const double* x, + const double* delta, + double* x_plus_delta) { + // x_plus_delta = QuaternionProduct(q_delta, x), where q_delta is the + // quaternion constructed from delta. + const double norm_delta = std::sqrt( + delta[0] * delta[0] + delta[1] * delta[1] + delta[2] * delta[2]); + + if (norm_delta == 0.0) { + for (int i = 0; i < 4; ++i) { + x_plus_delta[i] = x[i]; + } + return; + } + + const double sin_delta_by_delta = (std::sin(norm_delta) / norm_delta); + double q_delta[4]; + q_delta[Order::kW] = std::cos(norm_delta); + q_delta[Order::kX] = sin_delta_by_delta * delta[0]; + q_delta[Order::kY] = sin_delta_by_delta * delta[1]; + q_delta[Order::kZ] = sin_delta_by_delta * delta[2]; + + x_plus_delta[Order::kW] = + q_delta[Order::kW] * x[Order::kW] - q_delta[Order::kX] * x[Order::kX] - + q_delta[Order::kY] * x[Order::kY] - q_delta[Order::kZ] * x[Order::kZ]; + x_plus_delta[Order::kX] = + q_delta[Order::kW] * x[Order::kX] + q_delta[Order::kX] * x[Order::kW] + + q_delta[Order::kY] * x[Order::kZ] - q_delta[Order::kZ] * x[Order::kY]; + x_plus_delta[Order::kY] = + q_delta[Order::kW] * x[Order::kY] - q_delta[Order::kX] * x[Order::kZ] + + q_delta[Order::kY] * x[Order::kW] + q_delta[Order::kZ] * x[Order::kX]; + x_plus_delta[Order::kZ] = + q_delta[Order::kW] * x[Order::kZ] + q_delta[Order::kX] * x[Order::kY] - + q_delta[Order::kY] * x[Order::kX] + q_delta[Order::kZ] * x[Order::kW]; +} + +template <typename Order> +inline void QuaternionPlusJacobianImpl(const double* x, double* jacobian_ptr) { + Eigen::Map<Eigen::Matrix<double, 4, 3, Eigen::RowMajor>> jacobian( + jacobian_ptr); + + jacobian(Order::kW, 0) = -x[Order::kX]; + jacobian(Order::kW, 1) = -x[Order::kY]; + jacobian(Order::kW, 2) = -x[Order::kZ]; + jacobian(Order::kX, 0) = x[Order::kW]; + jacobian(Order::kX, 1) = x[Order::kZ]; + jacobian(Order::kX, 2) = -x[Order::kY]; + jacobian(Order::kY, 0) = -x[Order::kZ]; + jacobian(Order::kY, 1) = x[Order::kW]; + jacobian(Order::kY, 2) = x[Order::kX]; + jacobian(Order::kZ, 0) = x[Order::kY]; + jacobian(Order::kZ, 1) = -x[Order::kX]; + jacobian(Order::kZ, 2) = x[Order::kW]; +} + +template <typename Order> +inline void QuaternionMinusImpl(const double* y, + const double* x, + double* y_minus_x) { + // ambient_y_minus_x = QuaternionProduct(y, -x) where -x is the conjugate of + // x. + double ambient_y_minus_x[4]; + ambient_y_minus_x[Order::kW] = + y[Order::kW] * x[Order::kW] + y[Order::kX] * x[Order::kX] + + y[Order::kY] * x[Order::kY] + y[Order::kZ] * x[Order::kZ]; + ambient_y_minus_x[Order::kX] = + -y[Order::kW] * x[Order::kX] + y[Order::kX] * x[Order::kW] - + y[Order::kY] * x[Order::kZ] + y[Order::kZ] * x[Order::kY]; + ambient_y_minus_x[Order::kY] = + -y[Order::kW] * x[Order::kY] + y[Order::kX] * x[Order::kZ] + + y[Order::kY] * x[Order::kW] - y[Order::kZ] * x[Order::kX]; + ambient_y_minus_x[Order::kZ] = + -y[Order::kW] * x[Order::kZ] - y[Order::kX] * x[Order::kY] + + y[Order::kY] * x[Order::kX] + y[Order::kZ] * x[Order::kW]; + + const double u_norm = + std::sqrt(ambient_y_minus_x[Order::kX] * ambient_y_minus_x[Order::kX] + + ambient_y_minus_x[Order::kY] * ambient_y_minus_x[Order::kY] + + ambient_y_minus_x[Order::kZ] * ambient_y_minus_x[Order::kZ]); + if (u_norm > 0.0) { + const double theta = std::atan2(u_norm, ambient_y_minus_x[Order::kW]); + y_minus_x[0] = theta * ambient_y_minus_x[Order::kX] / u_norm; + y_minus_x[1] = theta * ambient_y_minus_x[Order::kY] / u_norm; + y_minus_x[2] = theta * ambient_y_minus_x[Order::kZ] / u_norm; + } else { + y_minus_x[0] = 0.0; + y_minus_x[1] = 0.0; + y_minus_x[2] = 0.0; + } +} + +template <typename Order> +inline void QuaternionMinusJacobianImpl(const double* x, double* jacobian_ptr) { + Eigen::Map<Eigen::Matrix<double, 3, 4, Eigen::RowMajor>> jacobian( + jacobian_ptr); + + jacobian(0, Order::kW) = -x[Order::kX]; + jacobian(0, Order::kX) = x[Order::kW]; + jacobian(0, Order::kY) = -x[Order::kZ]; + jacobian(0, Order::kZ) = x[Order::kY]; + jacobian(1, Order::kW) = -x[Order::kY]; + jacobian(1, Order::kX) = x[Order::kZ]; + jacobian(1, Order::kY) = x[Order::kW]; + jacobian(1, Order::kZ) = -x[Order::kX]; + jacobian(2, Order::kW) = -x[Order::kZ]; + jacobian(2, Order::kX) = -x[Order::kY]; + jacobian(2, Order::kY) = x[Order::kX]; + jacobian(2, Order::kZ) = x[Order::kW]; +} + +} // namespace + +Manifold::~Manifold() = default; + +bool Manifold::RightMultiplyByPlusJacobian(const double* x, + const int num_rows, + const double* ambient_matrix, + double* tangent_matrix) const { + const int tangent_size = TangentSize(); + if (tangent_size == 0) { + return true; + } + + const int ambient_size = AmbientSize(); + Matrix plus_jacobian(ambient_size, tangent_size); + if (!PlusJacobian(x, plus_jacobian.data())) { + return false; + } + + MatrixRef(tangent_matrix, num_rows, tangent_size) = + ConstMatrixRef(ambient_matrix, num_rows, ambient_size) * plus_jacobian; + return true; +} + +SubsetManifold::SubsetManifold(const int size, + const std::vector<int>& constant_parameters) + + : tangent_size_(size - constant_parameters.size()), + constancy_mask_(size, false) { + if (constant_parameters.empty()) { + return; + } + + std::vector<int> constant = constant_parameters; + std::sort(constant.begin(), constant.end()); + CHECK_GE(constant.front(), 0) << "Indices indicating constant parameter must " + "be greater than equal to zero."; + CHECK_LT(constant.back(), size) + << "Indices indicating constant parameter must be less than the size " + << "of the parameter block."; + CHECK(std::adjacent_find(constant.begin(), constant.end()) == constant.end()) + << "The set of constant parameters cannot contain duplicates"; + + for (auto index : constant_parameters) { + constancy_mask_[index] = true; + } +} + +int SubsetManifold::AmbientSize() const { return constancy_mask_.size(); } + +int SubsetManifold::TangentSize() const { return tangent_size_; } + +bool SubsetManifold::Plus(const double* x, + const double* delta, + double* x_plus_delta) const { + const int ambient_size = AmbientSize(); + for (int i = 0, j = 0; i < ambient_size; ++i) { + if (constancy_mask_[i]) { + x_plus_delta[i] = x[i]; + } else { + x_plus_delta[i] = x[i] + delta[j++]; + } + } + return true; +} + +bool SubsetManifold::PlusJacobian(const double* x, + double* plus_jacobian) const { + if (tangent_size_ == 0) { + return true; + } + + const int ambient_size = AmbientSize(); + MatrixRef m(plus_jacobian, ambient_size, tangent_size_); + m.setZero(); + for (int r = 0, c = 0; r < ambient_size; ++r) { + if (!constancy_mask_[r]) { + m(r, c++) = 1.0; + } + } + return true; +} + +bool SubsetManifold::RightMultiplyByPlusJacobian(const double* x, + const int num_rows, + const double* ambient_matrix, + double* tangent_matrix) const { + if (tangent_size_ == 0) { + return true; + } + + const int ambient_size = AmbientSize(); + for (int r = 0; r < num_rows; ++r) { + for (int idx = 0, c = 0; idx < ambient_size; ++idx) { + if (!constancy_mask_[idx]) { + tangent_matrix[r * tangent_size_ + c++] = + ambient_matrix[r * ambient_size + idx]; + } + } + } + return true; +} + +bool SubsetManifold::Minus(const double* y, + const double* x, + double* y_minus_x) const { + if (tangent_size_ == 0) { + return true; + } + + const int ambient_size = AmbientSize(); + for (int i = 0, j = 0; i < ambient_size; ++i) { + if (!constancy_mask_[i]) { + y_minus_x[j++] = y[i] - x[i]; + } + } + return true; +} + +bool SubsetManifold::MinusJacobian(const double* x, + double* minus_jacobian) const { + const int ambient_size = AmbientSize(); + MatrixRef m(minus_jacobian, tangent_size_, ambient_size); + m.setZero(); + for (int c = 0, r = 0; c < ambient_size; ++c) { + if (!constancy_mask_[c]) { + m(r++, c) = 1.0; + } + } + return true; +} + +bool QuaternionManifold::Plus(const double* x, + const double* delta, + double* x_plus_delta) const { + QuaternionPlusImpl<CeresQuaternionOrder>(x, delta, x_plus_delta); + return true; +} + +bool QuaternionManifold::PlusJacobian(const double* x, double* jacobian) const { + QuaternionPlusJacobianImpl<CeresQuaternionOrder>(x, jacobian); + return true; +} + +bool QuaternionManifold::Minus(const double* y, + const double* x, + double* y_minus_x) const { + QuaternionMinusImpl<CeresQuaternionOrder>(y, x, y_minus_x); + return true; +} + +bool QuaternionManifold::MinusJacobian(const double* x, + double* jacobian) const { + QuaternionMinusJacobianImpl<CeresQuaternionOrder>(x, jacobian); + return true; +} + +bool EigenQuaternionManifold::Plus(const double* x, + const double* delta, + double* x_plus_delta) const { + QuaternionPlusImpl<EigenQuaternionOrder>(x, delta, x_plus_delta); + return true; +} + +bool EigenQuaternionManifold::PlusJacobian(const double* x, + double* jacobian) const { + QuaternionPlusJacobianImpl<EigenQuaternionOrder>(x, jacobian); + return true; +} + +bool EigenQuaternionManifold::Minus(const double* y, + const double* x, + double* y_minus_x) const { + QuaternionMinusImpl<EigenQuaternionOrder>(y, x, y_minus_x); + return true; +} + +bool EigenQuaternionManifold::MinusJacobian(const double* x, + double* jacobian) const { + QuaternionMinusJacobianImpl<EigenQuaternionOrder>(x, jacobian); + return true; +} + +} // namespace ceres diff --git a/extern/ceres/internal/ceres/manifold_adapter.h b/extern/ceres/internal/ceres/manifold_adapter.h new file mode 100644 index 00000000000..9a21456a731 --- /dev/null +++ b/extern/ceres/internal/ceres/manifold_adapter.h @@ -0,0 +1,60 @@ +#include "ceres/internal/export.h" +#include "ceres/local_parameterization.h" +#include "ceres/manifold.h" +#include "glog/logging.h" + +namespace ceres { +namespace internal { + +// Adapter to wrap LocalParameterization and make them look like Manifolds. +// +// ManifoldAdapter NEVER takes ownership of local_parameterization. +class CERES_NO_EXPORT ManifoldAdapter final : public Manifold { + public: + explicit ManifoldAdapter(const LocalParameterization* local_parameterization) + : local_parameterization_(local_parameterization) { + CHECK(local_parameterization != nullptr); + } + + bool Plus(const double* x, + const double* delta, + double* x_plus_delta) const override { + return local_parameterization_->Plus(x, delta, x_plus_delta); + } + + bool PlusJacobian(const double* x, double* jacobian) const override { + return local_parameterization_->ComputeJacobian(x, jacobian); + } + + bool RightMultiplyByPlusJacobian(const double* x, + const int num_rows, + const double* ambient_matrix, + double* tangent_matrix) const override { + return local_parameterization_->MultiplyByJacobian( + x, num_rows, ambient_matrix, tangent_matrix); + } + + bool Minus(const double* y, const double* x, double* delta) const override { + LOG(FATAL) << "This should never be called."; + return false; + } + + bool MinusJacobian(const double* x, double* jacobian) const override { + LOG(FATAL) << "This should never be called."; + return false; + } + + int AmbientSize() const override { + return local_parameterization_->GlobalSize(); + } + + int TangentSize() const override { + return local_parameterization_->LocalSize(); + } + + private: + const LocalParameterization* local_parameterization_; +}; + +} // namespace internal +} // namespace ceres diff --git a/extern/ceres/internal/ceres/map_util.h b/extern/ceres/internal/ceres/map_util.h index 6e310f8db2d..5632c22e916 100644 --- a/extern/ceres/internal/ceres/map_util.h +++ b/extern/ceres/internal/ceres/map_util.h @@ -35,7 +35,7 @@ #include <utility> -#include "ceres/internal/port.h" +#include "ceres/internal/export.h" #include "glog/logging.h" namespace ceres { @@ -121,7 +121,7 @@ template <class Collection> void InsertOrDie(Collection* const collection, const typename Collection::value_type::first_type& key, const typename Collection::value_type::second_type& data) { - typedef typename Collection::value_type value_type; + using value_type = typename Collection::value_type; CHECK(collection->insert(value_type(key, data)).second) << "duplicate key: " << key; } diff --git a/extern/ceres/internal/ceres/minimizer.cc b/extern/ceres/internal/ceres/minimizer.cc index b96e0c9de44..449c728774d 100644 --- a/extern/ceres/internal/ceres/minimizer.cc +++ b/extern/ceres/internal/ceres/minimizer.cc @@ -30,6 +30,8 @@ #include "ceres/minimizer.h" +#include <memory> + #include "ceres/line_search_minimizer.h" #include "ceres/trust_region_minimizer.h" #include "ceres/types.h" @@ -38,20 +40,20 @@ namespace ceres { namespace internal { -Minimizer* Minimizer::Create(MinimizerType minimizer_type) { +std::unique_ptr<Minimizer> Minimizer::Create(MinimizerType minimizer_type) { if (minimizer_type == TRUST_REGION) { - return new TrustRegionMinimizer; + return std::make_unique<TrustRegionMinimizer>(); } if (minimizer_type == LINE_SEARCH) { - return new LineSearchMinimizer; + return std::make_unique<LineSearchMinimizer>(); } LOG(FATAL) << "Unknown minimizer_type: " << minimizer_type; - return NULL; + return nullptr; } -Minimizer::~Minimizer() {} +Minimizer::~Minimizer() = default; bool Minimizer::RunCallbacks(const Minimizer::Options& options, const IterationSummary& iteration_summary, diff --git a/extern/ceres/internal/ceres/minimizer.h b/extern/ceres/internal/ceres/minimizer.h index 246550de7cd..c2c1f71df9f 100644 --- a/extern/ceres/internal/ceres/minimizer.h +++ b/extern/ceres/internal/ceres/minimizer.h @@ -35,7 +35,8 @@ #include <string> #include <vector> -#include "ceres/internal/port.h" +#include "ceres/internal/disable_warnings.h" +#include "ceres/internal/export.h" #include "ceres/iteration_callback.h" #include "ceres/solver.h" @@ -49,7 +50,7 @@ class CoordinateDescentMinimizer; class LinearSolver; // Interface for non-linear least squares solvers. -class CERES_EXPORT_INTERNAL Minimizer { +class CERES_NO_EXPORT Minimizer { public: // Options struct to control the behaviour of the Minimizer. Please // see solver.h for detailed information about the meaning and @@ -178,7 +179,7 @@ class CERES_EXPORT_INTERNAL Minimizer { std::shared_ptr<CoordinateDescentMinimizer> inner_iteration_minimizer; }; - static Minimizer* Create(MinimizerType minimizer_type); + static std::unique_ptr<Minimizer> Create(MinimizerType minimizer_type); static bool RunCallbacks(const Options& options, const IterationSummary& iteration_summary, Solver::Summary* summary); @@ -195,4 +196,6 @@ class CERES_EXPORT_INTERNAL Minimizer { } // namespace internal } // namespace ceres +#include "ceres/internal/reenable_warnings.h" + #endif // CERES_INTERNAL_MINIMIZER_H_ diff --git a/extern/ceres/internal/ceres/normal_prior.cc b/extern/ceres/internal/ceres/normal_prior.cc index 4a62132dbda..17de40f2e77 100644 --- a/extern/ceres/internal/ceres/normal_prior.cc +++ b/extern/ceres/internal/ceres/normal_prior.cc @@ -1,5 +1,5 @@ // Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2015 Google Inc. All rights reserved. +// Copyright 2022 Google Inc. All rights reserved. // http://ceres-solver.org/ // // Redistribution and use in source and binary forms, with or without @@ -56,7 +56,7 @@ bool NormalPrior::Evaluate(double const* const* parameters, // r = A_ * (p - b_); // The extra eval is to get around a bug in the eigen library. r = A_ * (p - b_).eval(); - if ((jacobians != NULL) && (jacobians[0] != NULL)) { + if ((jacobians != nullptr) && (jacobians[0] != nullptr)) { MatrixRef(jacobians[0], num_residuals(), parameter_block_sizes()[0]) = A_; } return true; diff --git a/extern/ceres/internal/ceres/pair_hash.h b/extern/ceres/internal/ceres/pair_hash.h index abbedccf961..83ff2b46401 100644 --- a/extern/ceres/internal/ceres/pair_hash.h +++ b/extern/ceres/internal/ceres/pair_hash.h @@ -1,5 +1,5 @@ // Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2018 Google Inc. All rights reserved. +// Copyright 2022 Google Inc. All rights reserved. // http://ceres-solver.org/ // // Redistribution and use in source and binary forms, with or without @@ -33,10 +33,12 @@ #ifndef CERES_INTERNAL_PAIR_HASH_H_ #define CERES_INTERNAL_PAIR_HASH_H_ +#include <cstddef> #include <cstdint> +#include <functional> #include <utility> -#include "ceres/internal/port.h" +#include "ceres/internal/export.h" namespace ceres { namespace internal { diff --git a/extern/ceres/internal/ceres/parallel_for.h b/extern/ceres/internal/ceres/parallel_for.h index b64bd310650..9528c267d49 100644 --- a/extern/ceres/internal/ceres/parallel_for.h +++ b/extern/ceres/internal/ceres/parallel_for.h @@ -28,37 +28,38 @@ // // Author: vitus@google.com (Michael Vitus) -#ifndef CERES_INTERNAL_PARALLEL_FOR_ -#define CERES_INTERNAL_PARALLEL_FOR_ +#ifndef CERES_INTERNAL_PARALLEL_FOR_H_ +#define CERES_INTERNAL_PARALLEL_FOR_H_ #include <functional> #include "ceres/context_impl.h" -#include "ceres/internal/port.h" +#include "ceres/internal/disable_warnings.h" +#include "ceres/internal/export.h" namespace ceres { namespace internal { // Returns the maximum number of threads supported by the threading backend // Ceres was compiled with. +CERES_NO_EXPORT int MaxNumThreadsAvailable(); // Execute the function for every element in the range [start, end) with at most // num_threads. It will execute all the work on the calling thread if // num_threads is 1. -CERES_EXPORT_INTERNAL void ParallelFor( - ContextImpl* context, - int start, - int end, - int num_threads, - const std::function<void(int)>& function); +CERES_NO_EXPORT void ParallelFor(ContextImpl* context, + int start, + int end, + int num_threads, + const std::function<void(int)>& function); // Execute the function for every element in the range [start, end) with at most // num_threads. It will execute all the work on the calling thread if // num_threads is 1. Each invocation of function() will be passed a thread_id // in [0, num_threads) that is guaranteed to be distinct from the value passed // to any concurrent execution of function(). -CERES_EXPORT_INTERNAL void ParallelFor( +CERES_NO_EXPORT void ParallelFor( ContextImpl* context, int start, int end, @@ -67,4 +68,6 @@ CERES_EXPORT_INTERNAL void ParallelFor( } // namespace internal } // namespace ceres +#include "ceres/internal/disable_warnings.h" + #endif // CERES_INTERNAL_PARALLEL_FOR_H_ diff --git a/extern/ceres/internal/ceres/parallel_for_cxx.cc b/extern/ceres/internal/ceres/parallel_for_cxx.cc index 4da40c01eb6..5b78db19a44 100644 --- a/extern/ceres/internal/ceres/parallel_for_cxx.cc +++ b/extern/ceres/internal/ceres/parallel_for_cxx.cc @@ -29,7 +29,7 @@ // Author: vitus@google.com (Michael Vitus) // This include must come before any #ifndef check on Ceres compile options. -#include "ceres/internal/port.h" +#include "ceres/internal/config.h" #ifdef CERES_USE_CXX_THREADS @@ -125,7 +125,7 @@ void ParallelFor(ContextImpl* context, int num_threads, const std::function<void(int)>& function) { CHECK_GT(num_threads, 0); - CHECK(context != NULL); + CHECK(context != nullptr); if (end <= start) { return; } @@ -167,7 +167,7 @@ void ParallelFor(ContextImpl* context, int num_threads, const std::function<void(int thread_id, int i)>& function) { CHECK_GT(num_threads, 0); - CHECK(context != NULL); + CHECK(context != nullptr); if (end <= start) { return; } diff --git a/extern/ceres/internal/ceres/parallel_for_nothreads.cc b/extern/ceres/internal/ceres/parallel_for_nothreads.cc index d036569fcd7..1c1871662c8 100644 --- a/extern/ceres/internal/ceres/parallel_for_nothreads.cc +++ b/extern/ceres/internal/ceres/parallel_for_nothreads.cc @@ -29,7 +29,7 @@ // Author: alexs.mac@gmail.com (Alex Stewart) // This include must come before any #ifndef check on Ceres compile options. -#include "ceres/internal/port.h" +#include "ceres/internal/config.h" #ifdef CERES_NO_THREADS @@ -47,7 +47,7 @@ void ParallelFor(ContextImpl* context, int num_threads, const std::function<void(int)>& function) { CHECK_GT(num_threads, 0); - CHECK(context != NULL); + CHECK(context != nullptr); if (end <= start) { return; } @@ -62,7 +62,7 @@ void ParallelFor(ContextImpl* context, int num_threads, const std::function<void(int thread_id, int i)>& function) { CHECK_GT(num_threads, 0); - CHECK(context != NULL); + CHECK(context != nullptr); if (end <= start) { return; } diff --git a/extern/ceres/internal/ceres/parallel_for_openmp.cc b/extern/ceres/internal/ceres/parallel_for_openmp.cc index eb9d90581ae..1d44bf9977a 100644 --- a/extern/ceres/internal/ceres/parallel_for_openmp.cc +++ b/extern/ceres/internal/ceres/parallel_for_openmp.cc @@ -29,7 +29,7 @@ // Author: vitus@google.com (Michael Vitus) // This include must come before any #ifndef check on Ceres compile options. -#include "ceres/internal/port.h" +#include "ceres/internal/config.h" #if defined(CERES_USE_OPENMP) @@ -50,7 +50,7 @@ void ParallelFor(ContextImpl* context, int num_threads, const std::function<void(int)>& function) { CHECK_GT(num_threads, 0); - CHECK(context != NULL); + CHECK(context != nullptr); if (end <= start) { return; } @@ -69,7 +69,7 @@ void ParallelFor(ContextImpl* context, int end, int num_threads, const std::function<void(int thread_id, int i)>& function) { - CHECK(context != NULL); + CHECK(context != nullptr); ThreadTokenProvider thread_token_provider(num_threads); ParallelFor(context, start, end, num_threads, [&](int i) { diff --git a/extern/ceres/internal/ceres/parallel_utils.h b/extern/ceres/internal/ceres/parallel_utils.h index 89d21106d74..b2d9e0da765 100644 --- a/extern/ceres/internal/ceres/parallel_utils.h +++ b/extern/ceres/internal/ceres/parallel_utils.h @@ -31,7 +31,7 @@ #ifndef CERES_INTERNAL_PARALLEL_UTILS_H_ #define CERES_INTERNAL_PARALLEL_UTILS_H_ -#include "ceres/internal/port.h" +#include "ceres/internal/export.h" namespace ceres { namespace internal { @@ -61,10 +61,10 @@ namespace internal { // }); // which in each iteration will produce i and j satisfying // 0 <= i <= j < n -CERES_EXPORT_INTERNAL void LinearIndexToUpperTriangularIndex(int k, - int n, - int* i, - int* j); +CERES_NO_EXPORT void LinearIndexToUpperTriangularIndex(int k, + int n, + int* i, + int* j); } // namespace internal } // namespace ceres diff --git a/extern/ceres/internal/ceres/parameter_block.h b/extern/ceres/internal/ceres/parameter_block.h index 88943dfbcff..a9845a3a9e3 100644 --- a/extern/ceres/internal/ceres/parameter_block.h +++ b/extern/ceres/internal/ceres/parameter_block.h @@ -1,5 +1,5 @@ // Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2019 Google Inc. All rights reserved. +// Copyright 2021 Google Inc. All rights reserved. // http://ceres-solver.org/ // // Redistribution and use in source and binary forms, with or without @@ -40,9 +40,10 @@ #include <unordered_set> #include "ceres/array_utils.h" +#include "ceres/internal/disable_warnings.h" #include "ceres/internal/eigen.h" -#include "ceres/internal/port.h" -#include "ceres/local_parameterization.h" +#include "ceres/internal/export.h" +#include "ceres/manifold.h" #include "ceres/stringprintf.h" #include "glog/logging.h" @@ -58,12 +59,12 @@ class ResidualBlock; // methods are performance sensitive. // // The class is not thread-safe, unless only const methods are called. The -// parameter block may also hold a pointer to a local parameterization; the -// parameter block does not take ownership of this pointer, so the user is -// responsible for the proper disposal of the local parameterization. -class ParameterBlock { +// parameter block may also hold a pointer to a manifold; the parameter block +// does not take ownership of this pointer, so the user is responsible for the +// proper disposal of the manifold. +class CERES_NO_EXPORT ParameterBlock { public: - typedef std::unordered_set<ResidualBlock*> ResidualBlockSet; + using ResidualBlockSet = std::unordered_set<ResidualBlock*>; // Create a parameter block with the user state, size, and index specified. // The size is the size of the parameter block and the index is the position @@ -74,16 +75,13 @@ class ParameterBlock { state_(user_state), index_(index) {} - ParameterBlock(double* user_state, - int size, - int index, - LocalParameterization* local_parameterization) + ParameterBlock(double* user_state, int size, int index, Manifold* manifold) : user_state_(user_state), size_(size), state_(user_state), index_(index) { - if (local_parameterization != nullptr) { - SetParameterization(local_parameterization); + if (manifold != nullptr) { + SetManifold(manifold); } } @@ -98,7 +96,7 @@ class ParameterBlock { << "with user location " << user_state_; state_ = x; - return UpdateLocalParameterizationJacobian(); + return UpdatePlusJacobian(); } // Copy the current parameter state out to x. This is "GetState()" rather than @@ -114,17 +112,13 @@ class ParameterBlock { const double* state() const { return state_; } const double* user_state() const { return user_state_; } double* mutable_user_state() { return user_state_; } - const LocalParameterization* local_parameterization() const { - return local_parameterization_; - } - LocalParameterization* mutable_local_parameterization() { - return local_parameterization_; - } + const Manifold* manifold() const { return manifold_; } + Manifold* mutable_manifold() { return manifold_; } // Set this parameter block to vary or not. void SetConstant() { is_set_constant_ = true; } void SetVarying() { is_set_constant_ = false; } - bool IsConstant() const { return (is_set_constant_ || LocalSize() == 0); } + bool IsConstant() const { return (is_set_constant_ || TangentSize() == 0); } double UpperBound(int index) const { return (upper_bounds_ ? upper_bounds_[index] @@ -151,51 +145,46 @@ class ParameterBlock { int delta_offset() const { return delta_offset_; } void set_delta_offset(int delta_offset) { delta_offset_ = delta_offset; } - // Methods relating to the parameter block's parameterization. + // Methods relating to the parameter block's manifold. - // The local to global jacobian. Returns nullptr if there is no local - // parameterization for this parameter block. The returned matrix is row-major - // and has Size() rows and LocalSize() columns. - const double* LocalParameterizationJacobian() const { - return local_parameterization_jacobian_.get(); - } + // The local to global jacobian. Returns nullptr if there is no manifold for + // this parameter block. The returned matrix is row-major and has Size() rows + // and TangentSize() columns. + const double* PlusJacobian() const { return plus_jacobian_.get(); } - int LocalSize() const { - return (local_parameterization_ == nullptr) - ? size_ - : local_parameterization_->LocalSize(); + int TangentSize() const { + return (manifold_ == nullptr) ? size_ : manifold_->TangentSize(); } - // Set the parameterization. The parameter block does not take - // ownership of the parameterization. - void SetParameterization(LocalParameterization* new_parameterization) { - // Nothing to do if the new parameterization is the same as the - // old parameterization. - if (new_parameterization == local_parameterization_) { + // Set the manifold. The parameter block does not take ownership of + // the manifold. + void SetManifold(Manifold* new_manifold) { + // Nothing to do if the new manifold is the same as the old + // manifold. + if (new_manifold == manifold_) { return; } - if (new_parameterization == nullptr) { - local_parameterization_ = nullptr; + if (new_manifold == nullptr) { + manifold_ = nullptr; + plus_jacobian_ = nullptr; return; } - CHECK(new_parameterization->GlobalSize() == size_) - << "Invalid parameterization for parameter block. The parameter block " - << "has size " << size_ << " while the parameterization has a global " - << "size of " << new_parameterization->GlobalSize() << ". Did you " - << "accidentally use the wrong parameter block or parameterization?"; + CHECK_EQ(new_manifold->AmbientSize(), size_) + << "The parameter block has size = " << size_ + << " while the manifold has ambient size = " + << new_manifold->AmbientSize(); - CHECK_GE(new_parameterization->LocalSize(), 0) - << "Invalid parameterization. Parameterizations must have a " + CHECK_GE(new_manifold->TangentSize(), 0) + << "Invalid Manifold. Manifolds must have a " << "non-negative dimensional tangent space."; - local_parameterization_ = new_parameterization; - local_parameterization_jacobian_.reset( - new double[local_parameterization_->GlobalSize() * - local_parameterization_->LocalSize()]); - CHECK(UpdateLocalParameterizationJacobian()) - << "Local parameterization Jacobian computation failed for x: " + manifold_ = new_manifold; + plus_jacobian_ = std::make_unique<double[]>(manifold_->AmbientSize() * + manifold_->TangentSize()); + CHECK(UpdatePlusJacobian()) + << "Manifold::PlusJacobian computation failed for x: " << ConstVectorRef(state_, Size()).transpose(); } @@ -207,7 +196,7 @@ class ParameterBlock { } if (!upper_bounds_) { - upper_bounds_.reset(new double[size_]); + upper_bounds_ = std::make_unique<double[]>(size_); std::fill(upper_bounds_.get(), upper_bounds_.get() + size_, std::numeric_limits<double>::max()); @@ -224,7 +213,7 @@ class ParameterBlock { } if (!lower_bounds_) { - lower_bounds_.reset(new double[size_]); + lower_bounds_ = std::make_unique<double[]>(size_); std::fill(lower_bounds_.get(), lower_bounds_.get() + size_, -std::numeric_limits<double>::max()); @@ -234,11 +223,11 @@ class ParameterBlock { } // Generalization of the addition operation. This is the same as - // LocalParameterization::Plus() followed by projection onto the + // Manifold::Plus() followed by projection onto the // hyper cube implied by the bounds constraints. bool Plus(const double* x, const double* delta, double* x_plus_delta) { - if (local_parameterization_ != nullptr) { - if (!local_parameterization_->Plus(x, delta, x_plus_delta)) { + if (manifold_ != nullptr) { + if (!manifold_->Plus(x, delta, x_plus_delta)) { return false; } } else { @@ -281,7 +270,7 @@ class ParameterBlock { CHECK(residual_blocks_.get() == nullptr) << "Ceres bug: There is already a residual block collection " << "for parameter block: " << ToString(); - residual_blocks_.reset(new ResidualBlockSet); + residual_blocks_ = std::make_unique<ResidualBlockSet>(); } void AddResidualBlock(ResidualBlock* residual_block) { @@ -321,33 +310,30 @@ class ParameterBlock { } private: - bool UpdateLocalParameterizationJacobian() { - if (local_parameterization_ == nullptr) { + bool UpdatePlusJacobian() { + if (manifold_ == nullptr) { return true; } - // Update the local to global Jacobian. In some cases this is + // Update the Plus Jacobian. In some cases this is // wasted effort; if this is a bottleneck, we will find a solution // at that time. - - const int jacobian_size = Size() * LocalSize(); - InvalidateArray(jacobian_size, local_parameterization_jacobian_.get()); - if (!local_parameterization_->ComputeJacobian( - state_, local_parameterization_jacobian_.get())) { - LOG(WARNING) << "Local parameterization Jacobian computation failed" + const int jacobian_size = Size() * TangentSize(); + InvalidateArray(jacobian_size, plus_jacobian_.get()); + if (!manifold_->PlusJacobian(state_, plus_jacobian_.get())) { + LOG(WARNING) << "Manifold::PlusJacobian computation failed" "for x: " << ConstVectorRef(state_, Size()).transpose(); return false; } - if (!IsArrayValid(jacobian_size, local_parameterization_jacobian_.get())) { - LOG(WARNING) << "Local parameterization Jacobian computation returned" + if (!IsArrayValid(jacobian_size, plus_jacobian_.get())) { + LOG(WARNING) << "Manifold::PlusJacobian computation returned " << "an invalid matrix for x: " << ConstVectorRef(state_, Size()).transpose() << "\n Jacobian matrix : " - << ConstMatrixRef(local_parameterization_jacobian_.get(), - Size(), - LocalSize()); + << ConstMatrixRef( + plus_jacobian_.get(), Size(), TangentSize()); return false; } return true; @@ -356,14 +342,14 @@ class ParameterBlock { double* user_state_ = nullptr; int size_ = -1; bool is_set_constant_ = false; - LocalParameterization* local_parameterization_ = nullptr; + Manifold* manifold_ = nullptr; // The "state" of the parameter. These fields are only needed while the // solver is running. While at first glance using mutable is a bad idea, this // ends up simplifying the internals of Ceres enough to justify the potential // pitfalls of using "mutable." mutable const double* state_ = nullptr; - mutable std::unique_ptr<double[]> local_parameterization_jacobian_; + mutable std::unique_ptr<double[]> plus_jacobian_; // The index of the parameter. This is used by various other parts of Ceres to // permit switching from a ParameterBlock* to an index in another array. @@ -392,11 +378,13 @@ class ParameterBlock { std::unique_ptr<double[]> upper_bounds_; std::unique_ptr<double[]> lower_bounds_; - // Necessary so ProblemImpl can clean up the parameterizations. + // Necessary so ProblemImpl can clean up the manifolds. friend class ProblemImpl; }; } // namespace internal } // namespace ceres +#include "ceres/internal/reenable_warnings.h" + #endif // CERES_INTERNAL_PARAMETER_BLOCK_H_ diff --git a/extern/ceres/internal/ceres/parameter_block_ordering.cc b/extern/ceres/internal/ceres/parameter_block_ordering.cc index 9899c243899..570a09c60ba 100644 --- a/extern/ceres/internal/ceres/parameter_block_ordering.cc +++ b/extern/ceres/internal/ceres/parameter_block_ordering.cc @@ -1,5 +1,5 @@ // Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2015 Google Inc. All rights reserved. +// Copyright 2022 Google Inc. All rights reserved. // http://ceres-solver.org/ // // Redistribution and use in source and binary forms, with or without @@ -54,14 +54,14 @@ int ComputeStableSchurOrdering(const Program& program, CHECK(ordering != nullptr); ordering->clear(); EventLogger event_logger("ComputeStableSchurOrdering"); - std::unique_ptr<Graph<ParameterBlock*>> graph(CreateHessianGraph(program)); + auto graph = CreateHessianGraph(program); event_logger.AddEvent("CreateHessianGraph"); const vector<ParameterBlock*>& parameter_blocks = program.parameter_blocks(); const std::unordered_set<ParameterBlock*>& vertices = graph->vertices(); - for (int i = 0; i < parameter_blocks.size(); ++i) { - if (vertices.count(parameter_blocks[i]) > 0) { - ordering->push_back(parameter_blocks[i]); + for (auto* parameter_block : parameter_blocks) { + if (vertices.count(parameter_block) > 0) { + ordering->push_back(parameter_block); } } event_logger.AddEvent("Preordering"); @@ -70,8 +70,7 @@ int ComputeStableSchurOrdering(const Program& program, event_logger.AddEvent("StableIndependentSet"); // Add the excluded blocks to back of the ordering vector. - for (int i = 0; i < parameter_blocks.size(); ++i) { - ParameterBlock* parameter_block = parameter_blocks[i]; + for (auto* parameter_block : parameter_blocks) { if (parameter_block->IsConstant()) { ordering->push_back(parameter_block); } @@ -86,13 +85,12 @@ int ComputeSchurOrdering(const Program& program, CHECK(ordering != nullptr); ordering->clear(); - std::unique_ptr<Graph<ParameterBlock*>> graph(CreateHessianGraph(program)); + auto graph = CreateHessianGraph(program); int independent_set_size = IndependentSetOrdering(*graph, ordering); const vector<ParameterBlock*>& parameter_blocks = program.parameter_blocks(); // Add the excluded blocks to back of the ordering vector. - for (int i = 0; i < parameter_blocks.size(); ++i) { - ParameterBlock* parameter_block = parameter_blocks[i]; + for (auto* parameter_block : parameter_blocks) { if (parameter_block->IsConstant()) { ordering->push_back(parameter_block); } @@ -106,7 +104,7 @@ void ComputeRecursiveIndependentSetOrdering(const Program& program, CHECK(ordering != nullptr); ordering->Clear(); const vector<ParameterBlock*> parameter_blocks = program.parameter_blocks(); - std::unique_ptr<Graph<ParameterBlock*>> graph(CreateHessianGraph(program)); + auto graph = CreateHessianGraph(program); int num_covered = 0; int round = 0; @@ -124,20 +122,19 @@ void ComputeRecursiveIndependentSetOrdering(const Program& program, } } -Graph<ParameterBlock*>* CreateHessianGraph(const Program& program) { - Graph<ParameterBlock*>* graph = new Graph<ParameterBlock*>; +std::unique_ptr<Graph<ParameterBlock*>> CreateHessianGraph( + const Program& program) { + auto graph = std::make_unique<Graph<ParameterBlock*>>(); CHECK(graph != nullptr); const vector<ParameterBlock*>& parameter_blocks = program.parameter_blocks(); - for (int i = 0; i < parameter_blocks.size(); ++i) { - ParameterBlock* parameter_block = parameter_blocks[i]; + for (auto* parameter_block : parameter_blocks) { if (!parameter_block->IsConstant()) { graph->AddVertex(parameter_block); } } const vector<ResidualBlock*>& residual_blocks = program.residual_blocks(); - for (int i = 0; i < residual_blocks.size(); ++i) { - const ResidualBlock* residual_block = residual_blocks[i]; + for (auto* residual_block : residual_blocks) { const int num_parameter_blocks = residual_block->NumParameterBlocks(); ParameterBlock* const* parameter_blocks = residual_block->parameter_blocks(); @@ -163,7 +160,7 @@ void OrderingToGroupSizes(const ParameterBlockOrdering* ordering, vector<int>* group_sizes) { CHECK(group_sizes != nullptr); group_sizes->clear(); - if (ordering == NULL) { + if (ordering == nullptr) { return; } diff --git a/extern/ceres/internal/ceres/parameter_block_ordering.h b/extern/ceres/internal/ceres/parameter_block_ordering.h index 82ab75dc6dc..f9a447adf87 100644 --- a/extern/ceres/internal/ceres/parameter_block_ordering.h +++ b/extern/ceres/internal/ceres/parameter_block_ordering.h @@ -31,10 +31,12 @@ #ifndef CERES_INTERNAL_PARAMETER_BLOCK_ORDERING_H_ #define CERES_INTERNAL_PARAMETER_BLOCK_ORDERING_H_ +#include <memory> #include <vector> #include "ceres/graph.h" -#include "ceres/internal/port.h" +#include "ceres/internal/disable_warnings.h" +#include "ceres/internal/export.h" #include "ceres/ordered_groups.h" #include "ceres/types.h" @@ -57,20 +59,20 @@ class ParameterBlock; // ordering = [independent set, // complement of the independent set, // fixed blocks] -CERES_EXPORT_INTERNAL int ComputeSchurOrdering( +CERES_NO_EXPORT int ComputeSchurOrdering( const Program& program, std::vector<ParameterBlock*>* ordering); // Same as above, except that ties while computing the independent set // ordering are resolved in favour of the order in which the parameter // blocks occur in the program. -CERES_EXPORT_INTERNAL int ComputeStableSchurOrdering( +CERES_NO_EXPORT int ComputeStableSchurOrdering( const Program& program, std::vector<ParameterBlock*>* ordering); // Use an approximate independent set ordering to decompose the // parameter blocks of a problem in a sequence of independent // sets. The ordering covers all the non-constant parameter blocks in // the program. -CERES_EXPORT_INTERNAL void ComputeRecursiveIndependentSetOrdering( +CERES_NO_EXPORT void ComputeRecursiveIndependentSetOrdering( const Program& program, ParameterBlockOrdering* ordering); // Builds a graph on the parameter blocks of a Problem, whose @@ -78,15 +80,17 @@ CERES_EXPORT_INTERNAL void ComputeRecursiveIndependentSetOrdering( // vertex corresponds to a parameter block in the Problem except for // parameter blocks that are marked constant. An edge connects two // parameter blocks, if they co-occur in a residual block. -CERES_EXPORT_INTERNAL Graph<ParameterBlock*>* CreateHessianGraph( +CERES_NO_EXPORT std::unique_ptr<Graph<ParameterBlock*>> CreateHessianGraph( const Program& program); // Iterate over each of the groups in order of their priority and fill // summary with their sizes. -CERES_EXPORT_INTERNAL void OrderingToGroupSizes( +CERES_NO_EXPORT void OrderingToGroupSizes( const ParameterBlockOrdering* ordering, std::vector<int>* group_sizes); } // namespace internal } // namespace ceres +#include "ceres/internal/reenable_warnings.h" + #endif // CERES_INTERNAL_PARAMETER_BLOCK_ORDERING_H_ diff --git a/extern/ceres/internal/ceres/partitioned_matrix_view.cc b/extern/ceres/internal/ceres/partitioned_matrix_view.cc index b67bc905d15..d38f30a09d7 100644 --- a/extern/ceres/internal/ceres/partitioned_matrix_view.cc +++ b/extern/ceres/internal/ceres/partitioned_matrix_view.cc @@ -39,143 +39,147 @@ // // This file is generated using generate_template_specializations.py. +#include <memory> + #include "ceres/linear_solver.h" #include "ceres/partitioned_matrix_view.h" namespace ceres { namespace internal { -PartitionedMatrixViewBase* PartitionedMatrixViewBase::Create( +PartitionedMatrixViewBase::~PartitionedMatrixViewBase() = default; + +std::unique_ptr<PartitionedMatrixViewBase> PartitionedMatrixViewBase::Create( const LinearSolver::Options& options, const BlockSparseMatrix& matrix) { #ifndef CERES_RESTRICT_SCHUR_SPECIALIZATION if ((options.row_block_size == 2) && (options.e_block_size == 2) && (options.f_block_size == 2)) { - return new PartitionedMatrixView<2, 2, 2>(matrix, - options.elimination_groups[0]); + return std::make_unique<PartitionedMatrixView<2,2, 2>>( + matrix, options.elimination_groups[0]); } if ((options.row_block_size == 2) && (options.e_block_size == 2) && (options.f_block_size == 3)) { - return new PartitionedMatrixView<2, 2, 3>(matrix, - options.elimination_groups[0]); + return std::make_unique<PartitionedMatrixView<2,2, 3>>( + matrix, options.elimination_groups[0]); } if ((options.row_block_size == 2) && (options.e_block_size == 2) && (options.f_block_size == 4)) { - return new PartitionedMatrixView<2, 2, 4>(matrix, - options.elimination_groups[0]); + return std::make_unique<PartitionedMatrixView<2,2, 4>>( + matrix, options.elimination_groups[0]); } if ((options.row_block_size == 2) && (options.e_block_size == 2)) { - return new PartitionedMatrixView<2, 2, Eigen::Dynamic>(matrix, - options.elimination_groups[0]); + return std::make_unique<PartitionedMatrixView<2,2, Eigen::Dynamic>>( + matrix, options.elimination_groups[0]); } if ((options.row_block_size == 2) && (options.e_block_size == 3) && (options.f_block_size == 3)) { - return new PartitionedMatrixView<2, 3, 3>(matrix, - options.elimination_groups[0]); + return std::make_unique<PartitionedMatrixView<2,3, 3>>( + matrix, options.elimination_groups[0]); } if ((options.row_block_size == 2) && (options.e_block_size == 3) && (options.f_block_size == 4)) { - return new PartitionedMatrixView<2, 3, 4>(matrix, - options.elimination_groups[0]); + return std::make_unique<PartitionedMatrixView<2,3, 4>>( + matrix, options.elimination_groups[0]); } if ((options.row_block_size == 2) && (options.e_block_size == 3) && (options.f_block_size == 6)) { - return new PartitionedMatrixView<2, 3, 6>(matrix, - options.elimination_groups[0]); + return std::make_unique<PartitionedMatrixView<2,3, 6>>( + matrix, options.elimination_groups[0]); } if ((options.row_block_size == 2) && (options.e_block_size == 3) && (options.f_block_size == 9)) { - return new PartitionedMatrixView<2, 3, 9>(matrix, - options.elimination_groups[0]); + return std::make_unique<PartitionedMatrixView<2,3, 9>>( + matrix, options.elimination_groups[0]); } if ((options.row_block_size == 2) && (options.e_block_size == 3)) { - return new PartitionedMatrixView<2, 3, Eigen::Dynamic>(matrix, - options.elimination_groups[0]); + return std::make_unique<PartitionedMatrixView<2,3, Eigen::Dynamic>>( + matrix, options.elimination_groups[0]); } if ((options.row_block_size == 2) && (options.e_block_size == 4) && (options.f_block_size == 3)) { - return new PartitionedMatrixView<2, 4, 3>(matrix, - options.elimination_groups[0]); + return std::make_unique<PartitionedMatrixView<2,4, 3>>( + matrix, options.elimination_groups[0]); } if ((options.row_block_size == 2) && (options.e_block_size == 4) && (options.f_block_size == 4)) { - return new PartitionedMatrixView<2, 4, 4>(matrix, - options.elimination_groups[0]); + return std::make_unique<PartitionedMatrixView<2,4, 4>>( + matrix, options.elimination_groups[0]); } if ((options.row_block_size == 2) && (options.e_block_size == 4) && (options.f_block_size == 6)) { - return new PartitionedMatrixView<2, 4, 6>(matrix, - options.elimination_groups[0]); + return std::make_unique<PartitionedMatrixView<2,4, 6>>( + matrix, options.elimination_groups[0]); } if ((options.row_block_size == 2) && (options.e_block_size == 4) && (options.f_block_size == 8)) { - return new PartitionedMatrixView<2, 4, 8>(matrix, - options.elimination_groups[0]); + return std::make_unique<PartitionedMatrixView<2,4, 8>>( + matrix, options.elimination_groups[0]); } if ((options.row_block_size == 2) && (options.e_block_size == 4) && (options.f_block_size == 9)) { - return new PartitionedMatrixView<2, 4, 9>(matrix, - options.elimination_groups[0]); + return std::make_unique<PartitionedMatrixView<2,4, 9>>( + matrix, options.elimination_groups[0]); } if ((options.row_block_size == 2) && (options.e_block_size == 4)) { - return new PartitionedMatrixView<2, 4, Eigen::Dynamic>(matrix, - options.elimination_groups[0]); + return std::make_unique<PartitionedMatrixView<2,4, Eigen::Dynamic>>( + matrix, options.elimination_groups[0]); } if (options.row_block_size == 2) { - return new PartitionedMatrixView<2, Eigen::Dynamic, Eigen::Dynamic>(matrix, - options.elimination_groups[0]); + return std::make_unique<PartitionedMatrixView<2,Eigen::Dynamic, Eigen::Dynamic>>( + matrix, options.elimination_groups[0]); } if ((options.row_block_size == 3) && (options.e_block_size == 3) && (options.f_block_size == 3)) { - return new PartitionedMatrixView<3, 3, 3>(matrix, - options.elimination_groups[0]); + return std::make_unique<PartitionedMatrixView<3,3, 3>>( + matrix, options.elimination_groups[0]); } if ((options.row_block_size == 4) && (options.e_block_size == 4) && (options.f_block_size == 2)) { - return new PartitionedMatrixView<4, 4, 2>(matrix, - options.elimination_groups[0]); + return std::make_unique<PartitionedMatrixView<4,4, 2>>( + matrix, options.elimination_groups[0]); } if ((options.row_block_size == 4) && (options.e_block_size == 4) && (options.f_block_size == 3)) { - return new PartitionedMatrixView<4, 4, 3>(matrix, - options.elimination_groups[0]); + return std::make_unique<PartitionedMatrixView<4,4, 3>>( + matrix, options.elimination_groups[0]); } if ((options.row_block_size == 4) && (options.e_block_size == 4) && (options.f_block_size == 4)) { - return new PartitionedMatrixView<4, 4, 4>(matrix, - options.elimination_groups[0]); + return std::make_unique<PartitionedMatrixView<4,4, 4>>( + matrix, options.elimination_groups[0]); } if ((options.row_block_size == 4) && (options.e_block_size == 4)) { - return new PartitionedMatrixView<4, 4, Eigen::Dynamic>(matrix, - options.elimination_groups[0]); + return std::make_unique<PartitionedMatrixView<4,4, Eigen::Dynamic>>( + matrix, options.elimination_groups[0]); } #endif VLOG(1) << "Template specializations not found for <" << options.row_block_size << "," << options.e_block_size << "," << options.f_block_size << ">"; - return new PartitionedMatrixView<Eigen::Dynamic, - Eigen::Dynamic, - Eigen::Dynamic>( + return std::make_unique<PartitionedMatrixView<Eigen::Dynamic, + Eigen::Dynamic, + Eigen::Dynamic>>( matrix, options.elimination_groups[0]); }; diff --git a/extern/ceres/internal/ceres/partitioned_matrix_view.h b/extern/ceres/internal/ceres/partitioned_matrix_view.h index 9f204ee1fdd..5623d3b6bca 100644 --- a/extern/ceres/internal/ceres/partitioned_matrix_view.h +++ b/extern/ceres/internal/ceres/partitioned_matrix_view.h @@ -38,11 +38,14 @@ #include <algorithm> #include <cstring> +#include <memory> #include <vector> #include "ceres/block_structure.h" +#include "ceres/internal/config.h" +#include "ceres/internal/disable_warnings.h" #include "ceres/internal/eigen.h" -#include "ceres/internal/port.h" +#include "ceres/internal/export.h" #include "ceres/linear_solver.h" #include "ceres/small_blas.h" #include "glog/logging.h" @@ -60,9 +63,9 @@ namespace internal { // block structure of the matrix does not satisfy the requirements of // the Schur complement solver it will result in unpredictable and // wrong output. -class CERES_EXPORT_INTERNAL PartitionedMatrixViewBase { +class CERES_NO_EXPORT PartitionedMatrixViewBase { public: - virtual ~PartitionedMatrixViewBase() {} + virtual ~PartitionedMatrixViewBase(); // y += E'x virtual void LeftMultiplyE(const double* x, double* y) const = 0; @@ -77,11 +80,11 @@ class CERES_EXPORT_INTERNAL PartitionedMatrixViewBase { virtual void RightMultiplyF(const double* x, double* y) const = 0; // Create and return the block diagonal of the matrix E'E. - virtual BlockSparseMatrix* CreateBlockDiagonalEtE() const = 0; + virtual std::unique_ptr<BlockSparseMatrix> CreateBlockDiagonalEtE() const = 0; // Create and return the block diagonal of the matrix F'F. Caller // owns the result. - virtual BlockSparseMatrix* CreateBlockDiagonalFtF() const = 0; + virtual std::unique_ptr<BlockSparseMatrix> CreateBlockDiagonalFtF() const = 0; // Compute the block diagonal of the matrix E'E and store it in // block_diagonal. The matrix block_diagonal is expected to have a @@ -108,26 +111,26 @@ class CERES_EXPORT_INTERNAL PartitionedMatrixViewBase { virtual int num_cols() const = 0; // clang-format on - static PartitionedMatrixViewBase* Create(const LinearSolver::Options& options, - const BlockSparseMatrix& matrix); + static std::unique_ptr<PartitionedMatrixViewBase> Create( + const LinearSolver::Options& options, const BlockSparseMatrix& matrix); }; template <int kRowBlockSize = Eigen::Dynamic, int kEBlockSize = Eigen::Dynamic, int kFBlockSize = Eigen::Dynamic> -class PartitionedMatrixView : public PartitionedMatrixViewBase { +class CERES_NO_EXPORT PartitionedMatrixView final + : public PartitionedMatrixViewBase { public: // matrix = [E F], where the matrix E contains the first // num_col_blocks_a column blocks. PartitionedMatrixView(const BlockSparseMatrix& matrix, int num_col_blocks_e); - virtual ~PartitionedMatrixView(); void LeftMultiplyE(const double* x, double* y) const final; void LeftMultiplyF(const double* x, double* y) const final; void RightMultiplyE(const double* x, double* y) const final; void RightMultiplyF(const double* x, double* y) const final; - BlockSparseMatrix* CreateBlockDiagonalEtE() const final; - BlockSparseMatrix* CreateBlockDiagonalFtF() const final; + std::unique_ptr<BlockSparseMatrix> CreateBlockDiagonalEtE() const final; + std::unique_ptr<BlockSparseMatrix> CreateBlockDiagonalFtF() const final; void UpdateBlockDiagonalEtE(BlockSparseMatrix* block_diagonal) const final; void UpdateBlockDiagonalFtF(BlockSparseMatrix* block_diagonal) const final; // clang-format off @@ -140,8 +143,8 @@ class PartitionedMatrixView : public PartitionedMatrixViewBase { // clang-format on private: - BlockSparseMatrix* CreateBlockDiagonalMatrixLayout(int start_col_block, - int end_col_block) const; + std::unique_ptr<BlockSparseMatrix> CreateBlockDiagonalMatrixLayout( + int start_col_block, int end_col_block) const; const BlockSparseMatrix& matrix_; int num_row_blocks_e_; @@ -154,4 +157,6 @@ class PartitionedMatrixView : public PartitionedMatrixViewBase { } // namespace internal } // namespace ceres +#include "ceres/internal/reenable_warnings.h" + #endif // CERES_INTERNAL_PARTITIONED_MATRIX_VIEW_H_ diff --git a/extern/ceres/internal/ceres/partitioned_matrix_view_impl.h b/extern/ceres/internal/ceres/partitioned_matrix_view_impl.h index 0b6a57fb9f2..2e818caa6ef 100644 --- a/extern/ceres/internal/ceres/partitioned_matrix_view_impl.h +++ b/extern/ceres/internal/ceres/partitioned_matrix_view_impl.h @@ -1,5 +1,5 @@ // Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2015 Google Inc. All rights reserved. +// Copyright 2022 Google Inc. All rights reserved. // http://ceres-solver.org/ // // Redistribution and use in source and binary forms, with or without @@ -30,6 +30,7 @@ #include <algorithm> #include <cstring> +#include <memory> #include <vector> #include "ceres/block_sparse_matrix.h" @@ -57,8 +58,8 @@ PartitionedMatrixView<kRowBlockSize, kEBlockSize, kFBlockSize>:: // e_blocks. For a definition of what an e_block is, please see // explicit_schur_complement_solver.h num_row_blocks_e_ = 0; - for (int r = 0; r < bs->rows.size(); ++r) { - const std::vector<Cell>& cells = bs->rows[r].cells; + for (const auto& row : bs->rows) { + const std::vector<Cell>& cells = row.cells; if (cells[0].block_id < num_col_blocks_e_) { ++num_row_blocks_e_; } @@ -80,10 +81,6 @@ PartitionedMatrixView<kRowBlockSize, kEBlockSize, kFBlockSize>:: CHECK_EQ(num_cols_e_ + num_cols_f_, matrix_.num_cols()); } -template <int kRowBlockSize, int kEBlockSize, int kFBlockSize> -PartitionedMatrixView<kRowBlockSize, kEBlockSize, kFBlockSize>:: - ~PartitionedMatrixView() {} - // The next four methods don't seem to be particularly cache // friendly. This is an artifact of how the BlockStructure of the // input matrix is constructed. These methods will benefit from @@ -145,13 +142,13 @@ void PartitionedMatrixView<kRowBlockSize, kEBlockSize, kFBlockSize>:: const int row_block_pos = bs->rows[r].block.position; const int row_block_size = bs->rows[r].block.size; const std::vector<Cell>& cells = bs->rows[r].cells; - for (int c = 0; c < cells.size(); ++c) { - const int col_block_id = cells[c].block_id; + for (const auto& cell : cells) { + const int col_block_id = cell.block_id; const int col_block_pos = bs->cols[col_block_id].position; const int col_block_size = bs->cols[col_block_id].size; // clang-format off MatrixVectorMultiply<Eigen::Dynamic, Eigen::Dynamic, 1>( - values + cells[c].position, row_block_size, col_block_size, + values + cell.position, row_block_size, col_block_size, x + col_block_pos - num_cols_e_, y + row_block_pos); // clang-format on @@ -215,13 +212,13 @@ void PartitionedMatrixView<kRowBlockSize, kEBlockSize, kFBlockSize>:: const int row_block_pos = bs->rows[r].block.position; const int row_block_size = bs->rows[r].block.size; const std::vector<Cell>& cells = bs->rows[r].cells; - for (int c = 0; c < cells.size(); ++c) { - const int col_block_id = cells[c].block_id; + for (const auto& cell : cells) { + const int col_block_id = cell.block_id; const int col_block_pos = bs->cols[col_block_id].position; const int col_block_size = bs->cols[col_block_id].size; // clang-format off MatrixTransposeVectorMultiply<Eigen::Dynamic, Eigen::Dynamic, 1>( - values + cells[c].position, row_block_size, col_block_size, + values + cell.position, row_block_size, col_block_size, x + row_block_pos, y + col_block_pos - num_cols_e_); // clang-format on @@ -235,13 +232,12 @@ void PartitionedMatrixView<kRowBlockSize, kEBlockSize, kFBlockSize>:: // and return a BlockSparseMatrix with the this block structure. The // caller owns the result. template <int kRowBlockSize, int kEBlockSize, int kFBlockSize> -BlockSparseMatrix* +std::unique_ptr<BlockSparseMatrix> PartitionedMatrixView<kRowBlockSize, kEBlockSize, kFBlockSize>:: CreateBlockDiagonalMatrixLayout(int start_col_block, int end_col_block) const { const CompressedRowBlockStructure* bs = matrix_.block_structure(); - CompressedRowBlockStructure* block_diagonal_structure = - new CompressedRowBlockStructure; + auto* block_diagonal_structure = new CompressedRowBlockStructure; int block_position = 0; int diagonal_cell_position = 0; @@ -250,16 +246,16 @@ PartitionedMatrixView<kRowBlockSize, kEBlockSize, kFBlockSize>:: // each column block. for (int c = start_col_block; c < end_col_block; ++c) { const Block& block = bs->cols[c]; - block_diagonal_structure->cols.push_back(Block()); + block_diagonal_structure->cols.emplace_back(); Block& diagonal_block = block_diagonal_structure->cols.back(); diagonal_block.size = block.size; diagonal_block.position = block_position; - block_diagonal_structure->rows.push_back(CompressedRow()); + block_diagonal_structure->rows.emplace_back(); CompressedRow& row = block_diagonal_structure->rows.back(); row.block = diagonal_block; - row.cells.push_back(Cell()); + row.cells.emplace_back(); Cell& cell = row.cells.back(); cell.block_id = c - start_col_block; cell.position = diagonal_cell_position; @@ -270,28 +266,27 @@ PartitionedMatrixView<kRowBlockSize, kEBlockSize, kFBlockSize>:: // Build a BlockSparseMatrix with the just computed block // structure. - return new BlockSparseMatrix(block_diagonal_structure); + return std::make_unique<BlockSparseMatrix>(block_diagonal_structure); } template <int kRowBlockSize, int kEBlockSize, int kFBlockSize> -BlockSparseMatrix* PartitionedMatrixView<kRowBlockSize, - kEBlockSize, - kFBlockSize>::CreateBlockDiagonalEtE() - const { - BlockSparseMatrix* block_diagonal = +std::unique_ptr<BlockSparseMatrix> +PartitionedMatrixView<kRowBlockSize, kEBlockSize, kFBlockSize>:: + CreateBlockDiagonalEtE() const { + std::unique_ptr<BlockSparseMatrix> block_diagonal = CreateBlockDiagonalMatrixLayout(0, num_col_blocks_e_); - UpdateBlockDiagonalEtE(block_diagonal); + UpdateBlockDiagonalEtE(block_diagonal.get()); return block_diagonal; } template <int kRowBlockSize, int kEBlockSize, int kFBlockSize> -BlockSparseMatrix* PartitionedMatrixView<kRowBlockSize, - kEBlockSize, - kFBlockSize>::CreateBlockDiagonalFtF() - const { - BlockSparseMatrix* block_diagonal = CreateBlockDiagonalMatrixLayout( - num_col_blocks_e_, num_col_blocks_e_ + num_col_blocks_f_); - UpdateBlockDiagonalFtF(block_diagonal); +std::unique_ptr<BlockSparseMatrix> +PartitionedMatrixView<kRowBlockSize, kEBlockSize, kFBlockSize>:: + CreateBlockDiagonalFtF() const { + std::unique_ptr<BlockSparseMatrix> block_diagonal = + CreateBlockDiagonalMatrixLayout(num_col_blocks_e_, + num_col_blocks_e_ + num_col_blocks_f_); + UpdateBlockDiagonalFtF(block_diagonal.get()); return block_diagonal; } @@ -366,8 +361,8 @@ void PartitionedMatrixView<kRowBlockSize, kEBlockSize, kFBlockSize>:: for (int r = num_row_blocks_e_; r < bs->rows.size(); ++r) { const int row_block_size = bs->rows[r].block.size; const std::vector<Cell>& cells = bs->rows[r].cells; - for (int c = 0; c < cells.size(); ++c) { - const int col_block_id = cells[c].block_id; + for (const auto& cell : cells) { + const int col_block_id = cell.block_id; const int col_block_size = bs->cols[col_block_id].size; const int diagonal_block_id = col_block_id - num_col_blocks_e_; const int cell_position = @@ -376,8 +371,8 @@ void PartitionedMatrixView<kRowBlockSize, kEBlockSize, kFBlockSize>:: // clang-format off MatrixTransposeMatrixMultiply <Eigen::Dynamic, Eigen::Dynamic, Eigen::Dynamic, Eigen::Dynamic, 1>( - values + cells[c].position, row_block_size, col_block_size, - values + cells[c].position, row_block_size, col_block_size, + values + cell.position, row_block_size, col_block_size, + values + cell.position, row_block_size, col_block_size, block_diagonal->mutable_values() + cell_position, 0, 0, col_block_size, col_block_size); // clang-format on diff --git a/extern/ceres/internal/ceres/partitioned_matrix_view_template.py b/extern/ceres/internal/ceres/partitioned_matrix_view_template.py deleted file mode 100644 index 05a25bf8335..00000000000 --- a/extern/ceres/internal/ceres/partitioned_matrix_view_template.py +++ /dev/null @@ -1,151 +0,0 @@ -# Ceres Solver - A fast non-linear least squares minimizer -# Copyright 2015 Google Inc. All rights reserved. -# http://ceres-solver.org/ -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions are met: -# -# * Redistributions of source code must retain the above copyright notice, -# this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above copyright notice, -# this list of conditions and the following disclaimer in the documentation -# and/or other materials provided with the distribution. -# * Neither the name of Google Inc. nor the names of its contributors may be -# used to endorse or promote products derived from this software without -# specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE -# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -# Author: sameeragarwal@google.com (Sameer Agarwal) -# -# Script for explicitly generating template specialization of the -# PartitionedMatrixView class. Explicitly generating these -# instantiations in separate .cc files breaks the compilation into -# separate compilation unit rather than one large cc file. -# -# This script creates two sets of files. -# -# 1. partitioned_matrix_view_x_x_x.cc -# where the x indicates the template parameters and -# -# 2. partitioned_matrix_view.cc -# -# that contains a factory function for instantiating these classes -# based on runtime parameters. -# -# The list of tuples, specializations indicates the set of -# specializations that is generated. - -HEADER = """// Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2017 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) -// -// Template specialization of PartitionedMatrixView. -// -// ======================================== -// THIS FILE IS AUTOGENERATED. DO NOT EDIT. -// THIS FILE IS AUTOGENERATED. DO NOT EDIT. -// THIS FILE IS AUTOGENERATED. DO NOT EDIT. -// THIS FILE IS AUTOGENERATED. DO NOT EDIT. -//========================================= -// -// This file is generated using generate_template_specializations.py. -""" - -DYNAMIC_FILE = """ -#include "ceres/partitioned_matrix_view_impl.h" - -namespace ceres { -namespace internal { - -template class PartitionedMatrixView<%s, - %s, - %s>; - -} // namespace internal -} // namespace ceres -""" - -SPECIALIZATION_FILE = """ -// This include must come before any #ifndef check on Ceres compile options. -#include "ceres/internal/port.h" - -#ifndef CERES_RESTRICT_SCHUR_SPECIALIZATION - -#include "ceres/partitioned_matrix_view_impl.h" - -namespace ceres { -namespace internal { - -template class PartitionedMatrixView<%s, %s, %s>; - -} // namespace internal -} // namespace ceres - -#endif // CERES_RESTRICT_SCHUR_SPECIALIZATION -""" - -FACTORY_FILE_HEADER = """ -#include "ceres/linear_solver.h" -#include "ceres/partitioned_matrix_view.h" - -namespace ceres { -namespace internal { - -PartitionedMatrixViewBase* PartitionedMatrixViewBase::Create( - const LinearSolver::Options& options, const BlockSparseMatrix& matrix) { -#ifndef CERES_RESTRICT_SCHUR_SPECIALIZATION -""" -FACTORY = """ return new PartitionedMatrixView<%s, %s, %s>(matrix, - options.elimination_groups[0]);""" - -FACTORY_FOOTER = """ -#endif - VLOG(1) << "Template specializations not found for <" - << options.row_block_size << "," << options.e_block_size << "," - << options.f_block_size << ">"; - return new PartitionedMatrixView<Eigen::Dynamic, - Eigen::Dynamic, - Eigen::Dynamic>( - matrix, options.elimination_groups[0]); -}; - -} // namespace internal -} // namespace ceres -""" diff --git a/extern/ceres/internal/ceres/polynomial.cc b/extern/ceres/internal/ceres/polynomial.cc index 20812f4de81..96267aae97f 100644 --- a/extern/ceres/internal/ceres/polynomial.cc +++ b/extern/ceres/internal/ceres/polynomial.cc @@ -37,7 +37,7 @@ #include "Eigen/Dense" #include "ceres/function_sample.h" -#include "ceres/internal/port.h" +#include "ceres/internal/export.h" #include "glog/logging.h" namespace ceres { @@ -128,12 +128,12 @@ void FindLinearPolynomialRoots(const Vector& polynomial, Vector* real, Vector* imaginary) { CHECK_EQ(polynomial.size(), 2); - if (real != NULL) { + if (real != nullptr) { real->resize(1); (*real)(0) = -polynomial(1) / polynomial(0); } - if (imaginary != NULL) { + if (imaginary != nullptr) { imaginary->setZero(1); } } @@ -147,16 +147,16 @@ void FindQuadraticPolynomialRoots(const Vector& polynomial, const double c = polynomial(2); const double D = b * b - 4 * a * c; const double sqrt_D = sqrt(fabs(D)); - if (real != NULL) { + if (real != nullptr) { real->setZero(2); } - if (imaginary != NULL) { + if (imaginary != nullptr) { imaginary->setZero(2); } // Real roots. if (D >= 0) { - if (real != NULL) { + if (real != nullptr) { // Stable quadratic roots according to BKP Horn. // http://people.csail.mit.edu/bkph/articles/Quadratics.pdf if (b >= 0) { @@ -171,11 +171,11 @@ void FindQuadraticPolynomialRoots(const Vector& polynomial, } // Use the normal quadratic formula for the complex case. - if (real != NULL) { + if (real != nullptr) { (*real)(0) = -b / (2.0 * a); (*real)(1) = -b / (2.0 * a); } - if (imaginary != NULL) { + if (imaginary != nullptr) { (*imaginary)(0) = sqrt_D / (2.0 * a); (*imaginary)(1) = -sqrt_D / (2.0 * a); } @@ -240,14 +240,14 @@ bool FindPolynomialRoots(const Vector& polynomial_in, } // Output roots - if (real != NULL) { + if (real != nullptr) { *real = solver.eigenvalues().real(); } else { - LOG(WARNING) << "NULL pointer passed as real argument to " + LOG(WARNING) << "nullptr pointer passed as real argument to " << "FindPolynomialRoots. Real parts of the roots will not " << "be returned."; } - if (imaginary != NULL) { + if (imaginary != nullptr) { *imaginary = solver.eigenvalues().imag(); } return true; @@ -304,7 +304,7 @@ void MinimizePolynomial(const Vector& polynomial, const Vector derivative = DifferentiatePolynomial(polynomial); Vector roots_real; - if (!FindPolynomialRoots(derivative, &roots_real, NULL)) { + if (!FindPolynomialRoots(derivative, &roots_real, nullptr)) { LOG(WARNING) << "Unable to find the critical points of " << "the interpolating polynomial."; return; @@ -376,8 +376,7 @@ void MinimizeInterpolatingPolynomial(const vector<FunctionSample>& samples, double* optimal_value) { const Vector polynomial = FindInterpolatingPolynomial(samples); MinimizePolynomial(polynomial, x_min, x_max, optimal_x, optimal_value); - for (int i = 0; i < samples.size(); ++i) { - const FunctionSample& sample = samples[i]; + for (const auto& sample : samples) { if ((sample.x < x_min) || (sample.x > x_max)) { continue; } diff --git a/extern/ceres/internal/ceres/polynomial.h b/extern/ceres/internal/ceres/polynomial.h index 20071f2c693..3ca753c4618 100644 --- a/extern/ceres/internal/ceres/polynomial.h +++ b/extern/ceres/internal/ceres/polynomial.h @@ -34,8 +34,9 @@ #include <vector> +#include "ceres/internal/disable_warnings.h" #include "ceres/internal/eigen.h" -#include "ceres/internal/port.h" +#include "ceres/internal/export.h" namespace ceres { namespace internal { @@ -49,6 +50,7 @@ struct FunctionSample; // and are given by a vector of coefficients of size N + 1. // Evaluate the polynomial at x using the Horner scheme. +CERES_NO_EXPORT inline double EvaluatePolynomial(const Vector& polynomial, double x) { double v = 0.0; for (int i = 0; i < polynomial.size(); ++i) { @@ -64,15 +66,16 @@ inline double EvaluatePolynomial(const Vector& polynomial, double x) { // Failure indicates that the polynomial is invalid (of size 0) or // that the eigenvalues of the companion matrix could not be computed. // On failure, a more detailed message will be written to LOG(ERROR). -// If real is not NULL, the real parts of the roots will be returned in it. -// Likewise, if imaginary is not NULL, imaginary parts will be returned in it. -CERES_EXPORT_INTERNAL bool FindPolynomialRoots(const Vector& polynomial, - Vector* real, - Vector* imaginary); +// If real is not nullptr, the real parts of the roots will be returned in it. +// Likewise, if imaginary is not nullptr, imaginary parts will be returned in +// it. +CERES_NO_EXPORT bool FindPolynomialRoots(const Vector& polynomial, + Vector* real, + Vector* imaginary); // Return the derivative of the given polynomial. It is assumed that // the input polynomial is at least of degree zero. -CERES_EXPORT_INTERNAL Vector DifferentiatePolynomial(const Vector& polynomial); +CERES_NO_EXPORT Vector DifferentiatePolynomial(const Vector& polynomial); // Find the minimum value of the polynomial in the interval [x_min, // x_max]. The minimum is obtained by computing all the roots of the @@ -80,11 +83,11 @@ CERES_EXPORT_INTERNAL Vector DifferentiatePolynomial(const Vector& polynomial); // interval [x_min, x_max] are considered as well as the end points // x_min and x_max. Since polynomials are differentiable functions, // this ensures that the true minimum is found. -CERES_EXPORT_INTERNAL void MinimizePolynomial(const Vector& polynomial, - double x_min, - double x_max, - double* optimal_x, - double* optimal_value); +CERES_NO_EXPORT void MinimizePolynomial(const Vector& polynomial, + double x_min, + double x_max, + double* optimal_x, + double* optimal_value); // Given a set of function value and/or gradient samples, find a // polynomial whose value and gradients are exactly equal to the ones @@ -97,7 +100,7 @@ CERES_EXPORT_INTERNAL void MinimizePolynomial(const Vector& polynomial, // Of course its possible to sample a polynomial any number of times, // in which case, generally speaking the spurious higher order // coefficients will be zero. -CERES_EXPORT_INTERNAL Vector +CERES_NO_EXPORT Vector FindInterpolatingPolynomial(const std::vector<FunctionSample>& samples); // Interpolate the function described by samples with a polynomial, @@ -106,7 +109,7 @@ FindInterpolatingPolynomial(const std::vector<FunctionSample>& samples); // finding algorithms may fail due to numerical difficulties. But the // function is guaranteed to return its best guess of an answer, by // considering the samples and the end points as possible solutions. -CERES_EXPORT_INTERNAL void MinimizeInterpolatingPolynomial( +CERES_NO_EXPORT void MinimizeInterpolatingPolynomial( const std::vector<FunctionSample>& samples, double x_min, double x_max, @@ -116,4 +119,6 @@ CERES_EXPORT_INTERNAL void MinimizeInterpolatingPolynomial( } // namespace internal } // namespace ceres +#include "ceres/internal/reenable_warnings.h" + #endif // CERES_INTERNAL_POLYNOMIAL_SOLVER_H_ diff --git a/extern/ceres/internal/ceres/preconditioner.cc b/extern/ceres/internal/ceres/preconditioner.cc index 69ba04db8f5..17b9629cf94 100644 --- a/extern/ceres/internal/ceres/preconditioner.cc +++ b/extern/ceres/internal/ceres/preconditioner.cc @@ -35,7 +35,7 @@ namespace ceres { namespace internal { -Preconditioner::~Preconditioner() {} +Preconditioner::~Preconditioner() = default; PreconditionerType Preconditioner::PreconditionerForZeroEBlocks( PreconditionerType preconditioner_type) { @@ -53,7 +53,8 @@ SparseMatrixPreconditionerWrapper::SparseMatrixPreconditionerWrapper( CHECK(matrix != nullptr); } -SparseMatrixPreconditionerWrapper::~SparseMatrixPreconditionerWrapper() {} +SparseMatrixPreconditionerWrapper::~SparseMatrixPreconditionerWrapper() = + default; bool SparseMatrixPreconditionerWrapper::UpdateImpl(const SparseMatrix& A, const double* D) { diff --git a/extern/ceres/internal/ceres/preconditioner.h b/extern/ceres/internal/ceres/preconditioner.h index dd843b01ce3..6433cc7dd38 100644 --- a/extern/ceres/internal/ceres/preconditioner.h +++ b/extern/ceres/internal/ceres/preconditioner.h @@ -36,7 +36,8 @@ #include "ceres/casts.h" #include "ceres/compressed_row_sparse_matrix.h" #include "ceres/context_impl.h" -#include "ceres/internal/port.h" +#include "ceres/internal/disable_warnings.h" +#include "ceres/internal/export.h" #include "ceres/linear_operator.h" #include "ceres/sparse_matrix.h" #include "ceres/types.h" @@ -47,7 +48,7 @@ namespace internal { class BlockSparseMatrix; class SparseMatrix; -class CERES_EXPORT_INTERNAL Preconditioner : public LinearOperator { +class CERES_NO_EXPORT Preconditioner : public LinearOperator { public: struct Options { PreconditionerType type = JACOBI; @@ -115,7 +116,7 @@ class CERES_EXPORT_INTERNAL Preconditioner : public LinearOperator { static PreconditionerType PreconditionerForZeroEBlocks( PreconditionerType preconditioner_type); - virtual ~Preconditioner(); + ~Preconditioner() override; // Update the numerical value of the preconditioner for the linear // system: @@ -126,7 +127,7 @@ class CERES_EXPORT_INTERNAL Preconditioner : public LinearOperator { // for some vector b. It is important that the matrix A have the // same block structure as the one used to construct this object. // - // D can be NULL, in which case its interpreted as a diagonal matrix + // D can be nullptr, in which case its interpreted as a diagonal matrix // of size zero. virtual bool Update(const LinearOperator& A, const double* D) = 0; @@ -147,9 +148,8 @@ class CERES_EXPORT_INTERNAL Preconditioner : public LinearOperator { // other preconditioners that depend on the particular matrix layout of // the underlying linear operator. template <typename MatrixType> -class TypedPreconditioner : public Preconditioner { +class CERES_NO_EXPORT TypedPreconditioner : public Preconditioner { public: - virtual ~TypedPreconditioner() {} bool Update(const LinearOperator& A, const double* D) final { return UpdateImpl(*down_cast<const MatrixType*>(&A), D); } @@ -161,28 +161,31 @@ class TypedPreconditioner : public Preconditioner { // Preconditioners that depend on access to the low level structure // of a SparseMatrix. // clang-format off -typedef TypedPreconditioner<SparseMatrix> SparseMatrixPreconditioner; -typedef TypedPreconditioner<BlockSparseMatrix> BlockSparseMatrixPreconditioner; -typedef TypedPreconditioner<CompressedRowSparseMatrix> CompressedRowSparseMatrixPreconditioner; +using SparseMatrixPreconditioner = TypedPreconditioner<SparseMatrix>; +using BlockSparseMatrixPreconditioner = TypedPreconditioner<BlockSparseMatrix>; +using CompressedRowSparseMatrixPreconditioner = TypedPreconditioner<CompressedRowSparseMatrix>; // clang-format on // Wrap a SparseMatrix object as a preconditioner. -class SparseMatrixPreconditionerWrapper : public SparseMatrixPreconditioner { +class CERES_NO_EXPORT SparseMatrixPreconditionerWrapper final + : public SparseMatrixPreconditioner { public: // Wrapper does NOT take ownership of the matrix pointer. explicit SparseMatrixPreconditionerWrapper(const SparseMatrix* matrix); - virtual ~SparseMatrixPreconditionerWrapper(); + ~SparseMatrixPreconditionerWrapper() override; // Preconditioner interface - virtual void RightMultiply(const double* x, double* y) const; - virtual int num_rows() const; + void RightMultiply(const double* x, double* y) const override; + int num_rows() const override; private: - virtual bool UpdateImpl(const SparseMatrix& A, const double* D); + bool UpdateImpl(const SparseMatrix& A, const double* D) override; const SparseMatrix* matrix_; }; } // namespace internal } // namespace ceres +#include "ceres/internal/reenable_warnings.h" + #endif // CERES_INTERNAL_PRECONDITIONER_H_ diff --git a/extern/ceres/internal/ceres/preprocessor.cc b/extern/ceres/internal/ceres/preprocessor.cc index 6a67d385645..44f0974dc5a 100644 --- a/extern/ceres/internal/ceres/preprocessor.cc +++ b/extern/ceres/internal/ceres/preprocessor.cc @@ -30,6 +30,8 @@ #include "ceres/preprocessor.h" +#include <memory> + #include "ceres/callbacks.h" #include "ceres/gradient_checking_cost_function.h" #include "ceres/line_search_preprocessor.h" @@ -41,22 +43,26 @@ namespace ceres { namespace internal { -Preprocessor* Preprocessor::Create(MinimizerType minimizer_type) { +std::unique_ptr<Preprocessor> Preprocessor::Create( + MinimizerType minimizer_type) { if (minimizer_type == TRUST_REGION) { - return new TrustRegionPreprocessor; + return std::make_unique<TrustRegionPreprocessor>(); } if (minimizer_type == LINE_SEARCH) { - return new LineSearchPreprocessor; + return std::make_unique<LineSearchPreprocessor>(); } LOG(FATAL) << "Unknown minimizer_type: " << minimizer_type; - return NULL; + return nullptr; } -Preprocessor::~Preprocessor() {} +Preprocessor::~Preprocessor() = default; void ChangeNumThreadsIfNeeded(Solver::Options* options) { + if (options->num_threads == 1) { + return; + } const int num_threads_available = MaxNumThreadsAvailable(); if (options->num_threads > num_threads_available) { LOG(WARNING) << "Specified options.num_threads: " << options->num_threads @@ -82,15 +88,15 @@ void SetupCommonMinimizerOptions(PreprocessedProblem* pp) { minimizer_options.evaluator = pp->evaluator; if (options.logging_type != SILENT) { - pp->logging_callback.reset(new LoggingCallback( - options.minimizer_type, options.minimizer_progress_to_stdout)); + pp->logging_callback = std::make_unique<LoggingCallback>( + options.minimizer_type, options.minimizer_progress_to_stdout); minimizer_options.callbacks.insert(minimizer_options.callbacks.begin(), pp->logging_callback.get()); } if (options.update_state_every_iteration) { - pp->state_updating_callback.reset( - new StateUpdatingCallback(program, reduced_parameters)); + pp->state_updating_callback = + std::make_unique<StateUpdatingCallback>(program, reduced_parameters); // This must get pushed to the front of the callbacks so that it // is run before any of the user callbacks. minimizer_options.callbacks.insert(minimizer_options.callbacks.begin(), diff --git a/extern/ceres/internal/ceres/preprocessor.h b/extern/ceres/internal/ceres/preprocessor.h index ec56c6e430a..b5db80af7e6 100644 --- a/extern/ceres/internal/ceres/preprocessor.h +++ b/extern/ceres/internal/ceres/preprocessor.h @@ -37,8 +37,9 @@ #include "ceres/coordinate_descent_minimizer.h" #include "ceres/evaluator.h" +#include "ceres/internal/disable_warnings.h" #include "ceres/internal/eigen.h" -#include "ceres/internal/port.h" +#include "ceres/internal/export.h" #include "ceres/iteration_callback.h" #include "ceres/linear_solver.h" #include "ceres/minimizer.h" @@ -67,10 +68,10 @@ struct PreprocessedProblem; // // The output of the Preprocessor is stored in a PreprocessedProblem // object. -class CERES_EXPORT_INTERNAL Preprocessor { +class CERES_NO_EXPORT Preprocessor { public: // Factory. - static Preprocessor* Create(MinimizerType minimizer_type); + static std::unique_ptr<Preprocessor> Create(MinimizerType minimizer_type); virtual ~Preprocessor(); virtual bool Preprocess(const Solver::Options& options, ProblemImpl* problem, @@ -79,8 +80,8 @@ class CERES_EXPORT_INTERNAL Preprocessor { // A PreprocessedProblem is the result of running the Preprocessor on // a Problem and Solver::Options object. -struct PreprocessedProblem { - PreprocessedProblem() : fixed_cost(0.0) {} +struct CERES_NO_EXPORT PreprocessedProblem { + PreprocessedProblem() = default; std::string error; Solver::Options options; @@ -100,7 +101,7 @@ struct PreprocessedProblem { std::vector<double*> removed_parameter_blocks; Vector reduced_parameters; - double fixed_cost; + double fixed_cost{0.0}; }; // Common functions used by various preprocessors. @@ -108,14 +109,18 @@ struct PreprocessedProblem { // If the user has specified a num_threads > the maximum number of threads // available from the compiled threading model, bound the number of threads // to the maximum. +CERES_NO_EXPORT void ChangeNumThreadsIfNeeded(Solver::Options* options); // Extract the effective parameter vector from the preprocessed // problem and setup bits of the Minimizer::Options object that are // common to all Preprocessors. +CERES_NO_EXPORT void SetupCommonMinimizerOptions(PreprocessedProblem* pp); } // namespace internal } // namespace ceres +#include "ceres/internal/reenable_warnings.h" + #endif // CERES_INTERNAL_PREPROCESSOR_H_ diff --git a/extern/ceres/internal/ceres/problem.cc b/extern/ceres/internal/ceres/problem.cc index f3ffd546ef7..4269ca3ebc3 100644 --- a/extern/ceres/internal/ceres/problem.cc +++ b/extern/ceres/internal/ceres/problem.cc @@ -1,5 +1,5 @@ // Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2015 Google Inc. All rights reserved. +// Copyright 2021 Google Inc. All rights reserved. // http://ceres-solver.org/ // // Redistribution and use in source and binary forms, with or without @@ -31,6 +31,7 @@ #include "ceres/problem.h" +#include <memory> #include <vector> #include "ceres/crs_matrix.h" @@ -46,7 +47,7 @@ Problem::Problem(const Problem::Options& options) // Not inline defaulted in declaration due to use of std::unique_ptr. Problem::Problem(Problem&&) = default; Problem& Problem::operator=(Problem&&) = default; -Problem::~Problem() {} +Problem::~Problem() = default; ResidualBlockId Problem::AddResidualBlock( CostFunction* cost_function, @@ -76,6 +77,10 @@ void Problem::AddParameterBlock(double* values, impl_->AddParameterBlock(values, size, local_parameterization); } +void Problem::AddParameterBlock(double* values, int size, Manifold* manifold) { + impl_->AddParameterBlock(values, size, manifold); +} + void Problem::RemoveResidualBlock(ResidualBlockId residual_block) { impl_->RemoveResidualBlock(residual_block); } @@ -106,6 +111,22 @@ const LocalParameterization* Problem::GetParameterization( return impl_->GetParameterization(values); } +bool Problem::HasParameterization(const double* values) const { + return impl_->HasParameterization(values); +} + +void Problem::SetManifold(double* values, Manifold* manifold) { + impl_->SetManifold(values, manifold); +} + +const Manifold* Problem::GetManifold(const double* values) const { + return impl_->GetManifold(values); +} + +bool Problem::HasManifold(const double* values) const { + return impl_->HasManifold(values); +} + void Problem::SetParameterLowerBound(double* values, int index, double lower_bound) { @@ -169,12 +190,16 @@ int Problem::NumResidualBlocks() const { return impl_->NumResidualBlocks(); } int Problem::NumResiduals() const { return impl_->NumResiduals(); } -int Problem::ParameterBlockSize(const double* parameter_block) const { - return impl_->ParameterBlockSize(parameter_block); +int Problem::ParameterBlockSize(const double* values) const { + return impl_->ParameterBlockSize(values); +} + +int Problem::ParameterBlockLocalSize(const double* values) const { + return impl_->ParameterBlockTangentSize(values); } -int Problem::ParameterBlockLocalSize(const double* parameter_block) const { - return impl_->ParameterBlockLocalSize(parameter_block); +int Problem::ParameterBlockTangentSize(const double* values) const { + return impl_->ParameterBlockTangentSize(values); } bool Problem::HasParameterBlock(const double* values) const { diff --git a/extern/ceres/internal/ceres/problem_impl.cc b/extern/ceres/internal/ceres/problem_impl.cc index 3155bc3569e..01a22c128be 100644 --- a/extern/ceres/internal/ceres/problem_impl.cc +++ b/extern/ceres/internal/ceres/problem_impl.cc @@ -1,5 +1,5 @@ // Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2019 Google Inc. All rights reserved. +// Copyright 2022 Google Inc. All rights reserved. // http://ceres-solver.org/ // // Redistribution and use in source and binary forms, with or without @@ -49,9 +49,11 @@ #include "ceres/crs_matrix.h" #include "ceres/evaluation_callback.h" #include "ceres/evaluator.h" +#include "ceres/internal/export.h" #include "ceres/internal/fixed_array.h" -#include "ceres/internal/port.h" #include "ceres/loss_function.h" +#include "ceres/manifold.h" +#include "ceres/manifold_adapter.h" #include "ceres/map_util.h" #include "ceres/parameter_block.h" #include "ceres/program.h" @@ -64,11 +66,6 @@ namespace ceres { namespace internal { - -using std::map; -using std::string; -using std::vector; - namespace { // Returns true if two regions of memory, a and b, with sizes size_a and size_b // respectively, overlap. @@ -130,7 +127,7 @@ ParameterBlock* ProblemImpl::InternalAddParameterBlock(double* values, << "for a parameter with size " << size; // Ignore the request if there is a block for the given pointer already. - ParameterMap::iterator it = parameter_block_map_.find(values); + auto it = parameter_block_map_.find(values); if (it != parameter_block_map_.end()) { if (!options_.disable_all_safety_checks) { int existing_size = it->second->Size(); @@ -146,11 +143,11 @@ ParameterBlock* ProblemImpl::InternalAddParameterBlock(double* values, // Before adding the parameter block, also check that it doesn't alias any // other parameter blocks. if (!parameter_block_map_.empty()) { - ParameterMap::iterator lb = parameter_block_map_.lower_bound(values); + auto lb = parameter_block_map_.lower_bound(values); // If lb is not the first block, check the previous block for aliasing. if (lb != parameter_block_map_.begin()) { - ParameterMap::iterator previous = lb; + auto previous = lb; --previous; CheckForNoAliasing( previous->first, previous->second->Size(), values, size); @@ -165,7 +162,7 @@ ParameterBlock* ProblemImpl::InternalAddParameterBlock(double* values, // Pass the index of the new parameter block as well to keep the index in // sync with the position of the parameter in the program's parameter vector. - ParameterBlock* new_parameter_block = + auto* new_parameter_block = new ParameterBlock(values, size, program_->parameter_blocks_.size()); // For dynamic problems, add the list of dependent residual blocks, which is @@ -192,7 +189,7 @@ void ProblemImpl::InternalRemoveResidualBlock(ResidualBlock* residual_block) { residual_block); } - ResidualBlockSet::iterator it = residual_block_set_.find(residual_block); + auto it = residual_block_set_.find(residual_block); residual_block_set_.erase(it); } DeleteBlockInVector(program_->mutable_residual_blocks(), residual_block); @@ -207,13 +204,13 @@ void ProblemImpl::DeleteBlock(ResidualBlock* residual_block) { // The const casts here are legit, since ResidualBlock holds these // pointers as const pointers but we have ownership of them and // have the right to destroy them when the destructor is called. - CostFunction* cost_function = + auto* cost_function = const_cast<CostFunction*>(residual_block->cost_function()); if (options_.cost_function_ownership == TAKE_OWNERSHIP) { DecrementValueOrDeleteKey(cost_function, &cost_function_ref_count_); } - LossFunction* loss_function = + auto* loss_function = const_cast<LossFunction*>(residual_block->loss_function()); if (options_.loss_function_ownership == TAKE_OWNERSHIP && loss_function != nullptr) { @@ -225,15 +222,7 @@ void ProblemImpl::DeleteBlock(ResidualBlock* residual_block) { // Deletes the parameter block in question, assuming there are no other // references to it inside the problem (e.g. by any residual blocks). -// Referenced parameterizations are tucked away for future deletion, since it -// is not possible to know whether other parts of the problem depend on them -// without doing a full scan. void ProblemImpl::DeleteBlock(ParameterBlock* parameter_block) { - if (options_.local_parameterization_ownership == TAKE_OWNERSHIP && - parameter_block->local_parameterization() != nullptr) { - local_parameterizations_to_delete_.push_back( - parameter_block->mutable_local_parameterization()); - } parameter_block_map_.erase(parameter_block->mutable_user_state()); delete parameter_block; } @@ -264,14 +253,18 @@ ProblemImpl::~ProblemImpl() { } // Collect the unique parameterizations and delete the parameters. - for (int i = 0; i < program_->parameter_blocks_.size(); ++i) { - DeleteBlock(program_->parameter_blocks_[i]); + for (auto* parameter_block : program_->parameter_blocks_) { + DeleteBlock(parameter_block); } // Delete the owned parameterizations. STLDeleteUniqueContainerPointers(local_parameterizations_to_delete_.begin(), local_parameterizations_to_delete_.end()); + // Delete the owned manifolds. + STLDeleteUniqueContainerPointers(manifolds_to_delete_.begin(), + manifolds_to_delete_.end()); + if (context_impl_owned_) { delete context_impl_; } @@ -286,7 +279,7 @@ ResidualBlockId ProblemImpl::AddResidualBlock( CHECK_EQ(num_parameter_blocks, cost_function->parameter_block_sizes().size()); // Check the sizes match. - const vector<int32_t>& parameter_block_sizes = + const std::vector<int32_t>& parameter_block_sizes = cost_function->parameter_block_sizes(); if (!options_.disable_all_safety_checks) { @@ -295,7 +288,7 @@ ResidualBlockId ProblemImpl::AddResidualBlock( << "that the cost function expects."; // Check for duplicate parameter blocks. - vector<double*> sorted_parameter_blocks( + std::vector<double*> sorted_parameter_blocks( parameter_blocks, parameter_blocks + num_parameter_blocks); sort(sorted_parameter_blocks.begin(), sorted_parameter_blocks.end()); const bool has_duplicate_items = @@ -303,7 +296,7 @@ ResidualBlockId ProblemImpl::AddResidualBlock( sorted_parameter_blocks.end()) != sorted_parameter_blocks.end()); if (has_duplicate_items) { - string blocks; + std::string blocks; for (int i = 0; i < num_parameter_blocks; ++i) { blocks += StringPrintf(" %p ", parameter_blocks[i]); } @@ -315,7 +308,7 @@ ResidualBlockId ProblemImpl::AddResidualBlock( } // Add parameter blocks and convert the double*'s to parameter blocks. - vector<ParameterBlock*> parameter_block_ptrs(num_parameter_blocks); + std::vector<ParameterBlock*> parameter_block_ptrs(num_parameter_blocks); for (int i = 0; i < num_parameter_blocks; ++i) { parameter_block_ptrs[i] = InternalAddParameterBlock( parameter_blocks[i], parameter_block_sizes[i]); @@ -334,7 +327,7 @@ ResidualBlockId ProblemImpl::AddResidualBlock( } } - ResidualBlock* new_residual_block = + auto* new_residual_block = new ResidualBlock(cost_function, loss_function, parameter_block_ptrs, @@ -372,12 +365,50 @@ void ProblemImpl::AddParameterBlock(double* values, int size) { InternalAddParameterBlock(values, size); } +void ProblemImpl::InternalSetParameterization( + double* values, + ParameterBlock* parameter_block, + LocalParameterization* local_parameterization) { + parameter_block_to_local_param_[values] = local_parameterization; + Manifold* manifold = nullptr; + if (local_parameterization != nullptr) { + if (options_.local_parameterization_ownership == TAKE_OWNERSHIP) { + local_parameterizations_to_delete_.push_back(local_parameterization); + } + + manifold = new ManifoldAdapter(local_parameterization); + // Add the manifold to manifolds_to_delete_ unconditionally since + // we own it and it will need to be deleted. + manifolds_to_delete_.push_back(manifold); + } + + parameter_block->SetManifold(manifold); +} + +void ProblemImpl::InternalSetManifold(double* values, + ParameterBlock* parameter_block, + Manifold* manifold) { + // Reset any association between this parameter block and a local + // parameterization. This only needs done while we are in the transition from + // LocalParameterization to Manifold. + parameter_block_to_local_param_[values] = nullptr; + if (manifold != nullptr && options_.manifold_ownership == TAKE_OWNERSHIP) { + manifolds_to_delete_.push_back(manifold); + } + parameter_block->SetManifold(manifold); +} + void ProblemImpl::AddParameterBlock( double* values, int size, LocalParameterization* local_parameterization) { ParameterBlock* parameter_block = InternalAddParameterBlock(values, size); - if (local_parameterization != nullptr) { - parameter_block->SetParameterization(local_parameterization); - } + InternalSetParameterization(values, parameter_block, local_parameterization); +} + +void ProblemImpl::AddParameterBlock(double* values, + int size, + Manifold* manifold) { + ParameterBlock* parameter_block = InternalAddParameterBlock(values, size); + InternalSetManifold(values, parameter_block, manifold); } // Delete a block from a vector of blocks, maintaining the indexing invariant. @@ -385,7 +416,7 @@ void ProblemImpl::AddParameterBlock( // vector over the element to remove, then popping the last element. It // destroys the ordering in the interest of speed. template <typename Block> -void ProblemImpl::DeleteBlockInVector(vector<Block*>* mutable_blocks, +void ProblemImpl::DeleteBlockInVector(std::vector<Block*>* mutable_blocks, Block* block_to_remove) { CHECK_EQ((*mutable_blocks)[block_to_remove->index()], block_to_remove) << "You found a Ceres bug! \n" @@ -411,7 +442,7 @@ void ProblemImpl::RemoveResidualBlock(ResidualBlock* residual_block) { CHECK(residual_block != nullptr); // Verify that residual_block identifies a residual in the current problem. - const string residual_not_found_message = StringPrintf( + const std::string residual_not_found_message = StringPrintf( "Residual block to remove: %p not found. This usually means " "one of three things have happened:\n" " 1) residual_block is uninitialised and points to a random " @@ -449,11 +480,11 @@ void ProblemImpl::RemoveParameterBlock(const double* values) { if (options_.enable_fast_removal) { // Copy the dependent residuals from the parameter block because the set of // dependents will change after each call to RemoveResidualBlock(). - vector<ResidualBlock*> residual_blocks_to_remove( + std::vector<ResidualBlock*> residual_blocks_to_remove( parameter_block->mutable_residual_blocks()->begin(), parameter_block->mutable_residual_blocks()->end()); - for (int i = 0; i < residual_blocks_to_remove.size(); ++i) { - InternalRemoveResidualBlock(residual_blocks_to_remove[i]); + for (auto* residual_block : residual_blocks_to_remove) { + InternalRemoveResidualBlock(residual_block); } } else { // Scan all the residual blocks to remove ones that depend on the parameter @@ -518,20 +549,31 @@ void ProblemImpl::SetParameterization( << "you can set its local parameterization."; } - // If the parameter block already has a local parameterization and - // we are to take ownership of local parameterizations, then add it - // to local_parameterizations_to_delete_ for eventual deletion. - if (parameter_block->local_parameterization_ && - options_.local_parameterization_ownership == TAKE_OWNERSHIP) { - local_parameterizations_to_delete_.push_back( - parameter_block->local_parameterization_); + InternalSetParameterization(values, parameter_block, local_parameterization); +} + +void ProblemImpl::SetManifold(double* values, Manifold* manifold) { + ParameterBlock* parameter_block = + FindWithDefault(parameter_block_map_, values, nullptr); + if (parameter_block == nullptr) { + LOG(FATAL) << "Parameter block not found: " << values + << ". You must add the parameter block to the problem before " + << "you can set its manifold."; } - parameter_block->SetParameterization(local_parameterization); + InternalSetManifold(values, parameter_block, manifold); } const LocalParameterization* ProblemImpl::GetParameterization( const double* values) const { + return FindWithDefault(parameter_block_to_local_param_, values, nullptr); +} + +bool ProblemImpl::HasParameterization(const double* values) const { + return GetParameterization(values) != nullptr; +} + +const Manifold* ProblemImpl::GetManifold(const double* values) const { ParameterBlock* parameter_block = FindWithDefault( parameter_block_map_, const_cast<double*>(values), nullptr); if (parameter_block == nullptr) { @@ -540,7 +582,11 @@ const LocalParameterization* ProblemImpl::GetParameterization( << "you can get its local parameterization."; } - return parameter_block->local_parameterization(); + return parameter_block->manifold(); +} + +bool ProblemImpl::HasManifold(const double* values) const { + return GetManifold(values) != nullptr; } void ProblemImpl::SetParameterLowerBound(double* values, @@ -596,8 +642,8 @@ double ProblemImpl::GetParameterUpperBound(const double* values, bool ProblemImpl::Evaluate(const Problem::EvaluateOptions& evaluate_options, double* cost, - vector<double>* residuals, - vector<double>* gradient, + std::vector<double>* residuals, + std::vector<double>* gradient, CRSMatrix* jacobian) { if (cost == nullptr && residuals == nullptr && gradient == nullptr && jacobian == nullptr) { @@ -612,11 +658,11 @@ bool ProblemImpl::Evaluate(const Problem::EvaluateOptions& evaluate_options, ? evaluate_options.residual_blocks : program_->residual_blocks()); - const vector<double*>& parameter_block_ptrs = + const std::vector<double*>& parameter_block_ptrs = evaluate_options.parameter_blocks; - vector<ParameterBlock*> variable_parameter_blocks; - vector<ParameterBlock*>& parameter_blocks = + std::vector<ParameterBlock*> variable_parameter_blocks; + std::vector<ParameterBlock*>& parameter_blocks = *program.mutable_parameter_blocks(); if (parameter_block_ptrs.size() == 0) { @@ -649,11 +695,12 @@ bool ProblemImpl::Evaluate(const Problem::EvaluateOptions& evaluate_options, // columns of the jacobian, we need to make sure that they are // constant during evaluation and then make them variable again // after we are done. - vector<ParameterBlock*> all_parameter_blocks(program_->parameter_blocks()); - vector<ParameterBlock*> included_parameter_blocks( + std::vector<ParameterBlock*> all_parameter_blocks( + program_->parameter_blocks()); + std::vector<ParameterBlock*> included_parameter_blocks( program.parameter_blocks()); - vector<ParameterBlock*> excluded_parameter_blocks; + std::vector<ParameterBlock*> excluded_parameter_blocks; sort(all_parameter_blocks.begin(), all_parameter_blocks.end()); sort(included_parameter_blocks.begin(), included_parameter_blocks.end()); set_difference(all_parameter_blocks.begin(), @@ -663,8 +710,7 @@ bool ProblemImpl::Evaluate(const Problem::EvaluateOptions& evaluate_options, back_inserter(excluded_parameter_blocks)); variable_parameter_blocks.reserve(excluded_parameter_blocks.size()); - for (int i = 0; i < excluded_parameter_blocks.size(); ++i) { - ParameterBlock* parameter_block = excluded_parameter_blocks[i]; + for (auto* parameter_block : excluded_parameter_blocks) { if (!parameter_block->IsConstant()) { variable_parameter_blocks.push_back(parameter_block); parameter_block->SetConstant(); @@ -716,8 +762,8 @@ bool ProblemImpl::Evaluate(const Problem::EvaluateOptions& evaluate_options, std::unique_ptr<CompressedRowSparseMatrix> tmp_jacobian; if (jacobian != nullptr) { - tmp_jacobian.reset( - down_cast<CompressedRowSparseMatrix*>(evaluator->CreateJacobian())); + tmp_jacobian.reset(down_cast<CompressedRowSparseMatrix*>( + evaluator->CreateJacobian().release())); } // Point the state pointers to the user state pointers. This is @@ -749,8 +795,8 @@ bool ProblemImpl::Evaluate(const Problem::EvaluateOptions& evaluate_options, // Make the parameter blocks that were temporarily marked constant, // variable again. - for (int i = 0; i < variable_parameter_blocks.size(); ++i) { - variable_parameter_blocks[i]->SetVarying(); + for (auto* parameter_block : variable_parameter_blocks) { + parameter_block->SetVarying(); } if (status) { @@ -829,24 +875,25 @@ int ProblemImpl::ParameterBlockSize(const double* values) const { return parameter_block->Size(); } -int ProblemImpl::ParameterBlockLocalSize(const double* values) const { +int ProblemImpl::ParameterBlockTangentSize(const double* values) const { ParameterBlock* parameter_block = FindWithDefault( parameter_block_map_, const_cast<double*>(values), nullptr); if (parameter_block == nullptr) { LOG(FATAL) << "Parameter block not found: " << values << ". You must add the parameter block to the problem before " - << "you can get its local size."; + << "you can get its tangent size."; } - return parameter_block->LocalSize(); + return parameter_block->TangentSize(); } -bool ProblemImpl::HasParameterBlock(const double* parameter_block) const { - return (parameter_block_map_.find(const_cast<double*>(parameter_block)) != +bool ProblemImpl::HasParameterBlock(const double* values) const { + return (parameter_block_map_.find(const_cast<double*>(values)) != parameter_block_map_.end()); } -void ProblemImpl::GetParameterBlocks(vector<double*>* parameter_blocks) const { +void ProblemImpl::GetParameterBlocks( + std::vector<double*>* parameter_blocks) const { CHECK(parameter_blocks != nullptr); parameter_blocks->resize(0); parameter_blocks->reserve(parameter_block_map_.size()); @@ -856,14 +903,14 @@ void ProblemImpl::GetParameterBlocks(vector<double*>* parameter_blocks) const { } void ProblemImpl::GetResidualBlocks( - vector<ResidualBlockId>* residual_blocks) const { + std::vector<ResidualBlockId>* residual_blocks) const { CHECK(residual_blocks != nullptr); *residual_blocks = program().residual_blocks(); } void ProblemImpl::GetParameterBlocksForResidualBlock( const ResidualBlockId residual_block, - vector<double*>* parameter_blocks) const { + std::vector<double*>* parameter_blocks) const { int num_parameter_blocks = residual_block->NumParameterBlocks(); CHECK(parameter_blocks != nullptr); parameter_blocks->resize(num_parameter_blocks); @@ -884,7 +931,7 @@ const LossFunction* ProblemImpl::GetLossFunctionForResidualBlock( } void ProblemImpl::GetResidualBlocksForParameterBlock( - const double* values, vector<ResidualBlockId>* residual_blocks) const { + const double* values, std::vector<ResidualBlockId>* residual_blocks) const { ParameterBlock* parameter_block = FindWithDefault( parameter_block_map_, const_cast<double*>(values), nullptr); if (parameter_block == nullptr) { diff --git a/extern/ceres/internal/ceres/problem_impl.h b/extern/ceres/internal/ceres/problem_impl.h index 9abff3f19ae..22073b674f1 100644 --- a/extern/ceres/internal/ceres/problem_impl.h +++ b/extern/ceres/internal/ceres/problem_impl.h @@ -1,5 +1,5 @@ // Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2019 Google Inc. All rights reserved. +// Copyright 2021 Google Inc. All rights reserved. // http://ceres-solver.org/ // // Redistribution and use in source and binary forms, with or without @@ -42,11 +42,15 @@ #include <array> #include <map> #include <memory> +#include <unordered_map> #include <unordered_set> #include <vector> #include "ceres/context_impl.h" +#include "ceres/internal/disable_warnings.h" +#include "ceres/internal/export.h" #include "ceres/internal/port.h" +#include "ceres/manifold.h" #include "ceres/problem.h" #include "ceres/types.h" @@ -63,12 +67,12 @@ namespace internal { class Program; class ResidualBlock; -class CERES_EXPORT_INTERNAL ProblemImpl { +class CERES_NO_EXPORT ProblemImpl { public: - typedef std::map<double*, ParameterBlock*> ParameterMap; - typedef std::unordered_set<ResidualBlock*> ResidualBlockSet; - typedef std::map<CostFunction*, int> CostFunctionRefCount; - typedef std::map<LossFunction*, int> LossFunctionRefCount; + using ParameterMap = std::map<double*, ParameterBlock*>; + using ResidualBlockSet = std::unordered_set<ResidualBlock*>; + using CostFunctionRefCount = std::map<CostFunction*, int>; + using LossFunctionRefCount = std::map<LossFunction*, int>; ProblemImpl(); explicit ProblemImpl(const Problem::Options& options); @@ -100,6 +104,8 @@ class CERES_EXPORT_INTERNAL ProblemImpl { int size, LocalParameterization* local_parameterization); + void AddParameterBlock(double* values, int size, Manifold* manifold); + void RemoveResidualBlock(ResidualBlock* residual_block); void RemoveParameterBlock(const double* values); @@ -110,6 +116,11 @@ class CERES_EXPORT_INTERNAL ProblemImpl { void SetParameterization(double* values, LocalParameterization* local_parameterization); const LocalParameterization* GetParameterization(const double* values) const; + bool HasParameterization(const double* values) const; + + void SetManifold(double* values, Manifold* manifold); + const Manifold* GetManifold(const double* values) const; + bool HasManifold(const double* values) const; void SetParameterLowerBound(double* values, int index, double lower_bound); void SetParameterUpperBound(double* values, int index, double upper_bound); @@ -134,10 +145,10 @@ class CERES_EXPORT_INTERNAL ProblemImpl { int NumResidualBlocks() const; int NumResiduals() const; - int ParameterBlockSize(const double* parameter_block) const; - int ParameterBlockLocalSize(const double* parameter_block) const; + int ParameterBlockSize(const double* values) const; + int ParameterBlockTangentSize(const double* values) const; - bool HasParameterBlock(const double* parameter_block) const; + bool HasParameterBlock(const double* values) const; void GetParameterBlocks(std::vector<double*>* parameter_blocks) const; void GetResidualBlocks(std::vector<ResidualBlockId>* residual_blocks) const; @@ -169,6 +180,14 @@ class CERES_EXPORT_INTERNAL ProblemImpl { private: ParameterBlock* InternalAddParameterBlock(double* values, int size); + void InternalSetParameterization( + double* values, + ParameterBlock* parameter_block, + LocalParameterization* local_parameterization); + void InternalSetManifold(double* values, + ParameterBlock* parameter_block, + Manifold* manifold); + void InternalRemoveResidualBlock(ResidualBlock* residual_block); // Delete the arguments in question. These differ from the Remove* functions @@ -194,23 +213,45 @@ class CERES_EXPORT_INTERNAL ProblemImpl { // The actual parameter and residual blocks. std::unique_ptr<internal::Program> program_; + // TODO(sameeragarwal): Unify the shared object handling across object types. + // Right now we are using vectors for LocalParameterization and Manifold + // objects and reference counting for CostFunctions and LossFunctions. Ideally + // this should be done uniformly. + // When removing parameter blocks, parameterizations have ambiguous // ownership. Instead of scanning the entire problem to see if the // parameterization is shared with other parameter blocks, buffer // them until destruction. - // - // TODO(keir): See if it makes sense to use sets instead. std::vector<LocalParameterization*> local_parameterizations_to_delete_; + // When removing parameter blocks, manifolds have ambiguous + // ownership. Instead of scanning the entire problem to see if the + // manifold is shared with other parameter blocks, buffer + // them until destruction. + std::vector<Manifold*> manifolds_to_delete_; + // For each cost function and loss function in the problem, a count // of the number of residual blocks that refer to them. When the // count goes to zero and the problem owns these objects, they are // destroyed. CostFunctionRefCount cost_function_ref_count_; LossFunctionRefCount loss_function_ref_count_; + + // Because we wrap LocalParameterization objects using a ManifoldAdapter, when + // the user calls GetParameterization we cannot use the same logic as + // GetManifold as the ParameterBlock object only returns a Manifold object. So + // this map stores the association between parameter blocks and local + // parameterizations. + // + // This is a temporary object which will be removed once the + // LocalParameterization to Manifold transition is complete. + std::unordered_map<const double*, LocalParameterization*> + parameter_block_to_local_param_; }; } // namespace internal } // namespace ceres +#include "ceres/internal/reenable_warnings.h" + #endif // CERES_PUBLIC_PROBLEM_IMPL_H_ diff --git a/extern/ceres/internal/ceres/program.cc b/extern/ceres/internal/ceres/program.cc index f1ded2e5d5a..1cb9ebcbe73 100644 --- a/extern/ceres/internal/ceres/program.cc +++ b/extern/ceres/internal/ceres/program.cc @@ -33,6 +33,7 @@ #include <algorithm> #include <map> #include <memory> +#include <string> #include <vector> #include "ceres/array_utils.h" @@ -40,9 +41,9 @@ #include "ceres/compressed_row_sparse_matrix.h" #include "ceres/cost_function.h" #include "ceres/evaluator.h" -#include "ceres/internal/port.h" -#include "ceres/local_parameterization.h" +#include "ceres/internal/export.h" #include "ceres/loss_function.h" +#include "ceres/manifold.h" #include "ceres/map_util.h" #include "ceres/parameter_block.h" #include "ceres/problem.h" @@ -53,31 +54,19 @@ namespace ceres { namespace internal { -using std::max; -using std::set; -using std::string; -using std::vector; - -Program::Program() {} - -Program::Program(const Program& program) - : parameter_blocks_(program.parameter_blocks_), - residual_blocks_(program.residual_blocks_), - evaluation_callback_(program.evaluation_callback_) {} - -const vector<ParameterBlock*>& Program::parameter_blocks() const { +const std::vector<ParameterBlock*>& Program::parameter_blocks() const { return parameter_blocks_; } -const vector<ResidualBlock*>& Program::residual_blocks() const { +const std::vector<ResidualBlock*>& Program::residual_blocks() const { return residual_blocks_; } -vector<ParameterBlock*>* Program::mutable_parameter_blocks() { +std::vector<ParameterBlock*>* Program::mutable_parameter_blocks() { return ¶meter_blocks_; } -vector<ResidualBlock*>* Program::mutable_residual_blocks() { +std::vector<ResidualBlock*>* Program::mutable_residual_blocks() { return &residual_blocks_; } @@ -86,33 +75,32 @@ EvaluationCallback* Program::mutable_evaluation_callback() { } bool Program::StateVectorToParameterBlocks(const double* state) { - for (int i = 0; i < parameter_blocks_.size(); ++i) { - if (!parameter_blocks_[i]->IsConstant() && - !parameter_blocks_[i]->SetState(state)) { + for (auto* parameter_block : parameter_blocks_) { + if (!parameter_block->IsConstant() && !parameter_block->SetState(state)) { return false; } - state += parameter_blocks_[i]->Size(); + state += parameter_block->Size(); } return true; } void Program::ParameterBlocksToStateVector(double* state) const { - for (int i = 0; i < parameter_blocks_.size(); ++i) { - parameter_blocks_[i]->GetState(state); - state += parameter_blocks_[i]->Size(); + for (auto* parameter_block : parameter_blocks_) { + parameter_block->GetState(state); + state += parameter_block->Size(); } } void Program::CopyParameterBlockStateToUserState() { - for (int i = 0; i < parameter_blocks_.size(); ++i) { - parameter_blocks_[i]->GetState(parameter_blocks_[i]->mutable_user_state()); + for (auto* parameter_block : parameter_blocks_) { + parameter_block->GetState(parameter_block->mutable_user_state()); } } bool Program::SetParameterBlockStatePtrsToUserStatePtrs() { - for (int i = 0; i < parameter_blocks_.size(); ++i) { - if (!parameter_blocks_[i]->IsConstant() && - !parameter_blocks_[i]->SetState(parameter_blocks_[i]->user_state())) { + for (auto* parameter_block : parameter_blocks_) { + if (!parameter_block->IsConstant() && + !parameter_block->SetState(parameter_block->user_state())) { return false; } } @@ -122,13 +110,13 @@ bool Program::SetParameterBlockStatePtrsToUserStatePtrs() { bool Program::Plus(const double* state, const double* delta, double* state_plus_delta) const { - for (int i = 0; i < parameter_blocks_.size(); ++i) { - if (!parameter_blocks_[i]->Plus(state, delta, state_plus_delta)) { + for (auto* parameter_block : parameter_blocks_) { + if (!parameter_block->Plus(state, delta, state_plus_delta)) { return false; } - state += parameter_blocks_[i]->Size(); - delta += parameter_blocks_[i]->LocalSize(); - state_plus_delta += parameter_blocks_[i]->Size(); + state += parameter_block->Size(); + delta += parameter_block->TangentSize(); + state_plus_delta += parameter_block->Size(); } return true; } @@ -136,8 +124,7 @@ bool Program::Plus(const double* state, void Program::SetParameterOffsetsAndIndex() { // Set positions for all parameters appearing as arguments to residuals to one // past the end of the parameter block array. - for (int i = 0; i < residual_blocks_.size(); ++i) { - ResidualBlock* residual_block = residual_blocks_[i]; + for (auto* residual_block : residual_blocks_) { for (int j = 0; j < residual_block->NumParameterBlocks(); ++j) { residual_block->parameter_blocks()[j]->set_index(-1); } @@ -150,7 +137,7 @@ void Program::SetParameterOffsetsAndIndex() { parameter_blocks_[i]->set_state_offset(state_offset); parameter_blocks_[i]->set_delta_offset(delta_offset); state_offset += parameter_blocks_[i]->Size(); - delta_offset += parameter_blocks_[i]->LocalSize(); + delta_offset += parameter_blocks_[i]->TangentSize(); } } @@ -178,16 +165,15 @@ bool Program::IsValid() const { } state_offset += parameter_blocks_[i]->Size(); - delta_offset += parameter_blocks_[i]->LocalSize(); + delta_offset += parameter_blocks_[i]->TangentSize(); } return true; } -bool Program::ParameterBlocksAreFinite(string* message) const { +bool Program::ParameterBlocksAreFinite(std::string* message) const { CHECK(message != nullptr); - for (int i = 0; i < parameter_blocks_.size(); ++i) { - const ParameterBlock* parameter_block = parameter_blocks_[i]; + for (auto* parameter_block : parameter_blocks_) { const double* array = parameter_block->user_state(); const int size = parameter_block->Size(); const int invalid_index = FindInvalidValue(size, array); @@ -207,8 +193,7 @@ bool Program::ParameterBlocksAreFinite(string* message) const { } bool Program::IsBoundsConstrained() const { - for (int i = 0; i < parameter_blocks_.size(); ++i) { - const ParameterBlock* parameter_block = parameter_blocks_[i]; + for (auto* parameter_block : parameter_blocks_) { if (parameter_block->IsConstant()) { continue; } @@ -225,10 +210,9 @@ bool Program::IsBoundsConstrained() const { return false; } -bool Program::IsFeasible(string* message) const { +bool Program::IsFeasible(std::string* message) const { CHECK(message != nullptr); - for (int i = 0; i < parameter_blocks_.size(); ++i) { - const ParameterBlock* parameter_block = parameter_blocks_[i]; + for (auto* parameter_block : parameter_blocks_) { const double* parameters = parameter_block->user_state(); const int size = parameter_block->Size(); if (parameter_block->IsConstant()) { @@ -284,42 +268,42 @@ bool Program::IsFeasible(string* message) const { return true; } -Program* Program::CreateReducedProgram( - vector<double*>* removed_parameter_blocks, +std::unique_ptr<Program> Program::CreateReducedProgram( + std::vector<double*>* removed_parameter_blocks, double* fixed_cost, - string* error) const { + std::string* error) const { CHECK(removed_parameter_blocks != nullptr); CHECK(fixed_cost != nullptr); CHECK(error != nullptr); - std::unique_ptr<Program> reduced_program(new Program(*this)); + std::unique_ptr<Program> reduced_program = std::make_unique<Program>(*this); if (!reduced_program->RemoveFixedBlocks( removed_parameter_blocks, fixed_cost, error)) { return nullptr; } reduced_program->SetParameterOffsetsAndIndex(); - return reduced_program.release(); + return reduced_program; } -bool Program::RemoveFixedBlocks(vector<double*>* removed_parameter_blocks, +bool Program::RemoveFixedBlocks(std::vector<double*>* removed_parameter_blocks, double* fixed_cost, - string* error) { + std::string* error) { CHECK(removed_parameter_blocks != nullptr); CHECK(fixed_cost != nullptr); CHECK(error != nullptr); std::unique_ptr<double[]> residual_block_evaluate_scratch; - residual_block_evaluate_scratch.reset( - new double[MaxScratchDoublesNeededForEvaluate()]); + residual_block_evaluate_scratch = + std::make_unique<double[]>(MaxScratchDoublesNeededForEvaluate()); *fixed_cost = 0.0; bool need_to_call_prepare_for_evaluation = evaluation_callback_ != nullptr; // Mark all the parameters as unused. Abuse the index member of the // parameter blocks for the marking. - for (int i = 0; i < parameter_blocks_.size(); ++i) { - parameter_blocks_[i]->set_index(-1); + for (auto* parameter_block : parameter_blocks_) { + parameter_block->set_index(-1); } // Filter out residual that have all-constant parameters, and mark @@ -391,8 +375,7 @@ bool Program::RemoveFixedBlocks(vector<double*>* removed_parameter_blocks, // Filter out unused or fixed parameter blocks. int num_active_parameter_blocks = 0; removed_parameter_blocks->clear(); - for (int i = 0; i < parameter_blocks_.size(); ++i) { - ParameterBlock* parameter_block = parameter_blocks_[i]; + for (auto* parameter_block : parameter_blocks_) { if (parameter_block->index() == -1) { removed_parameter_blocks->push_back( parameter_block->mutable_user_state()); @@ -412,7 +395,7 @@ bool Program::RemoveFixedBlocks(vector<double*>* removed_parameter_blocks, } bool Program::IsParameterBlockSetIndependent( - const set<double*>& independent_set) const { + const std::set<double*>& independent_set) const { // Loop over each residual block and ensure that no two parameter // blocks in the same residual block are part of // parameter_block_ptrs as that would violate the assumption that it @@ -483,24 +466,24 @@ int Program::NumParameterBlocks() const { return parameter_blocks_.size(); } int Program::NumResiduals() const { int num_residuals = 0; - for (int i = 0; i < residual_blocks_.size(); ++i) { - num_residuals += residual_blocks_[i]->NumResiduals(); + for (auto* residual_block : residual_blocks_) { + num_residuals += residual_block->NumResiduals(); } return num_residuals; } int Program::NumParameters() const { int num_parameters = 0; - for (int i = 0; i < parameter_blocks_.size(); ++i) { - num_parameters += parameter_blocks_[i]->Size(); + for (auto* parameter_block : parameter_blocks_) { + num_parameters += parameter_block->Size(); } return num_parameters; } int Program::NumEffectiveParameters() const { int num_parameters = 0; - for (int i = 0; i < parameter_blocks_.size(); ++i) { - num_parameters += parameter_blocks_[i]->LocalSize(); + for (auto* parameter_block : parameter_blocks_) { + num_parameters += parameter_block->TangentSize(); } return num_parameters; } @@ -511,48 +494,47 @@ int Program::NumEffectiveParameters() const { int Program::MaxScratchDoublesNeededForEvaluate() const { // Compute the scratch space needed for evaluate. int max_scratch_bytes_for_evaluate = 0; - for (int i = 0; i < residual_blocks_.size(); ++i) { + for (auto* residual_block : residual_blocks_) { max_scratch_bytes_for_evaluate = - max(max_scratch_bytes_for_evaluate, - residual_blocks_[i]->NumScratchDoublesForEvaluate()); + std::max(max_scratch_bytes_for_evaluate, + residual_block->NumScratchDoublesForEvaluate()); } return max_scratch_bytes_for_evaluate; } int Program::MaxDerivativesPerResidualBlock() const { int max_derivatives = 0; - for (int i = 0; i < residual_blocks_.size(); ++i) { + for (auto* residual_block : residual_blocks_) { int derivatives = 0; - ResidualBlock* residual_block = residual_blocks_[i]; int num_parameters = residual_block->NumParameterBlocks(); for (int j = 0; j < num_parameters; ++j) { derivatives += residual_block->NumResiduals() * - residual_block->parameter_blocks()[j]->LocalSize(); + residual_block->parameter_blocks()[j]->TangentSize(); } - max_derivatives = max(max_derivatives, derivatives); + max_derivatives = std::max(max_derivatives, derivatives); } return max_derivatives; } int Program::MaxParametersPerResidualBlock() const { int max_parameters = 0; - for (int i = 0; i < residual_blocks_.size(); ++i) { + for (auto* residual_block : residual_blocks_) { max_parameters = - max(max_parameters, residual_blocks_[i]->NumParameterBlocks()); + std::max(max_parameters, residual_block->NumParameterBlocks()); } return max_parameters; } int Program::MaxResidualsPerResidualBlock() const { int max_residuals = 0; - for (int i = 0; i < residual_blocks_.size(); ++i) { - max_residuals = max(max_residuals, residual_blocks_[i]->NumResiduals()); + for (auto* residual_block : residual_blocks_) { + max_residuals = std::max(max_residuals, residual_block->NumResiduals()); } return max_residuals; } -string Program::ToString() const { - string ret = "Program dump\n"; +std::string Program::ToString() const { + std::string ret = "Program dump\n"; ret += StringPrintf("Number of parameter blocks: %d\n", NumParameterBlocks()); ret += StringPrintf("Number of parameters: %d\n", NumParameters()); ret += "Parameters:\n"; diff --git a/extern/ceres/internal/ceres/program.h b/extern/ceres/internal/ceres/program.h index ca29d316284..4dbd1ba5ff1 100644 --- a/extern/ceres/internal/ceres/program.h +++ b/extern/ceres/internal/ceres/program.h @@ -37,7 +37,8 @@ #include <vector> #include "ceres/evaluation_callback.h" -#include "ceres/internal/port.h" +#include "ceres/internal/disable_warnings.h" +#include "ceres/internal/export.h" namespace ceres { namespace internal { @@ -57,11 +58,8 @@ class TripletSparseMatrix; // another; for example, the first stage of solving involves stripping all // constant parameters and residuals. This is in contrast with Problem, which is // not built for transformation. -class CERES_EXPORT_INTERNAL Program { +class CERES_NO_EXPORT Program { public: - Program(); - explicit Program(const Program& program); - // The ordered parameter and residual blocks for the program. const std::vector<ParameterBlock*>& parameter_blocks() const; const std::vector<ResidualBlock*>& residual_blocks() const; @@ -72,9 +70,9 @@ class CERES_EXPORT_INTERNAL Program { // Serialize to/from the program and update states. // // NOTE: Setting the state of a parameter block can trigger the - // computation of the Jacobian of its local parameterization. If - // this computation fails for some reason, then this method returns - // false and the state of the parameter blocks cannot be trusted. + // computation of the Jacobian of its manifold. If this computation fails for + // some reason, then this method returns false and the state of the parameter + // blocks cannot be trusted. bool StateVectorToParameterBlocks(const double* state); void ParameterBlocksToStateVector(double* state) const; @@ -82,8 +80,8 @@ class CERES_EXPORT_INTERNAL Program { void CopyParameterBlockStateToUserState(); // Set the parameter block pointers to the user pointers. Since this - // runs parameter block set state internally, which may call local - // parameterizations, this can fail. False is returned on failure. + // runs parameter block set state internally, which may call manifold, this + // can fail. False is returned on failure. bool SetParameterBlockStatePtrsToUserStatePtrs(); // Update a state vector for the program given a delta. @@ -146,12 +144,13 @@ class CERES_EXPORT_INTERNAL Program { // fixed_cost will be equal to the sum of the costs of the residual // blocks that were removed. // - // If there was a problem, then the function will return a NULL + // If there was a problem, then the function will return a nullptr // pointer and error will contain a human readable description of // the problem. - Program* CreateReducedProgram(std::vector<double*>* removed_parameter_blocks, - double* fixed_cost, - std::string* error) const; + std::unique_ptr<Program> CreateReducedProgram( + std::vector<double*>* removed_parameter_blocks, + double* fixed_cost, + std::string* error) const; // See problem.h for what these do. int NumParameterBlocks() const; @@ -196,4 +195,6 @@ class CERES_EXPORT_INTERNAL Program { } // namespace internal } // namespace ceres +#include "ceres/internal/reenable_warnings.h" + #endif // CERES_INTERNAL_PROGRAM_H_ diff --git a/extern/ceres/internal/ceres/program_evaluator.h b/extern/ceres/internal/ceres/program_evaluator.h index 36c9c64baf6..826a73a9af1 100644 --- a/extern/ceres/internal/ceres/program_evaluator.h +++ b/extern/ceres/internal/ceres/program_evaluator.h @@ -59,11 +59,13 @@ // class JacobianWriter { // // Create a jacobian that this writer can write. Same as // // Evaluator::CreateJacobian. -// SparseMatrix* CreateJacobian() const; +// std::unique_ptr<SparseMatrix> CreateJacobian() const; // -// // Create num_threads evaluate preparers. Caller owns result which must -// // be freed with delete[]. Resulting preparers are valid while *this is. -// EvaluatePreparer* CreateEvaluatePreparers(int num_threads); +// // Create num_threads evaluate preparers.Resulting preparers are valid +// // while *this is. +// +// std::unique_ptr<EvaluatePreparer[]> CreateEvaluatePreparers( +// int num_threads); // // // Write the block jacobians from a residual block evaluation to the // // larger sparse jacobian. @@ -81,7 +83,7 @@ // This include must come before any #ifndef check on Ceres compile options. // clang-format off -#include "ceres/internal/port.h" +#include "ceres/internal/config.h" // clang-format on #include <atomic> @@ -109,14 +111,14 @@ struct NullJacobianFinalizer { template <typename EvaluatePreparer, typename JacobianWriter, typename JacobianFinalizer = NullJacobianFinalizer> -class ProgramEvaluator : public Evaluator { +class ProgramEvaluator final : public Evaluator { public: ProgramEvaluator(const Evaluator::Options& options, Program* program) : options_(options), program_(program), jacobian_writer_(options, program), - evaluate_preparers_( - jacobian_writer_.CreateEvaluatePreparers(options.num_threads)) { + evaluate_preparers_(std::move( + jacobian_writer_.CreateEvaluatePreparers(options.num_threads))) { #ifdef CERES_NO_THREADS if (options_.num_threads > 1) { LOG(WARNING) << "No threading support is compiled into this binary; " @@ -127,12 +129,12 @@ class ProgramEvaluator : public Evaluator { #endif // CERES_NO_THREADS BuildResidualLayout(*program, &residual_layout_); - evaluate_scratch_.reset( - CreateEvaluatorScratch(*program, options.num_threads)); + evaluate_scratch_ = + std::move(CreateEvaluatorScratch(*program, options.num_threads)); } // Implementation of Evaluator interface. - SparseMatrix* CreateJacobian() const final { + std::unique_ptr<SparseMatrix> CreateJacobian() const final { return jacobian_writer_.CreateJacobian(); } @@ -250,7 +252,7 @@ class ProgramEvaluator : public Evaluator { MatrixTransposeVectorMultiply<Eigen::Dynamic, Eigen::Dynamic, 1>( block_jacobians[j], num_residuals, - parameter_block->LocalSize(), + parameter_block->TangentSize(), block_residuals, scratch->gradient.get() + parameter_block->delta_offset()); } @@ -309,18 +311,19 @@ class ProgramEvaluator : public Evaluator { int max_scratch_doubles_needed_for_evaluate, int max_residuals_per_residual_block, int num_parameters) { - residual_block_evaluate_scratch.reset( - new double[max_scratch_doubles_needed_for_evaluate]); - gradient.reset(new double[num_parameters]); + residual_block_evaluate_scratch = + std::make_unique<double[]>(max_scratch_doubles_needed_for_evaluate); + gradient = std::make_unique<double[]>(num_parameters); VectorRef(gradient.get(), num_parameters).setZero(); - residual_block_residuals.reset( - new double[max_residuals_per_residual_block]); - jacobian_block_ptrs.reset(new double*[max_parameters_per_residual_block]); + residual_block_residuals = + std::make_unique<double[]>(max_residuals_per_residual_block); + jacobian_block_ptrs = + std::make_unique<double*[]>(max_parameters_per_residual_block); } double cost; std::unique_ptr<double[]> residual_block_evaluate_scratch; - // The gradient in the local parameterization. + // The gradient on the manifold. std::unique_ptr<double[]> gradient; // Enough space to store the residual for the largest residual block. std::unique_ptr<double[]> residual_block_residuals; @@ -341,8 +344,8 @@ class ProgramEvaluator : public Evaluator { } // Create scratch space for each thread evaluating the program. - static EvaluateScratch* CreateEvaluatorScratch(const Program& program, - int num_threads) { + static std::unique_ptr<EvaluateScratch[]> CreateEvaluatorScratch( + const Program& program, int num_threads) { int max_parameters_per_residual_block = program.MaxParametersPerResidualBlock(); int max_scratch_doubles_needed_for_evaluate = @@ -351,7 +354,7 @@ class ProgramEvaluator : public Evaluator { program.MaxResidualsPerResidualBlock(); int num_parameters = program.NumEffectiveParameters(); - EvaluateScratch* evaluate_scratch = new EvaluateScratch[num_threads]; + auto evaluate_scratch = std::make_unique<EvaluateScratch[]>(num_threads); for (int i = 0; i < num_threads; i++) { evaluate_scratch[i].Init(max_parameters_per_residual_block, max_scratch_doubles_needed_for_evaluate, diff --git a/extern/ceres/internal/ceres/random.h b/extern/ceres/internal/ceres/random.h index 6b280f9ee64..0495d67581d 100644 --- a/extern/ceres/internal/ceres/random.h +++ b/extern/ceres/internal/ceres/random.h @@ -35,7 +35,7 @@ #include <cmath> #include <cstdlib> -#include "ceres/internal/port.h" +#include "ceres/internal/export.h" namespace ceres { @@ -50,7 +50,7 @@ inline int Uniform(int n) { } inline double RandDouble() { - double r = static_cast<double>(rand()); + auto r = static_cast<double>(rand()); return r / RAND_MAX; } diff --git a/extern/ceres/internal/ceres/reorder_program.cc b/extern/ceres/internal/ceres/reorder_program.cc index 5d802365f33..d552ebf3de3 100644 --- a/extern/ceres/internal/ceres/reorder_program.cc +++ b/extern/ceres/internal/ceres/reorder_program.cc @@ -1,5 +1,5 @@ // Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2015 Google Inc. All rights reserved. +// Copyright 2022 Google Inc. All rights reserved. // http://ceres-solver.org/ // // Redistribution and use in source and binary forms, with or without @@ -37,7 +37,8 @@ #include "Eigen/SparseCore" #include "ceres/cxsparse.h" -#include "ceres/internal/port.h" +#include "ceres/internal/config.h" +#include "ceres/internal/export.h" #include "ceres/ordered_groups.h" #include "ceres/parameter_block.h" #include "ceres/parameter_block_ordering.h" @@ -88,8 +89,8 @@ static int MinParameterBlock(const ResidualBlock* residual_block, #if defined(CERES_USE_EIGEN_SPARSE) Eigen::SparseMatrix<int> CreateBlockJacobian( const TripletSparseMatrix& block_jacobian_transpose) { - typedef Eigen::SparseMatrix<int> SparseMatrix; - typedef Eigen::Triplet<int> Triplet; + using SparseMatrix = Eigen::SparseMatrix<int>; + using Triplet = Eigen::Triplet<int>; const int* rows = block_jacobian_transpose.rows(); const int* cols = block_jacobian_transpose.cols(); @@ -97,7 +98,7 @@ Eigen::SparseMatrix<int> CreateBlockJacobian( vector<Triplet> triplets; triplets.reserve(num_nonzeros); for (int i = 0; i < num_nonzeros; ++i) { - triplets.push_back(Triplet(cols[i], rows[i], 1)); + triplets.emplace_back(cols[i], rows[i], 1); } SparseMatrix block_jacobian(block_jacobian_transpose.num_cols(), @@ -127,9 +128,9 @@ void OrderingForSparseNormalCholeskyUsingSuiteSparse( ss.ApproximateMinimumDegreeOrdering(block_jacobian_transpose, &ordering[0]); } else { vector<int> constraints; - for (int i = 0; i < parameter_blocks.size(); ++i) { + for (auto* parameter_block : parameter_blocks) { constraints.push_back(parameter_block_ordering.GroupId( - parameter_blocks[i]->mutable_user_state())); + parameter_block->mutable_user_state())); } // Renumber the entries of constraints to be contiguous integers @@ -188,7 +189,7 @@ void OrderingForSparseNormalCholeskyUsingEigenSparse( // things. The right thing to do here would be to get a compressed // row sparse matrix representation of the jacobian and go from // there. But that is a project for another day. - typedef Eigen::SparseMatrix<int> SparseMatrix; + using SparseMatrix = Eigen::SparseMatrix<int>; const SparseMatrix block_jacobian = CreateBlockJacobian(tsm_block_jacobian_transpose); @@ -279,7 +280,7 @@ bool LexicographicallyOrderResidualBlocks( CHECK(find(residual_blocks_per_e_block.begin(), residual_blocks_per_e_block.end() - 1, - 0) != residual_blocks_per_e_block.end()) + 0) == residual_blocks_per_e_block.end() - 1) << "Congratulations, you found a Ceres bug! Please report this error " << "to the developers."; @@ -291,7 +292,7 @@ bool LexicographicallyOrderResidualBlocks( // filling is finished, the offset pointerts should have shifted down one // entry (this is verified below). vector<ResidualBlock*> reordered_residual_blocks( - (*residual_blocks).size(), static_cast<ResidualBlock*>(NULL)); + (*residual_blocks).size(), static_cast<ResidualBlock*>(nullptr)); for (int i = 0; i < residual_blocks->size(); ++i) { int bucket = min_position_per_residual[i]; @@ -299,7 +300,7 @@ bool LexicographicallyOrderResidualBlocks( offsets[bucket]--; // Sanity. - CHECK(reordered_residual_blocks[offsets[bucket]] == NULL) + CHECK(reordered_residual_blocks[offsets[bucket]] == nullptr) << "Congratulations, you found a Ceres bug! Please report this error " << "to the developers."; @@ -313,9 +314,9 @@ bool LexicographicallyOrderResidualBlocks( << "Congratulations, you found a Ceres bug! Please report this error " << "to the developers."; } - // Sanity check #2: No NULL's left behind. - for (int i = 0; i < reordered_residual_blocks.size(); ++i) { - CHECK(reordered_residual_blocks[i] != NULL) + // Sanity check #2: No nullptr's left behind. + for (auto* residual_block : reordered_residual_blocks) { + CHECK(residual_block != nullptr) << "Congratulations, you found a Ceres bug! Please report this error " << "to the developers."; } @@ -339,9 +340,9 @@ static void MaybeReorderSchurComplementColumnsUsingSuiteSparse( vector<ParameterBlock*>& parameter_blocks = *(program->mutable_parameter_blocks()); - for (int i = 0; i < parameter_blocks.size(); ++i) { + for (auto* parameter_block : parameter_blocks) { constraints.push_back(parameter_block_ordering.GroupId( - parameter_blocks[i]->mutable_user_state())); + parameter_block->mutable_user_state())); } // Renumber the entries of constraints to be contiguous integers as @@ -378,7 +379,7 @@ static void MaybeReorderSchurComplementColumnsUsingEigen( std::unique_ptr<TripletSparseMatrix> tsm_block_jacobian_transpose( program->CreateJacobianBlockSparsityTranspose()); - typedef Eigen::SparseMatrix<int> SparseMatrix; + using SparseMatrix = Eigen::SparseMatrix<int>; const SparseMatrix block_jacobian = CreateBlockJacobian(*tsm_block_jacobian_transpose); const int num_rows = block_jacobian.rows(); @@ -441,7 +442,7 @@ bool ReorderProgramForSchurTypeLinearSolver( if (parameter_block_ordering->NumGroups() == 1) { // If the user supplied an parameter_block_ordering with just one - // group, it is equivalent to the user supplying NULL as an + // group, it is equivalent to the user supplying nullptr as an // parameter_block_ordering. Ceres is completely free to choose the // parameter block ordering as it sees fit. For Schur type solvers, // this means that the user wishes for Ceres to identify the diff --git a/extern/ceres/internal/ceres/reorder_program.h b/extern/ceres/internal/ceres/reorder_program.h index 2e0c3264377..fbc49231c33 100644 --- a/extern/ceres/internal/ceres/reorder_program.h +++ b/extern/ceres/internal/ceres/reorder_program.h @@ -33,7 +33,8 @@ #include <string> -#include "ceres/internal/port.h" +#include "ceres/internal/disable_warnings.h" +#include "ceres/internal/export.h" #include "ceres/parameter_block_ordering.h" #include "ceres/problem_impl.h" #include "ceres/types.h" @@ -44,7 +45,7 @@ namespace internal { class Program; // Reorder the parameter blocks in program using the ordering -CERES_EXPORT_INTERNAL bool ApplyOrdering( +CERES_NO_EXPORT bool ApplyOrdering( const ProblemImpl::ParameterMap& parameter_map, const ParameterBlockOrdering& ordering, Program* program, @@ -53,7 +54,7 @@ CERES_EXPORT_INTERNAL bool ApplyOrdering( // Reorder the residuals for program, if necessary, so that the residuals // involving each E block occur together. This is a necessary condition for the // Schur eliminator, which works on these "row blocks" in the jacobian. -CERES_EXPORT_INTERNAL bool LexicographicallyOrderResidualBlocks( +CERES_NO_EXPORT bool LexicographicallyOrderResidualBlocks( int size_of_first_elimination_group, Program* program, std::string* error); // Schur type solvers require that all parameter blocks eliminated @@ -72,7 +73,7 @@ CERES_EXPORT_INTERNAL bool LexicographicallyOrderResidualBlocks( // // Upon return, ordering contains the parameter block ordering that // was used to order the program. -CERES_EXPORT_INTERNAL bool ReorderProgramForSchurTypeLinearSolver( +CERES_NO_EXPORT bool ReorderProgramForSchurTypeLinearSolver( LinearSolverType linear_solver_type, SparseLinearAlgebraLibraryType sparse_linear_algebra_library_type, const ProblemImpl::ParameterMap& parameter_map, @@ -90,7 +91,7 @@ CERES_EXPORT_INTERNAL bool ReorderProgramForSchurTypeLinearSolver( // fill-reducing ordering is available in the sparse linear algebra // library (SuiteSparse version >= 4.2.0) then the fill reducing // ordering will take it into account, otherwise it will be ignored. -CERES_EXPORT_INTERNAL bool ReorderProgramForSparseCholesky( +CERES_NO_EXPORT bool ReorderProgramForSparseCholesky( SparseLinearAlgebraLibraryType sparse_linear_algebra_library_type, const ParameterBlockOrdering& parameter_block_ordering, int start_row_block, @@ -107,11 +108,13 @@ CERES_EXPORT_INTERNAL bool ReorderProgramForSparseCholesky( // bottom_residual_blocks.size() because we allow // bottom_residual_blocks to contain residual blocks not present in // the Program. -CERES_EXPORT_INTERNAL int ReorderResidualBlocksByPartition( +CERES_NO_EXPORT int ReorderResidualBlocksByPartition( const std::unordered_set<ResidualBlockId>& bottom_residual_blocks, Program* program); } // namespace internal } // namespace ceres -#endif // CERES_INTERNAL_REORDER_PROGRAM_ +#include "ceres/internal/reenable_warnings.h" + +#endif // CERES_INTERNAL_REORDER_PROGRAM_H_ diff --git a/extern/ceres/internal/ceres/residual_block.cc b/extern/ceres/internal/ceres/residual_block.cc index 067c9efe83d..cd408f2f98e 100644 --- a/extern/ceres/internal/ceres/residual_block.cc +++ b/extern/ceres/internal/ceres/residual_block.cc @@ -39,8 +39,8 @@ #include "ceres/cost_function.h" #include "ceres/internal/eigen.h" #include "ceres/internal/fixed_array.h" -#include "ceres/local_parameterization.h" #include "ceres/loss_function.h" +#include "ceres/manifold.h" #include "ceres/parameter_block.h" #include "ceres/residual_block_utils.h" #include "ceres/small_blas.h" @@ -87,7 +87,7 @@ bool ResidualBlock::Evaluate(const bool apply_loss_function, for (int i = 0; i < num_parameter_blocks; ++i) { const ParameterBlock* parameter_block = parameter_blocks_[i]; if (jacobians[i] != nullptr && - parameter_block->LocalParameterizationJacobian() != nullptr) { + parameter_block->PlusJacobian() != nullptr) { global_jacobians[i] = scratch; scratch += num_residuals * parameter_block->Size(); } else { @@ -132,27 +132,27 @@ bool ResidualBlock::Evaluate(const bool apply_loss_function, double squared_norm = VectorRef(residuals, num_residuals).squaredNorm(); - // Update the jacobians with the local parameterizations. + // Update the plus_jacobian for the manifolds. if (jacobians != nullptr) { for (int i = 0; i < num_parameter_blocks; ++i) { if (jacobians[i] != nullptr) { const ParameterBlock* parameter_block = parameter_blocks_[i]; - // Apply local reparameterization to the jacobians. - if (parameter_block->LocalParameterizationJacobian() != nullptr) { + // Apply the Manifold::PlusJacobian to the ambient jacobians. + if (parameter_block->PlusJacobian() != nullptr) { // jacobians[i] = global_jacobians[i] * global_to_local_jacobian. MatrixMatrixMultiply<Dynamic, Dynamic, Dynamic, Dynamic, 0>( global_jacobians[i], num_residuals, parameter_block->Size(), - parameter_block->LocalParameterizationJacobian(), + parameter_block->PlusJacobian(), parameter_block->Size(), - parameter_block->LocalSize(), + parameter_block->TangentSize(), jacobians[i], 0, 0, num_residuals, - parameter_block->LocalSize()); + parameter_block->TangentSize()); } } } @@ -183,7 +183,7 @@ bool ResidualBlock::Evaluate(const bool apply_loss_function, // Correct the jacobians for the loss function. correct.CorrectJacobian(num_residuals, - parameter_block->LocalSize(), + parameter_block->TangentSize(), residuals, jacobians[i]); } @@ -199,16 +199,16 @@ bool ResidualBlock::Evaluate(const bool apply_loss_function, int ResidualBlock::NumScratchDoublesForEvaluate() const { // Compute the amount of scratch space needed to store the full-sized - // jacobians. For parameters that have no local parameterization no storage - // is needed and the passed-in jacobian array is used directly. Also include - // space to store the residuals, which is needed for cost-only evaluations. - // This is slightly pessimistic, since both won't be needed all the time, but - // the amount of excess should not cause problems for the caller. + // jacobians. For parameters that have no manifold no storage is needed and + // the passed-in jacobian array is used directly. Also include space to store + // the residuals, which is needed for cost-only evaluations. This is slightly + // pessimistic, since both won't be needed all the time, but the amount of + // excess should not cause problems for the caller. int num_parameters = NumParameterBlocks(); int scratch_doubles = 1; for (int i = 0; i < num_parameters; ++i) { const ParameterBlock* parameter_block = parameter_blocks_[i]; - if (parameter_block->LocalParameterizationJacobian() != nullptr) { + if (parameter_block->PlusJacobian() != nullptr) { scratch_doubles += parameter_block->Size(); } } diff --git a/extern/ceres/internal/ceres/residual_block.h b/extern/ceres/internal/ceres/residual_block.h index f28fd42857c..978b94640fe 100644 --- a/extern/ceres/internal/ceres/residual_block.h +++ b/extern/ceres/internal/ceres/residual_block.h @@ -40,7 +40,8 @@ #include <vector> #include "ceres/cost_function.h" -#include "ceres/internal/port.h" +#include "ceres/internal/disable_warnings.h" +#include "ceres/internal/export.h" #include "ceres/stringprintf.h" #include "ceres/types.h" @@ -65,7 +66,7 @@ class ParameterBlock; // // The residual block stores pointers to but does not own the cost functions, // loss functions, and parameter blocks. -class CERES_EXPORT_INTERNAL ResidualBlock { +class CERES_NO_EXPORT ResidualBlock { public: // Construct the residual block with the given cost/loss functions. Loss may // be null. The index is the index of the residual block in the Program's @@ -77,10 +78,10 @@ class CERES_EXPORT_INTERNAL ResidualBlock { // Evaluates the residual term, 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 NULL, the - // residuals are not computed. If jacobians is NULL, no jacobians are - // computed. If jacobians[i] is NULL, then the jacobian for that parameter is - // not computed. + // 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 + // is not computed. // // cost must not be null. // @@ -92,10 +93,10 @@ class CERES_EXPORT_INTERNAL ResidualBlock { // 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 + // projection applied already; for example, the jacobian for a 4-dimensional + // quaternion parameter using the "Quaternion" manifold 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. @@ -147,4 +148,6 @@ class CERES_EXPORT_INTERNAL ResidualBlock { } // namespace internal } // namespace ceres +#include "ceres/internal/reenable_warnings.h" + #endif // CERES_INTERNAL_RESIDUAL_BLOCK_H_ diff --git a/extern/ceres/internal/ceres/residual_block_utils.cc b/extern/ceres/internal/ceres/residual_block_utils.cc index d5b3fa1fa37..11c7623ce22 100644 --- a/extern/ceres/internal/ceres/residual_block_utils.cc +++ b/extern/ceres/internal/ceres/residual_block_utils.cc @@ -36,7 +36,7 @@ #include "ceres/array_utils.h" #include "ceres/internal/eigen.h" -#include "ceres/internal/port.h" +#include "ceres/internal/export.h" #include "ceres/parameter_block.h" #include "ceres/residual_block.h" #include "ceres/stringprintf.h" @@ -56,7 +56,7 @@ void InvalidateEvaluation(const ResidualBlock& block, InvalidateArray(1, cost); InvalidateArray(num_residuals, residuals); - if (jacobians != NULL) { + if (jacobians != nullptr) { for (int i = 0; i < num_parameter_blocks; ++i) { const int parameter_block_size = block.parameter_blocks()[i]->Size(); InvalidateArray(num_residuals * parameter_block_size, jacobians[i]); @@ -104,9 +104,9 @@ string EvaluationToString(const ResidualBlock& block, StringAppendF(&result, "| "); for (int k = 0; k < num_residuals; ++k) { AppendArrayToString(1, - (jacobians != NULL && jacobians[i] != NULL) + (jacobians != nullptr && jacobians[i] != nullptr) ? jacobians[i] + k * parameter_block_size + j - : NULL, + : nullptr, &result); } StringAppendF(&result, "\n"); @@ -129,7 +129,7 @@ bool IsEvaluationValid(const ResidualBlock& block, return false; } - if (jacobians != NULL) { + if (jacobians != nullptr) { for (int i = 0; i < num_parameter_blocks; ++i) { const int parameter_block_size = block.parameter_blocks()[i]->Size(); if (!IsArrayValid(num_residuals * parameter_block_size, jacobians[i])) { diff --git a/extern/ceres/internal/ceres/residual_block_utils.h b/extern/ceres/internal/ceres/residual_block_utils.h index 41ae81abc99..f75b6aecce9 100644 --- a/extern/ceres/internal/ceres/residual_block_utils.h +++ b/extern/ceres/internal/ceres/residual_block_utils.h @@ -45,14 +45,15 @@ #include <string> -#include "ceres/internal/port.h" +#include "ceres/internal/export.h" namespace ceres { namespace internal { class ResidualBlock; -// Invalidate cost, resdual and jacobian arrays (if not NULL). +// Invalidate cost, resdual and jacobian arrays (if not nullptr). +CERES_NO_EXPORT void InvalidateEvaluation(const ResidualBlock& block, double* cost, double* residuals, @@ -60,6 +61,7 @@ void InvalidateEvaluation(const ResidualBlock& block, // Check if any of the arrays cost, residuals or jacobians contains an // NaN, return true if it does. +CERES_NO_EXPORT bool IsEvaluationValid(const ResidualBlock& block, double const* const* parameters, double* cost, @@ -69,6 +71,7 @@ bool IsEvaluationValid(const ResidualBlock& block, // Create a string representation of the Residual block containing the // value of the parameters, residuals and jacobians if present. // Useful for debugging output. +CERES_NO_EXPORT std::string EvaluationToString(const ResidualBlock& block, double const* const* parameters, double* cost, diff --git a/extern/ceres/internal/ceres/schur_complement_solver.cc b/extern/ceres/internal/ceres/schur_complement_solver.cc index 65e7854f9e5..bb442b4280b 100644 --- a/extern/ceres/internal/ceres/schur_complement_solver.cc +++ b/extern/ceres/internal/ceres/schur_complement_solver.cc @@ -1,5 +1,5 @@ // Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2015 Google Inc. All rights reserved. +// Copyright 2022 Google Inc. All rights reserved. // http://ceres-solver.org/ // // Redistribution and use in source and binary forms, with or without @@ -46,7 +46,6 @@ #include "ceres/conjugate_gradients_solver.h" #include "ceres/detect_structure.h" #include "ceres/internal/eigen.h" -#include "ceres/lapack.h" #include "ceres/linear_solver.h" #include "ceres/sparse_cholesky.h" #include "ceres/triplet_sparse_matrix.h" @@ -63,14 +62,12 @@ using std::vector; namespace { -class BlockRandomAccessSparseMatrixAdapter : public LinearOperator { +class BlockRandomAccessSparseMatrixAdapter final : public LinearOperator { public: explicit BlockRandomAccessSparseMatrixAdapter( const BlockRandomAccessSparseMatrix& m) : m_(m) {} - virtual ~BlockRandomAccessSparseMatrixAdapter() {} - // y = y + Ax; void RightMultiply(const double* x, double* y) const final { m_.SymmetricRightMultiply(x, y); @@ -88,14 +85,12 @@ class BlockRandomAccessSparseMatrixAdapter : public LinearOperator { const BlockRandomAccessSparseMatrix& m_; }; -class BlockRandomAccessDiagonalMatrixAdapter : public LinearOperator { +class BlockRandomAccessDiagonalMatrixAdapter final : public LinearOperator { public: explicit BlockRandomAccessDiagonalMatrixAdapter( const BlockRandomAccessDiagonalMatrix& m) : m_(m) {} - virtual ~BlockRandomAccessDiagonalMatrixAdapter() {} - // y = y + Ax; void RightMultiply(const double* x, double* y) const final { m_.RightMultiply(x, y); @@ -115,6 +110,14 @@ class BlockRandomAccessDiagonalMatrixAdapter : public LinearOperator { } // namespace +SchurComplementSolver::SchurComplementSolver( + const LinearSolver::Options& options) + : options_(options) { + CHECK_GT(options.elimination_groups.size(), 1); + CHECK_GT(options.elimination_groups[0], 0); + CHECK(options.context != nullptr); +} + LinearSolver::Summary SchurComplementSolver::SolveImpl( BlockSparseMatrix* A, const double* b, @@ -123,7 +126,7 @@ LinearSolver::Summary SchurComplementSolver::SolveImpl( EventLogger event_logger("SchurComplementSolver::Solve"); const CompressedRowBlockStructure* bs = A->block_structure(); - if (eliminator_.get() == NULL) { + if (eliminator_.get() == nullptr) { const int num_eliminate_blocks = options_.elimination_groups[0]; const int num_f_blocks = bs->cols.size() - num_eliminate_blocks; @@ -141,9 +144,9 @@ LinearSolver::Summary SchurComplementSolver::SolveImpl( // mechanism that does not cause binary bloat. if (options_.row_block_size == 2 && options_.e_block_size == 3 && options_.f_block_size == 6 && num_f_blocks == 1) { - eliminator_.reset(new SchurEliminatorForOneFBlock<2, 3, 6>); + eliminator_ = std::make_unique<SchurEliminatorForOneFBlock<2, 3, 6>>(); } else { - eliminator_.reset(SchurEliminatorBase::Create(options_)); + eliminator_ = SchurEliminatorBase::Create(options_); } CHECK(eliminator_); @@ -174,6 +177,12 @@ LinearSolver::Summary SchurComplementSolver::SolveImpl( return summary; } +DenseSchurComplementSolver::DenseSchurComplementSolver( + const LinearSolver::Options& options) + : SchurComplementSolver(options), + cholesky_(DenseCholesky::Create(options)) {} + +DenseSchurComplementSolver::~DenseSchurComplementSolver() = default; // Initialize a BlockRandomAccessDenseMatrix to store the Schur // complement. @@ -187,8 +196,8 @@ void DenseSchurComplementSolver::InitStorage( blocks[j] = bs->cols[i].size; } - set_lhs(new BlockRandomAccessDenseMatrix(blocks)); - set_rhs(new double[lhs()->num_rows()]); + set_lhs(std::make_unique<BlockRandomAccessDenseMatrix>(blocks)); + set_rhs(std::make_unique<double[]>(lhs()->num_rows())); } // Solve the system Sx = r, assuming that the matrix S is stored in a @@ -201,8 +210,7 @@ LinearSolver::Summary DenseSchurComplementSolver::SolveReducedLinearSystem( summary.termination_type = LINEAR_SOLVER_SUCCESS; summary.message = "Success."; - const BlockRandomAccessDenseMatrix* m = - down_cast<const BlockRandomAccessDenseMatrix*>(lhs()); + auto* m = down_cast<BlockRandomAccessDenseMatrix*>(mutable_lhs()); const int num_rows = m->num_rows(); // The case where there are no f blocks, and the system is block @@ -212,26 +220,8 @@ LinearSolver::Summary DenseSchurComplementSolver::SolveReducedLinearSystem( } summary.num_iterations = 1; - - if (options().dense_linear_algebra_library_type == EIGEN) { - Eigen::LLT<Matrix, Eigen::Upper> llt = - ConstMatrixRef(m->values(), num_rows, num_rows) - .selfadjointView<Eigen::Upper>() - .llt(); - if (llt.info() != Eigen::Success) { - summary.termination_type = LINEAR_SOLVER_FAILURE; - summary.message = - "Eigen failure. Unable to perform dense Cholesky factorization."; - return summary; - } - - VectorRef(solution, num_rows) = llt.solve(ConstVectorRef(rhs(), num_rows)); - } else { - VectorRef(solution, num_rows) = ConstVectorRef(rhs(), num_rows); - summary.termination_type = LAPACK::SolveInPlaceUsingCholesky( - num_rows, m->values(), solution, &summary.message); - } - + summary.termination_type = cholesky_->FactorAndSolve( + num_rows, m->mutable_values(), rhs(), solution, &summary.message); return summary; } @@ -243,7 +233,7 @@ SparseSchurComplementSolver::SparseSchurComplementSolver( } } -SparseSchurComplementSolver::~SparseSchurComplementSolver() {} +SparseSchurComplementSolver::~SparseSchurComplementSolver() = default; // Determine the non-zero blocks in the Schur Complement matrix, and // initialize a BlockRandomAccessSparseMatrix object. @@ -303,8 +293,8 @@ void SparseSchurComplementSolver::InitStorage( CHECK_GE(row.cells.front().block_id, num_eliminate_blocks); for (int i = 0; i < row.cells.size(); ++i) { int r_block1_id = row.cells[i].block_id - num_eliminate_blocks; - for (int j = 0; j < row.cells.size(); ++j) { - int r_block2_id = row.cells[j].block_id - num_eliminate_blocks; + for (const auto& cell : row.cells) { + int r_block2_id = cell.block_id - num_eliminate_blocks; if (r_block1_id <= r_block2_id) { block_pairs.insert(make_pair(r_block1_id, r_block2_id)); } @@ -312,8 +302,9 @@ void SparseSchurComplementSolver::InitStorage( } } - set_lhs(new BlockRandomAccessSparseMatrix(blocks_, block_pairs)); - set_rhs(new double[lhs()->num_rows()]); + set_lhs( + std::make_unique<BlockRandomAccessSparseMatrix>(blocks_, block_pairs)); + set_rhs(std::make_unique<double[]>(lhs()->num_rows())); } LinearSolver::Summary SparseSchurComplementSolver::SolveReducedLinearSystem( @@ -338,11 +329,10 @@ LinearSolver::Summary SparseSchurComplementSolver::SolveReducedLinearSystem( const CompressedRowSparseMatrix::StorageType storage_type = sparse_cholesky_->StorageType(); if (storage_type == CompressedRowSparseMatrix::UPPER_TRIANGULAR) { - lhs.reset(CompressedRowSparseMatrix::FromTripletSparseMatrix(*tsm)); + lhs = CompressedRowSparseMatrix::FromTripletSparseMatrix(*tsm); lhs->set_storage_type(CompressedRowSparseMatrix::UPPER_TRIANGULAR); } else { - lhs.reset( - CompressedRowSparseMatrix::FromTripletSparseMatrixTransposed(*tsm)); + lhs = CompressedRowSparseMatrix::FromTripletSparseMatrixTransposed(*tsm); lhs->set_storage_type(CompressedRowSparseMatrix::LOWER_TRIANGULAR); } @@ -373,12 +363,12 @@ SparseSchurComplementSolver::SolveReducedLinearSystemUsingConjugateGradients( // Only SCHUR_JACOBI is supported over here right now. CHECK_EQ(options().preconditioner_type, SCHUR_JACOBI); - if (preconditioner_.get() == NULL) { - preconditioner_.reset(new BlockRandomAccessDiagonalMatrix(blocks_)); + if (preconditioner_.get() == nullptr) { + preconditioner_ = + std::make_unique<BlockRandomAccessDiagonalMatrix>(blocks_); } - BlockRandomAccessSparseMatrix* sc = down_cast<BlockRandomAccessSparseMatrix*>( - const_cast<BlockRandomAccessMatrix*>(lhs())); + auto* sc = down_cast<BlockRandomAccessSparseMatrix*>(mutable_lhs()); // Extract block diagonal from the Schur complement to construct the // schur_jacobi preconditioner. @@ -404,10 +394,11 @@ SparseSchurComplementSolver::SolveReducedLinearSystemUsingConjugateGradients( VectorRef(solution, num_rows).setZero(); - std::unique_ptr<LinearOperator> lhs_adapter( - new BlockRandomAccessSparseMatrixAdapter(*sc)); - std::unique_ptr<LinearOperator> preconditioner_adapter( - new BlockRandomAccessDiagonalMatrixAdapter(*preconditioner_)); + std::unique_ptr<LinearOperator> lhs_adapter = + std::make_unique<BlockRandomAccessSparseMatrixAdapter>(*sc); + std::unique_ptr<LinearOperator> preconditioner_adapter = + std::make_unique<BlockRandomAccessDiagonalMatrixAdapter>( + *preconditioner_); LinearSolver::Options cg_options; cg_options.min_num_iterations = options().min_num_iterations; diff --git a/extern/ceres/internal/ceres/schur_complement_solver.h b/extern/ceres/internal/ceres/schur_complement_solver.h index 3bfa22f22e4..859a086cdf4 100644 --- a/extern/ceres/internal/ceres/schur_complement_solver.h +++ b/extern/ceres/internal/ceres/schur_complement_solver.h @@ -40,7 +40,9 @@ #include "ceres/block_random_access_matrix.h" #include "ceres/block_sparse_matrix.h" #include "ceres/block_structure.h" -#include "ceres/internal/port.h" +#include "ceres/dense_cholesky.h" +#include "ceres/internal/config.h" +#include "ceres/internal/export.h" #include "ceres/linear_solver.h" #include "ceres/schur_eliminator.h" #include "ceres/types.h" @@ -50,6 +52,8 @@ #include "Eigen/SparseCholesky" #endif +#include "ceres/internal/disable_warnings.h" + namespace ceres { namespace internal { @@ -107,20 +111,12 @@ class SparseCholesky; // set to DENSE_SCHUR and SPARSE_SCHUR // respectively. LinearSolver::Options::elimination_groups[0] should // be at least 1. -class CERES_EXPORT_INTERNAL SchurComplementSolver - : public BlockSparseMatrixSolver { +class CERES_NO_EXPORT SchurComplementSolver : public BlockSparseMatrixSolver { public: - explicit SchurComplementSolver(const LinearSolver::Options& options) - : options_(options) { - CHECK_GT(options.elimination_groups.size(), 1); - CHECK_GT(options.elimination_groups[0], 0); - CHECK(options.context != NULL); - } + explicit SchurComplementSolver(const LinearSolver::Options& options); SchurComplementSolver(const SchurComplementSolver&) = delete; void operator=(const SchurComplementSolver&) = delete; - // LinearSolver methods - virtual ~SchurComplementSolver() {} LinearSolver::Summary SolveImpl( BlockSparseMatrix* A, const double* b, @@ -130,10 +126,14 @@ class CERES_EXPORT_INTERNAL SchurComplementSolver protected: const LinearSolver::Options& options() const { return options_; } + void set_lhs(std::unique_ptr<BlockRandomAccessMatrix> lhs) { + lhs_ = std::move(lhs); + } const BlockRandomAccessMatrix* lhs() const { return lhs_.get(); } - void set_lhs(BlockRandomAccessMatrix* lhs) { lhs_.reset(lhs); } + BlockRandomAccessMatrix* mutable_lhs() { return lhs_.get(); } + + void set_rhs(std::unique_ptr<double[]> rhs) { rhs_ = std::move(rhs); } const double* rhs() const { return rhs_.get(); } - void set_rhs(double* rhs) { rhs_.reset(rhs); } private: virtual void InitStorage(const CompressedRowBlockStructure* bs) = 0; @@ -149,30 +149,33 @@ class CERES_EXPORT_INTERNAL SchurComplementSolver }; // Dense Cholesky factorization based solver. -class DenseSchurComplementSolver : public SchurComplementSolver { +class CERES_NO_EXPORT DenseSchurComplementSolver final + : public SchurComplementSolver { public: - explicit DenseSchurComplementSolver(const LinearSolver::Options& options) - : SchurComplementSolver(options) {} + explicit DenseSchurComplementSolver(const LinearSolver::Options& options); DenseSchurComplementSolver(const DenseSchurComplementSolver&) = delete; void operator=(const DenseSchurComplementSolver&) = delete; - virtual ~DenseSchurComplementSolver() {} + ~DenseSchurComplementSolver() override; private: void InitStorage(const CompressedRowBlockStructure* bs) final; LinearSolver::Summary SolveReducedLinearSystem( const LinearSolver::PerSolveOptions& per_solve_options, double* solution) final; + + std::unique_ptr<DenseCholesky> cholesky_; }; // Sparse Cholesky factorization based solver. -class SparseSchurComplementSolver : public SchurComplementSolver { +class CERES_NO_EXPORT SparseSchurComplementSolver final + : public SchurComplementSolver { public: explicit SparseSchurComplementSolver(const LinearSolver::Options& options); SparseSchurComplementSolver(const SparseSchurComplementSolver&) = delete; void operator=(const SparseSchurComplementSolver&) = delete; - virtual ~SparseSchurComplementSolver(); + ~SparseSchurComplementSolver() override; private: void InitStorage(const CompressedRowBlockStructure* bs) final; @@ -191,4 +194,6 @@ class SparseSchurComplementSolver : public SchurComplementSolver { } // namespace internal } // namespace ceres +#include "ceres/internal/reenable_warnings.h" + #endif // CERES_INTERNAL_SCHUR_COMPLEMENT_SOLVER_H_ diff --git a/extern/ceres/internal/ceres/schur_eliminator.cc b/extern/ceres/internal/ceres/schur_eliminator.cc index 613ae9558e9..22e7358070f 100644 --- a/extern/ceres/internal/ceres/schur_eliminator.cc +++ b/extern/ceres/internal/ceres/schur_eliminator.cc @@ -39,121 +39,126 @@ // // This file is generated using generate_template_specializations.py. +#include <memory> + #include "ceres/linear_solver.h" #include "ceres/schur_eliminator.h" namespace ceres { namespace internal { -SchurEliminatorBase* SchurEliminatorBase::Create( +SchurEliminatorBase::~SchurEliminatorBase() = default; + +std::unique_ptr<SchurEliminatorBase> SchurEliminatorBase::Create( const LinearSolver::Options& options) { #ifndef CERES_RESTRICT_SCHUR_SPECIALIZATION if ((options.row_block_size == 2) && (options.e_block_size == 2) && (options.f_block_size == 2)) { - return new SchurEliminator<2, 2, 2>(options); + return std::make_unique<SchurEliminator<2, 2, 2>>(options); } if ((options.row_block_size == 2) && (options.e_block_size == 2) && (options.f_block_size == 3)) { - return new SchurEliminator<2, 2, 3>(options); + return std::make_unique<SchurEliminator<2, 2, 3>>(options); } if ((options.row_block_size == 2) && (options.e_block_size == 2) && (options.f_block_size == 4)) { - return new SchurEliminator<2, 2, 4>(options); + return std::make_unique<SchurEliminator<2, 2, 4>>(options); } if ((options.row_block_size == 2) && (options.e_block_size == 2)) { - return new SchurEliminator<2, 2, Eigen::Dynamic>(options); + return std::make_unique<SchurEliminator<2, 2, Eigen::Dynamic>>(options); } if ((options.row_block_size == 2) && (options.e_block_size == 3) && (options.f_block_size == 3)) { - return new SchurEliminator<2, 3, 3>(options); + return std::make_unique<SchurEliminator<2, 3, 3>>(options); } if ((options.row_block_size == 2) && (options.e_block_size == 3) && (options.f_block_size == 4)) { - return new SchurEliminator<2, 3, 4>(options); + return std::make_unique<SchurEliminator<2, 3, 4>>(options); } if ((options.row_block_size == 2) && (options.e_block_size == 3) && (options.f_block_size == 6)) { - return new SchurEliminator<2, 3, 6>(options); + return std::make_unique<SchurEliminator<2, 3, 6>>(options); } if ((options.row_block_size == 2) && (options.e_block_size == 3) && (options.f_block_size == 9)) { - return new SchurEliminator<2, 3, 9>(options); + return std::make_unique<SchurEliminator<2, 3, 9>>(options); } if ((options.row_block_size == 2) && (options.e_block_size == 3)) { - return new SchurEliminator<2, 3, Eigen::Dynamic>(options); + return std::make_unique<SchurEliminator<2, 3, Eigen::Dynamic>>(options); } if ((options.row_block_size == 2) && (options.e_block_size == 4) && (options.f_block_size == 3)) { - return new SchurEliminator<2, 4, 3>(options); + return std::make_unique<SchurEliminator<2, 4, 3>>(options); } if ((options.row_block_size == 2) && (options.e_block_size == 4) && (options.f_block_size == 4)) { - return new SchurEliminator<2, 4, 4>(options); + return std::make_unique<SchurEliminator<2, 4, 4>>(options); } if ((options.row_block_size == 2) && (options.e_block_size == 4) && (options.f_block_size == 6)) { - return new SchurEliminator<2, 4, 6>(options); + return std::make_unique<SchurEliminator<2, 4, 6>>(options); } if ((options.row_block_size == 2) && (options.e_block_size == 4) && (options.f_block_size == 8)) { - return new SchurEliminator<2, 4, 8>(options); + return std::make_unique<SchurEliminator<2, 4, 8>>(options); } if ((options.row_block_size == 2) && (options.e_block_size == 4) && (options.f_block_size == 9)) { - return new SchurEliminator<2, 4, 9>(options); + return std::make_unique<SchurEliminator<2, 4, 9>>(options); } if ((options.row_block_size == 2) && (options.e_block_size == 4)) { - return new SchurEliminator<2, 4, Eigen::Dynamic>(options); + return std::make_unique<SchurEliminator<2, 4, Eigen::Dynamic>>(options); } if (options.row_block_size == 2) { - return new SchurEliminator<2, Eigen::Dynamic, Eigen::Dynamic>(options); + return std::make_unique<SchurEliminator<2, Eigen::Dynamic, Eigen::Dynamic>>(options); } if ((options.row_block_size == 3) && (options.e_block_size == 3) && (options.f_block_size == 3)) { - return new SchurEliminator<3, 3, 3>(options); + return std::make_unique<SchurEliminator<3, 3, 3>>(options); } if ((options.row_block_size == 4) && (options.e_block_size == 4) && (options.f_block_size == 2)) { - return new SchurEliminator<4, 4, 2>(options); + return std::make_unique<SchurEliminator<4, 4, 2>>(options); } if ((options.row_block_size == 4) && (options.e_block_size == 4) && (options.f_block_size == 3)) { - return new SchurEliminator<4, 4, 3>(options); + return std::make_unique<SchurEliminator<4, 4, 3>>(options); } if ((options.row_block_size == 4) && (options.e_block_size == 4) && (options.f_block_size == 4)) { - return new SchurEliminator<4, 4, 4>(options); + return std::make_unique<SchurEliminator<4, 4, 4>>(options); } if ((options.row_block_size == 4) && (options.e_block_size == 4)) { - return new SchurEliminator<4, 4, Eigen::Dynamic>(options); + return std::make_unique<SchurEliminator<4, 4, Eigen::Dynamic>>(options); } #endif VLOG(1) << "Template specializations not found for <" << options.row_block_size << "," << options.e_block_size << "," << options.f_block_size << ">"; - return new SchurEliminator<Eigen::Dynamic, Eigen::Dynamic, Eigen::Dynamic>( - options); + return std::make_unique<SchurEliminator<Eigen::Dynamic, + Eigen::Dynamic, + Eigen::Dynamic>>(options); } } // namespace internal diff --git a/extern/ceres/internal/ceres/schur_eliminator.h b/extern/ceres/internal/ceres/schur_eliminator.h index 42c016ee9b0..91831dceb5a 100644 --- a/extern/ceres/internal/ceres/schur_eliminator.h +++ b/extern/ceres/internal/ceres/schur_eliminator.h @@ -40,8 +40,10 @@ #include "ceres/block_random_access_matrix.h" #include "ceres/block_sparse_matrix.h" #include "ceres/block_structure.h" +#include "ceres/internal/config.h" +#include "ceres/internal/disable_warnings.h" #include "ceres/internal/eigen.h" -#include "ceres/internal/port.h" +#include "ceres/internal/export.h" #include "ceres/linear_solver.h" namespace ceres { @@ -56,9 +58,8 @@ namespace internal { // Where x = [y;z] is a partition of the variables. The partitioning // of the variables is such that, E'E is a block diagonal matrix. Or // in other words, the parameter blocks in E form an independent set -// of the of the graph implied by the block matrix A'A. Then, this -// class provides the functionality to compute the Schur complement -// system +// of the graph implied by the block matrix A'A. Then, this class +// provides the functionality to compute the Schur complement system // // S z = r // @@ -164,9 +165,9 @@ namespace internal { // 2008 for an example of such use]. // // Example usage: Please see schur_complement_solver.cc -class CERES_EXPORT_INTERNAL SchurEliminatorBase { +class CERES_NO_EXPORT SchurEliminatorBase { public: - virtual ~SchurEliminatorBase() {} + virtual ~SchurEliminatorBase(); // Initialize the eliminator. It is the user's responsibilty to call // this function before calling Eliminate or BackSubstitute. It is @@ -210,7 +211,8 @@ class CERES_EXPORT_INTERNAL SchurEliminatorBase { const double* z, double* y) = 0; // Factory - static SchurEliminatorBase* Create(const LinearSolver::Options& options); + static std::unique_ptr<SchurEliminatorBase> Create( + const LinearSolver::Options& options); }; // Templated implementation of the SchurEliminatorBase interface. The @@ -223,7 +225,7 @@ class CERES_EXPORT_INTERNAL SchurEliminatorBase { template <int kRowBlockSize = Eigen::Dynamic, int kEBlockSize = Eigen::Dynamic, int kFBlockSize = Eigen::Dynamic> -class SchurEliminator : public SchurEliminatorBase { +class CERES_NO_EXPORT SchurEliminator final : public SchurEliminatorBase { public: explicit SchurEliminator(const LinearSolver::Options& options) : num_threads_(options.num_threads), context_(options.context) { @@ -231,7 +233,7 @@ class SchurEliminator : public SchurEliminatorBase { } // SchurEliminatorBase Interface - virtual ~SchurEliminator(); + ~SchurEliminator() override; void Init(int num_eliminate_blocks, bool assume_full_rank_ete, const CompressedRowBlockStructure* bs) final; @@ -272,9 +274,9 @@ class SchurEliminator : public SchurEliminatorBase { // buffer_layout[z1] = 0 // buffer_layout[z5] = y1 * z1 // buffer_layout[z2] = y1 * z1 + y1 * z5 - typedef std::map<int, int> BufferLayoutType; + using BufferLayoutType = std::map<int, int>; struct Chunk { - Chunk() : size(0) {} + explicit Chunk(int start) : size(0), start(start) {} int size; int start; BufferLayoutType buffer_layout; @@ -378,9 +380,9 @@ class SchurEliminator : public SchurEliminatorBase { template <int kRowBlockSize = Eigen::Dynamic, int kEBlockSize = Eigen::Dynamic, int kFBlockSize = Eigen::Dynamic> -class SchurEliminatorForOneFBlock : public SchurEliminatorBase { +class CERES_NO_EXPORT SchurEliminatorForOneFBlock final + : public SchurEliminatorBase { public: - virtual ~SchurEliminatorForOneFBlock() {} void Init(int num_eliminate_blocks, bool assume_full_rank_ete, const CompressedRowBlockStructure* bs) override { @@ -484,7 +486,7 @@ class SchurEliminatorForOneFBlock : public SchurEliminatorBase { Eigen::Matrix<double, kFBlockSize, 1> f_t_b; // Add the square of the diagonal to e_t_e. - if (D != NULL) { + if (D != nullptr) { const typename EigenTypes<kEBlockSize>::ConstVectorRef diag( D + bs->cols[e_block_id].position, kEBlockSize); e_t_e = diag.array().square().matrix().asDiagonal(); @@ -624,4 +626,6 @@ class SchurEliminatorForOneFBlock : public SchurEliminatorBase { } // namespace internal } // namespace ceres +#include "ceres/internal/reenable_warnings.h" + #endif // CERES_INTERNAL_SCHUR_ELIMINATOR_H_ diff --git a/extern/ceres/internal/ceres/schur_eliminator_impl.h b/extern/ceres/internal/ceres/schur_eliminator_impl.h index 1f0b4fa481d..de3ba3e5dcb 100644 --- a/extern/ceres/internal/ceres/schur_eliminator_impl.h +++ b/extern/ceres/internal/ceres/schur_eliminator_impl.h @@ -47,7 +47,7 @@ // This include must come before any #ifndef check on Ceres compile options. // clang-format off -#include "ceres/internal/port.h" +#include "ceres/internal/config.h" // clang-format on #include <algorithm> @@ -125,10 +125,8 @@ void SchurEliminator<kRowBlockSize, kEBlockSize, kFBlockSize>::Init( break; } - chunks_.push_back(Chunk()); + chunks_.push_back(Chunk(r)); Chunk& chunk = chunks_.back(); - chunk.size = 0; - chunk.start = r; int buffer_size = 0; const int e_block_size = bs->cols[chunk_block_id].size; @@ -161,12 +159,13 @@ void SchurEliminator<kRowBlockSize, kEBlockSize, kFBlockSize>::Init( uneliminated_row_begins_ = chunk.start + chunk.size; - buffer_.reset(new double[buffer_size_ * num_threads_]); + buffer_ = std::make_unique<double[]>(buffer_size_ * num_threads_); // chunk_outer_product_buffer_ only needs to store e_block_size * // f_block_size, which is always less than buffer_size_, so we just // allocate buffer_size_ per thread. - chunk_outer_product_buffer_.reset(new double[buffer_size_ * num_threads_]); + chunk_outer_product_buffer_ = + std::make_unique<double[]>(buffer_size_ * num_threads_); STLDeleteElements(&rhs_locks_); rhs_locks_.resize(num_col_blocks - num_eliminate_blocks_); @@ -193,7 +192,7 @@ void SchurEliminator<kRowBlockSize, kEBlockSize, kFBlockSize>::Eliminate( const int num_col_blocks = bs->cols.size(); // Add the diagonal to the schur complement. - if (D != NULL) { + if (D != nullptr) { ParallelFor(context_, num_eliminate_blocks_, num_col_blocks, @@ -203,7 +202,7 @@ void SchurEliminator<kRowBlockSize, kEBlockSize, kFBlockSize>::Eliminate( int r, c, row_stride, col_stride; CellInfo* cell_info = lhs->GetCell( block_id, block_id, &r, &c, &row_stride, &col_stride); - if (cell_info != NULL) { + if (cell_info != nullptr) { const int block_size = bs->cols[i].size; typename EigenTypes<Eigen::Dynamic>::ConstVectorRef diag( D + bs->cols[i].position, block_size); @@ -245,7 +244,7 @@ void SchurEliminator<kRowBlockSize, kEBlockSize, kFBlockSize>::Eliminate( typename EigenTypes<kEBlockSize, kEBlockSize>::Matrix ete(e_block_size, e_block_size); - if (D != NULL) { + if (D != nullptr) { const typename EigenTypes<kEBlockSize>::ConstVectorRef diag( D + bs->cols[e_block_id].position, e_block_size); ete = diag.array().square().matrix().asDiagonal(); @@ -327,7 +326,7 @@ void SchurEliminator<kRowBlockSize, kEBlockSize, kFBlockSize>::BackSubstitute( typename EigenTypes<kEBlockSize, kEBlockSize>::Matrix ete(e_block_size, e_block_size); - if (D != NULL) { + if (D != nullptr) { const typename EigenTypes<kEBlockSize>::ConstVectorRef diag( D + bs->cols[e_block_id].position, e_block_size); ete = diag.array().square().matrix().asDiagonal(); @@ -525,7 +524,7 @@ void SchurEliminator<kRowBlockSize, kEBlockSize, kFBlockSize>:: // computation of the right-hand matrix product, but memory // references to the left hand side. const int e_block_size = inverse_ete.rows(); - BufferLayoutType::const_iterator it1 = buffer_layout.begin(); + auto it1 = buffer_layout.begin(); double* b1_transpose_inverse_ete = chunk_outer_product_buffer_.get() + thread_id * buffer_size_; @@ -542,14 +541,14 @@ void SchurEliminator<kRowBlockSize, kEBlockSize, kFBlockSize>:: b1_transpose_inverse_ete, 0, 0, block1_size, e_block_size); // clang-format on - BufferLayoutType::const_iterator it2 = it1; + auto it2 = it1; for (; it2 != buffer_layout.end(); ++it2) { const int block2 = it2->first - num_eliminate_blocks_; int r, c, row_stride, col_stride; CellInfo* cell_info = lhs->GetCell(block1, block2, &r, &c, &row_stride, &col_stride); - if (cell_info != NULL) { + if (cell_info != nullptr) { const int block2_size = bs->cols[it2->first].size; std::lock_guard<std::mutex> l(cell_info->m); // clang-format off @@ -627,7 +626,7 @@ void SchurEliminator<kRowBlockSize, kEBlockSize, kFBlockSize>:: int r, c, row_stride, col_stride; CellInfo* cell_info = lhs->GetCell(block1, block1, &r, &c, &row_stride, &col_stride); - if (cell_info != NULL) { + if (cell_info != nullptr) { std::lock_guard<std::mutex> l(cell_info->m); // This multiply currently ignores the fact that this is a // symmetric outer product. @@ -647,7 +646,7 @@ void SchurEliminator<kRowBlockSize, kEBlockSize, kFBlockSize>:: int r, c, row_stride, col_stride; CellInfo* cell_info = lhs->GetCell(block1, block2, &r, &c, &row_stride, &col_stride); - if (cell_info != NULL) { + if (cell_info != nullptr) { const int block2_size = bs->cols[row.cells[j].block_id].size; std::lock_guard<std::mutex> l(cell_info->m); // clang-format off @@ -682,7 +681,7 @@ void SchurEliminator<kRowBlockSize, kEBlockSize, kFBlockSize>:: int r, c, row_stride, col_stride; CellInfo* cell_info = lhs->GetCell(block1, block1, &r, &c, &row_stride, &col_stride); - if (cell_info != NULL) { + if (cell_info != nullptr) { std::lock_guard<std::mutex> l(cell_info->m); // block += b1.transpose() * b1; // clang-format off @@ -702,7 +701,7 @@ void SchurEliminator<kRowBlockSize, kEBlockSize, kFBlockSize>:: int r, c, row_stride, col_stride; CellInfo* cell_info = lhs->GetCell(block1, block2, &r, &c, &row_stride, &col_stride); - if (cell_info != NULL) { + if (cell_info != nullptr) { // block += b1.transpose() * b2; std::lock_guard<std::mutex> l(cell_info->m); // clang-format off diff --git a/extern/ceres/internal/ceres/schur_eliminator_template.py b/extern/ceres/internal/ceres/schur_eliminator_template.py deleted file mode 100644 index 50515956e89..00000000000 --- a/extern/ceres/internal/ceres/schur_eliminator_template.py +++ /dev/null @@ -1,151 +0,0 @@ -# Ceres Solver - A fast non-linear least squares minimizer -# Copyright 2017 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) -# -# Script for explicitly generating template specialization of the -# SchurEliminator class. It is a rather large class -# and the number of explicit instantiations is also large. Explicitly -# generating these instantiations in separate .cc files breaks the -# compilation into separate compilation unit rather than one large cc -# file which takes 2+GB of RAM to compile. -# -# This script creates two sets of files. -# -# 1. schur_eliminator_x_x_x.cc -# where, the x indicates the template parameters and -# -# 2. schur_eliminator.cc -# -# that contains a factory function for instantiating these classes -# based on runtime parameters. -# -# The list of tuples, specializations indicates the set of -# specializations that is generated. - -# Set of template specializations to generate - -HEADER = """// Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2017 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) -// -// Template specialization of SchurEliminator. -// -// ======================================== -// THIS FILE IS AUTOGENERATED. DO NOT EDIT. -// THIS FILE IS AUTOGENERATED. DO NOT EDIT. -// THIS FILE IS AUTOGENERATED. DO NOT EDIT. -// THIS FILE IS AUTOGENERATED. DO NOT EDIT. -//========================================= -// -// This file is generated using generate_template_specializations.py. -""" - -DYNAMIC_FILE = """ -#include "ceres/schur_eliminator_impl.h" - -namespace ceres { -namespace internal { - -template class SchurEliminator<%s, %s, %s>; - -} // namespace internal -} // namespace ceres -""" - -SPECIALIZATION_FILE = """ -// This include must come before any #ifndef check on Ceres compile options. -#include "ceres/internal/port.h" - -#ifndef CERES_RESTRICT_SCHUR_SPECIALIZATION - -#include "ceres/schur_eliminator_impl.h" - -namespace ceres { -namespace internal { - -template class SchurEliminator<%s, %s, %s>; - -} // namespace internal -} // namespace ceres - -#endif // CERES_RESTRICT_SCHUR_SPECIALIZATION -""" - -FACTORY_FILE_HEADER = """ -#include "ceres/linear_solver.h" -#include "ceres/schur_eliminator.h" - -namespace ceres { -namespace internal { - -SchurEliminatorBase* SchurEliminatorBase::Create( - const LinearSolver::Options& options) { -#ifndef CERES_RESTRICT_SCHUR_SPECIALIZATION -""" - -FACTORY = """ return new SchurEliminator<%s, %s, %s>(options);""" - -FACTORY_FOOTER = """ -#endif - VLOG(1) << "Template specializations not found for <" - << options.row_block_size << "," << options.e_block_size << "," - << options.f_block_size << ">"; - return new SchurEliminator<Eigen::Dynamic, Eigen::Dynamic, Eigen::Dynamic>( - options); -} - -} // namespace internal -} // namespace ceres -""" diff --git a/extern/ceres/internal/ceres/schur_jacobi_preconditioner.cc b/extern/ceres/internal/ceres/schur_jacobi_preconditioner.cc index 89d770b405a..3ecec728262 100644 --- a/extern/ceres/internal/ceres/schur_jacobi_preconditioner.cc +++ b/extern/ceres/internal/ceres/schur_jacobi_preconditioner.cc @@ -43,26 +43,25 @@ namespace ceres { namespace internal { SchurJacobiPreconditioner::SchurJacobiPreconditioner( - const CompressedRowBlockStructure& bs, - const Preconditioner::Options& options) - : options_(options) { + const CompressedRowBlockStructure& bs, Preconditioner::Options options) + : options_(std::move(options)) { CHECK_GT(options_.elimination_groups.size(), 1); CHECK_GT(options_.elimination_groups[0], 0); const int num_blocks = bs.cols.size() - options_.elimination_groups[0]; CHECK_GT(num_blocks, 0) << "Jacobian should have at least 1 f_block for " << "SCHUR_JACOBI preconditioner."; - CHECK(options_.context != NULL); + CHECK(options_.context != nullptr); std::vector<int> blocks(num_blocks); for (int i = 0; i < num_blocks; ++i) { blocks[i] = bs.cols[i + options_.elimination_groups[0]].size; } - m_.reset(new BlockRandomAccessDiagonalMatrix(blocks)); + m_ = std::make_unique<BlockRandomAccessDiagonalMatrix>(blocks); InitEliminator(bs); } -SchurJacobiPreconditioner::~SchurJacobiPreconditioner() {} +SchurJacobiPreconditioner::~SchurJacobiPreconditioner() = default; // Initialize the SchurEliminator. void SchurJacobiPreconditioner::InitEliminator( @@ -74,7 +73,7 @@ void SchurJacobiPreconditioner::InitEliminator( eliminator_options.f_block_size = options_.f_block_size; eliminator_options.row_block_size = options_.row_block_size; eliminator_options.context = options_.context; - eliminator_.reset(SchurEliminatorBase::Create(eliminator_options)); + eliminator_ = SchurEliminatorBase::Create(eliminator_options); const bool kFullRankETE = true; eliminator_->Init( eliminator_options.elimination_groups[0], kFullRankETE, &bs); diff --git a/extern/ceres/internal/ceres/schur_jacobi_preconditioner.h b/extern/ceres/internal/ceres/schur_jacobi_preconditioner.h index 372b790b82f..a43bc3388a1 100644 --- a/extern/ceres/internal/ceres/schur_jacobi_preconditioner.h +++ b/extern/ceres/internal/ceres/schur_jacobi_preconditioner.h @@ -43,6 +43,8 @@ #include <utility> #include <vector> +#include "ceres/internal/disable_warnings.h" +#include "ceres/internal/export.h" #include "ceres/preconditioner.h" namespace ceres { @@ -69,10 +71,11 @@ class SchurEliminatorBase; // options.elimination_groups.push_back(num_cameras); // SchurJacobiPreconditioner preconditioner( // *A.block_structure(), options); -// preconditioner.Update(A, NULL); +// preconditioner.Update(A, nullptr); // preconditioner.RightMultiply(x, y); // -class SchurJacobiPreconditioner : public BlockSparseMatrixPreconditioner { +class CERES_NO_EXPORT SchurJacobiPreconditioner + : public BlockSparseMatrixPreconditioner { public: // Initialize the symbolic structure of the preconditioner. bs is // the block structure of the linear system to be solved. It is used @@ -81,11 +84,11 @@ class SchurJacobiPreconditioner : public BlockSparseMatrixPreconditioner { // It has the same structural requirement as other Schur complement // based solvers. Please see schur_eliminator.h for more details. SchurJacobiPreconditioner(const CompressedRowBlockStructure& bs, - const Preconditioner::Options& options); + Preconditioner::Options options); SchurJacobiPreconditioner(const SchurJacobiPreconditioner&) = delete; void operator=(const SchurJacobiPreconditioner&) = delete; - virtual ~SchurJacobiPreconditioner(); + ~SchurJacobiPreconditioner() override; // Preconditioner interface. void RightMultiply(const double* x, double* y) const final; @@ -104,4 +107,6 @@ class SchurJacobiPreconditioner : public BlockSparseMatrixPreconditioner { } // namespace internal } // namespace ceres +#include "ceres/internal/reenable_warnings.h" + #endif // CERES_INTERNAL_SCHUR_JACOBI_PRECONDITIONER_H_ diff --git a/extern/ceres/internal/ceres/schur_templates.h b/extern/ceres/internal/ceres/schur_templates.h index 90aee0a1afc..cacee20c412 100644 --- a/extern/ceres/internal/ceres/schur_templates.h +++ b/extern/ceres/internal/ceres/schur_templates.h @@ -32,11 +32,14 @@ #ifndef CERES_INTERNAL_SCHUR_TEMPLATES_H_ #define CERES_INTERNAL_SCHUR_TEMPLATES_H_ +#include "ceres/internal/config.h" +#include "ceres/internal/export.h" #include "ceres/linear_solver.h" namespace ceres { namespace internal { +CERES_NO_EXPORT void GetBestSchurTemplateSpecialization(int* row_block_size, int* e_block_size, int* f_block_size); diff --git a/extern/ceres/internal/ceres/scoped_thread_token.h b/extern/ceres/internal/ceres/scoped_thread_token.h index c167397cce9..533bfd5a387 100644 --- a/extern/ceres/internal/ceres/scoped_thread_token.h +++ b/extern/ceres/internal/ceres/scoped_thread_token.h @@ -31,6 +31,7 @@ #ifndef CERES_INTERNAL_SCOPED_THREAD_TOKEN_H_ #define CERES_INTERNAL_SCOPED_THREAD_TOKEN_H_ +#include "ceres/internal/export.h" #include "ceres/thread_token_provider.h" namespace ceres { @@ -38,21 +39,20 @@ namespace internal { // Helper class for ThreadTokenProvider. This object acquires a token in its // constructor and puts that token back with destruction. -class ScopedThreadToken { +class CERES_NO_EXPORT ScopedThreadToken { public: - ScopedThreadToken(ThreadTokenProvider* provider) + explicit ScopedThreadToken(ThreadTokenProvider* provider) : provider_(provider), token_(provider->Acquire()) {} ~ScopedThreadToken() { provider_->Release(token_); } + ScopedThreadToken(ScopedThreadToken&) = delete; + ScopedThreadToken& operator=(ScopedThreadToken&) = delete; int token() const { return token_; } private: ThreadTokenProvider* provider_; int token_; - - ScopedThreadToken(ScopedThreadToken&); - ScopedThreadToken& operator=(ScopedThreadToken&); }; } // namespace internal diff --git a/extern/ceres/internal/ceres/scratch_evaluate_preparer.cc b/extern/ceres/internal/ceres/scratch_evaluate_preparer.cc index 9905b220fbf..0a1b0f3e7d1 100644 --- a/extern/ceres/internal/ceres/scratch_evaluate_preparer.cc +++ b/extern/ceres/internal/ceres/scratch_evaluate_preparer.cc @@ -30,6 +30,8 @@ #include "ceres/scratch_evaluate_preparer.h" +#include <memory> + #include "ceres/parameter_block.h" #include "ceres/program.h" #include "ceres/residual_block.h" @@ -37,9 +39,9 @@ namespace ceres { namespace internal { -ScratchEvaluatePreparer* ScratchEvaluatePreparer::Create(const Program& program, - int num_threads) { - ScratchEvaluatePreparer* preparers = new ScratchEvaluatePreparer[num_threads]; +std::unique_ptr<ScratchEvaluatePreparer[]> ScratchEvaluatePreparer::Create( + const Program& program, int num_threads) { + auto preparers = std::make_unique<ScratchEvaluatePreparer[]>(num_threads); int max_derivatives_per_residual_block = program.MaxDerivativesPerResidualBlock(); for (int i = 0; i < num_threads; i++) { @@ -49,7 +51,8 @@ ScratchEvaluatePreparer* ScratchEvaluatePreparer::Create(const Program& program, } void ScratchEvaluatePreparer::Init(int max_derivatives_per_residual_block) { - jacobian_scratch_.reset(new double[max_derivatives_per_residual_block]); + jacobian_scratch_ = + std::make_unique<double[]>(max_derivatives_per_residual_block); } // Point the jacobian blocks into the scratch area of this evaluate preparer. @@ -64,10 +67,10 @@ void ScratchEvaluatePreparer::Prepare(const ResidualBlock* residual_block, const ParameterBlock* parameter_block = residual_block->parameter_blocks()[j]; if (parameter_block->IsConstant()) { - jacobians[j] = NULL; + jacobians[j] = nullptr; } else { jacobians[j] = jacobian_block_cursor; - jacobian_block_cursor += num_residuals * parameter_block->LocalSize(); + jacobian_block_cursor += num_residuals * parameter_block->TangentSize(); } } } diff --git a/extern/ceres/internal/ceres/scratch_evaluate_preparer.h b/extern/ceres/internal/ceres/scratch_evaluate_preparer.h index 2d2745d6269..3f4e7df8de0 100644 --- a/extern/ceres/internal/ceres/scratch_evaluate_preparer.h +++ b/extern/ceres/internal/ceres/scratch_evaluate_preparer.h @@ -37,6 +37,9 @@ #include <memory> +#include "ceres/internal/disable_warnings.h" +#include "ceres/internal/export.h" + namespace ceres { namespace internal { @@ -44,11 +47,11 @@ class Program; class ResidualBlock; class SparseMatrix; -class ScratchEvaluatePreparer { +class CERES_NO_EXPORT ScratchEvaluatePreparer { public: // Create num_threads ScratchEvaluatePreparers. - static ScratchEvaluatePreparer* Create(const Program& program, - int num_threads); + static std::unique_ptr<ScratchEvaluatePreparer[]> Create( + const Program& program, int num_threads); // EvaluatePreparer interface void Init(int max_derivatives_per_residual_block); @@ -66,4 +69,6 @@ class ScratchEvaluatePreparer { } // namespace internal } // namespace ceres +#include "ceres/internal/reenable_warnings.h" + #endif // CERES_INTERNAL_SCRATCH_EVALUATE_PREPARER_H_ diff --git a/extern/ceres/internal/ceres/single_linkage_clustering.h b/extern/ceres/internal/ceres/single_linkage_clustering.h index e891a9eec0a..b4a7e077619 100644 --- a/extern/ceres/internal/ceres/single_linkage_clustering.h +++ b/extern/ceres/internal/ceres/single_linkage_clustering.h @@ -34,7 +34,8 @@ #include <unordered_map> #include "ceres/graph.h" -#include "ceres/internal/port.h" +#include "ceres/internal/disable_warnings.h" +#include "ceres/internal/export.h" namespace ceres { namespace internal { @@ -55,12 +56,14 @@ struct SingleLinkageClusteringOptions { // // The return value of this function is the number of clusters // identified by the algorithm. -int CERES_EXPORT_INTERNAL -ComputeSingleLinkageClustering(const SingleLinkageClusteringOptions& options, - const WeightedGraph<int>& graph, - std::unordered_map<int, int>* membership); +CERES_NO_EXPORT int ComputeSingleLinkageClustering( + const SingleLinkageClusteringOptions& options, + const WeightedGraph<int>& graph, + std::unordered_map<int, int>* membership); } // namespace internal } // namespace ceres +#include "ceres/internal/reenable_warnings.h" + #endif // CERES_INTERNAL_SINGLE_LINKAGE_CLUSTERING_H_ diff --git a/extern/ceres/internal/ceres/small_blas.h b/extern/ceres/internal/ceres/small_blas.h index 4ee9229f35f..1cf41a5f1c2 100644 --- a/extern/ceres/internal/ceres/small_blas.h +++ b/extern/ceres/internal/ceres/small_blas.h @@ -36,7 +36,7 @@ #define CERES_INTERNAL_SMALL_BLAS_H_ #include "ceres/internal/eigen.h" -#include "ceres/internal/port.h" +#include "ceres/internal/export.h" #include "glog/logging.h" #include "small_blas_generic.h" @@ -210,7 +210,7 @@ CERES_GEMM_BEGIN(MatrixMatrixMultiplyNaive) { // Process the couple columns in remainder if present. if (NUM_COL_C & 2) { - int col = NUM_COL_C & (int)(~(span - 1)); + int col = NUM_COL_C & (~(span - 1)); const double* pa = &A[0]; for (int row = 0; row < NUM_ROW_C; ++row, pa += NUM_COL_A) { const double* pb = &B[col]; @@ -232,7 +232,7 @@ CERES_GEMM_BEGIN(MatrixMatrixMultiplyNaive) { } // Calculate the main part with multiples of 4. - int col_m = NUM_COL_C & (int)(~(span - 1)); + int col_m = NUM_COL_C & (~(span - 1)); for (int col = 0; col < col_m; col += span) { for (int row = 0; row < NUM_ROW_C; ++row) { const int index = (row + start_row_c) * col_stride_c + start_col_c + col; @@ -315,7 +315,7 @@ CERES_GEMM_BEGIN(MatrixTransposeMatrixMultiplyNaive) { // Process the couple columns in remainder if present. if (NUM_COL_C & 2) { - int col = NUM_COL_C & (int)(~(span - 1)); + int col = NUM_COL_C & (~(span - 1)); for (int row = 0; row < NUM_ROW_C; ++row) { const double* pa = &A[row]; const double* pb = &B[col]; @@ -339,7 +339,7 @@ CERES_GEMM_BEGIN(MatrixTransposeMatrixMultiplyNaive) { } // Process the main part with multiples of 4. - int col_m = NUM_COL_C & (int)(~(span - 1)); + int col_m = NUM_COL_C & (~(span - 1)); for (int col = 0; col < col_m; col += span) { for (int row = 0; row < NUM_ROW_C; ++row) { const int index = (row + start_row_c) * col_stride_c + start_col_c + col; @@ -435,7 +435,7 @@ inline void MatrixVectorMultiply(const double* A, // Process the couple rows in remainder if present. if (NUM_ROW_A & 2) { - int row = NUM_ROW_A & (int)(~(span - 1)); + int row = NUM_ROW_A & (~(span - 1)); const double* pa1 = &A[row * NUM_COL_A]; const double* pa2 = pa1 + NUM_COL_A; const double* pb = &b[0]; @@ -454,7 +454,7 @@ inline void MatrixVectorMultiply(const double* A, } // Calculate the main part with multiples of 4. - int row_m = NUM_ROW_A & (int)(~(span - 1)); + int row_m = NUM_ROW_A & (~(span - 1)); for (int row = 0; row < row_m; row += span) { // clang-format off MVM_mat4x1(NUM_COL_A, &A[row * NUM_COL_A], NUM_COL_A, @@ -522,7 +522,7 @@ inline void MatrixTransposeVectorMultiply(const double* A, // Process the couple columns in remainder if present. if (NUM_COL_A & 2) { - int row = NUM_COL_A & (int)(~(span - 1)); + int row = NUM_COL_A & (~(span - 1)); const double* pa = &A[row]; const double* pb = &b[0]; double tmp1 = 0.0, tmp2 = 0.0; @@ -543,7 +543,7 @@ inline void MatrixTransposeVectorMultiply(const double* A, } // Calculate the main part with multiples of 4. - int row_m = NUM_COL_A & (int)(~(span - 1)); + int row_m = NUM_COL_A & (~(span - 1)); for (int row = 0; row < row_m; row += span) { // clang-format off MTV_mat4x1(NUM_ROW_A, &A[row], NUM_COL_A, diff --git a/extern/ceres/internal/ceres/small_blas_generic.h b/extern/ceres/internal/ceres/small_blas_generic.h index 3f3ea424c80..f5aa909a8a3 100644 --- a/extern/ceres/internal/ceres/small_blas_generic.h +++ b/extern/ceres/internal/ceres/small_blas_generic.h @@ -1,5 +1,5 @@ // Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2018 Google Inc. All rights reserved. +// Copyright 2022 Google Inc. All rights reserved. // http://ceres-solver.org/ // // Redistribution and use in source and binary forms, with or without @@ -100,10 +100,11 @@ static inline void MMM_mat1x4(const int col_a, #define CERES_GEMM_OPT_MMM_MAT1X4_MUL \ av = pa[k]; \ pb = b + bi; \ - c0 += av * *pb++; \ - c1 += av * *pb++; \ - c2 += av * *pb++; \ - c3 += av * *pb++; \ + c0 += av * pb[0]; \ + c1 += av * pb[1]; \ + c2 += av * pb[2]; \ + c3 += av * pb[3]; \ + pb += 4; \ bi += col_stride_b; \ k++; @@ -167,10 +168,11 @@ static inline void MTM_mat1x4(const int col_a, #define CERES_GEMM_OPT_MTM_MAT1X4_MUL \ av = pa[ai]; \ pb = b + bi; \ - c0 += av * *pb++; \ - c1 += av * *pb++; \ - c2 += av * *pb++; \ - c3 += av * *pb++; \ + c0 += av * pb[0]; \ + c1 += av * pb[1]; \ + c2 += av * pb[2]; \ + c3 += av * pb[3]; \ + pb += 4; \ ai += col_stride_a; \ bi += col_stride_b; diff --git a/extern/ceres/internal/ceres/solver.cc b/extern/ceres/internal/ceres/solver.cc index dfde1221b61..150c5550fc9 100644 --- a/extern/ceres/internal/ceres/solver.cc +++ b/extern/ceres/internal/ceres/solver.cc @@ -41,7 +41,7 @@ #include "ceres/context_impl.h" #include "ceres/detect_structure.h" #include "ceres/gradient_checking_cost_function.h" -#include "ceres/internal/port.h" +#include "ceres/internal/export.h" #include "ceres/parameter_block_ordering.h" #include "ceres/preprocessor.h" #include "ceres/problem.h" @@ -141,16 +141,20 @@ bool TrustRegionOptionsAreValid(const Solver::Options& options, string* error) { return false; } - if (options.dense_linear_algebra_library_type == LAPACK && - !IsDenseLinearAlgebraLibraryTypeAvailable(LAPACK) && + if (!IsDenseLinearAlgebraLibraryTypeAvailable( + options.dense_linear_algebra_library_type) && (options.linear_solver_type == DENSE_NORMAL_CHOLESKY || options.linear_solver_type == DENSE_QR || options.linear_solver_type == DENSE_SCHUR)) { *error = StringPrintf( "Can't use %s with " - "Solver::Options::dense_linear_algebra_library_type = LAPACK " - "because LAPACK was not enabled when Ceres was built.", - LinearSolverTypeToString(options.linear_solver_type)); + "Solver::Options::dense_linear_algebra_library_type = %s " + "because %s was not enabled when Ceres was built.", + LinearSolverTypeToString(options.linear_solver_type), + DenseLinearAlgebraLibraryTypeToString( + options.dense_linear_algebra_library_type), + DenseLinearAlgebraLibraryTypeToString( + options.dense_linear_algebra_library_type)); return false; } @@ -367,7 +371,7 @@ void PostSolveSummarize(const internal::PreprocessedProblem& pp, &(summary->inner_iteration_ordering_used)); // clang-format off - summary->inner_iterations_used = pp.inner_iteration_minimizer.get() != NULL; // NOLINT + summary->inner_iterations_used = pp.inner_iteration_minimizer.get() != nullptr; // NOLINT summary->linear_solver_type_used = pp.linear_solver_options.type; summary->num_threads_used = pp.options.num_threads; summary->preconditioner_type_used = pp.options.preconditioner_type; @@ -375,7 +379,7 @@ void PostSolveSummarize(const internal::PreprocessedProblem& pp, internal::SetSummaryFinalCost(summary); - if (pp.reduced_program.get() != NULL) { + if (pp.reduced_program.get() != nullptr) { SummarizeReducedProgram(*pp.reduced_program, summary); } @@ -385,7 +389,7 @@ void PostSolveSummarize(const internal::PreprocessedProblem& pp, // case if the preprocessor failed, or if the reduced problem did // not contain any parameter blocks. Thus, only extract the // evaluator statistics if one exists. - if (pp.evaluator.get() != NULL) { + if (pp.evaluator.get() != nullptr) { const map<string, CallStatistics>& evaluator_statistics = pp.evaluator->Statistics(); { @@ -407,7 +411,7 @@ void PostSolveSummarize(const internal::PreprocessedProblem& pp, // Again, like the evaluator, there may or may not be a linear // solver from which we can extract run time statistics. In // particular the line search solver does not use a linear solver. - if (pp.linear_solver.get() != NULL) { + if (pp.linear_solver.get() != nullptr) { const map<string, CallStatistics>& linear_solver_statistics = pp.linear_solver->Statistics(); const CallStatistics& call_stats = FindWithDefault( @@ -436,8 +440,7 @@ void Minimize(internal::PreprocessedProblem* pp, Solver::Summary* summary) { } const Vector original_reduced_parameters = pp->reduced_parameters; - std::unique_ptr<Minimizer> minimizer( - Minimizer::Create(pp->options.minimizer_type)); + auto minimizer = Minimizer::Create(pp->options.minimizer_type); minimizer->Minimize( pp->minimizer_options, pp->reduced_parameters.data(), summary); @@ -485,7 +488,7 @@ bool Solver::Options::IsValid(string* error) const { return LineSearchOptionsAreValid(*this, error); } -Solver::~Solver() {} +Solver::~Solver() = default; void Solver::Solve(const Solver::Options& options, Problem* problem, @@ -518,11 +521,11 @@ void Solver::Solve(const Solver::Options& options, Solver::Options modified_options = options; if (options.check_gradients) { modified_options.callbacks.push_back(&gradient_checking_callback); - gradient_checking_problem.reset(CreateGradientCheckingProblemImpl( + gradient_checking_problem = CreateGradientCheckingProblemImpl( problem_impl, options.gradient_check_numeric_derivative_relative_step_size, options.gradient_check_relative_precision, - &gradient_checking_callback)); + &gradient_checking_callback); problem_impl = gradient_checking_problem.get(); program = problem_impl->mutable_program(); } @@ -534,8 +537,7 @@ void Solver::Solve(const Solver::Options& options, // The main thread also does work so we only need to launch num_threads - 1. problem_impl->context()->EnsureMinimumThreads(options.num_threads - 1); - std::unique_ptr<Preprocessor> preprocessor( - Preprocessor::Create(modified_options.minimizer_type)); + auto preprocessor = Preprocessor::Create(modified_options.minimizer_type); PreprocessedProblem pp; const bool status = diff --git a/extern/ceres/internal/ceres/solver_utils.cc b/extern/ceres/internal/ceres/solver_utils.cc index eb5aafa061c..22fa137055d 100644 --- a/extern/ceres/internal/ceres/solver_utils.cc +++ b/extern/ceres/internal/ceres/solver_utils.cc @@ -34,8 +34,11 @@ #include "Eigen/Core" #include "ceres/internal/config.h" -#include "ceres/internal/port.h" +#include "ceres/internal/export.h" #include "ceres/version.h" +#ifndef CERES_NO_CUDA +#include "cuda_runtime.h" +#endif // CERES_NO_CUDA namespace ceres { namespace internal { @@ -87,6 +90,10 @@ std::string VersionString() { value += "-no_custom_blas"; #endif +#ifndef CERES_NO_CUDA + value += "-cuda-(" + std::to_string(CUDART_VERSION) + ")"; +#endif + return value; } diff --git a/extern/ceres/internal/ceres/solver_utils.h b/extern/ceres/internal/ceres/solver_utils.h index 85fbf3776ab..298564a897d 100644 --- a/extern/ceres/internal/ceres/solver_utils.h +++ b/extern/ceres/internal/ceres/solver_utils.h @@ -28,9 +28,14 @@ // // Author: sameeragarwal@google.com (Sameer Agarwal) +#ifndef CERES_INTERNAL_SOLVER_UTILS_H_ +#define CERES_INTERNAL_SOLVER_UTILS_H_ + #include <algorithm> #include <string> +#include "ceres/internal/disable_warnings.h" +#include "ceres/internal/export.h" #include "ceres/iteration_callback.h" #include "ceres/types.h" @@ -55,7 +60,12 @@ void SetSummaryFinalCost(SummaryType* summary) { } } +CERES_NO_EXPORT std::string VersionString(); } // namespace internal } // namespace ceres + +#include "ceres/internal/reenable_warnings.h" + +#endif // CERES_INTERNAL_SOLVER_UTILS_H_ diff --git a/extern/ceres/internal/ceres/sparse_cholesky.cc b/extern/ceres/internal/ceres/sparse_cholesky.cc index 91cdf671b1a..4a80470ffb7 100644 --- a/extern/ceres/internal/ceres/sparse_cholesky.cc +++ b/extern/ceres/internal/ceres/sparse_cholesky.cc @@ -30,6 +30,8 @@ #include "ceres/sparse_cholesky.h" +#include <memory> + #include "ceres/accelerate_sparse.h" #include "ceres/cxsparse.h" #include "ceres/eigensparse.h" @@ -113,7 +115,7 @@ std::unique_ptr<SparseCholesky> SparseCholesky::Create( return sparse_cholesky; } -SparseCholesky::~SparseCholesky() {} +SparseCholesky::~SparseCholesky() = default; LinearSolverTerminationType SparseCholesky::FactorAndSolve( CompressedRowSparseMatrix* lhs, @@ -133,7 +135,7 @@ RefinedSparseCholesky::RefinedSparseCholesky( : sparse_cholesky_(std::move(sparse_cholesky)), iterative_refiner_(std::move(iterative_refiner)) {} -RefinedSparseCholesky::~RefinedSparseCholesky() {} +RefinedSparseCholesky::~RefinedSparseCholesky() = default; CompressedRowSparseMatrix::StorageType RefinedSparseCholesky::StorageType() const { diff --git a/extern/ceres/internal/ceres/sparse_cholesky.h b/extern/ceres/internal/ceres/sparse_cholesky.h index a6af6b2c207..80c5cb2b83b 100644 --- a/extern/ceres/internal/ceres/sparse_cholesky.h +++ b/extern/ceres/internal/ceres/sparse_cholesky.h @@ -33,11 +33,13 @@ // This include must come before any #ifndef check on Ceres compile options. // clang-format off -#include "ceres/internal/port.h" +#include "ceres/internal/config.h" // clang-format on #include <memory> +#include "ceres/internal/disable_warnings.h" +#include "ceres/internal/export.h" #include "ceres/linear_solver.h" #include "glog/logging.h" @@ -67,7 +69,7 @@ namespace internal { // CHECK_EQ(sparse_cholesky->Solve(rhs.data(), solution.data(), &message), // LINEAR_SOLVER_SUCCESS); -class CERES_EXPORT_INTERNAL SparseCholesky { +class CERES_NO_EXPORT SparseCholesky { public: static std::unique_ptr<SparseCholesky> Create( const LinearSolver::Options& options); @@ -104,29 +106,28 @@ class CERES_EXPORT_INTERNAL SparseCholesky { // Convenience method which combines a call to Factorize and // Solve. Solve is only called if Factorize returns // LINEAR_SOLVER_SUCCESS. - virtual LinearSolverTerminationType FactorAndSolve( - CompressedRowSparseMatrix* lhs, - const double* rhs, - double* solution, - std::string* message); + LinearSolverTerminationType FactorAndSolve(CompressedRowSparseMatrix* lhs, + const double* rhs, + double* solution, + std::string* message); }; class IterativeRefiner; // Computes an initial solution using the given instance of // SparseCholesky, and then refines it using the IterativeRefiner. -class CERES_EXPORT_INTERNAL RefinedSparseCholesky : public SparseCholesky { +class CERES_NO_EXPORT RefinedSparseCholesky final : public SparseCholesky { public: RefinedSparseCholesky(std::unique_ptr<SparseCholesky> sparse_cholesky, std::unique_ptr<IterativeRefiner> iterative_refiner); - virtual ~RefinedSparseCholesky(); + ~RefinedSparseCholesky() override; - virtual CompressedRowSparseMatrix::StorageType StorageType() const; - virtual LinearSolverTerminationType Factorize(CompressedRowSparseMatrix* lhs, - std::string* message); - virtual LinearSolverTerminationType Solve(const double* rhs, - double* solution, - std::string* message); + CompressedRowSparseMatrix::StorageType StorageType() const override; + LinearSolverTerminationType Factorize(CompressedRowSparseMatrix* lhs, + std::string* message) override; + LinearSolverTerminationType Solve(const double* rhs, + double* solution, + std::string* message) override; private: std::unique_ptr<SparseCholesky> sparse_cholesky_; @@ -137,4 +138,6 @@ class CERES_EXPORT_INTERNAL RefinedSparseCholesky : public SparseCholesky { } // namespace internal } // namespace ceres +#include "ceres/internal/reenable_warnings.h" + #endif // CERES_INTERNAL_SPARSE_CHOLESKY_H_ diff --git a/extern/ceres/internal/ceres/sparse_matrix.cc b/extern/ceres/internal/ceres/sparse_matrix.cc index 32388f58fc3..bc757ead361 100644 --- a/extern/ceres/internal/ceres/sparse_matrix.cc +++ b/extern/ceres/internal/ceres/sparse_matrix.cc @@ -33,7 +33,7 @@ namespace ceres { namespace internal { -SparseMatrix::~SparseMatrix() {} +SparseMatrix::~SparseMatrix() = default; } // namespace internal } // namespace ceres diff --git a/extern/ceres/internal/ceres/sparse_matrix.h b/extern/ceres/internal/ceres/sparse_matrix.h index b57f10890fc..1dbb96e6070 100644 --- a/extern/ceres/internal/ceres/sparse_matrix.h +++ b/extern/ceres/internal/ceres/sparse_matrix.h @@ -36,7 +36,7 @@ #include <cstdio> #include "ceres/internal/eigen.h" -#include "ceres/internal/port.h" +#include "ceres/internal/export.h" #include "ceres/linear_operator.h" #include "ceres/types.h" @@ -64,14 +64,14 @@ namespace internal { // matrix type dependent and we are at this stage unable to come up // with an efficient high level interface that spans multiple sparse // matrix types. -class CERES_EXPORT_INTERNAL SparseMatrix : public LinearOperator { +class CERES_NO_EXPORT SparseMatrix : public LinearOperator { public: - virtual ~SparseMatrix(); + ~SparseMatrix() override; // y += Ax; - virtual void RightMultiply(const double* x, double* y) const = 0; + void RightMultiply(const double* x, double* y) const override = 0; // y += A'x; - virtual void LeftMultiply(const double* x, double* y) const = 0; + void LeftMultiply(const double* x, double* y) const override = 0; // In MATLAB notation sum(A.*A, 1) virtual void SquaredColumnNorm(double* x) const = 0; @@ -98,8 +98,8 @@ class CERES_EXPORT_INTERNAL SparseMatrix : public LinearOperator { virtual double* mutable_values() = 0; virtual const double* values() const = 0; - virtual int num_rows() const = 0; - virtual int num_cols() const = 0; + int num_rows() const override = 0; + int num_cols() const override = 0; virtual int num_nonzeros() const = 0; }; diff --git a/extern/ceres/internal/ceres/sparse_normal_cholesky_solver.cc b/extern/ceres/internal/ceres/sparse_normal_cholesky_solver.cc index 0f2e589d041..2e52ae6d908 100644 --- a/extern/ceres/internal/ceres/sparse_normal_cholesky_solver.cc +++ b/extern/ceres/internal/ceres/sparse_normal_cholesky_solver.cc @@ -54,7 +54,7 @@ SparseNormalCholeskySolver::SparseNormalCholeskySolver( sparse_cholesky_ = SparseCholesky::Create(options); } -SparseNormalCholeskySolver::~SparseNormalCholeskySolver() {} +SparseNormalCholeskySolver::~SparseNormalCholeskySolver() = default; LinearSolver::Summary SparseNormalCholeskySolver::SolveImpl( BlockSparseMatrix* A, @@ -75,21 +75,21 @@ LinearSolver::Summary SparseNormalCholeskySolver::SolveImpl( A->LeftMultiply(b, rhs_.data()); event_logger.AddEvent("Compute RHS"); - if (per_solve_options.D != NULL) { + if (per_solve_options.D != nullptr) { // Temporarily append a diagonal block to the A matrix, but undo // it before returning the matrix to the user. - std::unique_ptr<BlockSparseMatrix> regularizer; - regularizer.reset(BlockSparseMatrix::CreateDiagonalMatrix( - per_solve_options.D, A->block_structure()->cols)); + std::unique_ptr<BlockSparseMatrix> regularizer = + BlockSparseMatrix::CreateDiagonalMatrix(per_solve_options.D, + A->block_structure()->cols); event_logger.AddEvent("Diagonal"); A->AppendRows(*regularizer); event_logger.AddEvent("Append"); } event_logger.AddEvent("Append Rows"); - if (inner_product_computer_.get() == NULL) { - inner_product_computer_.reset( - InnerProductComputer::Create(*A, sparse_cholesky_->StorageType())); + if (inner_product_computer_.get() == nullptr) { + inner_product_computer_ = + InnerProductComputer::Create(*A, sparse_cholesky_->StorageType()); event_logger.AddEvent("InnerProductComputer::Create"); } @@ -97,7 +97,7 @@ LinearSolver::Summary SparseNormalCholeskySolver::SolveImpl( inner_product_computer_->Compute(); event_logger.AddEvent("InnerProductComputer::Compute"); - if (per_solve_options.D != NULL) { + if (per_solve_options.D != nullptr) { A->DeleteRowBlocks(A->block_structure()->cols.size()); } diff --git a/extern/ceres/internal/ceres/sparse_normal_cholesky_solver.h b/extern/ceres/internal/ceres/sparse_normal_cholesky_solver.h index ef3274323f5..caec566612e 100644 --- a/extern/ceres/internal/ceres/sparse_normal_cholesky_solver.h +++ b/extern/ceres/internal/ceres/sparse_normal_cholesky_solver.h @@ -36,11 +36,13 @@ // This include must come before any #ifndef check on Ceres compile options. // clang-format off -#include "ceres/internal/port.h" +#include "ceres/internal/config.h" // clang-format on +#include <memory> #include <vector> +#include "ceres/internal/export.h" #include "ceres/linear_solver.h" namespace ceres { @@ -52,13 +54,14 @@ class SparseCholesky; // Solves the normal equations (A'A + D'D) x = A'b, using the sparse // linear algebra library of the user's choice. -class SparseNormalCholeskySolver : public BlockSparseMatrixSolver { +class CERES_NO_EXPORT SparseNormalCholeskySolver + : public BlockSparseMatrixSolver { public: explicit SparseNormalCholeskySolver(const LinearSolver::Options& options); SparseNormalCholeskySolver(const SparseNormalCholeskySolver&) = delete; void operator=(const SparseNormalCholeskySolver&) = delete; - virtual ~SparseNormalCholeskySolver(); + ~SparseNormalCholeskySolver() override; private: LinearSolver::Summary SolveImpl(BlockSparseMatrix* A, diff --git a/extern/ceres/internal/ceres/split.cc b/extern/ceres/internal/ceres/split.cc deleted file mode 100644 index 804f4412deb..00000000000 --- a/extern/ceres/internal/ceres/split.cc +++ /dev/null @@ -1,122 +0,0 @@ -// Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2015 Google Inc. All rights reserved. -// http://ceres-solver.org/ -// -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions are met: -// -// * Redistributions of source code must retain the above copyright notice, -// this list of conditions and the following disclaimer. -// * Redistributions in binary form must reproduce the above copyright notice, -// this list of conditions and the following disclaimer in the documentation -// and/or other materials provided with the distribution. -// * Neither the name of Google Inc. nor the names of its contributors may be -// used to endorse or promote products derived from this software without -// specific prior written permission. -// -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE -// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -// POSSIBILITY OF SUCH DAMAGE. -// -// Author: keir@google.com (Keir Mierle) - -#include "ceres/split.h" - -#include <iterator> -#include <string> -#include <vector> - -#include "ceres/internal/port.h" - -namespace ceres { -namespace internal { - -using std::string; -using std::vector; - -// If we know how much to allocate for a vector of strings, we can allocate the -// vector<string> only once and directly to the right size. This saves in -// between 33-66 % of memory space needed for the result, and runs faster in the -// microbenchmarks. -// -// The reserve is only implemented for the single character delim. -// -// The implementation for counting is cut-and-pasted from -// SplitStringToIteratorUsing. I could have written my own counting iterator, -// and use the existing template function, but probably this is more clear and -// more sure to get optimized to reasonable code. -static int CalculateReserveForVector(const string& full, const char* delim) { - int count = 0; - if (delim[0] != '\0' && delim[1] == '\0') { - // Optimize the common case where delim is a single character. - char c = delim[0]; - const char* p = full.data(); - const char* end = p + full.size(); - while (p != end) { - if (*p == c) { // This could be optimized with hasless(v,1) trick. - ++p; - } else { - while (++p != end && *p != c) { - // Skip to the next occurence of the delimiter. - } - ++count; - } - } - } - return count; -} - -template <typename StringType, typename ITR> -static inline void SplitStringToIteratorUsing(const StringType& full, - const char* delim, - ITR& result) { - // Optimize the common case where delim is a single character. - if (delim[0] != '\0' && delim[1] == '\0') { - char c = delim[0]; - const char* p = full.data(); - const char* end = p + full.size(); - while (p != end) { - if (*p == c) { - ++p; - } else { - const char* start = p; - while (++p != end && *p != c) { - // Skip to the next occurence of the delimiter. - } - *result++ = StringType(start, p - start); - } - } - return; - } - - string::size_type begin_index, end_index; - begin_index = full.find_first_not_of(delim); - while (begin_index != string::npos) { - end_index = full.find_first_of(delim, begin_index); - if (end_index == string::npos) { - *result++ = full.substr(begin_index); - return; - } - *result++ = full.substr(begin_index, (end_index - begin_index)); - begin_index = full.find_first_not_of(delim, end_index); - } -} - -void SplitStringUsing(const string& full, - const char* delim, - vector<string>* result) { - result->reserve(result->size() + CalculateReserveForVector(full, delim)); - std::back_insert_iterator<vector<string>> it(*result); - SplitStringToIteratorUsing(full, delim, it); -} - -} // namespace internal -} // namespace ceres diff --git a/extern/ceres/internal/ceres/stl_util.h b/extern/ceres/internal/ceres/stl_util.h index d3411b73376..2af2518f837 100644 --- a/extern/ceres/internal/ceres/stl_util.h +++ b/extern/ceres/internal/ceres/stl_util.h @@ -73,7 +73,7 @@ void STLDeleteUniqueContainerPointers(ForwardIterator begin, // hash_set, or any other STL container which defines sensible begin(), end(), // and clear() methods. // -// If container is NULL, this function is a no-op. +// If container is nullptr, this function is a no-op. // // As an alternative to calling STLDeleteElements() directly, consider // ElementDeleter (defined below), which ensures that your container's elements diff --git a/extern/ceres/internal/ceres/stringprintf.cc b/extern/ceres/internal/ceres/stringprintf.cc index b0e2acce8f9..e45b4301eef 100644 --- a/extern/ceres/internal/ceres/stringprintf.cc +++ b/extern/ceres/internal/ceres/stringprintf.cc @@ -36,7 +36,7 @@ #include <string> #include <vector> -#include "ceres/internal/port.h" +#include "ceres/internal/export.h" namespace ceres { namespace internal { @@ -66,7 +66,7 @@ void StringAppendV(string* dst, const char* format, va_list ap) { // Error or MSVC running out of space. MSVC 8.0 and higher // can be asked about space needed with the special idiom below: va_copy(backup_ap, ap); - result = vsnprintf(NULL, 0, format, backup_ap); + result = vsnprintf(nullptr, 0, format, backup_ap); va_end(backup_ap); #endif diff --git a/extern/ceres/internal/ceres/stringprintf.h b/extern/ceres/internal/ceres/stringprintf.h index 4d512784905..e24325fbd35 100644 --- a/extern/ceres/internal/ceres/stringprintf.h +++ b/extern/ceres/internal/ceres/stringprintf.h @@ -41,7 +41,8 @@ #include <cstdarg> #include <string> -#include "ceres/internal/port.h" +#include "ceres/internal/disable_warnings.h" +#include "ceres/internal/export.h" namespace ceres { namespace internal { @@ -63,32 +64,35 @@ namespace internal { #endif // Return a C++ string. -CERES_EXPORT_INTERNAL extern std::string StringPrintf(const char* format, ...) +CERES_NO_EXPORT extern std::string StringPrintf(const char* format, ...) // Tell the compiler to do printf format string checking. CERES_PRINTF_ATTRIBUTE(1, 2); // Store result into a supplied string and return it. -CERES_EXPORT_INTERNAL extern const std::string& SStringPrintf( - std::string* dst, const char* format, ...) +CERES_NO_EXPORT extern const std::string& SStringPrintf(std::string* dst, + const char* format, + ...) // Tell the compiler to do printf format string checking. CERES_PRINTF_ATTRIBUTE(2, 3); // Append result to a supplied string. -CERES_EXPORT_INTERNAL extern void StringAppendF(std::string* dst, - const char* format, - ...) +CERES_NO_EXPORT extern void StringAppendF(std::string* dst, + const char* format, + ...) // Tell the compiler to do printf format string checking. CERES_PRINTF_ATTRIBUTE(2, 3); // Lower-level routine that takes a va_list and appends to a specified string. // All other routines are just convenience wrappers around it. -CERES_EXPORT_INTERNAL extern void StringAppendV(std::string* dst, - const char* format, - va_list ap); +CERES_NO_EXPORT extern void StringAppendV(std::string* dst, + const char* format, + va_list ap); #undef CERES_PRINTF_ATTRIBUTE } // namespace internal } // namespace ceres +#include "ceres/internal/reenable_warnings.h" + #endif // CERES_INTERNAL_STRINGPRINTF_H_ diff --git a/extern/ceres/internal/ceres/subset_preconditioner.cc b/extern/ceres/internal/ceres/subset_preconditioner.cc index 779a34ae741..221530c0dd5 100644 --- a/extern/ceres/internal/ceres/subset_preconditioner.cc +++ b/extern/ceres/internal/ceres/subset_preconditioner.cc @@ -32,6 +32,7 @@ #include <memory> #include <string> +#include <utility> #include "ceres/compressed_row_sparse_matrix.h" #include "ceres/inner_product_computer.h" @@ -42,9 +43,9 @@ namespace ceres { namespace internal { -SubsetPreconditioner::SubsetPreconditioner( - const Preconditioner::Options& options, const BlockSparseMatrix& A) - : options_(options), num_cols_(A.num_cols()) { +SubsetPreconditioner::SubsetPreconditioner(Preconditioner::Options options, + const BlockSparseMatrix& A) + : options_(std::move(options)), num_cols_(A.num_cols()) { CHECK_GE(options_.subset_preconditioner_start_row_block, 0) << "Congratulations, you found a bug in Ceres. Please report it."; @@ -55,7 +56,7 @@ SubsetPreconditioner::SubsetPreconditioner( sparse_cholesky_ = SparseCholesky::Create(sparse_cholesky_options); } -SubsetPreconditioner::~SubsetPreconditioner() {} +SubsetPreconditioner::~SubsetPreconditioner() = default; void SubsetPreconditioner::RightMultiply(const double* x, double* y) const { CHECK(x != nullptr); @@ -66,14 +67,14 @@ void SubsetPreconditioner::RightMultiply(const double* x, double* y) const { bool SubsetPreconditioner::UpdateImpl(const BlockSparseMatrix& A, const double* D) { - BlockSparseMatrix* m = const_cast<BlockSparseMatrix*>(&A); + auto* m = const_cast<BlockSparseMatrix*>(&A); const CompressedRowBlockStructure* bs = m->block_structure(); // A = [P] // [Q] // Now add D to A if needed. - if (D != NULL) { + if (D != nullptr) { // A = [P] // [Q] // [D] @@ -82,19 +83,19 @@ bool SubsetPreconditioner::UpdateImpl(const BlockSparseMatrix& A, m->AppendRows(*regularizer); } - if (inner_product_computer_.get() == NULL) { - inner_product_computer_.reset(InnerProductComputer::Create( + if (inner_product_computer_ == nullptr) { + inner_product_computer_ = InnerProductComputer::Create( *m, options_.subset_preconditioner_start_row_block, bs->rows.size(), - sparse_cholesky_->StorageType())); + sparse_cholesky_->StorageType()); } // Compute inner_product = [Q'*Q + D'*D] inner_product_computer_->Compute(); // Unappend D if needed. - if (D != NULL) { + if (D != nullptr) { // A = [P] // [Q] m->DeleteRowBlocks(bs->cols.size()); diff --git a/extern/ceres/internal/ceres/subset_preconditioner.h b/extern/ceres/internal/ceres/subset_preconditioner.h index 9844a669f45..6d07995a136 100644 --- a/extern/ceres/internal/ceres/subset_preconditioner.h +++ b/extern/ceres/internal/ceres/subset_preconditioner.h @@ -33,7 +33,8 @@ #include <memory> -#include "ceres/internal/port.h" +#include "ceres/internal/disable_warnings.h" +#include "ceres/internal/export.h" #include "ceres/preconditioner.h" namespace ceres { @@ -67,12 +68,12 @@ class InnerProductComputer; // computationally expensive this preconditioner will be. // // See the tests for example usage. -class CERES_EXPORT_INTERNAL SubsetPreconditioner +class CERES_NO_EXPORT SubsetPreconditioner : public BlockSparseMatrixPreconditioner { public: - SubsetPreconditioner(const Preconditioner::Options& options, + SubsetPreconditioner(Preconditioner::Options options, const BlockSparseMatrix& A); - virtual ~SubsetPreconditioner(); + ~SubsetPreconditioner() override; // Preconditioner interface void RightMultiply(const double* x, double* y) const final; @@ -91,4 +92,6 @@ class CERES_EXPORT_INTERNAL SubsetPreconditioner } // namespace internal } // namespace ceres +#include "ceres/internal/reenable_warnings.h" + #endif // CERES_INTERNAL_SUBSET_PRECONDITIONER_H_ diff --git a/extern/ceres/internal/ceres/suitesparse.cc b/extern/ceres/internal/ceres/suitesparse.cc index 0d6f6bdfb88..883dcc8f63e 100644 --- a/extern/ceres/internal/ceres/suitesparse.cc +++ b/extern/ceres/internal/ceres/suitesparse.cc @@ -29,9 +29,10 @@ // Author: sameeragarwal@google.com (Sameer Agarwal) // This include must come before any #ifndef check on Ceres compile options. -#include "ceres/internal/port.h" +#include "ceres/internal/config.h" #ifndef CERES_NO_SUITESPARSE +#include <memory> #include <vector> #include "ceres/compressed_col_sparse_matrix_utils.h" @@ -368,7 +369,7 @@ SuiteSparseCholesky::~SuiteSparseCholesky() { LinearSolverTerminationType SuiteSparseCholesky::Factorize( CompressedRowSparseMatrix* lhs, string* message) { if (lhs == nullptr) { - *message = "Failure: Input lhs is NULL."; + *message = "Failure: Input lhs is nullptr."; return LINEAR_SOLVER_FATAL_ERROR; } diff --git a/extern/ceres/internal/ceres/suitesparse.h b/extern/ceres/internal/ceres/suitesparse.h index 5dcc53f0167..3f62e7c7b7d 100644 --- a/extern/ceres/internal/ceres/suitesparse.h +++ b/extern/ceres/internal/ceres/suitesparse.h @@ -34,11 +34,12 @@ #define CERES_INTERNAL_SUITESPARSE_H_ // This include must come before any #ifndef check on Ceres compile options. -#include "ceres/internal/port.h" +#include "ceres/internal/config.h" #ifndef CERES_NO_SUITESPARSE #include <cstring> +#include <memory> #include <string> #include <vector> @@ -70,6 +71,8 @@ #define SuiteSparse_long UF_long #endif +#include "ceres/internal/disable_warnings.h" + namespace ceres { namespace internal { @@ -81,7 +84,7 @@ class TripletSparseMatrix; // provides the user with a simpler interface. The methods here cannot // be static as a cholmod_common object serves as a global variable // for all cholmod function calls. -class SuiteSparse { +class CERES_NO_EXPORT SuiteSparse { public: SuiteSparse(); ~SuiteSparse(); @@ -106,7 +109,7 @@ class SuiteSparse { cholmod_dense CreateDenseVectorView(const double* x, int size); // Given a vector x, build a cholmod_dense vector of size out_size - // with the first in_size entries copied from x. If x is NULL, then + // with the first in_size entries copied from x. If x is nullptr, then // an all zeros vector is returned. Caller owns the result. cholmod_dense* CreateDenseVector(const double* x, int in_size, int out_size); @@ -123,7 +126,7 @@ class SuiteSparse { // Create and return a matrix m = A * A'. Caller owns the // result. The matrix A is not modified. cholmod_sparse* AATranspose(cholmod_sparse* A) { - cholmod_sparse* m = cholmod_aat(A, NULL, A->nrow, 1, &cc_); + cholmod_sparse* m = cholmod_aat(A, nullptr, A->nrow, 1, &cc_); m->stype = 1; // Pay attention to the upper triangular part. return m; } @@ -196,7 +199,7 @@ class SuiteSparse { // Given a Cholesky factorization of a matrix A = LL^T, solve the // linear system Ax = b, and return the result. If the Solve fails - // NULL is returned. Caller owns the result. + // nullptr is returned. Caller owns the result. // // message contains an explanation of the failures if any. cholmod_dense* Solve(cholmod_factor* L, @@ -288,12 +291,12 @@ class SuiteSparse { cholmod_common cc_; }; -class SuiteSparseCholesky : public SparseCholesky { +class CERES_NO_EXPORT SuiteSparseCholesky final : public SparseCholesky { public: static std::unique_ptr<SparseCholesky> Create(OrderingType ordering_type); // SparseCholesky interface. - virtual ~SuiteSparseCholesky(); + ~SuiteSparseCholesky() override; CompressedRowSparseMatrix::StorageType StorageType() const final; LinearSolverTerminationType Factorize(CompressedRowSparseMatrix* lhs, std::string* message) final; @@ -302,7 +305,7 @@ class SuiteSparseCholesky : public SparseCholesky { std::string* message) final; private: - SuiteSparseCholesky(const OrderingType ordering_type); + explicit SuiteSparseCholesky(const OrderingType ordering_type); const OrderingType ordering_type_; SuiteSparse ss_; @@ -312,14 +315,18 @@ class SuiteSparseCholesky : public SparseCholesky { } // namespace internal } // namespace ceres +#include "ceres/internal/reenable_warnings.h" + #else // CERES_NO_SUITESPARSE typedef void cholmod_factor; +#include "ceres/internal/disable_warnings.h" + namespace ceres { namespace internal { -class SuiteSparse { +class CERES_NO_EXPORT SuiteSparse { public: // Defining this static function even when SuiteSparse is not // available, allows client code to check for the presence of CAMD @@ -332,12 +339,14 @@ class SuiteSparse { return false; } - void Free(void* arg) {} + void Free(void* /*arg*/) {} }; } // namespace internal } // namespace ceres +#include "ceres/internal/reenable_warnings.h" + #endif // CERES_NO_SUITESPARSE #endif // CERES_INTERNAL_SUITESPARSE_H_ diff --git a/extern/ceres/internal/ceres/thread_pool.cc b/extern/ceres/internal/ceres/thread_pool.cc index 821431cedb4..57f01af5476 100644 --- a/extern/ceres/internal/ceres/thread_pool.cc +++ b/extern/ceres/internal/ceres/thread_pool.cc @@ -29,7 +29,7 @@ // Author: vitus@google.com (Michael Vitus) // This include must come before any #ifndef check on Ceres compile options. -#include "ceres/internal/port.h" +#include "ceres/internal/config.h" #ifdef CERES_USE_CXX_THREADS @@ -57,7 +57,7 @@ int ThreadPool::MaxNumThreadsAvailable() { : num_hardware_threads; } -ThreadPool::ThreadPool() {} +ThreadPool::ThreadPool() = default; ThreadPool::ThreadPool(int num_threads) { Resize(num_threads); } @@ -83,7 +83,7 @@ void ThreadPool::Resize(int num_threads) { GetNumAllowedThreads(num_threads) - num_current_threads; for (int i = 0; i < create_num_threads; ++i) { - thread_pool_.push_back(std::thread(&ThreadPool::ThreadMainLoop, this)); + thread_pool_.emplace_back(&ThreadPool::ThreadMainLoop, this); } } diff --git a/extern/ceres/internal/ceres/thread_pool.h b/extern/ceres/internal/ceres/thread_pool.h index cdf6625e196..94ab1e66bd4 100644 --- a/extern/ceres/internal/ceres/thread_pool.h +++ b/extern/ceres/internal/ceres/thread_pool.h @@ -37,7 +37,7 @@ #include <vector> #include "ceres/concurrent_queue.h" -#include "ceres/internal/port.h" +#include "ceres/internal/export.h" namespace ceres { namespace internal { @@ -58,7 +58,7 @@ namespace internal { // workers to stop. The workers will finish all of the tasks that have already // been added to the thread pool. // -class CERES_EXPORT_INTERNAL ThreadPool { +class CERES_NO_EXPORT ThreadPool { public: // Returns the maximum number of hardware threads. static int MaxNumThreadsAvailable(); diff --git a/extern/ceres/internal/ceres/thread_token_provider.h b/extern/ceres/internal/ceres/thread_token_provider.h index 06dc0438572..918c687eb24 100644 --- a/extern/ceres/internal/ceres/thread_token_provider.h +++ b/extern/ceres/internal/ceres/thread_token_provider.h @@ -32,7 +32,7 @@ #define CERES_INTERNAL_THREAD_TOKEN_PROVIDER_H_ #include "ceres/internal/config.h" -#include "ceres/internal/port.h" +#include "ceres/internal/export.h" #ifdef CERES_USE_CXX_THREADS #include "ceres/concurrent_queue.h" @@ -66,9 +66,9 @@ namespace internal { // ttp.Release(token); // return token to the pool // } // -class ThreadTokenProvider { +class CERES_NO_EXPORT ThreadTokenProvider { public: - ThreadTokenProvider(int num_threads); + explicit ThreadTokenProvider(int num_threads); // Returns the first token from the queue. The acquired value must be // given back by Release(). @@ -87,8 +87,8 @@ class ThreadTokenProvider { ConcurrentQueue<int> pool_; #endif - ThreadTokenProvider(ThreadTokenProvider&); - ThreadTokenProvider& operator=(ThreadTokenProvider&); + ThreadTokenProvider(ThreadTokenProvider&) = delete; + ThreadTokenProvider& operator=(ThreadTokenProvider&) = delete; }; } // namespace internal diff --git a/extern/ceres/internal/ceres/triplet_sparse_matrix.cc b/extern/ceres/internal/ceres/triplet_sparse_matrix.cc index 5dbf0e7cd3a..bbb5f676a5d 100644 --- a/extern/ceres/internal/ceres/triplet_sparse_matrix.cc +++ b/extern/ceres/internal/ceres/triplet_sparse_matrix.cc @@ -31,10 +31,10 @@ #include "ceres/triplet_sparse_matrix.h" #include <algorithm> -#include <cstddef> +#include <memory> #include "ceres/internal/eigen.h" -#include "ceres/internal/port.h" +#include "ceres/internal/export.h" #include "ceres/random.h" #include "ceres/types.h" #include "glog/logging.h" @@ -45,7 +45,7 @@ namespace internal { TripletSparseMatrix::TripletSparseMatrix() : num_rows_(0), num_cols_(0), max_num_nonzeros_(0), num_nonzeros_(0) {} -TripletSparseMatrix::~TripletSparseMatrix() {} +TripletSparseMatrix::~TripletSparseMatrix() = default; TripletSparseMatrix::TripletSparseMatrix(int num_rows, int num_cols, @@ -109,8 +109,9 @@ bool TripletSparseMatrix::AllTripletsWithinBounds() const { for (int i = 0; i < num_nonzeros_; ++i) { // clang-format off if ((rows_[i] < 0) || (rows_[i] >= num_rows_) || - (cols_[i] < 0) || (cols_[i] >= num_cols_)) + (cols_[i] < 0) || (cols_[i] >= num_cols_)) { return false; + } // clang-format on } return true; @@ -123,9 +124,12 @@ void TripletSparseMatrix::Reserve(int new_max_num_nonzeros) { // Nothing to do if we have enough space already. if (new_max_num_nonzeros <= max_num_nonzeros_) return; - int* new_rows = new int[new_max_num_nonzeros]; - int* new_cols = new int[new_max_num_nonzeros]; - double* new_values = new double[new_max_num_nonzeros]; + std::unique_ptr<int[]> new_rows = + std::make_unique<int[]>(new_max_num_nonzeros); + std::unique_ptr<int[]> new_cols = + std::make_unique<int[]>(new_max_num_nonzeros); + std::unique_ptr<double[]> new_values = + std::make_unique<double[]>(new_max_num_nonzeros); for (int i = 0; i < num_nonzeros_; ++i) { new_rows[i] = rows_[i]; @@ -133,10 +137,9 @@ void TripletSparseMatrix::Reserve(int new_max_num_nonzeros) { new_values[i] = values_[i]; } - rows_.reset(new_rows); - cols_.reset(new_cols); - values_.reset(new_values); - + rows_ = std::move(new_rows); + cols_ = std::move(new_cols); + values_ = std::move(new_values); max_num_nonzeros_ = new_max_num_nonzeros; } @@ -152,9 +155,9 @@ void TripletSparseMatrix::set_num_nonzeros(int num_nonzeros) { } void TripletSparseMatrix::AllocateMemory() { - rows_.reset(new int[max_num_nonzeros_]); - cols_.reset(new int[max_num_nonzeros_]); - values_.reset(new double[max_num_nonzeros_]); + rows_ = std::make_unique<int[]>(max_num_nonzeros_); + cols_ = std::make_unique<int[]>(max_num_nonzeros_); + values_ = std::make_unique<double[]>(max_num_nonzeros_); } void TripletSparseMatrix::CopyData(const TripletSparseMatrix& orig) { @@ -252,10 +255,11 @@ void TripletSparseMatrix::Resize(int new_num_rows, int new_num_cols) { num_nonzeros_ -= dropped_terms; } -TripletSparseMatrix* TripletSparseMatrix::CreateSparseDiagonalMatrix( - const double* values, int num_rows) { - TripletSparseMatrix* m = - new TripletSparseMatrix(num_rows, num_rows, num_rows); +std::unique_ptr<TripletSparseMatrix> +TripletSparseMatrix::CreateSparseDiagonalMatrix(const double* values, + int num_rows) { + std::unique_ptr<TripletSparseMatrix> m = + std::make_unique<TripletSparseMatrix>(num_rows, num_rows, num_rows); for (int i = 0; i < num_rows; ++i) { m->mutable_rows()[i] = i; m->mutable_cols()[i] = i; @@ -272,7 +276,7 @@ void TripletSparseMatrix::ToTextFile(FILE* file) const { } } -TripletSparseMatrix* TripletSparseMatrix::CreateRandomMatrix( +std::unique_ptr<TripletSparseMatrix> TripletSparseMatrix::CreateRandomMatrix( const TripletSparseMatrix::RandomMatrixOptions& options) { CHECK_GT(options.num_rows, 0); CHECK_GT(options.num_cols, 0); @@ -297,7 +301,7 @@ TripletSparseMatrix* TripletSparseMatrix::CreateRandomMatrix( } } - return new TripletSparseMatrix( + return std::make_unique<TripletSparseMatrix>( options.num_rows, options.num_cols, rows, cols, values); } diff --git a/extern/ceres/internal/ceres/triplet_sparse_matrix.h b/extern/ceres/internal/ceres/triplet_sparse_matrix.h index cc9fee572a2..065c690dba3 100644 --- a/extern/ceres/internal/ceres/triplet_sparse_matrix.h +++ b/extern/ceres/internal/ceres/triplet_sparse_matrix.h @@ -34,8 +34,9 @@ #include <memory> #include <vector> +#include "ceres/internal/disable_warnings.h" #include "ceres/internal/eigen.h" -#include "ceres/internal/port.h" +#include "ceres/internal/export.h" #include "ceres/sparse_matrix.h" #include "ceres/types.h" @@ -46,7 +47,7 @@ namespace internal { // manipulate sparse matrices in triplet (i,j,s) form. This object is // inspired by the design of the cholmod_triplet struct used in the // SuiteSparse package and is memory layout compatible with it. -class CERES_EXPORT_INTERNAL TripletSparseMatrix : public SparseMatrix { +class CERES_NO_EXPORT TripletSparseMatrix final : public SparseMatrix { public: TripletSparseMatrix(); TripletSparseMatrix(int num_rows, int num_cols, int max_num_nonzeros); @@ -56,11 +57,11 @@ class CERES_EXPORT_INTERNAL TripletSparseMatrix : public SparseMatrix { const std::vector<int>& cols, const std::vector<double>& values); - explicit TripletSparseMatrix(const TripletSparseMatrix& orig); + TripletSparseMatrix(const TripletSparseMatrix& orig); TripletSparseMatrix& operator=(const TripletSparseMatrix& rhs); - virtual ~TripletSparseMatrix(); + ~TripletSparseMatrix() override; // Implementation of the SparseMatrix interface. void SetZero() final; @@ -115,8 +116,8 @@ class CERES_EXPORT_INTERNAL TripletSparseMatrix : public SparseMatrix { // Build a sparse diagonal matrix of size num_rows x num_rows from // the array values. Entries of the values array are copied into the // sparse matrix. - static TripletSparseMatrix* CreateSparseDiagonalMatrix(const double* values, - int num_rows); + static std::unique_ptr<TripletSparseMatrix> CreateSparseDiagonalMatrix( + const double* values, int num_rows); // Options struct to control the generation of random // TripletSparseMatrix objects. @@ -132,9 +133,7 @@ class CERES_EXPORT_INTERNAL TripletSparseMatrix : public SparseMatrix { // Create a random CompressedRowSparseMatrix whose entries are // normally distributed and whose structure is determined by // RandomMatrixOptions. - // - // Caller owns the result. - static TripletSparseMatrix* CreateRandomMatrix( + static std::unique_ptr<TripletSparseMatrix> CreateRandomMatrix( const TripletSparseMatrix::RandomMatrixOptions& options); private: @@ -158,4 +157,6 @@ class CERES_EXPORT_INTERNAL TripletSparseMatrix : public SparseMatrix { } // namespace internal } // namespace ceres +#include "ceres/internal/reenable_warnings.h" + #endif // CERES_INTERNAL_TRIPLET_SPARSE_MATRIX_H__ diff --git a/extern/ceres/internal/ceres/trust_region_minimizer.cc b/extern/ceres/internal/ceres/trust_region_minimizer.cc index bcf05b3ddfb..9ef5167ba6c 100644 --- a/extern/ceres/internal/ceres/trust_region_minimizer.cc +++ b/extern/ceres/internal/ceres/trust_region_minimizer.cc @@ -62,8 +62,6 @@ namespace ceres { namespace internal { -TrustRegionMinimizer::~TrustRegionMinimizer() {} - void TrustRegionMinimizer::Minimize(const Minimizer::Options& options, double* parameters, Solver::Summary* solver_summary) { @@ -75,11 +73,11 @@ void TrustRegionMinimizer::Minimize(const Minimizer::Options& options, // Create the TrustRegionStepEvaluator. The construction needs to be // delayed to this point because we need the cost for the starting // point to initialize the step evaluator. - step_evaluator_.reset(new TrustRegionStepEvaluator( + step_evaluator_ = std::make_unique<TrustRegionStepEvaluator>( x_cost_, options_.use_nonmonotonic_steps ? options_.max_consecutive_nonmonotonic_steps - : 0)); + : 0); while (FinalizeIterationAndCheckIfMinimizerCanContinue()) { iteration_start_time_in_secs_ = WallTimeInSeconds(); @@ -750,14 +748,12 @@ bool TrustRegionMinimizer::FunctionToleranceReached() { // Compute candidate_x_ = Plus(x_, delta_) // Evaluate the cost of candidate_x_ as candidate_cost_. // -// Failure to compute the step or the cost mean that candidate_cost_ -// is set to std::numeric_limits<double>::max(). Unlike -// EvaluateGradientAndJacobian, failure in this function is not fatal -// as we are only computing and evaluating a candidate point, and if -// for some reason we are unable to evaluate it, we consider it to be -// a point with very high cost. This allows the user to deal with edge -// cases/constraints as part of the LocalParameterization and -// CostFunction objects. +// Failure to compute the step or the cost mean that candidate_cost_ is set to +// std::numeric_limits<double>::max(). Unlike EvaluateGradientAndJacobian, +// failure in this function is not fatal as we are only computing and evaluating +// a candidate point, and if for some reason we are unable to evaluate it, we +// consider it to be a point with very high cost. This allows the user to deal +// with edge cases/constraints as part of the Manifold and CostFunction objects. void TrustRegionMinimizer::ComputeCandidatePointAndEvaluateCost() { if (!evaluator_->Plus(x_.data(), delta_.data(), candidate_x_.data())) { if (is_not_silent_) { diff --git a/extern/ceres/internal/ceres/trust_region_minimizer.h b/extern/ceres/internal/ceres/trust_region_minimizer.h index be4d40653c4..c6fc542a063 100644 --- a/extern/ceres/internal/ceres/trust_region_minimizer.h +++ b/extern/ceres/internal/ceres/trust_region_minimizer.h @@ -33,8 +33,9 @@ #include <memory> +#include "ceres/internal/disable_warnings.h" #include "ceres/internal/eigen.h" -#include "ceres/internal/port.h" +#include "ceres/internal/export.h" #include "ceres/minimizer.h" #include "ceres/solver.h" #include "ceres/sparse_matrix.h" @@ -48,10 +49,8 @@ namespace internal { // Generic trust region minimization algorithm. // // For example usage, see SolverImpl::Minimize. -class CERES_EXPORT_INTERNAL TrustRegionMinimizer : public Minimizer { +class CERES_NO_EXPORT TrustRegionMinimizer final : public Minimizer { public: - ~TrustRegionMinimizer(); - // This method is not thread safe. void Minimize(const Minimizer::Options& options, double* parameters, @@ -164,4 +163,6 @@ class CERES_EXPORT_INTERNAL TrustRegionMinimizer : public Minimizer { } // namespace internal } // namespace ceres +#include "ceres/internal/reenable_warnings.h" + #endif // CERES_INTERNAL_TRUST_REGION_MINIMIZER_H_ diff --git a/extern/ceres/internal/ceres/trust_region_preprocessor.cc b/extern/ceres/internal/ceres/trust_region_preprocessor.cc index 0943edbba85..edba47d88a5 100644 --- a/extern/ceres/internal/ceres/trust_region_preprocessor.cc +++ b/extern/ceres/internal/ceres/trust_region_preprocessor.cc @@ -55,13 +55,14 @@ using std::vector; namespace { -ParameterBlockOrdering* CreateDefaultLinearSolverOrdering( +std::shared_ptr<ParameterBlockOrdering> CreateDefaultLinearSolverOrdering( const Program& program) { - ParameterBlockOrdering* ordering = new ParameterBlockOrdering; + std::shared_ptr<ParameterBlockOrdering> ordering = + std::make_shared<ParameterBlockOrdering>(); const vector<ParameterBlock*>& parameter_blocks = program.parameter_blocks(); - for (int i = 0; i < parameter_blocks.size(); ++i) { + for (auto* parameter_block : parameter_blocks) { ordering->AddElementToGroup( - const_cast<double*>(parameter_blocks[i]->user_state()), 0); + const_cast<double*>(parameter_block->user_state()), 0); } return ordering; } @@ -160,8 +161,8 @@ bool SetupLinearSolver(PreprocessedProblem* pp) { // assume that they are giving all the freedom to us in choosing // the best possible ordering. This intent can be indicated by // putting all the parameter blocks in the same elimination group. - options.linear_solver_ordering.reset( - CreateDefaultLinearSolverOrdering(*pp->reduced_program)); + options.linear_solver_ordering = + CreateDefaultLinearSolverOrdering(*pp->reduced_program); } else { // If the user supplied an ordering, then check if the first // elimination group is still non-empty after the reduced problem @@ -247,7 +248,7 @@ bool SetupLinearSolver(PreprocessedProblem* pp) { } } - pp->linear_solver.reset(LinearSolver::Create(pp->linear_solver_options)); + pp->linear_solver = LinearSolver::Create(pp->linear_solver_options); return (pp->linear_solver != nullptr); } @@ -269,8 +270,8 @@ bool SetupEvaluator(PreprocessedProblem* pp) { pp->evaluator_options.context = pp->problem->context(); pp->evaluator_options.evaluation_callback = pp->reduced_program->mutable_evaluation_callback(); - pp->evaluator.reset(Evaluator::Create( - pp->evaluator_options, pp->reduced_program.get(), &pp->error)); + pp->evaluator = Evaluator::Create( + pp->evaluator_options, pp->reduced_program.get(), &pp->error); return (pp->evaluator != nullptr); } @@ -316,12 +317,12 @@ bool SetupInnerIterationMinimizer(PreprocessedProblem* pp) { } } else { // The user did not supply an ordering, so create one. - options.inner_iteration_ordering.reset( - CoordinateDescentMinimizer::CreateOrdering(*pp->reduced_program)); + options.inner_iteration_ordering = + CoordinateDescentMinimizer::CreateOrdering(*pp->reduced_program); } - pp->inner_iteration_minimizer.reset( - new CoordinateDescentMinimizer(pp->problem->context())); + pp->inner_iteration_minimizer = + std::make_unique<CoordinateDescentMinimizer>(pp->problem->context()); return pp->inner_iteration_minimizer->Init(*pp->reduced_program, pp->problem->parameter_map(), *options.inner_iteration_ordering, @@ -335,7 +336,7 @@ void SetupMinimizerOptions(PreprocessedProblem* pp) { SetupCommonMinimizerOptions(pp); pp->minimizer_options.is_constrained = pp->reduced_program->IsBoundsConstrained(); - pp->minimizer_options.jacobian.reset(pp->evaluator->CreateJacobian()); + pp->minimizer_options.jacobian = pp->evaluator->CreateJacobian(); pp->minimizer_options.inner_iteration_minimizer = pp->inner_iteration_minimizer; @@ -348,15 +349,13 @@ void SetupMinimizerOptions(PreprocessedProblem* pp) { strategy_options.trust_region_strategy_type = options.trust_region_strategy_type; strategy_options.dogleg_type = options.dogleg_type; - pp->minimizer_options.trust_region_strategy.reset( - TrustRegionStrategy::Create(strategy_options)); + pp->minimizer_options.trust_region_strategy = + TrustRegionStrategy::Create(strategy_options); CHECK(pp->minimizer_options.trust_region_strategy != nullptr); } } // namespace -TrustRegionPreprocessor::~TrustRegionPreprocessor() {} - bool TrustRegionPreprocessor::Preprocess(const Solver::Options& options, ProblemImpl* problem, PreprocessedProblem* pp) { @@ -370,10 +369,10 @@ bool TrustRegionPreprocessor::Preprocess(const Solver::Options& options, return false; } - pp->reduced_program.reset(program->CreateReducedProgram( - &pp->removed_parameter_blocks, &pp->fixed_cost, &pp->error)); + pp->reduced_program = program->CreateReducedProgram( + &pp->removed_parameter_blocks, &pp->fixed_cost, &pp->error); - if (pp->reduced_program.get() == NULL) { + if (pp->reduced_program.get() == nullptr) { return false; } diff --git a/extern/ceres/internal/ceres/trust_region_preprocessor.h b/extern/ceres/internal/ceres/trust_region_preprocessor.h index 2655abe4b2e..26ef8fad37d 100644 --- a/extern/ceres/internal/ceres/trust_region_preprocessor.h +++ b/extern/ceres/internal/ceres/trust_region_preprocessor.h @@ -31,15 +31,15 @@ #ifndef CERES_INTERNAL_TRUST_REGION_PREPROCESSOR_H_ #define CERES_INTERNAL_TRUST_REGION_PREPROCESSOR_H_ -#include "ceres/internal/port.h" +#include "ceres/internal/disable_warnings.h" +#include "ceres/internal/export.h" #include "ceres/preprocessor.h" namespace ceres { namespace internal { -class CERES_EXPORT_INTERNAL TrustRegionPreprocessor : public Preprocessor { +class CERES_NO_EXPORT TrustRegionPreprocessor final : public Preprocessor { public: - virtual ~TrustRegionPreprocessor(); bool Preprocess(const Solver::Options& options, ProblemImpl* problem, PreprocessedProblem* preprocessed_problem) override; @@ -48,4 +48,6 @@ class CERES_EXPORT_INTERNAL TrustRegionPreprocessor : public Preprocessor { } // namespace internal } // namespace ceres +#include "ceres/internal/reenable_warnings.h" + #endif // CERES_INTERNAL_TRUST_REGION_PREPROCESSOR_H_ diff --git a/extern/ceres/internal/ceres/trust_region_step_evaluator.h b/extern/ceres/internal/ceres/trust_region_step_evaluator.h index 03c00362dac..8e0c4e91f49 100644 --- a/extern/ceres/internal/ceres/trust_region_step_evaluator.h +++ b/extern/ceres/internal/ceres/trust_region_step_evaluator.h @@ -31,6 +31,8 @@ #ifndef CERES_INTERNAL_TRUST_REGION_STEP_EVALUATOR_H_ #define CERES_INTERNAL_TRUST_REGION_STEP_EVALUATOR_H_ +#include "ceres/internal/export.h" + namespace ceres { namespace internal { @@ -74,7 +76,7 @@ namespace internal { // x = x + delta; // step_evaluator->StepAccepted(cost, model_cost_change); // } -class TrustRegionStepEvaluator { +class CERES_NO_EXPORT TrustRegionStepEvaluator { public: // initial_cost is as the name implies the cost of the starting // state of the trust region minimizer. diff --git a/extern/ceres/internal/ceres/trust_region_strategy.cc b/extern/ceres/internal/ceres/trust_region_strategy.cc index 7e429d5e557..1096cd3c8aa 100644 --- a/extern/ceres/internal/ceres/trust_region_strategy.cc +++ b/extern/ceres/internal/ceres/trust_region_strategy.cc @@ -32,20 +32,23 @@ #include "ceres/trust_region_strategy.h" +#include <memory> + #include "ceres/dogleg_strategy.h" #include "ceres/levenberg_marquardt_strategy.h" namespace ceres { namespace internal { -TrustRegionStrategy::~TrustRegionStrategy() {} +TrustRegionStrategy::~TrustRegionStrategy() = default; -TrustRegionStrategy* TrustRegionStrategy::Create(const Options& options) { +std::unique_ptr<TrustRegionStrategy> TrustRegionStrategy::Create( + const Options& options) { switch (options.trust_region_strategy_type) { case LEVENBERG_MARQUARDT: - return new LevenbergMarquardtStrategy(options); + return std::make_unique<LevenbergMarquardtStrategy>(options); case DOGLEG: - return new DoglegStrategy(options); + return std::make_unique<DoglegStrategy>(options); default: LOG(FATAL) << "Unknown trust region strategy: " << options.trust_region_strategy_type; @@ -53,7 +56,7 @@ TrustRegionStrategy* TrustRegionStrategy::Create(const Options& options) { LOG(FATAL) << "Unknown trust region strategy: " << options.trust_region_strategy_type; - return NULL; + return nullptr; } } // namespace internal diff --git a/extern/ceres/internal/ceres/trust_region_strategy.h b/extern/ceres/internal/ceres/trust_region_strategy.h index 176f73a4876..33086cafb52 100644 --- a/extern/ceres/internal/ceres/trust_region_strategy.h +++ b/extern/ceres/internal/ceres/trust_region_strategy.h @@ -31,9 +31,11 @@ #ifndef CERES_INTERNAL_TRUST_REGION_STRATEGY_H_ #define CERES_INTERNAL_TRUST_REGION_STRATEGY_H_ +#include <memory> #include <string> -#include "ceres/internal/port.h" +#include "ceres/internal/disable_warnings.h" +#include "ceres/internal/export.h" #include "ceres/linear_solver.h" namespace ceres { @@ -54,7 +56,7 @@ class SparseMatrix; // the LevenbergMarquardtStrategy uses the inverse of the trust region // radius to scale the damping term, which controls the step size, but // does not set a hard limit on its size. -class CERES_EXPORT_INTERNAL TrustRegionStrategy { +class CERES_NO_EXPORT TrustRegionStrategy { public: struct Options { TrustRegionStrategyType trust_region_strategy_type = LEVENBERG_MARQUARDT; @@ -75,7 +77,7 @@ class CERES_EXPORT_INTERNAL TrustRegionStrategy { }; // Factory. - static TrustRegionStrategy* Create(const Options& options); + static std::unique_ptr<TrustRegionStrategy> Create(const Options& options); virtual ~TrustRegionStrategy(); @@ -142,4 +144,6 @@ class CERES_EXPORT_INTERNAL TrustRegionStrategy { } // namespace internal } // namespace ceres +#include "ceres/internal/reenable_warnings.h" + #endif // CERES_INTERNAL_TRUST_REGION_STRATEGY_H_ diff --git a/extern/ceres/internal/ceres/types.cc b/extern/ceres/internal/ceres/types.cc index 39bb2d8cc4d..48242678b46 100644 --- a/extern/ceres/internal/ceres/types.cc +++ b/extern/ceres/internal/ceres/types.cc @@ -34,6 +34,7 @@ #include <cctype> #include <string> +#include "ceres/internal/config.h" #include "glog/logging.h" namespace ceres { @@ -128,6 +129,7 @@ const char* DenseLinearAlgebraLibraryTypeToString( switch (type) { CASESTR(EIGEN); CASESTR(LAPACK); + CASESTR(CUDA); default: return "UNKNOWN"; } @@ -138,6 +140,7 @@ bool StringToDenseLinearAlgebraLibraryType( UpperCase(&value); STRENUM(EIGEN); STRENUM(LAPACK); + STRENUM(CUDA); return false; } @@ -417,6 +420,7 @@ bool IsDenseLinearAlgebraLibraryTypeAvailable( if (type == EIGEN) { return true; } + if (type == LAPACK) { #ifdef CERES_NO_LAPACK return false; @@ -425,6 +429,14 @@ bool IsDenseLinearAlgebraLibraryTypeAvailable( #endif } + if (type == CUDA) { +#ifdef CERES_NO_CUDA + return false; +#else + return true; +#endif + } + LOG(WARNING) << "Unknown dense linear algebra library " << type; return false; } diff --git a/extern/ceres/internal/ceres/visibility.cc b/extern/ceres/internal/ceres/visibility.cc index 82bf6f170b8..f666ce0c4bb 100644 --- a/extern/ceres/internal/ceres/visibility.cc +++ b/extern/ceres/internal/ceres/visibility.cc @@ -1,5 +1,5 @@ // Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2015 Google Inc. All rights reserved. +// Copyright 2022 Google Inc. All rights reserved. // http://ceres-solver.org/ // // Redistribution and use in source and binary forms, with or without @@ -33,6 +33,7 @@ #include <algorithm> #include <cmath> #include <ctime> +#include <memory> #include <set> #include <unordered_map> #include <utility> @@ -62,8 +63,8 @@ void ComputeVisibility(const CompressedRowBlockStructure& block_structure, visibility->resize(0); visibility->resize(block_structure.cols.size() - num_eliminate_blocks); - for (int i = 0; i < block_structure.rows.size(); ++i) { - const vector<Cell>& cells = block_structure.rows[i].cells; + for (const auto& row : block_structure.rows) { + const vector<Cell>& cells = row.cells; int block_id = cells[0].block_id; // If the first block is not an e_block, then skip this row block. if (block_id >= num_eliminate_blocks) { @@ -79,16 +80,16 @@ void ComputeVisibility(const CompressedRowBlockStructure& block_structure, } } -WeightedGraph<int>* CreateSchurComplementGraph( +std::unique_ptr<WeightedGraph<int>> CreateSchurComplementGraph( const vector<set<int>>& visibility) { - const time_t start_time = time(NULL); + const time_t start_time = time(nullptr); // Compute the number of e_blocks/point blocks. Since the visibility // set for each e_block/camera contains the set of e_blocks/points // visible to it, we find the maximum across all visibility sets. int num_points = 0; - for (int i = 0; i < visibility.size(); i++) { - if (visibility[i].size() > 0) { - num_points = max(num_points, (*visibility[i].rbegin()) + 1); + for (const auto& visible : visibility) { + if (!visible.empty()) { + num_points = max(num_points, (*visible.rbegin()) + 1); } } @@ -100,7 +101,7 @@ WeightedGraph<int>* CreateSchurComplementGraph( vector<set<int>> inverse_visibility(num_points); for (int i = 0; i < visibility.size(); i++) { const set<int>& visibility_set = visibility[i]; - for (const int v : visibility_set) { + for (int v : visibility_set) { inverse_visibility[v].insert(i); } } @@ -111,17 +112,17 @@ WeightedGraph<int>* CreateSchurComplementGraph( // Count the number of points visible to each camera/f_block pair. for (const auto& inverse_visibility_set : inverse_visibility) { - for (set<int>::const_iterator camera1 = inverse_visibility_set.begin(); + for (auto camera1 = inverse_visibility_set.begin(); camera1 != inverse_visibility_set.end(); ++camera1) { - set<int>::const_iterator camera2 = camera1; + auto camera2 = camera1; for (++camera2; camera2 != inverse_visibility_set.end(); ++camera2) { ++(camera_pairs[make_pair(*camera1, *camera2)]); } } } - WeightedGraph<int>* graph = new WeightedGraph<int>; + auto graph = std::make_unique<WeightedGraph<int>>(); // Add vertices and initialize the pairs for self edges so that self // edges are guaranteed. This is needed for the Canonical views @@ -146,7 +147,7 @@ WeightedGraph<int>* CreateSchurComplementGraph( graph->AddEdge(camera1, camera2, weight); } - VLOG(2) << "Schur complement graph time: " << (time(NULL) - start_time); + VLOG(2) << "Schur complement graph time: " << (time(nullptr) - start_time); return graph; } diff --git a/extern/ceres/internal/ceres/visibility.h b/extern/ceres/internal/ceres/visibility.h index 68c6723fad7..d8f6968d98f 100644 --- a/extern/ceres/internal/ceres/visibility.h +++ b/extern/ceres/internal/ceres/visibility.h @@ -35,11 +35,13 @@ #ifndef CERES_INTERNAL_VISIBILITY_H_ #define CERES_INTERNAL_VISIBILITY_H_ +#include <memory> #include <set> #include <vector> #include "ceres/graph.h" -#include "ceres/internal/port.h" +#include "ceres/internal/disable_warnings.h" +#include "ceres/internal/export.h" namespace ceres { namespace internal { @@ -54,7 +56,7 @@ struct CompressedRowBlockStructure; // // In a structure from motion problem, e_blocks correspond to 3D // points and f_blocks correspond to cameras. -CERES_EXPORT_INTERNAL void ComputeVisibility( +CERES_NO_EXPORT void ComputeVisibility( const CompressedRowBlockStructure& block_structure, int num_eliminate_blocks, std::vector<std::set<int>>* visibility); @@ -72,10 +74,12 @@ CERES_EXPORT_INTERNAL void ComputeVisibility( // // Caller acquires ownership of the returned WeightedGraph pointer // (heap-allocated). -CERES_EXPORT_INTERNAL WeightedGraph<int>* CreateSchurComplementGraph( +CERES_NO_EXPORT std::unique_ptr<WeightedGraph<int>> CreateSchurComplementGraph( const std::vector<std::set<int>>& visibility); } // namespace internal } // namespace ceres +#include "ceres/internal/reenable_warnings.h" + #endif // CERES_INTERNAL_VISIBILITY_H_ diff --git a/extern/ceres/internal/ceres/visibility_based_preconditioner.cc b/extern/ceres/internal/ceres/visibility_based_preconditioner.cc index 0cf4afaae06..831a8663027 100644 --- a/extern/ceres/internal/ceres/visibility_based_preconditioner.cc +++ b/extern/ceres/internal/ceres/visibility_based_preconditioner.cc @@ -1,5 +1,5 @@ // Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2015 Google Inc. All rights reserved. +// Copyright 2022 Google Inc. All rights reserved. // http://ceres-solver.org/ // // Redistribution and use in source and binary forms, with or without @@ -70,9 +70,8 @@ static constexpr double kCanonicalViewsSimilarityPenaltyWeight = 0.0; static constexpr double kSingleLinkageMinSimilarity = 0.9; VisibilityBasedPreconditioner::VisibilityBasedPreconditioner( - const CompressedRowBlockStructure& bs, - const Preconditioner::Options& options) - : options_(options), num_blocks_(0), num_clusters_(0) { + const CompressedRowBlockStructure& bs, Preconditioner::Options options) + : options_(std::move(options)), num_blocks_(0), num_clusters_(0) { CHECK_GT(options_.elimination_groups.size(), 1); CHECK_GT(options_.elimination_groups[0], 0); CHECK(options_.type == CLUSTER_JACOBI || options_.type == CLUSTER_TRIDIAGONAL) @@ -80,7 +79,7 @@ VisibilityBasedPreconditioner::VisibilityBasedPreconditioner( num_blocks_ = bs.cols.size() - options_.elimination_groups[0]; CHECK_GT(num_blocks_, 0) << "Jacobian should have at least 1 f_block for " << "visibility based preconditioning."; - CHECK(options_.context != NULL); + CHECK(options_.context != nullptr); // Vector of camera block sizes block_size_.resize(num_blocks_); @@ -88,7 +87,7 @@ VisibilityBasedPreconditioner::VisibilityBasedPreconditioner( block_size_[i] = bs.cols[i + options_.elimination_groups[0]].size; } - const time_t start_time = time(NULL); + const time_t start_time = time(nullptr); switch (options_.type) { case CLUSTER_JACOBI: ComputeClusterJacobiSparsity(bs); @@ -99,11 +98,11 @@ VisibilityBasedPreconditioner::VisibilityBasedPreconditioner( default: LOG(FATAL) << "Unknown preconditioner type"; } - const time_t structure_time = time(NULL); + const time_t structure_time = time(nullptr); InitStorage(bs); - const time_t storage_time = time(NULL); + const time_t storage_time = time(nullptr); InitEliminator(bs); - const time_t eliminator_time = time(NULL); + const time_t eliminator_time = time(nullptr); LinearSolver::Options sparse_cholesky_options; sparse_cholesky_options.sparse_linear_algebra_library_type = @@ -118,14 +117,14 @@ VisibilityBasedPreconditioner::VisibilityBasedPreconditioner( sparse_cholesky_options.use_postordering = true; sparse_cholesky_ = SparseCholesky::Create(sparse_cholesky_options); - const time_t init_time = time(NULL); + const time_t init_time = time(nullptr); VLOG(2) << "init time: " << init_time - start_time << " structure time: " << structure_time - start_time << " storage time:" << storage_time - structure_time << " eliminator time: " << eliminator_time - storage_time; } -VisibilityBasedPreconditioner::~VisibilityBasedPreconditioner() {} +VisibilityBasedPreconditioner::~VisibilityBasedPreconditioner() = default; // Determine the sparsity structure of the CLUSTER_JACOBI // preconditioner. It clusters cameras using their scene @@ -162,11 +161,9 @@ void VisibilityBasedPreconditioner::ComputeClusterTridiagonalSparsity( // maximum spanning forest of this graph. vector<set<int>> cluster_visibility; ComputeClusterVisibility(visibility, &cluster_visibility); - std::unique_ptr<WeightedGraph<int>> cluster_graph( - CreateClusterGraph(cluster_visibility)); + auto cluster_graph = CreateClusterGraph(cluster_visibility); CHECK(cluster_graph != nullptr); - std::unique_ptr<WeightedGraph<int>> forest( - Degree2MaximumSpanningForest(*cluster_graph)); + auto forest = Degree2MaximumSpanningForest(*cluster_graph); CHECK(forest != nullptr); ForestToClusterPairs(*forest, &cluster_pairs_); } @@ -175,7 +172,8 @@ void VisibilityBasedPreconditioner::ComputeClusterTridiagonalSparsity( void VisibilityBasedPreconditioner::InitStorage( const CompressedRowBlockStructure& bs) { ComputeBlockPairsInPreconditioner(bs); - m_.reset(new BlockRandomAccessSparseMatrix(block_size_, block_pairs_)); + m_ = std::make_unique<BlockRandomAccessSparseMatrix>(block_size_, + block_pairs_); } // Call the canonical views algorithm and cluster the cameras based on @@ -186,8 +184,7 @@ void VisibilityBasedPreconditioner::InitStorage( // memberships for each camera block. void VisibilityBasedPreconditioner::ClusterCameras( const vector<set<int>>& visibility) { - std::unique_ptr<WeightedGraph<int>> schur_complement_graph( - CreateSchurComplementGraph(visibility)); + auto schur_complement_graph = CreateSchurComplementGraph(visibility); CHECK(schur_complement_graph != nullptr); std::unordered_map<int, int> membership; @@ -285,14 +282,12 @@ void VisibilityBasedPreconditioner::ComputeBlockPairsInPreconditioner( } } - for (set<int>::const_iterator block1 = f_blocks.begin(); - block1 != f_blocks.end(); - ++block1) { - set<int>::const_iterator block2 = block1; + for (auto block1 = f_blocks.begin(); block1 != f_blocks.end(); ++block1) { + auto block2 = block1; ++block2; for (; block2 != f_blocks.end(); ++block2) { if (IsBlockPairInPreconditioner(*block1, *block2)) { - block_pairs_.insert(make_pair(*block1, *block2)); + block_pairs_.emplace(*block1, *block2); } } } @@ -304,8 +299,8 @@ void VisibilityBasedPreconditioner::ComputeBlockPairsInPreconditioner( CHECK_GE(row.cells.front().block_id, num_eliminate_blocks); for (int i = 0; i < row.cells.size(); ++i) { const int block1 = row.cells[i].block_id - num_eliminate_blocks; - for (int j = 0; j < row.cells.size(); ++j) { - const int block2 = row.cells[j].block_id - num_eliminate_blocks; + for (const auto& cell : row.cells) { + const int block2 = cell.block_id - num_eliminate_blocks; if (block1 <= block2) { if (IsBlockPairInPreconditioner(block1, block2)) { block_pairs_.insert(make_pair(block1, block2)); @@ -328,7 +323,7 @@ void VisibilityBasedPreconditioner::InitEliminator( eliminator_options.f_block_size = options_.f_block_size; eliminator_options.row_block_size = options_.row_block_size; eliminator_options.context = options_.context; - eliminator_.reset(SchurEliminatorBase::Create(eliminator_options)); + eliminator_ = SchurEliminatorBase::Create(eliminator_options); const bool kFullRankETE = true; eliminator_->Init( eliminator_options.elimination_groups[0], kFullRankETE, &bs); @@ -337,7 +332,7 @@ void VisibilityBasedPreconditioner::InitEliminator( // Update the values of the preconditioner matrix and factorize it. bool VisibilityBasedPreconditioner::UpdateImpl(const BlockSparseMatrix& A, const double* D) { - const time_t start_time = time(NULL); + const time_t start_time = time(nullptr); const int num_rows = m_->num_rows(); CHECK_GT(num_rows, 0); @@ -375,7 +370,7 @@ bool VisibilityBasedPreconditioner::UpdateImpl(const BlockSparseMatrix& A, status = Factorize(); } - VLOG(2) << "Compute time: " << time(NULL) - start_time; + VLOG(2) << "Compute time: " << time(nullptr) - start_time; return (status == LINEAR_SOLVER_SUCCESS); } @@ -395,7 +390,7 @@ void VisibilityBasedPreconditioner::ScaleOffDiagonalCells() { int r, c, row_stride, col_stride; CellInfo* cell_info = m_->GetCell(block1, block2, &r, &c, &row_stride, &col_stride); - CHECK(cell_info != NULL) + CHECK(cell_info != nullptr) << "Cell missing for block pair (" << block1 << "," << block2 << ")" << " cluster pair (" << cluster_membership_[block1] << " " << cluster_membership_[block2] << ")"; @@ -420,11 +415,10 @@ LinearSolverTerminationType VisibilityBasedPreconditioner::Factorize() { const CompressedRowSparseMatrix::StorageType storage_type = sparse_cholesky_->StorageType(); if (storage_type == CompressedRowSparseMatrix::UPPER_TRIANGULAR) { - lhs.reset(CompressedRowSparseMatrix::FromTripletSparseMatrix(*tsm)); + lhs = CompressedRowSparseMatrix::FromTripletSparseMatrix(*tsm); lhs->set_storage_type(CompressedRowSparseMatrix::UPPER_TRIANGULAR); } else { - lhs.reset( - CompressedRowSparseMatrix::FromTripletSparseMatrixTransposed(*tsm)); + lhs = CompressedRowSparseMatrix::FromTripletSparseMatrixTransposed(*tsm); lhs->set_storage_type(CompressedRowSparseMatrix::LOWER_TRIANGULAR); } @@ -503,9 +497,10 @@ void VisibilityBasedPreconditioner::ComputeClusterVisibility( // Construct a graph whose vertices are the clusters, and the edge // weights are the number of 3D points visible to cameras in both the // vertices. -WeightedGraph<int>* VisibilityBasedPreconditioner::CreateClusterGraph( +std::unique_ptr<WeightedGraph<int>> +VisibilityBasedPreconditioner::CreateClusterGraph( const vector<set<int>>& cluster_visibility) const { - WeightedGraph<int>* cluster_graph = new WeightedGraph<int>; + auto cluster_graph = std::make_unique<WeightedGraph<int>>(); for (int i = 0; i < num_clusters_; ++i) { cluster_graph->AddVertex(i); diff --git a/extern/ceres/internal/ceres/visibility_based_preconditioner.h b/extern/ceres/internal/ceres/visibility_based_preconditioner.h index 0457b9a376a..8079dc3f3ce 100644 --- a/extern/ceres/internal/ceres/visibility_based_preconditioner.h +++ b/extern/ceres/internal/ceres/visibility_based_preconditioner.h @@ -122,9 +122,10 @@ class SchurEliminatorBase; // options.elimination_groups.push_back(num_cameras); // VisibilityBasedPreconditioner preconditioner( // *A.block_structure(), options); -// preconditioner.Update(A, NULL); +// preconditioner.Update(A, nullptr); // preconditioner.RightMultiply(x, y); -class VisibilityBasedPreconditioner : public BlockSparseMatrixPreconditioner { +class CERES_NO_EXPORT VisibilityBasedPreconditioner + : public BlockSparseMatrixPreconditioner { public: // Initialize the symbolic structure of the preconditioner. bs is // the block structure of the linear system to be solved. It is used @@ -133,11 +134,11 @@ class VisibilityBasedPreconditioner : public BlockSparseMatrixPreconditioner { // It has the same structural requirement as other Schur complement // based solvers. Please see schur_eliminator.h for more details. VisibilityBasedPreconditioner(const CompressedRowBlockStructure& bs, - const Preconditioner::Options& options); + Preconditioner::Options options); VisibilityBasedPreconditioner(const VisibilityBasedPreconditioner&) = delete; void operator=(const VisibilityBasedPreconditioner&) = delete; - virtual ~VisibilityBasedPreconditioner(); + ~VisibilityBasedPreconditioner() override; // Preconditioner interface void RightMultiply(const double* x, double* y) const final; @@ -160,7 +161,7 @@ class VisibilityBasedPreconditioner : public BlockSparseMatrixPreconditioner { void ComputeClusterVisibility( const std::vector<std::set<int>>& visibility, std::vector<std::set<int>>* cluster_visibility) const; - WeightedGraph<int>* CreateClusterGraph( + std::unique_ptr<WeightedGraph<int>> CreateClusterGraph( const std::vector<std::set<int>>& visibility) const; void ForestToClusterPairs( const WeightedGraph<int>& forest, diff --git a/extern/ceres/internal/ceres/wall_time.cc b/extern/ceres/internal/ceres/wall_time.cc index 716392741e9..a54ab640b3e 100644 --- a/extern/ceres/internal/ceres/wall_time.cc +++ b/extern/ceres/internal/ceres/wall_time.cc @@ -30,6 +30,8 @@ #include "ceres/wall_time.h" +#include "ceres/internal/config.h" + #ifdef CERES_USE_OPENMP #include <omp.h> #else @@ -58,7 +60,7 @@ double WallTimeInSeconds() { static_cast<double>(frequency.QuadPart); #else timeval time_val; - gettimeofday(&time_val, NULL); + gettimeofday(&time_val, nullptr); return (time_val.tv_sec + time_val.tv_usec * 1e-6); #endif #endif diff --git a/extern/ceres/internal/ceres/wall_time.h b/extern/ceres/internal/ceres/wall_time.h index 9c92e9e60ef..f093eed0418 100644 --- a/extern/ceres/internal/ceres/wall_time.h +++ b/extern/ceres/internal/ceres/wall_time.h @@ -34,7 +34,8 @@ #include <map> #include <string> -#include "ceres/internal/port.h" +#include "ceres/internal/disable_warnings.h" +#include "ceres/internal/export.h" #include "ceres/stringprintf.h" #include "glog/logging.h" @@ -45,7 +46,7 @@ namespace internal { // OpenMP is available then the high precision openmp_get_wtime() // function is used. Otherwise on unixes, gettimeofday is used. The // granularity is in seconds on windows systems. -CERES_EXPORT_INTERNAL double WallTimeInSeconds(); +CERES_NO_EXPORT double WallTimeInSeconds(); // Log a series of events, recording for each event the time elapsed // since the last event and since the creation of the object. @@ -71,7 +72,7 @@ CERES_EXPORT_INTERNAL double WallTimeInSeconds(); // Bar1: time1 time1 // Bar2: time2 time1 + time2; // Total: time3 time1 + time2 + time3; -class EventLogger { +class CERES_NO_EXPORT EventLogger { public: explicit EventLogger(const std::string& logger_name); ~EventLogger(); @@ -86,4 +87,6 @@ class EventLogger { } // namespace internal } // namespace ceres +#include "ceres/internal/reenable_warnings.h" + #endif // CERES_INTERNAL_WALL_TIME_H_ diff --git a/extern/ceres/mkfiles.sh b/extern/ceres/mkfiles.sh deleted file mode 100755 index cb07663e94a..00000000000 --- a/extern/ceres/mkfiles.sh +++ /dev/null @@ -1,5 +0,0 @@ -#!/bin/sh - -find ./include/ -type f | sed -r 's/^\.\///' | sort > files.txt -find ./internal/ -type f | sed -r 's/^\.\///' | sort >> files.txt -find ./config/ -type f | sed -r 's/^\.\///' | sort >> files.txt diff --git a/extern/ceres/patches/series b/extern/ceres/patches/series deleted file mode 100644 index e69de29bb2d..00000000000 --- a/extern/ceres/patches/series +++ /dev/null diff --git a/intern/libmv/CMakeLists.txt b/intern/libmv/CMakeLists.txt index f9fef9f7a29..e0ed68eb20e 100644 --- a/intern/libmv/CMakeLists.txt +++ b/intern/libmv/CMakeLists.txt @@ -26,7 +26,6 @@ if(WITH_LIBMV) endif() add_definitions(${GFLAGS_DEFINES}) add_definitions(${GLOG_DEFINES}) - add_definitions(${CERES_DEFINES}) add_definitions(-DLIBMV_GFLAGS_NAMESPACE=${GFLAGS_NAMESPACE}) list(APPEND INC diff --git a/intern/libmv/bundle.sh b/intern/libmv/bundle.sh index 6808e244c05..82293068745 100755 --- a/intern/libmv/bundle.sh +++ b/intern/libmv/bundle.sh @@ -124,7 +124,6 @@ if(WITH_LIBMV) endif() add_definitions(\${GFLAGS_DEFINES}) add_definitions(\${GLOG_DEFINES}) - add_definitions(\${CERES_DEFINES}) add_definitions(-DLIBMV_GFLAGS_NAMESPACE=\${GFLAGS_NAMESPACE}) list(APPEND INC diff --git a/intern/libmv/libmv/simple_pipeline/bundle.cc b/intern/libmv/libmv/simple_pipeline/bundle.cc index e86c3bca57f..355c167d000 100644 --- a/intern/libmv/libmv/simple_pipeline/bundle.cc +++ b/intern/libmv/libmv/simple_pipeline/bundle.cc @@ -685,7 +685,7 @@ void EuclideanBundleCommonIntrinsics(const Tracks& tracks, PackCamerasRotationAndTranslation(*reconstruction); // Parameterization used to restrict camera motion for modal solvers. - ceres::SubsetParameterization* constant_translation_parameterization = NULL; + ceres::SubsetManifold* constant_translation_manifold = NULL; if (bundle_constraints & BUNDLE_NO_TRANSLATION) { std::vector<int> constant_translation; @@ -694,8 +694,8 @@ void EuclideanBundleCommonIntrinsics(const Tracks& tracks, constant_translation.push_back(4); constant_translation.push_back(5); - constant_translation_parameterization = - new ceres::SubsetParameterization(6, constant_translation); + constant_translation_manifold = + new ceres::SubsetManifold(6, constant_translation); } // Add residual blocks to the problem. @@ -735,8 +735,7 @@ void EuclideanBundleCommonIntrinsics(const Tracks& tracks, } if (bundle_constraints & BUNDLE_NO_TRANSLATION) { - problem.SetParameterization(current_camera_R_t, - constant_translation_parameterization); + problem.SetManifold(current_camera_R_t, constant_translation_manifold); } zero_weight_tracks_flags[marker.track] = false; @@ -787,11 +786,11 @@ void EuclideanBundleCommonIntrinsics(const Tracks& tracks, #undef MAYBE_SET_CONSTANT if (!constant_intrinsics.empty()) { - ceres::SubsetParameterization* subset_parameterization = - new ceres::SubsetParameterization(PackedIntrinsics::NUM_PARAMETERS, - constant_intrinsics); + ceres::SubsetManifold* subset_parameterization = + new ceres::SubsetManifold(PackedIntrinsics::NUM_PARAMETERS, + constant_intrinsics); - problem.SetParameterization(intrinsics_block, subset_parameterization); + problem.SetManifold(intrinsics_block, subset_parameterization); } } diff --git a/intern/libmv/libmv/simple_pipeline/modal_solver.cc b/intern/libmv/libmv/simple_pipeline/modal_solver.cc index 845b299e31e..206d264f1f8 100644 --- a/intern/libmv/libmv/simple_pipeline/modal_solver.cc +++ b/intern/libmv/libmv/simple_pipeline/modal_solver.cc @@ -180,7 +180,7 @@ void ModalSolver(const Tracks& tracks, // NOTE: Parameterization is lazily initialized when it is really needed, // and is re-used by all parameters block. - ceres::LocalParameterization* quaternion_parameterization = NULL; + ceres::Manifold* quaternion_manifold = NULL; int num_residuals = 0; for (int i = 0; i < all_markers.size(); ++i) { @@ -197,12 +197,11 @@ void ModalSolver(const Tracks& tracks, &quaternion(0)); num_residuals++; - if (quaternion_parameterization == NULL) { - quaternion_parameterization = new ceres::QuaternionParameterization(); + if (quaternion_manifold == NULL) { + quaternion_manifold = new ceres::QuaternionManifold(); } - problem.SetParameterization(&quaternion(0), - quaternion_parameterization); + problem.SetManifold(&quaternion(0), quaternion_manifold); } } |