1 files changed, 36 insertions, 10 deletions

diff --git a/extern/ceres/internal/ceres/gradient_problem.cc b/extern/ceres/internal/ceres/gradient_problem.cc
index ba33fbc90f8..cdd472fe87f 100644
--- a/extern/ceres/internal/ceres/gradient_problem.cc
+++ b/extern/ceres/internal/ceres/gradient_problem.cc
@@ -30,49 +30,75 @@
 
 #include "ceres/gradient_problem.h"
 
+#include <memory>
+
 #include "ceres/local_parameterization.h"
+#include "ceres/manifold_adapter.h"
 #include "glog/logging.h"
 
 namespace ceres {
 
 GradientProblem::GradientProblem(FirstOrderFunction* function)
     : function_(function),
-      parameterization_(
-          new IdentityParameterization(function_->NumParameters())),
-      scratch_(new double[function_->NumParameters()]) {}
+      manifold_(std::make_unique<EuclideanManifold<DYNAMIC>>(
+          function_->NumParameters())),
+      scratch_(new double[function_->NumParameters()]) {
+  CHECK(function != nullptr);
+}
 
 GradientProblem::GradientProblem(FirstOrderFunction* function,
                                  LocalParameterization* parameterization)
     : function_(function),
       parameterization_(parameterization),
       scratch_(new double[function_->NumParameters()]) {
-  CHECK_EQ(function_->NumParameters(), parameterization_->GlobalSize());
+  CHECK(function != nullptr);
+  if (parameterization != nullptr) {
+    manifold_ =
+        std::make_unique<internal::ManifoldAdapter>(parameterization_.get());
+  } else {
+    manifold_ = std::make_unique<EuclideanManifold<DYNAMIC>>(
+        function_->NumParameters());
+  }
+  CHECK_EQ(function_->NumParameters(), manifold_->AmbientSize());
+}
+
+GradientProblem::GradientProblem(FirstOrderFunction* function,
+                                 Manifold* manifold)
+    : function_(function), scratch_(new double[function_->NumParameters()]) {
+  CHECK(function != nullptr);
+  if (manifold != nullptr) {
+    manifold_.reset(manifold);
+  } else {
+    manifold_ = std::make_unique<EuclideanManifold<DYNAMIC>>(
+        function_->NumParameters());
+  }
+  CHECK_EQ(function_->NumParameters(), manifold_->AmbientSize());
 }
 
 int GradientProblem::NumParameters() const {
   return function_->NumParameters();
 }
 
-int GradientProblem::NumLocalParameters() const {
-  return parameterization_->LocalSize();
+int GradientProblem::NumTangentParameters() const {
+  return manifold_->TangentSize();
 }
 
 bool GradientProblem::Evaluate(const double* parameters,
                                double* cost,
                                double* gradient) const {
-  if (gradient == NULL) {
-    return function_->Evaluate(parameters, cost, NULL);
+  if (gradient == nullptr) {
+    return function_->Evaluate(parameters, cost, nullptr);
   }
 
   return (function_->Evaluate(parameters, cost, scratch_.get()) &&
-          parameterization_->MultiplyByJacobian(
+          manifold_->RightMultiplyByPlusJacobian(
               parameters, 1, scratch_.get(), gradient));
 }
 
 bool GradientProblem::Plus(const double* x,
                            const double* delta,
                            double* x_plus_delta) const {
-  return parameterization_->Plus(x, delta, x_plus_delta);
+  return manifold_->Plus(x, delta, x_plus_delta);
 }
 
 }  // namespace ceres