1 files changed, 144 insertions, 18 deletions

diff --git a/internal/ceres/problem_impl.cc b/internal/ceres/problem_impl.cc
index 8302702..7c86efb 100644
--- a/internal/ceres/problem_impl.cc
+++ b/internal/ceres/problem_impl.cc
@@ -1,5 +1,5 @@
 // Ceres Solver - A fast non-linear least squares minimizer
-// Copyright 2010, 2011, 2012 Google Inc. All rights reserved.
+// Copyright 2013 Google Inc. All rights reserved.
 // http://code.google.com/p/ceres-solver/
 //
 // Redistribution and use in source and binary forms, with or without
@@ -27,7 +27,7 @@
 // POSSIBILITY OF SUCH DAMAGE.
 //
 // Author: sameeragarwal@google.com (Sameer Agarwal)
-//         keir@google.com (Keir Mierle)
+//         mierle@gmail.com (Keir Mierle)
 
 #include "ceres/problem_impl.h"
 
@@ -142,7 +142,7 @@ ParameterBlock* ProblemImpl::InternalAddParameterBlock(double* values,
 
   // For dynamic problems, add the list of dependent residual blocks, which is
   // empty to start.
-  if (options_.enable_fast_parameter_block_removal) {
+  if (options_.enable_fast_removal) {
     new_parameter_block->EnableResidualBlockDependencies();
   }
   parameter_block_map_[values] = new_parameter_block;
@@ -150,6 +150,26 @@ ParameterBlock* ProblemImpl::InternalAddParameterBlock(double* values,
   return new_parameter_block;
 }
 
+void ProblemImpl::InternalRemoveResidualBlock(ResidualBlock* residual_block) {
+  CHECK_NOTNULL(residual_block);
+  // Perform no check on the validity of residual_block, that is handled in
+  // the public method: RemoveResidualBlock().
+
+  // If needed, remove the parameter dependencies on this residual block.
+  if (options_.enable_fast_removal) {
+    const int num_parameter_blocks_for_residual =
+        residual_block->NumParameterBlocks();
+    for (int i = 0; i < num_parameter_blocks_for_residual; ++i) {
+      residual_block->parameter_blocks()[i]
+          ->RemoveResidualBlock(residual_block);
+    }
+
+    ResidualBlockSet::iterator it = residual_block_set_.find(residual_block);
+    residual_block_set_.erase(it);
+  }
+  DeleteBlockInVector(program_->mutable_residual_blocks(), residual_block);
+}
+
 // Deletes the residual block in question, assuming there are no other
 // references to it inside the problem (e.g. by another parameter). Referenced
 // cost and loss functions are tucked away for future deletion, since it is not
@@ -224,7 +244,7 @@ ResidualBlock* ProblemImpl::AddResidualBlock(
            cost_function->parameter_block_sizes().size());
 
   // Check the sizes match.
-  const vector<int16>& parameter_block_sizes =
+  const vector<int32>& parameter_block_sizes =
       cost_function->parameter_block_sizes();
 
   if (!options_.disable_all_safety_checks) {
@@ -278,13 +298,18 @@ ResidualBlock* ProblemImpl::AddResidualBlock(
                         program_->residual_blocks_.size());
 
   // Add dependencies on the residual to the parameter blocks.
-  if (options_.enable_fast_parameter_block_removal) {
+  if (options_.enable_fast_removal) {
     for (int i = 0; i < parameter_blocks.size(); ++i) {
       parameter_block_ptrs[i]->AddResidualBlock(new_residual_block);
     }
   }
 
   program_->residual_blocks_.push_back(new_residual_block);
+
+  if (options_.enable_fast_removal) {
+    residual_block_set_.insert(new_residual_block);
+  }
+
   return new_residual_block;
 }
 
@@ -452,7 +477,11 @@ template<typename Block>
 void ProblemImpl::DeleteBlockInVector(vector<Block*>* mutable_blocks,
                                       Block* block_to_remove) {
   CHECK_EQ((*mutable_blocks)[block_to_remove->index()], block_to_remove)
-      << "You found a Ceres bug! Block: " << block_to_remove->ToString();
+      << "You found a Ceres bug! \n"
+      << "Block requested: "
+      << block_to_remove->ToString() << "\n"
+      << "Block present: "
+      << (*mutable_blocks)[block_to_remove->index()]->ToString();
 
   // Prepare the to-be-moved block for the new, lower-in-index position by
   // setting the index to the blocks final location.
@@ -471,30 +500,46 @@ void ProblemImpl::DeleteBlockInVector(vector<Block*>* mutable_blocks,
 void ProblemImpl::RemoveResidualBlock(ResidualBlock* residual_block) {
   CHECK_NOTNULL(residual_block);
 
-  // If needed, remove the parameter dependencies on this residual block.
-  if (options_.enable_fast_parameter_block_removal) {
-    const int num_parameter_blocks_for_residual =
-        residual_block->NumParameterBlocks();
-    for (int i = 0; i < num_parameter_blocks_for_residual; ++i) {
-      residual_block->parameter_blocks()[i]
-          ->RemoveResidualBlock(residual_block);
-    }
+  // Verify that residual_block identifies a residual in the current problem.
+  const string residual_not_found_message =
+      StringPrintf("Residual block to remove: %p not found. This usually means "
+                   "one of three things have happened:\n"
+                   " 1) residual_block is uninitialised and points to a random "
+                   "area in memory.\n"
+                   " 2) residual_block represented a residual that was added to"
+                   " the problem, but referred to a parameter block which has "
+                   "since been removed, which removes all residuals which "
+                   "depend on that parameter block, and was thus removed.\n"
+                   " 3) residual_block referred to a residual that has already "
+                   "been removed from the problem (by the user).",
+                   residual_block);
+  if (options_.enable_fast_removal) {
+    CHECK(residual_block_set_.find(residual_block) !=
+          residual_block_set_.end())
+        << residual_not_found_message;
+  } else {
+    // Perform a full search over all current residuals.
+    CHECK(std::find(program_->residual_blocks().begin(),
+                    program_->residual_blocks().end(),
+                    residual_block) != program_->residual_blocks().end())
+        << residual_not_found_message;
   }
-  DeleteBlockInVector(program_->mutable_residual_blocks(), residual_block);
+
+  InternalRemoveResidualBlock(residual_block);
 }
 
 void ProblemImpl::RemoveParameterBlock(double* values) {
   ParameterBlock* parameter_block =
       FindParameterBlockOrDie(parameter_block_map_, values);
 
-  if (options_.enable_fast_parameter_block_removal) {
+  if (options_.enable_fast_removal) {
     // Copy the dependent residuals from the parameter block because the set of
     // dependents will change after each call to RemoveResidualBlock().
     vector<ResidualBlock*> residual_blocks_to_remove(
         parameter_block->mutable_residual_blocks()->begin(),
         parameter_block->mutable_residual_blocks()->end());
     for (int i = 0; i < residual_blocks_to_remove.size(); ++i) {
-      RemoveResidualBlock(residual_blocks_to_remove[i]);
+      InternalRemoveResidualBlock(residual_blocks_to_remove[i]);
     }
   } else {
     // Scan all the residual blocks to remove ones that depend on the parameter
@@ -506,7 +551,7 @@ void ProblemImpl::RemoveParameterBlock(double* values) {
       const int num_parameter_blocks = residual_block->NumParameterBlocks();
       for (int j = 0; j < num_parameter_blocks; ++j) {
         if (residual_block->parameter_blocks()[j] == parameter_block) {
-          RemoveResidualBlock(residual_block);
+          InternalRemoveResidualBlock(residual_block);
           // The parameter blocks are guaranteed unique.
           break;
         }
@@ -531,6 +576,26 @@ void ProblemImpl::SetParameterization(
       ->SetParameterization(local_parameterization);
 }
 
+const LocalParameterization* ProblemImpl::GetParameterization(
+    double* values) const {
+  return FindParameterBlockOrDie(parameter_block_map_, values)
+      ->local_parameterization();
+}
+
+void ProblemImpl::SetParameterLowerBound(double* values,
+                                         int index,
+                                         double lower_bound) {
+  FindParameterBlockOrDie(parameter_block_map_, values)
+      ->SetLowerBound(index, lower_bound);
+}
+
+void ProblemImpl::SetParameterUpperBound(double* values,
+                                         int index,
+                                         double upper_bound) {
+  FindParameterBlockOrDie(parameter_block_map_, values)
+      ->SetUpperBound(index, upper_bound);
+}
+
 bool ProblemImpl::Evaluate(const Problem::EvaluateOptions& evaluate_options,
                            double* cost,
                            vector<double>* residuals,
@@ -634,6 +699,9 @@ bool ProblemImpl::Evaluate(const Problem::EvaluateOptions& evaluate_options,
     for (int i = 0; i < variable_parameter_blocks.size(); ++i) {
       variable_parameter_blocks[i]->SetVarying();
     }
+
+    program_->SetParameterBlockStatePtrsToUserStatePtrs();
+    program_->SetParameterOffsetsAndIndex();
     return false;
   }
 
@@ -692,6 +760,8 @@ bool ProblemImpl::Evaluate(const Problem::EvaluateOptions& evaluate_options,
     }
   }
 
+  program_->SetParameterBlockStatePtrsToUserStatePtrs();
+  program_->SetParameterOffsetsAndIndex();
   return status;
 }
 
@@ -721,6 +791,11 @@ int ProblemImpl::ParameterBlockLocalSize(const double* parameter_block) const {
       parameter_block_map_, const_cast<double*>(parameter_block))->LocalSize();
 };
 
+bool ProblemImpl::HasParameterBlock(const double* parameter_block) const {
+  return (parameter_block_map_.find(const_cast<double*>(parameter_block)) !=
+          parameter_block_map_.end());
+}
+
 void ProblemImpl::GetParameterBlocks(vector<double*>* parameter_blocks) const {
   CHECK_NOTNULL(parameter_blocks);
   parameter_blocks->resize(0);
@@ -731,6 +806,57 @@ void ProblemImpl::GetParameterBlocks(vector<double*>* parameter_blocks) const {
   }
 }
 
+void ProblemImpl::GetResidualBlocks(
+    vector<ResidualBlockId>* residual_blocks) const {
+  CHECK_NOTNULL(residual_blocks);
+  *residual_blocks = program().residual_blocks();
+}
+
+void ProblemImpl::GetParameterBlocksForResidualBlock(
+    const ResidualBlockId residual_block,
+    vector<double*>* parameter_blocks) const {
+  int num_parameter_blocks = residual_block->NumParameterBlocks();
+  CHECK_NOTNULL(parameter_blocks)->resize(num_parameter_blocks);
+  for (int i = 0; i < num_parameter_blocks; ++i) {
+    (*parameter_blocks)[i] =
+        residual_block->parameter_blocks()[i]->mutable_user_state();
+  }
+}
+
+void ProblemImpl::GetResidualBlocksForParameterBlock(
+    const double* values,
+    vector<ResidualBlockId>* residual_blocks) const {
+  ParameterBlock* parameter_block =
+      FindParameterBlockOrDie(parameter_block_map_,
+                              const_cast<double*>(values));
+
+  if (options_.enable_fast_removal) {
+    // In this case the residual blocks that depend on the parameter block are
+    // stored in the parameter block already, so just copy them out.
+    CHECK_NOTNULL(residual_blocks)->resize(
+        parameter_block->mutable_residual_blocks()->size());
+    std::copy(parameter_block->mutable_residual_blocks()->begin(),
+              parameter_block->mutable_residual_blocks()->end(),
+              residual_blocks->begin());
+    return;
+  }
+
+  // Find residual blocks that depend on the parameter block.
+  CHECK_NOTNULL(residual_blocks)->clear();
+  const int num_residual_blocks = NumResidualBlocks();
+  for (int i = 0; i < num_residual_blocks; ++i) {
+    ResidualBlock* residual_block =
+        (*(program_->mutable_residual_blocks()))[i];
+    const int num_parameter_blocks = residual_block->NumParameterBlocks();
+    for (int j = 0; j < num_parameter_blocks; ++j) {
+      if (residual_block->parameter_blocks()[j] == parameter_block) {
+        residual_blocks->push_back(residual_block);
+        // The parameter blocks are guaranteed unique.
+        break;
+      }
+    }
+  }
+}
 
 }  // namespace internal
 }  // namespace ceres