Merge pull request #843 from borglab/wrap-gnc

Wrap GNC

gtsam/nonlinear/GncOptimizer.h

@@ -42,7 +42,7 @@ static double Chi2inv(const double alpha, const size_t dofs) {
 
 /* ************************************************************************* */
 template<class GncParameters>
-class GncOptimizer {
+class GTSAM_EXPORT GncOptimizer {
  public:
   /// For each parameter, specify the corresponding optimizer: e.g., GaussNewtonParams -> GaussNewtonOptimizer.
   typedef typename GncParameters::OptimizerType BaseOptimizer;

gtsam/nonlinear/GncParams.h

@@ -39,7 +39,7 @@ enum GncLossType {
 };
 
 template<class BaseOptimizerParameters>
-class GncParams {
+class GTSAM_EXPORT GncParams {
  public:
   /// For each parameter, specify the corresponding optimizer: e.g., GaussNewtonParams -> GaussNewtonOptimizer.
   typedef typename BaseOptimizerParameters::OptimizerType OptimizerType;

gtsam/nonlinear/nonlinear.i

@@ -522,6 +522,17 @@ virtual class DoglegParams : gtsam::NonlinearOptimizerParams {
   void setVerbosityDL(string verbosityDL) const;
 };
 
+#include <gtsam/nonlinear/GncParams.h>
+template<PARAMS>
+virtual class GncParams {
+  GncParams(const PARAMS& baseOptimizerParams);
+  GncParams();
+  void print(const string& str) const;
+};
+
+typedef gtsam::GncParams<gtsam::GaussNewtonParams> GncGaussNewtonParams;
+typedef gtsam::GncParams<gtsam::LevenbergMarquardtParams> GncLMParams;
+  
 #include <gtsam/nonlinear/NonlinearOptimizer.h>
 virtual class NonlinearOptimizer {
   gtsam::Values optimize();
@@ -551,6 +562,18 @@ virtual class DoglegOptimizer : gtsam::NonlinearOptimizer {
                   const gtsam::DoglegParams& params);
   double getDelta() const;
 };
+  
+#include <gtsam/nonlinear/GncOptimizer.h>
+template<PARAMS>
+virtual class GncOptimizer {
+  GncOptimizer(const gtsam::NonlinearFactorGraph& graph,
+               const gtsam::Values& initialValues,
+               const PARAMS& params);
+  gtsam::Values optimize();
+};
+
+typedef gtsam::GncOptimizer<gtsam::GncParams<gtsam::GaussNewtonParams> > GncGaussNewtonOptimizer;
+typedef gtsam::GncOptimizer<gtsam::GncParams<gtsam::LevenbergMarquardtParams> > GncLMOptimizer;
 
 #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
 virtual class LevenbergMarquardtOptimizer : gtsam::NonlinearOptimizer {

python/gtsam/tests/test_NonlinearOptimizer.py

@@ -17,8 +17,9 @@ import unittest
 import gtsam
 from gtsam import (DoglegOptimizer, DoglegParams,
                    DummyPreconditionerParameters, GaussNewtonOptimizer,
-                   GaussNewtonParams, LevenbergMarquardtOptimizer,
-                   LevenbergMarquardtParams, NonlinearFactorGraph, Ordering,
+                   GaussNewtonParams, GncLMParams, GncLMOptimizer,
+                   LevenbergMarquardtOptimizer, LevenbergMarquardtParams,
+                   NonlinearFactorGraph, Ordering,
                    PCGSolverParameters, Point2, PriorFactorPoint2, Values)
 from gtsam.utils.test_case import GtsamTestCase
 
@@ -77,6 +78,12 @@ class TestScenario(GtsamTestCase):
         dlParams.setOrdering(ordering)
         actual3 = DoglegOptimizer(fg, initial_values, dlParams).optimize()
         self.assertAlmostEqual(0, fg.error(actual3))
+        
+        # Graduated Non-Convexity (GNC)
+        gncParams = GncLMParams()
+        actual4 = GncLMOptimizer(fg, initial_values, gncParams).optimize()
+        self.assertAlmostEqual(0, fg.error(actual4))
+        
 
 
 if __name__ == "__main__":