10ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Ceres Solver - A fast non-linear least squares minimizer 279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// Copyright 2013 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: sameeragarwal@google.com (Sameer Agarwal) 300ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// keir@google.com (Keir Mierle) 310ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 320ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/problem.h" 331d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#include "ceres/problem_impl.h" 340ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 351d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#include "ceres/casts.h" 360ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/cost_function.h" 371d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#include "ceres/crs_matrix.h" 381d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#include "ceres/evaluator_test_utils.cc" 391d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#include "ceres/internal/eigen.h" 401d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#include "ceres/internal/scoped_ptr.h" 410ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/local_parameterization.h" 421d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#include "ceres/map_util.h" 431d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#include "ceres/parameter_block.h" 441d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#include "ceres/program.h" 450ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/sized_cost_function.h" 461d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#include "ceres/sparse_matrix.h" 471d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#include "ceres/types.h" 481d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#include "gtest/gtest.h" 490ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 500ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongnamespace ceres { 510ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongnamespace internal { 520ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 530ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// The following three classes are for the purposes of defining 540ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// function signatures. They have dummy Evaluate functions. 550ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 560ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Trivial cost function that accepts a single argument. 570ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongclass UnaryCostFunction : public CostFunction { 580ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong public: 5979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez UnaryCostFunction(int num_residuals, int32 parameter_block_size) { 600ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong set_num_residuals(num_residuals); 610ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong mutable_parameter_block_sizes()->push_back(parameter_block_size); 620ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 630ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong virtual ~UnaryCostFunction() {} 640ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 650ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong virtual bool Evaluate(double const* const* parameters, 660ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double* residuals, 670ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double** jacobians) const { 680ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int i = 0; i < num_residuals(); ++i) { 690ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong residuals[i] = 1; 700ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 710ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return true; 720ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 730ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong}; 740ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 750ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Trivial cost function that accepts two arguments. 760ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongclass BinaryCostFunction: public CostFunction { 770ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong public: 780ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong BinaryCostFunction(int num_residuals, 7979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez int32 parameter_block1_size, 8079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez int32 parameter_block2_size) { 810ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong set_num_residuals(num_residuals); 820ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong mutable_parameter_block_sizes()->push_back(parameter_block1_size); 830ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong mutable_parameter_block_sizes()->push_back(parameter_block2_size); 840ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 850ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 860ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong virtual bool Evaluate(double const* const* parameters, 870ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double* residuals, 880ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double** jacobians) const { 890ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int i = 0; i < num_residuals(); ++i) { 900ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong residuals[i] = 2; 910ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 920ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return true; 930ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 940ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong}; 950ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 960ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Trivial cost function that accepts three arguments. 970ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongclass TernaryCostFunction: public CostFunction { 980ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong public: 990ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong TernaryCostFunction(int num_residuals, 10079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez int32 parameter_block1_size, 10179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez int32 parameter_block2_size, 10279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez int32 parameter_block3_size) { 1030ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong set_num_residuals(num_residuals); 1040ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong mutable_parameter_block_sizes()->push_back(parameter_block1_size); 1050ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong mutable_parameter_block_sizes()->push_back(parameter_block2_size); 1060ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong mutable_parameter_block_sizes()->push_back(parameter_block3_size); 1070ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1080ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1090ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong virtual bool Evaluate(double const* const* parameters, 1100ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double* residuals, 1110ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double** jacobians) const { 1120ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int i = 0; i < num_residuals(); ++i) { 1130ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong residuals[i] = 3; 1140ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1150ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return true; 1160ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1170ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong}; 1180ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1190ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus KongTEST(Problem, AddResidualWithNullCostFunctionDies) { 1200ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double x[3], y[4], z[5]; 1210ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1220ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong Problem problem; 1230ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong problem.AddParameterBlock(x, 3); 1240ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong problem.AddParameterBlock(y, 4); 1250ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong problem.AddParameterBlock(z, 5); 1260ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1270ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong EXPECT_DEATH_IF_SUPPORTED(problem.AddResidualBlock(NULL, NULL, x), 1280ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong "'cost_function' Must be non NULL"); 1290ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 1300ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1310ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus KongTEST(Problem, AddResidualWithIncorrectNumberOfParameterBlocksDies) { 1320ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double x[3], y[4], z[5]; 1330ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1340ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong Problem problem; 1350ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong problem.AddParameterBlock(x, 3); 1360ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong problem.AddParameterBlock(y, 4); 1370ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong problem.AddParameterBlock(z, 5); 1380ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1390ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // UnaryCostFunction takes only one parameter, but two are passed. 1400ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong EXPECT_DEATH_IF_SUPPORTED( 1410ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong problem.AddResidualBlock(new UnaryCostFunction(2, 3), NULL, x, y), 14279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez "parameter_blocks.size"); 1430ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 1440ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1450ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus KongTEST(Problem, AddResidualWithDifferentSizesOnTheSameVariableDies) { 1460ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double x[3]; 1470ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1480ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong Problem problem; 1490ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong problem.AddResidualBlock(new UnaryCostFunction(2, 3), NULL, x); 1500ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong EXPECT_DEATH_IF_SUPPORTED(problem.AddResidualBlock( 1510ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong new UnaryCostFunction( 1520ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2, 4 /* 4 != 3 */), NULL, x), 1530ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong "different block sizes"); 1540ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 1550ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1560ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus KongTEST(Problem, AddResidualWithDuplicateParametersDies) { 1570ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double x[3], z[5]; 1580ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1590ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong Problem problem; 1600ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong EXPECT_DEATH_IF_SUPPORTED(problem.AddResidualBlock( 1610ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong new BinaryCostFunction(2, 3, 3), NULL, x, x), 1620ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong "Duplicate parameter blocks"); 1630ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong EXPECT_DEATH_IF_SUPPORTED(problem.AddResidualBlock( 1640ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong new TernaryCostFunction(1, 5, 3, 5), 1650ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong NULL, z, x, z), 1660ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong "Duplicate parameter blocks"); 1670ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 1680ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1690ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus KongTEST(Problem, AddResidualWithIncorrectSizesOfParameterBlockDies) { 1700ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double x[3], y[4], z[5]; 1710ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1720ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong Problem problem; 1730ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong problem.AddParameterBlock(x, 3); 1740ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong problem.AddParameterBlock(y, 4); 1750ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong problem.AddParameterBlock(z, 5); 1760ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1770ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // The cost function expects the size of the second parameter, z, to be 4 1780ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // instead of 5 as declared above. This is fatal. 1790ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong EXPECT_DEATH_IF_SUPPORTED(problem.AddResidualBlock( 1800ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong new BinaryCostFunction(2, 3, 4), NULL, x, z), 1810ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong "different block sizes"); 1820ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 1830ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1840ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus KongTEST(Problem, AddResidualAddsDuplicatedParametersOnlyOnce) { 1850ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double x[3], y[4], z[5]; 1860ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1870ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong Problem problem; 1880ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong problem.AddResidualBlock(new UnaryCostFunction(2, 3), NULL, x); 1890ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong problem.AddResidualBlock(new UnaryCostFunction(2, 3), NULL, x); 1900ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong problem.AddResidualBlock(new UnaryCostFunction(2, 4), NULL, y); 1910ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong problem.AddResidualBlock(new UnaryCostFunction(2, 5), NULL, z); 1920ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1930ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong EXPECT_EQ(3, problem.NumParameterBlocks()); 1940ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong EXPECT_EQ(12, problem.NumParameters()); 1950ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 1960ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1970ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus KongTEST(Problem, AddParameterWithDifferentSizesOnTheSameVariableDies) { 1980ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double x[3], y[4]; 1990ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2000ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong Problem problem; 2010ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong problem.AddParameterBlock(x, 3); 2020ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong problem.AddParameterBlock(y, 4); 2030ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2040ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong EXPECT_DEATH_IF_SUPPORTED(problem.AddParameterBlock(x, 4), 2050ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong "different block sizes"); 2060ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 2070ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2080ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongstatic double *IntToPtr(int i) { 2090ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return reinterpret_cast<double*>(sizeof(double) * i); // NOLINT 2100ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 2110ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2120ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus KongTEST(Problem, AddParameterWithAliasedParametersDies) { 2130ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Layout is 2140ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // 2150ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // 0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 2160ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // [x] x x x x [y] y y 2170ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // o==o==o o==o==o o==o 2180ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // o--o--o o--o--o o--o o--o--o 2190ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // 2200ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Parameter block additions are tested as listed above; expected successful 2210ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // ones marked with o==o and aliasing ones marked with o--o. 2220ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2230ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong Problem problem; 2240ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong problem.AddParameterBlock(IntToPtr(5), 5); // x 2250ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong problem.AddParameterBlock(IntToPtr(13), 3); // y 2260ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2270ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong EXPECT_DEATH_IF_SUPPORTED(problem.AddParameterBlock(IntToPtr( 4), 2), 2280ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong "Aliasing detected"); 2290ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong EXPECT_DEATH_IF_SUPPORTED(problem.AddParameterBlock(IntToPtr( 4), 3), 2300ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong "Aliasing detected"); 2310ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong EXPECT_DEATH_IF_SUPPORTED(problem.AddParameterBlock(IntToPtr( 4), 9), 2320ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong "Aliasing detected"); 2330ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong EXPECT_DEATH_IF_SUPPORTED(problem.AddParameterBlock(IntToPtr( 8), 3), 2340ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong "Aliasing detected"); 2350ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong EXPECT_DEATH_IF_SUPPORTED(problem.AddParameterBlock(IntToPtr(12), 2), 2360ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong "Aliasing detected"); 2370ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong EXPECT_DEATH_IF_SUPPORTED(problem.AddParameterBlock(IntToPtr(14), 3), 2380ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong "Aliasing detected"); 2390ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2400ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // These ones should work. 2410ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong problem.AddParameterBlock(IntToPtr( 2), 3); 2420ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong problem.AddParameterBlock(IntToPtr(10), 3); 2430ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong problem.AddParameterBlock(IntToPtr(16), 2); 2440ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2450ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong ASSERT_EQ(5, problem.NumParameterBlocks()); 2460ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 2470ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2480ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus KongTEST(Problem, AddParameterIgnoresDuplicateCalls) { 2490ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double x[3], y[4]; 2500ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2510ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong Problem problem; 2520ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong problem.AddParameterBlock(x, 3); 2530ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong problem.AddParameterBlock(y, 4); 2540ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2550ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Creating parameter blocks multiple times is ignored. 2560ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong problem.AddParameterBlock(x, 3); 2570ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong problem.AddResidualBlock(new UnaryCostFunction(2, 3), NULL, x); 2580ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2590ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // ... even repeatedly. 2600ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong problem.AddParameterBlock(x, 3); 2610ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong problem.AddResidualBlock(new UnaryCostFunction(2, 3), NULL, x); 2620ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2630ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // More parameters are fine. 2640ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong problem.AddParameterBlock(y, 4); 2650ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong problem.AddResidualBlock(new UnaryCostFunction(2, 4), NULL, y); 2660ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2670ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong EXPECT_EQ(2, problem.NumParameterBlocks()); 2680ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong EXPECT_EQ(7, problem.NumParameters()); 2690ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 2700ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2710ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus KongTEST(Problem, AddingParametersAndResidualsResultsInExpectedProblem) { 2720ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double x[3], y[4], z[5], w[4]; 2730ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2740ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong Problem problem; 2750ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong problem.AddParameterBlock(x, 3); 2760ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong EXPECT_EQ(1, problem.NumParameterBlocks()); 2770ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong EXPECT_EQ(3, problem.NumParameters()); 2780ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2790ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong problem.AddParameterBlock(y, 4); 2800ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong EXPECT_EQ(2, problem.NumParameterBlocks()); 2810ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong EXPECT_EQ(7, problem.NumParameters()); 2820ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2830ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong problem.AddParameterBlock(z, 5); 2840ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong EXPECT_EQ(3, problem.NumParameterBlocks()); 2850ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong EXPECT_EQ(12, problem.NumParameters()); 2860ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2870ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Add a parameter that has a local parameterization. 2880ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong w[0] = 1.0; w[1] = 0.0; w[2] = 0.0; w[3] = 0.0; 2890ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong problem.AddParameterBlock(w, 4, new QuaternionParameterization); 2900ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong EXPECT_EQ(4, problem.NumParameterBlocks()); 2910ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong EXPECT_EQ(16, problem.NumParameters()); 2920ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2930ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong problem.AddResidualBlock(new UnaryCostFunction(2, 3), NULL, x); 2940ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong problem.AddResidualBlock(new BinaryCostFunction(6, 5, 4) , NULL, z, y); 2950ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong problem.AddResidualBlock(new BinaryCostFunction(3, 3, 5), NULL, x, z); 2960ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong problem.AddResidualBlock(new BinaryCostFunction(7, 5, 3), NULL, z, x); 2970ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong problem.AddResidualBlock(new TernaryCostFunction(1, 5, 3, 4), NULL, z, x, y); 2980ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2990ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const int total_residuals = 2 + 6 + 3 + 7 + 1; 3000ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong EXPECT_EQ(problem.NumResidualBlocks(), 5); 3010ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong EXPECT_EQ(problem.NumResiduals(), total_residuals); 3020ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 3030ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 3040ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongclass DestructorCountingCostFunction : public SizedCostFunction<3, 4, 5> { 3050ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong public: 3061d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling explicit DestructorCountingCostFunction(int *num_destructions) 3071d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling : num_destructions_(num_destructions) {} 3080ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 3090ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong virtual ~DestructorCountingCostFunction() { 3101d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling *num_destructions_ += 1; 3110ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 3120ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 3130ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong virtual bool Evaluate(double const* const* parameters, 3140ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double* residuals, 3150ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double** jacobians) const { 3160ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return true; 3170ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 3180ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 3190ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong private: 3201d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling int* num_destructions_; 3210ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong}; 3220ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 3230ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus KongTEST(Problem, ReusedCostFunctionsAreOnlyDeletedOnce) { 3240ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double y[4], z[5]; 3251d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling int num_destructions = 0; 3260ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 3270ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Add a cost function multiple times and check to make sure that 3280ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // the destructor on the cost function is only called once. 3290ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong { 3300ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong Problem problem; 3310ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong problem.AddParameterBlock(y, 4); 3320ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong problem.AddParameterBlock(z, 5); 3330ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 3341d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling CostFunction* cost = new DestructorCountingCostFunction(&num_destructions); 3350ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong problem.AddResidualBlock(cost, NULL, y, z); 3360ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong problem.AddResidualBlock(cost, NULL, y, z); 3370ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong problem.AddResidualBlock(cost, NULL, y, z); 3381d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_EQ(3, problem.NumResidualBlocks()); 3390ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 3400ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 3410ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Check that the destructor was called only once. 3421d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling CHECK_EQ(num_destructions, 1); 3431d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling} 3441d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 3451d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha HaeberlingTEST(Problem, CostFunctionsAreDeletedEvenWithRemovals) { 3461d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling double y[4], z[5], w[4]; 3471d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling int num_destructions = 0; 3481d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling { 3491d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling Problem problem; 3501d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling problem.AddParameterBlock(y, 4); 3511d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling problem.AddParameterBlock(z, 5); 3521d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 3531d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling CostFunction* cost_yz = 3541d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling new DestructorCountingCostFunction(&num_destructions); 3551d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling CostFunction* cost_wz = 3561d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling new DestructorCountingCostFunction(&num_destructions); 3571d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ResidualBlock* r_yz = problem.AddResidualBlock(cost_yz, NULL, y, z); 3581d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ResidualBlock* r_wz = problem.AddResidualBlock(cost_wz, NULL, w, z); 3591d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_EQ(2, problem.NumResidualBlocks()); 3601d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 3611d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // In the current implementation, the destructor shouldn't get run yet. 3621d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling problem.RemoveResidualBlock(r_yz); 3631d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling CHECK_EQ(num_destructions, 0); 3641d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling problem.RemoveResidualBlock(r_wz); 3651d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling CHECK_EQ(num_destructions, 0); 3661d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 3671d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_EQ(0, problem.NumResidualBlocks()); 3681d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 3691d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling CHECK_EQ(num_destructions, 2); 3701d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling} 3711d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 3721d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// Make the dynamic problem tests (e.g. for removing residual blocks) 3731d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// parameterized on whether the low-latency mode is enabled or not. 3741d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// 3751d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// This tests against ProblemImpl instead of Problem in order to inspect the 3761d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// state of the resulting Program; this is difficult with only the thin Problem 3771d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// interface. 3781d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingstruct DynamicProblem : public ::testing::TestWithParam<bool> { 3791d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling DynamicProblem() { 3801d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling Problem::Options options; 38179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez options.enable_fast_removal = GetParam(); 3821d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling problem.reset(new ProblemImpl(options)); 3831d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 3841d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 3851d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ParameterBlock* GetParameterBlock(int block) { 3861d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling return problem->program().parameter_blocks()[block]; 3871d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 3881d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ResidualBlock* GetResidualBlock(int block) { 3891d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling return problem->program().residual_blocks()[block]; 3901d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 3911d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 3921d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling bool HasResidualBlock(ResidualBlock* residual_block) { 39379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez bool have_residual_block = true; 39479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez if (GetParam()) { 39579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez have_residual_block &= 39679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez (problem->residual_block_set().find(residual_block) != 39779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez problem->residual_block_set().end()); 39879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez } 39979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez have_residual_block &= 40079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez find(problem->program().residual_blocks().begin(), 40179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez problem->program().residual_blocks().end(), 40279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez residual_block) != problem->program().residual_blocks().end(); 40379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez return have_residual_block; 40479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez } 40579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 40679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez int NumResidualBlocks() { 40779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // Verify that the hash set of residuals is maintained consistently. 40879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez if (GetParam()) { 40979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez EXPECT_EQ(problem->residual_block_set().size(), 41079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez problem->NumResidualBlocks()); 41179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez } 41279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez return problem->NumResidualBlocks(); 4131d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 4141d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 4151d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // The next block of functions until the end are only for testing the 4161d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // residual block removals. 4171d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling void ExpectParameterBlockContainsResidualBlock( 4181d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling double* values, 4191d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ResidualBlock* residual_block) { 4201d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ParameterBlock* parameter_block = 4211d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling FindOrDie(problem->parameter_map(), values); 4221d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_TRUE(ContainsKey(*(parameter_block->mutable_residual_blocks()), 4231d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling residual_block)); 4241d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 4251d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 4261d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling void ExpectSize(double* values, int size) { 4271d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ParameterBlock* parameter_block = 4281d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling FindOrDie(problem->parameter_map(), values); 4291d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_EQ(size, parameter_block->mutable_residual_blocks()->size()); 4301d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 4311d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 4321d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Degenerate case. 4331d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling void ExpectParameterBlockContains(double* values) { 4341d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ExpectSize(values, 0); 4351d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 4361d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 4371d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling void ExpectParameterBlockContains(double* values, 4381d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ResidualBlock* r1) { 4391d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ExpectSize(values, 1); 4401d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ExpectParameterBlockContainsResidualBlock(values, r1); 4411d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 4421d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 4431d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling void ExpectParameterBlockContains(double* values, 4441d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ResidualBlock* r1, 4451d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ResidualBlock* r2) { 4461d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ExpectSize(values, 2); 4471d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ExpectParameterBlockContainsResidualBlock(values, r1); 4481d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ExpectParameterBlockContainsResidualBlock(values, r2); 4491d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 4501d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 4511d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling void ExpectParameterBlockContains(double* values, 4521d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ResidualBlock* r1, 4531d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ResidualBlock* r2, 4541d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ResidualBlock* r3) { 4551d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ExpectSize(values, 3); 4561d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ExpectParameterBlockContainsResidualBlock(values, r1); 4571d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ExpectParameterBlockContainsResidualBlock(values, r2); 4581d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ExpectParameterBlockContainsResidualBlock(values, r3); 4591d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 4601d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 4611d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling void ExpectParameterBlockContains(double* values, 4621d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ResidualBlock* r1, 4631d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ResidualBlock* r2, 4641d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ResidualBlock* r3, 4651d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ResidualBlock* r4) { 4661d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ExpectSize(values, 4); 4671d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ExpectParameterBlockContainsResidualBlock(values, r1); 4681d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ExpectParameterBlockContainsResidualBlock(values, r2); 4691d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ExpectParameterBlockContainsResidualBlock(values, r3); 4701d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ExpectParameterBlockContainsResidualBlock(values, r4); 4711d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 4721d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 4731d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling scoped_ptr<ProblemImpl> problem; 4741d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling double y[4], z[5], w[3]; 4751d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling}; 4761d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 4771d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha HaeberlingTEST(Problem, SetParameterBlockConstantWithUnknownPtrDies) { 4781d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling double x[3]; 4791d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling double y[2]; 4801d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 4811d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling Problem problem; 4821d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling problem.AddParameterBlock(x, 3); 4831d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 4841d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_DEATH_IF_SUPPORTED(problem.SetParameterBlockConstant(y), 4851d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling "Parameter block not found:"); 4861d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling} 4871d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 4881d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha HaeberlingTEST(Problem, SetParameterBlockVariableWithUnknownPtrDies) { 4891d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling double x[3]; 4901d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling double y[2]; 4911d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 4921d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling Problem problem; 4931d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling problem.AddParameterBlock(x, 3); 4941d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 4951d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_DEATH_IF_SUPPORTED(problem.SetParameterBlockVariable(y), 4961d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling "Parameter block not found:"); 4971d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling} 4981d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 4991d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha HaeberlingTEST(Problem, SetLocalParameterizationWithUnknownPtrDies) { 5001d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling double x[3]; 5011d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling double y[2]; 5021d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 5031d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling Problem problem; 5041d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling problem.AddParameterBlock(x, 3); 5051d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 5061d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_DEATH_IF_SUPPORTED( 5071d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling problem.SetParameterization(y, new IdentityParameterization(3)), 5081d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling "Parameter block not found:"); 5091d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling} 5101d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 5111d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha HaeberlingTEST(Problem, RemoveParameterBlockWithUnknownPtrDies) { 5121d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling double x[3]; 5131d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling double y[2]; 5141d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 5151d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling Problem problem; 5161d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling problem.AddParameterBlock(x, 3); 5171d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 5181d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_DEATH_IF_SUPPORTED( 5191d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling problem.RemoveParameterBlock(y), "Parameter block not found:"); 5201d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling} 5211d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 52279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos HernandezTEST(Problem, GetParameterization) { 52379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez double x[3]; 52479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez double y[2]; 52579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 52679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez Problem problem; 52779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez problem.AddParameterBlock(x, 3); 52879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez problem.AddParameterBlock(y, 2); 52979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 53079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez LocalParameterization* parameterization = new IdentityParameterization(3); 53179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez problem.SetParameterization(x, parameterization); 53279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez EXPECT_EQ(problem.GetParameterization(x), parameterization); 53379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez EXPECT_TRUE(problem.GetParameterization(y) == NULL); 53479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez} 53579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 5361d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha HaeberlingTEST(Problem, ParameterBlockQueryTest) { 5371d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling double x[3]; 5381d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling double y[4]; 5391d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling Problem problem; 5401d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling problem.AddParameterBlock(x, 3); 5411d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling problem.AddParameterBlock(y, 4); 5421d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 5431d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling vector<int> constant_parameters; 5441d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling constant_parameters.push_back(0); 5451d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling problem.SetParameterization( 5461d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling x, 5471d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling new SubsetParameterization(3, constant_parameters)); 5481d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_EQ(problem.ParameterBlockSize(x), 3); 5491d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_EQ(problem.ParameterBlockLocalSize(x), 2); 5501d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_EQ(problem.ParameterBlockLocalSize(y), 4); 5511d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 5521d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling vector<double*> parameter_blocks; 5531d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling problem.GetParameterBlocks(¶meter_blocks); 5541d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_EQ(parameter_blocks.size(), 2); 5551d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_NE(parameter_blocks[0], parameter_blocks[1]); 5561d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_TRUE(parameter_blocks[0] == x || parameter_blocks[0] == y); 5571d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_TRUE(parameter_blocks[1] == x || parameter_blocks[1] == y); 5581d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 55979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez EXPECT_TRUE(problem.HasParameterBlock(x)); 5601d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling problem.RemoveParameterBlock(x); 56179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez EXPECT_FALSE(problem.HasParameterBlock(x)); 5621d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling problem.GetParameterBlocks(¶meter_blocks); 5631d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_EQ(parameter_blocks.size(), 1); 5641d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_TRUE(parameter_blocks[0] == y); 5651d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling} 5661d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 5671d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha HaeberlingTEST_P(DynamicProblem, RemoveParameterBlockWithNoResiduals) { 5681d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling problem->AddParameterBlock(y, 4); 5691d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling problem->AddParameterBlock(z, 5); 5701d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling problem->AddParameterBlock(w, 3); 5711d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ASSERT_EQ(3, problem->NumParameterBlocks()); 57279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez ASSERT_EQ(0, NumResidualBlocks()); 5731d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_EQ(y, GetParameterBlock(0)->user_state()); 5741d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_EQ(z, GetParameterBlock(1)->user_state()); 5751d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_EQ(w, GetParameterBlock(2)->user_state()); 5761d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 5771d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // w is at the end, which might break the swapping logic so try adding and 5781d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // removing it. 5791d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling problem->RemoveParameterBlock(w); 5801d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ASSERT_EQ(2, problem->NumParameterBlocks()); 58179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez ASSERT_EQ(0, NumResidualBlocks()); 5821d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_EQ(y, GetParameterBlock(0)->user_state()); 5831d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_EQ(z, GetParameterBlock(1)->user_state()); 5841d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling problem->AddParameterBlock(w, 3); 5851d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ASSERT_EQ(3, problem->NumParameterBlocks()); 58679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez ASSERT_EQ(0, NumResidualBlocks()); 5871d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_EQ(y, GetParameterBlock(0)->user_state()); 5881d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_EQ(z, GetParameterBlock(1)->user_state()); 5891d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_EQ(w, GetParameterBlock(2)->user_state()); 5901d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 5911d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Now remove z, which is in the middle, and add it back. 5921d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling problem->RemoveParameterBlock(z); 5931d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ASSERT_EQ(2, problem->NumParameterBlocks()); 59479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez ASSERT_EQ(0, NumResidualBlocks()); 5951d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_EQ(y, GetParameterBlock(0)->user_state()); 5961d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_EQ(w, GetParameterBlock(1)->user_state()); 5971d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling problem->AddParameterBlock(z, 5); 5981d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ASSERT_EQ(3, problem->NumParameterBlocks()); 59979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez ASSERT_EQ(0, NumResidualBlocks()); 6001d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_EQ(y, GetParameterBlock(0)->user_state()); 6011d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_EQ(w, GetParameterBlock(1)->user_state()); 6021d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_EQ(z, GetParameterBlock(2)->user_state()); 6031d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 6041d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Now remove everything. 6051d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // y 6061d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling problem->RemoveParameterBlock(y); 6071d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ASSERT_EQ(2, problem->NumParameterBlocks()); 60879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez ASSERT_EQ(0, NumResidualBlocks()); 6091d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_EQ(z, GetParameterBlock(0)->user_state()); 6101d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_EQ(w, GetParameterBlock(1)->user_state()); 6111d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 6121d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // z 6131d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling problem->RemoveParameterBlock(z); 6141d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ASSERT_EQ(1, problem->NumParameterBlocks()); 61579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez ASSERT_EQ(0, NumResidualBlocks()); 6161d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_EQ(w, GetParameterBlock(0)->user_state()); 6171d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 6181d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // w 6191d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling problem->RemoveParameterBlock(w); 6201d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_EQ(0, problem->NumParameterBlocks()); 62179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez EXPECT_EQ(0, NumResidualBlocks()); 6221d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling} 6231d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 6241d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha HaeberlingTEST_P(DynamicProblem, RemoveParameterBlockWithResiduals) { 6251d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling problem->AddParameterBlock(y, 4); 6261d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling problem->AddParameterBlock(z, 5); 6271d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling problem->AddParameterBlock(w, 3); 6281d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ASSERT_EQ(3, problem->NumParameterBlocks()); 62979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez ASSERT_EQ(0, NumResidualBlocks()); 6301d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_EQ(y, GetParameterBlock(0)->user_state()); 6311d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_EQ(z, GetParameterBlock(1)->user_state()); 6321d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_EQ(w, GetParameterBlock(2)->user_state()); 6331d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 6341d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Add all combinations of cost functions. 6351d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling CostFunction* cost_yzw = new TernaryCostFunction(1, 4, 5, 3); 6361d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling CostFunction* cost_yz = new BinaryCostFunction (1, 4, 5); 6371d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling CostFunction* cost_yw = new BinaryCostFunction (1, 4, 3); 6381d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling CostFunction* cost_zw = new BinaryCostFunction (1, 5, 3); 6391d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling CostFunction* cost_y = new UnaryCostFunction (1, 4); 6401d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling CostFunction* cost_z = new UnaryCostFunction (1, 5); 6411d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling CostFunction* cost_w = new UnaryCostFunction (1, 3); 6421d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 6431d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ResidualBlock* r_yzw = problem->AddResidualBlock(cost_yzw, NULL, y, z, w); 6441d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ResidualBlock* r_yz = problem->AddResidualBlock(cost_yz, NULL, y, z); 6451d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ResidualBlock* r_yw = problem->AddResidualBlock(cost_yw, NULL, y, w); 6461d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ResidualBlock* r_zw = problem->AddResidualBlock(cost_zw, NULL, z, w); 6471d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ResidualBlock* r_y = problem->AddResidualBlock(cost_y, NULL, y); 6481d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ResidualBlock* r_z = problem->AddResidualBlock(cost_z, NULL, z); 6491d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ResidualBlock* r_w = problem->AddResidualBlock(cost_w, NULL, w); 6501d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 6511d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_EQ(3, problem->NumParameterBlocks()); 65279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez EXPECT_EQ(7, NumResidualBlocks()); 6531d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 6541d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Remove w, which should remove r_yzw, r_yw, r_zw, r_w. 6551d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling problem->RemoveParameterBlock(w); 6561d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ASSERT_EQ(2, problem->NumParameterBlocks()); 65779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez ASSERT_EQ(3, NumResidualBlocks()); 6581d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 6591d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ASSERT_FALSE(HasResidualBlock(r_yzw)); 6601d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ASSERT_TRUE (HasResidualBlock(r_yz )); 6611d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ASSERT_FALSE(HasResidualBlock(r_yw )); 6621d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ASSERT_FALSE(HasResidualBlock(r_zw )); 6631d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ASSERT_TRUE (HasResidualBlock(r_y )); 6641d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ASSERT_TRUE (HasResidualBlock(r_z )); 6651d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ASSERT_FALSE(HasResidualBlock(r_w )); 6661d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 6671d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Remove z, which will remove almost everything else. 6681d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling problem->RemoveParameterBlock(z); 6691d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ASSERT_EQ(1, problem->NumParameterBlocks()); 67079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez ASSERT_EQ(1, NumResidualBlocks()); 6711d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 6721d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ASSERT_FALSE(HasResidualBlock(r_yzw)); 6731d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ASSERT_FALSE(HasResidualBlock(r_yz )); 6741d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ASSERT_FALSE(HasResidualBlock(r_yw )); 6751d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ASSERT_FALSE(HasResidualBlock(r_zw )); 6761d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ASSERT_TRUE (HasResidualBlock(r_y )); 6771d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ASSERT_FALSE(HasResidualBlock(r_z )); 6781d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ASSERT_FALSE(HasResidualBlock(r_w )); 6791d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 6801d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Remove y; all gone. 6811d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling problem->RemoveParameterBlock(y); 6821d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_EQ(0, problem->NumParameterBlocks()); 68379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez EXPECT_EQ(0, NumResidualBlocks()); 6841d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling} 6851d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 6861d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha HaeberlingTEST_P(DynamicProblem, RemoveResidualBlock) { 6871d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling problem->AddParameterBlock(y, 4); 6881d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling problem->AddParameterBlock(z, 5); 6891d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling problem->AddParameterBlock(w, 3); 6901d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 6911d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Add all combinations of cost functions. 6921d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling CostFunction* cost_yzw = new TernaryCostFunction(1, 4, 5, 3); 6931d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling CostFunction* cost_yz = new BinaryCostFunction (1, 4, 5); 6941d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling CostFunction* cost_yw = new BinaryCostFunction (1, 4, 3); 6951d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling CostFunction* cost_zw = new BinaryCostFunction (1, 5, 3); 6961d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling CostFunction* cost_y = new UnaryCostFunction (1, 4); 6971d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling CostFunction* cost_z = new UnaryCostFunction (1, 5); 6981d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling CostFunction* cost_w = new UnaryCostFunction (1, 3); 6991d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 7001d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ResidualBlock* r_yzw = problem->AddResidualBlock(cost_yzw, NULL, y, z, w); 7011d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ResidualBlock* r_yz = problem->AddResidualBlock(cost_yz, NULL, y, z); 7021d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ResidualBlock* r_yw = problem->AddResidualBlock(cost_yw, NULL, y, w); 7031d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ResidualBlock* r_zw = problem->AddResidualBlock(cost_zw, NULL, z, w); 7041d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ResidualBlock* r_y = problem->AddResidualBlock(cost_y, NULL, y); 7051d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ResidualBlock* r_z = problem->AddResidualBlock(cost_z, NULL, z); 7061d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ResidualBlock* r_w = problem->AddResidualBlock(cost_w, NULL, w); 7071d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 7081d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling if (GetParam()) { 7091d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // In this test parameterization, there should be back-pointers from the 7101d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // parameter blocks to the residual blocks. 7111d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ExpectParameterBlockContains(y, r_yzw, r_yz, r_yw, r_y); 7121d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ExpectParameterBlockContains(z, r_yzw, r_yz, r_zw, r_z); 7131d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ExpectParameterBlockContains(w, r_yzw, r_yw, r_zw, r_w); 7141d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } else { 7151d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Otherwise, nothing. 7161d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_TRUE(GetParameterBlock(0)->mutable_residual_blocks() == NULL); 7171d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_TRUE(GetParameterBlock(1)->mutable_residual_blocks() == NULL); 7181d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_TRUE(GetParameterBlock(2)->mutable_residual_blocks() == NULL); 7191d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 7201d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_EQ(3, problem->NumParameterBlocks()); 72179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez EXPECT_EQ(7, NumResidualBlocks()); 7221d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 7231d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Remove each residual and check the state after each removal. 7241d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 7251d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Remove r_yzw. 7261d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling problem->RemoveResidualBlock(r_yzw); 7271d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ASSERT_EQ(3, problem->NumParameterBlocks()); 72879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez ASSERT_EQ(6, NumResidualBlocks()); 7291d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling if (GetParam()) { 7301d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ExpectParameterBlockContains(y, r_yz, r_yw, r_y); 7311d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ExpectParameterBlockContains(z, r_yz, r_zw, r_z); 7321d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ExpectParameterBlockContains(w, r_yw, r_zw, r_w); 7331d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 7341d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ASSERT_TRUE (HasResidualBlock(r_yz )); 7351d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ASSERT_TRUE (HasResidualBlock(r_yw )); 7361d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ASSERT_TRUE (HasResidualBlock(r_zw )); 7371d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ASSERT_TRUE (HasResidualBlock(r_y )); 7381d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ASSERT_TRUE (HasResidualBlock(r_z )); 7391d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ASSERT_TRUE (HasResidualBlock(r_w )); 7401d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 7411d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Remove r_yw. 7421d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling problem->RemoveResidualBlock(r_yw); 7431d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ASSERT_EQ(3, problem->NumParameterBlocks()); 74479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez ASSERT_EQ(5, NumResidualBlocks()); 7451d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling if (GetParam()) { 7461d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ExpectParameterBlockContains(y, r_yz, r_y); 7471d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ExpectParameterBlockContains(z, r_yz, r_zw, r_z); 7481d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ExpectParameterBlockContains(w, r_zw, r_w); 7491d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 7501d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ASSERT_TRUE (HasResidualBlock(r_yz )); 7511d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ASSERT_TRUE (HasResidualBlock(r_zw )); 7521d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ASSERT_TRUE (HasResidualBlock(r_y )); 7531d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ASSERT_TRUE (HasResidualBlock(r_z )); 7541d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ASSERT_TRUE (HasResidualBlock(r_w )); 7551d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 7561d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Remove r_zw. 7571d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling problem->RemoveResidualBlock(r_zw); 7581d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ASSERT_EQ(3, problem->NumParameterBlocks()); 75979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez ASSERT_EQ(4, NumResidualBlocks()); 7601d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling if (GetParam()) { 7611d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ExpectParameterBlockContains(y, r_yz, r_y); 7621d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ExpectParameterBlockContains(z, r_yz, r_z); 7631d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ExpectParameterBlockContains(w, r_w); 7641d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 7651d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ASSERT_TRUE (HasResidualBlock(r_yz )); 7661d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ASSERT_TRUE (HasResidualBlock(r_y )); 7671d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ASSERT_TRUE (HasResidualBlock(r_z )); 7681d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ASSERT_TRUE (HasResidualBlock(r_w )); 7691d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 7701d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Remove r_w. 7711d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling problem->RemoveResidualBlock(r_w); 7721d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ASSERT_EQ(3, problem->NumParameterBlocks()); 77379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez ASSERT_EQ(3, NumResidualBlocks()); 7741d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling if (GetParam()) { 7751d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ExpectParameterBlockContains(y, r_yz, r_y); 7761d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ExpectParameterBlockContains(z, r_yz, r_z); 7771d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ExpectParameterBlockContains(w); 7781d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 7791d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ASSERT_TRUE (HasResidualBlock(r_yz )); 7801d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ASSERT_TRUE (HasResidualBlock(r_y )); 7811d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ASSERT_TRUE (HasResidualBlock(r_z )); 7821d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 7831d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Remove r_yz. 7841d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling problem->RemoveResidualBlock(r_yz); 7851d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ASSERT_EQ(3, problem->NumParameterBlocks()); 78679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez ASSERT_EQ(2, NumResidualBlocks()); 7871d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling if (GetParam()) { 7881d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ExpectParameterBlockContains(y, r_y); 7891d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ExpectParameterBlockContains(z, r_z); 7901d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ExpectParameterBlockContains(w); 7911d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 7921d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ASSERT_TRUE (HasResidualBlock(r_y )); 7931d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ASSERT_TRUE (HasResidualBlock(r_z )); 7941d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 7951d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Remove the last two. 7961d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling problem->RemoveResidualBlock(r_z); 7971d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling problem->RemoveResidualBlock(r_y); 7981d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ASSERT_EQ(3, problem->NumParameterBlocks()); 79979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez ASSERT_EQ(0, NumResidualBlocks()); 8001d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling if (GetParam()) { 8011d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ExpectParameterBlockContains(y); 8021d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ExpectParameterBlockContains(z); 8031d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ExpectParameterBlockContains(w); 8041d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 8051d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling} 8061d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 80779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos HernandezTEST_P(DynamicProblem, RemoveInvalidResidualBlockDies) { 80879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez problem->AddParameterBlock(y, 4); 80979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez problem->AddParameterBlock(z, 5); 81079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez problem->AddParameterBlock(w, 3); 81179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 81279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // Add all combinations of cost functions. 81379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez CostFunction* cost_yzw = new TernaryCostFunction(1, 4, 5, 3); 81479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez CostFunction* cost_yz = new BinaryCostFunction (1, 4, 5); 81579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez CostFunction* cost_yw = new BinaryCostFunction (1, 4, 3); 81679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez CostFunction* cost_zw = new BinaryCostFunction (1, 5, 3); 81779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez CostFunction* cost_y = new UnaryCostFunction (1, 4); 81879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez CostFunction* cost_z = new UnaryCostFunction (1, 5); 81979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez CostFunction* cost_w = new UnaryCostFunction (1, 3); 82079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 82179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez ResidualBlock* r_yzw = problem->AddResidualBlock(cost_yzw, NULL, y, z, w); 82279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez ResidualBlock* r_yz = problem->AddResidualBlock(cost_yz, NULL, y, z); 82379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez ResidualBlock* r_yw = problem->AddResidualBlock(cost_yw, NULL, y, w); 82479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez ResidualBlock* r_zw = problem->AddResidualBlock(cost_zw, NULL, z, w); 82579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez ResidualBlock* r_y = problem->AddResidualBlock(cost_y, NULL, y); 82679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez ResidualBlock* r_z = problem->AddResidualBlock(cost_z, NULL, z); 82779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez ResidualBlock* r_w = problem->AddResidualBlock(cost_w, NULL, w); 82879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 82979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // Remove r_yzw. 83079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez problem->RemoveResidualBlock(r_yzw); 83179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez ASSERT_EQ(3, problem->NumParameterBlocks()); 83279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez ASSERT_EQ(6, NumResidualBlocks()); 83379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // Attempt to remove r_yzw again. 83479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez EXPECT_DEATH_IF_SUPPORTED(problem->RemoveResidualBlock(r_yzw), "not found"); 83579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 83679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // Attempt to remove a cast pointer never added as a residual. 83779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez int trash_memory = 1234; 83879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez ResidualBlock* invalid_residual = 83979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez reinterpret_cast<ResidualBlock*>(&trash_memory); 84079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez EXPECT_DEATH_IF_SUPPORTED(problem->RemoveResidualBlock(invalid_residual), 84179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez "not found"); 84279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 84379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // Remove a parameter block, which in turn removes the dependent residuals 84479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // then attempt to remove them directly. 84579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez problem->RemoveParameterBlock(z); 84679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez ASSERT_EQ(2, problem->NumParameterBlocks()); 84779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez ASSERT_EQ(3, NumResidualBlocks()); 84879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez EXPECT_DEATH_IF_SUPPORTED(problem->RemoveResidualBlock(r_yz), "not found"); 84979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez EXPECT_DEATH_IF_SUPPORTED(problem->RemoveResidualBlock(r_zw), "not found"); 85079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez EXPECT_DEATH_IF_SUPPORTED(problem->RemoveResidualBlock(r_z), "not found"); 85179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 85279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez problem->RemoveResidualBlock(r_yw); 85379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez problem->RemoveResidualBlock(r_w); 85479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez problem->RemoveResidualBlock(r_y); 85579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez} 85679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 85779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// Check that a null-terminated array, a, has the same elements as b. 85879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandeztemplate<typename T> 85979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandezvoid ExpectVectorContainsUnordered(const T* a, const vector<T>& b) { 86079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // Compute the size of a. 86179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez int size = 0; 86279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez while (a[size]) { 86379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez ++size; 86479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez } 86579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez ASSERT_EQ(size, b.size()); 86679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 86779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // Sort a. 86879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez vector<T> a_sorted(size); 86979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez copy(a, a + size, a_sorted.begin()); 87079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez sort(a_sorted.begin(), a_sorted.end()); 87179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 87279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // Sort b. 87379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez vector<T> b_sorted(b); 87479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez sort(b_sorted.begin(), b_sorted.end()); 87579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 87679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // Compare. 87779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez for (int i = 0; i < size; ++i) { 87879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez EXPECT_EQ(a_sorted[i], b_sorted[i]); 87979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez } 88079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez} 88179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 88279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandezvoid ExpectProblemHasResidualBlocks( 88379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez const ProblemImpl &problem, 88479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez const ResidualBlockId *expected_residual_blocks) { 88579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez vector<ResidualBlockId> residual_blocks; 88679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez problem.GetResidualBlocks(&residual_blocks); 88779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez ExpectVectorContainsUnordered(expected_residual_blocks, residual_blocks); 88879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez} 88979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 89079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos HernandezTEST_P(DynamicProblem, GetXXXBlocksForYYYBlock) { 89179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez problem->AddParameterBlock(y, 4); 89279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez problem->AddParameterBlock(z, 5); 89379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez problem->AddParameterBlock(w, 3); 89479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 89579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // Add all combinations of cost functions. 89679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez CostFunction* cost_yzw = new TernaryCostFunction(1, 4, 5, 3); 89779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez CostFunction* cost_yz = new BinaryCostFunction (1, 4, 5); 89879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez CostFunction* cost_yw = new BinaryCostFunction (1, 4, 3); 89979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez CostFunction* cost_zw = new BinaryCostFunction (1, 5, 3); 90079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez CostFunction* cost_y = new UnaryCostFunction (1, 4); 90179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez CostFunction* cost_z = new UnaryCostFunction (1, 5); 90279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez CostFunction* cost_w = new UnaryCostFunction (1, 3); 90379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 90479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez ResidualBlock* r_yzw = problem->AddResidualBlock(cost_yzw, NULL, y, z, w); 90579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez { 90679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez ResidualBlockId expected_residuals[] = {r_yzw, 0}; 90779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez ExpectProblemHasResidualBlocks(*problem, expected_residuals); 90879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez } 90979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez ResidualBlock* r_yz = problem->AddResidualBlock(cost_yz, NULL, y, z); 91079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez { 91179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez ResidualBlockId expected_residuals[] = {r_yzw, r_yz, 0}; 91279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez ExpectProblemHasResidualBlocks(*problem, expected_residuals); 91379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez } 91479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez ResidualBlock* r_yw = problem->AddResidualBlock(cost_yw, NULL, y, w); 91579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez { 91679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez ResidualBlock *expected_residuals[] = {r_yzw, r_yz, r_yw, 0}; 91779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez ExpectProblemHasResidualBlocks(*problem, expected_residuals); 91879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez } 91979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez ResidualBlock* r_zw = problem->AddResidualBlock(cost_zw, NULL, z, w); 92079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez { 92179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez ResidualBlock *expected_residuals[] = {r_yzw, r_yz, r_yw, r_zw, 0}; 92279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez ExpectProblemHasResidualBlocks(*problem, expected_residuals); 92379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez } 92479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez ResidualBlock* r_y = problem->AddResidualBlock(cost_y, NULL, y); 92579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez { 92679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez ResidualBlock *expected_residuals[] = {r_yzw, r_yz, r_yw, r_zw, r_y, 0}; 92779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez ExpectProblemHasResidualBlocks(*problem, expected_residuals); 92879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez } 92979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez ResidualBlock* r_z = problem->AddResidualBlock(cost_z, NULL, z); 93079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez { 93179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez ResidualBlock *expected_residuals[] = { 93279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez r_yzw, r_yz, r_yw, r_zw, r_y, r_z, 0 93379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez }; 93479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez ExpectProblemHasResidualBlocks(*problem, expected_residuals); 93579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez } 93679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez ResidualBlock* r_w = problem->AddResidualBlock(cost_w, NULL, w); 93779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez { 93879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez ResidualBlock *expected_residuals[] = { 93979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez r_yzw, r_yz, r_yw, r_zw, r_y, r_z, r_w, 0 94079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez }; 94179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez ExpectProblemHasResidualBlocks(*problem, expected_residuals); 94279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez } 94379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 94479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez vector<double*> parameter_blocks; 94579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez vector<ResidualBlockId> residual_blocks; 94679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 94779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // Check GetResidualBlocksForParameterBlock() for all parameter blocks. 94879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez struct GetResidualBlocksForParameterBlockTestCase { 94979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez double* parameter_block; 95079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez ResidualBlockId expected_residual_blocks[10]; 95179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez }; 95279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez GetResidualBlocksForParameterBlockTestCase get_residual_blocks_cases[] = { 95379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez { y, { r_yzw, r_yz, r_yw, r_y, NULL} }, 95479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez { z, { r_yzw, r_yz, r_zw, r_z, NULL} }, 95579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez { w, { r_yzw, r_yw, r_zw, r_w, NULL} }, 95679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez { NULL } 95779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez }; 95879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez for (int i = 0; get_residual_blocks_cases[i].parameter_block; ++i) { 95979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez problem->GetResidualBlocksForParameterBlock( 96079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez get_residual_blocks_cases[i].parameter_block, 96179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez &residual_blocks); 96279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez ExpectVectorContainsUnordered( 96379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez get_residual_blocks_cases[i].expected_residual_blocks, 96479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez residual_blocks); 96579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez } 96679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 96779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // Check GetParameterBlocksForResidualBlock() for all residual blocks. 96879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez struct GetParameterBlocksForResidualBlockTestCase { 96979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez ResidualBlockId residual_block; 97079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez double* expected_parameter_blocks[10]; 97179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez }; 97279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez GetParameterBlocksForResidualBlockTestCase get_parameter_blocks_cases[] = { 97379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez { r_yzw, { y, z, w, NULL } }, 97479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez { r_yz , { y, z, NULL } }, 97579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez { r_yw , { y, w, NULL } }, 97679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez { r_zw , { z, w, NULL } }, 97779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez { r_y , { y, NULL } }, 97879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez { r_z , { z, NULL } }, 97979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez { r_w , { w, NULL } }, 98079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez { NULL } 98179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez }; 98279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez for (int i = 0; get_parameter_blocks_cases[i].residual_block; ++i) { 98379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez problem->GetParameterBlocksForResidualBlock( 98479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez get_parameter_blocks_cases[i].residual_block, 98579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez ¶meter_blocks); 98679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez ExpectVectorContainsUnordered( 98779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez get_parameter_blocks_cases[i].expected_parameter_blocks, 98879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez parameter_blocks); 98979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez } 99079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez} 99179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 9921d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha HaeberlingINSTANTIATE_TEST_CASE_P(OptionsInstantiation, 9931d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling DynamicProblem, 9941d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ::testing::Values(true, false)); 9951d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 9961d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// Test for Problem::Evaluate 9971d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 9981d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// r_i = i - (j + 1) * x_ij^2 9991d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingtemplate <int kNumResiduals, int kNumParameterBlocks> 10001d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingclass QuadraticCostFunction : public CostFunction { 10011d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling public: 10021d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling QuadraticCostFunction() { 10031d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling CHECK_GT(kNumResiduals, 0); 10041d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling CHECK_GT(kNumParameterBlocks, 0); 10051d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling set_num_residuals(kNumResiduals); 10061d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling for (int i = 0; i < kNumParameterBlocks; ++i) { 10071d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling mutable_parameter_block_sizes()->push_back(kNumResiduals); 10081d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 10091d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 10101d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 10111d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling virtual bool Evaluate(double const* const* parameters, 10121d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling double* residuals, 10131d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling double** jacobians) const { 10141d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling for (int i = 0; i < kNumResiduals; ++i) { 10151d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling residuals[i] = i; 10161d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling for (int j = 0; j < kNumParameterBlocks; ++j) { 10171d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling residuals[i] -= (j + 1.0) * parameters[j][i] * parameters[j][i]; 10181d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 10191d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 10201d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 10211d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling if (jacobians == NULL) { 10221d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling return true; 10231d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 10241d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 10251d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling for (int j = 0; j < kNumParameterBlocks; ++j) { 10261d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling if (jacobians[j] != NULL) { 10271d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling MatrixRef(jacobians[j], kNumResiduals, kNumResiduals) = 10281d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling (-2.0 * (j + 1.0) * 10291d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ConstVectorRef(parameters[j], kNumResiduals)).asDiagonal(); 10301d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 10311d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 10321d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 10331d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling return true; 10341d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 10351d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling}; 10361d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 10371d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// Convert a CRSMatrix to a dense Eigen matrix. 10381d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingvoid CRSToDenseMatrix(const CRSMatrix& input, Matrix* output) { 10391d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling Matrix& m = *CHECK_NOTNULL(output); 10401d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling m.resize(input.num_rows, input.num_cols); 10411d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling m.setZero(); 10421d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling for (int row = 0; row < input.num_rows; ++row) { 10431d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling for (int j = input.rows[row]; j < input.rows[row + 1]; ++j) { 10441d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const int col = input.cols[j]; 10451d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling m(row, col) = input.values[j]; 10461d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 10471d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 10481d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling} 10491d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 10501d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingclass ProblemEvaluateTest : public ::testing::Test { 10511d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling protected: 10521d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling void SetUp() { 10531d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling for (int i = 0; i < 6; ++i) { 10541d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling parameters_[i] = static_cast<double>(i + 1); 10551d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 10561d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 10571d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling parameter_blocks_.push_back(parameters_); 10581d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling parameter_blocks_.push_back(parameters_ + 2); 10591d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling parameter_blocks_.push_back(parameters_ + 4); 10601d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 10611d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 10621d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling CostFunction* cost_function = new QuadraticCostFunction<2, 2>; 10631d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 10641d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // f(x, y) 10651d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling residual_blocks_.push_back( 10661d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling problem_.AddResidualBlock(cost_function, 10671d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling NULL, 10681d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling parameters_, 10691d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling parameters_ + 2)); 10701d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // g(y, z) 10711d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling residual_blocks_.push_back( 10721d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling problem_.AddResidualBlock(cost_function, 10731d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling NULL, parameters_ + 2, 10741d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling parameters_ + 4)); 10751d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // h(z, x) 10761d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling residual_blocks_.push_back( 10771d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling problem_.AddResidualBlock(cost_function, 10781d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling NULL, 10791d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling parameters_ + 4, 10801d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling parameters_)); 10811d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 10821d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 108379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez void TearDown() { 108479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez EXPECT_TRUE(problem_.program().IsValid()); 108579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez } 10861d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 10871d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling void EvaluateAndCompare(const Problem::EvaluateOptions& options, 10881d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const int expected_num_rows, 10891d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const int expected_num_cols, 10901d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const double expected_cost, 10911d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const double* expected_residuals, 10921d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const double* expected_gradient, 10931d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const double* expected_jacobian) { 10941d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling double cost; 10951d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling vector<double> residuals; 10961d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling vector<double> gradient; 10971d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling CRSMatrix jacobian; 10981d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 10991d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_TRUE( 11001d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling problem_.Evaluate(options, 11011d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling &cost, 11021d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling expected_residuals != NULL ? &residuals : NULL, 11031d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling expected_gradient != NULL ? &gradient : NULL, 11041d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling expected_jacobian != NULL ? &jacobian : NULL)); 11051d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 11061d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling if (expected_residuals != NULL) { 11071d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_EQ(residuals.size(), expected_num_rows); 11081d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 11091d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 11101d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling if (expected_gradient != NULL) { 11111d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_EQ(gradient.size(), expected_num_cols); 11121d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 11131d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 11141d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling if (expected_jacobian != NULL) { 11151d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_EQ(jacobian.num_rows, expected_num_rows); 11161d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_EQ(jacobian.num_cols, expected_num_cols); 11171d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 11181d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 11191d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling Matrix dense_jacobian; 11201d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling if (expected_jacobian != NULL) { 11211d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling CRSToDenseMatrix(jacobian, &dense_jacobian); 11221d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 11231d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 11241d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling CompareEvaluations(expected_num_rows, 11251d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling expected_num_cols, 11261d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling expected_cost, 11271d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling expected_residuals, 11281d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling expected_gradient, 11291d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling expected_jacobian, 11301d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling cost, 11311d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling residuals.size() > 0 ? &residuals[0] : NULL, 11321d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling gradient.size() > 0 ? &gradient[0] : NULL, 11331d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling dense_jacobian.data()); 11341d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 11351d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 11361d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling void CheckAllEvaluationCombinations(const Problem::EvaluateOptions& options, 11371d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const ExpectedEvaluation& expected) { 11381d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling for (int i = 0; i < 8; ++i) { 11391d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EvaluateAndCompare(options, 11401d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling expected.num_rows, 11411d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling expected.num_cols, 11421d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling expected.cost, 11431d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling (i & 1) ? expected.residuals : NULL, 11441d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling (i & 2) ? expected.gradient : NULL, 11451d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling (i & 4) ? expected.jacobian : NULL); 11461d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 11471d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 11481d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 11491d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ProblemImpl problem_; 11501d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling double parameters_[6]; 11511d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling vector<double*> parameter_blocks_; 11521d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling vector<ResidualBlockId> residual_blocks_; 11531d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling}; 11541d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 11551d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 11561d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha HaeberlingTEST_F(ProblemEvaluateTest, MultipleParameterAndResidualBlocks) { 11571d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ExpectedEvaluation expected = { 11581d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Rows/columns 11591d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 6, 6, 11601d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Cost 11611d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 7607.0, 11621d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Residuals 11631d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling { -19.0, -35.0, // f 11641d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling -59.0, -87.0, // g 11651d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling -27.0, -43.0 // h 11661d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling }, 11671d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Gradient 11681d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling { 146.0, 484.0, // x 11691d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 582.0, 1256.0, // y 11701d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1450.0, 2604.0, // z 11711d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling }, 11721d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Jacobian 11731d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // x y z 11741d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling { /* f(x, y) */ -2.0, 0.0, -12.0, 0.0, 0.0, 0.0, 11751d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 0.0, -4.0, 0.0, -16.0, 0.0, 0.0, 11761d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling /* g(y, z) */ 0.0, 0.0, -6.0, 0.0, -20.0, 0.0, 11771d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 0.0, 0.0, 0.0, -8.0, 0.0, -24.0, 11781d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling /* h(z, x) */ -4.0, 0.0, 0.0, 0.0, -10.0, 0.0, 11791d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 0.0, -8.0, 0.0, 0.0, 0.0, -12.0 11801d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 11811d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling }; 11821d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 11831d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling CheckAllEvaluationCombinations(Problem::EvaluateOptions(), expected); 11841d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling} 11851d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 11861d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha HaeberlingTEST_F(ProblemEvaluateTest, ParameterAndResidualBlocksPassedInOptions) { 11871d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ExpectedEvaluation expected = { 11881d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Rows/columns 11891d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 6, 6, 11901d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Cost 11911d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 7607.0, 11921d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Residuals 11931d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling { -19.0, -35.0, // f 11941d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling -59.0, -87.0, // g 11951d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling -27.0, -43.0 // h 11961d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling }, 11971d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Gradient 11981d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling { 146.0, 484.0, // x 11991d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 582.0, 1256.0, // y 12001d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1450.0, 2604.0, // z 12011d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling }, 12021d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Jacobian 12031d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // x y z 12041d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling { /* f(x, y) */ -2.0, 0.0, -12.0, 0.0, 0.0, 0.0, 12051d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 0.0, -4.0, 0.0, -16.0, 0.0, 0.0, 12061d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling /* g(y, z) */ 0.0, 0.0, -6.0, 0.0, -20.0, 0.0, 12071d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 0.0, 0.0, 0.0, -8.0, 0.0, -24.0, 12081d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling /* h(z, x) */ -4.0, 0.0, 0.0, 0.0, -10.0, 0.0, 12091d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 0.0, -8.0, 0.0, 0.0, 0.0, -12.0 12101d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 12111d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling }; 12121d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 12131d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling Problem::EvaluateOptions evaluate_options; 12141d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling evaluate_options.parameter_blocks = parameter_blocks_; 12151d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling evaluate_options.residual_blocks = residual_blocks_; 12161d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling CheckAllEvaluationCombinations(evaluate_options, expected); 12171d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling} 12181d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 12191d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha HaeberlingTEST_F(ProblemEvaluateTest, ReorderedResidualBlocks) { 12201d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ExpectedEvaluation expected = { 12211d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Rows/columns 12221d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 6, 6, 12231d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Cost 12241d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 7607.0, 12251d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Residuals 12261d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling { -19.0, -35.0, // f 12271d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling -27.0, -43.0, // h 12281d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling -59.0, -87.0 // g 12291d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling }, 12301d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Gradient 12311d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling { 146.0, 484.0, // x 12321d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 582.0, 1256.0, // y 12331d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1450.0, 2604.0, // z 12341d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling }, 12351d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Jacobian 12361d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // x y z 12371d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling { /* f(x, y) */ -2.0, 0.0, -12.0, 0.0, 0.0, 0.0, 12381d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 0.0, -4.0, 0.0, -16.0, 0.0, 0.0, 12391d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling /* h(z, x) */ -4.0, 0.0, 0.0, 0.0, -10.0, 0.0, 12401d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 0.0, -8.0, 0.0, 0.0, 0.0, -12.0, 12411d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling /* g(y, z) */ 0.0, 0.0, -6.0, 0.0, -20.0, 0.0, 12421d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 0.0, 0.0, 0.0, -8.0, 0.0, -24.0 12431d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 12441d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling }; 12451d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 12461d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling Problem::EvaluateOptions evaluate_options; 12471d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling evaluate_options.parameter_blocks = parameter_blocks_; 12481d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 12491d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // f, h, g 12501d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling evaluate_options.residual_blocks.push_back(residual_blocks_[0]); 12511d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling evaluate_options.residual_blocks.push_back(residual_blocks_[2]); 12521d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling evaluate_options.residual_blocks.push_back(residual_blocks_[1]); 12531d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 12541d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling CheckAllEvaluationCombinations(evaluate_options, expected); 12551d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling} 12561d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 12571d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha HaeberlingTEST_F(ProblemEvaluateTest, ReorderedResidualBlocksAndReorderedParameterBlocks) { 12581d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ExpectedEvaluation expected = { 12591d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Rows/columns 12601d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 6, 6, 12611d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Cost 12621d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 7607.0, 12631d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Residuals 12641d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling { -19.0, -35.0, // f 12651d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling -27.0, -43.0, // h 12661d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling -59.0, -87.0 // g 12671d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling }, 12681d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Gradient 12691d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling { 1450.0, 2604.0, // z 12701d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 582.0, 1256.0, // y 12711d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 146.0, 484.0, // x 12721d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling }, 12731d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Jacobian 12741d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // z y x 12751d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling { /* f(x, y) */ 0.0, 0.0, -12.0, 0.0, -2.0, 0.0, 12761d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 0.0, 0.0, 0.0, -16.0, 0.0, -4.0, 12771d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling /* h(z, x) */ -10.0, 0.0, 0.0, 0.0, -4.0, 0.0, 12781d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 0.0, -12.0, 0.0, 0.0, 0.0, -8.0, 12791d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling /* g(y, z) */ -20.0, 0.0, -6.0, 0.0, 0.0, 0.0, 12801d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 0.0, -24.0, 0.0, -8.0, 0.0, 0.0 12811d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 12821d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling }; 12831d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 12841d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling Problem::EvaluateOptions evaluate_options; 12851d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // z, y, x 12861d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling evaluate_options.parameter_blocks.push_back(parameter_blocks_[2]); 12871d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling evaluate_options.parameter_blocks.push_back(parameter_blocks_[1]); 12881d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling evaluate_options.parameter_blocks.push_back(parameter_blocks_[0]); 12891d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 12901d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // f, h, g 12911d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling evaluate_options.residual_blocks.push_back(residual_blocks_[0]); 12921d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling evaluate_options.residual_blocks.push_back(residual_blocks_[2]); 12931d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling evaluate_options.residual_blocks.push_back(residual_blocks_[1]); 12941d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 12951d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling CheckAllEvaluationCombinations(evaluate_options, expected); 12961d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling} 12971d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 12981d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha HaeberlingTEST_F(ProblemEvaluateTest, ConstantParameterBlock) { 12991d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ExpectedEvaluation expected = { 13001d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Rows/columns 13011d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 6, 6, 13021d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Cost 13031d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 7607.0, 13041d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Residuals 13051d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling { -19.0, -35.0, // f 13061d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling -59.0, -87.0, // g 13071d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling -27.0, -43.0 // h 13081d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling }, 13091d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 13101d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Gradient 13111d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling { 146.0, 484.0, // x 13121d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 0.0, 0.0, // y 13131d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1450.0, 2604.0, // z 13141d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling }, 13151d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 13161d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Jacobian 13171d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // x y z 13181d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling { /* f(x, y) */ -2.0, 0.0, 0.0, 0.0, 0.0, 0.0, 13191d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 0.0, -4.0, 0.0, 0.0, 0.0, 0.0, 13201d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling /* g(y, z) */ 0.0, 0.0, 0.0, 0.0, -20.0, 0.0, 13211d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 0.0, 0.0, 0.0, 0.0, 0.0, -24.0, 13221d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling /* h(z, x) */ -4.0, 0.0, 0.0, 0.0, -10.0, 0.0, 13231d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 0.0, -8.0, 0.0, 0.0, 0.0, -12.0 13241d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 13251d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling }; 13261d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 13271d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling problem_.SetParameterBlockConstant(parameters_ + 2); 13281d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling CheckAllEvaluationCombinations(Problem::EvaluateOptions(), expected); 13291d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling} 13301d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 13311d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha HaeberlingTEST_F(ProblemEvaluateTest, ExcludedAResidualBlock) { 13321d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ExpectedEvaluation expected = { 13331d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Rows/columns 13341d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 4, 6, 13351d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Cost 13361d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 2082.0, 13371d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Residuals 13381d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling { -19.0, -35.0, // f 13391d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling -27.0, -43.0 // h 13401d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling }, 13411d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Gradient 13421d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling { 146.0, 484.0, // x 13431d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 228.0, 560.0, // y 13441d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 270.0, 516.0, // z 13451d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling }, 13461d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Jacobian 13471d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // x y z 13481d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling { /* f(x, y) */ -2.0, 0.0, -12.0, 0.0, 0.0, 0.0, 13491d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 0.0, -4.0, 0.0, -16.0, 0.0, 0.0, 13501d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling /* h(z, x) */ -4.0, 0.0, 0.0, 0.0, -10.0, 0.0, 13511d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 0.0, -8.0, 0.0, 0.0, 0.0, -12.0 13521d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 13531d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling }; 13541d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 13551d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling Problem::EvaluateOptions evaluate_options; 13561d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling evaluate_options.residual_blocks.push_back(residual_blocks_[0]); 13571d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling evaluate_options.residual_blocks.push_back(residual_blocks_[2]); 13581d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 13591d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling CheckAllEvaluationCombinations(evaluate_options, expected); 13601d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling} 13611d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 13621d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha HaeberlingTEST_F(ProblemEvaluateTest, ExcludedParameterBlock) { 13631d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ExpectedEvaluation expected = { 13641d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Rows/columns 13651d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 6, 4, 13661d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Cost 13671d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 7607.0, 13681d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Residuals 13691d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling { -19.0, -35.0, // f 13701d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling -59.0, -87.0, // g 13711d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling -27.0, -43.0 // h 13721d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling }, 13731d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 13741d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Gradient 13751d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling { 146.0, 484.0, // x 13761d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1450.0, 2604.0, // z 13771d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling }, 13781d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 13791d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Jacobian 13801d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // x z 13811d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling { /* f(x, y) */ -2.0, 0.0, 0.0, 0.0, 13821d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 0.0, -4.0, 0.0, 0.0, 13831d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling /* g(y, z) */ 0.0, 0.0, -20.0, 0.0, 13841d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 0.0, 0.0, 0.0, -24.0, 13851d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling /* h(z, x) */ -4.0, 0.0, -10.0, 0.0, 13861d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 0.0, -8.0, 0.0, -12.0 13871d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 13881d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling }; 13891d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 13901d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling Problem::EvaluateOptions evaluate_options; 13911d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // x, z 13921d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling evaluate_options.parameter_blocks.push_back(parameter_blocks_[0]); 13931d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling evaluate_options.parameter_blocks.push_back(parameter_blocks_[2]); 13941d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling evaluate_options.residual_blocks = residual_blocks_; 13951d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling CheckAllEvaluationCombinations(evaluate_options, expected); 13961d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling} 13971d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 13981d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha HaeberlingTEST_F(ProblemEvaluateTest, ExcludedParameterBlockAndExcludedResidualBlock) { 13991d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ExpectedEvaluation expected = { 14001d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Rows/columns 14011d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 4, 4, 14021d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Cost 14031d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 6318.0, 14041d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Residuals 14051d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling { -19.0, -35.0, // f 14061d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling -59.0, -87.0, // g 14071d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling }, 14081d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 14091d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Gradient 14101d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling { 38.0, 140.0, // x 14111d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1180.0, 2088.0, // z 14121d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling }, 14131d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 14141d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Jacobian 14151d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // x z 14161d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling { /* f(x, y) */ -2.0, 0.0, 0.0, 0.0, 14171d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 0.0, -4.0, 0.0, 0.0, 14181d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling /* g(y, z) */ 0.0, 0.0, -20.0, 0.0, 14191d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 0.0, 0.0, 0.0, -24.0, 14201d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 14211d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling }; 14221d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 14231d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling Problem::EvaluateOptions evaluate_options; 14241d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // x, z 14251d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling evaluate_options.parameter_blocks.push_back(parameter_blocks_[0]); 14261d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling evaluate_options.parameter_blocks.push_back(parameter_blocks_[2]); 14271d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling evaluate_options.residual_blocks.push_back(residual_blocks_[0]); 14281d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling evaluate_options.residual_blocks.push_back(residual_blocks_[1]); 14291d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 14301d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling CheckAllEvaluationCombinations(evaluate_options, expected); 14311d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling} 14321d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 14331d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha HaeberlingTEST_F(ProblemEvaluateTest, LocalParameterization) { 14341d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ExpectedEvaluation expected = { 14351d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Rows/columns 14361d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 6, 5, 14371d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Cost 14381d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 7607.0, 14391d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Residuals 14401d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling { -19.0, -35.0, // f 14411d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling -59.0, -87.0, // g 14421d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling -27.0, -43.0 // h 14431d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling }, 14441d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Gradient 14451d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling { 146.0, 484.0, // x 14461d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1256.0, // y with SubsetParameterization 14471d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1450.0, 2604.0, // z 14481d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling }, 14491d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Jacobian 14501d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // x y z 14511d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling { /* f(x, y) */ -2.0, 0.0, 0.0, 0.0, 0.0, 14521d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 0.0, -4.0, -16.0, 0.0, 0.0, 14531d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling /* g(y, z) */ 0.0, 0.0, 0.0, -20.0, 0.0, 14541d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 0.0, 0.0, -8.0, 0.0, -24.0, 14551d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling /* h(z, x) */ -4.0, 0.0, 0.0, -10.0, 0.0, 14561d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 0.0, -8.0, 0.0, 0.0, -12.0 14571d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 14581d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling }; 14591d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 14601d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling vector<int> constant_parameters; 14611d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling constant_parameters.push_back(0); 14621d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling problem_.SetParameterization(parameters_ + 2, 14631d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling new SubsetParameterization(2, 14641d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling constant_parameters)); 14651d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 14661d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling CheckAllEvaluationCombinations(Problem::EvaluateOptions(), expected); 14670ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 14680ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 14690ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} // namespace internal 14700ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} // namespace ceres 1471