11d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// Ceres Solver - A fast non-linear least squares minimizer 21d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// Copyright 2012 Google Inc. All rights reserved. 31d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// http://code.google.com/p/ceres-solver/ 41d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// 51d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// Redistribution and use in source and binary forms, with or without 61d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// modification, are permitted provided that the following conditions are met: 71d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// 81d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// * Redistributions of source code must retain the above copyright notice, 91d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// this list of conditions and the following disclaimer. 101d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// * Redistributions in binary form must reproduce the above copyright notice, 111d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// this list of conditions and the following disclaimer in the documentation 121d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// and/or other materials provided with the distribution. 131d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// * Neither the name of Google Inc. nor the names of its contributors may be 141d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// used to endorse or promote products derived from this software without 151d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// specific prior written permission. 161d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// 171d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 181d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 191d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 201d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 211d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 221d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 231d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 241d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 251d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 261d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 271d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// POSSIBILITY OF SUCH DAMAGE. 281d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// 291d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// Author: thadh@gmail.com (Thad Hughes) 301d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// mierle@gmail.com (Keir Mierle) 311d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// sameeragarwal@google.com (Sameer Agarwal) 321d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 331d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#include <cstddef> 341d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 351d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#include "ceres/dynamic_autodiff_cost_function.h" 361d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#include "ceres/internal/scoped_ptr.h" 371d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#include "gtest/gtest.h" 381d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 391d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingnamespace ceres { 401d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingnamespace internal { 411d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 421d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// Takes 2 parameter blocks: 431d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// parameters[0] is size 10. 441d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// parameters[1] is size 5. 451d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// Emits 21 residuals: 461d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// A: i - parameters[0][i], for i in [0,10) -- this is 10 residuals 471d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// B: parameters[0][i] - i, for i in [0,10) -- this is another 10. 481d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// C: sum(parameters[0][i]^2 - 8*parameters[0][i]) + sum(parameters[1][i]) 491d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingclass MyCostFunctor { 501d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling public: 511d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling template <typename T> 521d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling bool operator()(T const* const* parameters, T* residuals) const { 531d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const T* params0 = parameters[0]; 541d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling int r = 0; 551d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling for (int i = 0; i < 10; ++i) { 561d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling residuals[r++] = T(i) - params0[i]; 571d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling residuals[r++] = params0[i] - T(i); 581d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 591d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 601d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling T c_residual(0.0); 611d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling for (int i = 0; i < 10; ++i) { 621d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling c_residual += pow(params0[i], 2) - T(8) * params0[i]; 631d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 641d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 651d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const T* params1 = parameters[1]; 661d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling for (int i = 0; i < 5; ++i) { 671d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling c_residual += params1[i]; 681d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 691d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling residuals[r++] = c_residual; 701d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling return true; 711d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 721d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling}; 731d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 741d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha HaeberlingTEST(DynamicAutodiffCostFunctionTest, TestResiduals) { 751d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling vector<double> param_block_0(10, 0.0); 761d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling vector<double> param_block_1(5, 0.0); 771d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling DynamicAutoDiffCostFunction<MyCostFunctor, 3> cost_function( 781d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling new MyCostFunctor()); 791d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling cost_function.AddParameterBlock(param_block_0.size()); 801d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling cost_function.AddParameterBlock(param_block_1.size()); 811d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling cost_function.SetNumResiduals(21); 821d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 831d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Test residual computation. 841d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling vector<double> residuals(21, -100000); 851d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling vector<double*> parameter_blocks(2); 861d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling parameter_blocks[0] = ¶m_block_0[0]; 871d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling parameter_blocks[1] = ¶m_block_1[0]; 881d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_TRUE(cost_function.Evaluate(¶meter_blocks[0], 891d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling residuals.data(), 901d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling NULL)); 911d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling for (int r = 0; r < 10; ++r) { 921d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_EQ(1.0 * r, residuals.at(r * 2)); 931d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_EQ(-1.0 * r, residuals.at(r * 2 + 1)); 941d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 951d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_EQ(0, residuals.at(20)); 961d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling} 971d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 981d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha HaeberlingTEST(DynamicAutodiffCostFunctionTest, TestJacobian) { 991d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Test the residual counting. 1001d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling vector<double> param_block_0(10, 0.0); 1011d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling for (int i = 0; i < 10; ++i) { 1021d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling param_block_0[i] = 2 * i; 1031d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 1041d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling vector<double> param_block_1(5, 0.0); 1051d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling DynamicAutoDiffCostFunction<MyCostFunctor, 3> cost_function( 1061d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling new MyCostFunctor()); 1071d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling cost_function.AddParameterBlock(param_block_0.size()); 1081d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling cost_function.AddParameterBlock(param_block_1.size()); 1091d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling cost_function.SetNumResiduals(21); 1101d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1111d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Prepare the residuals. 1121d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling vector<double> residuals(21, -100000); 1131d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1141d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Prepare the parameters. 1151d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling vector<double*> parameter_blocks(2); 1161d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling parameter_blocks[0] = ¶m_block_0[0]; 1171d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling parameter_blocks[1] = ¶m_block_1[0]; 1181d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1191d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Prepare the jacobian. 1201d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling vector<vector<double> > jacobian_vect(2); 1211d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling jacobian_vect[0].resize(21 * 10, -100000); 1221d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling jacobian_vect[1].resize(21 * 5, -100000); 1231d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling vector<double*> jacobian; 1241d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling jacobian.push_back(jacobian_vect[0].data()); 1251d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling jacobian.push_back(jacobian_vect[1].data()); 1261d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1271d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Test jacobian computation. 1281d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_TRUE(cost_function.Evaluate(parameter_blocks.data(), 1291d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling residuals.data(), 1301d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling jacobian.data())); 1311d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1321d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling for (int r = 0; r < 10; ++r) { 1331d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_EQ(-1.0 * r, residuals.at(r * 2)); 1341d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_EQ(+1.0 * r, residuals.at(r * 2 + 1)); 1351d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 1361d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_EQ(420, residuals.at(20)); 1371d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling for (int p = 0; p < 10; ++p) { 1381d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Check "A" Jacobian. 1391d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_EQ(-1.0, jacobian_vect[0][2*p * 10 + p]); 1401d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Check "B" Jacobian. 1411d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_EQ(+1.0, jacobian_vect[0][(2*p+1) * 10 + p]); 1421d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling jacobian_vect[0][2*p * 10 + p] = 0.0; 1431d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling jacobian_vect[0][(2*p+1) * 10 + p] = 0.0; 1441d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 1451d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1461d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Check "C" Jacobian for first parameter block. 1471d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling for (int p = 0; p < 10; ++p) { 1481d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_EQ(4 * p - 8, jacobian_vect[0][20 * 10 + p]); 1491d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling jacobian_vect[0][20 * 10 + p] = 0.0; 1501d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 1511d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling for (int i = 0; i < jacobian_vect[0].size(); ++i) { 1521d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_EQ(0.0, jacobian_vect[0][i]); 1531d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 1541d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1551d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Check "C" Jacobian for second parameter block. 1561d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling for (int p = 0; p < 5; ++p) { 1571d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_EQ(1.0, jacobian_vect[1][20 * 5 + p]); 1581d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling jacobian_vect[1][20 * 5 + p] = 0.0; 1591d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 1601d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling for (int i = 0; i < jacobian_vect[1].size(); ++i) { 1611d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_EQ(0.0, jacobian_vect[1][i]); 1621d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 1631d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling} 1641d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1651d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha HaeberlingTEST(DynamicAutodiffCostFunctionTest, JacobianWithFirstParameterBlockConstant) { 1661d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Test the residual counting. 1671d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling vector<double> param_block_0(10, 0.0); 1681d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling for (int i = 0; i < 10; ++i) { 1691d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling param_block_0[i] = 2 * i; 1701d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 1711d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling vector<double> param_block_1(5, 0.0); 1721d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling DynamicAutoDiffCostFunction<MyCostFunctor, 3> cost_function( 1731d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling new MyCostFunctor()); 1741d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling cost_function.AddParameterBlock(param_block_0.size()); 1751d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling cost_function.AddParameterBlock(param_block_1.size()); 1761d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling cost_function.SetNumResiduals(21); 1771d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1781d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Prepare the residuals. 1791d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling vector<double> residuals(21, -100000); 1801d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1811d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Prepare the parameters. 1821d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling vector<double*> parameter_blocks(2); 1831d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling parameter_blocks[0] = ¶m_block_0[0]; 1841d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling parameter_blocks[1] = ¶m_block_1[0]; 1851d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1861d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Prepare the jacobian. 1871d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling vector<vector<double> > jacobian_vect(2); 1881d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling jacobian_vect[0].resize(21 * 10, -100000); 1891d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling jacobian_vect[1].resize(21 * 5, -100000); 1901d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling vector<double*> jacobian; 1911d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling jacobian.push_back(NULL); 1921d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling jacobian.push_back(jacobian_vect[1].data()); 1931d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1941d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Test jacobian computation. 1951d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_TRUE(cost_function.Evaluate(parameter_blocks.data(), 1961d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling residuals.data(), 1971d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling jacobian.data())); 1981d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1991d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling for (int r = 0; r < 10; ++r) { 2001d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_EQ(-1.0 * r, residuals.at(r * 2)); 2011d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_EQ(+1.0 * r, residuals.at(r * 2 + 1)); 2021d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 2031d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_EQ(420, residuals.at(20)); 2041d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 2051d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Check "C" Jacobian for second parameter block. 2061d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling for (int p = 0; p < 5; ++p) { 2071d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_EQ(1.0, jacobian_vect[1][20 * 5 + p]); 2081d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling jacobian_vect[1][20 * 5 + p] = 0.0; 2091d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 2101d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling for (int i = 0; i < jacobian_vect[1].size(); ++i) { 2111d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_EQ(0.0, jacobian_vect[1][i]); 2121d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 2131d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling} 2141d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 2151d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha HaeberlingTEST(DynamicAutodiffCostFunctionTest, JacobianWithSecondParameterBlockConstant) { 2161d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Test the residual counting. 2171d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling vector<double> param_block_0(10, 0.0); 2181d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling for (int i = 0; i < 10; ++i) { 2191d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling param_block_0[i] = 2 * i; 2201d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 2211d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling vector<double> param_block_1(5, 0.0); 2221d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling DynamicAutoDiffCostFunction<MyCostFunctor, 3> cost_function( 2231d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling new MyCostFunctor()); 2241d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling cost_function.AddParameterBlock(param_block_0.size()); 2251d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling cost_function.AddParameterBlock(param_block_1.size()); 2261d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling cost_function.SetNumResiduals(21); 2271d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 2281d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Prepare the residuals. 2291d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling vector<double> residuals(21, -100000); 2301d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 2311d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Prepare the parameters. 2321d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling vector<double*> parameter_blocks(2); 2331d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling parameter_blocks[0] = ¶m_block_0[0]; 2341d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling parameter_blocks[1] = ¶m_block_1[0]; 2351d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 2361d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Prepare the jacobian. 2371d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling vector<vector<double> > jacobian_vect(2); 2381d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling jacobian_vect[0].resize(21 * 10, -100000); 2391d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling jacobian_vect[1].resize(21 * 5, -100000); 2401d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling vector<double*> jacobian; 2411d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling jacobian.push_back(jacobian_vect[0].data()); 2421d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling jacobian.push_back(NULL); 2431d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 2441d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Test jacobian computation. 2451d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_TRUE(cost_function.Evaluate(parameter_blocks.data(), 2461d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling residuals.data(), 2471d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling jacobian.data())); 2481d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 2491d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling for (int r = 0; r < 10; ++r) { 2501d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_EQ(-1.0 * r, residuals.at(r * 2)); 2511d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_EQ(+1.0 * r, residuals.at(r * 2 + 1)); 2521d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 2531d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_EQ(420, residuals.at(20)); 2541d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling for (int p = 0; p < 10; ++p) { 2551d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Check "A" Jacobian. 2561d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_EQ(-1.0, jacobian_vect[0][2*p * 10 + p]); 2571d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Check "B" Jacobian. 2581d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_EQ(+1.0, jacobian_vect[0][(2*p+1) * 10 + p]); 2591d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling jacobian_vect[0][2*p * 10 + p] = 0.0; 2601d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling jacobian_vect[0][(2*p+1) * 10 + p] = 0.0; 2611d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 2621d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 2631d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Check "C" Jacobian for first parameter block. 2641d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling for (int p = 0; p < 10; ++p) { 2651d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_EQ(4 * p - 8, jacobian_vect[0][20 * 10 + p]); 2661d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling jacobian_vect[0][20 * 10 + p] = 0.0; 2671d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 2681d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling for (int i = 0; i < jacobian_vect[0].size(); ++i) { 2691d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_EQ(0.0, jacobian_vect[0][i]); 2701d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 2711d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling} 2721d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 2731d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// Takes 3 parameter blocks: 2741d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// parameters[0] (x) is size 1. 2751d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// parameters[1] (y) is size 2. 2761d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// parameters[2] (z) is size 3. 2771d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// Emits 7 residuals: 2781d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// A: x[0] (= sum_x) 2791d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// B: y[0] + 2.0 * y[1] (= sum_y) 2801d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// C: z[0] + 3.0 * z[1] + 6.0 * z[2] (= sum_z) 2811d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// D: sum_x * sum_y 2821d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// E: sum_y * sum_z 2831d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// F: sum_x * sum_z 2841d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// G: sum_x * sum_y * sum_z 2851d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingclass MyThreeParameterCostFunctor { 2861d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling public: 2871d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling template <typename T> 2881d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling bool operator()(T const* const* parameters, T* residuals) const { 2891d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const T* x = parameters[0]; 2901d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const T* y = parameters[1]; 2911d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const T* z = parameters[2]; 2921d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 2931d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling T sum_x = x[0]; 2941d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling T sum_y = y[0] + 2.0 * y[1]; 2951d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling T sum_z = z[0] + 3.0 * z[1] + 6.0 * z[2]; 2961d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 2971d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling residuals[0] = sum_x; 2981d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling residuals[1] = sum_y; 2991d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling residuals[2] = sum_z; 3001d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling residuals[3] = sum_x * sum_y; 3011d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling residuals[4] = sum_y * sum_z; 3021d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling residuals[5] = sum_x * sum_z; 3031d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling residuals[6] = sum_x * sum_y * sum_z; 3041d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling return true; 3051d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 3061d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling}; 3071d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 3081d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingclass ThreeParameterCostFunctorTest : public ::testing::Test { 3091d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling protected: 3101d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling virtual void SetUp() { 3111d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Prepare the parameters. 3121d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling x_.resize(1); 3131d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling x_[0] = 0.0; 3141d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 3151d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling y_.resize(2); 3161d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling y_[0] = 1.0; 3171d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling y_[1] = 3.0; 3181d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 3191d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling z_.resize(3); 3201d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling z_[0] = 2.0; 3211d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling z_[1] = 4.0; 3221d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling z_[2] = 6.0; 3231d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 3241d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling parameter_blocks_.resize(3); 3251d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling parameter_blocks_[0] = &x_[0]; 3261d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling parameter_blocks_[1] = &y_[0]; 3271d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling parameter_blocks_[2] = &z_[0]; 3281d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 3291d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Prepare the cost function. 3301d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling typedef DynamicAutoDiffCostFunction<MyThreeParameterCostFunctor, 3> 3311d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling DynamicMyThreeParameterCostFunction; 3321d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling DynamicMyThreeParameterCostFunction * cost_function = 3331d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling new DynamicMyThreeParameterCostFunction( 3341d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling new MyThreeParameterCostFunctor()); 3351d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling cost_function->AddParameterBlock(1); 3361d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling cost_function->AddParameterBlock(2); 3371d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling cost_function->AddParameterBlock(3); 3381d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling cost_function->SetNumResiduals(7); 3391d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 3401d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling cost_function_.reset(cost_function); 3411d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 3421d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Setup jacobian data. 3431d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling jacobian_vect_.resize(3); 3441d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling jacobian_vect_[0].resize(7 * x_.size(), -100000); 3451d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling jacobian_vect_[1].resize(7 * y_.size(), -100000); 3461d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling jacobian_vect_[2].resize(7 * z_.size(), -100000); 3471d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 3481d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Prepare the expected residuals. 3491d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const double sum_x = x_[0]; 3501d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const double sum_y = y_[0] + 2.0 * y_[1]; 3511d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const double sum_z = z_[0] + 3.0 * z_[1] + 6.0 * z_[2]; 3521d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 3531d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling expected_residuals_.resize(7); 3541d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling expected_residuals_[0] = sum_x; 3551d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling expected_residuals_[1] = sum_y; 3561d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling expected_residuals_[2] = sum_z; 3571d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling expected_residuals_[3] = sum_x * sum_y; 3581d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling expected_residuals_[4] = sum_y * sum_z; 3591d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling expected_residuals_[5] = sum_x * sum_z; 3601d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling expected_residuals_[6] = sum_x * sum_y * sum_z; 3611d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 3621d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Prepare the expected jacobian entries. 3631d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling expected_jacobian_x_.resize(7); 3641d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling expected_jacobian_x_[0] = 1.0; 3651d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling expected_jacobian_x_[1] = 0.0; 3661d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling expected_jacobian_x_[2] = 0.0; 3671d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling expected_jacobian_x_[3] = sum_y; 3681d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling expected_jacobian_x_[4] = 0.0; 3691d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling expected_jacobian_x_[5] = sum_z; 3701d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling expected_jacobian_x_[6] = sum_y * sum_z; 3711d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 3721d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling expected_jacobian_y_.resize(14); 3731d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling expected_jacobian_y_[0] = 0.0; 3741d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling expected_jacobian_y_[1] = 0.0; 3751d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling expected_jacobian_y_[2] = 1.0; 3761d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling expected_jacobian_y_[3] = 2.0; 3771d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling expected_jacobian_y_[4] = 0.0; 3781d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling expected_jacobian_y_[5] = 0.0; 3791d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling expected_jacobian_y_[6] = sum_x; 3801d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling expected_jacobian_y_[7] = 2.0 * sum_x; 3811d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling expected_jacobian_y_[8] = sum_z; 3821d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling expected_jacobian_y_[9] = 2.0 * sum_z; 3831d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling expected_jacobian_y_[10] = 0.0; 3841d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling expected_jacobian_y_[11] = 0.0; 3851d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling expected_jacobian_y_[12] = sum_x * sum_z; 3861d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling expected_jacobian_y_[13] = 2.0 * sum_x * sum_z; 3871d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 3881d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling expected_jacobian_z_.resize(21); 3891d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling expected_jacobian_z_[0] = 0.0; 3901d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling expected_jacobian_z_[1] = 0.0; 3911d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling expected_jacobian_z_[2] = 0.0; 3921d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling expected_jacobian_z_[3] = 0.0; 3931d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling expected_jacobian_z_[4] = 0.0; 3941d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling expected_jacobian_z_[5] = 0.0; 3951d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling expected_jacobian_z_[6] = 1.0; 3961d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling expected_jacobian_z_[7] = 3.0; 3971d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling expected_jacobian_z_[8] = 6.0; 3981d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling expected_jacobian_z_[9] = 0.0; 3991d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling expected_jacobian_z_[10] = 0.0; 4001d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling expected_jacobian_z_[11] = 0.0; 4011d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling expected_jacobian_z_[12] = sum_y; 4021d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling expected_jacobian_z_[13] = 3.0 * sum_y; 4031d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling expected_jacobian_z_[14] = 6.0 * sum_y; 4041d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling expected_jacobian_z_[15] = sum_x; 4051d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling expected_jacobian_z_[16] = 3.0 * sum_x; 4061d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling expected_jacobian_z_[17] = 6.0 * sum_x; 4071d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling expected_jacobian_z_[18] = sum_x * sum_y; 4081d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling expected_jacobian_z_[19] = 3.0 * sum_x * sum_y; 4091d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling expected_jacobian_z_[20] = 6.0 * sum_x * sum_y; 4101d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 4111d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 4121d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling protected: 4131d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling vector<double> x_; 4141d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling vector<double> y_; 4151d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling vector<double> z_; 4161d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 4171d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling vector<double*> parameter_blocks_; 4181d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 4191d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling scoped_ptr<CostFunction> cost_function_; 4201d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 4211d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling vector<vector<double> > jacobian_vect_; 4221d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 4231d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling vector<double> expected_residuals_; 4241d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 4251d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling vector<double> expected_jacobian_x_; 4261d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling vector<double> expected_jacobian_y_; 4271d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling vector<double> expected_jacobian_z_; 4281d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling}; 4291d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 4301d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha HaeberlingTEST_F(ThreeParameterCostFunctorTest, TestThreeParameterResiduals) { 4311d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling vector<double> residuals(7, -100000); 4321d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_TRUE(cost_function_->Evaluate(parameter_blocks_.data(), 4331d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling residuals.data(), 4341d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling NULL)); 4351d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling for (int i = 0; i < 7; ++i) { 4361d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_EQ(expected_residuals_[i], residuals[i]); 4371d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 4381d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling} 4391d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 4401d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha HaeberlingTEST_F(ThreeParameterCostFunctorTest, TestThreeParameterJacobian) { 4411d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling vector<double> residuals(7, -100000); 4421d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 4431d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling vector<double*> jacobian; 4441d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling jacobian.push_back(jacobian_vect_[0].data()); 4451d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling jacobian.push_back(jacobian_vect_[1].data()); 4461d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling jacobian.push_back(jacobian_vect_[2].data()); 4471d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 4481d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_TRUE(cost_function_->Evaluate(parameter_blocks_.data(), 4491d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling residuals.data(), 4501d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling jacobian.data())); 4511d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 4521d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling for (int i = 0; i < 7; ++i) { 4531d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_EQ(expected_residuals_[i], residuals[i]); 4541d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 4551d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 4561d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling for (int i = 0; i < 7; ++i) { 4571d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_EQ(expected_jacobian_x_[i], jacobian[0][i]); 4581d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 4591d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 4601d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling for (int i = 0; i < 14; ++i) { 4611d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_EQ(expected_jacobian_y_[i], jacobian[1][i]); 4621d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 4631d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 4641d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling for (int i = 0; i < 21; ++i) { 4651d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_EQ(expected_jacobian_z_[i], jacobian[2][i]); 4661d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 4671d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling} 4681d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 4691d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha HaeberlingTEST_F(ThreeParameterCostFunctorTest, 4701d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ThreeParameterJacobianWithFirstAndLastParameterBlockConstant) { 4711d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling vector<double> residuals(7, -100000); 4721d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 4731d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling vector<double*> jacobian; 4741d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling jacobian.push_back(NULL); 4751d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling jacobian.push_back(jacobian_vect_[1].data()); 4761d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling jacobian.push_back(NULL); 4771d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 4781d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_TRUE(cost_function_->Evaluate(parameter_blocks_.data(), 4791d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling residuals.data(), 4801d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling jacobian.data())); 4811d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 4821d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling for (int i = 0; i < 7; ++i) { 4831d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_EQ(expected_residuals_[i], residuals[i]); 4841d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 4851d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 4861d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling for (int i = 0; i < 14; ++i) { 4871d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_EQ(expected_jacobian_y_[i], jacobian[1][i]); 4881d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 4891d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling} 4901d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 4911d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha HaeberlingTEST_F(ThreeParameterCostFunctorTest, 4921d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ThreeParameterJacobianWithSecondParameterBlockConstant) { 4931d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling vector<double> residuals(7, -100000); 4941d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 4951d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling vector<double*> jacobian; 4961d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling jacobian.push_back(jacobian_vect_[0].data()); 4971d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling jacobian.push_back(NULL); 4981d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling jacobian.push_back(jacobian_vect_[2].data()); 4991d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 5001d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_TRUE(cost_function_->Evaluate(parameter_blocks_.data(), 5011d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling residuals.data(), 5021d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling jacobian.data())); 5031d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 5041d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling for (int i = 0; i < 7; ++i) { 5051d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_EQ(expected_residuals_[i], residuals[i]); 5061d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 5071d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 5081d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling for (int i = 0; i < 7; ++i) { 5091d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_EQ(expected_jacobian_x_[i], jacobian[0][i]); 5101d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 5111d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 5121d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling for (int i = 0; i < 21; ++i) { 5131d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_EQ(expected_jacobian_z_[i], jacobian[2][i]); 5141d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 5151d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling} 5161d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 5171d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// Takes 6 parameter blocks all of size 1: 5181d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// x0, y0, y1, z0, z1, z2 5191d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// Same 7 residuals as MyThreeParameterCostFunctor. 5201d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// Naming convention for tests is (V)ariable and (C)onstant. 5211d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingclass MySixParameterCostFunctor { 5221d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling public: 5231d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling template <typename T> 5241d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling bool operator()(T const* const* parameters, T* residuals) const { 5251d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const T* x0 = parameters[0]; 5261d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const T* y0 = parameters[1]; 5271d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const T* y1 = parameters[2]; 5281d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const T* z0 = parameters[3]; 5291d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const T* z1 = parameters[4]; 5301d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const T* z2 = parameters[5]; 5311d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 5321d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling T sum_x = x0[0]; 5331d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling T sum_y = y0[0] + 2.0 * y1[0]; 5341d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling T sum_z = z0[0] + 3.0 * z1[0] + 6.0 * z2[0]; 5351d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 5361d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling residuals[0] = sum_x; 5371d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling residuals[1] = sum_y; 5381d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling residuals[2] = sum_z; 5391d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling residuals[3] = sum_x * sum_y; 5401d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling residuals[4] = sum_y * sum_z; 5411d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling residuals[5] = sum_x * sum_z; 5421d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling residuals[6] = sum_x * sum_y * sum_z; 5431d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling return true; 5441d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 5451d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling}; 5461d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 5471d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingclass SixParameterCostFunctorTest : public ::testing::Test { 5481d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling protected: 5491d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling virtual void SetUp() { 5501d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Prepare the parameters. 5511d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling x0_ = 0.0; 5521d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling y0_ = 1.0; 5531d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling y1_ = 3.0; 5541d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling z0_ = 2.0; 5551d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling z1_ = 4.0; 5561d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling z2_ = 6.0; 5571d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 5581d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling parameter_blocks_.resize(6); 5591d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling parameter_blocks_[0] = &x0_; 5601d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling parameter_blocks_[1] = &y0_; 5611d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling parameter_blocks_[2] = &y1_; 5621d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling parameter_blocks_[3] = &z0_; 5631d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling parameter_blocks_[4] = &z1_; 5641d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling parameter_blocks_[5] = &z2_; 5651d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 5661d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Prepare the cost function. 5671d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling typedef DynamicAutoDiffCostFunction<MySixParameterCostFunctor, 3> 5681d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling DynamicMySixParameterCostFunction; 5691d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling DynamicMySixParameterCostFunction * cost_function = 5701d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling new DynamicMySixParameterCostFunction( 5711d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling new MySixParameterCostFunctor()); 5721d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling for (int i = 0; i < 6; ++i) { 5731d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling cost_function->AddParameterBlock(1); 5741d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 5751d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling cost_function->SetNumResiduals(7); 5761d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 5771d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling cost_function_.reset(cost_function); 5781d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 5791d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Setup jacobian data. 5801d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling jacobian_vect_.resize(6); 5811d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling for (int i = 0; i < 6; ++i) { 5821d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling jacobian_vect_[i].resize(7, -100000); 5831d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 5841d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 5851d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Prepare the expected residuals. 5861d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const double sum_x = x0_; 5871d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const double sum_y = y0_ + 2.0 * y1_; 5881d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const double sum_z = z0_ + 3.0 * z1_ + 6.0 * z2_; 5891d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 5901d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling expected_residuals_.resize(7); 5911d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling expected_residuals_[0] = sum_x; 5921d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling expected_residuals_[1] = sum_y; 5931d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling expected_residuals_[2] = sum_z; 5941d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling expected_residuals_[3] = sum_x * sum_y; 5951d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling expected_residuals_[4] = sum_y * sum_z; 5961d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling expected_residuals_[5] = sum_x * sum_z; 5971d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling expected_residuals_[6] = sum_x * sum_y * sum_z; 5981d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 5991d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Prepare the expected jacobian entries. 6001d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling expected_jacobians_.resize(6); 6011d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling expected_jacobians_[0].resize(7); 6021d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling expected_jacobians_[0][0] = 1.0; 6031d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling expected_jacobians_[0][1] = 0.0; 6041d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling expected_jacobians_[0][2] = 0.0; 6051d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling expected_jacobians_[0][3] = sum_y; 6061d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling expected_jacobians_[0][4] = 0.0; 6071d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling expected_jacobians_[0][5] = sum_z; 6081d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling expected_jacobians_[0][6] = sum_y * sum_z; 6091d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 6101d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling expected_jacobians_[1].resize(7); 6111d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling expected_jacobians_[1][0] = 0.0; 6121d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling expected_jacobians_[1][1] = 1.0; 6131d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling expected_jacobians_[1][2] = 0.0; 6141d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling expected_jacobians_[1][3] = sum_x; 6151d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling expected_jacobians_[1][4] = sum_z; 6161d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling expected_jacobians_[1][5] = 0.0; 6171d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling expected_jacobians_[1][6] = sum_x * sum_z; 6181d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 6191d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling expected_jacobians_[2].resize(7); 6201d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling expected_jacobians_[2][0] = 0.0; 6211d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling expected_jacobians_[2][1] = 2.0; 6221d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling expected_jacobians_[2][2] = 0.0; 6231d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling expected_jacobians_[2][3] = 2.0 * sum_x; 6241d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling expected_jacobians_[2][4] = 2.0 * sum_z; 6251d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling expected_jacobians_[2][5] = 0.0; 6261d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling expected_jacobians_[2][6] = 2.0 * sum_x * sum_z; 6271d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 6281d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling expected_jacobians_[3].resize(7); 6291d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling expected_jacobians_[3][0] = 0.0; 6301d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling expected_jacobians_[3][1] = 0.0; 6311d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling expected_jacobians_[3][2] = 1.0; 6321d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling expected_jacobians_[3][3] = 0.0; 6331d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling expected_jacobians_[3][4] = sum_y; 6341d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling expected_jacobians_[3][5] = sum_x; 6351d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling expected_jacobians_[3][6] = sum_x * sum_y; 6361d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 6371d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling expected_jacobians_[4].resize(7); 6381d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling expected_jacobians_[4][0] = 0.0; 6391d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling expected_jacobians_[4][1] = 0.0; 6401d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling expected_jacobians_[4][2] = 3.0; 6411d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling expected_jacobians_[4][3] = 0.0; 6421d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling expected_jacobians_[4][4] = 3.0 * sum_y; 6431d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling expected_jacobians_[4][5] = 3.0 * sum_x; 6441d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling expected_jacobians_[4][6] = 3.0 * sum_x * sum_y; 6451d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 6461d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling expected_jacobians_[5].resize(7); 6471d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling expected_jacobians_[5][0] = 0.0; 6481d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling expected_jacobians_[5][1] = 0.0; 6491d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling expected_jacobians_[5][2] = 6.0; 6501d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling expected_jacobians_[5][3] = 0.0; 6511d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling expected_jacobians_[5][4] = 6.0 * sum_y; 6521d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling expected_jacobians_[5][5] = 6.0 * sum_x; 6531d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling expected_jacobians_[5][6] = 6.0 * sum_x * sum_y; 6541d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 6551d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 6561d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling protected: 6571d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling double x0_; 6581d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling double y0_; 6591d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling double y1_; 6601d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling double z0_; 6611d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling double z1_; 6621d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling double z2_; 6631d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 6641d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling vector<double*> parameter_blocks_; 6651d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 6661d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling scoped_ptr<CostFunction> cost_function_; 6671d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 6681d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling vector<vector<double> > jacobian_vect_; 6691d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 6701d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling vector<double> expected_residuals_; 6711d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling vector<vector<double> > expected_jacobians_; 6721d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling}; 6731d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 6741d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha HaeberlingTEST_F(SixParameterCostFunctorTest, TestSixParameterResiduals) { 6751d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling vector<double> residuals(7, -100000); 6761d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_TRUE(cost_function_->Evaluate(parameter_blocks_.data(), 6771d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling residuals.data(), 6781d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling NULL)); 6791d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling for (int i = 0; i < 7; ++i) { 6801d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_EQ(expected_residuals_[i], residuals[i]); 6811d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 6821d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling} 6831d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 6841d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha HaeberlingTEST_F(SixParameterCostFunctorTest, TestSixParameterJacobian) { 6851d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling vector<double> residuals(7, -100000); 6861d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 6871d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling vector<double*> jacobian; 6881d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling jacobian.push_back(jacobian_vect_[0].data()); 6891d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling jacobian.push_back(jacobian_vect_[1].data()); 6901d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling jacobian.push_back(jacobian_vect_[2].data()); 6911d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling jacobian.push_back(jacobian_vect_[3].data()); 6921d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling jacobian.push_back(jacobian_vect_[4].data()); 6931d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling jacobian.push_back(jacobian_vect_[5].data()); 6941d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 6951d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_TRUE(cost_function_->Evaluate(parameter_blocks_.data(), 6961d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling residuals.data(), 6971d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling jacobian.data())); 6981d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 6991d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling for (int i = 0; i < 7; ++i) { 7001d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_EQ(expected_residuals_[i], residuals[i]); 7011d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 7021d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 7031d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling for (int i = 0; i < 6; ++i) { 7041d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling for (int j = 0; j < 7; ++j) { 7051d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_EQ(expected_jacobians_[i][j], jacobian[i][j]); 7061d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 7071d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 7081d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling} 7091d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 7101d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha HaeberlingTEST_F(SixParameterCostFunctorTest, TestSixParameterJacobianVVCVVC) { 7111d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling vector<double> residuals(7, -100000); 7121d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 7131d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling vector<double*> jacobian; 7141d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling jacobian.push_back(jacobian_vect_[0].data()); 7151d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling jacobian.push_back(jacobian_vect_[1].data()); 7161d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling jacobian.push_back(NULL); 7171d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling jacobian.push_back(jacobian_vect_[3].data()); 7181d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling jacobian.push_back(jacobian_vect_[4].data()); 7191d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling jacobian.push_back(NULL); 7201d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 7211d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_TRUE(cost_function_->Evaluate(parameter_blocks_.data(), 7221d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling residuals.data(), 7231d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling jacobian.data())); 7241d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 7251d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling for (int i = 0; i < 7; ++i) { 7261d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_EQ(expected_residuals_[i], residuals[i]); 7271d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 7281d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 7291d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling for (int i = 0; i < 6; ++i) { 7301d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Skip the constant variables. 7311d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling if (i == 2 || i == 5) { 7321d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling continue; 7331d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 7341d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 7351d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling for (int j = 0; j < 7; ++j) { 7361d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_EQ(expected_jacobians_[i][j], jacobian[i][j]); 7371d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 7381d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 7391d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling} 7401d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 7411d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha HaeberlingTEST_F(SixParameterCostFunctorTest, TestSixParameterJacobianVCCVCV) { 7421d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling vector<double> residuals(7, -100000); 7431d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 7441d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling vector<double*> jacobian; 7451d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling jacobian.push_back(jacobian_vect_[0].data()); 7461d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling jacobian.push_back(NULL); 7471d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling jacobian.push_back(NULL); 7481d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling jacobian.push_back(jacobian_vect_[3].data()); 7491d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling jacobian.push_back(NULL); 7501d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling jacobian.push_back(jacobian_vect_[5].data()); 7511d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 7521d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_TRUE(cost_function_->Evaluate(parameter_blocks_.data(), 7531d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling residuals.data(), 7541d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling jacobian.data())); 7551d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 7561d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling for (int i = 0; i < 7; ++i) { 7571d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_EQ(expected_residuals_[i], residuals[i]); 7581d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 7591d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 7601d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling for (int i = 0; i < 6; ++i) { 7611d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Skip the constant variables. 7621d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling if (i == 1 || i == 2 || i == 4) { 7631d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling continue; 7641d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 7651d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 7661d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling for (int j = 0; j < 7; ++j) { 7671d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_EQ(expected_jacobians_[i][j], jacobian[i][j]); 7681d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 7691d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 7701d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling} 7711d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 7721d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling} // namespace internal 7731d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling} // namespace ceres 774