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: sameeragarwal@google.com (Sameer Agarwal) 301d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 311d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#include <cmath> 321d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#include "ceres/autodiff_local_parameterization.h" 331d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#include "ceres/fpclassify.h" 341d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#include "ceres/local_parameterization.h" 351d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#include "ceres/rotation.h" 361d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#include "gtest/gtest.h" 371d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 381d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingnamespace ceres { 391d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingnamespace internal { 401d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 411d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingstruct IdentityPlus { 421d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling template <typename T> 431d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling bool operator()(const T* x, const T* delta, T* x_plus_delta) const { 441d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling for (int i = 0; i < 3; ++i) { 451d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling x_plus_delta[i] = x[i] + delta[i]; 461d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 471d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling return true; 481d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 491d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling}; 501d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 511d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha HaeberlingTEST(AutoDiffLocalParameterizationTest, IdentityParameterization) { 521d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling AutoDiffLocalParameterization<IdentityPlus, 3, 3> 531d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling parameterization; 541d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 551d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling double x[3] = {1.0, 2.0, 3.0}; 561d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling double delta[3] = {0.0, 1.0, 2.0}; 571d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling double x_plus_delta[3] = {0.0, 0.0, 0.0}; 581d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling parameterization.Plus(x, delta, x_plus_delta); 591d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 601d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_EQ(x_plus_delta[0], 1.0); 611d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_EQ(x_plus_delta[1], 3.0); 621d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_EQ(x_plus_delta[2], 5.0); 631d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 641d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling double jacobian[9]; 651d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling parameterization.ComputeJacobian(x, jacobian); 661d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling int k = 0; 671d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling for (int i = 0; i < 3; ++i) { 681d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling for (int j = 0; j < 3; ++j, ++k) { 691d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_EQ(jacobian[k], (i == j) ? 1.0 : 0.0); 701d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 711d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 721d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling} 731d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 7479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandezstruct ScaledPlus { 7579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez ScaledPlus(const double &scale_factor) 7679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez : scale_factor_(scale_factor) 7779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez {} 7879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 7979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez template <typename T> 8079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez bool operator()(const T* x, const T* delta, T* x_plus_delta) const { 8179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez for (int i = 0; i < 3; ++i) { 8279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez x_plus_delta[i] = x[i] + T(scale_factor_) * delta[i]; 8379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez } 8479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez return true; 8579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez } 8679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 8779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez const double scale_factor_; 8879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez}; 8979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 9079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos HernandezTEST(AutoDiffLocalParameterizationTest, ScaledParameterization) { 9179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez const double kTolerance = 1e-14; 9279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 9379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez AutoDiffLocalParameterization<ScaledPlus, 3, 3> 9479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez parameterization(new ScaledPlus(1.2345)); 9579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 9679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez double x[3] = {1.0, 2.0, 3.0}; 9779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez double delta[3] = {0.0, 1.0, 2.0}; 9879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez double x_plus_delta[3] = {0.0, 0.0, 0.0}; 9979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez parameterization.Plus(x, delta, x_plus_delta); 10079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 10179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez EXPECT_NEAR(x_plus_delta[0], 1.0, kTolerance); 10279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez EXPECT_NEAR(x_plus_delta[1], 3.2345, kTolerance); 10379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez EXPECT_NEAR(x_plus_delta[2], 5.469, kTolerance); 10479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 10579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez double jacobian[9]; 10679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez parameterization.ComputeJacobian(x, jacobian); 10779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez int k = 0; 10879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez for (int i = 0; i < 3; ++i) { 10979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez for (int j = 0; j < 3; ++j, ++k) { 11079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez EXPECT_NEAR(jacobian[k], (i == j) ? 1.2345 : 0.0, kTolerance); 11179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez } 11279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez } 11379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez} 11479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 1151d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingstruct QuaternionPlus { 1161d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling template<typename T> 1171d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling bool operator()(const T* x, const T* delta, T* x_plus_delta) const { 1181d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const T squared_norm_delta = 1191d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling delta[0] * delta[0] + delta[1] * delta[1] + delta[2] * delta[2]; 1201d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1211d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling T q_delta[4]; 1221d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling if (squared_norm_delta > T(0.0)) { 1231d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling T norm_delta = sqrt(squared_norm_delta); 1241d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const T sin_delta_by_delta = sin(norm_delta) / norm_delta; 1251d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling q_delta[0] = cos(norm_delta); 1261d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling q_delta[1] = sin_delta_by_delta * delta[0]; 1271d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling q_delta[2] = sin_delta_by_delta * delta[1]; 1281d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling q_delta[3] = sin_delta_by_delta * delta[2]; 1291d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } else { 1301d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // We do not just use q_delta = [1,0,0,0] here because that is a 1311d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // constant and when used for automatic differentiation will 1321d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // lead to a zero derivative. Instead we take a first order 1331d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // approximation and evaluate it at zero. 1341d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling q_delta[0] = T(1.0); 1351d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling q_delta[1] = delta[0]; 1361d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling q_delta[2] = delta[1]; 1371d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling q_delta[3] = delta[2]; 1381d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 1391d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1401d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling QuaternionProduct(q_delta, x, x_plus_delta); 1411d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling return true; 1421d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 1431d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling}; 1441d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1451d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingvoid QuaternionParameterizationTestHelper(const double* x, 1461d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const double* delta) { 1471d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const double kTolerance = 1e-14; 1481d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling double x_plus_delta_ref[4] = {0.0, 0.0, 0.0, 0.0}; 1491d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling double jacobian_ref[12]; 1501d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1511d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1521d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling QuaternionParameterization ref_parameterization; 1531d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ref_parameterization.Plus(x, delta, x_plus_delta_ref); 1541d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ref_parameterization.ComputeJacobian(x, jacobian_ref); 1551d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1561d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling double x_plus_delta[4] = {0.0, 0.0, 0.0, 0.0}; 1571d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling double jacobian[12]; 1581d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling AutoDiffLocalParameterization<QuaternionPlus, 4, 3> parameterization; 1591d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling parameterization.Plus(x, delta, x_plus_delta); 1601d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling parameterization.ComputeJacobian(x, jacobian); 1611d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1621d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling for (int i = 0; i < 4; ++i) { 1631d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_NEAR(x_plus_delta[i], x_plus_delta_ref[i], kTolerance); 1641d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 1651d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1661d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const double x_plus_delta_norm = 1671d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling sqrt(x_plus_delta[0] * x_plus_delta[0] + 1681d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling x_plus_delta[1] * x_plus_delta[1] + 1691d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling x_plus_delta[2] * x_plus_delta[2] + 1701d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling x_plus_delta[3] * x_plus_delta[3]); 1711d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1721d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_NEAR(x_plus_delta_norm, 1.0, kTolerance); 1731d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1741d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling for (int i = 0; i < 12; ++i) { 1751d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_TRUE(IsFinite(jacobian[i])); 1761d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_NEAR(jacobian[i], jacobian_ref[i], kTolerance) 1771d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling << "Jacobian mismatch: i = " << i 1781d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling << "\n Expected \n" << ConstMatrixRef(jacobian_ref, 4, 3) 1791d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling << "\n Actual \n" << ConstMatrixRef(jacobian, 4, 3); 1801d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 1811d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling} 1821d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1831d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha HaeberlingTEST(AutoDiffLocalParameterization, QuaternionParameterizationZeroTest) { 1841d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling double x[4] = {0.5, 0.5, 0.5, 0.5}; 1851d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling double delta[3] = {0.0, 0.0, 0.0}; 1861d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling QuaternionParameterizationTestHelper(x, delta); 1871d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling} 1881d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1891d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1901d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha HaeberlingTEST(AutoDiffLocalParameterization, QuaternionParameterizationNearZeroTest) { 1911d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling double x[4] = {0.52, 0.25, 0.15, 0.45}; 1921d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling double norm_x = sqrt(x[0] * x[0] + 1931d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling x[1] * x[1] + 1941d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling x[2] * x[2] + 1951d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling x[3] * x[3]); 1961d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling for (int i = 0; i < 4; ++i) { 1971d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling x[i] = x[i] / norm_x; 1981d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 1991d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 2001d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling double delta[3] = {0.24, 0.15, 0.10}; 2011d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling for (int i = 0; i < 3; ++i) { 2021d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling delta[i] = delta[i] * 1e-14; 2031d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 2041d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 2051d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling QuaternionParameterizationTestHelper(x, delta); 2061d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling} 2071d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 2081d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha HaeberlingTEST(AutoDiffLocalParameterization, QuaternionParameterizationNonZeroTest) { 2091d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling double x[4] = {0.52, 0.25, 0.15, 0.45}; 2101d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling double norm_x = sqrt(x[0] * x[0] + 2111d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling x[1] * x[1] + 2121d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling x[2] * x[2] + 2131d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling x[3] * x[3]); 2141d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 2151d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling for (int i = 0; i < 4; ++i) { 2161d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling x[i] = x[i] / norm_x; 2171d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 2181d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 2191d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling double delta[3] = {0.24, 0.15, 0.10}; 2201d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling QuaternionParameterizationTestHelper(x, delta); 2211d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling} 2221d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 2231d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling} // namespace internal 2241d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling} // namespace ceres 225