saberbud/ceres-solver
1
0
Fork 0
mirror of https://github.com/ceres-solver/ceres-solver.git synced 2026-08-29 08:34:37 +08:00
Code Issues Packages Projects Releases Wiki Activity

Implement some C++11 math functions for Jet

Browse Source 
Change-Id: Ie164aeaac980cd2fe48a86230e1948c9fd75424a
This commit is contained in:
Emil Ernerfeldt
2018-01-28 11:29:59 +01:00
committed by Sameer Agarwal
parent 6c835f81a7
commit 39ccb79ae6
2 changed files with 235 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
include/ceres/jet.h
+61
View File
@@ -446,6 +446,16 @@ inline double ceil (double x) { return std::ceil(x); }
inline double pow (double x, double y) { return std::pow(x, y); }
inline double atan2(double y, double x) { return std::atan2(y, x); }
#ifdef CERES_USE_CXX11
// Some new additions to C++11:
inline double cbrt (double x) { return std::cbrt(x); }
inline double exp2 (double x) { return std::exp2(x); }
inline double log2 (double x) { return std::log2(x); }
inline double hypot(double x, double y) { return std::hypot(x, y); }
inline double fmax(double x, double y) { return std::fmax(x, y); }
inline double fmin(double x, double y) { return std::fmin(x, y); }
#endif
// In general, f(a + h) ~= f(a) + f'(a) h, via the chain rule.
// abs(x + h) ~= x + h or -(x + h)
@@ -555,6 +565,57 @@ Jet<T, N> ceil(const Jet<T, N>& f) {
return Jet<T, N>(ceil(f.a));
}
#ifdef CERES_USE_CXX11
// Some new additions to C++11:
// cbrt(a + h) ~= cbrt(a) + h / (3 a ^ (2/3))
template <typename T, int N> inline
Jet<T, N> cbrt(const Jet<T, N>& f) {
const T derivative = T(1.0) / (T(3.0) * cbrt(f.a * f.a));
return Jet<T, N>(cbrt(f.a), f.v * derivative);
}
// exp2(x + h) = 2^(x+h) ~= 2^x + h*2^x*log(2)
template <typename T, int N> inline
Jet<T, N> exp2(const Jet<T, N>& f) {
const T tmp = exp2(f.a);
const T derivative = tmp * log(T(2));
return Jet<T, N>(tmp, f.v * derivative);
}
// log2(x + h) ~= log2(x) + h / (x * log(2))
template <typename T, int N> inline
Jet<T, N> log2(const Jet<T, N>& f) {
const T derivative = T(1.0) / (f.a * log(T(2)));
return Jet<T, N>(log2(f.a), f.v * derivative);
}
// Like sqrt(x^2 + y^2),
// but acts to prevent underflow/overflow for small/large x/y.
// Note that the function is non-smooth at x=y=0,
// so the derivative is undefined there.
template <typename T, int N> inline
Jet<T, N> hypot(const Jet<T, N>& x, const Jet<T, N>& y) {
// d/da sqrt(a) = 0.5 / sqrt(a)
// d/dx x^2 + y^2 = 2x
// So by the chain rule:
// d/dx sqrt(x^2 + y^2) = 0.5 / sqrt(x^2 + y^2) * 2x = x / sqrt(x^2 + y^2)
// d/dy sqrt(x^2 + y^2) = y / sqrt(x^2 + y^2)
const T tmp = hypot(x.a, y.a);
return Jet<T, N>(tmp, x.a / tmp * x.v + y.a / tmp * y.v);
}
template <typename T, int N> inline
const Jet<T, N>& fmax(const Jet<T, N>& x, const Jet<T, N>& y) {
return x < y ? y : x;
}
template <typename T, int N> inline
const Jet<T, N>& fmin(const Jet<T, N>& x, const Jet<T, N>& y) {
return y < x ? y : x;
}
#endif // CERES_USE_CXX11
// Bessel functions of the first kind with integer order equal to 0, 1, n.
//
// Microsoft has deprecated the j[0,1,n]() POSIX Bessel functions in favour of
internal/ceres/jet_test.cc
+174
View File
@@ -66,6 +66,46 @@ void ExpectJetsClose(const J &x, const J &y) {
ExpectClose(x.v[1], y.v[1], kTolerance);
}
const double kStep = 1e-8;
const double kNumericalTolerance = 1e-6; // Numeric derivation is quite inexact
// Differentiate using Jet and confirm results with numerical derivation.
template<typename Function>
void NumericalTest(const char* name, const Function& f, const double x) {
const double exact_dx = f(MakeJet(x, 1.0, 0.0)).v[0];
const double estimated_dx =
(f(J(x + kStep)).a - f(J(x - kStep)).a) / (2.0 * kStep);
VL << name << "(" << x << "), exact dx: "
<< exact_dx << ", estimated dx: " << estimated_dx;
ExpectClose(exact_dx, estimated_dx, kNumericalTolerance);
}
// Same as NumericalTest, but given a function taking two arguments.
template<typename Function>
void NumericalTest2(const char* name, const Function& f,
const double x, const double y) {
const J exact_delta = f(MakeJet(x, 1.0, 0.0), MakeJet(y, 0.0, 1.0));
const double exact_dx = exact_delta.v[0];
const double exact_dy = exact_delta.v[1];
// Sanity check – these should be equivalent:
EXPECT_EQ(exact_dx, f(MakeJet(x, 1.0, 0.0), MakeJet(y, 0.0, 0.0)).v[0]);
EXPECT_EQ(exact_dx, f(MakeJet(x, 0.0, 1.0), MakeJet(y, 0.0, 0.0)).v[1]);
EXPECT_EQ(exact_dy, f(MakeJet(x, 0.0, 0.0), MakeJet(y, 1.0, 0.0)).v[0]);
EXPECT_EQ(exact_dy, f(MakeJet(x, 0.0, 0.0), MakeJet(y, 0.0, 1.0)).v[1]);
const double estimated_dx =
(f(J(x + kStep), J(y)).a - f(J(x - kStep), J(y)).a) / (2.0 * kStep);
const double estimated_dy =
(f(J(x), J(y + kStep)).a - f(J(x), J(y - kStep)).a) / (2.0 * kStep);
VL << name << "(" << x << ", " << y << "), exact dx: "
<< exact_dx << ", estimated dx: " << estimated_dx;
ExpectClose(exact_dx, estimated_dx, kNumericalTolerance);
VL << name << "(" << x << ", " << y << "), exact dy: "
<< exact_dy << ", estimated dy: " << estimated_dy;
ExpectClose(exact_dy, estimated_dy, kNumericalTolerance);
}
TEST(Jet, Jet) {
// Pick arbitrary values for x and y.
J x = MakeJet(2.3, -2.7, 1e-3);
@@ -106,6 +146,9 @@ TEST(Jet, Jet) {
ExpectJetsClose(w, y);
}
NumericalTest("sqrt", sqrt<double, 2>, 0.00001);
NumericalTest("sqrt", sqrt<double, 2>, 1.0);
{ // Check that cos(2*x) = cos(x)^2 - sin(x)^2
J z = cos(J(2.0) * x);
J w = cos(x)*cos(x) - sin(x)*sin(x);
@@ -523,6 +566,137 @@ TEST(Jet, Jet) {
J expected = MakeJet(ceil(a.a), 0.0, 0.0);
EXPECT_EQ(expected, b);
}
#ifdef CERES_USE_CXX11
{ // Check that cbrt(x * x * x) == x.
J z = x * x * x;
J w = cbrt(z);
VL << "z = " << z;
VL << "w = " << w;
ExpectJetsClose(w, x);
}
{ // Check that cbrt(y) * cbrt(y) * cbrt(y) == y.
J z = cbrt(y);
J w = z * z * z;
VL << "z = " << z;
VL << "w = " << w;
ExpectJetsClose(w, y);
}
{ // Check that cbrt(x) == pow(x, 1/3).
J z = cbrt(x);
J w = pow(x, 1.0 / 3.0);
VL << "z = " << z;
VL << "w = " << w;
ExpectJetsClose(z, w);
}
NumericalTest("cbrt", cbrt<double, 2>, -1.0);
NumericalTest("cbrt", cbrt<double, 2>, -1e-5);
NumericalTest("cbrt", cbrt<double, 2>, 1e-5);
NumericalTest("cbrt", cbrt<double, 2>, 1.0);
{ // Check that exp2(x) == exp(x * log(2))
J z = exp2(x);
J w = exp(x * log(2.0));
VL << "z = " << z;
VL << "w = " << w;
ExpectJetsClose(z, w);
}
NumericalTest("exp2", exp2<double, 2>, -1.0);
NumericalTest("exp2", exp2<double, 2>, -1e-5);
NumericalTest("exp2", exp2<double, 2>, -1e-200);
NumericalTest("exp2", exp2<double, 2>, 0.0);
NumericalTest("exp2", exp2<double, 2>, 1e-200);
NumericalTest("exp2", exp2<double, 2>, 1e-5);
NumericalTest("exp2", exp2<double, 2>, 1.0);
{ // Check that log2(x) == log(x) / log(2)
J z = log2(x);
J w = log(x) / log(2.0);
VL << "z = " << z;
VL << "w = " << w;
ExpectJetsClose(z, w);
}
NumericalTest("log2", log2<double, 2>, 1e-5);
NumericalTest("log2", log2<double, 2>, 1.0);
NumericalTest("log2", log2<double, 2>, 100.0);
{ // Check that hypot(x, y) == sqrt(x^2 + y^2)
J h = hypot(x, y);
J s = sqrt(x*x + y*y);
VL << "h = " << h;
VL << "s = " << s;
ExpectJetsClose(h, s);
}
{ // Check that hypot(x, x) == sqrt(2) * abs(x)
J h = hypot(x, x);
J s = sqrt(2.0) * abs(x);
VL << "h = " << h;
VL << "s = " << s;
ExpectJetsClose(h, s);
}
{ // Check that the derivative is zero tangentially to the circle:
J h = hypot(MakeJet(2.0, 1.0, 1.0), MakeJet(2.0, 1.0, -1.0));
VL << "h = " << h;
ExpectJetsClose(h, MakeJet(sqrt(8.0), std::sqrt(2.0), 0.0));
}
{ // Check that hypot(x, 0) == x
J zero = MakeJet(0.0, 2.0, 3.14);
J h = hypot(x, zero);
VL << "h = " << h;
ExpectJetsClose(x, h);
}
{ // Check that hypot(0, y) == y
J zero = MakeJet(0.0, 2.0, 3.14);
J h = hypot(zero, y);
VL << "h = " << h;
ExpectJetsClose(y, h);
}
{ // Check that hypot(x, 0) == sqrt(x * x) == x, even when x * x underflows:
EXPECT_EQ(DBL_MIN * DBL_MIN, 0.0); // Make sure it underflows
J huge = MakeJet(DBL_MIN, 2.0, 3.14);
J h = hypot(huge, J(0.0));
VL << "h = " << h;
ExpectJetsClose(h, huge);
}
{ // Check that hypot(x, 0) == sqrt(x * x) == x, even when x * x overflows:
EXPECT_EQ(DBL_MAX * DBL_MAX, std::numeric_limits<double>::infinity());
J huge = MakeJet(DBL_MAX, 2.0, 3.14);
J h = hypot(huge, J(0.0));
VL << "h = " << h;
ExpectJetsClose(h, huge);
}
NumericalTest2("hypot", hypot<double, 2>, 0.0, 1e-5);
NumericalTest2("hypot", hypot<double, 2>, -1e-5, 0.0);
NumericalTest2("hypot", hypot<double, 2>, 1e-5, 1e-5);
NumericalTest2("hypot", hypot<double, 2>, 0.0, 1.0);
NumericalTest2("hypot", hypot<double, 2>, 1e-3, 1.0);
NumericalTest2("hypot", hypot<double, 2>, 1e-3, -1.0);
NumericalTest2("hypot", hypot<double, 2>, -1e-3, 1.0);
NumericalTest2("hypot", hypot<double, 2>, -1e-3, -1.0);
NumericalTest2("hypot", hypot<double, 2>, 1.0, 2.0);
{
J z = fmax(x, y);
VL << "z = " << z;
ExpectJetsClose(x, z);
}
{
J z = fmin(x, y);
VL << "z = " << z;
ExpectJetsClose(y, z);
}
#endif
}
TEST(Jet, JetsInEigenMatrices) {