1 files changed, 41 insertions, 33 deletions

diff --git a/internal/ceres/compressed_row_jacobian_writer.cc b/internal/ceres/compressed_row_jacobian_writer.cc
index bbadb77..ed8db14 100644
--- a/internal/ceres/compressed_row_jacobian_writer.cc
+++ b/internal/ceres/compressed_row_jacobian_writer.cc
@@ -40,6 +40,44 @@
 namespace ceres {
 namespace internal {
 
+void CompressedRowJacobianWriter::PopulateJacobianRowAndColumnBlockVectors(
+    const Program* program, CompressedRowSparseMatrix* jacobian) {
+  const vector<ParameterBlock*>& parameter_blocks =
+      program->parameter_blocks();
+  vector<int>& col_blocks = *(jacobian->mutable_col_blocks());
+  col_blocks.resize(parameter_blocks.size());
+  for (int i = 0; i < parameter_blocks.size(); ++i) {
+    col_blocks[i] = parameter_blocks[i]->LocalSize();
+  }
+
+  const vector<ResidualBlock*>& residual_blocks =
+      program->residual_blocks();
+  vector<int>& row_blocks = *(jacobian->mutable_row_blocks());
+  row_blocks.resize(residual_blocks.size());
+  for (int i = 0; i < residual_blocks.size(); ++i) {
+    row_blocks[i] = residual_blocks[i]->NumResiduals();
+  }
+}
+
+void CompressedRowJacobianWriter::GetOrderedParameterBlocks(
+      const Program* program,
+      int residual_id,
+      vector<pair<int, int> >* evaluated_jacobian_blocks) {
+  const ResidualBlock* residual_block =
+      program->residual_blocks()[residual_id];
+  const int num_parameter_blocks = residual_block->NumParameterBlocks();
+
+  for (int j = 0; j < num_parameter_blocks; ++j) {
+    const ParameterBlock* parameter_block =
+        residual_block->parameter_blocks()[j];
+    if (!parameter_block->IsConstant()) {
+      evaluated_jacobian_blocks->push_back(
+          make_pair(parameter_block->index(), j));
+    }
+  }
+  sort(evaluated_jacobian_blocks->begin(), evaluated_jacobian_blocks->end());
+}
+
 SparseMatrix* CompressedRowJacobianWriter::CreateJacobian() const {
   const vector<ResidualBlock*>& residual_blocks =
       program_->residual_blocks();
@@ -71,7 +109,7 @@ SparseMatrix* CompressedRowJacobianWriter::CreateJacobian() const {
           total_num_effective_parameters,
           num_jacobian_nonzeros + total_num_effective_parameters);
 
-  // At this stage, the CompressedSparseMatrix is an invalid state. But this
+  // At this stage, the CompressedRowSparseMatrix is an invalid state. But this
   // seems to be the only way to construct it without doing a memory copy.
   int* rows = jacobian->mutable_rows();
   int* cols = jacobian->mutable_cols();
@@ -132,22 +170,7 @@ SparseMatrix* CompressedRowJacobianWriter::CreateJacobian() const {
   }
   CHECK_EQ(num_jacobian_nonzeros, rows[total_num_residuals]);
 
-  // Populate the row and column block vectors for use by block
-  // oriented ordering algorithms. This is useful when
-  // Solver::Options::use_block_amd = true.
-  const vector<ParameterBlock*>& parameter_blocks =
-      program_->parameter_blocks();
-  vector<int>& col_blocks = *(jacobian->mutable_col_blocks());
-  col_blocks.resize(parameter_blocks.size());
-  for (int i = 0; i <  parameter_blocks.size(); ++i) {
-    col_blocks[i] = parameter_blocks[i]->LocalSize();
-  }
-
-  vector<int>& row_blocks = *(jacobian->mutable_row_blocks());
-  row_blocks.resize(residual_blocks.size());
-  for (int i = 0; i <  residual_blocks.size(); ++i) {
-    row_blocks[i] = residual_blocks[i]->NumResiduals();
-  }
+  PopulateJacobianRowAndColumnBlockVectors(program_, jacobian);
 
   return jacobian;
 }
@@ -164,25 +187,10 @@ void CompressedRowJacobianWriter::Write(int residual_id,
 
   const ResidualBlock* residual_block =
       program_->residual_blocks()[residual_id];
-  const int num_parameter_blocks = residual_block->NumParameterBlocks();
   const int num_residuals = residual_block->NumResiduals();
 
-  // It is necessary to determine the order of the jacobian blocks before
-  // copying them into the CompressedRowSparseMatrix. Just because a cost
-  // function uses parameter blocks 1 after 2 in its arguments does not mean
-  // that the block 1 occurs before block 2 in the column layout of the
-  // jacobian. Thus, determine the order by sorting the jacobian blocks by their
-  // position in the state vector.
   vector<pair<int, int> > evaluated_jacobian_blocks;
-  for (int j = 0; j < num_parameter_blocks; ++j) {
-    const ParameterBlock* parameter_block =
-        residual_block->parameter_blocks()[j];
-    if (!parameter_block->IsConstant()) {
-      evaluated_jacobian_blocks.push_back(
-          make_pair(parameter_block->index(), j));
-    }
-  }
-  sort(evaluated_jacobian_blocks.begin(), evaluated_jacobian_blocks.end());
+  GetOrderedParameterBlocks(program_, residual_id, &evaluated_jacobian_blocks);
 
   // Where in the current row does the jacobian for a parameter block begin.
   int col_pos = 0;