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

include/ceres/autodiff_local_parameterization.h

@@ -107,11 +107,18 @@ namespace ceres {
 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 @@ class AutoDiffLocalParameterization : public LocalParameterization {
     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 @@ class AutoDiffLocalParameterization : public LocalParameterization {
 
   virtual int GlobalSize() const { return kGlobalSize; }
   virtual int LocalSize() const { return kLocalSize; }
+
+ private:
+  internal::scoped_ptr<Functor> functor_;
 };
 
 }  // namespace ceres

internal/ceres/autodiff_local_parameterization_test.cc

@@ -48,7 +48,6 @@ struct IdentityPlus {
   }
 };
 
-
 TEST(AutoDiffLocalParameterizationTest, IdentityParameterization) {
   AutoDiffLocalParameterization<IdentityPlus, 3, 3>
       parameterization;
@@ -72,6 +71,47 @@ TEST(AutoDiffLocalParameterizationTest, IdentityParameterization) {
   }
 }
 
+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 {