internal/ceres/program.cc - 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)

 #include "ceres/program.h"

 #include <algorithm>
 #include <map>
 #include <memory>
 #include <string>
 #include <vector>

 #include "ceres/array_utils.h"
 #include "ceres/casts.h"
 #include "ceres/compressed_row_sparse_matrix.h"
 #include "ceres/cost_function.h"
 #include "ceres/evaluator.h"
 #include "ceres/internal/export.h"
 #include "ceres/loss_function.h"
 #include "ceres/manifold.h"
 #include "ceres/map_util.h"
 #include "ceres/parameter_block.h"
 #include "ceres/problem.h"
 #include "ceres/residual_block.h"
 #include "ceres/stl_util.h"
 #include "ceres/triplet_sparse_matrix.h"

 namespace ceres {
 namespace internal {


 const std::vector<ParameterBlock*>& Program::parameter_blocks() const {
   return parameter_blocks_;
 }

 const std::vector<ResidualBlock*>& Program::residual_blocks() const {
   return residual_blocks_;
 }

 std::vector<ParameterBlock*>* Program::mutable_parameter_blocks() {
   return &parameter_blocks_;
 }

 std::vector<ResidualBlock*>* Program::mutable_residual_blocks() {
   return &residual_blocks_;
 }

 EvaluationCallback* Program::mutable_evaluation_callback() {
   return evaluation_callback_;
 }

 bool Program::StateVectorToParameterBlocks(const double* state) {
   for (int i = 0; i < parameter_blocks_.size(); ++i) {
     if (!parameter_blocks_[i]->IsConstant() &&
         !parameter_blocks_[i]->SetState(state)) {
       return false;
     }
     state += parameter_blocks_[i]->Size();
   }
   return true;
 }

 void Program::ParameterBlocksToStateVector(double* state) const {
   for (int i = 0; i < parameter_blocks_.size(); ++i) {
     parameter_blocks_[i]->GetState(state);
     state += parameter_blocks_[i]->Size();
   }
 }

 void Program::CopyParameterBlockStateToUserState() {
   for (int i = 0; i < parameter_blocks_.size(); ++i) {
     parameter_blocks_[i]->GetState(parameter_blocks_[i]->mutable_user_state());
   }
 }

 bool Program::SetParameterBlockStatePtrsToUserStatePtrs() {
   for (int i = 0; i < parameter_blocks_.size(); ++i) {
     if (!parameter_blocks_[i]->IsConstant() &&
         !parameter_blocks_[i]->SetState(parameter_blocks_[i]->user_state())) {
       return false;
     }
   }
   return true;
 }

 bool Program::Plus(const double* state,
                    const double* delta,
                    double* state_plus_delta) const {
   for (int i = 0; i < parameter_blocks_.size(); ++i) {
     if (!parameter_blocks_[i]->Plus(state, delta, state_plus_delta)) {
       return false;
     }
     state += parameter_blocks_[i]->Size();
     delta += parameter_blocks_[i]->TangentSize();
     state_plus_delta += parameter_blocks_[i]->Size();
   }
   return true;
 }

 void Program::SetParameterOffsetsAndIndex() {
   // Set positions for all parameters appearing as arguments to residuals to one
   // past the end of the parameter block array.
   for (int i = 0; i < residual_blocks_.size(); ++i) {
     ResidualBlock* residual_block = residual_blocks_[i];
     for (int j = 0; j < residual_block->NumParameterBlocks(); ++j) {
       residual_block->parameter_blocks()[j]->set_index(-1);
     }
   }
   // For parameters that appear in the program, set their position and offset.
   int state_offset = 0;
   int delta_offset = 0;
   for (int i = 0; i < parameter_blocks_.size(); ++i) {
     parameter_blocks_[i]->set_index(i);
     parameter_blocks_[i]->set_state_offset(state_offset);
     parameter_blocks_[i]->set_delta_offset(delta_offset);
     state_offset += parameter_blocks_[i]->Size();
     delta_offset += parameter_blocks_[i]->TangentSize();
   }
 }

 bool Program::IsValid() const {
   for (int i = 0; i < residual_blocks_.size(); ++i) {
     const ResidualBlock* residual_block = residual_blocks_[i];
     if (residual_block->index() != i) {
       LOG(WARNING) << "Residual block: " << i
                    << " has incorrect index: " << residual_block->index();
       return false;
     }
   }

   int state_offset = 0;
   int delta_offset = 0;
   for (int i = 0; i < parameter_blocks_.size(); ++i) {
     const ParameterBlock* parameter_block = parameter_blocks_[i];
     if (parameter_block->index() != i ||
         parameter_block->state_offset() != state_offset ||
         parameter_block->delta_offset() != delta_offset) {
       LOG(WARNING) << "Parameter block: " << i
                    << "has incorrect indexing information: "
                    << parameter_block->ToString();
       return false;
     }

     state_offset += parameter_blocks_[i]->Size();
     delta_offset += parameter_blocks_[i]->TangentSize();
   }

   return true;
 }

 bool Program::ParameterBlocksAreFinite(std::string* message) const {
   CHECK(message != nullptr);
   for (int i = 0; i < parameter_blocks_.size(); ++i) {
     const ParameterBlock* parameter_block = parameter_blocks_[i];
     const double* array = parameter_block->user_state();
     const int size = parameter_block->Size();
     const int invalid_index = FindInvalidValue(size, array);
     if (invalid_index != size) {
       *message = StringPrintf(
           "ParameterBlock: %p with size %d has at least one invalid value.\n"
           "First invalid value is at index: %d.\n"
           "Parameter block values: ",
           array,
           size,
           invalid_index);
       AppendArrayToString(size, array, message);
       return false;
     }
   }
   return true;
 }

 bool Program::IsBoundsConstrained() const {
   for (int i = 0; i < parameter_blocks_.size(); ++i) {
     const ParameterBlock* parameter_block = parameter_blocks_[i];
     if (parameter_block->IsConstant()) {
       continue;
     }
     const int size = parameter_block->Size();
     for (int j = 0; j < size; ++j) {
       const double lower_bound = parameter_block->LowerBoundForParameter(j);
       const double upper_bound = parameter_block->UpperBoundForParameter(j);
       if (lower_bound > -std::numeric_limits<double>::max() ||
           upper_bound < std::numeric_limits<double>::max()) {
         return true;
       }
     }
   }
   return false;
 }

 bool Program::IsFeasible(std::string* message) const {
   CHECK(message != nullptr);
   for (int i = 0; i < parameter_blocks_.size(); ++i) {
     const ParameterBlock* parameter_block = parameter_blocks_[i];
     const double* parameters = parameter_block->user_state();
     const int size = parameter_block->Size();
     if (parameter_block->IsConstant()) {
       // Constant parameter blocks must start in the feasible region
       // to ultimately produce a feasible solution, since Ceres cannot
       // change them.
       for (int j = 0; j < size; ++j) {
         const double lower_bound = parameter_block->LowerBoundForParameter(j);
         const double upper_bound = parameter_block->UpperBoundForParameter(j);
         if (parameters[j] < lower_bound || parameters[j] > upper_bound) {
           *message = StringPrintf(
               "ParameterBlock: %p with size %d has at least one infeasible "
               "value."
               "\nFirst infeasible value is at index: %d."
               "\nLower bound: %e, value: %e, upper bound: %e"
               "\nParameter block values: ",
               parameters,
               size,
               j,
               lower_bound,
               parameters[j],
               upper_bound);
           AppendArrayToString(size, parameters, message);
           return false;
         }
       }
     } else {
       // Variable parameter blocks must have non-empty feasible
       // regions, otherwise there is no way to produce a feasible
       // solution.
       for (int j = 0; j < size; ++j) {
         const double lower_bound = parameter_block->LowerBoundForParameter(j);
         const double upper_bound = parameter_block->UpperBoundForParameter(j);
         if (lower_bound >= upper_bound) {
           *message = StringPrintf(
               "ParameterBlock: %p with size %d has at least one infeasible "
               "bound."
               "\nFirst infeasible bound is at index: %d."
               "\nLower bound: %e, upper bound: %e"
               "\nParameter block values: ",
               parameters,
               size,
               j,
               lower_bound,
               upper_bound);
           AppendArrayToString(size, parameters, message);
           return false;
         }
       }
     }
   }

   return true;
 }

 std::unique_ptr<Program> Program::CreateReducedProgram(
     std::vector<double*>* removed_parameter_blocks,
     double* fixed_cost,
     std::string* error) const {
   CHECK(removed_parameter_blocks != nullptr);
   CHECK(fixed_cost != nullptr);
   CHECK(error != nullptr);

   std::unique_ptr<Program> reduced_program = std::make_unique<Program>(*this);
   if (!reduced_program->RemoveFixedBlocks(
           removed_parameter_blocks, fixed_cost, error)) {
     return nullptr;
   }

   reduced_program->SetParameterOffsetsAndIndex();
   return reduced_program;
 }

 bool Program::RemoveFixedBlocks(std::vector<double*>* removed_parameter_blocks,
                                 double* fixed_cost,
                                 std::string* error) {
   CHECK(removed_parameter_blocks != nullptr);
   CHECK(fixed_cost != nullptr);
   CHECK(error != nullptr);

   std::unique_ptr<double[]> residual_block_evaluate_scratch;
   residual_block_evaluate_scratch =
       std::make_unique<double[]>(MaxScratchDoublesNeededForEvaluate());
   *fixed_cost = 0.0;

   bool need_to_call_prepare_for_evaluation = evaluation_callback_ != nullptr;

   // Mark all the parameters as unused. Abuse the index member of the
   // parameter blocks for the marking.
   for (int i = 0; i < parameter_blocks_.size(); ++i) {
     parameter_blocks_[i]->set_index(-1);
   }

   // Filter out residual that have all-constant parameters, and mark
   // all the parameter blocks that appear in residuals.
   int num_active_residual_blocks = 0;
   for (int i = 0; i < residual_blocks_.size(); ++i) {
     ResidualBlock* residual_block = residual_blocks_[i];
     int num_parameter_blocks = residual_block->NumParameterBlocks();

     // Determine if the residual block is fixed, and also mark varying
     // parameters that appear in the residual block.
     bool all_constant = true;
     for (int k = 0; k < num_parameter_blocks; k++) {
       ParameterBlock* parameter_block = residual_block->parameter_blocks()[k];
       if (!parameter_block->IsConstant()) {
         all_constant = false;
         parameter_block->set_index(1);
       }
     }

     if (!all_constant) {
       residual_blocks_[num_active_residual_blocks++] = residual_block;
       continue;
     }

     // This is an exceedingly rare case, where the user has residual
     // blocks which are effectively constant but they are also
     // performance sensitive enough to add an EvaluationCallback.
     //
     // In this case before we evaluate the cost of the constant
     // residual blocks, we must call
     // EvaluationCallback::PrepareForEvaluation(). Because this call
     // can be costly, we only call this if we actually encounter a
     // residual block with all constant parameter blocks.
     //
     // It is worth nothing that there is a minor inefficiency here,
     // that the iteration 0 of TrustRegionMinimizer will also cause
     // PrepareForEvaluation to be called on the same point, but with
     // evaluate_jacobians = true. We could try and optimize this here,
     // but given the rarity of this case, the additional complexity
     // and long range dependency is not worth it.
     if (need_to_call_prepare_for_evaluation) {
       constexpr bool kNewPoint = true;
       constexpr bool kDoNotEvaluateJacobians = false;
       evaluation_callback_->PrepareForEvaluation(kDoNotEvaluateJacobians,
                                                  kNewPoint);
       need_to_call_prepare_for_evaluation = false;
     }

     // The residual is constant and will be removed, so its cost is
     // added to the variable fixed_cost.
     double cost = 0.0;
     if (!residual_block->Evaluate(true,
                                   &cost,
                                   nullptr,
                                   nullptr,
                                   residual_block_evaluate_scratch.get())) {
       *error = StringPrintf(
           "Evaluation of the residual %d failed during "
           "removal of fixed residual blocks.",
           i);
       return false;
     }

     *fixed_cost += cost;
   }
   residual_blocks_.resize(num_active_residual_blocks);

   // Filter out unused or fixed parameter blocks.
   int num_active_parameter_blocks = 0;
   removed_parameter_blocks->clear();
   for (int i = 0; i < parameter_blocks_.size(); ++i) {
     ParameterBlock* parameter_block = parameter_blocks_[i];
     if (parameter_block->index() == -1) {
       removed_parameter_blocks->push_back(
           parameter_block->mutable_user_state());
     } else {
       parameter_blocks_[num_active_parameter_blocks++] = parameter_block;
     }
   }
   parameter_blocks_.resize(num_active_parameter_blocks);

   if (!(((NumResidualBlocks() == 0) && (NumParameterBlocks() == 0)) ||
         ((NumResidualBlocks() != 0) && (NumParameterBlocks() != 0)))) {
     *error = "Congratulations, you found a bug in Ceres. Please report it.";
     return false;
   }

   return true;
 }

 bool Program::IsParameterBlockSetIndependent(
     const std::set<double*>& independent_set) const {
   // Loop over each residual block and ensure that no two parameter
   // blocks in the same residual block are part of
   // parameter_block_ptrs as that would violate the assumption that it
   // is an independent set in the Hessian matrix.
   for (const ResidualBlock* residual_block : residual_blocks_) {
     ParameterBlock* const* parameter_blocks =
         residual_block->parameter_blocks();
     const int num_parameter_blocks = residual_block->NumParameterBlocks();
     int count = 0;
     for (int i = 0; i < num_parameter_blocks; ++i) {
       count += independent_set.count(parameter_blocks[i]->mutable_user_state());
     }
     if (count > 1) {
       return false;
     }
   }
   return true;
 }

 std::unique_ptr<TripletSparseMatrix>
 Program::CreateJacobianBlockSparsityTranspose(int start_residual_block) const {
   // Matrix to store the block sparsity structure of the Jacobian.
   const int num_rows = NumParameterBlocks();
   const int num_cols = NumResidualBlocks() - start_residual_block;

   std::unique_ptr<TripletSparseMatrix> tsm(
       new TripletSparseMatrix(num_rows, num_cols, 10 * num_cols));
   int num_nonzeros = 0;
   int* rows = tsm->mutable_rows();
   int* cols = tsm->mutable_cols();
   double* values = tsm->mutable_values();

   for (int c = start_residual_block; c < residual_blocks_.size(); ++c) {
     const ResidualBlock* residual_block = residual_blocks_[c];
     const int num_parameter_blocks = residual_block->NumParameterBlocks();
     ParameterBlock* const* parameter_blocks =
         residual_block->parameter_blocks();

     for (int j = 0; j < num_parameter_blocks; ++j) {
       if (parameter_blocks[j]->IsConstant()) {
         continue;
       }

       // Re-size the matrix if needed.
       if (num_nonzeros >= tsm->max_num_nonzeros()) {
         tsm->set_num_nonzeros(num_nonzeros);
         tsm->Reserve(2 * num_nonzeros);
         rows = tsm->mutable_rows();
         cols = tsm->mutable_cols();
         values = tsm->mutable_values();
       }

       const int r = parameter_blocks[j]->index();
       rows[num_nonzeros] = r;
       cols[num_nonzeros] = c - start_residual_block;
       values[num_nonzeros] = 1.0;
       ++num_nonzeros;
     }
   }

   tsm->set_num_nonzeros(num_nonzeros);
   return tsm;
 }

 int Program::NumResidualBlocks() const { return residual_blocks_.size(); }

 int Program::NumParameterBlocks() const { return parameter_blocks_.size(); }

 int Program::NumResiduals() const {
   int num_residuals = 0;
   for (int i = 0; i < residual_blocks_.size(); ++i) {
     num_residuals += residual_blocks_[i]->NumResiduals();
   }
   return num_residuals;
 }

 int Program::NumParameters() const {
   int num_parameters = 0;
   for (int i = 0; i < parameter_blocks_.size(); ++i) {
     num_parameters += parameter_blocks_[i]->Size();
   }
   return num_parameters;
 }

 int Program::NumEffectiveParameters() const {
   int num_parameters = 0;
   for (int i = 0; i < parameter_blocks_.size(); ++i) {
     num_parameters += parameter_blocks_[i]->TangentSize();
   }
   return num_parameters;
 }

 // TODO(sameeragarwal): The following methods should just be updated
 // incrementally and the values cached, rather than the linear
 // complexity we have right now on every call.
 int Program::MaxScratchDoublesNeededForEvaluate() const {
   // Compute the scratch space needed for evaluate.
   int max_scratch_bytes_for_evaluate = 0;
   for (int i = 0; i < residual_blocks_.size(); ++i) {
     max_scratch_bytes_for_evaluate =
         std::max(max_scratch_bytes_for_evaluate,
                  residual_blocks_[i]->NumScratchDoublesForEvaluate());
   }
   return max_scratch_bytes_for_evaluate;
 }

 int Program::MaxDerivativesPerResidualBlock() const {
   int max_derivatives = 0;
   for (int i = 0; i < residual_blocks_.size(); ++i) {
     int derivatives = 0;
     ResidualBlock* residual_block = residual_blocks_[i];
     int num_parameters = residual_block->NumParameterBlocks();
     for (int j = 0; j < num_parameters; ++j) {
       derivatives += residual_block->NumResiduals() *
                      residual_block->parameter_blocks()[j]->TangentSize();
     }
     max_derivatives = std::max(max_derivatives, derivatives);
   }
   return max_derivatives;
 }

 int Program::MaxParametersPerResidualBlock() const {
   int max_parameters = 0;
   for (int i = 0; i < residual_blocks_.size(); ++i) {
     max_parameters =
         std::max(max_parameters, residual_blocks_[i]->NumParameterBlocks());
   }
   return max_parameters;
 }

 int Program::MaxResidualsPerResidualBlock() const {
   int max_residuals = 0;
   for (int i = 0; i < residual_blocks_.size(); ++i) {
     max_residuals =
         std::max(max_residuals, residual_blocks_[i]->NumResiduals());
   }
   return max_residuals;
 }

 std::string Program::ToString() const {
   std::string ret = "Program dump\n";
   ret += StringPrintf("Number of parameter blocks: %d\n", NumParameterBlocks());
   ret += StringPrintf("Number of parameters: %d\n", NumParameters());
   ret += "Parameters:\n";
   for (int i = 0; i < parameter_blocks_.size(); ++i) {
     ret +=
         StringPrintf("%d: %s\n", i, parameter_blocks_[i]->ToString().c_str());
   }
   return ret;
 }

 }  // namespace internal
 }  // namespace ceres
	// 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)

	#include "ceres/program.h"

	#include <algorithm>
	#include <map>
	#include <memory>
	#include <string>
	#include <vector>

	#include "ceres/array_utils.h"
	#include "ceres/casts.h"
	#include "ceres/compressed_row_sparse_matrix.h"
	#include "ceres/cost_function.h"
	#include "ceres/evaluator.h"
	#include "ceres/internal/export.h"
	#include "ceres/loss_function.h"
	#include "ceres/manifold.h"
	#include "ceres/map_util.h"
	#include "ceres/parameter_block.h"
	#include "ceres/problem.h"
	#include "ceres/residual_block.h"
	#include "ceres/stl_util.h"
	#include "ceres/triplet_sparse_matrix.h"

	namespace ceres {
	namespace internal {


	const std::vector<ParameterBlock*>& Program::parameter_blocks() const {
	return parameter_blocks_;
	}

	const std::vector<ResidualBlock*>& Program::residual_blocks() const {
	return residual_blocks_;
	}

	std::vector<ParameterBlock> Program::mutable_parameter_blocks() {
	return &parameter_blocks_;
	}

	std::vector<ResidualBlock> Program::mutable_residual_blocks() {
	return &residual_blocks_;
	}

	EvaluationCallback* Program::mutable_evaluation_callback() {
	return evaluation_callback_;
	}

	bool Program::StateVectorToParameterBlocks(const double* state) {
	for (int i = 0; i < parameter_blocks_.size(); ++i) {
	if (!parameter_blocks_[i]->IsConstant() &&
	!parameter_blocks_[i]->SetState(state)) {
	return false;
	}
	state += parameter_blocks_[i]->Size();
	}
	return true;
	}

	void Program::ParameterBlocksToStateVector(double* state) const {
	for (int i = 0; i < parameter_blocks_.size(); ++i) {
	parameter_blocks_[i]->GetState(state);
	state += parameter_blocks_[i]->Size();
	}
	}

	void Program::CopyParameterBlockStateToUserState() {
	for (int i = 0; i < parameter_blocks_.size(); ++i) {
	parameter_blocks_[i]->GetState(parameter_blocks_[i]->mutable_user_state());
	}
	}

	bool Program::SetParameterBlockStatePtrsToUserStatePtrs() {
	for (int i = 0; i < parameter_blocks_.size(); ++i) {
	if (!parameter_blocks_[i]->IsConstant() &&
	!parameter_blocks_[i]->SetState(parameter_blocks_[i]->user_state())) {
	return false;
	}
	}
	return true;
	}

	bool Program::Plus(const double* state,
	const double* delta,
	double* state_plus_delta) const {
	for (int i = 0; i < parameter_blocks_.size(); ++i) {
	if (!parameter_blocks_[i]->Plus(state, delta, state_plus_delta)) {
	return false;
	}
	state += parameter_blocks_[i]->Size();
	delta += parameter_blocks_[i]->TangentSize();
	state_plus_delta += parameter_blocks_[i]->Size();
	}
	return true;
	}

	void Program::SetParameterOffsetsAndIndex() {
	// Set positions for all parameters appearing as arguments to residuals to one
	// past the end of the parameter block array.
	for (int i = 0; i < residual_blocks_.size(); ++i) {
	ResidualBlock* residual_block = residual_blocks_[i];
	for (int j = 0; j < residual_block->NumParameterBlocks(); ++j) {
	residual_block->parameter_blocks()[j]->set_index(-1);
	}
	}
	// For parameters that appear in the program, set their position and offset.
	int state_offset = 0;
	int delta_offset = 0;
	for (int i = 0; i < parameter_blocks_.size(); ++i) {
	parameter_blocks_[i]->set_index(i);
	parameter_blocks_[i]->set_state_offset(state_offset);
	parameter_blocks_[i]->set_delta_offset(delta_offset);
	state_offset += parameter_blocks_[i]->Size();
	delta_offset += parameter_blocks_[i]->TangentSize();
	}
	}

	bool Program::IsValid() const {
	for (int i = 0; i < residual_blocks_.size(); ++i) {
	const ResidualBlock* residual_block = residual_blocks_[i];
	if (residual_block->index() != i) {
	LOG(WARNING) << "Residual block: " << i
	<< " has incorrect index: " << residual_block->index();
	return false;
	}
	}

	int state_offset = 0;
	int delta_offset = 0;
	for (int i = 0; i < parameter_blocks_.size(); ++i) {
	const ParameterBlock* parameter_block = parameter_blocks_[i];
	if (parameter_block->index() != i \|\|
	parameter_block->state_offset() != state_offset \|\|
	parameter_block->delta_offset() != delta_offset) {
	LOG(WARNING) << "Parameter block: " << i
	<< "has incorrect indexing information: "
	<< parameter_block->ToString();
	return false;
	}

	state_offset += parameter_blocks_[i]->Size();
	delta_offset += parameter_blocks_[i]->TangentSize();
	}

	return true;
	}

	bool Program::ParameterBlocksAreFinite(std::string* message) const {
	CHECK(message != nullptr);
	for (int i = 0; i < parameter_blocks_.size(); ++i) {
	const ParameterBlock* parameter_block = parameter_blocks_[i];
	const double* array = parameter_block->user_state();
	const int size = parameter_block->Size();
	const int invalid_index = FindInvalidValue(size, array);
	if (invalid_index != size) {
	*message = StringPrintf(
	"ParameterBlock: %p with size %d has at least one invalid value.\n"
	"First invalid value is at index: %d.\n"
	"Parameter block values: ",
	array,
	size,
	invalid_index);
	AppendArrayToString(size, array, message);
	return false;
	}
	}
	return true;
	}

	bool Program::IsBoundsConstrained() const {
	for (int i = 0; i < parameter_blocks_.size(); ++i) {
	const ParameterBlock* parameter_block = parameter_blocks_[i];
	if (parameter_block->IsConstant()) {
	continue;
	}
	const int size = parameter_block->Size();
	for (int j = 0; j < size; ++j) {
	const double lower_bound = parameter_block->LowerBoundForParameter(j);
	const double upper_bound = parameter_block->UpperBoundForParameter(j);
	if (lower_bound > -std::numeric_limits<double>::max() \|\|
	upper_bound < std::numeric_limits<double>::max()) {
	return true;
	}
	}
	}
	return false;
	}

	bool Program::IsFeasible(std::string* message) const {
	CHECK(message != nullptr);
	for (int i = 0; i < parameter_blocks_.size(); ++i) {
	const ParameterBlock* parameter_block = parameter_blocks_[i];
	const double* parameters = parameter_block->user_state();
	const int size = parameter_block->Size();
	if (parameter_block->IsConstant()) {
	// Constant parameter blocks must start in the feasible region
	// to ultimately produce a feasible solution, since Ceres cannot
	// change them.
	for (int j = 0; j < size; ++j) {
	const double lower_bound = parameter_block->LowerBoundForParameter(j);
	const double upper_bound = parameter_block->UpperBoundForParameter(j);
	if (parameters[j] < lower_bound \|\| parameters[j] > upper_bound) {
	*message = StringPrintf(
	"ParameterBlock: %p with size %d has at least one infeasible "
	"value."
	"\nFirst infeasible value is at index: %d."
	"\nLower bound: %e, value: %e, upper bound: %e"
	"\nParameter block values: ",
	parameters,
	size,
	j,
	lower_bound,
	parameters[j],
	upper_bound);
	AppendArrayToString(size, parameters, message);
	return false;
	}
	}
	} else {
	// Variable parameter blocks must have non-empty feasible
	// regions, otherwise there is no way to produce a feasible
	// solution.
	for (int j = 0; j < size; ++j) {
	const double lower_bound = parameter_block->LowerBoundForParameter(j);
	const double upper_bound = parameter_block->UpperBoundForParameter(j);
	if (lower_bound >= upper_bound) {
	*message = StringPrintf(
	"ParameterBlock: %p with size %d has at least one infeasible "
	"bound."
	"\nFirst infeasible bound is at index: %d."
	"\nLower bound: %e, upper bound: %e"
	"\nParameter block values: ",
	parameters,
	size,
	j,
	lower_bound,
	upper_bound);
	AppendArrayToString(size, parameters, message);
	return false;
	}
	}
	}
	}

	return true;
	}

	std::unique_ptr<Program> Program::CreateReducedProgram(
	std::vector<double> removed_parameter_blocks,
	double* fixed_cost,
	std::string* error) const {
	CHECK(removed_parameter_blocks != nullptr);
	CHECK(fixed_cost != nullptr);
	CHECK(error != nullptr);

	std::unique_ptr<Program> reduced_program = std::make_unique<Program>(*this);
	if (!reduced_program->RemoveFixedBlocks(
	removed_parameter_blocks, fixed_cost, error)) {
	return nullptr;
	}

	reduced_program->SetParameterOffsetsAndIndex();
	return reduced_program;
	}

	bool Program::RemoveFixedBlocks(std::vector<double> removed_parameter_blocks,
	double* fixed_cost,
	std::string* error) {
	CHECK(removed_parameter_blocks != nullptr);
	CHECK(fixed_cost != nullptr);
	CHECK(error != nullptr);

	std::unique_ptr<double[]> residual_block_evaluate_scratch;
	residual_block_evaluate_scratch =
	std::make_unique<double[]>(MaxScratchDoublesNeededForEvaluate());
	*fixed_cost = 0.0;

	bool need_to_call_prepare_for_evaluation = evaluation_callback_ != nullptr;

	// Mark all the parameters as unused. Abuse the index member of the
	// parameter blocks for the marking.
	for (int i = 0; i < parameter_blocks_.size(); ++i) {
	parameter_blocks_[i]->set_index(-1);
	}

	// Filter out residual that have all-constant parameters, and mark
	// all the parameter blocks that appear in residuals.
	int num_active_residual_blocks = 0;
	for (int i = 0; i < residual_blocks_.size(); ++i) {
	ResidualBlock* residual_block = residual_blocks_[i];
	int num_parameter_blocks = residual_block->NumParameterBlocks();

	// Determine if the residual block is fixed, and also mark varying
	// parameters that appear in the residual block.
	bool all_constant = true;
	for (int k = 0; k < num_parameter_blocks; k++) {
	ParameterBlock* parameter_block = residual_block->parameter_blocks()[k];
	if (!parameter_block->IsConstant()) {
	all_constant = false;
	parameter_block->set_index(1);
	}
	}

	if (!all_constant) {
	residual_blocks_[num_active_residual_blocks++] = residual_block;
	continue;
	}

	// This is an exceedingly rare case, where the user has residual
	// blocks which are effectively constant but they are also
	// performance sensitive enough to add an EvaluationCallback.
	//
	// In this case before we evaluate the cost of the constant
	// residual blocks, we must call
	// EvaluationCallback::PrepareForEvaluation(). Because this call
	// can be costly, we only call this if we actually encounter a
	// residual block with all constant parameter blocks.
	//
	// It is worth nothing that there is a minor inefficiency here,
	// that the iteration 0 of TrustRegionMinimizer will also cause
	// PrepareForEvaluation to be called on the same point, but with
	// evaluate_jacobians = true. We could try and optimize this here,
	// but given the rarity of this case, the additional complexity
	// and long range dependency is not worth it.
	if (need_to_call_prepare_for_evaluation) {
	constexpr bool kNewPoint = true;
	constexpr bool kDoNotEvaluateJacobians = false;
	evaluation_callback_->PrepareForEvaluation(kDoNotEvaluateJacobians,
	kNewPoint);
	need_to_call_prepare_for_evaluation = false;
	}

	// The residual is constant and will be removed, so its cost is
	// added to the variable fixed_cost.
	double cost = 0.0;
	if (!residual_block->Evaluate(true,
	&cost,
	nullptr,
	nullptr,
	residual_block_evaluate_scratch.get())) {
	*error = StringPrintf(
	"Evaluation of the residual %d failed during "
	"removal of fixed residual blocks.",
	i);
	return false;
	}

	*fixed_cost += cost;
	}
	residual_blocks_.resize(num_active_residual_blocks);

	// Filter out unused or fixed parameter blocks.
	int num_active_parameter_blocks = 0;
	removed_parameter_blocks->clear();
	for (int i = 0; i < parameter_blocks_.size(); ++i) {
	ParameterBlock* parameter_block = parameter_blocks_[i];
	if (parameter_block->index() == -1) {
	removed_parameter_blocks->push_back(
	parameter_block->mutable_user_state());
	} else {
	parameter_blocks_[num_active_parameter_blocks++] = parameter_block;
	}
	}
	parameter_blocks_.resize(num_active_parameter_blocks);

	if (!(((NumResidualBlocks() == 0) && (NumParameterBlocks() == 0)) \|\|
	((NumResidualBlocks() != 0) && (NumParameterBlocks() != 0)))) {
	*error = "Congratulations, you found a bug in Ceres. Please report it.";
	return false;
	}

	return true;
	}

	bool Program::IsParameterBlockSetIndependent(
	const std::set<double*>& independent_set) const {
	// Loop over each residual block and ensure that no two parameter
	// blocks in the same residual block are part of
	// parameter_block_ptrs as that would violate the assumption that it
	// is an independent set in the Hessian matrix.
	for (const ResidualBlock* residual_block : residual_blocks_) {
	ParameterBlock* const* parameter_blocks =
	residual_block->parameter_blocks();
	const int num_parameter_blocks = residual_block->NumParameterBlocks();
	int count = 0;
	for (int i = 0; i < num_parameter_blocks; ++i) {
	count += independent_set.count(parameter_blocks[i]->mutable_user_state());
	}
	if (count > 1) {
	return false;
	}
	}
	return true;
	}

	std::unique_ptr<TripletSparseMatrix>
	Program::CreateJacobianBlockSparsityTranspose(int start_residual_block) const {
	// Matrix to store the block sparsity structure of the Jacobian.
	const int num_rows = NumParameterBlocks();
	const int num_cols = NumResidualBlocks() - start_residual_block;

	std::unique_ptr<TripletSparseMatrix> tsm(
	new TripletSparseMatrix(num_rows, num_cols, 10 * num_cols));
	int num_nonzeros = 0;
	int* rows = tsm->mutable_rows();
	int* cols = tsm->mutable_cols();
	double* values = tsm->mutable_values();

	for (int c = start_residual_block; c < residual_blocks_.size(); ++c) {
	const ResidualBlock* residual_block = residual_blocks_[c];
	const int num_parameter_blocks = residual_block->NumParameterBlocks();
	ParameterBlock* const* parameter_blocks =
	residual_block->parameter_blocks();

	for (int j = 0; j < num_parameter_blocks; ++j) {
	if (parameter_blocks[j]->IsConstant()) {
	continue;
	}

	// Re-size the matrix if needed.
	if (num_nonzeros >= tsm->max_num_nonzeros()) {
	tsm->set_num_nonzeros(num_nonzeros);
	tsm->Reserve(2 * num_nonzeros);
	rows = tsm->mutable_rows();
	cols = tsm->mutable_cols();
	values = tsm->mutable_values();
	}

	const int r = parameter_blocks[j]->index();
	rows[num_nonzeros] = r;
	cols[num_nonzeros] = c - start_residual_block;
	values[num_nonzeros] = 1.0;
	++num_nonzeros;
	}
	}

	tsm->set_num_nonzeros(num_nonzeros);
	return tsm;
	}

	int Program::NumResidualBlocks() const { return residual_blocks_.size(); }

	int Program::NumParameterBlocks() const { return parameter_blocks_.size(); }

	int Program::NumResiduals() const {
	int num_residuals = 0;
	for (int i = 0; i < residual_blocks_.size(); ++i) {
	num_residuals += residual_blocks_[i]->NumResiduals();
	}
	return num_residuals;
	}

	int Program::NumParameters() const {
	int num_parameters = 0;
	for (int i = 0; i < parameter_blocks_.size(); ++i) {
	num_parameters += parameter_blocks_[i]->Size();
	}
	return num_parameters;
	}

	int Program::NumEffectiveParameters() const {
	int num_parameters = 0;
	for (int i = 0; i < parameter_blocks_.size(); ++i) {
	num_parameters += parameter_blocks_[i]->TangentSize();
	}
	return num_parameters;
	}

	// TODO(sameeragarwal): The following methods should just be updated
	// incrementally and the values cached, rather than the linear
	// complexity we have right now on every call.
	int Program::MaxScratchDoublesNeededForEvaluate() const {
	// Compute the scratch space needed for evaluate.
	int max_scratch_bytes_for_evaluate = 0;
	for (int i = 0; i < residual_blocks_.size(); ++i) {
	max_scratch_bytes_for_evaluate =
	std::max(max_scratch_bytes_for_evaluate,
	residual_blocks_[i]->NumScratchDoublesForEvaluate());
	}
	return max_scratch_bytes_for_evaluate;
	}

	int Program::MaxDerivativesPerResidualBlock() const {
	int max_derivatives = 0;
	for (int i = 0; i < residual_blocks_.size(); ++i) {
	int derivatives = 0;
	ResidualBlock* residual_block = residual_blocks_[i];
	int num_parameters = residual_block->NumParameterBlocks();
	for (int j = 0; j < num_parameters; ++j) {
	derivatives += residual_block->NumResiduals() *
	residual_block->parameter_blocks()[j]->TangentSize();
	}
	max_derivatives = std::max(max_derivatives, derivatives);
	}
	return max_derivatives;
	}

	int Program::MaxParametersPerResidualBlock() const {
	int max_parameters = 0;
	for (int i = 0; i < residual_blocks_.size(); ++i) {
	max_parameters =
	std::max(max_parameters, residual_blocks_[i]->NumParameterBlocks());
	}
	return max_parameters;
	}

	int Program::MaxResidualsPerResidualBlock() const {
	int max_residuals = 0;
	for (int i = 0; i < residual_blocks_.size(); ++i) {
	max_residuals =
	std::max(max_residuals, residual_blocks_[i]->NumResiduals());
	}
	return max_residuals;
	}

	std::string Program::ToString() const {
	std::string ret = "Program dump\n";
	ret += StringPrintf("Number of parameter blocks: %d\n", NumParameterBlocks());
	ret += StringPrintf("Number of parameters: %d\n", NumParameters());
	ret += "Parameters:\n";
	for (int i = 0; i < parameter_blocks_.size(); ++i) {
	ret +=
	StringPrintf("%d: %s\n", i, parameter_blocks_[i]->ToString().c_str());
	}
	return ret;
	}

	} // namespace internal
	} // namespace ceres