10ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Ceres Solver - A fast non-linear least squares minimizer 20ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. 30ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// http://code.google.com/p/ceres-solver/ 40ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 50ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Redistribution and use in source and binary forms, with or without 60ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// modification, are permitted provided that the following conditions are met: 70ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 80ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// * Redistributions of source code must retain the above copyright notice, 90ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// this list of conditions and the following disclaimer. 100ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// * Redistributions in binary form must reproduce the above copyright notice, 110ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// this list of conditions and the following disclaimer in the documentation 120ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// and/or other materials provided with the distribution. 130ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// * Neither the name of Google Inc. nor the names of its contributors may be 140ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// used to endorse or promote products derived from this software without 150ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// specific prior written permission. 160ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 170ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 180ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 190ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 200ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 210ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 220ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 230ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 240ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 250ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 260ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 270ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// POSSIBILITY OF SUCH DAMAGE. 280ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 290ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Author: sameeragarwal@google.com (Sameer Agarwal) 300ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 310ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include <cmath> 320ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include <limits> 330ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include <string> 340ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/internal/eigen.h" 350ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/internal/port.h" 360ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/jet.h" 370ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/rotation.h" 380ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/stringprintf.h" 390ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/test_util.h" 400ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "glog/logging.h" 410ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "gmock/gmock.h" 420ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "gtest/gtest.h" 430ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 440ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongnamespace ceres { 450ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongnamespace internal { 460ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 470ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongconst double kPi = 3.14159265358979323846; 480ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongconst double kHalfSqrt2 = 0.707106781186547524401; 490ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 500ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongdouble RandDouble() { 510ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double r = rand(); 520ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return r / RAND_MAX; 530ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 540ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 550ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// A tolerance value for floating-point comparisons. 560ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongstatic double const kTolerance = numeric_limits<double>::epsilon() * 10; 570ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 581d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// Looser tolerance used for numerically unstable conversions. 590ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongstatic double const kLooseTolerance = 1e-9; 600ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 610ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Use as: 620ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// double quaternion[4]; 630ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// EXPECT_THAT(quaternion, IsNormalizedQuaternion()); 640ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus KongMATCHER(IsNormalizedQuaternion, "") { 650ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong if (arg == NULL) { 660ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong *result_listener << "Null quaternion"; 670ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return false; 680ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 690ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 700ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double norm2 = arg[0] * arg[0] + arg[1] * arg[1] + 710ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong arg[2] * arg[2] + arg[3] * arg[3]; 720ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong if (fabs(norm2 - 1.0) > kTolerance) { 730ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong *result_listener << "squared norm is " << norm2; 740ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return false; 750ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 760ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 770ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return true; 780ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 790ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 800ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Use as: 810ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// double expected_quaternion[4]; 820ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// double actual_quaternion[4]; 830ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// EXPECT_THAT(actual_quaternion, IsNearQuaternion(expected_quaternion)); 840ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus KongMATCHER_P(IsNearQuaternion, expected, "") { 850ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong if (arg == NULL) { 860ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong *result_listener << "Null quaternion"; 870ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return false; 880ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 890ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 900ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Quaternions are equivalent upto a sign change. So we will compare 910ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // both signs before declaring failure. 920ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong bool near = true; 930ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int i = 0; i < 4; i++) { 940ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong if (fabs(arg[i] - expected[i]) > kTolerance) { 950ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong near = false; 960ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong break; 970ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 980ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 990ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1000ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong if (near) { 1010ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return true; 1020ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1030ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1040ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong near = true; 1050ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int i = 0; i < 4; i++) { 1060ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong if (fabs(arg[i] + expected[i]) > kTolerance) { 1070ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong near = false; 1080ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong break; 1090ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1100ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1110ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1120ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong if (near) { 1130ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return true; 1140ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1150ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1160ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong *result_listener << "expected : " 1170ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong << expected[0] << " " 1180ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong << expected[1] << " " 1190ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong << expected[2] << " " 1200ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong << expected[3] << " " 1210ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong << "actual : " 1220ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong << arg[0] << " " 1230ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong << arg[1] << " " 1240ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong << arg[2] << " " 1250ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong << arg[3]; 1260ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return false; 1270ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 1280ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1290ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Use as: 1300ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// double expected_axis_angle[4]; 1310ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// double actual_axis_angle[4]; 1320ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// EXPECT_THAT(actual_axis_angle, IsNearAngleAxis(expected_axis_angle)); 1330ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus KongMATCHER_P(IsNearAngleAxis, expected, "") { 1340ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong if (arg == NULL) { 1350ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong *result_listener << "Null axis/angle"; 1360ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return false; 1370ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1380ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1390ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int i = 0; i < 3; i++) { 1400ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong if (fabs(arg[i] - expected[i]) > kTolerance) { 1410ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong *result_listener << "component " << i << " should be " << expected[i]; 1420ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return false; 1430ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1440ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1450ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1460ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return true; 1470ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 1480ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1490ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Use as: 1500ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// double matrix[9]; 1510ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// EXPECT_THAT(matrix, IsOrthonormal()); 1520ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus KongMATCHER(IsOrthonormal, "") { 1530ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong if (arg == NULL) { 1540ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong *result_listener << "Null matrix"; 1550ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return false; 1560ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1570ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1580ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int c1 = 0; c1 < 3; c1++) { 1590ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int c2 = 0; c2 < 3; c2++) { 1600ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double v = 0; 1610ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int i = 0; i < 3; i++) { 1620ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong v += arg[i + 3 * c1] * arg[i + 3 * c2]; 1630ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1640ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double expected = (c1 == c2) ? 1 : 0; 1650ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong if (fabs(expected - v) > kTolerance) { 1660ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong *result_listener << "Columns " << c1 << " and " << c2 1670ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong << " should have dot product " << expected 1680ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong << " but have " << v; 1690ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return false; 1700ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1710ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1720ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1730ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1740ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return true; 1750ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 1760ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1770ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Use as: 1780ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// double matrix1[9]; 1790ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// double matrix2[9]; 1800ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// EXPECT_THAT(matrix1, IsNear3x3Matrix(matrix2)); 1810ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus KongMATCHER_P(IsNear3x3Matrix, expected, "") { 1820ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong if (arg == NULL) { 1830ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong *result_listener << "Null matrix"; 1840ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return false; 1850ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1860ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1870ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int i = 0; i < 9; i++) { 1880ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong if (fabs(arg[i] - expected[i]) > kTolerance) { 1890ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong *result_listener << "component " << i << " should be " << expected[i]; 1900ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return false; 1910ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1920ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1930ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1940ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return true; 1950ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 1960ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1970ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Transforms a zero axis/angle to a quaternion. 1980ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus KongTEST(Rotation, ZeroAngleAxisToQuaternion) { 1990ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double axis_angle[3] = { 0, 0, 0 }; 2000ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double quaternion[4]; 2010ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double expected[4] = { 1, 0, 0, 0 }; 2020ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong AngleAxisToQuaternion(axis_angle, quaternion); 2030ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong EXPECT_THAT(quaternion, IsNormalizedQuaternion()); 2040ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong EXPECT_THAT(quaternion, IsNearQuaternion(expected)); 2050ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 2060ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2070ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Test that exact conversion works for small angles. 2080ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus KongTEST(Rotation, SmallAngleAxisToQuaternion) { 2090ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Small, finite value to test. 2100ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double theta = 1.0e-2; 2110ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double axis_angle[3] = { theta, 0, 0 }; 2120ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double quaternion[4]; 2130ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double expected[4] = { cos(theta/2), sin(theta/2.0), 0, 0 }; 2140ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong AngleAxisToQuaternion(axis_angle, quaternion); 2150ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong EXPECT_THAT(quaternion, IsNormalizedQuaternion()); 2160ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong EXPECT_THAT(quaternion, IsNearQuaternion(expected)); 2170ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 2180ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2190ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Test that approximate conversion works for very small angles. 2200ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus KongTEST(Rotation, TinyAngleAxisToQuaternion) { 2210ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Very small value that could potentially cause underflow. 2220ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double theta = pow(numeric_limits<double>::min(), 0.75); 2230ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double axis_angle[3] = { theta, 0, 0 }; 2240ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double quaternion[4]; 2250ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double expected[4] = { cos(theta/2), sin(theta/2.0), 0, 0 }; 2260ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong AngleAxisToQuaternion(axis_angle, quaternion); 2270ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong EXPECT_THAT(quaternion, IsNormalizedQuaternion()); 2280ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong EXPECT_THAT(quaternion, IsNearQuaternion(expected)); 2290ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 2300ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2310ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Transforms a rotation by pi/2 around X to a quaternion. 2320ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus KongTEST(Rotation, XRotationToQuaternion) { 2330ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double axis_angle[3] = { kPi / 2, 0, 0 }; 2340ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double quaternion[4]; 2350ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double expected[4] = { kHalfSqrt2, kHalfSqrt2, 0, 0 }; 2360ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong AngleAxisToQuaternion(axis_angle, quaternion); 2370ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong EXPECT_THAT(quaternion, IsNormalizedQuaternion()); 2380ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong EXPECT_THAT(quaternion, IsNearQuaternion(expected)); 2390ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 2400ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2410ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Transforms a unit quaternion to an axis angle. 2420ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus KongTEST(Rotation, UnitQuaternionToAngleAxis) { 2430ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double quaternion[4] = { 1, 0, 0, 0 }; 2440ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double axis_angle[3]; 2450ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double expected[3] = { 0, 0, 0 }; 2460ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong QuaternionToAngleAxis(quaternion, axis_angle); 2470ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong EXPECT_THAT(axis_angle, IsNearAngleAxis(expected)); 2480ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 2490ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2500ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Transforms a quaternion that rotates by pi about the Y axis to an axis angle. 2510ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus KongTEST(Rotation, YRotationQuaternionToAngleAxis) { 2520ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double quaternion[4] = { 0, 0, 1, 0 }; 2530ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double axis_angle[3]; 2540ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double expected[3] = { 0, kPi, 0 }; 2550ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong QuaternionToAngleAxis(quaternion, axis_angle); 2560ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong EXPECT_THAT(axis_angle, IsNearAngleAxis(expected)); 2570ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 2580ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2590ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Transforms a quaternion that rotates by pi/3 about the Z axis to an axis 2600ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// angle. 2610ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus KongTEST(Rotation, ZRotationQuaternionToAngleAxis) { 2620ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double quaternion[4] = { sqrt(3) / 2, 0, 0, 0.5 }; 2630ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double axis_angle[3]; 2640ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double expected[3] = { 0, 0, kPi / 3 }; 2650ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong QuaternionToAngleAxis(quaternion, axis_angle); 2660ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong EXPECT_THAT(axis_angle, IsNearAngleAxis(expected)); 2670ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 2680ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2690ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Test that exact conversion works for small angles. 2700ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus KongTEST(Rotation, SmallQuaternionToAngleAxis) { 2710ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Small, finite value to test. 2720ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double theta = 1.0e-2; 2730ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double quaternion[4] = { cos(theta/2), sin(theta/2.0), 0, 0 }; 2740ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double axis_angle[3]; 2750ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double expected[3] = { theta, 0, 0 }; 2760ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong QuaternionToAngleAxis(quaternion, axis_angle); 2770ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong EXPECT_THAT(axis_angle, IsNearAngleAxis(expected)); 2780ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 2790ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2800ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Test that approximate conversion works for very small angles. 2810ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus KongTEST(Rotation, TinyQuaternionToAngleAxis) { 2820ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Very small value that could potentially cause underflow. 2830ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double theta = pow(numeric_limits<double>::min(), 0.75); 2840ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double quaternion[4] = { cos(theta/2), sin(theta/2.0), 0, 0 }; 2850ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double axis_angle[3]; 2860ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double expected[3] = { theta, 0, 0 }; 2870ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong QuaternionToAngleAxis(quaternion, axis_angle); 2880ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong EXPECT_THAT(axis_angle, IsNearAngleAxis(expected)); 2890ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 2900ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2910ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus KongTEST(Rotation, QuaternionToAngleAxisAngleIsLessThanPi) { 2920ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double quaternion[4]; 2930ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double angle_axis[3]; 2940ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2950ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const double half_theta = 0.75 * kPi; 2960ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2970ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong quaternion[0] = cos(half_theta); 2980ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong quaternion[1] = 1.0 * sin(half_theta); 2990ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong quaternion[2] = 0.0; 3000ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong quaternion[3] = 0.0; 3010ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong QuaternionToAngleAxis(quaternion, angle_axis); 3020ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const double angle = sqrt(angle_axis[0] * angle_axis[0] + 3030ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong angle_axis[1] * angle_axis[1] + 3040ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong angle_axis[2] * angle_axis[2]); 3050ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong EXPECT_LE(angle, kPi); 3060ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 3070ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 3080ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongstatic const int kNumTrials = 10000; 3090ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 3100ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Takes a bunch of random axis/angle values, converts them to quaternions, 3110ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// and back again. 3120ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus KongTEST(Rotation, AngleAxisToQuaterionAndBack) { 3130ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong srand(5); 3140ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int i = 0; i < kNumTrials; i++) { 3150ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double axis_angle[3]; 3160ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Make an axis by choosing three random numbers in [-1, 1) and 3170ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // normalizing. 3180ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double norm = 0; 3190ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int i = 0; i < 3; i++) { 3200ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong axis_angle[i] = RandDouble() * 2 - 1; 3210ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong norm += axis_angle[i] * axis_angle[i]; 3220ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 3230ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong norm = sqrt(norm); 3240ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 3250ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Angle in [-pi, pi). 3260ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double theta = kPi * 2 * RandDouble() - kPi; 3270ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int i = 0; i < 3; i++) { 3280ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong axis_angle[i] = axis_angle[i] * theta / norm; 3290ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 3300ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 3310ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double quaternion[4]; 3320ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double round_trip[3]; 3330ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // We use ASSERTs here because if there's one failure, there are 3340ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // probably many and spewing a million failures doesn't make anyone's 3350ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // day. 3360ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong AngleAxisToQuaternion(axis_angle, quaternion); 3370ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong ASSERT_THAT(quaternion, IsNormalizedQuaternion()); 3380ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong QuaternionToAngleAxis(quaternion, round_trip); 3390ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong ASSERT_THAT(round_trip, IsNearAngleAxis(axis_angle)); 3400ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 3410ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 3420ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 3430ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Takes a bunch of random quaternions, converts them to axis/angle, 3440ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// and back again. 3450ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus KongTEST(Rotation, QuaterionToAngleAxisAndBack) { 3460ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong srand(5); 3470ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int i = 0; i < kNumTrials; i++) { 3480ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double quaternion[4]; 3490ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Choose four random numbers in [-1, 1) and normalize. 3500ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double norm = 0; 3510ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int i = 0; i < 4; i++) { 3520ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong quaternion[i] = RandDouble() * 2 - 1; 3530ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong norm += quaternion[i] * quaternion[i]; 3540ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 3550ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong norm = sqrt(norm); 3560ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 3570ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int i = 0; i < 4; i++) { 3580ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong quaternion[i] = quaternion[i] / norm; 3590ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 3600ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 3610ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double axis_angle[3]; 3620ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double round_trip[4]; 3630ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong QuaternionToAngleAxis(quaternion, axis_angle); 3640ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong AngleAxisToQuaternion(axis_angle, round_trip); 3650ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong ASSERT_THAT(round_trip, IsNormalizedQuaternion()); 3660ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong ASSERT_THAT(round_trip, IsNearQuaternion(quaternion)); 3670ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 3680ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 3690ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 3700ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Transforms a zero axis/angle to a rotation matrix. 3710ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus KongTEST(Rotation, ZeroAngleAxisToRotationMatrix) { 3720ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double axis_angle[3] = { 0, 0, 0 }; 3730ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double matrix[9]; 3740ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double expected[9] = { 1, 0, 0, 0, 1, 0, 0, 0, 1 }; 3750ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong AngleAxisToRotationMatrix(axis_angle, matrix); 3760ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong EXPECT_THAT(matrix, IsOrthonormal()); 3770ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong EXPECT_THAT(matrix, IsNear3x3Matrix(expected)); 3780ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 3790ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 3800ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus KongTEST(Rotation, NearZeroAngleAxisToRotationMatrix) { 3810ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double axis_angle[3] = { 1e-24, 2e-24, 3e-24 }; 3820ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double matrix[9]; 3830ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double expected[9] = { 1, 0, 0, 0, 1, 0, 0, 0, 1 }; 3840ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong AngleAxisToRotationMatrix(axis_angle, matrix); 3850ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong EXPECT_THAT(matrix, IsOrthonormal()); 3860ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong EXPECT_THAT(matrix, IsNear3x3Matrix(expected)); 3870ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 3880ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 3890ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Transforms a rotation by pi/2 around X to a rotation matrix and back. 3900ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus KongTEST(Rotation, XRotationToRotationMatrix) { 3910ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double axis_angle[3] = { kPi / 2, 0, 0 }; 3920ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double matrix[9]; 3930ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // The rotation matrices are stored column-major. 3940ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double expected[9] = { 1, 0, 0, 0, 0, 1, 0, -1, 0 }; 3950ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong AngleAxisToRotationMatrix(axis_angle, matrix); 3960ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong EXPECT_THAT(matrix, IsOrthonormal()); 3970ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong EXPECT_THAT(matrix, IsNear3x3Matrix(expected)); 3980ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double round_trip[3]; 3990ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong RotationMatrixToAngleAxis(matrix, round_trip); 4000ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong EXPECT_THAT(round_trip, IsNearAngleAxis(axis_angle)); 4010ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 4020ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 4030ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Transforms an axis angle that rotates by pi about the Y axis to a 4040ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// rotation matrix and back. 4050ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus KongTEST(Rotation, YRotationToRotationMatrix) { 4060ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double axis_angle[3] = { 0, kPi, 0 }; 4070ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double matrix[9]; 4080ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double expected[9] = { -1, 0, 0, 0, 1, 0, 0, 0, -1 }; 4090ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong AngleAxisToRotationMatrix(axis_angle, matrix); 4100ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong EXPECT_THAT(matrix, IsOrthonormal()); 4110ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong EXPECT_THAT(matrix, IsNear3x3Matrix(expected)); 4120ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 4130ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double round_trip[3]; 4140ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong RotationMatrixToAngleAxis(matrix, round_trip); 4150ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong EXPECT_THAT(round_trip, IsNearAngleAxis(axis_angle)); 4160ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 4170ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 4180ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus KongTEST(Rotation, NearPiAngleAxisRoundTrip) { 4190ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double in_axis_angle[3]; 4200ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double matrix[9]; 4210ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double out_axis_angle[3]; 4220ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 4230ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong srand(5); 4240ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int i = 0; i < kNumTrials; i++) { 4250ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Make an axis by choosing three random numbers in [-1, 1) and 4260ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // normalizing. 4270ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double norm = 0; 4280ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int i = 0; i < 3; i++) { 4290ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong in_axis_angle[i] = RandDouble() * 2 - 1; 4300ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong norm += in_axis_angle[i] * in_axis_angle[i]; 4310ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 4320ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong norm = sqrt(norm); 4330ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 4340ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Angle in [pi - kMaxSmallAngle, pi). 4350ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const double kMaxSmallAngle = 1e-2; 4360ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double theta = kPi - kMaxSmallAngle * RandDouble(); 4370ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 4380ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int i = 0; i < 3; i++) { 4390ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong in_axis_angle[i] *= (theta / norm); 4400ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 4410ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong AngleAxisToRotationMatrix(in_axis_angle, matrix); 4420ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong RotationMatrixToAngleAxis(matrix, out_axis_angle); 4430ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 4440ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int i = 0; i < 3; ++i) { 4450ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong EXPECT_NEAR(out_axis_angle[i], in_axis_angle[i], kLooseTolerance); 4460ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 4470ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 4480ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 4490ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 4500ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus KongTEST(Rotation, AtPiAngleAxisRoundTrip) { 4510ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // A rotation of kPi about the X axis; 4520ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong static const double kMatrix[3][3] = { 4530ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong {1.0, 0.0, 0.0}, 4540ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong {0.0, -1.0, 0.0}, 4550ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong {0.0, 0.0, -1.0} 4560ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong }; 4570ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 4580ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double in_matrix[9]; 4590ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Fill it from kMatrix in col-major order. 4600ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int j = 0, k = 0; j < 3; ++j) { 4610ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int i = 0; i < 3; ++i, ++k) { 4620ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong in_matrix[k] = kMatrix[i][j]; 4630ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 4640ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 4650ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 4660ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const double expected_axis_angle[3] = { kPi, 0, 0 }; 4670ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 4680ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double out_matrix[9]; 4690ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double axis_angle[3]; 4700ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong RotationMatrixToAngleAxis(in_matrix, axis_angle); 4710ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong AngleAxisToRotationMatrix(axis_angle, out_matrix); 4720ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 4730ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong LOG(INFO) << "AngleAxis = " << axis_angle[0] << " " << axis_angle[1] 4740ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong << " " << axis_angle[2]; 4750ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong LOG(INFO) << "Expected AngleAxis = " << kPi << " 0 0"; 4760ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double out_rowmajor[3][3]; 4770ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int j = 0, k = 0; j < 3; ++j) { 4780ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int i = 0; i < 3; ++i, ++k) { 4790ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong out_rowmajor[i][j] = out_matrix[k]; 4800ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 4810ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 4820ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong LOG(INFO) << "Rotation:"; 4830ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong LOG(INFO) << "EXPECTED | ACTUAL"; 4840ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int i = 0; i < 3; ++i) { 4850ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong string line; 4860ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int j = 0; j < 3; ++j) { 4870ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong StringAppendF(&line, "%g ", kMatrix[i][j]); 4880ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 4890ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong line += " | "; 4900ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int j = 0; j < 3; ++j) { 4910ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong StringAppendF(&line, "%g ", out_rowmajor[i][j]); 4920ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 4930ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong LOG(INFO) << line; 4940ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 4950ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 4960ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong EXPECT_THAT(axis_angle, IsNearAngleAxis(expected_axis_angle)); 4970ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong EXPECT_THAT(out_matrix, IsNear3x3Matrix(in_matrix)); 4980ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 4990ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 5000ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Transforms an axis angle that rotates by pi/3 about the Z axis to a 5010ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// rotation matrix. 5020ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus KongTEST(Rotation, ZRotationToRotationMatrix) { 5030ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double axis_angle[3] = { 0, 0, kPi / 3 }; 5040ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double matrix[9]; 5050ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // This is laid-out row-major on the screen but is actually stored 5060ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // column-major. 5070ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double expected[9] = { 0.5, sqrt(3) / 2, 0, // Column 1 5080ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong -sqrt(3) / 2, 0.5, 0, // Column 2 5090ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 0, 0, 1 }; // Column 3 5100ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong AngleAxisToRotationMatrix(axis_angle, matrix); 5110ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong EXPECT_THAT(matrix, IsOrthonormal()); 5120ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong EXPECT_THAT(matrix, IsNear3x3Matrix(expected)); 5130ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double round_trip[3]; 5140ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong RotationMatrixToAngleAxis(matrix, round_trip); 5150ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong EXPECT_THAT(round_trip, IsNearAngleAxis(axis_angle)); 5160ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 5170ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 5180ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Takes a bunch of random axis/angle values, converts them to rotation 5190ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// matrices, and back again. 5200ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus KongTEST(Rotation, AngleAxisToRotationMatrixAndBack) { 5210ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong srand(5); 5220ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int i = 0; i < kNumTrials; i++) { 5230ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double axis_angle[3]; 5240ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Make an axis by choosing three random numbers in [-1, 1) and 5250ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // normalizing. 5260ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double norm = 0; 5270ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int i = 0; i < 3; i++) { 5280ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong axis_angle[i] = RandDouble() * 2 - 1; 5290ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong norm += axis_angle[i] * axis_angle[i]; 5300ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 5310ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong norm = sqrt(norm); 5320ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 5330ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Angle in [-pi, pi). 5340ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double theta = kPi * 2 * RandDouble() - kPi; 5350ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int i = 0; i < 3; i++) { 5360ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong axis_angle[i] = axis_angle[i] * theta / norm; 5370ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 5380ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 5390ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double matrix[9]; 5400ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double round_trip[3]; 5410ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong AngleAxisToRotationMatrix(axis_angle, matrix); 5420ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong ASSERT_THAT(matrix, IsOrthonormal()); 5430ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong RotationMatrixToAngleAxis(matrix, round_trip); 5440ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 5450ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int i = 0; i < 3; ++i) { 5460ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong EXPECT_NEAR(round_trip[i], axis_angle[i], kLooseTolerance); 5470ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 5480ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 5490ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 5500ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 55179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// Takes a bunch of random axis/angle values near zero, converts them 55279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// to rotation matrices, and back again. 55379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos HernandezTEST(Rotation, AngleAxisToRotationMatrixAndBackNearZero) { 55479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez srand(5); 55579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez for (int i = 0; i < kNumTrials; i++) { 55679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez double axis_angle[3]; 55779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // Make an axis by choosing three random numbers in [-1, 1) and 55879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // normalizing. 55979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez double norm = 0; 56079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez for (int i = 0; i < 3; i++) { 56179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez axis_angle[i] = RandDouble() * 2 - 1; 56279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez norm += axis_angle[i] * axis_angle[i]; 56379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez } 56479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez norm = sqrt(norm); 56579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 56679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // Tiny theta. 56779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez double theta = 1e-16 * (kPi * 2 * RandDouble() - kPi); 56879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez for (int i = 0; i < 3; i++) { 56979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez axis_angle[i] = axis_angle[i] * theta / norm; 57079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez } 57179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 57279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez double matrix[9]; 57379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez double round_trip[3]; 57479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez AngleAxisToRotationMatrix(axis_angle, matrix); 57579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez ASSERT_THAT(matrix, IsOrthonormal()); 57679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez RotationMatrixToAngleAxis(matrix, round_trip); 57779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 57879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez for (int i = 0; i < 3; ++i) { 57979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez EXPECT_NEAR(round_trip[i], axis_angle[i], 58079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez std::numeric_limits<double>::epsilon()); 58179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez } 58279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez } 58379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez} 58479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 58579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 5860ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Transposes a 3x3 matrix. 5870ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongstatic void Transpose3x3(double m[9]) { 5880ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong std::swap(m[1], m[3]); 5890ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong std::swap(m[2], m[6]); 5900ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong std::swap(m[5], m[7]); 5910ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 5920ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 5930ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Convert Euler angles from radians to degrees. 5940ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongstatic void ToDegrees(double ea[3]) { 5950ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int i = 0; i < 3; ++i) 5960ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong ea[i] *= 180.0 / kPi; 5970ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 5980ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 5990ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Compare the 3x3 rotation matrices produced by the axis-angle 6000ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// rotation 'aa' and the Euler angle rotation 'ea' (in radians). 6010ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongstatic void CompareEulerToAngleAxis(double aa[3], double ea[3]) { 6020ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double aa_matrix[9]; 6030ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong AngleAxisToRotationMatrix(aa, aa_matrix); 6040ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong Transpose3x3(aa_matrix); // Column to row major order. 6050ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 6060ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double ea_matrix[9]; 6070ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong ToDegrees(ea); // Radians to degrees. 6080ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const int kRowStride = 3; 6090ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong EulerAnglesToRotationMatrix(ea, kRowStride, ea_matrix); 6100ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 6110ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong EXPECT_THAT(aa_matrix, IsOrthonormal()); 6120ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong EXPECT_THAT(ea_matrix, IsOrthonormal()); 6130ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong EXPECT_THAT(ea_matrix, IsNear3x3Matrix(aa_matrix)); 6140ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 6150ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 6160ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Test with rotation axis along the x/y/z axes. 6170ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Also test zero rotation. 6180ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus KongTEST(EulerAnglesToRotationMatrix, OnAxis) { 6190ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong int n_tests = 0; 6200ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (double x = -1.0; x <= 1.0; x += 1.0) { 6210ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (double y = -1.0; y <= 1.0; y += 1.0) { 6220ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (double z = -1.0; z <= 1.0; z += 1.0) { 6230ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong if ((x != 0) + (y != 0) + (z != 0) > 1) 6240ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong continue; 6250ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double axis_angle[3] = {x, y, z}; 6260ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double euler_angles[3] = {x, y, z}; 6270ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong CompareEulerToAngleAxis(axis_angle, euler_angles); 6280ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong ++n_tests; 6290ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 6300ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 6310ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 6320ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong CHECK_EQ(7, n_tests); 6330ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 6340ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 6350ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Test that a random rotation produces an orthonormal rotation 6360ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// matrix. 6370ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus KongTEST(EulerAnglesToRotationMatrix, IsOrthonormal) { 6380ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong srand(5); 6390ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int trial = 0; trial < kNumTrials; ++trial) { 6400ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double ea[3]; 6410ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int i = 0; i < 3; ++i) 6420ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong ea[i] = 360.0 * (RandDouble() * 2.0 - 1.0); 6430ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double ea_matrix[9]; 6440ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong ToDegrees(ea); // Radians to degrees. 6450ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong EulerAnglesToRotationMatrix(ea, 3, ea_matrix); 6460ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong EXPECT_THAT(ea_matrix, IsOrthonormal()); 6470ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 6480ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 6490ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 6500ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Tests using Jets for specific behavior involving auto differentiation 6510ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// near singularity points. 6520ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 6530ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongtypedef Jet<double, 3> J3; 6540ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongtypedef Jet<double, 4> J4; 6550ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 6560ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus KongJ3 MakeJ3(double a, double v0, double v1, double v2) { 6570ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong J3 j; 6580ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong j.a = a; 6590ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong j.v[0] = v0; 6600ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong j.v[1] = v1; 6610ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong j.v[2] = v2; 6620ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return j; 6630ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 6640ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 6650ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus KongJ4 MakeJ4(double a, double v0, double v1, double v2, double v3) { 6660ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong J4 j; 6670ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong j.a = a; 6680ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong j.v[0] = v0; 6690ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong j.v[1] = v1; 6700ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong j.v[2] = v2; 6710ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong j.v[3] = v3; 6720ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return j; 6730ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 6740ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 6750ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 6760ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongbool IsClose(double x, double y) { 6770ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong EXPECT_FALSE(IsNaN(x)); 6780ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong EXPECT_FALSE(IsNaN(y)); 6790ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double absdiff = fabs(x - y); 6800ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong if (x == 0 || y == 0) { 6810ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return absdiff <= kTolerance; 6820ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 6830ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double reldiff = absdiff / max(fabs(x), fabs(y)); 6840ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return reldiff <= kTolerance; 6850ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 6860ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 6870ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongtemplate <int N> 6880ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongbool IsClose(const Jet<double, N> &x, const Jet<double, N> &y) { 6890ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong if (IsClose(x.a, y.a)) { 6900ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int i = 0; i < N; i++) { 6910ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong if (!IsClose(x.v[i], y.v[i])) { 6920ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return false; 6930ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 6940ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 6950ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 6960ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return true; 6970ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 6980ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 6990ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongtemplate <int M, int N> 7000ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongvoid ExpectJetArraysClose(const Jet<double, N> *x, const Jet<double, N> *y) { 7010ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int i = 0; i < M; i++) { 7020ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong if (!IsClose(x[i], y[i])) { 7030ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong LOG(ERROR) << "Jet " << i << "/" << M << " not equal"; 7040ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong LOG(ERROR) << "x[" << i << "]: " << x[i]; 7050ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong LOG(ERROR) << "y[" << i << "]: " << y[i]; 7060ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong Jet<double, N> d, zero; 7070ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong d.a = y[i].a - x[i].a; 7080ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int j = 0; j < N; j++) { 7090ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong d.v[j] = y[i].v[j] - x[i].v[j]; 7100ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 7110ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong LOG(ERROR) << "diff: " << d; 7120ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong EXPECT_TRUE(IsClose(x[i], y[i])); 7130ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 7140ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 7150ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 7160ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 7170ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Log-10 of a value well below machine precision. 7180ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongstatic const int kSmallTinyCutoff = 7190ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong static_cast<int>(2 * log(numeric_limits<double>::epsilon())/log(10.0)); 7200ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 7210ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Log-10 of a value just below values representable by double. 7220ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongstatic const int kTinyZeroLimit = 7230ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong static_cast<int>(1 + log(numeric_limits<double>::min())/log(10.0)); 7240ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 7250ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Test that exact conversion works for small angles when jets are used. 7260ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus KongTEST(Rotation, SmallAngleAxisToQuaternionForJets) { 7270ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Examine small x rotations that are still large enough 7280ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // to be well within the range represented by doubles. 7290ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int i = -2; i >= kSmallTinyCutoff; i--) { 7300ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double theta = pow(10.0, i); 7310ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong J3 axis_angle[3] = { J3(theta, 0), J3(0, 1), J3(0, 2) }; 7320ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong J3 quaternion[4]; 7330ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong J3 expected[4] = { 7340ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong MakeJ3(cos(theta/2), -sin(theta/2)/2, 0, 0), 7350ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong MakeJ3(sin(theta/2), cos(theta/2)/2, 0, 0), 7360ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong MakeJ3(0, 0, sin(theta/2)/theta, 0), 7370ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong MakeJ3(0, 0, 0, sin(theta/2)/theta), 7380ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong }; 7390ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong AngleAxisToQuaternion(axis_angle, quaternion); 7400ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong ExpectJetArraysClose<4, 3>(quaternion, expected); 7410ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 7420ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 7430ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 7440ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 7450ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Test that conversion works for very small angles when jets are used. 7460ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus KongTEST(Rotation, TinyAngleAxisToQuaternionForJets) { 7470ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Examine tiny x rotations that extend all the way to where 7480ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // underflow occurs. 7490ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int i = kSmallTinyCutoff; i >= kTinyZeroLimit; i--) { 7500ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double theta = pow(10.0, i); 7510ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong J3 axis_angle[3] = { J3(theta, 0), J3(0, 1), J3(0, 2) }; 7520ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong J3 quaternion[4]; 7530ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // To avoid loss of precision in the test itself, 7540ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // a finite expansion is used here, which will 7550ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // be exact up to machine precision for the test values used. 7560ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong J3 expected[4] = { 7570ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong MakeJ3(1.0, 0, 0, 0), 7580ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong MakeJ3(0, 0.5, 0, 0), 7590ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong MakeJ3(0, 0, 0.5, 0), 7600ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong MakeJ3(0, 0, 0, 0.5), 7610ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong }; 7620ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong AngleAxisToQuaternion(axis_angle, quaternion); 7630ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong ExpectJetArraysClose<4, 3>(quaternion, expected); 7640ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 7650ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 7660ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 7670ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Test that derivatives are correct for zero rotation. 7680ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus KongTEST(Rotation, ZeroAngleAxisToQuaternionForJets) { 7690ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong J3 axis_angle[3] = { J3(0, 0), J3(0, 1), J3(0, 2) }; 7700ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong J3 quaternion[4]; 7710ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong J3 expected[4] = { 7720ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong MakeJ3(1.0, 0, 0, 0), 7730ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong MakeJ3(0, 0.5, 0, 0), 7740ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong MakeJ3(0, 0, 0.5, 0), 7750ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong MakeJ3(0, 0, 0, 0.5), 7760ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong }; 7770ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong AngleAxisToQuaternion(axis_angle, quaternion); 7780ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong ExpectJetArraysClose<4, 3>(quaternion, expected); 7790ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 7800ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 7810ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Test that exact conversion works for small angles. 7820ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus KongTEST(Rotation, SmallQuaternionToAngleAxisForJets) { 7830ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Examine small x rotations that are still large enough 7840ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // to be well within the range represented by doubles. 7850ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int i = -2; i >= kSmallTinyCutoff; i--) { 7860ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double theta = pow(10.0, i); 7870ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double s = sin(theta); 7880ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double c = cos(theta); 7890ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong J4 quaternion[4] = { J4(c, 0), J4(s, 1), J4(0, 2), J4(0, 3) }; 7900ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong J4 axis_angle[3]; 7910ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong J4 expected[3] = { 7920ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong MakeJ4(s, -2*theta, 2*theta*c, 0, 0), 7930ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong MakeJ4(0, 0, 0, 2*theta/s, 0), 7940ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong MakeJ4(0, 0, 0, 0, 2*theta/s), 7950ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong }; 7960ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong QuaternionToAngleAxis(quaternion, axis_angle); 7970ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong ExpectJetArraysClose<3, 4>(axis_angle, expected); 7980ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 7990ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 8000ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 8010ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Test that conversion works for very small angles. 8020ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus KongTEST(Rotation, TinyQuaternionToAngleAxisForJets) { 8030ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Examine tiny x rotations that extend all the way to where 8040ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // underflow occurs. 8050ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int i = kSmallTinyCutoff; i >= kTinyZeroLimit; i--) { 8060ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double theta = pow(10.0, i); 8070ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double s = sin(theta); 8080ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double c = cos(theta); 8090ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong J4 quaternion[4] = { J4(c, 0), J4(s, 1), J4(0, 2), J4(0, 3) }; 8100ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong J4 axis_angle[3]; 8110ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // To avoid loss of precision in the test itself, 8120ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // a finite expansion is used here, which will 8130ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // be exact up to machine precision for the test values used. 8140ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong J4 expected[3] = { 8150ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong MakeJ4(theta, -2*theta, 2.0, 0, 0), 8160ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong MakeJ4(0, 0, 0, 2.0, 0), 8170ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong MakeJ4(0, 0, 0, 0, 2.0), 8180ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong }; 8190ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong QuaternionToAngleAxis(quaternion, axis_angle); 8200ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong ExpectJetArraysClose<3, 4>(axis_angle, expected); 8210ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 8220ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 8230ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 8240ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Test that conversion works for no rotation. 8250ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus KongTEST(Rotation, ZeroQuaternionToAngleAxisForJets) { 8260ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong J4 quaternion[4] = { J4(1, 0), J4(0, 1), J4(0, 2), J4(0, 3) }; 8270ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong J4 axis_angle[3]; 8280ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong J4 expected[3] = { 8290ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong MakeJ4(0, 0, 2.0, 0, 0), 8300ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong MakeJ4(0, 0, 0, 2.0, 0), 8310ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong MakeJ4(0, 0, 0, 0, 2.0), 8320ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong }; 8330ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong QuaternionToAngleAxis(quaternion, axis_angle); 8340ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong ExpectJetArraysClose<3, 4>(axis_angle, expected); 8350ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 8360ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 8370ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus KongTEST(Quaternion, RotatePointGivesSameAnswerAsRotationByMatrixCanned) { 8380ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Canned data generated in octave. 8390ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double const q[4] = { 8400ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong +0.1956830471754074, 8410ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong -0.0150618562474847, 8420ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong +0.7634572982788086, 8430ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong -0.3019454777240753, 8440ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong }; 8450ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double const Q[3][3] = { // Scaled rotation matrix. 8460ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong { -0.6355194033477252, 0.0951730541682254, 0.3078870197911186 }, 8470ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong { -0.1411693904792992, 0.5297609702153905, -0.4551502574482019 }, 8480ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong { -0.2896955822708862, -0.4669396571547050, -0.4536309793389248 }, 8490ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong }; 8500ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double const R[3][3] = { // With unit rows and columns. 8510ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong { -0.8918859164053080, 0.1335655625725649, 0.4320876677394745 }, 8520ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong { -0.1981166751680096, 0.7434648665444399, -0.6387564287225856 }, 8530ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong { -0.4065578619806013, -0.6553016349046693, -0.6366242786393164 }, 8540ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong }; 8550ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 8560ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Compute R from q and compare to known answer. 8570ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double Rq[3][3]; 8580ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong QuaternionToScaledRotation<double>(q, Rq[0]); 8590ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong ExpectArraysClose(9, Q[0], Rq[0], kTolerance); 8600ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 8610ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Now do the same but compute R with normalization. 8620ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong QuaternionToRotation<double>(q, Rq[0]); 8630ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong ExpectArraysClose(9, R[0], Rq[0], kTolerance); 8640ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 8650ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 8660ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 8670ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus KongTEST(Quaternion, RotatePointGivesSameAnswerAsRotationByMatrix) { 8680ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Rotation defined by a unit quaternion. 8690ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double const q[4] = { 8700ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 0.2318160216097109, 8710ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong -0.0178430356832060, 8720ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 0.9044300776717159, 8730ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong -0.3576998641394597, 8740ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong }; 8750ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double const p[3] = { 8760ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong +0.11, 8770ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong -13.15, 8780ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1.17, 8790ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong }; 8800ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 8810ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double R[3 * 3]; 8820ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong QuaternionToRotation(q, R); 8830ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 8840ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double result1[3]; 8850ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong UnitQuaternionRotatePoint(q, p, result1); 8860ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 8870ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double result2[3]; 8880ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong VectorRef(result2, 3) = ConstMatrixRef(R, 3, 3)* ConstVectorRef(p, 3); 8890ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong ExpectArraysClose(3, result1, result2, kTolerance); 8900ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 8910ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 8920ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 8930ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Verify that (a * b) * c == a * (b * c). 8940ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus KongTEST(Quaternion, MultiplicationIsAssociative) { 8950ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double a[4]; 8960ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double b[4]; 8970ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double c[4]; 8980ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int i = 0; i < 4; ++i) { 8990ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong a[i] = 2 * RandDouble() - 1; 9000ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong b[i] = 2 * RandDouble() - 1; 9010ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong c[i] = 2 * RandDouble() - 1; 9020ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 9030ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 9040ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double ab[4]; 9050ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double ab_c[4]; 9060ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong QuaternionProduct(a, b, ab); 9070ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong QuaternionProduct(ab, c, ab_c); 9080ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 9090ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double bc[4]; 9100ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double a_bc[4]; 9110ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong QuaternionProduct(b, c, bc); 9120ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong QuaternionProduct(a, bc, a_bc); 9130ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 9140ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong ASSERT_NEAR(ab_c[0], a_bc[0], kTolerance); 9150ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong ASSERT_NEAR(ab_c[1], a_bc[1], kTolerance); 9160ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong ASSERT_NEAR(ab_c[2], a_bc[2], kTolerance); 9170ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong ASSERT_NEAR(ab_c[3], a_bc[3], kTolerance); 9180ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 9190ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 9200ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 9210ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus KongTEST(AngleAxis, RotatePointGivesSameAnswerAsRotationMatrix) { 9220ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double angle_axis[3]; 9230ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double R[9]; 9240ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double p[3]; 9250ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double angle_axis_rotated_p[3]; 9260ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double rotation_matrix_rotated_p[3]; 9270ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 9280ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int i = 0; i < 10000; ++i) { 9290ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double theta = (2.0 * i * 0.0011 - 1.0) * kPi; 9300ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int j = 0; j < 50; ++j) { 9310ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double norm2 = 0.0; 9320ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int k = 0; k < 3; ++k) { 9330ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong angle_axis[k] = 2.0 * RandDouble() - 1.0; 9340ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong p[k] = 2.0 * RandDouble() - 1.0; 9350ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong norm2 = angle_axis[k] * angle_axis[k]; 9360ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 9370ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 9380ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const double inv_norm = theta / sqrt(norm2); 9390ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int k = 0; k < 3; ++k) { 9400ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong angle_axis[k] *= inv_norm; 9410ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 9420ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 9430ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong AngleAxisToRotationMatrix(angle_axis, R); 9440ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong rotation_matrix_rotated_p[0] = R[0] * p[0] + R[3] * p[1] + R[6] * p[2]; 9450ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong rotation_matrix_rotated_p[1] = R[1] * p[0] + R[4] * p[1] + R[7] * p[2]; 9460ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong rotation_matrix_rotated_p[2] = R[2] * p[0] + R[5] * p[1] + R[8] * p[2]; 9470ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 9480ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong AngleAxisRotatePoint(angle_axis, p, angle_axis_rotated_p); 9490ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int k = 0; k < 3; ++k) { 9500ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong EXPECT_NEAR(rotation_matrix_rotated_p[k], 9510ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong angle_axis_rotated_p[k], 9520ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong kTolerance) << "p: " << p[0] 9530ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong << " " << p[1] 9540ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong << " " << p[2] 9550ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong << " angle_axis: " << angle_axis[0] 9560ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong << " " << angle_axis[1] 9570ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong << " " << angle_axis[2]; 9580ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 9590ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 9600ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 9610ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 9620ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 9630ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus KongTEST(AngleAxis, NearZeroRotatePointGivesSameAnswerAsRotationMatrix) { 9640ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double angle_axis[3]; 9650ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double R[9]; 9660ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double p[3]; 9670ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double angle_axis_rotated_p[3]; 9680ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double rotation_matrix_rotated_p[3]; 9690ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 9700ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int i = 0; i < 10000; ++i) { 9710ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double norm2 = 0.0; 9720ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int k = 0; k < 3; ++k) { 9730ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong angle_axis[k] = 2.0 * RandDouble() - 1.0; 9740ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong p[k] = 2.0 * RandDouble() - 1.0; 9750ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong norm2 = angle_axis[k] * angle_axis[k]; 9760ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 9770ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 9780ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double theta = (2.0 * i * 0.0001 - 1.0) * 1e-16; 9790ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const double inv_norm = theta / sqrt(norm2); 9800ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int k = 0; k < 3; ++k) { 9810ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong angle_axis[k] *= inv_norm; 9820ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 9830ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 9840ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong AngleAxisToRotationMatrix(angle_axis, R); 9850ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong rotation_matrix_rotated_p[0] = R[0] * p[0] + R[3] * p[1] + R[6] * p[2]; 9860ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong rotation_matrix_rotated_p[1] = R[1] * p[0] + R[4] * p[1] + R[7] * p[2]; 9870ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong rotation_matrix_rotated_p[2] = R[2] * p[0] + R[5] * p[1] + R[8] * p[2]; 9880ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 9890ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong AngleAxisRotatePoint(angle_axis, p, angle_axis_rotated_p); 9900ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int k = 0; k < 3; ++k) { 9910ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong EXPECT_NEAR(rotation_matrix_rotated_p[k], 9920ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong angle_axis_rotated_p[k], 9930ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong kTolerance) << "p: " << p[0] 9940ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong << " " << p[1] 9950ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong << " " << p[2] 9960ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong << " angle_axis: " << angle_axis[0] 9970ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong << " " << angle_axis[1] 9980ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong << " " << angle_axis[2]; 9990ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 10000ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 10010ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 10020ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 10031d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha HaeberlingTEST(MatrixAdapter, RowMajor3x3ReturnTypeAndAccessIsCorrect) { 10041d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling double array[9] = { 1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0 }; 10051d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const float const_array[9] = 10061d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling { 1.0f, 2.0f, 3.0f, 4.0f, 5.0f, 6.0f, 7.0f, 8.0f, 9.0f }; 10071d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling MatrixAdapter<double, 3, 1> A = RowMajorAdapter3x3(array); 10081d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling MatrixAdapter<const float, 3, 1> B = RowMajorAdapter3x3(const_array); 10091d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 10101d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling for (int i = 0; i < 3; ++i) { 10111d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling for (int j = 0; j < 3; ++j) { 10121d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // The values are integers from 1 to 9, so equality tests are appropriate 10131d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // even for float and double values. 10141d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_EQ(A(i, j), array[3*i+j]); 10151d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_EQ(B(i, j), const_array[3*i+j]); 10161d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 10171d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 10181d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling} 10191d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 10201d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha HaeberlingTEST(MatrixAdapter, ColumnMajor3x3ReturnTypeAndAccessIsCorrect) { 10211d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling double array[9] = { 1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0 }; 10221d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const float const_array[9] = 10231d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling { 1.0f, 2.0f, 3.0f, 4.0f, 5.0f, 6.0f, 7.0f, 8.0f, 9.0f }; 10241d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling MatrixAdapter<double, 1, 3> A = ColumnMajorAdapter3x3(array); 10251d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling MatrixAdapter<const float, 1, 3> B = ColumnMajorAdapter3x3(const_array); 10261d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 10271d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling for (int i = 0; i < 3; ++i) { 10281d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling for (int j = 0; j < 3; ++j) { 10291d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // The values are integers from 1 to 9, so equality tests are 10301d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // appropriate even for float and double values. 10311d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_EQ(A(i, j), array[3*j+i]); 10321d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_EQ(B(i, j), const_array[3*j+i]); 10331d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 10341d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 10351d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling} 10361d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 10371d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha HaeberlingTEST(MatrixAdapter, RowMajor2x4IsCorrect) { 10381d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const int expected[8] = { 1, 2, 3, 4, 5, 6, 7, 8 }; 10391d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling int array[8]; 10401d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling MatrixAdapter<int, 4, 1> M(array); 10411d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling M(0, 0) = 1; M(0, 1) = 2; M(0, 2) = 3; M(0, 3) = 4; 10421d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling M(1, 0) = 5; M(1, 1) = 6; M(1, 2) = 7; M(1, 3) = 8; 10431d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling for (int k = 0; k < 8; ++k) { 10441d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_EQ(array[k], expected[k]); 10451d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 10461d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling} 10471d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 10481d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha HaeberlingTEST(MatrixAdapter, ColumnMajor2x4IsCorrect) { 10491d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const int expected[8] = { 1, 5, 2, 6, 3, 7, 4, 8 }; 10501d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling int array[8]; 10511d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling MatrixAdapter<int, 1, 2> M(array); 10521d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling M(0, 0) = 1; M(0, 1) = 2; M(0, 2) = 3; M(0, 3) = 4; 10531d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling M(1, 0) = 5; M(1, 1) = 6; M(1, 2) = 7; M(1, 3) = 8; 10541d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling for (int k = 0; k < 8; ++k) { 10551d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_EQ(array[k], expected[k]); 10561d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 10571d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling} 10580ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 10590ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} // namespace internal 10600ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} // namespace ceres 1061