internal/ceres/parameter_block.h - ceres-solver - Git at Google

 // 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 <cstdlib>
 #include <limits>
 #include <string>
 #include "ceres/array_utils.h"
 #include "ceres/collections_port.h"
 #include "ceres/integral_types.h"
 #include "ceres/internal/eigen.h"
 #include "ceres/internal/port.h"
 #include "ceres/internal/scoped_ptr.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:
   // TODO(keir): Decide what data structure is best here. Should this be a set?
   // Probably not, because sets are memory inefficient. However, if it's a
   // vector, you can get into pathological linear performance when removing a
   // residual block from a problem where all the residual blocks depend on one
   // parameter; for example, shared focal length in a bundle adjustment
   // problem. It might be worth making a custom structure that is just an array
   // when it is small, but transitions to a hash set when it has more elements.
   //
   // For now, use a hash set.
   typedef HashSet<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) {
     Init(user_state, size, index, NULL);
   }

   ParameterBlock(double* user_state,
                  int size,
                  int index,
                  LocalParameterization* local_parameterization) {
     Init(user_state, size, index, local_parameterization);
   }

   // The size of the parameter block.
   int Size() const { return size_; }

   // Manipulate the parameter state.
   bool SetState(const double* x) {
     CHECK(x != NULL)
         << "Tried to set the state of constant parameter "
         << "with user location " << user_state_;
     CHECK(!is_constant_)
         << "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_) {
       memcpy(x, state_, sizeof(*state_) * size_);
     }
   }

   // 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_; }
   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_constant_ = true; }
   void SetVarying() { is_constant_ = false; }
   bool IsConstant() const { return is_constant_; }

   // 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 NULL 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_ == NULL)
         ? 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 NULL for the parameterization. The parameter block
   // does not take ownership of the parameterization.
   void SetParameterization(LocalParameterization* new_parameterization) {
     CHECK(new_parameterization != NULL) << "NULL 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_ == NULL)
         << "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_bounds_.get() == NULL) {
       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_bounds_.get() == NULL) {
       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_ != NULL) {
       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() != NULL) {
       for (int i = 0; i < size_; ++i) {
         x_plus_delta[i] = std::max(x_plus_delta[i], lower_bounds_[i]);
       }
     }

     if (upper_bounds_.get() != NULL) {
       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_constant_,
                         index_,
                         state_offset_,
                         delta_offset_);
   }

   void EnableResidualBlockDependencies() {
     CHECK(residual_blocks_.get() == NULL)
         << "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() != NULL)
         << "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() != NULL)
         << "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() == NULL) {
       return -std::numeric_limits<double>::max();
     } else {
       return lower_bounds_[index];
     }
   }

   double UpperBoundForParameter(int index) const {
     if (upper_bounds_.get() == NULL) {
       return std::numeric_limits<double>::max();
     } else {
       return upper_bounds_[index];
     }
   }

  private:
   void Init(double* user_state,
             int size,
             int index,
             LocalParameterization* local_parameterization) {
     user_state_ = user_state;
     size_ = size;
     index_ = index;
     is_constant_ = false;
     state_ = user_state_;

     local_parameterization_ = NULL;
     if (local_parameterization != NULL) {
       SetParameterization(local_parameterization);
     }

     state_offset_ = -1;
     delta_offset_ = -1;
   }

   bool UpdateLocalParameterizationJacobian() {
     if (local_parameterization_ == NULL) {
       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_;
   int size_;
   bool is_constant_;
   LocalParameterization* local_parameterization_;

   // 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_;
   mutable scoped_array<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 index_;

   // The offset of this parameter block inside a larger state vector.
   int32 state_offset_;

   // The offset of this parameter block inside a larger delta vector.
   int32 delta_offset_;

   // If non-null, contains the residual blocks this parameter block is in.
   scoped_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.
   scoped_array<double> upper_bounds_;
   scoped_array<double> lower_bounds_;

   // Necessary so ProblemImpl can clean up the parameterizations.
   friend class ProblemImpl;
 };

 }  // namespace internal
 }  // namespace ceres

 #endif  // CERES_INTERNAL_PARAMETER_BLOCK_H_
	// 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 <cstdlib>
	#include <limits>
	#include <string>
	#include "ceres/array_utils.h"
	#include "ceres/collections_port.h"
	#include "ceres/integral_types.h"
	#include "ceres/internal/eigen.h"
	#include "ceres/internal/port.h"
	#include "ceres/internal/scoped_ptr.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:
	// TODO(keir): Decide what data structure is best here. Should this be a set?
	// Probably not, because sets are memory inefficient. However, if it's a
	// vector, you can get into pathological linear performance when removing a
	// residual block from a problem where all the residual blocks depend on one
	// parameter; for example, shared focal length in a bundle adjustment
	// problem. It might be worth making a custom structure that is just an array
	// when it is small, but transitions to a hash set when it has more elements.
	//
	// For now, use a hash set.
	typedef HashSet<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) {
	Init(user_state, size, index, NULL);
	}

	ParameterBlock(double* user_state,
	int size,
	int index,
	LocalParameterization* local_parameterization) {
	Init(user_state, size, index, local_parameterization);
	}

	// The size of the parameter block.
	int Size() const { return size_; }

	// Manipulate the parameter state.
	bool SetState(const double* x) {
	CHECK(x != NULL)
	<< "Tried to set the state of constant parameter "
	<< "with user location " << user_state_;
	CHECK(!is_constant_)
	<< "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_) {
	memcpy(x, state_, sizeof(state_) size_);
	}
	}

	// 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_; }
	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_constant_ = true; }
	void SetVarying() { is_constant_ = false; }
	bool IsConstant() const { return is_constant_; }

	// 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 NULL 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_ == NULL)
	? 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 NULL for the parameterization. The parameter block
	// does not take ownership of the parameterization.
	void SetParameterization(LocalParameterization* new_parameterization) {
	CHECK(new_parameterization != NULL) << "NULL 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_ == NULL)
	<< "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_bounds_.get() == NULL) {
	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_bounds_.get() == NULL) {
	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_ != NULL) {
	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() != NULL) {
	for (int i = 0; i < size_; ++i) {
	x_plus_delta[i] = std::max(x_plus_delta[i], lower_bounds_[i]);
	}
	}

	if (upper_bounds_.get() != NULL) {
	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_constant_,
	index_,
	state_offset_,
	delta_offset_);
	}

	void EnableResidualBlockDependencies() {
	CHECK(residual_blocks_.get() == NULL)
	<< "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() != NULL)
	<< "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() != NULL)
	<< "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() == NULL) {
	return -std::numeric_limits<double>::max();
	} else {
	return lower_bounds_[index];
	}
	}

	double UpperBoundForParameter(int index) const {
	if (upper_bounds_.get() == NULL) {
	return std::numeric_limits<double>::max();
	} else {
	return upper_bounds_[index];
	}
	}

	private:
	void Init(double* user_state,
	int size,
	int index,
	LocalParameterization* local_parameterization) {
	user_state_ = user_state;
	size_ = size;
	index_ = index;
	is_constant_ = false;
	state_ = user_state_;

	local_parameterization_ = NULL;
	if (local_parameterization != NULL) {
	SetParameterization(local_parameterization);
	}

	state_offset_ = -1;
	delta_offset_ = -1;
	}

	bool UpdateLocalParameterizationJacobian() {
	if (local_parameterization_ == NULL) {
	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_;
	int size_;
	bool is_constant_;
	LocalParameterization* local_parameterization_;

	// 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_;
	mutable scoped_array<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 index_;

	// The offset of this parameter block inside a larger state vector.
	int32 state_offset_;

	// The offset of this parameter block inside a larger delta vector.
	int32 delta_offset_;

	// If non-null, contains the residual blocks this parameter block is in.
	scoped_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.
	scoped_array<double> upper_bounds_;
	scoped_array<double> lower_bounds_;

	// Necessary so ProblemImpl can clean up the parameterizations.
	friend class ProblemImpl;
	};

	} // namespace internal
	} // namespace ceres

	#endif // CERES_INTERNAL_PARAMETER_BLOCK_H_