10ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Ceres Solver - A fast non-linear least squares minimizer 20ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. 30ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// http://code.google.com/p/ceres-solver/ 40ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 50ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Redistribution and use in source and binary forms, with or without 60ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// modification, are permitted provided that the following conditions are met: 70ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 80ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// * Redistributions of source code must retain the above copyright notice, 90ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// this list of conditions and the following disclaimer. 100ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// * Redistributions in binary form must reproduce the above copyright notice, 110ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// this list of conditions and the following disclaimer in the documentation 120ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// and/or other materials provided with the distribution. 130ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// * Neither the name of Google Inc. nor the names of its contributors may be 140ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// used to endorse or promote products derived from this software without 150ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// specific prior written permission. 160ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 170ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 180ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 190ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 200ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 210ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 220ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 230ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 240ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 250ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 260ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 270ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// POSSIBILITY OF SUCH DAMAGE. 280ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 290ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Author: keir@google.com (Keir Mierle) 300ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 310ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/compressed_row_jacobian_writer.h" 320ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 330ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/casts.h" 340ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/compressed_row_sparse_matrix.h" 350ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/parameter_block.h" 360ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/program.h" 370ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/residual_block.h" 380ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/scratch_evaluate_preparer.h" 390ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 400ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongnamespace ceres { 410ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongnamespace internal { 420ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 4379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandezvoid CompressedRowJacobianWriter::PopulateJacobianRowAndColumnBlockVectors( 4479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez const Program* program, CompressedRowSparseMatrix* jacobian) { 4579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez const vector<ParameterBlock*>& parameter_blocks = 4679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez program->parameter_blocks(); 4779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez vector<int>& col_blocks = *(jacobian->mutable_col_blocks()); 4879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez col_blocks.resize(parameter_blocks.size()); 4979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez for (int i = 0; i < parameter_blocks.size(); ++i) { 5079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez col_blocks[i] = parameter_blocks[i]->LocalSize(); 5179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez } 5279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 5379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez const vector<ResidualBlock*>& residual_blocks = 5479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez program->residual_blocks(); 5579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez vector<int>& row_blocks = *(jacobian->mutable_row_blocks()); 5679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez row_blocks.resize(residual_blocks.size()); 5779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez for (int i = 0; i < residual_blocks.size(); ++i) { 5879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez row_blocks[i] = residual_blocks[i]->NumResiduals(); 5979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez } 6079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez} 6179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 6279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandezvoid CompressedRowJacobianWriter::GetOrderedParameterBlocks( 6379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez const Program* program, 6479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez int residual_id, 6579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez vector<pair<int, int> >* evaluated_jacobian_blocks) { 6679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez const ResidualBlock* residual_block = 6779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez program->residual_blocks()[residual_id]; 6879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez const int num_parameter_blocks = residual_block->NumParameterBlocks(); 6979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 7079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez for (int j = 0; j < num_parameter_blocks; ++j) { 7179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez const ParameterBlock* parameter_block = 7279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez residual_block->parameter_blocks()[j]; 7379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez if (!parameter_block->IsConstant()) { 7479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez evaluated_jacobian_blocks->push_back( 7579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez make_pair(parameter_block->index(), j)); 7679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez } 7779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez } 7879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez sort(evaluated_jacobian_blocks->begin(), evaluated_jacobian_blocks->end()); 7979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez} 8079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 810ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus KongSparseMatrix* CompressedRowJacobianWriter::CreateJacobian() const { 820ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const vector<ResidualBlock*>& residual_blocks = 830ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong program_->residual_blocks(); 840ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 850ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong int total_num_residuals = program_->NumResiduals(); 860ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong int total_num_effective_parameters = program_->NumEffectiveParameters(); 870ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 880ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Count the number of jacobian nonzeros. 890ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong int num_jacobian_nonzeros = 0; 900ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int i = 0; i < residual_blocks.size(); ++i) { 910ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong ResidualBlock* residual_block = residual_blocks[i]; 920ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const int num_residuals = residual_block->NumResiduals(); 930ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const int num_parameter_blocks = residual_block->NumParameterBlocks(); 940ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int j = 0; j < num_parameter_blocks; ++j) { 950ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong ParameterBlock* parameter_block = residual_block->parameter_blocks()[j]; 960ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong if (!parameter_block->IsConstant()) { 970ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong num_jacobian_nonzeros += num_residuals * parameter_block->LocalSize(); 980ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 990ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1000ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1010ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1020ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Allocate storage for the jacobian with some extra space at the end. 1030ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Allocate more space than needed to store the jacobian so that when the LM 1040ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // algorithm adds the diagonal, no reallocation is necessary. This reduces 1050ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // peak memory usage significantly. 1060ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong CompressedRowSparseMatrix* jacobian = 1070ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong new CompressedRowSparseMatrix( 1080ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong total_num_residuals, 1090ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong total_num_effective_parameters, 1100ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong num_jacobian_nonzeros + total_num_effective_parameters); 1110ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 11279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // At this stage, the CompressedRowSparseMatrix is an invalid state. But this 1130ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // seems to be the only way to construct it without doing a memory copy. 1140ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong int* rows = jacobian->mutable_rows(); 1150ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong int* cols = jacobian->mutable_cols(); 1160ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong int row_pos = 0; 1170ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong rows[0] = 0; 1180ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int i = 0; i < residual_blocks.size(); ++i) { 1190ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const ResidualBlock* residual_block = residual_blocks[i]; 1200ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const int num_parameter_blocks = residual_block->NumParameterBlocks(); 1210ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1220ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Count the number of derivatives for a row of this residual block and 1230ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // build a list of active parameter block indices. 1240ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong int num_derivatives = 0; 1250ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong vector<int> parameter_indices; 1260ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int j = 0; j < num_parameter_blocks; ++j) { 1270ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong ParameterBlock* parameter_block = residual_block->parameter_blocks()[j]; 1280ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong if (!parameter_block->IsConstant()) { 1290ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong parameter_indices.push_back(parameter_block->index()); 1300ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong num_derivatives += parameter_block->LocalSize(); 1310ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1320ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1330ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1340ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Sort the parameters by their position in the state vector. 1350ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong sort(parameter_indices.begin(), parameter_indices.end()); 1360ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong CHECK(unique(parameter_indices.begin(), parameter_indices.end()) == 1370ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong parameter_indices.end()) 1380ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong << "Ceres internal error: " 1390ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong << "Duplicate parameter blocks detected in a cost function. " 1400ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong << "This should never happen. Please report this to " 1410ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong << "the Ceres developers."; 1420ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1430ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Update the row indices. 1440ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const int num_residuals = residual_block->NumResiduals(); 1450ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int j = 0; j < num_residuals; ++j) { 1460ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong rows[row_pos + j + 1] = rows[row_pos + j] + num_derivatives; 1470ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1480ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1490ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Iterate over parameter blocks in the order which they occur in the 1500ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // parameter vector. This code mirrors that in Write(), where jacobian 1510ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // values are updated. 1520ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong int col_pos = 0; 1530ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int j = 0; j < parameter_indices.size(); ++j) { 1540ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong ParameterBlock* parameter_block = 1550ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong program_->parameter_blocks()[parameter_indices[j]]; 1560ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const int parameter_block_size = parameter_block->LocalSize(); 1570ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1580ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int r = 0; r < num_residuals; ++r) { 1590ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // This is the position in the values array of the jacobian where this 1600ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // row of the jacobian block should go. 1610ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const int column_block_begin = rows[row_pos + r] + col_pos; 1620ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1630ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int c = 0; c < parameter_block_size; ++c) { 1640ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong cols[column_block_begin + c] = parameter_block->delta_offset() + c; 1650ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1660ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1670ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong col_pos += parameter_block_size; 1680ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1690ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong row_pos += num_residuals; 1700ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1710ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong CHECK_EQ(num_jacobian_nonzeros, rows[total_num_residuals]); 1720ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 17379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez PopulateJacobianRowAndColumnBlockVectors(program_, jacobian); 1740ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1750ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return jacobian; 1760ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 1770ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1780ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongvoid CompressedRowJacobianWriter::Write(int residual_id, 1790ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong int residual_offset, 1800ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double **jacobians, 1810ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong SparseMatrix* base_jacobian) { 1820ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong CompressedRowSparseMatrix* jacobian = 1830ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong down_cast<CompressedRowSparseMatrix*>(base_jacobian); 1840ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1850ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double* jacobian_values = jacobian->mutable_values(); 1860ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const int* jacobian_rows = jacobian->rows(); 1870ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1880ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const ResidualBlock* residual_block = 1890ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong program_->residual_blocks()[residual_id]; 1900ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const int num_residuals = residual_block->NumResiduals(); 1910ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1920ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong vector<pair<int, int> > evaluated_jacobian_blocks; 19379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez GetOrderedParameterBlocks(program_, residual_id, &evaluated_jacobian_blocks); 1940ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1950ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Where in the current row does the jacobian for a parameter block begin. 1960ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong int col_pos = 0; 1970ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1980ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Iterate over the jacobian blocks in increasing order of their 1990ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // positions in the reduced parameter vector. 2000ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int i = 0; i < evaluated_jacobian_blocks.size(); ++i) { 2010ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const ParameterBlock* parameter_block = 2020ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong program_->parameter_blocks()[evaluated_jacobian_blocks[i].first]; 2030ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const int argument = evaluated_jacobian_blocks[i].second; 2040ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const int parameter_block_size = parameter_block->LocalSize(); 2050ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2060ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Copy one row of the jacobian block at a time. 2070ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int r = 0; r < num_residuals; ++r) { 2080ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Position of the r^th row of the current jacobian block. 2090ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const double* block_row_begin = 2100ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong jacobians[argument] + r * parameter_block_size; 2110ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2120ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Position in the values array of the jacobian where this 2130ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // row of the jacobian block should go. 2140ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double* column_block_begin = 2150ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong jacobian_values + jacobian_rows[residual_offset + r] + col_pos; 2160ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2170ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong copy(block_row_begin, 2180ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong block_row_begin + parameter_block_size, 2190ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong column_block_begin); 2200ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 2210ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong col_pos += parameter_block_size; 2220ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 2230ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 2240ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2250ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} // namespace internal 2260ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} // namespace ceres 227