Reduce copies involved in Jet operations - Avoid copies in compound assignment operators (+=, -=, *=, /=) - Add r-value overloads to +,-,*./ operators Addition<3>/1000 +0.0382 +0.0382 2798 2905 2798 2905 Addition<10>/1000 -0.0009 -0.0009 4831 4827 4831 4827 Addition<15>/1000 -0.0011 -0.0010 8005 7996 8005 7996 Addition<25>/1000 +0.0014 +0.0014 13951 13970 13951 13970 Addition<32>/1000 +0.0010 +0.0009 13638 13651 13638 13651 Addition<200>/160 -0.1732 -0.1732 40301 33321 40300 33320 AdditionScalar<3>/1000 +0.0016 +0.0016 2493 2497 2493 2497 AdditionScalar<10>/1000 +0.0060 +0.0060 2701 2718 2701 2717 AdditionScalar<15>/1000 -0.0024 -0.0024 2598 2592 2598 2592 AdditionScalar<25>/1000 -0.0038 -0.0038 3730 3716 3730 3716 AdditionScalar<32>/1000 +0.0062 +0.0062 4137 4162 4137 4162 AdditionScalar<200>/160 -0.5626 -0.5626 19355 8466 19355 8466 Subtraction<3>/1000 -0.0011 -0.0012 3162 3158 3162 3158 Subtraction<10>/1000 -0.0027 -0.0026 4982 4969 4982 4969 Subtraction<15>/1000 -0.0112 -0.0112 7820 7733 7820 7733 Subtraction<25>/1000 +0.0181 +0.0181 13088 13325 13088 13325 Subtraction<32>/1000 -0.0007 -0.0007 14514 14505 14514 14504 Subtraction<200>/160 -0.1668 -0.1668 44586 37148 44586 37147 SubtractionScalar<3>/1000 -0.0028 -0.0028 2540 2533 2540 2533 SubtractionScalar<10>/1000 -0.0024 -0.0024 2682 2676 2682 2676 SubtractionScalar<15>/1000 -0.0023 -0.0023 2603 2597 2603 2597 SubtractionScalar<25>/1000 -0.0018 -0.0018 3570 3563 3570 3563 SubtractionScalar<32>/1000 -0.0012 -0.0012 4223 4218 4223 4218 SubtractionScalar<200>/160 -0.5070 -0.5070 20375 10044 20375 10044 Multiplication<3>/1000 -0.0543 -0.0544 6171 5836 6172 5836 Multiplication<10>/1000 -0.0617 -0.0617 11860 11128 11860 11128 Multiplication<15>/1000 -0.0106 -0.0106 17456 17271 17456 17271 Multiplication<25>/1000 -0.0114 -0.0114 27204 26893 27204 26893 Multiplication<32>/1000 -0.0125 -0.0125 32704 32293 32703 32293 Multiplication<200>/160 -0.3849 -0.3849 60557 37246 60557 37246 MultiplicationLeftScalar<3>/1000 +0.0005 +0.0005 3086 3088 3086 3088 MultiplicationLeftScalar<10>/1000 -0.0008 -0.0008 4486 4483 4486 4483 MultiplicationLeftScalar<15>/1000 -0.0010 -0.0010 5840 5834 5840 5834 MultiplicationLeftScalar<25>/1000 -0.0047 -0.0047 11350 11297 11350 11298 MultiplicationLeftScalar<32>/1000 -0.0014 -0.0014 13892 13873 13892 13873 MultiplicationLeftScalar<200>/160 -0.2646 -0.2646 28124 20682 28124 20682 MultiplicationRightScalar<3>/1000 +0.0024 +0.0025 3090 3098 3090 3098 MultiplicationRightScalar<10>/1000 +0.0008 +0.0009 4596 4600 4596 4600 MultiplicationRightScalar<15>/1000 -0.0033 -0.0033 5832 5812 5832 5812 MultiplicationRightScalar<25>/1000 -0.0005 -0.0005 11353 11347 11353 11347 MultiplicationRightScalar<32>/1000 +0.0043 +0.0043 14041 14102 14041 14102 MultiplicationRightScalar<200>/160 -0.2600 -0.2600 28104 20796 28104 20796 Division<3>/1000 -0.0006 -0.0006 8977 8972 8977 8972 Division<10>/1000 -0.0095 -0.0095 14613 14474 14613 14473 Division<15>/1000 -0.0081 -0.0081 23109 22922 23108 22922 Division<25>/1000 -0.0081 -0.0081 37292 36992 37292 36992 Division<32>/1000 -0.0234 -0.0234 41693 40718 41692 40717 Division<200>/160 -0.1306 -0.1306 44486 38677 44486 38677 DivisionLeftScalar<3>/1000 -0.0080 -0.0080 6683 6630 6683 6630 DivisionLeftScalar<10>/1000 -0.0018 -0.0018 9255 9239 9255 9238 DivisionLeftScalar<15>/1000 -0.0011 -0.0011 12180 12166 12180 12166 DivisionLeftScalar<25>/1000 +0.0060 +0.0060 22381 22515 22381 22516 DivisionLeftScalar<32>/1000 -0.0016 -0.0016 20159 20126 20159 20126 DivisionLeftScalar<200>/160 -0.2528 -0.2528 28773 21499 28772 21499 DivisionRightScalar<3>/1000 -0.0006 -0.0006 3784 3782 3784 3782 DivisionRightScalar<10>/1000 -0.0014 -0.0014 5699 5691 5699 5691 DivisionRightScalar<15>/1000 -0.0009 -0.0009 7490 7484 7490 7484 DivisionRightScalar<25>/1000 +0.0031 +0.0031 13751 13793 13751 13793 DivisionRightScalar<32>/1000 -0.0006 -0.0006 16712 16702 16712 16702 DivisionRightScalar<200>/160 -0.2532 -0.2532 28412 21217 28411 21217 MultiplyAndAdd<3>/1000 -0.0191 -0.0191 4382 4299 4382 4299 MultiplyAndAdd<10>/1000 -0.0060 -0.0060 10238 10176 10238 10176 MultiplyAndAdd<15>/1000 -0.0053 -0.0053 11696 11634 11696 11634 MultiplyAndAdd<25>/1000 -0.0171 -0.0171 19185 18858 19185 18858 MultiplyAndAdd<32>/1000 +0.0070 +0.0070 23783 23951 23783 23951 MultiplyAndAdd<200>/160 -0.1252 -0.1252 61470 53776 61469 53776 Change-Id: Ib366d5753ed4f398667766384e05389d45e0d444
diff --git a/include/ceres/jet.h b/include/ceres/jet.h index 3264e2e..50aac1b 100644 --- a/include/ceres/jet.h +++ b/include/ceres/jet.h
@@ -203,43 +203,50 @@ // Compound operators Jet<T, N>& operator+=(const Jet<T, N>& y) { - *this = *this + y; + this->a += y.a; + this->v = this->v + y.v; // Better than += return *this; } Jet<T, N>& operator-=(const Jet<T, N>& y) { - *this = *this - y; + this->a -= y.a; + this->v = this->v - y.v; // Better than -= return *this; } Jet<T, N>& operator*=(const Jet<T, N>& y) { - *this = *this * y; + this->v = this->a * y.v + this->v * y.a; + this->a *= y.a; return *this; } Jet<T, N>& operator/=(const Jet<T, N>& y) { - *this = *this / y; + const T y_a_inverse = T(1.0) / y.a; + this->a *= y_a_inverse; + this->v = (this->v - this->a * y.v) * y_a_inverse; return *this; } // Compound with scalar operators. Jet<T, N>& operator+=(const T& s) { - *this = *this + s; + this->a += s; return *this; } Jet<T, N>& operator-=(const T& s) { - *this = *this - s; + this->a -= s; return *this; } Jet<T, N>& operator*=(const T& s) { - *this = *this * s; + this->a *= s; + this->v = this->v * s; // Better than *= return *this; } Jet<T, N>& operator/=(const T& s) { - *this = *this / s; + const T s_inverse = T(1.0) / s; + *this *= s_inverse; return *this; } @@ -259,6 +266,10 @@ inline Jet<T, N> const& operator+(const Jet<T, N>& f) { return f; } +template <typename T, int N> +inline Jet<T, N>&& operator+(Jet<T, N>&& f) { + return std::move(f); +} // TODO(keir): Try adding __attribute__((always_inline)) to these functions to // see if it causes a performance increase. @@ -268,60 +279,139 @@ inline Jet<T, N> operator-(const Jet<T, N>& f) { return Jet<T, N>(-f.a, -f.v); } +template <typename T, int N> +inline Jet<T, N>&& operator-(Jet<T, N>&& f) { + f.a = -f.a; + f.v = -f.v; + return std::move(f); +} // Binary + template <typename T, int N> inline Jet<T, N> operator+(const Jet<T, N>& f, const Jet<T, N>& g) { return Jet<T, N>(f.a + g.a, f.v + g.v); } +template <typename T, int N> +inline Jet<T, N>&& operator+(Jet<T, N>&& f, const Jet<T, N>& g) { + f += g; + return std::move(f); +} +template <typename T, int N> +inline Jet<T, N>&& operator+(const Jet<T, N>& f, Jet<T, N>&& g) { + g += f; + return std::move(g); +} +template <typename T, int N> +inline Jet<T, N>&& operator+(Jet<T, N>&& f, Jet<T, N>&& g) { + return std::move(f) + g; +} // Binary + with a scalar: x + s template <typename T, int N> inline Jet<T, N> operator+(const Jet<T, N>& f, T s) { return Jet<T, N>(f.a + s, f.v); } +template <typename T, int N> +inline Jet<T, N>&& operator+(Jet<T, N>&& f, T s) { + f.a += s; + return std::move(f); +} // Binary + with a scalar: s + x template <typename T, int N> inline Jet<T, N> operator+(T s, const Jet<T, N>& f) { return Jet<T, N>(f.a + s, f.v); } +template <typename T, int N> +inline Jet<T, N>&& operator+(T s, Jet<T, N>&& f) { + f.a += s; + return std::move(f); +} // Binary - template <typename T, int N> inline Jet<T, N> operator-(const Jet<T, N>& f, const Jet<T, N>& g) { return Jet<T, N>(f.a - g.a, f.v - g.v); } +template <typename T, int N> +inline Jet<T, N>&& operator-(Jet<T, N>&& f, const Jet<T, N>& g) { + f -= g; + return std::move(f); +} +template <typename T, int N> +inline Jet<T, N>&& operator-(const Jet<T, N>& f, Jet<T, N>&& g) { + g -= f; + return -std::move(g); +} +template <typename T, int N> +inline Jet<T, N>&& operator-(Jet<T, N>&& f, Jet<T, N>&& g) { + return std::move(f) - g; +} // Binary - with a scalar: x - s template <typename T, int N> inline Jet<T, N> operator-(const Jet<T, N>& f, T s) { return Jet<T, N>(f.a - s, f.v); } +template <typename T, int N> +inline Jet<T, N>&& operator-(Jet<T, N>&& f, T s) { + f.a -= s; + return std::move(f); +} // Binary - with a scalar: s - x template <typename T, int N> inline Jet<T, N> operator-(T s, const Jet<T, N>& f) { return Jet<T, N>(s - f.a, -f.v); } +template <typename T, int N> +inline Jet<T, N>&& operator-(T s, Jet<T, N>&& f) { + f.a -= s; + return -std::move(f); +} // Binary * template <typename T, int N> inline Jet<T, N> operator*(const Jet<T, N>& f, const Jet<T, N>& g) { return Jet<T, N>(f.a * g.a, f.a * g.v + f.v * g.a); } +template <typename T, int N> +inline Jet<T, N>&& operator*(Jet<T, N>&& f, const Jet<T, N>& g) { + f *= g; + return std::move(f); +} +template <typename T, int N> +inline Jet<T, N>&& operator*(const Jet<T, N>& f, Jet<T, N>&& g) { + g *= f; + return std::move(g); +} +template <typename T, int N> +inline Jet<T, N>&& operator*(Jet<T, N>&& f, Jet<T, N>&& g) { + return std::move(f) * g; +} // Binary * with a scalar: x * s template <typename T, int N> inline Jet<T, N> operator*(const Jet<T, N>& f, T s) { return Jet<T, N>(f.a * s, f.v * s); } +// Binary * with a scalar: x * s +template <typename T, int N> +inline Jet<T, N>&& operator*(Jet<T, N>&& f, T s) { + f *= s; + return std::move(f); +} // Binary * with a scalar: s * x template <typename T, int N> inline Jet<T, N> operator*(T s, const Jet<T, N>& f) { return Jet<T, N>(f.a * s, f.v * s); } +template <typename T, int N> +inline Jet<T, N>&& operator*(T s, Jet<T, N>&& f) { + f *= s; + return std::move(f); +} // Binary / template <typename T, int N> @@ -337,6 +427,22 @@ const T f_a_by_g_a = f.a * g_a_inverse; return Jet<T, N>(f_a_by_g_a, (f.v - f_a_by_g_a * g.v) * g_a_inverse); } +template <typename T, int N> +inline Jet<T, N>&& operator/(Jet<T, N>&& f, const Jet<T, N>& g) { + f /= g; + return std::move(f); +} +template <typename T, int N> +inline Jet<T, N>&& operator/(const Jet<T, N>& f, Jet<T, N>&& g) { + const T g_a_inverse = T(1.0) / g.a; + g.a = f.a * g_a_inverse; + g.v = (f.v - g.a * g.v) * g_a_inverse; + return std::move(g); +} +template <typename T, int N> +inline Jet<T, N>&& operator/(Jet<T, N>&& f, Jet<T, N>&& g) { + return std::move(f) / g; +} // Binary / with a scalar: s / x template <typename T, int N> @@ -344,6 +450,13 @@ const T minus_s_g_a_inverse2 = -s / (g.a * g.a); return Jet<T, N>(s / g.a, g.v * minus_s_g_a_inverse2); } +template <typename T, int N> +inline Jet<T, N>&& operator/(T s, Jet<T, N>&& g) { + const T minus_s_g_a_inverse2 = -s / (g.a * g.a); + g.a = s / g.a; + g.v = g.v * minus_s_g_a_inverse2; // Better than *= + return std::move(g); +} // Binary / with a scalar: x / s template <typename T, int N> @@ -351,6 +464,11 @@ const T s_inverse = T(1.0) / s; return Jet<T, N>(f.a * s_inverse, f.v * s_inverse); } +template <typename T, int N> +inline Jet<T, N>&& operator/(Jet<T, N>&& f, T s) { + f /= s; + return std::move(f); +} // Binary comparison operators for both scalars and jets. #define CERES_DEFINE_JET_COMPARISON_OPERATOR(op) \
diff --git a/internal/ceres/jet_test.cc b/internal/ceres/jet_test.cc index 36f279d..c65f68d 100644 --- a/internal/ceres/jet_test.cc +++ b/internal/ceres/jet_test.cc
@@ -442,9 +442,13 @@ J b = 1.0 + x; J c = x; c += 1.0; + J d = J{x} + 1.0; + J e = 1.0 + J{x}; ExpectJetsClose(a, b); ExpectJetsClose(a, c); + ExpectJetsClose(a, d); + ExpectJetsClose(a, e); } { // Check that 1 - x == -(x - 1). @@ -452,9 +456,13 @@ J b = -(x - 1.0); J c = x; c -= 1.0; + J d = -(J{x} - 1.0); + J e = 1.0 - J{x}; ExpectJetsClose(a, b); ExpectJetsClose(a, -c); + ExpectJetsClose(a, d); + ExpectJetsClose(a, e); } { // Check that (x/s)*s == (x*s)/s. @@ -464,19 +472,29 @@ c /= 5.0; J d = x; d *= 5.0; + J e = J{x} / 5.0; + J f = J{x} * 5.0; + J g = 1.0 / (5.0 / J{x}); + J h = 5.0 * J{x}; ExpectJetsClose(5.0 * a, b / 5.0); ExpectJetsClose(a, c); ExpectJetsClose(b, d); + ExpectJetsClose(5.0 * e, f / 5.0); + ExpectJetsClose(5.0 * g, h / 5.0); } { // Check that x / y == 1 / (y / x). J a = x / y; J b = 1.0 / (y / x); + J c = J{x} / y; + J d = x / J{y}; VL << "a = " << a; VL << "b = " << b; ExpectJetsClose(a, b); + ExpectJetsClose(a, c); + ExpectJetsClose(a, d); } { // Check that abs(-x * x) == sqrt(x * x).