blob: 0a2a65d057576c1aad955df594349d31611668f9 [file] [log] [blame]
// 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)
//
#ifndef CERES_PUBLIC_CODEGEN_AUTODIFF_H_
#define CERES_PUBLIC_CODEGEN_AUTODIFF_H_
#include "ceres/codegen/internal/code_generator.h"
#include "ceres/codegen/internal/expression_graph.h"
#include "ceres/codegen/internal/expression_ref.h"
#include "ceres/internal/autodiff.h"
#include "ceres/jet.h"
namespace ceres {
struct AutoDiffCodeGenOptions {};
// TODO(darius): Documentation
template <typename CostFunctor, int kNumResiduals, int... Ns>
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 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 kNumParameters = ParameterDims::kNumParameters;
constexpr int kNumParameterBlocks = ParameterDims::kNumParameterBlocks;
// Create the cost functor using the default constructor.
// 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;
// During recording phase all operations on ExpressionRefs are recorded to an
// internal data structure, the ExpressionGraph. This ExpressionGraph is then
// optimized and converted back into C++ code.
internal::StartRecordingExpressions();
// The Jet arrays are defined after StartRecordingExpressions, because Jets
// are zero-initialized in the default constructor. This already creates
// COMPILE_TIME_CONSTANT expressions.
std::array<ExprJet, kNumParameters> all_parameters;
std::array<ExprJet, kNumResiduals> residuals;
std::array<ExprJet*, kNumParameterBlocks> unpacked_parameters =
ParameterDims::GetUnpackedParameters(all_parameters.data());
// Create input expressions that convert from the doubles passed from Ceres
// into codegen Expressions. These inputs are assigned to the scalar part "a"
// of the corresponding Jets.
//
// Example code generated by these expressions:
// v_0 = parameters[0][0];
// v_1 = parameters[0][1];
// ...
for (int i = 0; i < kNumParameterBlocks; ++i) {
for (int j = 0; j < ParameterDims::GetDim(i); ++j) {
ExprJet& parameter = unpacked_parameters[i][j];
parameter.a = internal::MakeInputAssignment<ExpressionRef>(
0.0,
("parameters[" + std::to_string(i) + "][" + std::to_string(j) + "]")
.c_str());
}
}
// During the array initialization above, the derivative part of the Jets is
// set to zero. Here, we set the correct element to 1.
for (int i = 0; i < kNumParameters; ++i) {
all_parameters[i].v(i) = ExpressionRef(1);
}
// Run the cost functor with Jets of ExpressionRefs.
// Since we are still in recording mode, all operations of the cost functor
// will be added to the graph.
internal::VariadicEvaluate<ParameterDims>(
functor, unpacked_parameters.data(), residuals.data());
// At this point the Jets in 'residuals' contain references to the output
// expressions. Here we add new expressions that assign the generated
// temporaries to the actual residual array.
//
// Example code generated by these expressions:
// residuals[0] = v_200;
// residuals[1] = v_201;
// ...
for (int i = 0; i < kNumResiduals; ++i) {
auto& J = residuals[i];
// Note: MakeOutput automatically adds the expression to the active graph.
internal::MakeOutput(J.a, "residuals[" + std::to_string(i) + "]");
}
// Make a copy of the current graph so we can generated a function for the
// residuals without jacobians.
auto residual_graph = *internal::GetCurrentExpressionGraph();
// Same principle as above for the residuals.
//
// Example code generated by these expressions:
// jacobians[0][0] = v_351;
// jacobians[0][1] = v_352;
// ...
for (int i = 0, total_param_id = 0; i < kNumParameterBlocks;
total_param_id += ParameterDims::GetDim(i), ++i) {
for (int r = 0; r < kNumResiduals; ++r) {
for (int j = 0; j < ParameterDims::GetDim(i); ++j) {
internal::MakeOutput(
(residuals[r].v[total_param_id + j]),
"jacobians[" + std::to_string(i) + "][" +
std::to_string(r * ParameterDims::GetDim(i) + j) + "]");
}
}
}
// Stop recording and return the current active graph. Performing operations
// of ExpressionRef after this line will result in an error.
auto residual_and_jacobian_graph = internal::StopRecordingExpressions();
// TODO(darius): Once the optimizer is in place, call it from
// here to optimize the code before generating.
// We have the optimized code of the cost functor stored in the
// ExpressionGraphs. Now we generate C++ code for it and place it line-by-line
// in this vector of strings.
std::vector<std::string> output;
output.emplace_back("// This file is generated with ceres::AutoDiffCodeGen.");
output.emplace_back("// http://ceres-solver.org/");
output.emplace_back("");
{
// Generate C++ code for the EvaluateResidual function and append it to the
// output.
internal::CodeGenerator::Options generator_options;
generator_options.function_name =
"void EvaluateResidual(double const* const* parameters, double* "
"residuals)";
internal::CodeGenerator gen(residual_graph, generator_options);
std::vector<std::string> code = gen.Generate();
output.insert(output.end(), code.begin(), code.end());
}
output.emplace_back("");
{
// Generate C++ code for the EvaluateResidualAndJacobian function and append
// it to the output.
internal::CodeGenerator::Options generator_options;
generator_options.function_name =
"void EvaluateResidualAndJacobian(double const* const* parameters, "
"double* "
"residuals, double** jacobians)";
internal::CodeGenerator gen(residual_and_jacobian_graph, generator_options);
std::vector<std::string> code = gen.Generate();
output.insert(output.end(), code.begin(), code.end());
}
output.emplace_back("");
// Generate a generic combined function, which calls EvaluateResidual and
// EvaluateResidualAndJacobian. This combined function is compatible to
// CostFunction::Evaluate. Therefore the generated code can be directly used
// 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) {");
// Create a tmp array of all jacobians and use it for evaluation.
// The generated code for a <2,3,1,2> cost functor is:
// double jacobians_data[6];
// double* jacobians_ptrs[] = {
// jacobians_data + 0,
// jacobians_data + 6,
// jacobians_data + 8,
// };
output.emplace_back(" double jacobians_data[" +
std::to_string(kNumParameters * kNumResiduals) + "];");
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 + " +
std::to_string(kNumResiduals * total_param_id) + ",");
}
output.emplace_back(" };");
// Evaluate into the tmp array.
output.emplace_back(
" EvaluateResidualAndJacobian(parameters, residuals, "
"jacobians_ptrs);");
// Copy the computed jacobians into the output array. Add an if-statement to
// test for null-jacobians. The generated code for a <2,3,1,2> cost functor
// is:
// if (jacobians[0]) {
// for (int i = 0; i < 6; ++i) {
// jacobians[0][i] = jacobians_tmp[0][i];
// }
// }
// if (jacobians[1]) {
// for (int i = 0; i < 2; ++i) {
// jacobians[1][i] = jacobians_tmp[1][i];
// }
// }
// if (jacobians[2]) {
// for (int i = 0; i < 4; ++i) {
// jacobians[2][i] = jacobians_tmp[2][i];
// }
// }
for (int i = 0; i < kNumParameterBlocks; ++i) {
output.emplace_back(" if (jacobians[" + std::to_string(i) + "]) {");
output.emplace_back(
" for (int i = 0; i < " +
std::to_string(ParameterDims::GetDim(i) * kNumResiduals) + "; ++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(" return true;");
output.emplace_back(" }");
output.emplace_back(" EvaluateResidual(parameters, residuals);");
output.emplace_back(" return true;");
output.emplace_back("}");
return output;
}
} // namespace ceres
#endif // CERES_PUBLIC_CODEGEN_AUTODIFF_H_