Squashed 'third_party/ceres/' content from commit e51e9b4
Change-Id: I763587619d57e594d3fa158dc3a7fe0b89a1743b
git-subtree-dir: third_party/ceres
git-subtree-split: e51e9b46f6ca88ab8b2266d0e362771db6d98067
diff --git a/internal/ceres/parameter_block.h b/internal/ceres/parameter_block.h
new file mode 100644
index 0000000..7f2a911
--- /dev/null
+++ b/internal/ceres/parameter_block.h
@@ -0,0 +1,406 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2015 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: keir@google.com (Keir Mierle)
+
+#ifndef CERES_INTERNAL_PARAMETER_BLOCK_H_
+#define CERES_INTERNAL_PARAMETER_BLOCK_H_
+
+#include <algorithm>
+#include <cstdint>
+#include <cstdlib>
+#include <limits>
+#include <memory>
+#include <string>
+#include <unordered_set>
+#include "ceres/array_utils.h"
+#include "ceres/internal/eigen.h"
+#include "ceres/internal/port.h"
+#include "ceres/local_parameterization.h"
+#include "ceres/stringprintf.h"
+#include "glog/logging.h"
+
+namespace ceres {
+namespace internal {
+
+class ProblemImpl;
+class ResidualBlock;
+
+// The parameter block encodes the location of the user's original value, and
+// also the "current state" of the parameter. The evaluator uses whatever is in
+// the current state of the parameter when evaluating. This is inlined since the
+// methods are performance sensitive.
+//
+// The class is not thread-safe, unless only const methods are called. The
+// parameter block may also hold a pointer to a local parameterization; the
+// parameter block does not take ownership of this pointer, so the user is
+// responsible for the proper disposal of the local parameterization.
+class ParameterBlock {
+ public:
+ typedef std::unordered_set<ResidualBlock*> ResidualBlockSet;
+
+ // Create a parameter block with the user state, size, and index specified.
+ // The size is the size of the parameter block and the index is the position
+ // of the parameter block inside a Program (if any).
+ ParameterBlock(double* user_state, int size, int index)
+ : user_state_(user_state),
+ state_(user_state),
+ size_(size),
+ index_(index) {}
+
+ ParameterBlock(double* user_state,
+ int size,
+ int index,
+ LocalParameterization* local_parameterization)
+ : user_state_(user_state),
+ state_(user_state),
+ size_(size),
+ index_(index) {
+ if (local_parameterization != nullptr) {
+ SetParameterization(local_parameterization);
+ }
+ }
+
+ // The size of the parameter block.
+ int Size() const { return size_; }
+
+ // Manipulate the parameter state.
+ bool SetState(const double* x) {
+ CHECK(x != nullptr) << "Tried to set the state of constant parameter "
+ << "with user location " << user_state_;
+ CHECK(!IsConstant()) << "Tried to set the state of constant parameter "
+ << "with user location " << user_state_;
+
+ state_ = x;
+ return UpdateLocalParameterizationJacobian();
+ }
+
+ // Copy the current parameter state out to x. This is "GetState()" rather than
+ // simply "state()" since it is actively copying the data into the passed
+ // pointer.
+ void GetState(double* x) const {
+ if (x != state_) {
+ std::copy(state_, state_ + size_, x);
+ }
+ }
+
+ // Direct pointers to the current state.
+ const double* state() const { return state_; }
+ const double* user_state() const { return user_state_; }
+ double* mutable_user_state() { return user_state_; }
+ const LocalParameterization* local_parameterization() const {
+ return local_parameterization_;
+ }
+ LocalParameterization* mutable_local_parameterization() {
+ return local_parameterization_;
+ }
+
+ // Set this parameter block to vary or not.
+ void SetConstant() { is_set_constant_ = true; }
+ void SetVarying() { is_set_constant_ = false; }
+ bool IsSetConstantByUser() const { return is_set_constant_; }
+ bool IsConstant() const { return (is_set_constant_ || LocalSize() == 0); }
+
+ double UpperBound(int index) const {
+ return (upper_bounds_ ? upper_bounds_[index]
+ : std::numeric_limits<double>::max());
+ }
+
+ double LowerBound(int index) const {
+ return (lower_bounds_ ? lower_bounds_[index]
+ : -std::numeric_limits<double>::max());
+ }
+
+ bool IsUpperBounded() const { return (upper_bounds_ == nullptr); }
+ bool IsLowerBounded() const { return (lower_bounds_ == nullptr); }
+
+ // This parameter block's index in an array.
+ int index() const { return index_; }
+ void set_index(int index) { index_ = index; }
+
+ // This parameter offset inside a larger state vector.
+ int state_offset() const { return state_offset_; }
+ void set_state_offset(int state_offset) { state_offset_ = state_offset; }
+
+ // This parameter offset inside a larger delta vector.
+ int delta_offset() const { return delta_offset_; }
+ void set_delta_offset(int delta_offset) { delta_offset_ = delta_offset; }
+
+ // Methods relating to the parameter block's parameterization.
+
+ // The local to global jacobian. Returns nullptr if there is no local
+ // parameterization for this parameter block. The returned matrix is row-major
+ // and has Size() rows and LocalSize() columns.
+ const double* LocalParameterizationJacobian() const {
+ return local_parameterization_jacobian_.get();
+ }
+
+ int LocalSize() const {
+ return (local_parameterization_ == nullptr)
+ ? size_
+ : local_parameterization_->LocalSize();
+ }
+
+ // Set the parameterization. The parameterization can be set exactly once;
+ // multiple calls to set the parameterization to different values will crash.
+ // It is an error to pass nullptr for the parameterization. The parameter
+ // block does not take ownership of the parameterization.
+ void SetParameterization(LocalParameterization* new_parameterization) {
+ CHECK(new_parameterization != nullptr)
+ << "nullptr parameterization invalid.";
+ // Nothing to do if the new parameterization is the same as the
+ // old parameterization.
+ if (new_parameterization == local_parameterization_) {
+ return;
+ }
+
+ CHECK(local_parameterization_ == nullptr)
+ << "Can't re-set the local parameterization; it leads to "
+ << "ambiguous ownership. Current local parameterization is: "
+ << local_parameterization_;
+
+ CHECK(new_parameterization->GlobalSize() == size_)
+ << "Invalid parameterization for parameter block. The parameter block "
+ << "has size " << size_ << " while the parameterization has a global "
+ << "size of " << new_parameterization->GlobalSize() << ". Did you "
+ << "accidentally use the wrong parameter block or parameterization?";
+
+ CHECK_GT(new_parameterization->LocalSize(), 0)
+ << "Invalid parameterization. Parameterizations must have a "
+ << "positive dimensional tangent space.";
+
+ local_parameterization_ = new_parameterization;
+ local_parameterization_jacobian_.reset(
+ new double[local_parameterization_->GlobalSize() *
+ local_parameterization_->LocalSize()]);
+ CHECK(UpdateLocalParameterizationJacobian())
+ << "Local parameterization Jacobian computation failed for x: "
+ << ConstVectorRef(state_, Size()).transpose();
+ }
+
+ void SetUpperBound(int index, double upper_bound) {
+ CHECK_LT(index, size_);
+
+ if (upper_bound >= std::numeric_limits<double>::max() && !upper_bounds_) {
+ return;
+ }
+
+ if (!upper_bounds_) {
+ upper_bounds_.reset(new double[size_]);
+ std::fill(upper_bounds_.get(),
+ upper_bounds_.get() + size_,
+ std::numeric_limits<double>::max());
+ }
+
+ upper_bounds_[index] = upper_bound;
+ }
+
+ void SetLowerBound(int index, double lower_bound) {
+ CHECK_LT(index, size_);
+
+ if (lower_bound <= -std::numeric_limits<double>::max() && !lower_bounds_) {
+ return;
+ }
+
+ if (!lower_bounds_) {
+ lower_bounds_.reset(new double[size_]);
+ std::fill(lower_bounds_.get(),
+ lower_bounds_.get() + size_,
+ -std::numeric_limits<double>::max());
+ }
+
+ lower_bounds_[index] = lower_bound;
+ }
+
+ // Generalization of the addition operation. This is the same as
+ // LocalParameterization::Plus() followed by projection onto the
+ // hyper cube implied by the bounds constraints.
+ bool Plus(const double* x, const double* delta, double* x_plus_delta) {
+ if (local_parameterization_ != nullptr) {
+ if (!local_parameterization_->Plus(x, delta, x_plus_delta)) {
+ return false;
+ }
+ } else {
+ VectorRef(x_plus_delta, size_) =
+ ConstVectorRef(x, size_) + ConstVectorRef(delta, size_);
+ }
+
+ // Project onto the box constraints.
+ if (lower_bounds_.get() != nullptr) {
+ for (int i = 0; i < size_; ++i) {
+ x_plus_delta[i] = std::max(x_plus_delta[i], lower_bounds_[i]);
+ }
+ }
+
+ if (upper_bounds_.get() != nullptr) {
+ for (int i = 0; i < size_; ++i) {
+ x_plus_delta[i] = std::min(x_plus_delta[i], upper_bounds_[i]);
+ }
+ }
+
+ return true;
+ }
+
+ std::string ToString() const {
+ return StringPrintf(
+ "{ this=%p, user_state=%p, state=%p, size=%d, "
+ "constant=%d, index=%d, state_offset=%d, "
+ "delta_offset=%d }",
+ this,
+ user_state_,
+ state_,
+ size_,
+ is_set_constant_,
+ index_,
+ state_offset_,
+ delta_offset_);
+ }
+
+ void EnableResidualBlockDependencies() {
+ CHECK(residual_blocks_.get() == nullptr)
+ << "Ceres bug: There is already a residual block collection "
+ << "for parameter block: " << ToString();
+ residual_blocks_.reset(new ResidualBlockSet);
+ }
+
+ void AddResidualBlock(ResidualBlock* residual_block) {
+ CHECK(residual_blocks_.get() != nullptr)
+ << "Ceres bug: The residual block collection is null for parameter "
+ << "block: " << ToString();
+ residual_blocks_->insert(residual_block);
+ }
+
+ void RemoveResidualBlock(ResidualBlock* residual_block) {
+ CHECK(residual_blocks_.get() != nullptr)
+ << "Ceres bug: The residual block collection is null for parameter "
+ << "block: " << ToString();
+ CHECK(residual_blocks_->find(residual_block) != residual_blocks_->end())
+ << "Ceres bug: Missing residual for parameter block: " << ToString();
+ residual_blocks_->erase(residual_block);
+ }
+
+ // This is only intended for iterating; perhaps this should only expose
+ // .begin() and .end().
+ ResidualBlockSet* mutable_residual_blocks() { return residual_blocks_.get(); }
+
+ double LowerBoundForParameter(int index) const {
+ if (lower_bounds_.get() == nullptr) {
+ return -std::numeric_limits<double>::max();
+ } else {
+ return lower_bounds_[index];
+ }
+ }
+
+ double UpperBoundForParameter(int index) const {
+ if (upper_bounds_.get() == nullptr) {
+ return std::numeric_limits<double>::max();
+ } else {
+ return upper_bounds_[index];
+ }
+ }
+
+ private:
+ bool UpdateLocalParameterizationJacobian() {
+ if (local_parameterization_ == nullptr) {
+ return true;
+ }
+
+ // Update the local to global Jacobian. In some cases this is
+ // wasted effort; if this is a bottleneck, we will find a solution
+ // at that time.
+
+ const int jacobian_size = Size() * LocalSize();
+ InvalidateArray(jacobian_size, local_parameterization_jacobian_.get());
+ if (!local_parameterization_->ComputeJacobian(
+ state_, local_parameterization_jacobian_.get())) {
+ LOG(WARNING) << "Local parameterization Jacobian computation failed"
+ "for x: "
+ << ConstVectorRef(state_, Size()).transpose();
+ return false;
+ }
+
+ if (!IsArrayValid(jacobian_size, local_parameterization_jacobian_.get())) {
+ LOG(WARNING) << "Local parameterization Jacobian computation returned"
+ << "an invalid matrix for x: "
+ << ConstVectorRef(state_, Size()).transpose()
+ << "\n Jacobian matrix : "
+ << ConstMatrixRef(local_parameterization_jacobian_.get(),
+ Size(),
+ LocalSize());
+ return false;
+ }
+ return true;
+ }
+
+ double* user_state_ = nullptr;
+ int size_ = -1;
+ bool is_set_constant_ = false;
+ LocalParameterization* local_parameterization_ = nullptr;
+
+ // The "state" of the parameter. These fields are only needed while the
+ // solver is running. While at first glance using mutable is a bad idea, this
+ // ends up simplifying the internals of Ceres enough to justify the potential
+ // pitfalls of using "mutable."
+ mutable const double* state_ = nullptr;
+ mutable std::unique_ptr<double[]> local_parameterization_jacobian_;
+
+ // The index of the parameter. This is used by various other parts of Ceres to
+ // permit switching from a ParameterBlock* to an index in another array.
+ int32_t index_ = -1;
+
+ // The offset of this parameter block inside a larger state vector.
+ int32_t state_offset_ = -1;
+
+ // The offset of this parameter block inside a larger delta vector.
+ int32_t delta_offset_ = -1;
+
+ // If non-null, contains the residual blocks this parameter block is in.
+ std::unique_ptr<ResidualBlockSet> residual_blocks_;
+
+ // Upper and lower bounds for the parameter block. SetUpperBound
+ // and SetLowerBound lazily initialize the upper_bounds_ and
+ // lower_bounds_ arrays. If they are never called, then memory for
+ // these arrays is never allocated. Thus for problems where there
+ // are no bounds, or only one sided bounds we do not pay the cost of
+ // allocating memory for the inactive bounds constraints.
+ //
+ // Upon initialization these arrays are initialized to
+ // std::numeric_limits<double>::max() and
+ // -std::numeric_limits<double>::max() respectively which correspond
+ // to the parameter block being unconstrained.
+ std::unique_ptr<double[]> upper_bounds_;
+ std::unique_ptr<double[]> lower_bounds_;
+
+ // Necessary so ProblemImpl can clean up the parameterizations.
+ friend class ProblemImpl;
+};
+
+} // namespace internal
+} // namespace ceres
+
+#endif // CERES_INTERNAL_PARAMETER_BLOCK_H_