diff options
Diffstat (limited to 'include/ceres/jet.h')
| -rw-r--r-- | include/ceres/jet.h | 95 |
1 files changed, 83 insertions, 12 deletions
diff --git a/include/ceres/jet.h b/include/ceres/jet.h index 96e2256..4d2a857 100644 --- a/include/ceres/jet.h +++ b/include/ceres/jet.h @@ -348,8 +348,12 @@ Jet<T, N> operator/(const Jet<T, N>& f, // b + v (b + v)(b - v) b^2 // // which holds because v*v = 0. - h.a = f.a / g.a; - h.v = (f.v - f.a / g.a * g.v) / g.a; + const T g_a_inverse = T(1.0) / g.a; + h.a = f.a * g_a_inverse; + const T f_a_by_g_a = f.a * g_a_inverse; + for (int i = 0; i < N; ++i) { + h.v[i] = (f.v[i] - f_a_by_g_a * g.v[i]) * g_a_inverse; + } return h; } @@ -358,7 +362,8 @@ template<typename T, int N> inline Jet<T, N> operator/(T s, const Jet<T, N>& g) { Jet<T, N> h; h.a = s / g.a; - h.v = - s * g.v / (g.a * g.a); + const T minus_s_g_a_inverse2 = -s / (g.a * g.a); + h.v = g.v * minus_s_g_a_inverse2; return h; } @@ -366,8 +371,9 @@ Jet<T, N> operator/(T s, const Jet<T, N>& g) { template<typename T, int N> inline Jet<T, N> operator/(const Jet<T, N>& f, T s) { Jet<T, N> h; - h.a = f.a / s; - h.v = f.v / s; + const T s_inverse = 1.0 / s; + h.a = f.a * s_inverse; + h.v = f.v * s_inverse; return h; } @@ -399,7 +405,6 @@ CERES_DEFINE_JET_COMPARISON_OPERATOR( != ) // NOLINT // double-valued and Jet-valued functions, but we are not allowed to put // Jet-valued functions inside namespace std. // -// Missing: cosh, sinh, tanh, tan // TODO(keir): Switch to "using". inline double abs (double x) { return std::abs(x); } inline double log (double x) { return std::log(x); } @@ -409,6 +414,11 @@ inline double cos (double x) { return std::cos(x); } inline double acos (double x) { return std::acos(x); } inline double sin (double x) { return std::sin(x); } inline double asin (double x) { return std::asin(x); } +inline double tan (double x) { return std::tan(x); } +inline double atan (double x) { return std::atan(x); } +inline double sinh (double x) { return std::sinh(x); } +inline double cosh (double x) { return std::cosh(x); } +inline double tanh (double x) { return std::tanh(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); } @@ -425,7 +435,8 @@ template <typename T, int N> inline Jet<T, N> log(const Jet<T, N>& f) { Jet<T, N> g; g.a = log(f.a); - g.v = f.v / f.a; + const T a_inverse = T(1.0) / f.a; + g.v = f.v * a_inverse; return g; } @@ -443,7 +454,8 @@ template <typename T, int N> inline Jet<T, N> sqrt(const Jet<T, N>& f) { Jet<T, N> g; g.a = sqrt(f.a); - g.v = f.v / (T(2.0) * g.a); + const T two_a_inverse = T(1.0) / (T(2.0) * g.a); + g.v = f.v * two_a_inverse; return g; } @@ -452,7 +464,7 @@ template <typename T, int N> inline Jet<T, N> cos(const Jet<T, N>& f) { Jet<T, N> g; g.a = cos(f.a); - T sin_a = sin(f.a); + const T sin_a = sin(f.a); g.v = - sin_a * f.v; return g; } @@ -462,7 +474,8 @@ template <typename T, int N> inline Jet<T, N> acos(const Jet<T, N>& f) { Jet<T, N> g; g.a = acos(f.a); - g.v = - T(1.0) / sqrt(T(1.0) - f.a * f.a) * f.v; + const T tmp = - T(1.0) / sqrt(T(1.0) - f.a * f.a); + g.v = tmp * f.v; return g; } @@ -471,7 +484,7 @@ template <typename T, int N> inline Jet<T, N> sin(const Jet<T, N>& f) { Jet<T, N> g; g.a = sin(f.a); - T cos_a = cos(f.a); + const T cos_a = cos(f.a); g.v = cos_a * f.v; return g; } @@ -481,7 +494,60 @@ template <typename T, int N> inline Jet<T, N> asin(const Jet<T, N>& f) { Jet<T, N> g; g.a = asin(f.a); - g.v = T(1.0) / sqrt(T(1.0) - f.a * f.a) * f.v; + const T tmp = T(1.0) / sqrt(T(1.0) - f.a * f.a); + g.v = tmp * f.v; + return g; +} + +// tan(a + h) ~= tan(a) + (1 + tan(a)^2) h +template <typename T, int N> inline +Jet<T, N> tan(const Jet<T, N>& f) { + Jet<T, N> g; + g.a = tan(f.a); + double tan_a = tan(f.a); + const T tmp = T(1.0) + tan_a * tan_a; + g.v = tmp * f.v; + return g; +} + +// atan(a + h) ~= atan(a) + 1 / (1 + a^2) h +template <typename T, int N> inline +Jet<T, N> atan(const Jet<T, N>& f) { + Jet<T, N> g; + g.a = atan(f.a); + const T tmp = T(1.0) / (T(1.0) + f.a * f.a); + g.v = tmp * f.v; + return g; +} + +// sinh(a + h) ~= sinh(a) + cosh(a) h +template <typename T, int N> inline +Jet<T, N> sinh(const Jet<T, N>& f) { + Jet<T, N> g; + g.a = sinh(f.a); + const T cosh_a = cosh(f.a); + g.v = cosh_a * f.v; + return g; +} + +// cosh(a + h) ~= cosh(a) + sinh(a) h +template <typename T, int N> inline +Jet<T, N> cosh(const Jet<T, N>& f) { + Jet<T, N> g; + g.a = cosh(f.a); + const T sinh_a = sinh(f.a); + g.v = sinh_a * f.v; + return g; +} + +// tanh(a + h) ~= tanh(a) + (1 - tanh(a)^2) h +template <typename T, int N> inline +Jet<T, N> tanh(const Jet<T, N>& f) { + Jet<T, N> g; + g.a = tanh(f.a); + double tanh_a = tanh(f.a); + const T tmp = T(1.0) - tanh_a * tanh_a; + g.v = tmp * f.v; return g; } @@ -635,6 +701,11 @@ template<typename T, int N> inline Jet<T, N> ei_exp (const Jet<T, N>& x) template<typename T, int N> inline Jet<T, N> ei_log (const Jet<T, N>& x) { return log(x); } // NOLINT template<typename T, int N> inline Jet<T, N> ei_sin (const Jet<T, N>& x) { return sin(x); } // NOLINT template<typename T, int N> inline Jet<T, N> ei_cos (const Jet<T, N>& x) { return cos(x); } // NOLINT +template<typename T, int N> inline Jet<T, N> ei_tan (const Jet<T, N>& x) { return tan(x); } // NOLINT +template<typename T, int N> inline Jet<T, N> ei_atan(const Jet<T, N>& x) { return atan(x); } // NOLINT +template<typename T, int N> inline Jet<T, N> ei_sinh(const Jet<T, N>& x) { return sinh(x); } // NOLINT +template<typename T, int N> inline Jet<T, N> ei_cosh(const Jet<T, N>& x) { return cosh(x); } // NOLINT +template<typename T, int N> inline Jet<T, N> ei_tanh(const Jet<T, N>& x) { return tanh(x); } // NOLINT template<typename T, int N> inline Jet<T, N> ei_pow (const Jet<T, N>& x, Jet<T, N> y) { return pow(x, y); } // NOLINT // Note: This has to be in the ceres namespace for argument dependent lookup to |