├── CODEOWNERS ├── include └── uniform_bspline_ceres │ ├── internal │ ├── .gitignore │ ├── no_discard.hpp │ ├── uniform_bspline_ceres_impl.hpp │ ├── uniform_bspline_ceres_evaluator_impl.hpp │ ├── autodiff_smoothing_type.hpp │ └── smoothness_cost_functor.hpp │ ├── fixed_size_container_type_trait_ceres.hpp │ ├── value_type_trait_ceres.hpp │ ├── uniform_bspline_ceres_evaluator.hpp │ ├── uniform_bspline_ceres_generator.hpp │ └── uniform_bspline_ceres.hpp ├── Doxyfile ├── doc ├── 1d_smoothing.png ├── 2d_smoothing.png ├── 3d_smoothing.png ├── generator_example_spline.png ├── generator_example.nb ├── README_doxygen.md └── bspline_drawings.nb ├── package.xml ├── LICENSE ├── test ├── test_helper.hpp ├── generator_spline_autodiff.cpp ├── evaluator_example.cpp ├── generator_example.cpp ├── smoothness_prior.cpp ├── generator.cpp ├── benchmark.cpp ├── evaluator.cpp ├── smoothness.cpp └── evaluator_optimization.cpp ├── scripts └── generate_readme_md ├── CMakeLists.txt └── README.md /CODEOWNERS: -------------------------------------------------------------------------------- 1 | * johannes.beck@kit.edu 2 | -------------------------------------------------------------------------------- /include/uniform_bspline_ceres/internal/.gitignore: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /Doxyfile: -------------------------------------------------------------------------------- 1 | EXAMPLE_PATH = test 2 | IMAGE_PATH = ./ 3 | USE_MDFILE_AS_MAINPAGE = ./doc/README_doxygen.md 4 | -------------------------------------------------------------------------------- /doc/1d_smoothing.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/KIT-MRT/uniform_bspline_ceres/HEAD/doc/1d_smoothing.png -------------------------------------------------------------------------------- /doc/2d_smoothing.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/KIT-MRT/uniform_bspline_ceres/HEAD/doc/2d_smoothing.png -------------------------------------------------------------------------------- /doc/3d_smoothing.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/KIT-MRT/uniform_bspline_ceres/HEAD/doc/3d_smoothing.png -------------------------------------------------------------------------------- /doc/generator_example_spline.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/KIT-MRT/uniform_bspline_ceres/HEAD/doc/generator_example_spline.png -------------------------------------------------------------------------------- /include/uniform_bspline_ceres/internal/no_discard.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | // No discard macro to be compatible with pre c++17. 4 | 5 | #ifndef UBS_NO_DISCARD 6 | #if __cplusplus >= 201703L 7 | // NOLINTNEXTLINE(cppcoreguidelines-macro-usage) 8 | #define UBS_NO_DISCARD [[nodiscard]] 9 | #else 10 | #define UBS_NO_DISCARD 11 | #endif 12 | #endif 13 | -------------------------------------------------------------------------------- /include/uniform_bspline_ceres/fixed_size_container_type_trait_ceres.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | #include 5 | 6 | namespace ubs { 7 | 8 | template 9 | struct FixedSizeContainerTypeTrait> : public FixedSizeContainerTypeTraitDefault> { 10 | // The Jet type needs the Eigen aligned allocator as it contains a fixed size Eigen type. 11 | using Allocator = Eigen::aligned_allocator>; 12 | }; 13 | 14 | } // namespace ubs 15 | -------------------------------------------------------------------------------- /include/uniform_bspline_ceres/value_type_trait_ceres.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | namespace ubs { 7 | 8 | template 9 | struct ValueTypeTrait> { 10 | UBS_NO_DISCARD static int toInt(const ceres::Jet& val) { 11 | // To cast a Jet to int, use the value part. As this function is only used to get the segment index, the 12 | // gradient computation still works. 13 | return static_cast(val.a); 14 | } 15 | 16 | template 17 | UBS_NO_DISCARD static ceres::Jet pow(const ceres::Jet& val, T ex) { 18 | return ceres::pow(val, ex); 19 | } 20 | }; 21 | 22 | } // namespace ubs 23 | -------------------------------------------------------------------------------- /include/uniform_bspline_ceres/internal/uniform_bspline_ceres_impl.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | /** 4 | * @file 5 | * Definitions used for constexpr variables. This is only need for code pre C++17. 6 | */ 7 | #if not defined(__cplusplus) or __cplusplus < 201703L 8 | 9 | namespace ubs { 10 | 11 | template 12 | constexpr int UniformBSplineCeres::Degree; 13 | 14 | template 15 | constexpr int UniformBSplineCeres::Order; 16 | 17 | template 18 | constexpr int UniformBSplineCeres::InputDims; 19 | 20 | template 21 | constexpr int UniformBSplineCeres::OutputDims; 22 | 23 | template 24 | constexpr int UniformBSplineCeres::ControlPointsSupport; 25 | 26 | } // namespace ubs 27 | 28 | #endif 29 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | uniform_bspline_ceres 4 | 1.0.0 5 | A B-spline optimization library to optimize uniform B-spline with Ceres. 6 | 7 | MRT 8 | Johannes Beck 9 | Johannes Beck 10 | https://gitlab.mrt.uni-karlsruhe.de/MRT/develop/uniform_bspline_ceres 11 | 12 | catkin 13 | mrt_cmake_modules 14 | gtest 15 | benchmark 16 | 17 | uniform_bspline 18 | libceres-dev 19 | eigen 20 | 21 | -------------------------------------------------------------------------------- /include/uniform_bspline_ceres/internal/uniform_bspline_ceres_evaluator_impl.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | /** 4 | * @file 5 | * Definitions used for constexpr variables. This is only need for code pre C++17. 6 | */ 7 | #if not defined(__cplusplus) or __cplusplus < 201703L 8 | 9 | namespace ubs { 10 | 11 | template 12 | constexpr int UniformBSplineCeresEvaluator::Degree; 13 | 14 | template 15 | constexpr int UniformBSplineCeresEvaluator::Order; 16 | 17 | template 18 | constexpr int UniformBSplineCeresEvaluator::InputDims; 19 | 20 | template 21 | constexpr int UniformBSplineCeresEvaluator::OutputDims; 22 | 23 | template 24 | constexpr int UniformBSplineCeresEvaluator::ControlPointsSupport; 25 | 26 | } // namespace ubs 27 | 28 | #endif 29 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Boost Software License - Version 1.0 - August 17th, 2003 2 | 3 | Permission is hereby granted, free of charge, to any person or organization 4 | obtaining a copy of the software and accompanying documentation covered by 5 | this license (the "Software") to use, reproduce, display, distribute, 6 | execute, and transmit the Software, and to prepare derivative works of the 7 | Software, and to permit third-parties to whom the Software is furnished to 8 | do so, all subject to the following: 9 | 10 | The copyright notices in the Software and this entire statement, including 11 | the above license grant, this restriction and the following disclaimer, 12 | must be included in all copies of the Software, in whole or in part, and 13 | all derivative works of the Software, unless such copies or derivative 14 | works are solely in the form of machine-executable object code generated by 15 | a source language processor. 16 | 17 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 18 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 19 | FITNESS FOR A PARTICULAR PURPOSE, TITLE AND NON-INFRINGEMENT. IN NO EVENT 20 | SHALL THE COPYRIGHT HOLDERS OR ANYONE DISTRIBUTING THE SOFTWARE BE LIABLE 21 | FOR ANY DAMAGES OR OTHER LIABILITY, WHETHER IN CONTRACT, TORT OR OTHERWISE, 22 | ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER 23 | DEALINGS IN THE SOFTWARE. 24 | -------------------------------------------------------------------------------- /include/uniform_bspline_ceres/internal/autodiff_smoothing_type.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | 4 | #include 5 | 6 | /** 7 | * @file 8 | * Used to determine the Ceres auto differential smoothing type. 9 | * 10 | * The number of residuals is the spline order times the output dimensions and the parameter dimensions are the output 11 | * dimensions with the control points support. 12 | */ 13 | 14 | namespace ubs { 15 | namespace internal { 16 | 17 | template 18 | struct AutoDiffSmoothingImpl; 19 | 20 | template 21 | struct AutoDiffSmoothingImpl> { 22 | using Type = typename AutoDiffSmoothingImpl>::Type; 27 | }; 28 | 29 | template 30 | struct AutoDiffSmoothingImpl> { 31 | using Type = ceres::AutoDiffCostFunction; 32 | }; 33 | 34 | template 35 | using AutoDiffSmoothingType = 36 | typename AutoDiffSmoothingImpl<0, Spline::Order, Spline, CostFunctor, std::integer_sequence>::Type; 37 | 38 | } // namespace internal 39 | } // namespace ubs -------------------------------------------------------------------------------- /test/test_helper.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | #include 8 | 9 | namespace test_util { 10 | namespace internal { 11 | template 12 | struct LinspaceImpl { 13 | template 14 | static void apply(const std::array& counts, Eigen::Matrix& val, Func func) { 15 | for (int i = 0; i < counts[CurDim_]; ++i) { 16 | val[CurDim_] = double(i) / (counts[CurDim_] - 1); 17 | LinspaceImpl::apply(counts, val, func); 18 | } 19 | } 20 | }; 21 | 22 | template 23 | struct LinspaceImpl { 24 | template 25 | static void apply(const std::array& /*counts*/, Eigen::Matrix& val, Func func) { 26 | func(val); 27 | } 28 | }; 29 | } // namespace internal 30 | 31 | template 32 | void linspace(const std::array& counts, Func func) { 33 | Eigen::Matrix val; 34 | internal::LinspaceImpl<0, N>::apply(counts, val, func); 35 | } 36 | 37 | namespace internal { 38 | template 39 | constexpr decltype(auto) applyImpl(F&& f, Tuple&& t, std::index_sequence /*indexSequence*/) { 40 | return f(std::get(std::forward(t))...); 41 | } 42 | } // namespace internal 43 | 44 | template 45 | constexpr decltype(auto) apply(F&& f, Tuple&& t) { 46 | return internal::applyImpl(std::forward(f), 47 | std::forward(t), 48 | std::make_index_sequence>::value>{}); 49 | } 50 | 51 | namespace internal { 52 | template 53 | inline auto asTuple(std::array const& arr, std::index_sequence /*indexSequence*/) { 54 | return std::make_tuple(arr[Is]...); 55 | } 56 | } // namespace internal 57 | 58 | template 59 | inline auto asTuple(std::array const& arr) { 60 | return internal::asTuple(arr, std::make_index_sequence{}); 61 | } 62 | 63 | } // namespace test_util -------------------------------------------------------------------------------- /scripts/generate_readme_md: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | import os 4 | import re 5 | 6 | script_dir = os.path.dirname(os.path.realpath(__file__)) 7 | 8 | input_file = os.path.join(script_dir, "..", "doc/README_doxygen.md") 9 | 10 | with open(input_file) as f: 11 | readme = f.read() 12 | 13 | # Replace math syntax. 14 | replacements = { 15 | r'\f[': r'```math', r'@f[': r'```math', 16 | r'\f]': r'```', r'@f]': r'```'} 17 | 18 | rep = dict((re.escape(k), v) for k, v in replacements.items()) 19 | pattern = re.compile("|".join(rep.keys())) 20 | readme = pattern.sub(lambda m: rep[re.escape(m.group(0))], readme) 21 | 22 | string_patterns = [r'\\f\$(.*?)\\f\$', r'@f\$(.*?)@f\$'] 23 | for string_pattern in string_patterns: 24 | pattern = re.compile(string_pattern) 25 | readme = pattern.sub(lambda m: '$`' + m.group(1) + '`$', readme) 26 | 27 | # Embed code snippets. 28 | readme_new = [ 29 | ''] 33 | for l in readme.splitlines(): 34 | if not l.startswith('\snippet'): 35 | readme_new += [l] 36 | continue 37 | 38 | elements = l.split(' ') 39 | if len(elements) != 3: 40 | print('Unknown snippet found: ' + l) 41 | continue 42 | 43 | source_filename = elements[1] 44 | tag_name = elements[2] 45 | tag_comment = '//! [' + tag_name + ']' 46 | 47 | # Replace snippet with code. 48 | with open(os.path.join(script_dir, '..', 'test', source_filename)) as f: 49 | append = False 50 | for src_line in f.read().splitlines(): 51 | if tag_comment in src_line: 52 | if not append: 53 | readme_new += ["```cpp"] 54 | append = True 55 | continue 56 | else: 57 | readme_new += ["```"] 58 | break 59 | 60 | if append: 61 | readme_new += [src_line] 62 | 63 | if not append: 64 | print("Cannot find snippet '{}' in file '{}'".format(tag_name, source_filename)) 65 | continue 66 | 67 | 68 | with open(os.path.join(script_dir, '..', 'README.md'), 'w') as f: 69 | f.write("\n".join(readme_new)) 70 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | set(MRT_PKG_VERSION 4.0.0) 2 | # Modify only if you know what you are doing! 3 | cmake_minimum_required(VERSION 3.5.1) 4 | project(uniform_bspline_ceres) 5 | 6 | ################### 7 | ## Find packages ## 8 | ################### 9 | find_package(mrt_cmake_modules REQUIRED) 10 | include(UseMrtStdCompilerFlags) 11 | include(GatherDeps) 12 | 13 | # You can add a custom.cmake in order to add special handling for this package. E.g. you can do: 14 | # list(REMOVE_ITEM DEPENDEND_PACKAGES ...) 15 | # To remove libs which cannot be found automatically. You can also "find_package" other, custom dependencies there. 16 | # You can also set PROJECT_INSTALL_FILES to install files that are not installed by default. 17 | if(EXISTS "${CMAKE_CURRENT_SOURCE_DIR}/custom.cmake") 18 | include("${CMAKE_CURRENT_SOURCE_DIR}/custom.cmake") 19 | endif() 20 | 21 | find_package(AutoDeps REQUIRED COMPONENTS ${DEPENDEND_PACKAGES}) 22 | 23 | mrt_parse_package_xml() 24 | 25 | ######################## 26 | ## Add python modules ## 27 | ######################## 28 | # This adds a python module if located under src/{PROJECT_NAME) 29 | mrt_python_module_setup() 30 | 31 | mrt_glob_files(PROJECT_PYTHON_SOURCE_FILES_SRC "python_api/*.cpp") 32 | if (PROJECT_PYTHON_SOURCE_FILES_SRC) 33 | # Add a cpp-python api library. Make sure there are no name collisions with python modules in this project 34 | mrt_add_python_api( ${PROJECT_NAME} 35 | FILES ${PROJECT_PYTHON_SOURCE_FILES_SRC} 36 | ) 37 | endif() 38 | 39 | ############################ 40 | ## Read source code files ## 41 | ############################ 42 | mrt_glob_files_recurse(PROJECT_HEADER_FILES_INC "include/*.h" "include/*.hpp" "include/*.cuh") 43 | mrt_glob_files(PROJECT_SOURCE_FILES_INC "src/*.h" "src/*.hpp" "src/*.cuh") 44 | mrt_glob_files(PROJECT_SOURCE_FILES_SRC "src/*.cpp" "src/*.cu") 45 | 46 | ########### 47 | ## Build ## 48 | ########### 49 | # Declare a cpp library 50 | mrt_add_library(${PROJECT_NAME} 51 | INCLUDES ${PROJECT_HEADER_FILES_INC} ${PROJECT_SOURCE_FILES_INC} 52 | SOURCES ${PROJECT_SOURCE_FILES_SRC} 53 | ) 54 | 55 | ############# 56 | ## Install ## 57 | ############# 58 | # Install all targets, headers by default and scripts and other files if specified (folders or files). 59 | # This command also exports libraries and config files for dependent packages and this supersedes catkin_package. 60 | mrt_install(PROGRAMS scripts FILES res data ${PROJECT_INSTALL_FILES}) 61 | 62 | ############# 63 | ## Testing ## 64 | ############# 65 | # Add test targets for cpp and python tests 66 | if (CATKIN_ENABLE_TESTING) 67 | mrt_add_tests(test) 68 | mrt_add_nosetests(test) 69 | endif() 70 | -------------------------------------------------------------------------------- /test/generator_spline_autodiff.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | #include "uniform_bspline_ceres.hpp" 9 | 10 | namespace { 11 | 12 | using Spline = ubs::UniformBSpline11d<3>; 13 | 14 | template 15 | typename Spline::ValueType evaluateTestFunction(const Spline& spline) { 16 | using T = typename Spline::ValueType; 17 | 18 | T res{}; 19 | constexpr int NumTestPoints = 50; 20 | for (int i = 0; i < NumTestPoints; ++i) { 21 | T testPoint = T(double(i) / double(NumTestPoints - 1)); 22 | 23 | res += spline.evaluate(testPoint); 24 | res += spline.derivative(testPoint, 1); 25 | } 26 | 27 | res += spline.template smoothness<3>(); 28 | 29 | return res; 30 | } 31 | 32 | class CostFunctor1DSpline { 33 | public: 34 | explicit CostFunctor1DSpline(int numControlPoints) : numControlPoints_{numControlPoints} { 35 | } 36 | 37 | template 38 | bool operator()(const T* const* controlPointsRaw, T* residual) const { 39 | using OptSpline = ubs::UniformBSpline11; 40 | typename OptSpline::ControlPointsType controlPoints(numControlPoints_); 41 | 42 | std::copy(*controlPointsRaw, *controlPointsRaw + numControlPoints_, controlPoints.begin()); 43 | OptSpline spline(controlPoints); 44 | 45 | *residual = evaluateTestFunction(spline); 46 | return true; 47 | } 48 | 49 | private: 50 | int numControlPoints_; 51 | }; 52 | 53 | } // namespace 54 | 55 | TEST(CeresBSplineOptimization, Opt1d) { 56 | std::vector controlPoints(15); 57 | std::mt19937 rng; 58 | std::uniform_real_distribution<> dist(0, 1.0); 59 | std::generate(controlPoints.begin(), controlPoints.end(), [&]() { return dist(rng); }); 60 | 61 | ceres::Problem::Options problemOptions; 62 | problemOptions.cost_function_ownership = ceres::DO_NOT_TAKE_OWNERSHIP; 63 | ceres::Problem problem(problemOptions); 64 | auto costFunction = std::make_unique>( 65 | (new CostFunctor1DSpline(controlPoints.size()))); 66 | costFunction->AddParameterBlock(controlPoints.size()); 67 | costFunction->SetNumResiduals(1); 68 | problem.AddResidualBlock(costFunction.get(), nullptr, controlPoints.data()); 69 | 70 | // Check value, derivative and smoothness computation. 71 | std::vector residuals; 72 | problem.Evaluate(ceres::Problem::EvaluateOptions(), nullptr, &residuals, nullptr, nullptr); 73 | ASSERT_EQ((int)residuals.size(), 1); 74 | { 75 | Spline spline(controlPoints); 76 | EXPECT_EQ(evaluateTestFunction(spline), residuals[0]); 77 | } 78 | 79 | // Check gradient. 80 | ceres::GradientChecker gradientChecker(costFunction.get(), nullptr, ceres::NumericDiffOptions()); 81 | 82 | std::vector parameterBlocks(1); 83 | parameterBlocks[0] = controlPoints.data(); 84 | 85 | ceres::GradientChecker::ProbeResults results; 86 | EXPECT_TRUE(gradientChecker.Probe(parameterBlocks.data(), 1e-6, &results)) << results.error_log; 87 | } 88 | -------------------------------------------------------------------------------- /test/evaluator_example.cpp: -------------------------------------------------------------------------------- 1 | #include "uniform_bspline_ceres.hpp" 2 | 3 | #include 4 | #include 5 | 6 | // See README.md for a more detailed explanation of the following example. 7 | 8 | //! [Spline] 9 | using Spline = ubs::UniformBSpline>; 10 | //! [Spline] 11 | 12 | namespace { 13 | //! [Residual] 14 | class ExponentialResidual { 15 | public: 16 | explicit ExponentialResidual(const ubs::UniformBSplineCeresEvaluator& splineEvaluator, double measurement) 17 | : splineEvaluator_(splineEvaluator), measurement_{measurement} { 18 | } 19 | 20 | template 21 | bool operator()(const T* c0, const T* c1, const T* c2, const T* c3, T* residual) const { 22 | splineEvaluator_.evaluate(c0, c1, c2, c3, residual); 23 | *residual -= T(measurement_); 24 | return true; 25 | } 26 | 27 | private: 28 | ubs::UniformBSplineCeresEvaluator splineEvaluator_; 29 | double measurement_; 30 | }; 31 | //! [Residual] 32 | 33 | } // namespace 34 | 35 | TEST(UniformBSplineCeres, ExampleEvaluator1D) { // NOLINT(readability-function-size) 36 | //! [Init] 37 | std::vector controlPoints(20, 0.0); 38 | Spline spline(controlPoints); 39 | ubs::UniformBSplineCeres splineCeres(spline); 40 | //! [Init] 41 | 42 | const int numMeasurements = 1000; 43 | 44 | //! [Problem_Setup] 45 | std::vector parameterPointers(splineCeres.getNumPointParameterPointers()); 46 | ceres::Problem problem; 47 | 48 | for (int i = 0; i < numMeasurements; ++i) { 49 | const double posX = double(i) / double(numMeasurements); 50 | const double posY = std::exp(posX * 2.0); 51 | 52 | const auto data = splineCeres.getPointData(posX); 53 | splineCeres.fillParameterPointers(data, parameterPointers.begin(), parameterPointers.end()); 54 | ubs::UniformBSplineCeresEvaluator evaluator = splineCeres.getEvaluator(data); 55 | 56 | auto* costFunctor = new ceres::AutoDiffCostFunction( 57 | new ExponentialResidual(evaluator, posY)); 58 | 59 | problem.AddResidualBlock(costFunctor, nullptr, parameterPointers); 60 | } 61 | //! [Problem_Setup] 62 | 63 | //! [Solve] 64 | ceres::Solver::Options options; 65 | options.minimizer_progress_to_stdout = true; 66 | ceres::Solver::Summary summary; 67 | ceres::Solve(options, &problem, &summary); 68 | std::cout << summary.FullReport() << std::endl; 69 | //! [Solve] 70 | 71 | const int numTestPoints = 2000; 72 | for (int i = 0; i < numTestPoints; ++i) { 73 | const double posX = double(i) / double(numTestPoints); 74 | 75 | const double estY = spline.evaluate(posX); 76 | const double gtY = std::exp(posX * 2.0); 77 | EXPECT_NEAR(gtY, estY, 1e-5); 78 | } 79 | 80 | //! [Smoothing] 81 | const double weight = 1e-5; 82 | splineCeres.addSmoothnessResiduals<1>(problem, weight); 83 | //! [Smoothing] 84 | 85 | //! [Smoothing_Grid] 86 | splineCeres.addSmoothnessResidualsGrid<1>(problem, weight); 87 | //! [Smoothing_Grid] 88 | 89 | ceres::Solve(options, &problem, &summary); 90 | std::cout << summary.FullReport() << std::endl; 91 | } 92 | -------------------------------------------------------------------------------- /test/generator_example.cpp: -------------------------------------------------------------------------------- 1 | #include "uniform_bspline_ceres.hpp" 2 | 3 | #include 4 | #include 5 | 6 | // See README.md for a more detailed explanation of the following example. 7 | 8 | //! [Spline] 9 | template 10 | using Spline = ubs::UniformBSpline>; 11 | //! [Spline] 12 | 13 | namespace { 14 | //! [Residual] 15 | class SplineMinimumResidual { 16 | public: 17 | explicit SplineMinimumResidual(const ubs::UniformBSplineCeresGenerator& generator, int numControlPoints) 18 | : generator_(generator), numControlPoints_(numControlPoints) { 19 | } 20 | 21 | template 22 | bool operator()(const T* const* paramPointers, T* residual) const { 23 | const T* const* controlPoints = paramPointers; 24 | const T& pos = *(paramPointers[numControlPoints_]); 25 | 26 | auto spline = generator_.generate(controlPoints); 27 | residual[0] = spline.evaluate(pos); 28 | residual[1] = spline.derivative(pos, 1); 29 | return true; 30 | } 31 | 32 | private: 33 | ubs::UniformBSplineCeresGenerator generator_; 34 | int numControlPoints_; 35 | }; 36 | //! [Residual] 37 | 38 | } // namespace 39 | 40 | TEST(UniformBSplineCeres, ExampleEvaluator1D) { // NOLINT(readability-function-size) 41 | //! [Init] 42 | std::vector controlPoints{6.0, 1.0, 0.0, 1.0, 2.0, 3.0, 6.0}; 43 | Spline spline(controlPoints); 44 | ubs::UniformBSplineCeres> splineCeres(spline); 45 | 46 | double t = 0.8; 47 | //! [Init] 48 | 49 | //! [Range_Data] 50 | const auto data = splineCeres.getRangeData(0.0, 1.0); 51 | //! [Range_Data] 52 | 53 | //! [Parameter_Pointers] 54 | const int numControlPoints = splineCeres.getNumRangeParameterPointers(data); 55 | std::vector parameterPointers(numControlPoints + 1); 56 | 57 | splineCeres.fillParameterPointers(data, parameterPointers.begin(), parameterPointers.begin() + numControlPoints); 58 | parameterPointers.back() = &t; 59 | //! [Parameter_Pointers] 60 | 61 | //! [Cost_Function] 62 | ubs::UniformBSplineCeresGenerator generator = splineCeres.getGenerator(data); 63 | 64 | auto costFunction = std::make_unique>( 65 | new SplineMinimumResidual(generator, numControlPoints)); 66 | 67 | for (int i = 0; i < numControlPoints; ++i) { 68 | costFunction->AddParameterBlock(1); 69 | } 70 | costFunction->AddParameterBlock(1); 71 | costFunction->SetNumResiduals(2); 72 | //! [Cost_Function] 73 | 74 | //! [Add_Residual] 75 | ceres::Problem problem; 76 | problem.AddResidualBlock(costFunction.release(), nullptr, parameterPointers); 77 | //! [Add_Residual] 78 | 79 | //! [Set_Bounds] 80 | problem.SetParameterLowerBound(&t, 0, 0.0); 81 | problem.SetParameterUpperBound(&t, 0, 1.0); 82 | //! [Set_Bounds] 83 | 84 | //! [Fix_Control_Points] 85 | spline.getControlPointsContainer().forEach([&](double& c) { problem.SetParameterBlockConstant(&c); }); 86 | //! [Fix_Control_Points] 87 | 88 | //! [Solve] 89 | ceres::Solver::Options options; 90 | options.minimizer_progress_to_stdout = true; 91 | options.parameter_tolerance = 1e-15; 92 | options.gradient_tolerance = 1e-15; 93 | options.function_tolerance = 1e-15; 94 | 95 | ceres::Solver::Summary summary; 96 | ceres::Solve(options, &problem, &summary); 97 | std::cout << summary.FullReport() << std::endl; 98 | //! [Solve] 99 | 100 | EXPECT_NEAR(t, 0.25, 1e-6); 101 | } 102 | -------------------------------------------------------------------------------- /include/uniform_bspline_ceres/uniform_bspline_ceres_evaluator.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | #include 8 | 9 | namespace ubs { 10 | 11 | /** 12 | * @brief Helper class to evaluates a spline during optimization. 13 | * 14 | * The evaluator is usually passed to a ceres cost function. It supports autodiff and numeric diff cost 15 | * functions and dynamic autodiff and dynamic numeric diff cost functions. 16 | * 17 | * @tparam Spline_ The spline type. 18 | */ 19 | template 20 | class UniformBSplineCeresEvaluator { 21 | public: 22 | /** @brief The spline type. */ 23 | using Spline = Spline_; 24 | 25 | /** @brief The degree of the spline. */ 26 | static constexpr int Degree = Spline::Degree; 27 | /** @brief The order of the spline. */ 28 | static constexpr int Order = Spline::Order; 29 | 30 | /** @brief The input dimensions of the spline. */ 31 | static constexpr int InputDims = Spline::InputDims; 32 | /** @brief The output dimensions of the spline. */ 33 | static constexpr int OutputDims = Spline::OutputDims; 34 | /** @brief The number of control points needed to evaluate the spline. */ 35 | static constexpr int ControlPointsSupport = ubs::power(Order, InputDims); 36 | 37 | // As ceres uses double, the value type of the underlying spline must be double. 38 | static_assert(std::is_same::value, "Specified value type is not supported."); 39 | 40 | /** 41 | * @brief Constructor. 42 | * @param[in] basisVals The basis values. 43 | */ 44 | explicit UniformBSplineCeresEvaluator(const std::array& basisVals) 45 | : basisVals_(basisVals) { 46 | } 47 | 48 | /** 49 | * @brief Evaluates the uniform B-spline using the specified control points. 50 | * @param[in] controlPoints The control points. 51 | * @param[out] result The result. 52 | */ 53 | template 54 | void evaluate(T const* const* controlPoints, T* result) const { 55 | // Clear result. 56 | for (int j = 0; j < OutputDims; ++j) { 57 | result[j] = T(0.0); 58 | } 59 | 60 | // Calculate uniform B spline value. 61 | for (int i = 0; i < ControlPointsSupport; ++i) { 62 | for (int j = 0; j < OutputDims; ++j) { 63 | result[j] += controlPoints[i][j] * T(basisVals_[i]); 64 | } 65 | } 66 | } 67 | 68 | /** 69 | * @copybrief evaluate(T const* const* controlPoints, T* result) const 70 | * 71 | * This function takes the control point pointers as separate arguments. The parameter pointers must be kept in 72 | * order and the last element must be the result pointer (and non-cost). 73 | * 74 | * @param[in] controlPoint The first control point. 75 | * @param[in,out] ts The other control points. The last pointer is the result pointer. 76 | */ 77 | template 78 | void evaluate(const T* controlPoint, Ts*... ts) const { 79 | static_assert(ControlPointsSupport == int(sizeof...(ts)), 80 | "Invalid number of control points specified for evaluation"); 81 | 82 | std::array cPs{{controlPoint, ts...}}; 83 | 84 | // The last element of the control points is the place to store the output. 85 | T* result = std::get(std::make_tuple(controlPoint, ts...)); 86 | 87 | evaluate(cPs.data(), result); 88 | } 89 | 90 | private: 91 | /** @brief The basis values used to calculate the spline value. */ 92 | std::array basisVals_{}; 93 | }; 94 | } // namespace ubs 95 | 96 | #include "internal/uniform_bspline_ceres_evaluator_impl.hpp" 97 | -------------------------------------------------------------------------------- /test/smoothness_prior.cpp: -------------------------------------------------------------------------------- 1 | #include "uniform_bspline_ceres.hpp" 2 | 3 | #include 4 | 5 | #include 6 | #include 7 | #include 8 | 9 | #include "test_helper.hpp" 10 | #include "uniform_bspline_ceres.hpp" 11 | 12 | namespace { 13 | template 14 | class FitCostFunctor { 15 | public: 16 | static_assert(Spline_::OutputDims == 1, "Invalid number of output dims specified."); 17 | 18 | explicit FitCostFunctor(const ubs::UniformBSplineCeresEvaluator& splineEvaluator, double val) 19 | : splineEvaluator_(splineEvaluator), val_(val) { 20 | } 21 | 22 | template 23 | bool operator()(T const* const* paramPointers, T* residual) const { 24 | splineEvaluator_.evaluate(paramPointers, residual); 25 | *residual -= T(val_); 26 | return true; 27 | } 28 | 29 | private: 30 | ubs::UniformBSplineCeresEvaluator splineEvaluator_; 31 | double val_; 32 | }; 33 | 34 | } // namespace 35 | 36 | TEST(UniformBSplineCeres, PlaneFitPrior) { // NOLINT(readability-function-size) 37 | using Spline = ubs::UniformBSpline; 38 | 39 | Eigen::MatrixXd controlPoints = Eigen::MatrixXd::Random(20, 20); 40 | Spline spline(controlPoints); 41 | 42 | // Generate measurements. 43 | ubs::UniformBSplineCeres splineCeres(spline); 44 | 45 | std::vector parameterPointers(splineCeres.ControlPointsSupport); 46 | ceres::Problem problem; 47 | const int numMeasurements = 10; 48 | 49 | const Eigen::Vector3d supportPoint(1.0, 2.0, 3.0); 50 | const Eigen::Vector3d dir1 = Eigen::Vector3d(2.0, 0.12, 0.32).normalized(); 51 | const Eigen::Vector3d dir2 = Eigen::Vector3d(0.1, 1.5, 1.74).normalized(); 52 | 53 | Eigen::Matrix2d a = Eigen::Matrix2d::Zero(); 54 | a << dir1.head<2>(), dir2.head<2>(); 55 | 56 | const Eigen::Matrix2d aInv = a.inverse(); 57 | 58 | // Create ceres problem. 59 | for (int i = 0; i < numMeasurements; ++i) { 60 | for (int j = 0; j < numMeasurements; ++j) { 61 | Eigen::Vector2d planePos{}; 62 | planePos << i, j; 63 | planePos = planePos / (numMeasurements - 1) * 0.2 - supportPoint.head<2>(); 64 | 65 | const Eigen::Vector2d rs = aInv * planePos; 66 | const Eigen::Vector3d pos = supportPoint + dir1 * rs[0] + dir2 * rs[1]; 67 | 68 | const auto data = splineCeres.getPointData(pos.head<2>()); 69 | splineCeres.fillParameterPointers(data, parameterPointers.begin(), parameterPointers.end()); 70 | ubs::UniformBSplineCeresEvaluator evaluator = splineCeres.getEvaluator(data); 71 | 72 | auto* costFunctor = new ceres::DynamicAutoDiffCostFunction>( 73 | new FitCostFunctor(evaluator, pos[2])); 74 | for (int k = 0; k < splineCeres.ControlPointsSupport; ++k) { 75 | costFunctor->AddParameterBlock(1); 76 | } 77 | costFunctor->SetNumResiduals(1); 78 | 79 | problem.AddResidualBlock(costFunctor, nullptr, parameterPointers); 80 | } 81 | } 82 | 83 | splineCeres.addSmoothnessResiduals<2>(problem); 84 | 85 | // Solve problem. 86 | ceres::Solver::Options options; 87 | options.minimizer_progress_to_stdout = false; 88 | options.initial_trust_region_radius = 1e16; 89 | ceres::Solver::Summary summary; 90 | ceres::Solve(options, &problem, &summary); 91 | 92 | // Check result. 93 | test_util::linspace(std::array{200, 200}, [&](const Eigen::Vector2d& pos) { 94 | const Eigen::Vector3d splinePos(pos[0], pos[1], spline.evaluate(pos)); 95 | 96 | Eigen::Vector2d planePos = pos - supportPoint.head<2>(); 97 | Eigen::Vector2d rs = aInv * planePos; 98 | const Eigen::Vector3d gtPos = supportPoint + dir1 * rs[0] + dir2 * rs[1]; 99 | 100 | for (int i = 0; i < 3; ++i) { 101 | EXPECT_NEAR(gtPos[i], splinePos[i], 1e-8) << pos; 102 | } 103 | }); 104 | } 105 | -------------------------------------------------------------------------------- /doc/generator_example.nb: -------------------------------------------------------------------------------- 1 | (* Content-type: application/vnd.wolfram.mathematica *) 2 | 3 | (*** Wolfram Notebook File ***) 4 | (* http://www.wolfram.com/nb *) 5 | 6 | (* CreatedBy='Mathematica 11.3' *) 7 | 8 | (*CacheID: 234*) 9 | (* Internal cache information: 10 | NotebookFileLineBreakTest 11 | NotebookFileLineBreakTest 12 | NotebookDataPosition[ 158, 7] 13 | NotebookDataLength[ 3600, 98] 14 | NotebookOptionsPosition[ 2943, 81] 15 | NotebookOutlinePosition[ 3280, 96] 16 | CellTagsIndexPosition[ 3237, 93] 17 | WindowFrame->Normal*) 18 | 19 | (* Beginning of Notebook Content *) 20 | Notebook[{ 21 | Cell[BoxData[ 22 | RowBox[{"Remove", "[", "\"\\"", "]"}]], "Input", 23 | CellChangeTimes->{{3.773630626592537*^9, 3.7736306359027443`*^9}}, 24 | CellLabel->"In[29]:=",ExpressionUUID->"f910b06c-09c9-447b-bed2-0366d677c1df"], 25 | 26 | Cell[BoxData[{ 27 | RowBox[{ 28 | RowBox[{"pts", "=", 29 | RowBox[{"{", 30 | RowBox[{"6", ",", "1", ",", "0", ",", "1", ",", "2", ",", "3", ",", "6"}], 31 | "}"}]}], ";"}], "\[IndentingNewLine]", 32 | RowBox[{"spline", "=", 33 | RowBox[{"BSplineFunction", "[", 34 | RowBox[{"pts", ",", 35 | RowBox[{"SplineDegree", "\[Rule]", "3"}], ",", 36 | RowBox[{"SplineKnots", "->", "\"\\""}]}], "]"}]}]}], "Input", 37 | CellChangeTimes->{{3.773573388975196*^9, 3.7735734384295607`*^9}, { 38 | 3.773630422584638*^9, 3.7736304229266148`*^9}, {3.77363058035323*^9, 39 | 3.773630599567256*^9}}, 40 | CellLabel->"In[30]:=",ExpressionUUID->"64ab932c-fb21-46fe-9a70-ab29b1c092ef"], 41 | 42 | Cell[BoxData[ 43 | RowBox[{"img", "=", 44 | RowBox[{"Plot", "[", 45 | RowBox[{ 46 | RowBox[{"spline", "[", "u", "]"}], ",", 47 | RowBox[{"{", 48 | RowBox[{"u", ",", "0", ",", "1"}], "}"}], ",", 49 | RowBox[{"ImageSize", "\[Rule]", "400"}], ",", 50 | RowBox[{"AxesOrigin", "\[Rule]", 51 | RowBox[{"{", 52 | RowBox[{"0", ",", "0"}], "}"}]}]}], "]"}]}]], "Input", 53 | CellChangeTimes->{{3.773573431726123*^9, 3.7735734470531178`*^9}, { 54 | 3.773629414606495*^9, 3.773629417973174*^9}, {3.77362946552477*^9, 55 | 3.773629470396893*^9}, {3.773630131160833*^9, 3.773630132317465*^9}, { 56 | 3.77363038871248*^9, 3.773630400886331*^9}}, 57 | CellLabel->"In[32]:=",ExpressionUUID->"57e5f516-e65f-491e-8c13-51a04cf9ad23"], 58 | 59 | Cell[BoxData[ 60 | RowBox[{ 61 | RowBox[{"Export", "[", 62 | RowBox[{ 63 | RowBox[{ 64 | RowBox[{"NotebookDirectory", "[", "]"}], "<>", 65 | "\"\\""}], ",", "img"}], "]"}], 66 | ";"}]], "Input", 67 | CellChangeTimes->{{3.773629473541562*^9, 3.7736295045492153`*^9}, { 68 | 3.773630126896792*^9, 3.773630127197316*^9}, {3.773630641856985*^9, 69 | 3.7736306535673323`*^9}, 3.77363070445601*^9, 70 | 3.773630785920698*^9},ExpressionUUID->"5bbdb195-0d87-4cbf-bd4f-\ 71 | 01e6d9a679d3"], 72 | 73 | Cell[BoxData[ 74 | RowBox[{"FindMinimum", "[", 75 | RowBox[{ 76 | RowBox[{"spline", "[", "u", "]"}], ",", 77 | RowBox[{"{", 78 | RowBox[{"u", ",", "0.2"}], "}"}]}], "]"}]], "Input", 79 | CellChangeTimes->{{3.773630712759859*^9, 3.773630772491199*^9}}, 80 | CellLabel->"In[37]:=",ExpressionUUID->"78847e88-6b81-4b17-bac4-30c9ab3078bb"] 81 | }, 82 | WindowSize->{808, 911}, 83 | WindowMargins->{{Automatic, -1231}, {43, Automatic}}, 84 | FrontEndVersion->"11.3 for Linux x86 (64-bit) (March 6, 2018)", 85 | StyleDefinitions->"Default.nb" 86 | ] 87 | (* End of Notebook Content *) 88 | 89 | (* Internal cache information *) 90 | (*CellTagsOutline 91 | CellTagsIndex->{} 92 | *) 93 | (*CellTagsIndex 94 | CellTagsIndex->{} 95 | *) 96 | (*NotebookFileOutline 97 | Notebook[{ 98 | Cell[558, 20, 221, 3, 31, "Input",ExpressionUUID->"f910b06c-09c9-447b-bed2-0366d677c1df"], 99 | Cell[782, 25, 648, 14, 55, "Input",ExpressionUUID->"64ab932c-fb21-46fe-9a70-ab29b1c092ef"], 100 | Cell[1433, 41, 694, 15, 31, "Input",ExpressionUUID->"57e5f516-e65f-491e-8c13-51a04cf9ad23"], 101 | Cell[2130, 58, 489, 12, 31, "Input",ExpressionUUID->"5bbdb195-0d87-4cbf-bd4f-01e6d9a679d3"], 102 | Cell[2622, 72, 317, 7, 31, "Input",ExpressionUUID->"78847e88-6b81-4b17-bac4-30c9ab3078bb"] 103 | } 104 | ] 105 | *) 106 | 107 | -------------------------------------------------------------------------------- /test/generator.cpp: -------------------------------------------------------------------------------- 1 | #include "uniform_bspline_ceres.hpp" 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | template 8 | using Spline = ubs::UniformBSpline11; 9 | using SplineGenerator = ubs::UniformBSplineCeresGenerator; 10 | 11 | namespace { 12 | class SplineCostFunction { 13 | public: 14 | SplineCostFunction(int numControlPoints, const SplineGenerator& generator) 15 | : numControlPoints_{numControlPoints}, generator_{generator} { 16 | } 17 | 18 | template 19 | bool operator()(const T* const* parameterPointers, T* residual) { 20 | auto spline = generator_.generate(parameterPointers); 21 | 22 | T val = parameterPointers[numControlPoints_][0]; 23 | residual[0] = spline.evaluate(val); 24 | residual[1] = spline.derivative(val, 1); 25 | return true; 26 | } 27 | 28 | private: 29 | int numControlPoints_; 30 | SplineGenerator generator_; 31 | }; 32 | } // namespace 33 | 34 | TEST(UniformBSplineCeres, SplineGenerator) { 35 | double lowerBound = 1.0; 36 | double upperBound = 7.0; 37 | 38 | std::vector controlPoints{1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0}; 39 | 40 | Spline spline(lowerBound, upperBound, controlPoints); 41 | ubs::UniformBSplineCeres> splineCeres(spline); 42 | 43 | const auto data = splineCeres.getRangeData(1.0, 6.9999999); 44 | 45 | std::vector parameterPointers(splineCeres.getNumRangeParameterPointers(data)); 46 | splineCeres.fillParameterPointers(data, parameterPointers.begin(), parameterPointers.end()); 47 | 48 | auto gen = splineCeres.getGenerator(data); 49 | auto partialBSpline = gen.generate(parameterPointers.data()); 50 | EXPECT_EQ(partialBSpline.getLowerBound(), 1.0); 51 | EXPECT_EQ(partialBSpline.getUpperBound(), 7.0); 52 | EXPECT_NEAR(partialBSpline.evaluate(2.101), spline.evaluate(2.101), 1e-15); 53 | 54 | auto costFunction = std::make_unique>( 55 | new SplineCostFunction(parameterPointers.size(), gen)); 56 | 57 | const double evalPoint = 3.143; 58 | double modEvalPoint = evalPoint; 59 | parameterPointers.push_back(&modEvalPoint); 60 | 61 | for (int i = 0; i < int(parameterPointers.size()); ++i) { 62 | costFunction->AddParameterBlock(1); 63 | } 64 | costFunction->SetNumResiduals(2); 65 | 66 | ceres::Problem problem; 67 | problem.AddResidualBlock(costFunction.release(), nullptr, parameterPointers); 68 | 69 | std::vector residuals; 70 | problem.Evaluate(ceres::Problem::EvaluateOptions(), nullptr, &residuals, nullptr, nullptr); 71 | 72 | ASSERT_EQ(residuals.size(), 2U); 73 | EXPECT_NEAR(residuals[0], spline.evaluate(evalPoint), 1e-15); 74 | EXPECT_NEAR(residuals[1], spline.derivative(evalPoint, 1), 1e-15); 75 | } 76 | 77 | TEST(UniformBSplineCeres, GetRangeData) { 78 | const double lowerBound = -7.45; 79 | const double upperBound = 23.456; 80 | 81 | std::vector controlPoints{1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0, 10.0, 11.0, 12.0}; 82 | 83 | Spline spline(lowerBound, upperBound, controlPoints); 84 | ubs::UniformBSplineCeres> splineCeres(spline); 85 | 86 | const int numTestPoints = 100; 87 | for (int i = 0; i < numTestPoints; ++i) { 88 | const double p = (lowerBound + 0.1) + (upperBound - lowerBound - 0.2) * double(i) / (numTestPoints - 1); 89 | 90 | const auto data = splineCeres.getRangeData(p, 0.1, true); 91 | std::vector parameterPointers(splineCeres.getNumRangeParameterPointers(data)); 92 | splineCeres.fillParameterPointers(data, parameterPointers.begin(), parameterPointers.end()); 93 | 94 | auto gen = splineCeres.getGenerator(data); 95 | auto partialBSpline = gen.generate(parameterPointers.data()); 96 | 97 | EXPECT_LE((int)parameterPointers.size(), Spline::Order + 1); 98 | EXPECT_NEAR(partialBSpline.evaluate(p - 0.1), spline.evaluate(p - 0.1), 1e-14); 99 | EXPECT_NEAR(partialBSpline.evaluate(p), spline.evaluate(p), 1e-14); 100 | EXPECT_NEAR(partialBSpline.evaluate(p + 0.1), spline.evaluate(p + 0.1), 1e-14); 101 | } 102 | } 103 | -------------------------------------------------------------------------------- /include/uniform_bspline_ceres/uniform_bspline_ceres_generator.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #include 6 | 7 | #include "fixed_size_container_type_trait_ceres.hpp" 8 | #include "value_type_trait_ceres.hpp" 9 | 10 | namespace ubs { 11 | 12 | /** 13 | * @brief The uniform B-spline generator. 14 | * 15 | * A uniform B-spline generator is used to generate splines in a ceres cost function. The generated spline can be used 16 | * also in autodiff. 17 | * 18 | * @tparam UniformBSpline_ A spline templated on the value type. 19 | */ 20 | template