diff --git a/.cproject b/.cproject index 6423ee51a..7aa0b3f60 100644 --- a/.cproject +++ b/.cproject @@ -258,14 +258,6 @@ true true - - make - -j2 - testGaussianFactor.run - true - true - true - make -j2 @@ -292,6 +284,7 @@ make + tests/testBayesTree.run true false @@ -299,6 +292,7 @@ make + testBinaryBayesNet.run true false @@ -346,6 +340,7 @@ make + testSymbolicBayesNet.run true false @@ -353,6 +348,7 @@ make + tests/testSymbolicFactor.run true false @@ -360,6 +356,7 @@ make + testSymbolicFactorGraph.run true false @@ -375,11 +372,20 @@ make + tests/testBayesTree true false true + + make + -j2 + testGaussianFactor.run + true + true + true + make -j2 @@ -406,7 +412,6 @@ make - testGraph.run true false @@ -478,7 +483,6 @@ make - testInference.run true false @@ -486,7 +490,6 @@ make - testGaussianFactor.run true false @@ -494,7 +497,6 @@ make - testJunctionTree.run true false @@ -502,7 +504,6 @@ make - testSymbolicBayesNet.run true false @@ -510,7 +511,6 @@ make - testSymbolicFactorGraph.run true false @@ -580,22 +580,6 @@ false true - - make - -j2 - all - true - true - true - - - make - -j2 - clean - true - true - true - make -j2 @@ -612,6 +596,22 @@ true true + + make + -j2 + all + true + true + true + + + make + -j2 + clean + true + true + true + make -j2 @@ -636,7 +636,15 @@ true true - + + make + -j2 + all + true + true + true + + make -j2 check @@ -644,6 +652,14 @@ true true + + make + -j2 + clean + true + true + true + make -j2 @@ -684,15 +700,7 @@ true true - - make - -j2 - all - true - true - true - - + make -j2 check @@ -700,14 +708,6 @@ true true - - make - -j2 - clean - true - true - true - make -j2 @@ -1038,6 +1038,7 @@ make + testErrors.run true false @@ -1493,7 +1494,6 @@ make - testSimulated2DOriented.run true false @@ -1533,7 +1533,6 @@ make - testSimulated2D.run true false @@ -1541,7 +1540,6 @@ make - testSimulated3D.run true false @@ -1908,7 +1906,6 @@ make - tests/testGaussianISAM2 true false @@ -1930,6 +1927,46 @@ true true + + make + -j2 + install + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + check + true + true + true + + + make + -j2 + all + true + true + true + + + make + -j2 + dist + true + true + true + make -j2 @@ -2026,94 +2063,6 @@ true true - - make - -j2 - install - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - check - true - true - true - - - make - -j2 - all - true - true - true - - - make - -j2 - dist - true - true - true - - - make - -j2 - check - true - true - true - - - make - -j2 - install - true - true - true - - - make - -j2 - tests/testSpirit.run - true - true - true - - - make - -j2 - tests/testWrap.run - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - all - true - true - true - make -j2 @@ -2178,6 +2127,62 @@ false true + + make + -j2 + check install + true + true + true + + + make + -j2 + check + true + true + true + + + make + -j2 + install + true + true + true + + + make + -j2 + tests/testSpirit.run + true + true + true + + + make + -j2 + tests/testWrap.run + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + all + true + true + true + diff --git a/examples/Pose2SLAMExample_advanced.cpp b/examples/Pose2SLAMExample_advanced.cpp index 3ad5f4d7f..998ed3af6 100644 --- a/examples/Pose2SLAMExample_advanced.cpp +++ b/examples/Pose2SLAMExample_advanced.cpp @@ -68,7 +68,7 @@ int main(int argc, char** argv) { Ordering::shared_ptr ordering = graph->orderingCOLAMD(*initial); /* 4.2.2 set up solver and optimize */ - NonlinearOptimizationParameters::sharedThis params = NonlinearOptimizationParameters::newDrecreaseThresholds(1e-15, 1e-15); + NonlinearOptimizationParameters::shared_ptr params = NonlinearOptimizationParameters::newDrecreaseThresholds(1e-15, 1e-15); Optimizer optimizer(graph, initial, ordering, params); Optimizer optimizer_result = optimizer.levenbergMarquardt(); diff --git a/examples/vSLAMexample/vSFMexample.cpp b/examples/vSLAMexample/vSFMexample.cpp index f5e386ed7..4647bf95c 100644 --- a/examples/vSLAMexample/vSFMexample.cpp +++ b/examples/vSLAMexample/vSFMexample.cpp @@ -146,7 +146,7 @@ int main(int argc, char* argv[]) { // Optimize the graph cout << "*******************************************************" << endl; - NonlinearOptimizationParameters::sharedThis params = NonlinearOptimizationParameters::newVerbosity(Optimizer::Parameters::DAMPED); + NonlinearOptimizationParameters::shared_ptr params = NonlinearOptimizationParameters::newVerbosity(Optimizer::Parameters::DAMPED); visualSLAM::Optimizer::shared_values result = visualSLAM::Optimizer::optimizeGN(graph, initialEstimates, params); // Print final results diff --git a/gtsam/geometry/Makefile.am b/gtsam/geometry/Makefile.am index c0cfb66fc..5f9b34610 100644 --- a/gtsam/geometry/Makefile.am +++ b/gtsam/geometry/Makefile.am @@ -18,7 +18,7 @@ sources += Point2.cpp Rot2.cpp Pose2.cpp Point3.cpp Pose3.cpp check_PROGRAMS += tests/testPoint2 tests/testRot2 tests/testPose2 tests/testPoint3 tests/testRot3M tests/testRot3Q tests/testPose3 # Cameras -headers += GeneralCameraT.h Cal3_S2Stereo.h +headers += GeneralCameraT.h Cal3_S2Stereo.h PinholeCamera.h sources += Cal3_S2.cpp Cal3DS2.cpp Cal3Bundler.cpp CalibratedCamera.cpp SimpleCamera.cpp check_PROGRAMS += tests/testCal3DS2 tests/testCal3_S2 tests/testCal3Bundler tests/testCalibratedCamera tests/testSimpleCamera diff --git a/gtsam/geometry/PinholeCamera.h b/gtsam/geometry/PinholeCamera.h index bc0364970..b8c6428bb 100644 --- a/gtsam/geometry/PinholeCamera.h +++ b/gtsam/geometry/PinholeCamera.h @@ -120,7 +120,7 @@ namespace gtsam { /// move a cameras according to d PinholeCamera retract(const Vector& d) const { - if ( d.size() == pose_.dim() ) + if ((size_t) d.size() == pose_.dim() ) return PinholeCamera(pose().retract(d), calibration()) ; else return PinholeCamera(pose().retract(d.head(pose().dim())), diff --git a/gtsam/nonlinear/Makefile.am b/gtsam/nonlinear/Makefile.am index 58503466c..6d5f126c8 100644 --- a/gtsam/nonlinear/Makefile.am +++ b/gtsam/nonlinear/Makefile.am @@ -22,9 +22,9 @@ check_PROGRAMS += tests/testValues tests/testKey tests/testOrdering # Nonlinear nonlinear headers += Key.h headers += NonlinearFactorGraph.h NonlinearFactorGraph-inl.h -headers += NonlinearOptimizer-inl.h NonlinearOptimization.h NonlinearOptimization-inl.h NonlinearOptimizationParameters.h +headers += NonlinearOptimizer-inl.h NonlinearOptimization.h NonlinearOptimization-inl.h headers += NonlinearFactor.h -sources += NonlinearOptimizer.cpp Ordering.cpp DoglegOptimizerImpl.cpp +sources += NonlinearOptimizer.cpp Ordering.cpp DoglegOptimizerImpl.cpp NonlinearOptimizationParameters.cpp headers += DoglegOptimizer.h DoglegOptimizer-inl.h # Nonlinear iSAM(2) diff --git a/gtsam/nonlinear/NonlinearOptimizationParameters.cpp b/gtsam/nonlinear/NonlinearOptimizationParameters.cpp new file mode 100644 index 000000000..4da8fc7a5 --- /dev/null +++ b/gtsam/nonlinear/NonlinearOptimizationParameters.cpp @@ -0,0 +1,121 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file NonlinearOptimizationParameters.cpp + * @date Jan 28, 2012 + * @author Alex Cunningham + * @brief Implements parameters structure + */ + +#include + +#include + +#include + +namespace gtsam { + +using namespace std; + +/* ************************************************************************* */ +NonlinearOptimizationParameters::NonlinearOptimizationParameters() : + absDecrease_(1e-6), relDecrease_(1e-6), sumError_(0.0), maxIterations_( + 100), lambda_(1e-5), lambdaFactor_(10.0), verbosity_(SILENT), lambdaMode_( + BOUNDED), useQR_(false) { +} + +/* ************************************************************************* */ +NonlinearOptimizationParameters::NonlinearOptimizationParameters(double absDecrease, double relDecrease, + double sumError, int iIters, double lambda, + double lambdaFactor, verbosityLevel v, + LambdaMode lambdaMode, bool useQR) : + absDecrease_(absDecrease), relDecrease_(relDecrease), sumError_( + sumError), maxIterations_(iIters), lambda_(lambda), lambdaFactor_( + lambdaFactor), verbosity_(v), lambdaMode_(lambdaMode), useQR_(useQR) { +} + +/* ************************************************************************* */ +NonlinearOptimizationParameters::NonlinearOptimizationParameters( + const NonlinearOptimizationParameters ¶meters) : + absDecrease_(parameters.absDecrease_), relDecrease_( + parameters.relDecrease_), sumError_(parameters.sumError_), maxIterations_( + parameters.maxIterations_), lambda_(parameters.lambda_), lambdaFactor_( + parameters.lambdaFactor_), verbosity_(parameters.verbosity_), lambdaMode_( + parameters.lambdaMode_), useQR_(parameters.useQR_) { +} + +/* ************************************************************************* */ +void NonlinearOptimizationParameters::print(const std::string& s) const { + cout << "NonlinearOptimizationParameters " << s << endl; + cout << "absolute decrease threshold: " << absDecrease_ << endl; + cout << "relative decrease threshold: " << relDecrease_ << endl; + cout << " error sum threshold: " << sumError_ << endl; + cout << " maximum nr. of iterations: " << maxIterations_ << endl; + cout << " initial lambda value: " << lambda_ << endl; + cout << " factor to multiply lambda: " << lambdaFactor_ << endl; + cout << " verbosity level: " << verbosity_ << endl; + cout << " lambda mode: " << lambdaMode_ << endl; + cout << " use QR: " << useQR_ << endl; +} + +/* ************************************************************************* */ +NonlinearOptimizationParameters::shared_ptr +NonlinearOptimizationParameters::newLambda_(double lambda) const { + shared_ptr ptr( + boost::make_shared < NonlinearOptimizationParameters > (*this)); + ptr->lambda_ = lambda; + return ptr; +} + +/* ************************************************************************* */ +NonlinearOptimizationParameters::shared_ptr +NonlinearOptimizationParameters::newVerbosity(verbosityLevel verbosity) { + shared_ptr ptr(boost::make_shared()); + ptr->verbosity_ = verbosity; + return ptr; +} + +/* ************************************************************************* */ +NonlinearOptimizationParameters::shared_ptr +NonlinearOptimizationParameters::newMaxIterations(int maxIterations) { + shared_ptr ptr(boost::make_shared()); + ptr->maxIterations_ = maxIterations; + return ptr; +} + +/* ************************************************************************* */ +NonlinearOptimizationParameters::shared_ptr +NonlinearOptimizationParameters::newLambda(double lambda) { + shared_ptr ptr(boost::make_shared()); + ptr->lambda_ = lambda; + return ptr; +} + +/* ************************************************************************* */ +NonlinearOptimizationParameters::shared_ptr +NonlinearOptimizationParameters::newDrecreaseThresholds(double absDecrease, + double relDecrease) { + shared_ptr ptr(boost::make_shared()); + ptr->absDecrease_ = absDecrease; + ptr->relDecrease_ = relDecrease; + return ptr; +} + +/* ************************************************************************* */ +NonlinearOptimizationParameters::shared_ptr +NonlinearOptimizationParameters::newFactorization(bool useQR) { + shared_ptr ptr(boost::make_shared()); + ptr->useQR_ = useQR; + return ptr; +} + +} // \namespace gtsam diff --git a/gtsam/nonlinear/NonlinearOptimizationParameters.h b/gtsam/nonlinear/NonlinearOptimizationParameters.h index 6cfc5f1a8..68595b711 100644 --- a/gtsam/nonlinear/NonlinearOptimizationParameters.h +++ b/gtsam/nonlinear/NonlinearOptimizationParameters.h @@ -18,8 +18,8 @@ #pragma once +#include #include -#include #include namespace gtsam { @@ -32,7 +32,6 @@ namespace gtsam { typedef boost::shared_ptr shared_ptr; typedef NonlinearOptimizationParameters This; - typedef boost::shared_ptr sharedThis; double absDecrease_; ///< threshold for the absolute decrease per iteration @@ -40,12 +39,12 @@ namespace gtsam { * Relative decrease threshold, where relative error = (new-current)/current. * This can be set to 0 if there is a possibility for negative error values. */ - double relDecrease_; ///< threshold for the relative decrease per iteration + double relDecrease_; ///< threshold for the relative decrease per iteration - double sumError_; ///< threshold for the sum of error + double sumError_; ///< threshold for the sum of error size_t maxIterations_; ///< maximum number of iterations - double lambda_; ///< starting lambda value - double lambdaFactor_; ///< factor by which lambda is multiplied + double lambda_; ///< starting lambda value + double lambdaFactor_; ///< factor by which lambda is multiplied /// verbosity levels typedef enum { @@ -75,98 +74,47 @@ namespace gtsam { /// @{ /// Default constructor - NonlinearOptimizationParameters() : - absDecrease_(1e-6), relDecrease_(1e-6), sumError_(0.0), maxIterations_( - 100), lambda_(1e-5), lambdaFactor_(10.0), verbosity_(SILENT), lambdaMode_( - BOUNDED), useQR_(false) { - } + NonlinearOptimizationParameters(); /// Constructor from doubles NonlinearOptimizationParameters(double absDecrease, double relDecrease, double sumError, int iIters = 100, double lambda = 1e-5, double lambdaFactor = 10, verbosityLevel v = SILENT, - LambdaMode lambdaMode = BOUNDED, bool useQR = false) : - absDecrease_(absDecrease), relDecrease_(relDecrease), sumError_( - sumError), maxIterations_(iIters), lambda_(lambda), lambdaFactor_( - lambdaFactor), verbosity_(v), lambdaMode_(lambdaMode), useQR_(useQR) { - } + LambdaMode lambdaMode = BOUNDED, bool useQR = false); /// Copy constructor NonlinearOptimizationParameters( - const NonlinearOptimizationParameters ¶meters) : - absDecrease_(parameters.absDecrease_), relDecrease_( - parameters.relDecrease_), sumError_(parameters.sumError_), maxIterations_( - parameters.maxIterations_), lambda_(parameters.lambda_), lambdaFactor_( - parameters.lambdaFactor_), verbosity_(parameters.verbosity_), lambdaMode_( - parameters.lambdaMode_), useQR_(parameters.useQR_) { - } + const NonlinearOptimizationParameters ¶meters); /// @} /// @name Standard Interface /// @{ /// print - void print(const std::string& s="") const { - cout << "NonlinearOptimizationParameters " << s << endl; - cout << "absolute decrease threshold: " << absDecrease_ << endl; - cout << "relative decrease threshold: " << relDecrease_ << endl; - cout << " error sum threshold: " << sumError_ << endl; - cout << " maximum nr. of iterations: " << maxIterations_ << endl; - cout << " initial lambda value: " << lambda_ << endl; - cout << " factor to multiply lambda: " << lambdaFactor_ << endl; - cout << " verbosity level: " << verbosity_ << endl; - cout << " lambda mode: " << lambdaMode_ << endl; - cout << " use QR: " << useQR_ << endl; - } + void print(const std::string& s="") const; /// a copy of old instance except new lambda - sharedThis newLambda_(double lambda) const { - sharedThis ptr( - boost::make_shared < NonlinearOptimizationParameters > (*this)); - ptr->lambda_ = lambda; - return ptr; - } + shared_ptr newLambda_(double lambda) const; /// @} /// @name Advanced Interface /// @{ /// a copy of old instance except new verbosity - static sharedThis newVerbosity(verbosityLevel verbosity) { - sharedThis ptr(boost::make_shared()); - ptr->verbosity_ = verbosity; - return ptr; - } + static shared_ptr newVerbosity(verbosityLevel verbosity); /// a copy of old instance except new maxIterations - static sharedThis newMaxIterations(int maxIterations) { - sharedThis ptr(boost::make_shared()); - ptr->maxIterations_ = maxIterations; - return ptr; - } + static shared_ptr newMaxIterations(int maxIterations); /// a copy of old instance except new lambda - static sharedThis newLambda(double lambda) { - sharedThis ptr(boost::make_shared()); - ptr->lambda_ = lambda; - return ptr; - } + static shared_ptr newLambda(double lambda); /// a copy of old instance except new thresholds - static sharedThis newDrecreaseThresholds(double absDecrease, - double relDecrease) { - sharedThis ptr(boost::make_shared()); - ptr->absDecrease_ = absDecrease; - ptr->relDecrease_ = relDecrease; - return ptr; - } + static shared_ptr newDrecreaseThresholds(double absDecrease, + double relDecrease); /// a copy of old instance except new QR flag - static sharedThis newFactorization(bool useQR) { - sharedThis ptr(boost::make_shared()); - ptr->useQR_ = useQR; - return ptr; - } + static shared_ptr newFactorization(bool useQR); /// @} diff --git a/gtsam/slam/tests/testGeneralSFMFactor.cpp b/gtsam/slam/tests/testGeneralSFMFactor.cpp index 7825e0b75..0d5c7da9f 100644 --- a/gtsam/slam/tests/testGeneralSFMFactor.cpp +++ b/gtsam/slam/tests/testGeneralSFMFactor.cpp @@ -187,7 +187,7 @@ TEST( GeneralSFMFactor, optimize_defaultK ) { // Create an ordering of the variables shared_ptr ordering = getOrdering(X,L); - NonlinearOptimizationParameters::sharedThis params ( + NonlinearOptimizationParameters::shared_ptr params ( new NonlinearOptimizationParameters(1e-5, 1e-5, 0.0, 100, 1e-5, 10, NonlinearOptimizationParameters::SILENT)); Optimizer optimizer(graph, values, ordering, params); Optimizer optimizer2 = optimizer.levenbergMarquardt(); @@ -232,7 +232,7 @@ TEST( GeneralSFMFactor, optimize_varK_SingleMeasurementError ) { const double reproj_error = 1e-5; shared_ptr ordering = getOrdering(X,L); - NonlinearOptimizationParameters::sharedThis params ( + NonlinearOptimizationParameters::shared_ptr params ( new NonlinearOptimizationParameters(1e-5, 1e-5, 0.0, 100, 1e-5, 10, NonlinearOptimizationParameters::SILENT)); Optimizer optimizer(graph, values, ordering, params); Optimizer optimizer2 = optimizer.levenbergMarquardt(); @@ -275,7 +275,7 @@ TEST( GeneralSFMFactor, optimize_varK_FixCameras ) { const double reproj_error = 1e-5 ; shared_ptr ordering = getOrdering(X,L); - NonlinearOptimizationParameters::sharedThis params ( + NonlinearOptimizationParameters::shared_ptr params ( new NonlinearOptimizationParameters(1e-5, 1e-5, 0.0, 100, 1e-3, 10, NonlinearOptimizationParameters::SILENT)); Optimizer optimizer(graph, values, ordering, params); @@ -335,7 +335,7 @@ TEST( GeneralSFMFactor, optimize_varK_FixLandmarks ) { const double reproj_error = 1e-5 ; shared_ptr ordering = getOrdering(X,L); - NonlinearOptimizationParameters::sharedThis params ( + NonlinearOptimizationParameters::shared_ptr params ( new NonlinearOptimizationParameters(1e-5, 1e-5, 0.0, 100, 1e-3, 10, NonlinearOptimizationParameters::SILENT)); Optimizer optimizer(graph, values, ordering, params); @@ -378,7 +378,7 @@ TEST( GeneralSFMFactor, optimize_varK_BA ) { const double reproj_error = 1e-5 ; shared_ptr ordering = getOrdering(X,L); - NonlinearOptimizationParameters::sharedThis params ( + NonlinearOptimizationParameters::shared_ptr params ( new NonlinearOptimizationParameters(1e-2, 1e-2, 0.0, 100, 1e-5, 10, NonlinearOptimizationParameters::SILENT)); Optimizer optimizer(graph, values, ordering, params); diff --git a/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp b/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp index 40385c84f..b402b1d0b 100644 --- a/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp +++ b/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp @@ -188,7 +188,7 @@ TEST( GeneralSFMFactor, optimize_defaultK ) { // Create an ordering of the variables shared_ptr ordering = getOrdering(X,L); - NonlinearOptimizationParameters::sharedThis params ( + NonlinearOptimizationParameters::shared_ptr params ( new NonlinearOptimizationParameters(1e-5, 1e-5, 0.0, 100, 1e-5, 10, NonlinearOptimizationParameters::SILENT)); Optimizer optimizer(graph, values, ordering, params); Optimizer optimizer2 = optimizer.levenbergMarquardt(); @@ -233,7 +233,7 @@ TEST( GeneralSFMFactor, optimize_varK_SingleMeasurementError ) { const double reproj_error = 1e-5; shared_ptr ordering = getOrdering(X,L); - NonlinearOptimizationParameters::sharedThis params ( + NonlinearOptimizationParameters::shared_ptr params ( new NonlinearOptimizationParameters(1e-5, 1e-5, 0.0, 100, 1e-5, 10, NonlinearOptimizationParameters::SILENT)); Optimizer optimizer(graph, values, ordering, params); Optimizer optimizer2 = optimizer.levenbergMarquardt(); @@ -276,7 +276,7 @@ TEST( GeneralSFMFactor, optimize_varK_FixCameras ) { const double reproj_error = 1e-5 ; shared_ptr ordering = getOrdering(X,L); - NonlinearOptimizationParameters::sharedThis params ( + NonlinearOptimizationParameters::shared_ptr params ( new NonlinearOptimizationParameters(1e-5, 1e-5, 0.0, 100, 1e-3, 10, NonlinearOptimizationParameters::SILENT)); Optimizer optimizer(graph, values, ordering, params); @@ -332,7 +332,7 @@ TEST( GeneralSFMFactor, optimize_varK_FixLandmarks ) { const double reproj_error = 1e-5 ; shared_ptr ordering = getOrdering(X,L); - NonlinearOptimizationParameters::sharedThis params ( + NonlinearOptimizationParameters::shared_ptr params ( new NonlinearOptimizationParameters(1e-5, 1e-5, 0.0, 100, 1e-3, 10, NonlinearOptimizationParameters::SILENT)); Optimizer optimizer(graph, values, ordering, params); @@ -374,7 +374,7 @@ TEST( GeneralSFMFactor, optimize_varK_BA ) { const double reproj_error = 1e-5 ; shared_ptr ordering = getOrdering(X,L); - NonlinearOptimizationParameters::sharedThis params ( + NonlinearOptimizationParameters::shared_ptr params ( new NonlinearOptimizationParameters(1e-5, 1e-5, 0.0, 100, 1e-5, 10, NonlinearOptimizationParameters::SILENT)); Optimizer optimizer(graph, values, ordering, params); diff --git a/gtsam/slam/tests/testPose2SLAM.cpp b/gtsam/slam/tests/testPose2SLAM.cpp index f47339233..3cbc7e082 100644 --- a/gtsam/slam/tests/testPose2SLAM.cpp +++ b/gtsam/slam/tests/testPose2SLAM.cpp @@ -162,7 +162,7 @@ TEST(Pose2Graph, optimize) { *ordering += "x0","x1"; typedef NonlinearOptimizer Optimizer; - NonlinearOptimizationParameters::sharedThis params = NonlinearOptimizationParameters::newDrecreaseThresholds(1e-15, 1e-15); + NonlinearOptimizationParameters::shared_ptr params = NonlinearOptimizationParameters::newDrecreaseThresholds(1e-15, 1e-15); Optimizer optimizer0(fg, initial, ordering, params); Optimizer optimizer = optimizer0.levenbergMarquardt(); @@ -200,7 +200,7 @@ TEST(Pose2Graph, optimizeThreePoses) { *ordering += "x0","x1","x2"; // optimize - NonlinearOptimizationParameters::sharedThis params = NonlinearOptimizationParameters::newDrecreaseThresholds(1e-15, 1e-15); + NonlinearOptimizationParameters::shared_ptr params = NonlinearOptimizationParameters::newDrecreaseThresholds(1e-15, 1e-15); pose2SLAM::Optimizer optimizer0(fg, initial, ordering, params); pose2SLAM::Optimizer optimizer = optimizer0.levenbergMarquardt(); @@ -243,7 +243,7 @@ TEST_UNSAFE(Pose2SLAM, optimizeCircle) { *ordering += "x0","x1","x2","x3","x4","x5"; // optimize - NonlinearOptimizationParameters::sharedThis params = NonlinearOptimizationParameters::newDrecreaseThresholds(1e-15, 1e-15); + NonlinearOptimizationParameters::shared_ptr params = NonlinearOptimizationParameters::newDrecreaseThresholds(1e-15, 1e-15); pose2SLAM::Optimizer optimizer0(fg, initial, ordering, params); pose2SLAM::Optimizer optimizer = optimizer0.levenbergMarquardt(); diff --git a/gtsam/slam/tests/testPose3SLAM.cpp b/gtsam/slam/tests/testPose3SLAM.cpp index 949afea0f..b2bd86d4d 100644 --- a/gtsam/slam/tests/testPose3SLAM.cpp +++ b/gtsam/slam/tests/testPose3SLAM.cpp @@ -76,7 +76,7 @@ TEST(Pose3Graph, optimizeCircle) { // Choose an ordering and optimize shared_ptr ordering(new Ordering); *ordering += "x0","x1","x2","x3","x4","x5"; - NonlinearOptimizationParameters::sharedThis params = NonlinearOptimizationParameters::newDrecreaseThresholds(1e-15, 1e-15); + NonlinearOptimizationParameters::shared_ptr params = NonlinearOptimizationParameters::newDrecreaseThresholds(1e-15, 1e-15); pose3SLAM::Optimizer optimizer0(fg, initial, ordering, params); pose3SLAM::Optimizer optimizer = optimizer0.levenbergMarquardt(); diff --git a/tests/testNonlinearEquality.cpp b/tests/testNonlinearEquality.cpp index 501bb9db8..2fb5ec315 100644 --- a/tests/testNonlinearEquality.cpp +++ b/tests/testNonlinearEquality.cpp @@ -201,7 +201,7 @@ TEST ( NonlinearEquality, allow_error_optimize ) { // optimize boost::shared_ptr ord(new Ordering()); ord->push_back(key1); - NonlinearOptimizationParameters::sharedThis params = NonlinearOptimizationParameters::newDrecreaseThresholds(1e-5, 1e-5); + NonlinearOptimizationParameters::shared_ptr params = NonlinearOptimizationParameters::newDrecreaseThresholds(1e-5, 1e-5); PoseOptimizer optimizer(graph, init, ord, params); PoseOptimizer result = optimizer.levenbergMarquardt(); @@ -237,7 +237,7 @@ TEST ( NonlinearEquality, allow_error_optimize_with_factors ) { // optimize boost::shared_ptr ord(new Ordering()); ord->push_back(key1); - NonlinearOptimizationParameters::sharedThis params = NonlinearOptimizationParameters::newDrecreaseThresholds(1e-5, 1e-5); + NonlinearOptimizationParameters::shared_ptr params = NonlinearOptimizationParameters::newDrecreaseThresholds(1e-5, 1e-5); PoseOptimizer optimizer(graph, init, ord, params); PoseOptimizer result = optimizer.levenbergMarquardt();