saberbud/ceres-solver
1
0
Fork 0
mirror of https://github.com/ceres-solver/ceres-solver.git synced 2026-08-30 00:50:37 +08:00
Code Issues Packages Projects Releases Wiki Activity

Remove AutodiffCodegen

Browse Source 
- Remove Codegen files
- Revert Jet and Rotation

Change-Id: I005c5f98f2b6dfa5c7fd88d998b6aa83e47dab60
This commit is contained in:
Darius Rueckert
2020-04-08 10:43:53 +02:00
parent e9eb76f8ef
commit e751d6e4f0
22 changed files with 56 additions and 2757 deletions
Download Patch File Download Diff File Expand all files Collapse all files
include/ceres/codegen/codegen_cost_function.h
-90
View File
@@ -1,90 +0,0 @@
// 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_
include/ceres/codegen/generate_code_for_functor.h
-221
View File
@@ -1,221 +0,0 @@
// Ceres Solver - A fast non-linear least squares minimizer
// Copyright 2019 Google Inc. All rights reserved.
// http://ceres-solver.org/
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//
// * Redistributions of source code must retain the above copyright notice,
// this list of conditions and the following disclaimer.
// * Redistributions in binary form must reproduce the above copyright notice,
// this list of conditions and the following disclaimer in the documentation
// and/or other materials provided with the distribution.
// * Neither the name of Google Inc. nor the names of its contributors may be
// used to endorse or promote products derived from this software without
// specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//
// Author: 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 DerivedCostFunctor>
std::vector<std::string> GenerateCodeForFunctor(
const AutoDiffCodeGenOptions& options) {
// Define some types and shortcuts to make the code below more readable.
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;
// 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.
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
// 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) + "]");
}
// 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 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) 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());
}
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) const {");
output.emplace_back(" if (!jacobians) {");
output.emplace_back(" // Use the input cost functor");
output.emplace_back(" return (*this)(");
for (int i = 0; i < kNumParameterBlocks; ++i) {
output.emplace_back(" parameters[" + std::to_string(i) + "],");
}
output.emplace_back(" residuals");
output.emplace_back(" );");
output.emplace_back(" }");
// Create a tmp array of all jacobians and use it for evaluation if the input
// jacobian is null. The generated code for a <2,3,1,2> cost functor is:
// double jacobians_data[6];
// double* jacobians_ptrs[] = {
// jacobians[0] ? jacobians[0] : jacobians_data + 0,
// jacobians[1] ? jacobians[1] : jacobians_data + 6,
// jacobians[2] ? jacobians[2] : 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[" + std::to_string(i) +
"] ? jacobians[" + std::to_string(i) +
"] : jacobians_data + " +
std::to_string(kNumResiduals * total_param_id) + ",");
}
output.emplace_back(" };");
output.emplace_back(
" EvaluateResidualAndJacobian(parameters, residuals, "
"jacobians_ptrs);");
output.emplace_back(" return true;");
output.emplace_back("}");
return output;
}
} // namespace ceres
#endif // CERES_PUBLIC_CODEGEN_AUTODIFF_H_
include/ceres/codegen/internal/code_generator.h
-120
View File
@@ -1,120 +0,0 @@
// 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_INTERNAL_CODE_GENERATOR_H_
#define CERES_PUBLIC_CODEGEN_INTERNAL_CODE_GENERATOR_H_
#include "ceres/codegen/internal/expression.h"
#include "ceres/codegen/internal/expression_graph.h"
#include <string>
#include <vector>
namespace ceres {
namespace internal {
// This class is used to convert an expression graph into a string. The typical
// pipeline is:
//
// 1. Record ExpressionGraph
// 2. Optimize ExpressionGraph
// 3. Generate C++ code (this class here)
//
// The CodeGenerator operates in the following way:
//
// 1. Print Header
// - The header string is defined in the options.
// - This is usually the function name including the parameter list.
//
// 2. Print Declarations
// - Declare all used variables
// - Example:
// double v_0;
// double v_1;
// bool v_3;
// ...
//
// 3. Print Code
// - Convert each expression line by line to a string
// - Example:
// v_2 = v_0 + v_1
// if (v_5) {
// v_2 = v_0;
// ....
//
class CodeGenerator {
public:
struct Options {
// Name of the function.
// Example:
// bool Evaluate(const double* x, double* res)
std::string function_name = "";
// Number of spaces added for each level of indentation.
int indentation_spaces_per_level = 2;
// The prefix added to each variable name.
std::string variable_prefix = "v_";
};
CodeGenerator(const ExpressionGraph& graph, const Options& options);
// Generate the C++ code in the steps (1)-(3) described above.
// The result is a vector of strings, where each element is exactly one line
// of code. The order is important and must not be changed.
std::vector<std::string> Generate();
private:
// Converts a single expression given by id to a string.
// The format depends on the ExpressionType.
// See ExpressionType in expression.h for more detailed how the different
// lines will look like.
std::string ExpressionToString(ExpressionId id);
// Helper function to get the name of an expression.
// If the expression does not have a valid name an error is generated.
std::string VariableForExpressionId(ExpressionId id);
// Adds one level of indentation. Called when an IF expression is encountered.
void PushIndentation();
// Removes one level of indentation. Currently only used by ENDIF.
void PopIndentation();
const ExpressionGraph& graph_;
const Options options_;
std::string indentation_ = "";
static constexpr int kFloatingPointPrecision = 25;
};
} // namespace internal
} // namespace ceres
#endif // CERES_PUBLIC_CODEGEN_INTERNAL_CODE_GENERATOR_H_
include/ceres/codegen/internal/eliminate_nops.h
-67
View File
@@ -1,67 +0,0 @@
// Ceres Solver - A fast non-linear least squares minimizer
// Copyright 2020 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_INTERNAL_ELIMINATE_NOPS_H_
#define CERES_PUBLIC_CODEGEN_INTERNAL_ELIMINATE_NOPS_H_
#include "ceres/codegen/internal/expression_graph.h"
#include "ceres/codegen/internal/optimization_pass_summary.h"
namespace ceres {
namespace internal {
// [OptimizationPass] NOP Cleanup
//
// Short Description:
// Removes all expression with type==ExpressionType::NOP.
//
// Description:
// Removing an expression not at the end requires reindexing of all later
// expressions. Therefore, other optimization passes replace expression with
// NOP instead of removing them. This optimization pass removes all NOPs back
// to front.
// The returned value is equal to the number of removed NOPs.
//
// Example:
// v_0 = 1;
// // NOP
// v_2 = 2;
// v_3 = v_0 + v_2
// Transforms to:
// v_0 = 1;
// v_1 = 2;
// v_2 = v_0 + v_1;
OptimizationPassSummary EliminateNops(ExpressionGraph* graph);
} // namespace internal
} // namespace ceres
#endif // CERES_PUBLIC_CODEGEN_INTERNAL_ELIMINATE_NOPS_H_
include/ceres/codegen/internal/expression.h
-405
View File
@@ -1,405 +0,0 @@
// 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)
//
// During code generation, your cost functor is converted into a list of
// expressions stored in an expression graph. For each operator (+,-,=,...),
// function call (sin,cos,...), and special keyword (if,else,...) the
// appropriate ExpressionType is selected. On a high level all ExpressionTypes
// are grouped into two different classes: Arithmetic expressions and control
// expressions.
//
// Part 1: Arithmetic Expressions
//
// Arithmetic expression are the most basic and common types. They are all of
// the following form:
//
// <lhs> = <rhs>
//
// <lhs> is the variable name on the left hand side of the assignment. <rhs> can
// be different depending on the ExpressionType. It must evaluate to a single
// scalar value though. Here are a few examples of arithmetic expressions (the
// ExpressionType is given on the right):
//
// v_0 = 3.1415; // COMPILE_TIME_CONSTANT
// v_1 = v_0; // ASSIGNMENT
// v_2 = v_0 + v_1; // PLUS
// v_3 = v_2 / v_0; // DIVISION
// v_4 = sin(v_3); // FUNCTION_CALL
// v_5 = v_4 < v_3; // BINARY_COMPARISON
//
// As you can see, the right hand side of each expression contains exactly one
// operator/value/function call. If you write long expressions like
//
// T c = a + b - T(3) * a;
//
// it will broken up into the individual expressions like so:
//
// v_0 = a + b;
// v_1 = 3;
// v_2 = v_1 * a;
// c = v_0 - v_2;
//
// All arithmetic expressions are generated by operator and function
// overloading. These overloads are defined in expression_ref.h.
//
//
//
// Part 2: Control Expressions
//
// Control expressions include special instructions that handle the control flow
// of a program. So far, only if/else is supported, but while/for might come in
// the future.
//
// Generating code for conditional jumps (if/else) is more complicated than
// for arithmetic expressions. Let's look at a small example to see the
// problems. After that we explain how these problems are solved in Ceres.
//
// 1 T a = parameters[0][0];
// 2 T b = 1.0;
// 3 if (a < b) {
// 4 b = 3.0;
// 5 } else {
// 6 b = 4.0;
// 7 }
// 8 b += 1.0;
// 9 residuals[0] = b;
//
// Problem 1.
// We need to generate code for both branches. In C++ there is no way to execute
// both branches of an if, but we need to execute them to generate the code.
//
// Problem 2.
// The comparison a < b in line 3 is not convertible to bool. Since the value of
// a is not known during code generation, the expression a < b can not be
// evaluated. In fact, a < b will return an expression of type
// BINARY_COMPARISON.
//
// Problem 3.
// There is no way to record that an if was executed. "if" is a special operator
// which cannot be overloaded. Therefore we can't generate code that contains
// "if.
//
// Problem 4.
// We have no information about "blocks" or "scopes" during code generation.
// Even if we could overload the if-operator, there is now way to capture which
// expression was executed in which branches of the if. For example, we generate
// code for the else branch. How can we know that the else branch is finished?
// Is line 8 inside the else-block or already outside?
//
// Solution.
// Instead of using the keywords if/else we insert the macros
// CERES_IF, CERES_ELSE and CERES_ENDIF. These macros just map to a function,
// which inserts an expression into the graph. Here is how the example from
// above looks like with the expanded macros:
//
// 1 T a = parameters[0][0];
// 2 T b = 1.0;
// 3 CreateIf(a < b); {
// 4 b = 3.0;
// 5 } CreateElse(); {
// 6 b = 4.0;
// 7 } CreateEndif();
// 8 b += 1.0;
// 9 residuals[0] = b;
//
// Problem 1 solved.
// There are no branches during code generation, therefore both blocks are
// evaluated.
//
// Problem 2 solved.
// The function CreateIf(_) does not take a bool as argument, but an
// ComparisonExpression. Later during code generation an actual "if" is created
// with the condition as argument.
//
// Problem 3 solved.
// We replaced "if" by a function call so we can record it now.
//
// Problem 4 solved.
// Expressions are added into the graph in the correct order. That means, after
// seeing a CreateIf() we know that all following expressions until CreateElse()
// belong to the true-branch. Similar, all expression from CreateElse() to
// CreateEndif() belong to the false-branch. This also works recursively with
// nested ifs.
//
// If you want to use the AutoDiff code generation for your cost functors, you
// have to replace all if/else by the CERES_IF, CERES_ELSE and CERES_ENDIF
// macros. The example from above looks like this:
//
// 1 T a = parameters[0][0];
// 2 T b = 1.0;
// 3 CERES_IF (a < b) {
// 4 b = 3.0;
// 5 } CERES_ELSE {
// 6 b = 4.0;
// 7 } CERES_ENDIF;
// 8 b += 1.0;
// 9 residuals[0] = b;
//
// These macros don't have a negative impact on performance, because they only
// expand to the CreateIf/.. functions in code generation mode. Otherwise they
// expand to the if/else keywords. See expression_ref.h for the exact
// definition.
//
#ifndef CERES_PUBLIC_CODEGEN_INTERNAL_EXPRESSION_H_
#define CERES_PUBLIC_CODEGEN_INTERNAL_EXPRESSION_H_
#include <string>
#include <vector>
namespace ceres {
namespace internal {
using ExpressionId = int;
static constexpr ExpressionId kInvalidExpressionId = -1;
enum class ExpressionType {
// v_0 = 3.1415;
COMPILE_TIME_CONSTANT,
// Assignment from a user-variable to a generated variable that can be used by
// other expressions. This is used for local variables of cost functors and
// parameters of a functions.
// v_0 = _observed_point_x;
// v_0 = parameters[0][0];
INPUT_ASSIGNMENT,
// Assignment from a generated variable to a user-variable. Used to store the
// output of a generated cost functor.
// residual[0] = v_51;
OUTPUT_ASSIGNMENT,
// Trivial assignment
// v_3 = v_1
ASSIGNMENT,
// Binary Arithmetic Operations
// v_2 = v_0 + v_1
// The operator is stored in Expression::name_.
BINARY_ARITHMETIC,
// Unary Arithmetic Operation
// v_1 = -(v_0);
// v_2 = +(v_1);
// The operator is stored in Expression::name_.
UNARY_ARITHMETIC,
// Binary Comparison. (<,>,&&,...)
// This is the only expressions which returns a 'bool'.
// v_2 = v_0 < v_1
// The operator is stored in Expression::name_.
BINARY_COMPARISON,
// The !-operator on logical expression.
LOGICAL_NEGATION,
// General Function Call.
// v_5 = f(v_0,v_1,...)
FUNCTION_CALL,
// Conditional control expressions if/else/endif.
// These are special expressions, because they don't define a new variable.
IF,
ELSE,
ENDIF,
// A single comment line. Even though comments are 'unused' expression they
// will not be optimized away.
COMMENT,
// No Operation. A placeholder for an 'empty' expressions which will be
// optimized out during code generation.
NOP
};
enum class ExpressionReturnType {
// The expression returns a scalar value (float or double). Used for most
// arithmetic operations and function calls.
SCALAR,
// The expression returns a boolean value. Used for logical expressions
// v_3 = v_1 < v_2
// and functions returning a bool
// v_3 = isfinite(v_1);
BOOLEAN,
// The expressions doesn't return a value. Used for the control
// expressions
// and NOP.
VOID,
};
std::string ExpressionReturnTypeToString(ExpressionReturnType type);
// This class contains all data that is required to generate one line of code.
// Each line has the following form:
//
// lhs = rhs;
//
// The left hand side is the variable name given by its own id. The right hand
// side depends on the ExpressionType. For example, a COMPILE_TIME_CONSTANT
// expressions with id 4 generates the following line:
// v_4 = 3.1415;
//
// Objects of this class are created indirectly using the static CreateXX
// methods. During creation, the Expression objects are added to the
// ExpressionGraph (see expression_graph.h).
class Expression {
public:
// Creates a NOP expression.
Expression() = default;
Expression(ExpressionType type,
ExpressionReturnType return_type = ExpressionReturnType::VOID,
ExpressionId lhs_id = kInvalidExpressionId,
const std::vector<ExpressionId>& arguments = {},
const std::string& name = "",
double value = 0);
// Helper 'constructors' that create an Expression with the correct type. You
// can also use the actual constructor from above, but using the create
// functions is less prone to errors.
static Expression CreateCompileTimeConstant(double v);
static Expression CreateInputAssignment(const std::string& name);
static Expression CreateOutputAssignment(ExpressionId v,
const std::string& name);
static Expression CreateAssignment(ExpressionId dst, ExpressionId src);
static Expression CreateBinaryArithmetic(const std::string& op,
ExpressionId l,
ExpressionId r);
static Expression CreateUnaryArithmetic(const std::string& op,
ExpressionId v);
static Expression CreateBinaryCompare(const std::string& name,
ExpressionId l,
ExpressionId r);
static Expression CreateLogicalNegation(ExpressionId v);
static Expression CreateScalarFunctionCall(
const std::string& name, const std::vector<ExpressionId>& params);
static Expression CreateLogicalFunctionCall(
const std::string& name, const std::vector<ExpressionId>& params);
static Expression CreateIf(ExpressionId condition);
static Expression CreateElse();
static Expression CreateEndIf();
static Expression CreateComment(const std::string& comment);
// Returns true if this is an arithmetic expression.
// Arithmetic expressions must have a valid left hand side.
bool IsArithmeticExpression() const;
// Returns true if this is a control expression.
bool IsControlExpression() const;
// If this expression is the compile time constant with the given value.
// Used during optimization to collapse zero/one arithmetic operations.
// b = a + 0; -> b = a;
bool IsCompileTimeConstantAndEqualTo(double constant) const;
// Checks if "other" is identical to "this" so that one of the epxressions can
// be replaced by a trivial assignment. Used during common subexpression
// elimination.
bool IsReplaceableBy(const Expression& other) const;
// Replace this expression by 'other'.
// The current id will be not replaced. That means other experssions
// referencing this one stay valid.
void Replace(const Expression& other);
// If this expression has 'other' as an argument.
bool DirectlyDependsOn(ExpressionId other) const;
// Converts this expression into a NOP
void MakeNop();
// Returns true if this expression has a valid lhs.
bool HasValidLhs() const { return lhs_id_ != kInvalidExpressionId; }
// Compares all members with the == operator. If this function succeeds,
// IsSemanticallyEquivalentTo will also return true.
bool operator==(const Expression& other) const;
bool operator!=(const Expression& other) const { return !(*this == other); }
// Semantically equivalent expressions are similar in a way, that the type(),
// value(), name(), number of arguments is identical. The lhs_id() and the
// argument_ids can differ. For example, the following groups of expressions
// are semantically equivalent:
//
// v_0 = v_1 + v_2;
// v_0 = v_1 + v_3;
// v_1 = v_1 + v_2;
//
// v_0 = sin(v_1);
// v_3 = sin(v_2);
bool IsSemanticallyEquivalentTo(const Expression& other) const;
ExpressionType type() const { return type_; }
ExpressionReturnType return_type() const { return return_type_; }
ExpressionId lhs_id() const { return lhs_id_; }
double value() const { return value_; }
const std::string& name() const { return name_; }
const std::vector<ExpressionId>& arguments() const { return arguments_; }
void set_lhs_id(ExpressionId new_lhs_id) { lhs_id_ = new_lhs_id; }
std::vector<ExpressionId>* mutable_arguments() { return &arguments_; }
private:
ExpressionType type_ = ExpressionType::NOP;
ExpressionReturnType return_type_ = ExpressionReturnType::VOID;
// If lhs_id_ >= 0, then this expression is assigned to v_<lhs_id>.
// For example:
// v_1 = v_0 + v_0 (Type = PLUS)
// v_3 = sin(v_1) (Type = FUNCTION_CALL)
// ^
// lhs_id_
//
// If lhs_id_ == kInvalidExpressionId, then the expression type is not
// arithmetic. Currently, only the following types have lhs_id = invalid:
// IF,ELSE,ENDIF,NOP
ExpressionId lhs_id_ = kInvalidExpressionId;
// Expressions have different number of arguments. For example a binary "+"
// has 2 parameters and a function call to "sin" has 1 parameter. Here, a
// reference to these paratmers is stored. Note: The order matters!
std::vector<ExpressionId> arguments_;
// Depending on the type this name is one of the following:
// (type == FUNCTION_CALL) -> the function name
// (type == PARAMETER) -> the parameter name
// (type == OUTPUT_ASSIGN) -> the output variable name
// (type == BINARY_COMPARE)-> the comparison symbol "<","&&",...
// else -> unused
std::string name_;
// Only valid if type == COMPILE_TIME_CONSTANT
double value_ = 0;
};
} // namespace internal
} // namespace ceres
#endif // CERES_PUBLIC_CODEGEN_INTERNAL_EXPRESSION_H_
include/ceres/codegen/internal/expression_graph.h
-140
View File
@@ -1,140 +0,0 @@
// 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_INTERNAL_EXPRESSION_GRAPH_H_
#define CERES_PUBLIC_CODEGEN_INTERNAL_EXPRESSION_GRAPH_H_
#include <vector>
#include "expression.h"
namespace ceres {
namespace internal {
// A directed, acyclic, unconnected graph containing all expressions of a
// program.
//
// The expression graph is stored linear in the expressions_ array. The order is
// identical to the execution order. Each expression can have multiple children
// and multiple parents.
// A is child of B <=> B has A as a parameter <=> B.DirectlyDependsOn(A)
// A is parent of B <=> A has B as a parameter <=> A.DirectlyDependsOn(B)
class ExpressionGraph {
public:
// Checks if A depends on B.
// -> B is a descendant of A
bool DependsOn(ExpressionId A, ExpressionId B) const;
bool operator==(const ExpressionGraph& other) const;
bool operator!=(const ExpressionGraph& other) const {
return !(*this == other);
}
Expression& ExpressionForId(ExpressionId id) { return expressions_[id]; }
const Expression& ExpressionForId(ExpressionId id) const {
return expressions_[id];
}
int Size() const { return expressions_.size(); }
// Erases the expression at "location". All expression after "location" are
// moved by one element to the front. References to moved expressions are
// updated. Removing an expression that is still referenced somewhere is
// undefined behaviour.
void Erase(ExpressionId location);
// Insert a new expression at "location" into the graph. All expression
// after "location" are moved by one element to the back. References to
// moved expressions are updated.
void Insert(ExpressionId location, const Expression& expression);
// Adds an Expression to the end of the expression list and creates a new
// variable for the result. The id of the result variable is returned so it
// can be used for further operations.
ExpressionId InsertBack(const Expression& expression);
// Finds the closing ENDIF expression for a given IF expression. Calling this
// method is only valid on IF expressions. If no suitable ENDIF is found,
// kInvalidExpressionId is returned. Example:
// <id> <expr> FindMatchingEndif(id)
// 0 IF 7
// 1 IF 3
// 2 ELSE -
// 3 ENDIF -
// 4 ELSE -
// 5 IF 6
// 6 ENDIF -
// 7 ENDIF -
ExpressionId FindMatchingEndif(ExpressionId id) const;
// Similar to FindMatchingEndif, but returns the matching ELSE expression. If
// no suitable ELSE is found, kInvalidExpressionId is returned.
// FindMatchingElse does not throw an error is this case, because IF without
// ELSE is allowed.
// <id> <expr> FindMatchingEndif(id)
// 0 IF 4
// 1 IF 2
// 2 ELSE -
// 3 ENDIF -
// 4 ELSE -
// 5 IF kInvalidEpressionId
// 6 ENDIF -
// 7 ENDIF -
ExpressionId FindMatchingElse(ExpressionId id) const;
private:
// All Expressions are referenced by an ExpressionId. The ExpressionId is
// the index into this array. Each expression has a list of ExpressionId as
// arguments. These references form the graph.
std::vector<Expression> expressions_;
};
// After calling this method, all operations on 'ExpressionRef' objects will
// be recorded into an ExpressionGraph. You can obtain this graph by calling
// StopRecordingExpressions.
//
// Performing expression operations before calling StartRecordingExpressions
// or calling StartRecodring. twice is an error.
void StartRecordingExpressions();
// Stops recording and returns all expressions that have been executed since
// the call to StartRecordingExpressions. The internal ExpressionGraph will be
// invalidated and a second consecutive call to this method results in an
// error.
ExpressionGraph StopRecordingExpressions();
// Returns a pointer to the active expression tree.
// Normal users should not use this functions.
ExpressionGraph* GetCurrentExpressionGraph();
} // namespace internal
} // namespace ceres
#endif // CERES_PUBLIC_CODEGEN_INTERNAL_EXPRESSION_GRAPH_H_
include/ceres/codegen/internal/expression_ref.h
-248
View File
@@ -1,248 +0,0 @@
// 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)
//
// TODO: Documentation
#ifndef CERES_PUBLIC_EXPRESSION_REF_H_
#define CERES_PUBLIC_EXPRESSION_REF_H_
#include <string>
#include "ceres/codegen/internal/expression.h"
#include "ceres/codegen/internal/types.h"
namespace ceres {
namespace internal {
// This class represents a scalar value that creates new expressions during
// evaluation. ExpressionRef can be used as template parameter for cost functors
// and Jets.
//
// ExpressionRef should be passed by value.
struct ExpressionRef {
ExpressionRef() : ExpressionRef(0.0) {}
// Create a compile time constant expression directly from a double value.
// This is important so that we can write T(3.14) in our code and
// it's automatically converted to the correct expression.
//
// This constructor is implicit, because the line
// T a(0);
// must work for T = Jet<ExpressionRef>.
ExpressionRef(double compile_time_constant);
// Adds the expression to the active graph and initializes this ExpressionRef
// accordingly.
ExpressionRef(const Expression& expression);
// By adding this deleted constructor we can detect invalid usage of
// ExpressionRef. ExpressionRef must only be created from constexpr doubles.
//
// If you get a compile error here, you have probably written something like:
// T x = local_variable_;
// Change this into:
// T x = CERES_LOCAL_VARIABLE(local_variable_);
ExpressionRef(double&) = delete;
// Copy construction/assignment creates an ASSIGNMENT expression from
// 'other' to 'this'.
//
// For example:
// a = b; // With a.id = 5 and b.id = 3
// will generate the following assignment:
// v_5 = v_3;
//
// If 'this' ExpressionRef is currently not pointing to a variable
// (id==invalid), then an assignment to a new variable is generated. Example:
// T a = 5;
// T b;
// b = a; // During the assignment 'b' is invalid
//
// The right hand side of the assignment (= the argument 'other') must be
// valid in every case. The following code will result in an error.
// T a;
// T b = a; // Error: Uninitialized assignment
ExpressionRef(const ExpressionRef& other);
ExpressionRef& operator=(const ExpressionRef& other);
// Compound operators
ExpressionRef& operator+=(const ExpressionRef& x);
ExpressionRef& operator-=(const ExpressionRef& x);
ExpressionRef& operator*=(const ExpressionRef& x);
ExpressionRef& operator/=(const ExpressionRef& x);
bool IsInitialized() const { return id != kInvalidExpressionId; }
// The index into the ExpressionGraph data array.
ExpressionId id = kInvalidExpressionId;
};
// A helper function which calls 'InsertBack' on the currently active graph.
// This wrapper also checks if StartRecordingExpressions was called. See
// ExpressionGraph::InsertBack for more information.
ExpressionRef AddExpressionToGraph(const Expression& expression);
// Arithmetic Operators
ExpressionRef operator-(const ExpressionRef& x);
ExpressionRef operator+(const ExpressionRef& x);
ExpressionRef operator+(const ExpressionRef& x, const ExpressionRef& y);
ExpressionRef operator-(const ExpressionRef& x, const ExpressionRef& y);
ExpressionRef operator*(const ExpressionRef& x, const ExpressionRef& y);
ExpressionRef operator/(const ExpressionRef& x, const ExpressionRef& y);
// Functions
#define CERES_DEFINE_UNARY_FUNCTION_CALL(ns, name) \
inline ExpressionRef name(const ExpressionRef& x) { \
return AddExpressionToGraph( \
Expression::CreateScalarFunctionCall(#ns "::" #name, {x.id})); \
}
#define CERES_DEFINE_BINARY_FUNCTION_CALL(ns, name) \
inline ExpressionRef name(const ExpressionRef& x, const ExpressionRef& y) { \
return AddExpressionToGraph( \
Expression::CreateScalarFunctionCall(#ns "::" #name, {x.id, y.id})); \
}
CERES_DEFINE_UNARY_FUNCTION_CALL(std, abs);
CERES_DEFINE_UNARY_FUNCTION_CALL(std, acos);
CERES_DEFINE_UNARY_FUNCTION_CALL(std, asin);
CERES_DEFINE_UNARY_FUNCTION_CALL(std, atan);
CERES_DEFINE_UNARY_FUNCTION_CALL(std, cbrt);
CERES_DEFINE_UNARY_FUNCTION_CALL(std, ceil);
CERES_DEFINE_UNARY_FUNCTION_CALL(std, cos);
CERES_DEFINE_UNARY_FUNCTION_CALL(std, cosh);
CERES_DEFINE_UNARY_FUNCTION_CALL(std, exp);
CERES_DEFINE_UNARY_FUNCTION_CALL(std, exp2);
CERES_DEFINE_UNARY_FUNCTION_CALL(std, floor);
CERES_DEFINE_UNARY_FUNCTION_CALL(std, log);
CERES_DEFINE_UNARY_FUNCTION_CALL(std, log2);
CERES_DEFINE_UNARY_FUNCTION_CALL(std, sin);
CERES_DEFINE_UNARY_FUNCTION_CALL(std, sinh);
CERES_DEFINE_UNARY_FUNCTION_CALL(std, sqrt);
CERES_DEFINE_UNARY_FUNCTION_CALL(std, tan);
CERES_DEFINE_UNARY_FUNCTION_CALL(std, tanh);
CERES_DEFINE_BINARY_FUNCTION_CALL(std, atan2);
CERES_DEFINE_BINARY_FUNCTION_CALL(std, pow);
#undef CERES_DEFINE_UNARY_FUNCTION_CALL
#undef CERES_DEFINE_BINARY_FUNCTION_CALL
// This additonal type is required, so that we can detect invalid conditions
// during compile time. For example, the following should create a compile time
// error:
//
// ExpressionRef a(5);
// CERES_IF(a){ // Error: Invalid conversion
// ...
//
// Following will work:
//
// ExpressionRef a(5), b(7);
// ComparisonExpressionRef c = a < b;
// CERES_IF(c){
// ...
struct ComparisonExpressionRef {
ExpressionId id;
explicit ComparisonExpressionRef(const ExpressionRef& ref) : id(ref.id) {}
};
ExpressionRef Ternary(const ComparisonExpressionRef& c,
const ExpressionRef& x,
const ExpressionRef& y);
// Comparison operators
ComparisonExpressionRef operator<(const ExpressionRef& x,
const ExpressionRef& y);
ComparisonExpressionRef operator<=(const ExpressionRef& x,
const ExpressionRef& y);
ComparisonExpressionRef operator>(const ExpressionRef& x,
const ExpressionRef& y);
ComparisonExpressionRef operator>=(const ExpressionRef& x,
const ExpressionRef& y);
ComparisonExpressionRef operator==(const ExpressionRef& x,
const ExpressionRef& y);
ComparisonExpressionRef operator!=(const ExpressionRef& x,
const ExpressionRef& y);
// Logical Operators
ComparisonExpressionRef operator&&(const ComparisonExpressionRef& x,
const ComparisonExpressionRef& y);
ComparisonExpressionRef operator||(const ComparisonExpressionRef& x,
const ComparisonExpressionRef& y);
ComparisonExpressionRef operator&(const ComparisonExpressionRef& x,
const ComparisonExpressionRef& y);
ComparisonExpressionRef operator|(const ComparisonExpressionRef& x,
const ComparisonExpressionRef& y);
ComparisonExpressionRef operator!(const ComparisonExpressionRef& x);
#define CERES_DEFINE_UNARY_LOGICAL_FUNCTION_CALL(ns, name) \
inline ComparisonExpressionRef name(const ExpressionRef& x) { \
return ComparisonExpressionRef(AddExpressionToGraph( \
Expression::CreateLogicalFunctionCall(#ns "::" #name, {x.id}))); \
}
CERES_DEFINE_UNARY_LOGICAL_FUNCTION_CALL(std, isfinite);
CERES_DEFINE_UNARY_LOGICAL_FUNCTION_CALL(std, isinf);
CERES_DEFINE_UNARY_LOGICAL_FUNCTION_CALL(std, isnan);
CERES_DEFINE_UNARY_LOGICAL_FUNCTION_CALL(std, isnormal);
#undef CERES_DEFINE_UNARY_LOGICAL_FUNCTION_CALL
template <>
struct InputAssignment<ExpressionRef> {
using ReturnType = ExpressionRef;
static inline ReturnType Get(double /* unused */, const char* name) {
// Note: The scalar value of v will be thrown away, because we don't need it
// during code generation.
return AddExpressionToGraph(Expression::CreateInputAssignment(name));
}
};
template <typename T>
inline typename InputAssignment<T>::ReturnType MakeInputAssignment(
double v, const char* name) {
return InputAssignment<T>::Get(v, name);
}
inline ExpressionRef MakeParameter(const std::string& name) {
return AddExpressionToGraph(Expression::CreateInputAssignment(name));
}
inline ExpressionRef MakeOutput(const ExpressionRef& v,
const std::string& name) {
return AddExpressionToGraph(Expression::CreateOutputAssignment(v.id, name));
}
} // namespace internal
template <>
struct ComparisonReturnType<internal::ExpressionRef> {
using type = internal::ComparisonExpressionRef;
};
} // namespace ceres
#endif
include/ceres/codegen/internal/optimization_pass_summary.h
-51
View File
@@ -1,51 +0,0 @@
// Ceres Solver - A fast non-linear least squares minimizer
// Copyright 2020 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_INTERNAL_OPTIMIZATION_PASS_SUMMARY_H_
#define CERES_PUBLIC_CODEGEN_INTERNAL_OPTIMIZATION_PASS_SUMMARY_H_
#include <string>
namespace ceres {
namespace internal {
struct OptimizationPassSummary {
bool expression_graph_changed = false;
std::string optimization_pass_name;
int num_expressions_replaced_by_nop = 0;
int num_expressions_removed = 0;
int num_expressions_inserted = 0;
int num_expressions_modified = 0;
};
} // namespace internal
} // namespace ceres
#endif // CERES_PUBLIC_CODEGEN_INTERNAL_OPTIMIZATION_PASS_SUMMARY_H_
include/ceres/codegen/internal/optimize_expression_graph.h
-74
View File
@@ -1,74 +0,0 @@
// Ceres Solver - A fast non-linear least squares minimizer
// Copyright 2020 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_INTERNAL_OPTIMIZE_EXPRESSION_GRAPH_H_
#define CERES_PUBLIC_CODEGEN_INTERNAL_OPTIMIZE_EXPRESSION_GRAPH_H_
#include <memory>
#include <vector>
#include "ceres/codegen/internal/expression_graph.h"
#include "ceres/codegen/internal/optimization_pass_summary.h"
namespace ceres {
namespace internal {
struct OptimizeExpressionGraphOptions {
int max_num_iterations = 100;
bool eliminate_nops = true;
};
struct OptimizeExpressionGraphSummary {
int num_iterations;
std::vector<OptimizationPassSummary> summaries;
};
// Optimize the given ExpressionGraph in-place according to the defined
// OptimizeExpressionGraphOptions. This will change the ExpressionGraph, but the
// generated code will output the same numerical values.
//
// The Optimization iteratively applies all OptimizationPasses until the graph
// does not change anymore or max_num_iterations is reached. Pseudo Code:
//
// for(int it = 0; it < max_num_iterations; ++it) {
// graph.hasChanged = false;
// for each pass in OptimizationPasses
// pass.applyTo(graph)
// if(!graph.hasChanged)
// break;
// }
//
OptimizeExpressionGraphSummary OptimizeExpressionGraph(
const OptimizeExpressionGraphOptions& options, ExpressionGraph* graph);
} // namespace internal
} // namespace ceres
#endif // CERES_PUBLIC_CODEGEN_INTERNAL_OPTIMIZE_EXPRESSION_GRAPH_H_
include/ceres/codegen/internal/types.h
-67
View File
@@ -1,67 +0,0 @@
// 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_INTERNAL_TYPES_H_
#define CERES_PUBLIC_CODEGEN_INTERNAL_TYPES_H_
#include "ceres/codegen/macros.h"
namespace ceres {
// The return type of a comparison, for example from <, &&, ==.
//
// In the context of traditional Ceres Jet operations, this would
// always be a bool. However, in the autodiff code generation context,
// the return is always an expression, and so a different type must be
// used as a return from comparisons.
//
// In the autodiff codegen context, this function is overloaded so that 'type'
// is one of the autodiff code generation expression types.
template <typename T>
struct ComparisonReturnType {
using type = bool;
};
namespace internal {
// The InputAssignment struct is used to implement the CERES_LOCAL_VARIABLE
// macro defined in macros.h. The input is a double variable and the
// corresponding name as a string. During execution mode (T==double or
// T==Jet<double>) the variable name is unused and this function only returns
// the value. During code generation (T==ExpressionRef) the value is unused and
// an assignment expression is created. For example:
// v_0 = observed_x;
template <typename T>
struct InputAssignment {
static inline T Get(double v, const char* /* unused */) { return v; }
};
} // namespace internal
} // namespace ceres
#endif // CERES_PUBLIC_CODEGEN_INTERNAL_TYPES_H_
include/ceres/codegen/macros.h
-129
View File
@@ -1,129 +0,0 @@
// 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)
//
// This file defines the required macros to use local variables and if-else
// branches in the AutoDiffCodeGen system. This is also the only file that
// should be included by a user-defined cost functor.
//
// To generate code for your cost functor the following steps have to be
// implemented (see below for a full example):
//
// 1. Include this file
// 2. Wrap accesses to local variables in the CERES_LOCAL_VARIABLE macro.
// 3. Replace if, else by CERES_IF, CERES_ELSE and add CERES_ENDIF
// 4. Add a default constructor
//
// Example - my_cost_functor.h
// ========================================================
// #include "ceres/rotation.h"
// #include "ceres/autodiff_codegen_macros.h"
//
// struct MyReprojectionError {
// MyReprojectionError(double observed_x, double observed_y)
// : observed_x(observed_x), observed_y(observed_y) {}
//
// // The cost functor must be default constructible!
// MyReprojectionError() = default;
//
// template <typename T>
// bool operator()(const T* const camera,
// const T* const point,
// T* residuals) const {
// T p[3];
// AngleAxisRotatePoint(camera, point, p);
// p[0] += camera[3];
// p[1] += camera[4];
// p[2] += camera[5];
//
// // The if block is written using the macros!
// CERES_IF(p[2] < T(0)) {
// p[0] = -p[0];
// p[1] = -p[1];
// p[2] = -p[2];
// } CERES_ELSE {
// p[0] += T(1.0);
// }CERES_ENDIF;
//
// const T& focal = camera[6];
// const T predicted_x = focal * p[0];
// const T predicted_y = focal * p[1];
//
// // The read-access to the local variables observed_x and observed_y are
// // wrapped in the CERES_LOCAL_VARIABLE macro!
// residuals[0] = predicted_x - CERES_LOCAL_VARIABLE(T, observed_x);
// residuals[1] = predicted_y - CERES_LOCAL_VARIABLE(T, observed_y);
// return true;
// }
// double observed_x;
// double observed_y;
// };
//
// ========================================================
//
// This file defines the following macros:
//
// CERES_LOCAL_VARIABLE
// CERES_IF
// CERES_ELSE
// CERES_ENDIF
//
#ifndef CERES_PUBLIC_CODEGEN_MACROS_H_
#define CERES_PUBLIC_CODEGEN_MACROS_H_
// The CERES_CODEGEN macro is defined by the build system only during code
// generation.
#ifndef CERES_CODEGEN
#define CERES_LOCAL_VARIABLE(type, local_variable) type(local_variable)
#define CERES_IF(condition_) if (condition_)
#define CERES_ELSE else
#define CERES_ENDIF
#define CERES_COMMENT(comment_)
#else
#define CERES_LOCAL_VARIABLE(_template_type, _local_variable) \
ceres::internal::InputAssignment<_template_type>::Get(_local_variable, \
#_local_variable)
#define CERES_IF(condition_) \
AddExpressionToGraph(ceres::internal::Expression::CreateIf((condition_).id));
#define CERES_ELSE \
AddExpressionToGraph(ceres::internal::Expression::CreateElse());
#define CERES_ENDIF \
AddExpressionToGraph(ceres::internal::Expression::CreateEndIf());
#define CERES_COMMENT(comment_) \
AddExpressionToGraph(ceres::internal::Expression::CreateComment(comment_))
#endif
namespace ceres {
// A function equivalent to the ternary ?-operator.
// This function is required, because in the context of code generation a
// comparison returns an expression type which is not convertible to bool.
inline double Ternary(bool c, double a, double b) { return c ? a : b; }
} // namespace ceres
#endif // CERES_PUBLIC_CODEGEN_MACROS_H_
include/ceres/jet.h
+41 -75
View File
@@ -164,7 +164,6 @@
#include <string>
#include "Eigen/Core"
#include "ceres/codegen/internal/types.h"
#include "ceres/internal/port.h"
namespace ceres {
@@ -354,21 +353,18 @@ inline Jet<T, N> operator/(const Jet<T, N>& f, T s) {
}
// Binary comparison operators for both scalars and jets.
#define CERES_DEFINE_JET_COMPARISON_OPERATOR(op) \
template <typename T, int N> \
inline typename ComparisonReturnType<T>::type operator op( \
const Jet<T, N>& f, const Jet<T, N>& g) { \
return f.a op g.a; \
} \
template <typename T, int N> \
inline typename ComparisonReturnType<T>::type operator op( \
const T& s, const Jet<T, N>& g) { \
return s op g.a; \
} \
template <typename T, int N> \
inline typename ComparisonReturnType<T>::type operator op( \
const Jet<T, N>& f, const T& s) { \
return f.a op s; \
#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; \
}
CERES_DEFINE_JET_COMPARISON_OPERATOR(<) // NOLINT
CERES_DEFINE_JET_COMPARISON_OPERATOR(<=) // NOLINT
@@ -378,18 +374,6 @@ CERES_DEFINE_JET_COMPARISON_OPERATOR(==) // NOLINT
CERES_DEFINE_JET_COMPARISON_OPERATOR(!=) // NOLINT
#undef CERES_DEFINE_JET_COMPARISON_OPERATOR
template <typename T, int N>
inline Jet<T, N> Ternary(typename ComparisonReturnType<T>::type c,
const Jet<T, N>& f,
const Jet<T, N>& g) {
Jet<T, N> r;
r.a = Ternary(c, f.a, g.a);
for (int i = 0; i < N; ++i) {
r.v[i] = Ternary(c, f.v[i], g.v[i]);
}
return r;
}
// Pull some functions from namespace std.
//
// This is necessary because we want to use the same name (e.g. 'sqrt') for
@@ -436,7 +420,7 @@ inline bool IsNormal(double x) { return std::isnormal(x); }
// abs(x + h) ~= x + h or -(x + h)
template <typename T, int N>
inline Jet<T, N> abs(const Jet<T, N>& f) {
return Ternary(f.a < T(0.0), -f, f);
return (f.a < T(0.0) ? -f : f);
}
// log(a + h) ~= log(a) + h / a
@@ -581,12 +565,12 @@ inline Jet<T, N> hypot(const Jet<T, N>& x, const Jet<T, N>& y) {
template <typename T, int N>
inline Jet<T, N> fmax(const Jet<T, N>& x, const Jet<T, N>& y) {
return Ternary(x < y, y, x);
return x < y ? y : x;
}
template <typename T, int N>
inline Jet<T, N> fmin(const Jet<T, N>& x, const Jet<T, N>& y) {
return Ternary(y < x, y, x);
return y < x ? y : x;
}
// Bessel functions of the first kind with integer order equal to 0, 1, n.
@@ -659,7 +643,7 @@ inline Jet<T, N> BesselJn(int n, const Jet<T, N>& f) {
// The jet is finite if all parts of the jet are finite.
template <typename T, int N>
inline typename ComparisonReturnType<T>::type isfinite(const Jet<T, N>& f) {
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);
@@ -671,7 +655,7 @@ inline typename ComparisonReturnType<T>::type isfinite(const Jet<T, N>& f) {
// The jet is infinite if any part of the Jet is infinite.
template <typename T, int N>
inline typename ComparisonReturnType<T>::type isinf(const Jet<T, N>& f) {
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]);
@@ -681,7 +665,7 @@ inline typename ComparisonReturnType<T>::type isinf(const Jet<T, N>& f) {
// The jet is NaN if any part of the jet is NaN.
template <typename T, int N>
inline typename ComparisonReturnType<T>::type isnan(const Jet<T, N>& f) {
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]);
@@ -691,7 +675,7 @@ inline typename ComparisonReturnType<T>::type isnan(const Jet<T, N>& f) {
// The jet is normal if all parts of the jet are normal.
template <typename T, int N>
inline typename ComparisonReturnType<T>::type isnormal(const Jet<T, N>& f) {
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]);
@@ -701,23 +685,23 @@ inline typename ComparisonReturnType<T>::type isnormal(const Jet<T, N>& f) {
// Legacy functions from the pre-C++11 days.
template <typename T, int N>
inline typename ComparisonReturnType<T>::type IsFinite(const Jet<T, N>& f) {
inline bool IsFinite(const Jet<T, N>& f) {
return isfinite(f);
}
template <typename T, int N>
inline typename ComparisonReturnType<T>::type IsNaN(const Jet<T, N>& f) {
inline bool IsNaN(const Jet<T, N>& f) {
return isnan(f);
}
template <typename T, int N>
inline typename ComparisonReturnType<T>::type IsNormal(const Jet<T, N>& f) {
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>
inline typename ComparisonReturnType<T>::type IsInfinite(const Jet<T, N>& f) {
inline bool IsInfinite(const Jet<T, N>& f) {
return isinf(f);
}
@@ -759,29 +743,25 @@ template <typename T, int N>
inline Jet<T, N> pow(T f, const Jet<T, N>& g) {
Jet<T, N> result;
CERES_IF(f == T(0) && g.a > T(0)) {
if (f == T(0) && g.a > T(0)) {
// Handle case 2.
result = Jet<T, N>(T(0.0));
}
CERES_ELSE {
CERES_IF(f < 0 && g.a == floor(g.a)) { // Handle case 3.
} else {
if (f < 0 && g.a == floor(g.a)) { // Handle case 3.
result = Jet<T, N>(pow(f, g.a));
for (int i = 0; i < N; i++) {
CERES_IF(g.v[i] != T(0.0)) {
if (g.v[i] != T(0.0)) {
// Return a NaN when g.v != 0.
result.v[i] = std::numeric_limits<T>::quiet_NaN();
}
CERES_ENDIF
}
}
CERES_ELSE {
} else {
// Handle case 1.
T const tmp = pow(f, g.a);
result = Jet<T, N>(tmp, log(f) * tmp * g.v);
}
CERES_ENDIF;
}
CERES_ENDIF
return result;
}
@@ -825,26 +805,26 @@ template <typename T, int N>
inline Jet<T, N> pow(const Jet<T, N>& f, const Jet<T, N>& g) {
Jet<T, N> result;
CERES_IF(f.a == T(0) && g.a >= T(1)) {
if (f.a == T(0) && g.a >= T(1)) {
// Handle cases 2 and 3.
CERES_IF(g.a > T(1)) { result = Jet<T, N>(T(0.0)); }
CERES_ELSE { result = f; }
CERES_ENDIF;
}
CERES_ELSE {
CERES_IF(f.a < T(0) && g.a == floor(g.a)) {
if (g.a > T(1)) {
result = Jet<T, N>(T(0.0));
} else {
result = f;
}
} else {
if (f.a < T(0) && g.a == 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++) {
CERES_IF(g.v[i] != T(0.0)) {
if (g.v[i] != T(0.0)) {
// Return a NaN when g.v != 0.
result.v[i] = T(std::numeric_limits<double>::quiet_NaN());
}
CERES_ENDIF;
}
}
CERES_ELSE {
} else {
// Handle the remaining cases. For cases 4,5,6,9 we allow the log()
// function to generate -HUGE_VAL or NaN, since those cases result in a
// nonfinite derivative.
@@ -853,9 +833,8 @@ inline Jet<T, N> pow(const Jet<T, N>& f, const Jet<T, N>& g) {
T const tmp3 = tmp1 * log(f.a);
result = Jet<T, N>(tmp1, tmp2 * f.v + tmp3 * g.v);
}
CERES_ENDIF;
}
CERES_ENDIF;
return result;
}
@@ -874,19 +853,6 @@ inline std::ostream& operator<<(std::ostream& s, const Jet<T, N>& z) {
s << "]";
return s;
}
namespace internal {
// In the context of AutoDiffCodeGen, local variables can be added using the
// CERES_LOCAL_VARIABLE macro defined in ceres/codegen/macros.h. This partial
// specialization defined how local variables of type double are converted to
// Jet<T>.
template <typename T, int N>
struct InputAssignment<Jet<T, N>> {
static inline Jet<T, N> Get(double v, const char* name) {
return Jet<T, N>(InputAssignment<T>::Get(v, name));
}
};
} // namespace internal
} // namespace ceres
namespace Eigen {
include/ceres/rotation.h
+12 -20
View File
@@ -48,7 +48,7 @@
#include <algorithm>
#include <cmath>
#include <limits>
#include "codegen/macros.h"
#include "glog/logging.h"
namespace ceres {
@@ -253,7 +253,7 @@ inline void AngleAxisToQuaternion(const T* angle_axis, T* quaternion) {
const T theta_squared = a0 * a0 + a1 * a1 + a2 * a2;
// For points not at the origin, the full conversion is numerically stable.
CERES_IF(theta_squared > T(0.0)) {
if (theta_squared > T(0.0)) {
const T theta = sqrt(theta_squared);
const T half_theta = theta * T(0.5);
const T k = sin(half_theta) / theta;
@@ -261,8 +261,7 @@ inline void AngleAxisToQuaternion(const T* angle_axis, T* quaternion) {
quaternion[1] = a0 * k;
quaternion[2] = a1 * k;
quaternion[3] = a2 * k;
}
CERES_ELSE {
} else {
// At the origin, sqrt() will produce NaN in the derivative since
// the argument is zero. By approximating with a Taylor series,
// and truncating at one term, the value and first derivatives will be
@@ -273,7 +272,6 @@ inline void AngleAxisToQuaternion(const T* angle_axis, T* quaternion) {
quaternion[2] = a1 * k;
quaternion[3] = a2 * k;
}
CERES_ENDIF;
}
template <typename T>
@@ -285,7 +283,7 @@ inline void QuaternionToAngleAxis(const T* quaternion, T* angle_axis) {
// For quaternions representing non-zero rotation, the conversion
// is numerically stable.
CERES_IF(sin_squared_theta > T(0.0)) {
if (sin_squared_theta > T(0.0)) {
const T sin_theta = sqrt(sin_squared_theta);
const T& cos_theta = quaternion[0];
@@ -302,15 +300,14 @@ inline void QuaternionToAngleAxis(const T* quaternion, T* angle_axis) {
// theta - pi = atan(sin(theta - pi), cos(theta - pi))
// = atan(-sin(theta), -cos(theta))
//
const T two_theta = T(2.0) * Ternary((cos_theta < T(0.0)),
atan2(-sin_theta, -cos_theta),
atan2(sin_theta, cos_theta));
const T two_theta =
T(2.0) * ((cos_theta < T(0.0)) ? atan2(-sin_theta, -cos_theta)
: atan2(sin_theta, cos_theta));
const T k = two_theta / sin_theta;
angle_axis[0] = q1 * k;
angle_axis[1] = q2 * k;
angle_axis[2] = q3 * k;
}
CERES_ELSE {
} else {
// For zero rotation, sqrt() will produce NaN in the derivative since
// the argument is zero. By approximating with a Taylor series,
// and truncating at one term, the value and first derivatives will be
@@ -320,7 +317,6 @@ inline void QuaternionToAngleAxis(const T* quaternion, T* angle_axis) {
angle_axis[1] = q2 * k;
angle_axis[2] = q3 * k;
}
CERES_ENDIF;
}
template <typename T>
@@ -391,7 +387,7 @@ void AngleAxisToRotationMatrix(
const T* angle_axis, const MatrixAdapter<T, row_stride, col_stride>& R) {
static const T kOne = T(1.0);
const T theta2 = DotProduct(angle_axis, angle_axis);
CERES_IF(theta2 > T(std::numeric_limits<double>::epsilon())) {
if (theta2 > T(std::numeric_limits<double>::epsilon())) {
// We want to be careful to only evaluate the square root if the
// norm of the angle_axis vector is greater than zero. Otherwise
// we get a division by zero.
@@ -414,8 +410,7 @@ void AngleAxisToRotationMatrix(
R(1, 2) = -wx*sintheta + wy*wz*(kOne - costheta);
R(2, 2) = costheta + wz*wz*(kOne - costheta);
// clang-format on
}
CERES_ELSE {
} else {
// Near zero, we switch to using the first order Taylor expansion.
R(0, 0) = kOne;
R(1, 0) = angle_axis[2];
@@ -427,7 +422,6 @@ void AngleAxisToRotationMatrix(
R(1, 2) = -angle_axis[0];
R(2, 2) = kOne;
}
CERES_ENDIF;
}
template <typename T>
@@ -597,7 +591,7 @@ inline void AngleAxisRotatePoint(const T angle_axis[3],
DCHECK_NE(pt, result) << "Inplace rotation is not supported.";
const T theta2 = DotProduct(angle_axis, angle_axis);
CERES_IF(theta2 > T(std::numeric_limits<double>::epsilon())) {
if (theta2 > T(std::numeric_limits<double>::epsilon())) {
// Away from zero, use the rodriguez formula
//
// result = pt costheta +
@@ -628,8 +622,7 @@ inline void AngleAxisRotatePoint(const T angle_axis[3],
result[0] = pt[0] * costheta + w_cross_pt[0] * sintheta + w[0] * tmp;
result[1] = pt[1] * costheta + w_cross_pt[1] * sintheta + w[1] * tmp;
result[2] = pt[2] * costheta + w_cross_pt[2] * sintheta + w[2] * tmp;
}
CERES_ELSE {
} else {
// Near zero, the first order Taylor approximation of the rotation
// matrix R corresponding to a vector w and angle w is
//
@@ -655,7 +648,6 @@ inline void AngleAxisRotatePoint(const T angle_axis[3],
result[1] = pt[1] + w_cross_pt[1];
result[2] = pt[2] + w_cross_pt[2];
}
CERES_ENDIF;
}
} // namespace ceres