From 3f4731a9482d71ce65174140713b9c0384a38eec Mon Sep 17 00:00:00 2001
From: Fan Jiang <i@fanjiang.me>
Date: Fri, 10 Jul 2020 15:00:57 -0400
Subject: [PATCH 1/3] Added wrapping for the PCG solver in Cython

---
 gtsam.h                    | 16 ++++++++++++++++
 gtsam/linear/PCGSolver.cpp | 11 +++++++++++
 gtsam/linear/PCGSolver.h   |  4 ++++
 3 files changed, 31 insertions(+)

diff --git a/gtsam.h b/gtsam.h
index 614db91c7..d65186439 100644
--- a/gtsam.h
+++ b/gtsam.h
@@ -1937,6 +1937,22 @@ virtual class ConjugateGradientParameters : gtsam::IterativeOptimizationParamete
   void print() const;
 };
 
+#include <gtsam/linear/Preconditioner.h>
+virtual class PreconditionerParameters {
+  PreconditionerParameters();
+};
+
+virtual class DummyPreconditionerParameters : gtsam::PreconditionerParameters {
+  DummyPreconditionerParameters();
+};
+
+#include <gtsam/linear/PCGSolver.h>
+virtual class PCGSolverParameters : gtsam::ConjugateGradientParameters {
+  PCGSolverParameters();
+  void print(string s);
+  void setPreconditionerParams(gtsam::PreconditionerParameters* preconditioner);
+};
+
 #include <gtsam/linear/SubgraphSolver.h>
 virtual class SubgraphSolverParameters : gtsam::ConjugateGradientParameters {
   SubgraphSolverParameters();
diff --git a/gtsam/linear/PCGSolver.cpp b/gtsam/linear/PCGSolver.cpp
index 08307c5ab..a7af7d8d8 100644
--- a/gtsam/linear/PCGSolver.cpp
+++ b/gtsam/linear/PCGSolver.cpp
@@ -45,6 +45,17 @@ PCGSolver::PCGSolver(const PCGSolverParameters &p) {
   preconditioner_ = createPreconditioner(p.preconditioner_);
 }
 
+void PCGSolverParameters::setPreconditionerParams(const boost::shared_ptr<PreconditionerParameters> preconditioner) {
+  preconditioner_ = preconditioner;
+}
+
+void PCGSolverParameters::print(const std::string &s) const {
+  std::cout << s << std::endl;;
+  std::ostringstream os;
+  print(os);
+  std::cout << os.str() << std::endl;
+}
+
 /*****************************************************************************/
 VectorValues PCGSolver::optimize(const GaussianFactorGraph &gfg,
     const KeyInfo &keyInfo, const std::map<Key, Vector> &lambda,
diff --git a/gtsam/linear/PCGSolver.h b/gtsam/linear/PCGSolver.h
index f5b278ae5..3e72c7cbe 100644
--- a/gtsam/linear/PCGSolver.h
+++ b/gtsam/linear/PCGSolver.h
@@ -48,7 +48,11 @@ public:
     return *preconditioner_;
   }
 
+  void print(const std::string &s) const;
+
   boost::shared_ptr<PreconditionerParameters> preconditioner_;
+
+  void setPreconditionerParams(const boost::shared_ptr<PreconditionerParameters> preconditioner);
 };
 
 /**

From 7f293eb84e66b6468c92257e52741fbdfd8d13b0 Mon Sep 17 00:00:00 2001
From: Fan Jiang <i@fanjiang.me>
Date: Fri, 10 Jul 2020 15:01:54 -0400
Subject: [PATCH 2/3] add comments

---
 gtsam/linear/PCGSolver.h | 1 +
 1 file changed, 1 insertion(+)

diff --git a/gtsam/linear/PCGSolver.h b/gtsam/linear/PCGSolver.h
index 3e72c7cbe..7752902ba 100644
--- a/gtsam/linear/PCGSolver.h
+++ b/gtsam/linear/PCGSolver.h
@@ -48,6 +48,7 @@ public:
     return *preconditioner_;
   }
 
+  // needed for python wrapper
   void print(const std::string &s) const;
 
   boost::shared_ptr<PreconditionerParameters> preconditioner_;

From 25513379e319132bb0b79c4873500e22ec330945 Mon Sep 17 00:00:00 2001
From: Fan Jiang <i@fanjiang.me>
Date: Sat, 11 Jul 2020 14:37:54 -0400
Subject: [PATCH 3/3] Add unit test

---
 cython/gtsam/tests/test_NonlinearOptimizer.py | 13 ++++++++++++-
 1 file changed, 12 insertions(+), 1 deletion(-)

diff --git a/cython/gtsam/tests/test_NonlinearOptimizer.py b/cython/gtsam/tests/test_NonlinearOptimizer.py
index efefb218a..985dc30a2 100644
--- a/cython/gtsam/tests/test_NonlinearOptimizer.py
+++ b/cython/gtsam/tests/test_NonlinearOptimizer.py
@@ -17,7 +17,8 @@ import unittest
 import gtsam
 from gtsam import (DoglegOptimizer, DoglegParams, GaussNewtonOptimizer,
                    GaussNewtonParams, LevenbergMarquardtOptimizer,
-                   LevenbergMarquardtParams, NonlinearFactorGraph, Ordering,
+                   LevenbergMarquardtParams, PCGSolverParameters,
+                   DummyPreconditionerParameters, NonlinearFactorGraph, Ordering,
                    Point2, PriorFactorPoint2, Values)
 from gtsam.utils.test_case import GtsamTestCase
 
@@ -61,6 +62,16 @@ class TestScenario(GtsamTestCase):
             fg, initial_values, lmParams).optimize()
         self.assertAlmostEqual(0, fg.error(actual2))
 
+        # Levenberg-Marquardt
+        lmParams = LevenbergMarquardtParams.CeresDefaults()
+        lmParams.setLinearSolverType("ITERATIVE")
+        cgParams = PCGSolverParameters()
+        cgParams.setPreconditionerParams(DummyPreconditionerParameters())
+        lmParams.setIterativeParams(cgParams)
+        actual2 = LevenbergMarquardtOptimizer(
+            fg, initial_values, lmParams).optimize()
+        self.assertAlmostEqual(0, fg.error(actual2))
+
         # Dogleg
         dlParams = DoglegParams()
         dlParams.setOrdering(ordering)