| 123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227 | // 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)#include "ceres/codegen/internal/expression_graph.h"#include "glog/logging.h"namespace ceres {namespace internal {static ExpressionGraph* expression_pool = nullptr;void StartRecordingExpressions() {  CHECK(expression_pool == nullptr)      << "Expression recording must be stopped before calling "         "StartRecordingExpressions again.";  expression_pool = new ExpressionGraph;}ExpressionGraph StopRecordingExpressions() {  CHECK(expression_pool)      << "Expression recording hasn't started yet or you tried "         "to stop it twice.";  ExpressionGraph result = std::move(*expression_pool);  delete expression_pool;  expression_pool = nullptr;  return result;}ExpressionGraph* GetCurrentExpressionGraph() { return expression_pool; }bool ExpressionGraph::DependsOn(ExpressionId A, ExpressionId B) const {  // Depth first search on the expression graph  // Equivalent Recursive Implementation:  //   if (A.DirectlyDependsOn(B)) return true;  //   for (auto p : A.params_) {  //     if (pool[p.id].DependsOn(B, pool)) return true;  //   }  std::vector<ExpressionId> stack = ExpressionForId(A).arguments();  while (!stack.empty()) {    auto top = stack.back();    stack.pop_back();    if (top == B) {      return true;    }    auto& expr = ExpressionForId(top);    stack.insert(stack.end(), expr.arguments().begin(), expr.arguments().end());  }  return false;}bool ExpressionGraph::operator==(const ExpressionGraph& other) const {  if (Size() != other.Size()) {    return false;  }  for (ExpressionId id = 0; id < Size(); ++id) {    if (!(ExpressionForId(id) == other.ExpressionForId(id))) {      return false;    }  }  return true;}void ExpressionGraph::Erase(ExpressionId location) {  CHECK_GE(location, 0);  CHECK_LT(location, Size());  // Move everything after id to the front and update references  for (ExpressionId id = location + 1; id < Size(); ++id) {    expressions_[id - 1] = expressions_[id];    auto& expression = expressions_[id - 1];    // Decrement reference if it points to a shifted variable.    if (expression.lhs_id() >= location) {      expression.set_lhs_id(expression.lhs_id() - 1);    }    for (auto& arg : *expression.mutable_arguments()) {      if (arg >= location) {        arg--;      }    }  }  expressions_.resize(Size() - 1);}void ExpressionGraph::Insert(ExpressionId location,                             const Expression& expression) {  CHECK_GE(location, 0);  CHECK_LE(location, Size());  ExpressionId last_expression_id = Size() - 1;  // Increase size by adding a dummy expression.  expressions_.push_back(Expression());  // Move everything after id back and update references  for (ExpressionId id = last_expression_id; id >= location; --id) {    auto& expression = expressions_[id];    // Increment reference if it points to a shifted variable.    if (expression.lhs_id() >= location) {      expression.set_lhs_id(expression.lhs_id() + 1);    }    for (auto& arg : *expression.mutable_arguments()) {      if (arg >= location) {        arg++;      }    }    expressions_[id + 1] = expression;  }  if (expression.IsControlExpression() ||      expression.lhs_id() != kInvalidExpressionId) {    // Insert new expression at the correct place    expressions_[location] = expression;  } else {    // Arithmetic expression with invalid lhs    // -> Set lhs to location    Expression copy = expression;    copy.set_lhs_id(location);    expressions_[location] = copy;  }}ExpressionId ExpressionGraph::InsertBack(const Expression& expression) {  if (expression.IsControlExpression()) {    // Control expression are just added to the list. We do not return a    // reference to them.    CHECK(expression.lhs_id() == kInvalidExpressionId)        << "Control expressions must have an invalid lhs.";    expressions_.push_back(expression);    return kInvalidExpressionId;  }  if (expression.lhs_id() == kInvalidExpressionId) {    // Create a new variable name for this expression and set it as the lhs    Expression copy = expression;    copy.set_lhs_id(static_cast<ExpressionId>(expressions_.size()));    expressions_.push_back(copy);  } else {    // The expressions writes to a variable declared in the past    // -> Just add it to the list    CHECK_LE(expression.lhs_id(), expressions_.size())        << "The left hand side must reference a variable in the past.";    expressions_.push_back(expression);  }  return Size() - 1;}ExpressionId ExpressionGraph::FindMatchingEndif(ExpressionId id) const {  CHECK(ExpressionForId(id).type() == ExpressionType::IF)      << "FindClosingControlExpression is only valid on IF "         "expressions.";  // Traverse downwards  for (ExpressionId i = id + 1; i < Size(); ++i) {    const auto& expr = ExpressionForId(i);    if (expr.type() == ExpressionType::ENDIF) {      return i;    } else if (expr.type() == ExpressionType::IF) {      // Found a nested IF.      // -> Jump over the block and continue behind it.      auto matching_endif = FindMatchingEndif(i);      if (matching_endif == kInvalidExpressionId) {        return kInvalidExpressionId;      }      i = matching_endif;      continue;    }  }  return kInvalidExpressionId;}ExpressionId ExpressionGraph::FindMatchingElse(ExpressionId id) const {  CHECK(ExpressionForId(id).type() == ExpressionType::IF)      << "FindClosingControlExpression is only valid on IF "         "expressions.";  // Traverse downwards  for (ExpressionId i = id + 1; i < Size(); ++i) {    const auto& expr = ExpressionForId(i);    if (expr.type() == ExpressionType::ELSE) {      // Found it!      return i;    } else if (expr.type() == ExpressionType::ENDIF) {      // Found an endif even though we were looking for an ELSE.      // -> Return invalidId      return kInvalidExpressionId;    } else if (expr.type() == ExpressionType::IF) {      // Found a nested IF.      // -> Jump over the block and continue behind it.      auto matching_endif = FindMatchingEndif(i);      if (matching_endif == kInvalidExpressionId) {        return kInvalidExpressionId;      }      i = matching_endif;      continue;    }  }  return kInvalidExpressionId;}}  // namespace internal}  // namespace ceres
 |