11d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// Ceres Solver - A fast non-linear least squares minimizer 21d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// Copyright 2013 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: mierle@gmail.com (Keir Mierle) 301d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 311d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#include "ceres/c_api.h" 321d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 331d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#include <cmath> 341d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 351d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#include "glog/logging.h" 361d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#include "gtest/gtest.h" 371d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 381d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// Duplicated from curve_fitting.cc. 391d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingint num_observations = 67; 401d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingdouble data[] = { 411d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 0.000000e+00, 1.133898e+00, 421d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 7.500000e-02, 1.334902e+00, 431d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1.500000e-01, 1.213546e+00, 441d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 2.250000e-01, 1.252016e+00, 451d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 3.000000e-01, 1.392265e+00, 461d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 3.750000e-01, 1.314458e+00, 471d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 4.500000e-01, 1.472541e+00, 481d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 5.250000e-01, 1.536218e+00, 491d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 6.000000e-01, 1.355679e+00, 501d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 6.750000e-01, 1.463566e+00, 511d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 7.500000e-01, 1.490201e+00, 521d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 8.250000e-01, 1.658699e+00, 531d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 9.000000e-01, 1.067574e+00, 541d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 9.750000e-01, 1.464629e+00, 551d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1.050000e+00, 1.402653e+00, 561d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1.125000e+00, 1.713141e+00, 571d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1.200000e+00, 1.527021e+00, 581d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1.275000e+00, 1.702632e+00, 591d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1.350000e+00, 1.423899e+00, 601d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1.425000e+00, 1.543078e+00, 611d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1.500000e+00, 1.664015e+00, 621d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1.575000e+00, 1.732484e+00, 631d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1.650000e+00, 1.543296e+00, 641d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1.725000e+00, 1.959523e+00, 651d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1.800000e+00, 1.685132e+00, 661d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1.875000e+00, 1.951791e+00, 671d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1.950000e+00, 2.095346e+00, 681d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 2.025000e+00, 2.361460e+00, 691d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 2.100000e+00, 2.169119e+00, 701d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 2.175000e+00, 2.061745e+00, 711d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 2.250000e+00, 2.178641e+00, 721d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 2.325000e+00, 2.104346e+00, 731d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 2.400000e+00, 2.584470e+00, 741d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 2.475000e+00, 1.914158e+00, 751d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 2.550000e+00, 2.368375e+00, 761d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 2.625000e+00, 2.686125e+00, 771d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 2.700000e+00, 2.712395e+00, 781d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 2.775000e+00, 2.499511e+00, 791d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 2.850000e+00, 2.558897e+00, 801d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 2.925000e+00, 2.309154e+00, 811d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 3.000000e+00, 2.869503e+00, 821d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 3.075000e+00, 3.116645e+00, 831d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 3.150000e+00, 3.094907e+00, 841d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 3.225000e+00, 2.471759e+00, 851d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 3.300000e+00, 3.017131e+00, 861d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 3.375000e+00, 3.232381e+00, 871d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 3.450000e+00, 2.944596e+00, 881d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 3.525000e+00, 3.385343e+00, 891d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 3.600000e+00, 3.199826e+00, 901d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 3.675000e+00, 3.423039e+00, 911d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 3.750000e+00, 3.621552e+00, 921d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 3.825000e+00, 3.559255e+00, 931d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 3.900000e+00, 3.530713e+00, 941d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 3.975000e+00, 3.561766e+00, 951d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 4.050000e+00, 3.544574e+00, 961d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 4.125000e+00, 3.867945e+00, 971d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 4.200000e+00, 4.049776e+00, 981d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 4.275000e+00, 3.885601e+00, 991d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 4.350000e+00, 4.110505e+00, 1001d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 4.425000e+00, 4.345320e+00, 1011d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 4.500000e+00, 4.161241e+00, 1021d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 4.575000e+00, 4.363407e+00, 1031d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 4.650000e+00, 4.161576e+00, 1041d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 4.725000e+00, 4.619728e+00, 1051d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 4.800000e+00, 4.737410e+00, 1061d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 4.875000e+00, 4.727863e+00, 1071d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 4.950000e+00, 4.669206e+00, 1081d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling}; 1091d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1101d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// A test cost function, similar to the one in curve_fitting.c. 1111d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingint exponential_residual(void* user_data, 1121d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling double** parameters, 1131d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling double* residuals, 1141d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling double** jacobians) { 1151d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling double* measurement = (double*) user_data; 1161d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling double x = measurement[0]; 1171d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling double y = measurement[1]; 1181d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling double m = parameters[0][0]; 1191d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling double c = parameters[1][0]; 1201d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1211d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling residuals[0] = y - exp(m * x + c); 1221d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling if (jacobians == NULL) { 1231d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling return 1; 1241d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 1251d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling if (jacobians[0] != NULL) { 1261d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling jacobians[0][0] = - x * exp(m * x + c); // dr/dm 1271d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 1281d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling if (jacobians[1] != NULL) { 1291d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling jacobians[1][0] = - exp(m * x + c); // dr/dc 1301d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 1311d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling return 1; 1321d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling} 1331d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1341d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingnamespace ceres { 1351d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingnamespace internal { 1361d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1371d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha HaeberlingTEST(C_API, SimpleEndToEndTest) { 1381d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling double m = 0.0; 1391d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling double c = 0.0; 1401d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling double *parameter_pointers[] = { &m, &c }; 1411d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling int parameter_sizes[] = { 1, 1 }; 1421d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1431d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ceres_problem_t* problem = ceres_create_problem(); 1441d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling for (int i = 0; i < num_observations; ++i) { 1451d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ceres_problem_add_residual_block( 1461d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling problem, 1471d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling exponential_residual, // Cost function 1481d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling &data[2 * i], // Points to the (x,y) measurement 1491d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling NULL, // Loss function 1501d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling NULL, // Loss function user data 1511d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1, // Number of residuals 1521d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 2, // Number of parameter blocks 1531d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling parameter_sizes, 1541d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling parameter_pointers); 1551d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 1561d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1571d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ceres_solve(problem); 1581d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1591d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_NEAR(0.3, m, 0.02); 1601d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_NEAR(0.1, c, 0.04); 1611d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1621d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ceres_free_problem(problem); 1631d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling} 1641d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1651d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingtemplate<typename T> 1661d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingclass ScopedSetValue { 1671d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling public: 1681d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ScopedSetValue(T* variable, T new_value) 1691d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling : variable_(variable), old_value_(*variable) { 1701d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling *variable = new_value; 1711d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 1721d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ~ScopedSetValue() { 1731d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling *variable_ = old_value_; 1741d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 1751d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1761d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling private: 1771d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling T* variable_; 1781d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling T old_value_; 1791d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling}; 1801d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1811d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha HaeberlingTEST(C_API, LossFunctions) { 1821d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling double m = 0.2; 1831d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling double c = 0.03; 1841d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling double *parameter_pointers[] = { &m, &c }; 1851d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling int parameter_sizes[] = { 1, 1 }; 1861d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1871d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Create two outliers, but be careful to leave the data intact. 1881d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ScopedSetValue<double> outlier1x(&data[12], 2.5); 1891d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ScopedSetValue<double> outlier1y(&data[13], 1.0e3); 1901d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ScopedSetValue<double> outlier2x(&data[14], 3.2); 1911d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ScopedSetValue<double> outlier2y(&data[15], 30e3); 1921d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1931d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Create a cauchy cost function, and reuse it many times. 1941d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling void* cauchy_loss_data = 1951d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ceres_create_cauchy_loss_function_data(5.0); 1961d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1971d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ceres_problem_t* problem = ceres_create_problem(); 1981d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling for (int i = 0; i < num_observations; ++i) { 1991d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ceres_problem_add_residual_block( 2001d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling problem, 2011d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling exponential_residual, // Cost function 2021d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling &data[2 * i], // Points to the (x,y) measurement 2031d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ceres_stock_loss_function, 2041d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling cauchy_loss_data, // Loss function user data 2051d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1, // Number of residuals 2061d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 2, // Number of parameter blocks 2071d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling parameter_sizes, 2081d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling parameter_pointers); 2091d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 2101d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 2111d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ceres_solve(problem); 2121d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 2131d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_NEAR(0.3, m, 0.02); 2141d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_NEAR(0.1, c, 0.04); 2151d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 2161d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ceres_free_stock_loss_function_data(cauchy_loss_data); 2171d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ceres_free_problem(problem); 2181d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling} 2191d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 2201d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling} // namespace internal 2211d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling} // namespace ceres 222