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// A jacobian writer that writes to block sparse matrices. The "writer" name is 320ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// misleading, since the Write() operation on the block jacobian writer does not 330ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// write anything. Instead, the Prepare() method on the BlockEvaluatePreparers 340ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// makes a jacobians array which has direct pointers into the block sparse 350ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// jacobian. When the cost function is evaluated, the jacobian blocks get placed 360ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// directly in their final location. 370ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 380ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#ifndef CERES_INTERNAL_BLOCK_JACOBIAN_WRITER_H_ 390ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#define CERES_INTERNAL_BLOCK_JACOBIAN_WRITER_H_ 400ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 410ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include <vector> 420ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/evaluator.h" 430ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/internal/port.h" 440ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 450ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongnamespace ceres { 460ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongnamespace internal { 470ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 480ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongclass BlockEvaluatePreparer; 490ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongclass Program; 500ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongclass SparseMatrix; 510ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 520ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongclass BlockJacobianWriter { 530ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong public: 540ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong BlockJacobianWriter(const Evaluator::Options& options, 550ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong Program* program); 560ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 570ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // JacobianWriter interface. 580ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 590ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Create evaluate prepareres that point directly into the final jacobian. 600ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // This makes the final Write() a nop. 610ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong BlockEvaluatePreparer* CreateEvaluatePreparers(int num_threads); 620ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 630ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong SparseMatrix* CreateJacobian() const; 640ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 650ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong void Write(int /* residual_id */, 660ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong int /* residual_offset */, 670ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double** /* jacobians */, 680ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong SparseMatrix* /* jacobian */) { 690ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // This is a noop since the blocks were written directly into their final 700ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // position by the outside evaluate call, thanks to the jacobians array 710ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // prepared by the BlockEvaluatePreparers. 720ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 730ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 740ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong private: 750ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong Program* program_; 760ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 770ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Stores the position of each residual / parameter jacobian. 780ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // 790ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // The block sparse matrix that this writer writes to is stored as a set of 800ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // contiguos dense blocks, one after each other; see BlockSparseMatrix. The 810ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // "double* values_" member of the block sparse matrix contains all of these 820ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // blocks. Given a pointer to the first element of a block and the size of 830ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // that block, it's possible to write to it. 840ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // 850ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // In the case of a block sparse jacobian, the jacobian writer needs a way to 860ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // find the offset in the values_ array of each residual/parameter jacobian 870ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // block. 880ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // 890ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // That is the purpose of jacobian_layout_. 900ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // 910ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // In particular, jacobian_layout_[i][j] is the offset in the values_ array of 920ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // the derivative of residual block i with respect to the parameter block at 930ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // active argument position j. 940ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // 950ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // The active qualifier means that non-active parameters do not count. Care 960ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // must be taken when indexing into jacobian_layout_ to account for this. 970ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Consider a single residual example: 980ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // 990ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // r(x, y, z) 1000ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // 1010ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // with r in R^3, x in R^4, y in R^2, and z in R^5. 1020ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Take y as a constant (non-active) parameter. 1030ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Take r as residual number 0. 1040ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // 1050ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // In this case, the active arguments are only (x, z), so the active argument 1060ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // position for x is 0, and the active argument position for z is 1. This is 1070ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // similar to thinking of r as taking only 2 parameters: 1080ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // 1090ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // r(x, z) 1100ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // 1110ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // There are only 2 jacobian blocks: dr/dx and dr/dz. jacobian_layout_ would 1120ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // have the following contents: 1130ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // 1140ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // jacobian_layout_[0] = { 0, 12 } 1150ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // 1160ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // which indicates that dr/dx is located at values_[0], and dr/dz is at 1170ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // values_[12]. See BlockEvaluatePreparer::Prepare()'s comments about 'j'. 1180ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong vector<int*> jacobian_layout_; 1190ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1200ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // The pointers in jacobian_layout_ point directly into this vector. 1210ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong vector<int> jacobian_layout_storage_; 1220ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong}; 1230ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1240ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} // namespace internal 1250ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} // namespace ceres 1260ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1270ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#endif // CERES_INTERNAL_BLOCK_JACOBIAN_WRITER_H_ 128