AutoDiff Code Generation - CMake Integration This patch integrates the code generation module into the build system. All depenendcies are tracked through CMake targets. Modifying the cost functor will automatically trigger code re-generation. All this functionality is defined in the CMake function ceres_generate_cost_function_implementation_for_functor in CeresCodeGeneration.cmake. A hello world usage example is included in examples/CMakeLists.txt. Change-Id: I23b8b6698d1ea51cf3d788a47afcf39f8c5ce327
diff --git a/CMakeLists.txt b/CMakeLists.txt index f11e579..9bd1e90 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt
@@ -771,6 +771,12 @@ "${Ceres_SOURCE_DIR}/cmake/FindGlog.cmake" DESTINATION ${RELATIVE_CMAKECONFIG_INSTALL_DIR}) +# Install codegen cmake scripts +install(FILES "${Ceres_SOURCE_DIR}/cmake/codegen_include.h.in" + "${Ceres_SOURCE_DIR}/cmake/generate_code_for_functor.cc.in" + "${Ceres_SOURCE_DIR}/cmake/CeresCodeGeneration.cmake" + DESTINATION ${RELATIVE_CMAKECONFIG_INSTALL_DIR}) + if (PROVIDE_UNINSTALL_TARGET) # Create an uninstall target to remove all installed files. configure_file("${Ceres_SOURCE_DIR}/cmake/uninstall.cmake.in"
diff --git a/cmake/CeresCodeGeneration.cmake b/cmake/CeresCodeGeneration.cmake new file mode 100644 index 0000000..f3819ce --- /dev/null +++ b/cmake/CeresCodeGeneration.cmake
@@ -0,0 +1,153 @@ +# 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: darius.rueckert@fau.de (Darius Rueckert) + +# The directory containing the following files +# - codegen_include.h.in +# - generate_code_from_functor.cc.in +set(CODEGEN_CMAKE_SCRIPT_DIR "${CMAKE_CURRENT_LIST_DIR}") + +# Generates C-code implementation of Ceres' CostFunction::Evaluate() API from a +# templated C++ cost functor derived from ceres::CodegenCostFunction using +# autodiff. The resulting implementation replaces the direct instantiation of +# autodiff in client code, typically resulting in improved performance. +# +# Parameters: +# +# NAME +# The name of the cost functor. The name must exactly match the C++ type +# name of the functor. This is also the name of the CMake output target. +# NAMESPACE [optional] +# The C++ namespace of the cost functor type. For example, if the full +# type name is ceres::BundleAdjust, then NAME should be "BundleAdjust" +# and NAMESPACE should be "ceres". +# INPUT_FILE +# The path to the header defining the cost functor <NAME>. +# OUTPUT_DIRECTORY [default = "generated"] +# The relative output directory of the generated header file. This is the +# prefix that has to be added to the #include of the generated files, i.e: +# #include "<OUTPUT_DIRECTORY>/<GENERATED_FILE.h>" + +# +# Example Usage: +# ceres_generate_cost_function_implementation_for_functor( +# NAME SquareFunctor +# INPUT_FILE ${CMAKE_CURRENT_SOURCE_DIR}/square_functor.h +# ) +# add_executable(helloworld_codegen helloworld_codegen.cc ) +# target_link_libraries(helloworld_codegen ceres SquareFunctor) +function(ceres_generate_cost_function_implementation_for_functor) + # Define and parse arguments + set(OPTIONAL_ARGS) + set(ONE_VALUE_ARGS NAME INPUT_FILE OUTPUT_DIRECTORY NAMESPACE) + set(MULTI_VALUE_ARGS) + cmake_parse_arguments( + COST_FUNCTOR "${OPTIONAL_ARGS}" "${ONE_VALUE_ARGS}" + "${MULTI_VALUE_ARGS}" ${ARGN} ) + + # Default value of the output directory + set(OUTPUT_DIRECTORY "generated") + if(COST_FUNCTOR_OUTPUT_DIRECTORY) + set(OUTPUT_DIRECTORY "${COST_FUNCTOR_OUTPUT_DIRECTORY}") + endif() + + set(CALLER_CODEGEN_BUILD_DIR "${PROJECT_BINARY_DIR}/codegen") + set(CALLER_CODEGEN_INCLUDE_DIR "${CALLER_CODEGEN_BUILD_DIR}/include/") + + file(MAKE_DIRECTORY + "${CALLER_CODEGEN_BUILD_DIR}") + file(MAKE_DIRECTORY + "${CALLER_CODEGEN_INCLUDE_DIR}/${OUTPUT_DIRECTORY}") + file(MAKE_DIRECTORY + "${CALLER_CODEGEN_BUILD_DIR}/src") + + # Convert the input file to an absolute path and check if it exists + get_filename_component( + COST_FUNCTOR_INPUT_FILE "${COST_FUNCTOR_INPUT_FILE}" REALPATH) + if(NOT EXISTS "${COST_FUNCTOR_INPUT_FILE}") + message(FATAL_ERROR + "Could not find codegen input file ${COST_FUNCTOR_INPUT_FILE}") + endif() + + + # The full C++ type name of the cost functor. This is used inside the + # generator to create an object of it. + set(FULL_CXX_FUNCTOR_TYPE_NAME "${COST_FUNCTOR_NAME}") + if(COST_FUNCTOR_NAMESPACE) + set(FULL_CXX_FUNCTOR_TYPE_NAME + "${COST_FUNCTOR_NAMESPACE}::${FULL_CXX_FUNCTOR_TYPE_NAME}") + endif() + + # 1. Generate a wrapper include file which is included by the user. + # This is required, because + # - It must exist during compiliation of the code generator (otherwise + # the #include will fail) + # - We don't want to have the users add macros to their code + string(TOLOWER "${COST_FUNCTOR_NAME}" LOWER_CASE_FUNCTOR_NAME) + set(INCLUDE_FILE + "${CALLER_CODEGEN_INCLUDE_DIR}/${OUTPUT_DIRECTORY}/${LOWER_CASE_FUNCTOR_NAME}.h") + configure_file("${CODEGEN_CMAKE_SCRIPT_DIR}/codegen_include.inc.in" "${INCLUDE_FILE}") + + # 2. Generate the source file for the code generator + set(GENERATOR_SOURCE + "${CALLER_CODEGEN_BUILD_DIR}/src/${LOWER_CASE_FUNCTOR_NAME}_code_generator.cc") + set(GENERATED_EVALUATION_IMPL_FILE + "${CALLER_CODEGEN_INCLUDE_DIR}/${OUTPUT_DIRECTORY}/${LOWER_CASE_FUNCTOR_NAME}_evaluate_impl.inc") + configure_file( + "${CODEGEN_CMAKE_SCRIPT_DIR}/generate_code_for_functor.cc.in" "${GENERATOR_SOURCE}") + + # 3. Build the executable that generates the autodiff code + set(GENERATOR_TARGET ${COST_FUNCTOR_NAME}_generator) + add_executable(${GENERATOR_TARGET} "${GENERATOR_SOURCE}") + target_link_libraries(${GENERATOR_TARGET} ceres) + set_target_properties(${GENERATOR_TARGET} PROPERTIES + RUNTIME_OUTPUT_DIRECTORY "${CALLER_CODEGEN_BUILD_DIR}/bin") + target_compile_definitions(${GENERATOR_TARGET} PRIVATE -DCERES_CODEGEN) + target_include_directories(${GENERATOR_TARGET} + PRIVATE "${CALLER_CODEGEN_INCLUDE_DIR}") + + # 4. Execute the program from (3.) using a custom command + add_custom_command(OUTPUT "${GENERATED_EVALUATION_IMPL_FILE}" + COMMAND ${GENERATOR_TARGET} + DEPENDS "${COST_FUNCTOR_INPUT_FILE}" + VERBATIM + ) + set(GENERATE_TARGET ${COST_FUNCTOR_NAME}_generate) + add_custom_target(${GENERATE_TARGET} DEPENDS "${GENERATED_EVALUATION_IMPL_FILE}" VERBATIM) + + # 5. Create an output target which can be used by the client. This is required, + # because custom targets can't have include directories. + set(OUTPUT_TARGET ${COST_FUNCTOR_NAME}) + add_library(${OUTPUT_TARGET} INTERFACE) + target_include_directories( + ${OUTPUT_TARGET} INTERFACE "${CALLER_CODEGEN_INCLUDE_DIR}") + target_sources( + ${OUTPUT_TARGET} INTERFACE "${INCLUDE_FILE}" "${GENERATED_EVALUATION_IMPL_FILE}") + add_dependencies(${OUTPUT_TARGET} ${GENERATE_TARGET}) +endfunction()
diff --git a/cmake/CeresConfig.cmake.in b/cmake/CeresConfig.cmake.in index ced7daf..fd84dd2 100644 --- a/cmake/CeresConfig.cmake.in +++ b/cmake/CeresConfig.cmake.in
@@ -166,6 +166,10 @@ include(CMakeFindDependencyMacro) find_dependency(Threads) +# Import code generation functions s/t they are available for use in client code +# if find_package(Ceres) is successful. +include(CeresCodeGeneration.cmake) + # Eigen. # Flag set during configuration and build of Ceres. set(CERES_EIGEN_VERSION @EIGEN3_VERSION_STRING@)
diff --git a/cmake/codegen_include.inc.in b/cmake/codegen_include.inc.in new file mode 100644 index 0000000..f52485b --- /dev/null +++ b/cmake/codegen_include.inc.in
@@ -0,0 +1,10 @@ +// This file is generated by Ceres autodiff code generation. +// http://ceres-solver.org/ + +#ifdef CERES_CODEGEN +// Included by generator during generation of autodiff evaluation implementation, +// by definition the generated implementation does not yet exist to be imported. +#else +// Included from client code, import generated implementation. +#include "@OUTPUT_DIRECTORY@/@LOWER_CASE_FUNCTOR_NAME@_evaluate_impl.inc" +#endif
diff --git a/cmake/generate_code_for_functor.cc.in b/cmake/generate_code_for_functor.cc.in new file mode 100644 index 0000000..2ae2640 --- /dev/null +++ b/cmake/generate_code_for_functor.cc.in
@@ -0,0 +1,27 @@ +// This file is generated by Ceres autodiff code generation." +// http://ceres-solver.org/ + +#include <cstdlib> +#include <fstream> +#include <iostream> +#include <string> +#include <vector> + +#include "ceres/codegen/generate_code_for_functor.h" +#include "@COST_FUNCTOR_INPUT_FILE@" + +int main() { + const std::vector<std::string> lines = + ceres::GenerateCodeForFunctor<@FULL_CXX_FUNCTOR_TYPE_NAME@>( + ceres::AutoDiffCodeGenOptions()); + const std::string output_file = "@GENERATED_EVALUATION_IMPL_FILE@"; + std::ofstream stream(output_file); + if (!stream.is_open()) { + std::cerr << "Could not open file " << output_file << std::endl; + return EXIT_FAILURE; + } + for (auto line : lines) { + stream << line << std::endl; + } + return EXIT_SUCCESS; +}
diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index e2f4df4..f185d19 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt
@@ -73,8 +73,15 @@ target_link_libraries(simple_bundle_adjuster ceres) if(CODE_GENERATION) - add_executable(autodiff_codegen autodiff_codegen.cc) - target_link_libraries(autodiff_codegen ceres) + include(CeresCodeGeneration) + ceres_generate_cost_function_implementation_for_functor( + NAME HelloWorldCostFunction + INPUT_FILE helloworld_cost_function.h + OUTPUT_DIRECTORY examples + NAMESPACE helloworld + ) + add_executable(helloworld_codegen helloworld_codegen.cc ) + target_link_libraries(helloworld_codegen ceres HelloWorldCostFunction) endif(CODE_GENERATION) if (GFLAGS)
diff --git a/examples/autodiff_codegen.cc b/examples/helloworld_codegen.cc similarity index 69% copy from examples/autodiff_codegen.cc copy to examples/helloworld_codegen.cc index 7813ac4..6aa0792 100644 --- a/examples/autodiff_codegen.cc +++ b/examples/helloworld_codegen.cc
@@ -29,27 +29,37 @@ // Author: darius.rueckert@fau.de (Darius Rueckert) // // A simple example showing how to generate code for a cost functor -// -// We recommend to use the CMake integration instead of using -// GenerateCodeForFunctor directly. -// -#include "ceres/codegen/autodiff.h" -struct SquareFunctor { - template <typename T> - bool operator()(const T* x, T* residual) const { - residual[0] = x[0] * x[0]; - isfinite(x[0]); - return true; - } -}; +#include "ceres/ceres.h" +#include "glog/logging.h" +#include "helloworld_cost_function.h" + +using ceres::CostFunction; +using ceres::Problem; +using ceres::Solve; +using ceres::Solver; int main(int argc, char** argv) { - std::vector<std::string> code = - ceres::GenerateCodeForFunctor<SquareFunctor, 1, 1>( - ceres::AutoDiffCodeGenOptions()); - for (auto str : code) { - std::cout << str << std::endl; - } + google::InitGoogleLogging(argv[0]); + + // The variable to solve for with its initial value. It will be + // mutated in place by the solver. + double x = 0.5; + const double initial_x = x; + + Problem problem; + + const double kTargetValue = 10.0; + CostFunction* cost_function = + new helloworld::HelloWorldCostFunction(kTargetValue); + problem.AddResidualBlock(cost_function, NULL, &x); + + Solver::Options options; + options.minimizer_progress_to_stdout = true; + Solver::Summary summary; + Solve(options, &problem, &summary); + + std::cout << summary.BriefReport() << "\n"; + std::cout << "x : " << initial_x << " -> " << x << "\n"; return 0; }
diff --git a/examples/autodiff_codegen.cc b/examples/helloworld_cost_function.h similarity index 72% rename from examples/autodiff_codegen.cc rename to examples/helloworld_cost_function.h index 7813ac4..764d6c7 100644 --- a/examples/autodiff_codegen.cc +++ b/examples/helloworld_cost_function.h
@@ -28,28 +28,29 @@ // // Author: darius.rueckert@fau.de (Darius Rueckert) // -// A simple example showing how to generate code for a cost functor -// -// We recommend to use the CMake integration instead of using -// GenerateCodeForFunctor directly. -// -#include "ceres/codegen/autodiff.h" +#include "ceres/codegen/codegen_cost_function.h" -struct SquareFunctor { +namespace helloworld { + +struct HelloWorldCostFunction : public ceres::CodegenCostFunction<1, 1> { + // We need a default constructor, because code is generated for the cost + // functor and not a specific instantiation of it. + HelloWorldCostFunction() = default; + explicit HelloWorldCostFunction(double target_value) + : target_value_(target_value) {} + template <typename T> bool operator()(const T* x, T* residual) const { - residual[0] = x[0] * x[0]; - isfinite(x[0]); + residual[0] = CERES_LOCAL_VARIABLE(T, target_value_) - x[0]; return true; } + +// The include file name is automatically generated as +// "<output_dir>/<lower_case_class_name>.h" +#include "examples/helloworldcostfunction.h" + + private: + double target_value_; }; -int main(int argc, char** argv) { - std::vector<std::string> code = - ceres::GenerateCodeForFunctor<SquareFunctor, 1, 1>( - ceres::AutoDiffCodeGenOptions()); - for (auto str : code) { - std::cout << str << std::endl; - } - return 0; -} +} // namespace helloworld
diff --git a/include/ceres/codegen/codegen_cost_function.h b/include/ceres/codegen/codegen_cost_function.h new file mode 100644 index 0000000..57df088 --- /dev/null +++ b/include/ceres/codegen/codegen_cost_function.h
@@ -0,0 +1,90 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2019 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// 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: darius.rueckert@fau.de (Darius Rueckert) +// +#ifndef CERES_PUBLIC_CODEGEN_COST_FUNCTION_H_ +#define CERES_PUBLIC_CODEGEN_COST_FUNCTION_H_ + +#include "ceres/codegen/macros.h" +#include "ceres/sized_cost_function.h" + +namespace ceres { + +// This is the interface for automatically generating cost functor derivative +// code. The template parameters are identical to those of SizedCostFunction<>. +// The behaviour of this class changes between code generation and problem +// solving. +// +// During code generation (when CERES_CODEGEN is defined), this class doesn't do +// anything. The templated operator() from your derived cost functor is used. +// +// After code generation (when CERES_CODEGEN is not defined), this class is a +// SizedCostFunction. The required Evaluate() function was generated and must be +// included into your cost functor. +// +// Usage Example: +// +// #include "ceres/codegen/cost_function.h" +// +// struct HelloWorldCostFunction : public ceres::CodegenCostFunction<1, 1> { +// // A default constructor is required, because the code generator has to +// // create an object of the cost function. +// HelloWorldCostFunction() = default; +// template <typename T> +// bool operator()(const T* x, T* residual) const { +// residual[0] = x[0] * x[0]; +// return true; +// } +// #include "examples/helloworldcostfunction.h" +// }; + +template <int kNumResiduals_, int... Ns> +class CodegenCostFunction +#ifdef CERES_CODEGEN +// During code generation we can't derive from SizedCostFunction. +// The variadic evaluation with Jets would try to call the empty Evaluate() +// instead of the templated functor. +#else + : public SizedCostFunction<kNumResiduals_, Ns...> +#endif +{ + public: + static constexpr int kNumResiduals = kNumResiduals_; + static_assert(kNumResiduals > 0, + "Cost functions must have at least one residual block."); + static_assert(kNumResiduals != DYNAMIC, + "Code generation for dynamic residuals is not yet supported."); + static_assert(internal::StaticParameterDims<Ns...>::kIsValid, + "Invalid parameter block dimension detected. Each parameter " + "block dimension must be bigger than zero."); + using ParameterDims = internal::StaticParameterDims<Ns...>; +}; + +} // namespace ceres +#endif // CERES_PUBLIC_CODEGEN_COST_FUNCTION_H_
diff --git a/include/ceres/codegen/autodiff.h b/include/ceres/codegen/generate_code_for_functor.h similarity index 88% rename from include/ceres/codegen/autodiff.h rename to include/ceres/codegen/generate_code_for_functor.h index 0a2a65d..31ccdb2 100644 --- a/include/ceres/codegen/autodiff.h +++ b/include/ceres/codegen/generate_code_for_functor.h
@@ -42,18 +42,17 @@ struct AutoDiffCodeGenOptions {}; // TODO(darius): Documentation -template <typename CostFunctor, int kNumResiduals, int... Ns> +template <typename DerivedCostFunctor> std::vector<std::string> GenerateCodeForFunctor( const AutoDiffCodeGenOptions& options) { - static_assert(kNumResiduals != DYNAMIC, - "A dynamic number of residuals is currently not supported."); // Define some types and shortcuts to make the code below more readable. - using ParameterDims = internal::StaticParameterDims<Ns...>; + using ParameterDims = typename DerivedCostFunctor::ParameterDims; using Parameters = typename ParameterDims::Parameters; // Instead of using scalar Jets, we use Jets of ExpressionRef which record // their own operations during evaluation. using ExpressionRef = internal::ExpressionRef; using ExprJet = Jet<ExpressionRef, ParameterDims::kNumParameters>; + constexpr int kNumResiduals = DerivedCostFunctor::kNumResiduals; constexpr int kNumParameters = ParameterDims::kNumParameters; constexpr int kNumParameterBlocks = ParameterDims::kNumParameterBlocks; @@ -61,7 +60,11 @@ // Code is generated for the CostFunctor and not an instantiation of it. This // is different to AutoDiffCostFunction, which computes the derivatives for // a specific object. - CostFunctor functor; + static_assert(std::is_default_constructible<DerivedCostFunctor>::value, + "Cost functors used in code generation must have a default " + "constructor. If you are using local variables, make sure to " + "wrap them into the CERES_LOCAL_VARIABLE macro."); + DerivedCostFunctor functor; // During recording phase all operations on ExpressionRefs are recorded to an // internal data structure, the ExpressionGraph. This ExpressionGraph is then @@ -164,7 +167,7 @@ internal::CodeGenerator::Options generator_options; generator_options.function_name = "void EvaluateResidual(double const* const* parameters, double* " - "residuals)"; + "residuals) const"; internal::CodeGenerator gen(residual_graph, generator_options); std::vector<std::string> code = gen.Generate(); output.insert(output.end(), code.begin(), code.end()); @@ -179,7 +182,7 @@ generator_options.function_name = "void EvaluateResidualAndJacobian(double const* const* parameters, " "double* " - "residuals, double** jacobians)"; + "residuals, double** jacobians) const"; internal::CodeGenerator gen(residual_and_jacobian_graph, generator_options); std::vector<std::string> code = gen.Generate(); output.insert(output.end(), code.begin(), code.end()); @@ -193,8 +196,12 @@ // in SizedCostFunctions. output.emplace_back("bool Evaluate(double const* const* parameters,"); output.emplace_back(" double* residuals,"); - output.emplace_back(" double** jacobians) {"); - output.emplace_back(" if (jacobians) {"); + output.emplace_back(" double** jacobians) const {"); + + output.emplace_back(" if (!jacobians) {"); + output.emplace_back(" EvaluateResidual(parameters, residuals);"); + output.emplace_back(" return true;"); + output.emplace_back(" }"); // Create a tmp array of all jacobians and use it for evaluation. // The generated code for a <2,3,1,2> cost functor is: @@ -204,19 +211,19 @@ // jacobians_data + 6, // jacobians_data + 8, // }; - output.emplace_back(" double jacobians_data[" + + output.emplace_back(" double jacobians_data[" + std::to_string(kNumParameters * kNumResiduals) + "];"); - output.emplace_back(" double* jacobians_ptrs[] = {"); + output.emplace_back(" double* jacobians_ptrs[] = {"); for (int i = 0, total_param_id = 0; i < kNumParameterBlocks; total_param_id += ParameterDims::GetDim(i), ++i) { - output.emplace_back(" jacobians_data + " + + output.emplace_back(" jacobians_data + " + std::to_string(kNumResiduals * total_param_id) + ","); } - output.emplace_back(" };"); + output.emplace_back(" };"); // Evaluate into the tmp array. output.emplace_back( - " EvaluateResidualAndJacobian(parameters, residuals, " + " EvaluateResidualAndJacobian(parameters, residuals, " "jacobians_ptrs);"); // Copy the computed jacobians into the output array. Add an if-statement to @@ -238,18 +245,15 @@ // } // } for (int i = 0; i < kNumParameterBlocks; ++i) { - output.emplace_back(" if (jacobians[" + std::to_string(i) + "]) {"); + output.emplace_back(" if (jacobians[" + std::to_string(i) + "]) {"); output.emplace_back( - " for (int i = 0; i < " + + " for (int i = 0; i < " + std::to_string(ParameterDims::GetDim(i) * kNumResiduals) + "; ++i) {"); - output.emplace_back(" jacobians[" + std::to_string(i) + + output.emplace_back(" jacobians[" + std::to_string(i) + "][i] = jacobians_ptrs[" + std::to_string(i) + "][i];"); - output.emplace_back(" }"); output.emplace_back(" }"); + output.emplace_back(" }"); } - output.emplace_back(" return true;"); - output.emplace_back(" }"); - output.emplace_back(" EvaluateResidual(parameters, residuals);"); output.emplace_back(" return true;"); output.emplace_back("}");