diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i index d4e959c3dd..2671f0ef77 100644 --- a/gtsam/gtsam.i +++ b/gtsam/gtsam.i @@ -66,27 +66,6 @@ class KeySet { void serialize() const; }; -// Actually a vector -class KeyVector { - KeyVector(); - KeyVector(const gtsam::KeyVector& other); - - // Note: no print function - - // common STL methods - size_t size() const; - bool empty() const; - void clear(); - - // structure specific methods - size_t at(size_t i) const; - size_t front() const; - size_t back() const; - void push_back(size_t key) const; - - void serialize() const; -}; - // Actually a FastMap class KeyGroupMap { KeyGroupMap(); diff --git a/gtsam/nonlinear/CustomFactor.h b/gtsam/nonlinear/CustomFactor.h index 615b5418e8..8580a49496 100644 --- a/gtsam/nonlinear/CustomFactor.h +++ b/gtsam/nonlinear/CustomFactor.h @@ -19,8 +19,6 @@ #include -using namespace gtsam; - namespace gtsam { using JacobianVector = std::vector; diff --git a/gtsam/nonlinear/GncParams.h b/gtsam/nonlinear/GncParams.h index 1f324ae388..e069d5f3d9 100644 --- a/gtsam/nonlinear/GncParams.h +++ b/gtsam/nonlinear/GncParams.h @@ -72,8 +72,14 @@ class GTSAM_EXPORT GncParams { double relativeCostTol = 1e-5; ///< If relative cost change is below this threshold, stop iterating double weightsTol = 1e-4; ///< If the weights are within weightsTol from being binary, stop iterating (only for TLS) Verbosity verbosity = SILENT; ///< Verbosity level - std::vector knownInliers = std::vector(); ///< Slots in the factor graph corresponding to measurements that we know are inliers - std::vector knownOutliers = std::vector(); ///< Slots in the factor graph corresponding to measurements that we know are outliers + + //TODO(Varun) replace IndexVector with vector once pybind11/stl.h is globally enabled. + /// Use IndexVector for inliers and outliers since it is fast + wrapping + using IndexVector = FastVector; + ///< Slots in the factor graph corresponding to measurements that we know are inliers + IndexVector knownInliers = IndexVector(); + ///< Slots in the factor graph corresponding to measurements that we know are outliers + IndexVector knownOutliers = IndexVector(); /// Set the robust loss function to be used in GNC (chosen among the ones in GncLossType). void setLossType(const GncLossType type) { @@ -114,7 +120,7 @@ class GTSAM_EXPORT GncParams { * This functionality is commonly used in SLAM when one may assume the odometry is outlier free, and * only apply GNC to prune outliers from the loop closures. * */ - void setKnownInliers(const std::vector& knownIn) { + void setKnownInliers(const IndexVector& knownIn) { for (size_t i = 0; i < knownIn.size(); i++){ knownInliers.push_back(knownIn[i]); } @@ -125,7 +131,7 @@ class GTSAM_EXPORT GncParams { * corresponds to the slots in the factor graph. For instance, if you have a nonlinear factor graph nfg, * and you provide knownOut = {0, 2, 15}, GNC will not apply outlier rejection to nfg[0], nfg[2], and nfg[15]. * */ - void setKnownOutliers(const std::vector& knownOut) { + void setKnownOutliers(const IndexVector& knownOut) { for (size_t i = 0; i < knownOut.size(); i++){ knownOutliers.push_back(knownOut[i]); } @@ -163,7 +169,7 @@ class GTSAM_EXPORT GncParams { std::cout << "knownInliers: " << knownInliers[i] << "\n"; for (size_t i = 0; i < knownOutliers.size(); i++) std::cout << "knownOutliers: " << knownOutliers[i] << "\n"; - baseOptimizerParams.print(str); + baseOptimizerParams.print("Base optimizer params: "); } }; diff --git a/gtsam/nonlinear/NonlinearFactor.cpp b/gtsam/nonlinear/NonlinearFactor.cpp index 3d572e9702..debff54acd 100644 --- a/gtsam/nonlinear/NonlinearFactor.cpp +++ b/gtsam/nonlinear/NonlinearFactor.cpp @@ -167,8 +167,9 @@ boost::shared_ptr NoiseModelFactor::linearize( return GaussianFactor::shared_ptr( new JacobianFactor(terms, b, boost::static_pointer_cast(noiseModel_)->unit())); - else + else { return GaussianFactor::shared_ptr(new JacobianFactor(terms, b)); + } } /* ************************************************************************* */ diff --git a/gtsam/nonlinear/custom.i b/gtsam/nonlinear/custom.i new file mode 100644 index 0000000000..0d195a91b6 --- /dev/null +++ b/gtsam/nonlinear/custom.i @@ -0,0 +1,38 @@ +//************************************************************************* +// Custom Factor wrapping +//************************************************************************* + +namespace gtsam { + +#include +virtual class CustomFactor : gtsam::NoiseModelFactor { + /* + * Note CustomFactor will not be wrapped for MATLAB, as there is no supporting + * machinery there. This is achieved by adding `gtsam::CustomFactor` to the + * ignore list in `matlab/CMakeLists.txt`. + */ + CustomFactor(); + /* + * Example: + * ``` + * def error_func(this: CustomFactor, v: gtsam.Values, H: List[np.ndarray]): + * + * if not H is None: + * + * H[0] = J1 # 2-d numpy array for a Jacobian block + * H[1] = J2 + * ... + * return error # 1-d numpy array + * + * cf = CustomFactor(noise_model, keys, error_func) + * ``` + */ + CustomFactor(const gtsam::SharedNoiseModel& noiseModel, + const gtsam::KeyVector& keys, + const gtsam::CustomErrorFunction& errorFunction); + + void print(string s = "", + gtsam::KeyFormatter keyFormatter = gtsam::DefaultKeyFormatter); +}; + +} \ No newline at end of file diff --git a/gtsam/nonlinear/nonlinear.i b/gtsam/nonlinear/nonlinear.i index 033e5ced25..4bb80a0d62 100644 --- a/gtsam/nonlinear/nonlinear.i +++ b/gtsam/nonlinear/nonlinear.i @@ -99,11 +99,11 @@ class NonlinearFactorGraph { string dot( const gtsam::Values& values, const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter, - const GraphvizFormatting& writer = GraphvizFormatting()); + const gtsam::GraphvizFormatting& writer = gtsam::GraphvizFormatting()); void saveGraph( const string& s, const gtsam::Values& values, const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter, - const GraphvizFormatting& writer = GraphvizFormatting()) const; + const gtsam::GraphvizFormatting& writer = gtsam::GraphvizFormatting()) const; // enabling serialization functionality void serialize() const; @@ -135,37 +135,6 @@ virtual class NoiseModelFactor : gtsam::NonlinearFactor { Vector whitenedError(const gtsam::Values& x) const; }; -#include -virtual class CustomFactor : gtsam::NoiseModelFactor { - /* - * Note CustomFactor will not be wrapped for MATLAB, as there is no supporting - * machinery there. This is achieved by adding `gtsam::CustomFactor` to the - * ignore list in `matlab/CMakeLists.txt`. - */ - CustomFactor(); - /* - * Example: - * ``` - * def error_func(this: CustomFactor, v: gtsam.Values, H: List[np.ndarray]): - * - * if not H is None: - * - * H[0] = J1 # 2-d numpy array for a Jacobian block - * H[1] = J2 - * ... - * return error # 1-d numpy array - * - * cf = CustomFactor(noise_model, keys, error_func) - * ``` - */ - CustomFactor(const gtsam::SharedNoiseModel& noiseModel, - const gtsam::KeyVector& keys, - const gtsam::CustomErrorFunction& errorFunction); - - void print(string s = "", - gtsam::KeyFormatter keyFormatter = gtsam::DefaultKeyFormatter); -}; - #include class Values { Values(); @@ -544,12 +513,34 @@ virtual class DoglegParams : gtsam::NonlinearOptimizerParams { }; #include +enum GncLossType { + GM /*Geman McClure*/, + TLS /*Truncated least squares*/ +}; + template virtual class GncParams { GncParams(const PARAMS& baseOptimizerParams); GncParams(); - void setVerbosityGNC(const This::Verbosity value); - void print(const string& str) const; + BaseOptimizerParameters baseOptimizerParams; + gtsam::GncLossType lossType; + size_t maxIterations; + double muStep; + double relativeCostTol; + double weightsTol; + Verbosity verbosity; + gtsam::KeyVector knownInliers; + gtsam::KeyVector knownOutliers; + + void setLossType(const gtsam::GncLossType type); + void setMaxIterations(const size_t maxIter); + void setMuStep(const double step); + void setRelativeCostTol(double value); + void setWeightsTol(double value); + void setVerbosityGNC(const gtsam::This::Verbosity value); + void setKnownInliers(const gtsam::KeyVector& knownIn); + void setKnownOutliers(const gtsam::KeyVector& knownOut); + void print(const string& str = "GncParams: ") const; enum Verbosity { SILENT, @@ -597,6 +588,11 @@ virtual class GncOptimizer { GncOptimizer(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& initialValues, const PARAMS& params); + void setInlierCostThresholds(const double inth); + const Vector& getInlierCostThresholds(); + void setInlierCostThresholdsAtProbability(const double alpha); + void setWeights(const Vector w); + const Vector& getWeights(); gtsam::Values optimize(); }; @@ -873,7 +869,7 @@ template , gtsam::imuBias::ConstantBias}> virtual class NonlinearEquality2 : gtsam::NoiseModelFactor { - NonlinearEquality2(Key key1, Key key2, double mu = 1e4); + NonlinearEquality2(gtsam::Key key1, gtsam::Key key2, double mu = 1e4); gtsam::Vector evaluateError(const T& x1, const T& x2); }; diff --git a/matlab/CMakeLists.txt b/matlab/CMakeLists.txt index a657c6be7e..b83bf1f0fe 100644 --- a/matlab/CMakeLists.txt +++ b/matlab/CMakeLists.txt @@ -73,6 +73,7 @@ set(interface_files ${GTSAM_SOURCE_DIR}/gtsam/geometry/geometry.i ${GTSAM_SOURCE_DIR}/gtsam/linear/linear.i ${GTSAM_SOURCE_DIR}/gtsam/nonlinear/nonlinear.i + ${GTSAM_SOURCE_DIR}/gtsam/nonlinear/custom.i ${GTSAM_SOURCE_DIR}/gtsam/symbolic/symbolic.i ${GTSAM_SOURCE_DIR}/gtsam/sam/sam.i ${GTSAM_SOURCE_DIR}/gtsam/slam/slam.i diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index 0d63c6c543..4457678d26 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -61,6 +61,7 @@ set(interface_headers ${PROJECT_SOURCE_DIR}/gtsam/geometry/geometry.i ${PROJECT_SOURCE_DIR}/gtsam/linear/linear.i ${PROJECT_SOURCE_DIR}/gtsam/nonlinear/nonlinear.i + ${PROJECT_SOURCE_DIR}/gtsam/nonlinear/custom.i ${PROJECT_SOURCE_DIR}/gtsam/symbolic/symbolic.i ${PROJECT_SOURCE_DIR}/gtsam/sam/sam.i ${PROJECT_SOURCE_DIR}/gtsam/slam/slam.i diff --git a/python/gtsam/preamble/custom.h b/python/gtsam/preamble/custom.h new file mode 100644 index 0000000000..d07a75f6fb --- /dev/null +++ b/python/gtsam/preamble/custom.h @@ -0,0 +1,12 @@ +/* Please refer to: + * https://pybind11.readthedocs.io/en/stable/advanced/cast/stl.html + * These are required to save one copy operation on Python calls. + * + * NOTES + * ================= + * + * `PYBIND11_MAKE_OPAQUE` will mark the type as "opaque" for the pybind11 + * automatic STL binding, such that the raw objects can be accessed in Python. + * Without this they will be automatically converted to a Python object, and all + * mutations on Python side will not be reflected on C++. + */ diff --git a/python/gtsam/preamble/nonlinear.h b/python/gtsam/preamble/nonlinear.h index a34e730580..d07a75f6fb 100644 --- a/python/gtsam/preamble/nonlinear.h +++ b/python/gtsam/preamble/nonlinear.h @@ -9,4 +9,4 @@ * automatic STL binding, such that the raw objects can be accessed in Python. * Without this they will be automatically converted to a Python object, and all * mutations on Python side will not be reflected on C++. - */ \ No newline at end of file + */ diff --git a/python/gtsam/specializations/custom.h b/python/gtsam/specializations/custom.h new file mode 100644 index 0000000000..d46ccdc668 --- /dev/null +++ b/python/gtsam/specializations/custom.h @@ -0,0 +1,12 @@ +/* Please refer to: + * https://pybind11.readthedocs.io/en/stable/advanced/cast/stl.html + * These are required to save one copy operation on Python calls. + * + * NOTES + * ================= + * + * `py::bind_vector` and similar machinery gives the std container a Python-like + * interface, but without the `` copying mechanism. Combined + * with `PYBIND11_MAKE_OPAQUE` this allows the types to be modified with Python, + * and saves one copy operation. + */ \ No newline at end of file diff --git a/python/gtsam/specializations/nonlinear.h b/python/gtsam/specializations/nonlinear.h index d46ccdc668..da8842eaf4 100644 --- a/python/gtsam/specializations/nonlinear.h +++ b/python/gtsam/specializations/nonlinear.h @@ -9,4 +9,4 @@ * interface, but without the `` copying mechanism. Combined * with `PYBIND11_MAKE_OPAQUE` this allows the types to be modified with Python, * and saves one copy operation. - */ \ No newline at end of file + */ diff --git a/python/gtsam/tests/test_NonlinearOptimizer.py b/python/gtsam/tests/test_NonlinearOptimizer.py index 37afd9e1c0..a47c5ad620 100644 --- a/python/gtsam/tests/test_NonlinearOptimizer.py +++ b/python/gtsam/tests/test_NonlinearOptimizer.py @@ -15,10 +15,12 @@ import unittest import gtsam -from gtsam import (DoglegOptimizer, DoglegParams, DummyPreconditionerParameters, - GaussNewtonOptimizer, GaussNewtonParams, GncLMParams, GncLMOptimizer, - LevenbergMarquardtOptimizer, LevenbergMarquardtParams, NonlinearFactorGraph, - Ordering, PCGSolverParameters, Point2, PriorFactorPoint2, Values) +from gtsam import ( + DoglegOptimizer, DoglegParams, DummyPreconditionerParameters, GaussNewtonOptimizer, + GaussNewtonParams, GncLMParams, GncLossType, GncLMOptimizer, LevenbergMarquardtOptimizer, + LevenbergMarquardtParams, NonlinearFactorGraph, Ordering, PCGSolverParameters, Point2, + PriorFactorPoint2, Values +) from gtsam.utils.test_case import GtsamTestCase KEY1 = 1 @@ -27,7 +29,6 @@ class TestScenario(GtsamTestCase): """Do trivial test with three optimizer variants.""" - def setUp(self): """Set up the optimization problem and ordering""" # create graph @@ -83,16 +84,83 @@ def test_graduated_non_convexity(self): actual = GncLMOptimizer(self.fg, self.initial_values, gncParams).optimize() self.assertAlmostEqual(0, self.fg.error(actual)) + def test_gnc_params(self): + base_params = LevenbergMarquardtParams() + # Test base params + for base_max_iters in (50, 100): + base_params.setMaxIterations(base_max_iters) + params = GncLMParams(base_params) + self.assertEqual(params.baseOptimizerParams.getMaxIterations(), base_max_iters) + + # Test printing + params_str = str(params) + for s in ( + "lossType", + "maxIterations", + "muStep", + "relativeCostTol", + "weightsTol", + "verbosity", + ): + self.assertTrue(s in params_str) + + # Test each parameter + for loss_type in (GncLossType.TLS, GncLossType.GM): + params.setLossType(loss_type) # Default is TLS + self.assertEqual(params.lossType, loss_type) + for max_iter in (1, 10, 100): + params.setMaxIterations(max_iter) + self.assertEqual(params.maxIterations, max_iter) + for mu_step in (1.1, 1.2, 1.5): + params.setMuStep(mu_step) + self.assertEqual(params.muStep, mu_step) + for rel_cost_tol in (1e-5, 1e-6, 1e-7): + params.setRelativeCostTol(rel_cost_tol) + self.assertEqual(params.relativeCostTol, rel_cost_tol) + for weights_tol in (1e-4, 1e-3, 1e-2): + params.setWeightsTol(weights_tol) + self.assertEqual(params.weightsTol, weights_tol) + for i in (0, 1, 2): + verb = GncLMParams.Verbosity(i) + params.setVerbosityGNC(verb) + self.assertEqual(params.verbosity, verb) + for inl in ([], [10], [0, 100]): + params.setKnownInliers(inl) + self.assertEqual(params.knownInliers, inl) + params.knownInliers = [] + for out in ([], [1], [0, 10]): + params.setKnownInliers(out) + self.assertEqual(params.knownInliers, out) + params.knownInliers = [] + + # Test optimizer params + optimizer = GncLMOptimizer(self.fg, self.initial_values, params) + for ict_factor in (0.9, 1.1): + new_ict = ict_factor * optimizer.getInlierCostThresholds() + optimizer.setInlierCostThresholds(new_ict) + self.assertAlmostEqual(optimizer.getInlierCostThresholds(), new_ict) + for w_factor in (0.8, 0.9): + new_weights = w_factor * optimizer.getWeights() + optimizer.setWeights(new_weights) + self.assertAlmostEqual(optimizer.getWeights(), new_weights) + optimizer.setInlierCostThresholdsAtProbability(0.9) + w1 = optimizer.getInlierCostThresholds() + optimizer.setInlierCostThresholdsAtProbability(0.8) + w2 = optimizer.getInlierCostThresholds() + self.assertLess(w2, w1) + def test_iteration_hook(self): # set up iteration hook to track some testable values iteration_count = 0 final_error = 0 final_values = None + def iteration_hook(iter, error_before, error_after): nonlocal iteration_count, final_error, final_values iteration_count = iter final_error = error_after final_values = optimizer.values() + # optimize params = LevenbergMarquardtParams.CeresDefaults() params.setOrdering(self.ordering) @@ -104,5 +172,6 @@ def iteration_hook(iter, error_before, error_after): self.assertEqual(self.fg.error(actual), final_error) self.assertEqual(optimizer.iterations(), iteration_count) + if __name__ == "__main__": unittest.main() diff --git a/tests/testGncOptimizer.cpp b/tests/testGncOptimizer.cpp index c3335ce20c..15d660114f 100644 --- a/tests/testGncOptimizer.cpp +++ b/tests/testGncOptimizer.cpp @@ -567,7 +567,7 @@ TEST(GncOptimizer, optimizeWithKnownInliers) { Values initial; initial.insert(X(1), p0); - std::vector knownInliers; + GncParams::IndexVector knownInliers; knownInliers.push_back(0); knownInliers.push_back(1); knownInliers.push_back(2); @@ -644,7 +644,7 @@ TEST(GncOptimizer, barcsq) { Values initial; initial.insert(X(1), p0); - std::vector knownInliers; + GncParams::IndexVector knownInliers; knownInliers.push_back(0); knownInliers.push_back(1); knownInliers.push_back(2); @@ -691,7 +691,7 @@ TEST(GncOptimizer, setInlierCostThresholds) { Values initial; initial.insert(X(1), p0); - std::vector knownInliers; + GncParams::IndexVector knownInliers; knownInliers.push_back(0); knownInliers.push_back(1); knownInliers.push_back(2); @@ -763,7 +763,7 @@ TEST(GncOptimizer, optimizeSmallPoseGraph) { // GNC // Note: in difficult instances, we set the odometry measurements to be // inliers, but this problem is simple enought to succeed even without that - // assumption std::vector knownInliers; + // assumption GncParams::IndexVector knownInliers; GncParams gncParams; auto gnc = GncOptimizer>(*graph, *initial, gncParams); @@ -784,12 +784,12 @@ TEST(GncOptimizer, knownInliersAndOutliers) { // nonconvexity with known inliers and known outliers (check early stopping // when all measurements are known to be inliers or outliers) { - std::vector knownInliers; + GncParams::IndexVector knownInliers; knownInliers.push_back(0); knownInliers.push_back(1); knownInliers.push_back(2); - std::vector knownOutliers; + GncParams::IndexVector knownOutliers; knownOutliers.push_back(3); GncParams gncParams; @@ -813,11 +813,11 @@ TEST(GncOptimizer, knownInliersAndOutliers) { // nonconvexity with known inliers and known outliers { - std::vector knownInliers; + GncParams::IndexVector knownInliers; knownInliers.push_back(2); knownInliers.push_back(0); - std::vector knownOutliers; + GncParams::IndexVector knownOutliers; knownOutliers.push_back(3); GncParams gncParams; @@ -841,7 +841,7 @@ TEST(GncOptimizer, knownInliersAndOutliers) { // only known outliers { - std::vector knownOutliers; + GncParams::IndexVector knownOutliers; knownOutliers.push_back(3); GncParams gncParams; @@ -916,11 +916,11 @@ TEST(GncOptimizer, setWeights) { // initialize weights and also set known inliers/outliers { GncParams gncParams; - std::vector knownInliers; + GncParams::IndexVector knownInliers; knownInliers.push_back(2); knownInliers.push_back(0); - std::vector knownOutliers; + GncParams::IndexVector knownOutliers; knownOutliers.push_back(3); gncParams.setKnownInliers(knownInliers); gncParams.setKnownOutliers(knownOutliers);