Added some small optimizations to the jet class (initialize values directly from eigen expressions, instead of creating a temporary). Also changed several variable names from temp to tmp to be consistent with the code base. Change-Id: I3f7b834cedca9c4af8e5e086237b40301dbde619
diff --git a/include/ceres/jet.h b/include/ceres/jet.h index 7907b98..ee45911 100644 --- a/include/ceres/jet.h +++ b/include/ceres/jet.h
@@ -192,6 +192,17 @@ v[k] = T(1.0); } + // Constructor from scalar and vector part + // The use of Eigen::DenseBase allows Eigen expressions + // to be passed in without being fully evaluated until + // they are assigned to v + template<typename Derived> + Jet(const T& value, const Eigen::DenseBase<Derived> &vIn) + : a(value), + v(vIn) + { + } + // Compound operators Jet<T, N>& operator+=(const Jet<T, N> &y) { *this = *this + y; @@ -246,101 +257,70 @@ // Unary - template<typename T, int N> inline Jet<T, N> operator-(const Jet<T, N>&f) { - Jet<T, N> g; - g.a = -f.a; - g.v = -f.v; - return g; + return Jet<T, N>(-f.a, -f.v); } // Binary + template<typename T, int N> inline Jet<T, N> operator+(const Jet<T, N>& f, const Jet<T, N>& g) { - Jet<T, N> h; - h.a = f.a + g.a; - h.v = f.v + g.v; - return h; + return Jet<T, N>(f.a + g.a, f.v + g.v); } // Binary + with a scalar: x + s 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; - return h; + return Jet<T, N>(f.a + s, f.v); } // Binary + with a scalar: s + x template<typename T, int N> inline Jet<T, N> operator+(T s, const Jet<T, N>& f) { - Jet<T, N> h; - h.a = f.a + s; - h.v = f.v; - return h; + return Jet<T, N>(f.a + s, f.v); } // Binary - template<typename T, int N> inline Jet<T, N> operator-(const Jet<T, N>& f, const Jet<T, N>& g) { - Jet<T, N> h; - h.a = f.a - g.a; - h.v = f.v - g.v; - return h; + return Jet<T, N>(f.a - g.a, f.v - g.v); } // Binary - with a scalar: x - s 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; - return h; + return Jet<T, N>(f.a - s, f.v); } // Binary - with a scalar: s - x template<typename T, int N> inline Jet<T, N> operator-(T s, const Jet<T, N>& f) { - Jet<T, N> h; - h.a = s - f.a; - h.v = -f.v; - return h; + return Jet<T, N>(s - f.a, -f.v); } // Binary * template<typename T, int N> inline Jet<T, N> operator*(const Jet<T, N>& f, const Jet<T, N>& g) { - Jet<T, N> h; - h.a = f.a * g.a; - h.v = f.a * g.v + f.v * g.a; - return h; + return Jet<T, N>(f.a * g.a, f.a * g.v + f.v * g.a); } // Binary * with a scalar: x * s 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; - return h; + return Jet<T, N>(f.a * s, f.v * s); } // Binary * with a scalar: s * x template<typename T, int N> inline Jet<T, N> operator*(T s, const Jet<T, N>& f) { - Jet<T, N> h; - h.a = f.a * s; - h.v = f.v * s; - return h; + return Jet<T, N>(f.a * s, f.v * s); } // Binary / template<typename T, int N> inline Jet<T, N> operator/(const Jet<T, N>& f, const Jet<T, N>& g) { - Jet<T, N> h; // This uses: // // a + u (a + u)(b - v) (a + u)(b - v) @@ -349,32 +329,22 @@ // // which holds because v*v = 0. 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; + return Jet<T, N>(f.a * g_a_inverse, (f.v - f_a_by_g_a * g.v) * g_a_inverse); } // Binary / with a scalar: s / x 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; const T minus_s_g_a_inverse2 = -s / (g.a * g.a); - h.v = g.v * minus_s_g_a_inverse2; - return h; + return Jet<T, N>(s / g.a, g.v * minus_s_g_a_inverse2); } // Binary / with a scalar: x / s template<typename T, int N> inline Jet<T, N> operator/(const Jet<T, N>& f, T s) { - Jet<T, N> h; const T s_inverse = 1.0 / s; - h.a = f.a * s_inverse; - h.v = f.v * s_inverse; - return h; + return Jet<T, N>(f.a * s_inverse, f.v * s_inverse); } // Binary comparison operators for both scalars and jets. @@ -433,122 +403,84 @@ // log(a + h) ~= log(a) + h / a template <typename T, int N> inline Jet<T, N> log(const Jet<T, N>& f) { - Jet<T, N> g; - g.a = log(f.a); const T a_inverse = T(1.0) / f.a; - g.v = f.v * a_inverse; - return g; + return Jet<T, N>(log(f.a), f.v * a_inverse); } // exp(a + h) ~= exp(a) + exp(a) h template <typename T, int N> inline Jet<T, N> exp(const Jet<T, N>& f) { - Jet<T, N> g; - g.a = exp(f.a); - g.v = g.a * f.v; - return g; + const T tmp = exp(f.a); + return Jet<T, N>(tmp, tmp * f.v); } // sqrt(a + h) ~= sqrt(a) + h / (2 sqrt(a)) template <typename T, int N> inline Jet<T, N> sqrt(const Jet<T, N>& f) { - Jet<T, N> g; - g.a = sqrt(f.a); - const T two_a_inverse = T(1.0) / (T(2.0) * g.a); - g.v = f.v * two_a_inverse; - return g; + const T tmp = sqrt(f.a); + const T two_a_inverse = T(1.0) / (T(2.0) * tmp); + return Jet<T, N>(tmp, f.v * two_a_inverse); } // cos(a + h) ~= cos(a) - sin(a) h template <typename T, int N> inline Jet<T, N> cos(const Jet<T, N>& f) { - Jet<T, N> g; - g.a = cos(f.a); - const T sin_a = sin(f.a); - g.v = - sin_a * f.v; - return g; + return Jet<T, N>(cos(f.a), - sin(f.a) * f.v); } // acos(a + h) ~= acos(a) - 1 / sqrt(1 - a^2) h template <typename T, int N> inline Jet<T, N> acos(const Jet<T, N>& f) { - Jet<T, N> g; - g.a = acos(f.a); const T tmp = - T(1.0) / sqrt(T(1.0) - f.a * f.a); - g.v = tmp * f.v; - return g; + return Jet<T, N>(acos(f.a), tmp * f.v); } // sin(a + h) ~= sin(a) + cos(a) h template <typename T, int N> inline Jet<T, N> sin(const Jet<T, N>& f) { - Jet<T, N> g; - g.a = sin(f.a); - const T cos_a = cos(f.a); - g.v = cos_a * f.v; - return g; + return Jet<T, N>(sin(f.a), cos(f.a) * f.v); } // asin(a + h) ~= asin(a) + 1 / sqrt(1 - a^2) h template <typename T, int N> inline Jet<T, N> asin(const Jet<T, N>& f) { - Jet<T, N> g; - g.a = asin(f.a); const T tmp = T(1.0) / sqrt(T(1.0) - f.a * f.a); - g.v = tmp * f.v; - return g; + return Jet<T, N>(asin(f.a), tmp * f.v); } // 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 tan_a = tan(f.a); const T tmp = T(1.0) + tan_a * tan_a; - g.v = tmp * f.v; - return g; + return Jet<T, N>(tan_a, tmp * f.v); } // 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; + return Jet<T, N>(atan(f.a), tmp * f.v); } // 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; + return Jet<T, N>(sinh(f.a), cosh(f.a) * f.v); } // 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; + return Jet<T, N>(cosh(f.a), sinh(f.a) * f.v); } // 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 tanh_a = tanh(f.a); const T tmp = T(1.0) - tanh_a * tanh_a; - g.v = tmp * f.v; - return g; + return Jet<T, N>(tanh_a, tmp * f.v); } // Jet Classification. It is not clear what the appropriate semantics are for @@ -628,13 +560,8 @@ // f = a + da // g = b + db - Jet<T, N> out; - - out.a = atan2(g.a, f.a); - - T const temp = T(1.0) / (f.a * f.a + g.a * g.a); - out.v = temp * (- g.a * f.v + f.a * g.v); - return out; + T const tmp = T(1.0) / (f.a * f.a + g.a * g.a); + return Jet<T, N>(atan2(g.a, f.a), tmp * (- g.a * f.v + f.a * g.v)); } @@ -642,22 +569,17 @@ // (a+da)^p ~= a^p + p*a^(p-1) da template <typename T, int N> inline Jet<T, N> pow(const Jet<T, N>& f, double g) { - Jet<T, N> out; - out.a = pow(f.a, g); - T const temp = g * pow(f.a, g - T(1.0)); - out.v = temp * f.v; - return out; + T const tmp = g * pow(f.a, g - T(1.0)); + return Jet<T, N>(pow(f.a, g), tmp * f.v); } // pow -- base is a constant, exponent is a differentiable function. // (a)^(p+dp) ~= a^p + a^p log(a) dp template <typename T, int N> inline Jet<T, N> pow(double f, const Jet<T, N>& g) { - Jet<T, N> out; - out.a = pow(f, g.a); - T const temp = log(f) * out.a; - out.v = temp * g.v; - return out; + T const tmp1 = pow(f, g.a); + T const tmp2 = log(f) * tmp1; + return Jet<T, N>(tmp1, tmp2 * g.v); } @@ -665,15 +587,11 @@ // (a+da)^(b+db) ~= a^b + b * a^(b-1) da + a^b log(a) * db template <typename T, int N> inline Jet<T, N> pow(const Jet<T, N>& f, const Jet<T, N>& g) { - Jet<T, N> out; + T const tmp1 = pow(f.a, g.a); + T const tmp2 = g.a * pow(f.a, g.a - T(1.0)); + T const tmp3 = tmp1 * log(f.a); - T const temp1 = pow(f.a, g.a); - T const temp2 = g.a * pow(f.a, g.a - T(1.0)); - T const temp3 = temp1 * log(f.a); - - out.a = temp1; - out.v = temp2 * f.v + temp3 * g.v; - return out; + return Jet<T, N>(tmp1, tmp2 * f.v + tmp3 * g.v); } // Define the helper functions Eigen needs to embed Jet types.