Allow construction of an AutoDiffLocalParameterization with a functor.
- Previously AutoDiffLocalParameterization would internally instantiate
a functor instance whenever one was required. This prohibits the
user passing arguments to the constructor of the functor.
- Now AutoDiffLocalParameterization can take over ownership of an
allocated functor which the user created. This mimics the behaviour
of AutoDiffCostFunction.
Change-Id: I264e1face44ca5d5e71cc20c77cc7654d3f74cc0
diff --git a/include/ceres/autodiff_local_parameterization.h b/include/ceres/autodiff_local_parameterization.h
index 0aae6c7..c100d48 100644
--- a/include/ceres/autodiff_local_parameterization.h
+++ b/include/ceres/autodiff_local_parameterization.h
@@ -107,11 +107,18 @@
template <typename Functor, int kGlobalSize, int kLocalSize>
class AutoDiffLocalParameterization : public LocalParameterization {
public:
+ AutoDiffLocalParameterization() :
+ functor_(new Functor()) {}
+
+ // Takes ownership of functor.
+ explicit AutoDiffLocalParameterization(Functor* functor) :
+ functor_(functor) {}
+
virtual ~AutoDiffLocalParameterization() {}
virtual bool Plus(const double* x,
const double* delta,
double* x_plus_delta) const {
- return Functor()(x, delta, x_plus_delta);
+ return (*functor_)(x, delta, x_plus_delta);
}
virtual bool ComputeJacobian(const double* x, double* jacobian) const {
@@ -128,7 +135,7 @@
const double* parameter_ptrs[2] = {x, zero_delta};
double* jacobian_ptrs[2] = { NULL, jacobian };
return internal::AutoDiff<Functor, double, kGlobalSize, kLocalSize>
- ::Differentiate(Functor(),
+ ::Differentiate(*functor_,
parameter_ptrs,
kGlobalSize,
x_plus_delta,
@@ -137,6 +144,9 @@
virtual int GlobalSize() const { return kGlobalSize; }
virtual int LocalSize() const { return kLocalSize; }
+
+ private:
+ internal::scoped_ptr<Functor> functor_;
};
} // namespace ceres
diff --git a/internal/ceres/autodiff_local_parameterization_test.cc b/internal/ceres/autodiff_local_parameterization_test.cc
index 7e90177..a0f705d 100644
--- a/internal/ceres/autodiff_local_parameterization_test.cc
+++ b/internal/ceres/autodiff_local_parameterization_test.cc
@@ -48,7 +48,6 @@
}
};
-
TEST(AutoDiffLocalParameterizationTest, IdentityParameterization) {
AutoDiffLocalParameterization<IdentityPlus, 3, 3>
parameterization;
@@ -72,6 +71,47 @@
}
}
+struct ScaledPlus {
+ ScaledPlus(const double &scale_factor)
+ : scale_factor_(scale_factor)
+ {}
+
+ template <typename T>
+ bool operator()(const T* x, const T* delta, T* x_plus_delta) const {
+ for (int i = 0; i < 3; ++i) {
+ x_plus_delta[i] = x[i] + T(scale_factor_) * delta[i];
+ }
+ return true;
+ }
+
+ const double scale_factor_;
+};
+
+TEST(AutoDiffLocalParameterizationTest, ScaledParameterization) {
+ const double kTolerance = 1e-14;
+
+ AutoDiffLocalParameterization<ScaledPlus, 3, 3>
+ parameterization(new ScaledPlus(1.2345));
+
+ double x[3] = {1.0, 2.0, 3.0};
+ double delta[3] = {0.0, 1.0, 2.0};
+ double x_plus_delta[3] = {0.0, 0.0, 0.0};
+ parameterization.Plus(x, delta, x_plus_delta);
+
+ EXPECT_NEAR(x_plus_delta[0], 1.0, kTolerance);
+ EXPECT_NEAR(x_plus_delta[1], 3.2345, kTolerance);
+ EXPECT_NEAR(x_plus_delta[2], 5.469, kTolerance);
+
+ double jacobian[9];
+ parameterization.ComputeJacobian(x, jacobian);
+ int k = 0;
+ for (int i = 0; i < 3; ++i) {
+ for (int j = 0; j < 3; ++j, ++k) {
+ EXPECT_NEAR(jacobian[k], (i == j) ? 1.2345 : 0.0, kTolerance);
+ }
+ }
+}
+
struct QuaternionPlus {
template<typename T>
bool operator()(const T* x, const T* delta, T* x_plus_delta) const {
