diff --git a/.cproject b/.cproject index 108a70799..243637d65 100644 --- a/.cproject +++ b/.cproject @@ -21,7 +21,7 @@ - + @@ -2186,4 +2186,5 @@ + diff --git a/.project b/.project index c4d531b04..9856df2ea 100644 --- a/.project +++ b/.project @@ -23,7 +23,7 @@ org.eclipse.cdt.make.core.buildArguments - + -j5 org.eclipse.cdt.make.core.buildCommand diff --git a/gtsam/nonlinear/GaussNewtonOptimizer.h b/gtsam/nonlinear/GaussNewtonOptimizer.h index e4b4c31bb..3b1e69481 100644 --- a/gtsam/nonlinear/GaussNewtonOptimizer.h +++ b/gtsam/nonlinear/GaussNewtonOptimizer.h @@ -69,6 +69,7 @@ public: /** * This class performs Gauss-Newton nonlinear optimization + * TODO: use make_shared */ class GaussNewtonOptimizer : public NonlinearOptimizer { @@ -107,9 +108,9 @@ public: * @param params The optimization parameters */ GaussNewtonOptimizer(const SharedGraph& graph, const SharedValues& values, - const SharedGNParams& params = SharedGNParams(), + const GaussNewtonParams& params = GaussNewtonParams(), const SharedOrdering& ordering = SharedOrdering()) : - NonlinearOptimizer(graph, values, params ? params : SharedGNParams(new GaussNewtonParams())), + NonlinearOptimizer(graph, values, SharedGNParams(new GaussNewtonParams(params))), gnParams_(boost::static_pointer_cast(params_)), colamdOrdering_(!ordering || ordering->size() == 0), ordering_(colamdOrdering_ ? graph_->orderingCOLAMD(*values_) : ordering) {} diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.h b/gtsam/nonlinear/LevenbergMarquardtOptimizer.h index 20abf1fb7..b2605b968 100644 --- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.h +++ b/gtsam/nonlinear/LevenbergMarquardtOptimizer.h @@ -90,6 +90,7 @@ public: /** * This class performs Levenberg-Marquardt nonlinear optimization + * TODO: use make_shared */ class LevenbergMarquardtOptimizer : public NonlinearOptimizer { @@ -130,9 +131,9 @@ public: * @param params The optimization parameters */ LevenbergMarquardtOptimizer(const SharedGraph& graph, const SharedValues& values, - const SharedLMParams& params = SharedLMParams(), + const LevenbergMarquardtParams& params = LevenbergMarquardtParams(), const SharedOrdering& ordering = SharedOrdering()) : - NonlinearOptimizer(graph, values, params ? params : SharedLMParams(new LevenbergMarquardtParams())), + NonlinearOptimizer(graph, values, SharedLMParams(new LevenbergMarquardtParams(params))), lmParams_(boost::static_pointer_cast(params_)), colamdOrdering_(!ordering || ordering->size() == 0), ordering_(colamdOrdering_ ? graph_->orderingCOLAMD(*values_) : ordering), diff --git a/gtsam/slam/planarSLAM.cpp b/gtsam/slam/planarSLAM.cpp index db3d6b146..5e38acbd2 100644 --- a/gtsam/slam/planarSLAM.cpp +++ b/gtsam/slam/planarSLAM.cpp @@ -17,8 +17,6 @@ #include #include -#include -#include // Use planarSLAM namespace for specific SLAM instance namespace planarSLAM { diff --git a/gtsam/slam/planarSLAM.h b/gtsam/slam/planarSLAM.h index 33dc97a8c..bd7de3a91 100644 --- a/gtsam/slam/planarSLAM.h +++ b/gtsam/slam/planarSLAM.h @@ -24,7 +24,7 @@ #include #include #include -#include +#include #include #include @@ -110,15 +110,10 @@ namespace planarSLAM { /// Optimize Values optimize(const Values& initialEstimate) { - typedef NonlinearOptimizer Optimizer; - return *Optimizer::optimizeLM(*this, initialEstimate, - NonlinearOptimizationParameters::LAMBDA); + return *LevenbergMarquardtOptimizer(*this, initialEstimate).optimize()->values(); } }; - /// Optimizer - typedef NonlinearOptimizer Optimizer; - } // planarSLAM diff --git a/gtsam/slam/pose2SLAM.cpp b/gtsam/slam/pose2SLAM.cpp index b006c61f2..a500ed22e 100644 --- a/gtsam/slam/pose2SLAM.cpp +++ b/gtsam/slam/pose2SLAM.cpp @@ -21,8 +21,6 @@ // Use pose2SLAM namespace for specific SLAM instance - template class gtsam::NonlinearOptimizer; - namespace pose2SLAM { /* ************************************************************************* */ diff --git a/gtsam/slam/pose2SLAM.h b/gtsam/slam/pose2SLAM.h index 121e0c062..6e448b4ee 100644 --- a/gtsam/slam/pose2SLAM.h +++ b/gtsam/slam/pose2SLAM.h @@ -24,7 +24,7 @@ #include #include #include -#include +#include #include #include @@ -97,20 +97,10 @@ namespace pose2SLAM { /// Optimize Values optimize(const Values& initialEstimate) { - typedef NonlinearOptimizer Optimizer; - return *Optimizer::optimizeLM(*this, initialEstimate, - NonlinearOptimizationParameters::LAMBDA); + return *LevenbergMarquardtOptimizer(*this, initialEstimate).optimize()->values(); } }; - /// The sequential optimizer - typedef NonlinearOptimizer OptimizerSequential; - - /// The multifrontal optimizer - typedef NonlinearOptimizer Optimizer; - } // pose2SLAM diff --git a/gtsam/slam/pose3SLAM.h b/gtsam/slam/pose3SLAM.h index eb4eb9573..c394d800b 100644 --- a/gtsam/slam/pose3SLAM.h +++ b/gtsam/slam/pose3SLAM.h @@ -61,9 +61,6 @@ namespace gtsam { void addHardConstraint(Index i, const Pose3& p); }; - /// Optimizer - typedef NonlinearOptimizer Optimizer; - } // pose3SLAM /** diff --git a/gtsam/slam/tests/testGeneralSFMFactor.cpp b/gtsam/slam/tests/testGeneralSFMFactor.cpp index 7a8dc4825..5915fd4ff 100644 --- a/gtsam/slam/tests/testGeneralSFMFactor.cpp +++ b/gtsam/slam/tests/testGeneralSFMFactor.cpp @@ -16,7 +16,7 @@ using namespace boost; #include #include #include -#include +#include #include #include #include @@ -62,8 +62,6 @@ double getGaussian() return sqrt(-2.0f * (double)log(S) / S) * V1; } -typedef NonlinearOptimizer Optimizer; - const SharedNoiseModel sigma1(noiseModel::Unit::Create(1)); /* ************************************************************************* */ @@ -175,11 +173,9 @@ TEST( GeneralSFMFactor, optimize_defaultK ) { // Create an ordering of the variables shared_ptr ordering = getOrdering(X,L); - 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(); - EXPECT(optimizer2.error() < 0.5 * 1e-5 * nMeasurements); + NonlinearOptimizer::auto_ptr optimizer = + LevenbergMarquardtOptimizer(graph, values, LevenbergMarquardtParams(), ordering).optimize(); + EXPECT(optimizer->error() < 0.5 * 1e-5 * nMeasurements); } /* ************************************************************************* */ @@ -220,11 +216,9 @@ TEST( GeneralSFMFactor, optimize_varK_SingleMeasurementError ) { const double reproj_error = 1e-5; shared_ptr ordering = getOrdering(X,L); - 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(); - EXPECT(optimizer2.error() < 0.5 * reproj_error * nMeasurements); + NonlinearOptimizer::auto_ptr optimizer = + LevenbergMarquardtOptimizer(graph, values, LevenbergMarquardtParams(), ordering).optimize(); + EXPECT(optimizer->error() < 0.5 * reproj_error * nMeasurements); } /* ************************************************************************* */ @@ -263,12 +257,9 @@ TEST( GeneralSFMFactor, optimize_varK_FixCameras ) { const double reproj_error = 1e-5 ; shared_ptr ordering = getOrdering(X,L); - NonlinearOptimizationParameters::shared_ptr params ( - new NonlinearOptimizationParameters(1e-5, 1e-5, 0.0, 100, 1e-3, 10, NonlinearOptimizationParameters::SILENT)); - Optimizer optimizer(graph, values, ordering, params); - - Optimizer optimizer2 = optimizer.levenbergMarquardt(); - EXPECT(optimizer2.error() < 0.5 * reproj_error * nMeasurements); + NonlinearOptimizer::auto_ptr optimizer = + LevenbergMarquardtOptimizer(graph, values, LevenbergMarquardtParams(), ordering).optimize(); + EXPECT(optimizer->error() < 0.5 * reproj_error * nMeasurements); } /* ************************************************************************* */ @@ -323,13 +314,9 @@ TEST( GeneralSFMFactor, optimize_varK_FixLandmarks ) { const double reproj_error = 1e-5 ; shared_ptr ordering = getOrdering(X,L); - NonlinearOptimizationParameters::shared_ptr params ( - new NonlinearOptimizationParameters(1e-5, 1e-5, 0.0, 100, 1e-3, 10, NonlinearOptimizationParameters::SILENT)); - Optimizer optimizer(graph, values, ordering, params); - - Optimizer optimizer2 = optimizer.levenbergMarquardt(); - - EXPECT(optimizer2.error() < 0.5 * reproj_error * nMeasurements); + NonlinearOptimizer::auto_ptr optimizer = + LevenbergMarquardtOptimizer(graph, values, LevenbergMarquardtParams(), ordering).optimize(); + EXPECT(optimizer->error() < 0.5 * reproj_error * nMeasurements); } /* ************************************************************************* */ @@ -366,12 +353,9 @@ TEST( GeneralSFMFactor, optimize_varK_BA ) { const double reproj_error = 1e-5 ; shared_ptr ordering = getOrdering(X,L); - NonlinearOptimizationParameters::shared_ptr params ( - new NonlinearOptimizationParameters(1e-2, 1e-2, 0.0, 100, 1e-5, 10, NonlinearOptimizationParameters::SILENT)); - Optimizer optimizer(graph, values, ordering, params); - - Optimizer optimizer2 = optimizer.levenbergMarquardt(); - EXPECT(optimizer2.error() < 0.5 * reproj_error * nMeasurements); + NonlinearOptimizer::auto_ptr optimizer = + LevenbergMarquardtOptimizer(graph, values, LevenbergMarquardtParams(), ordering).optimize(); + EXPECT(optimizer->error() < 0.5 * reproj_error * nMeasurements); } /* ************************************************************************* */ diff --git a/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp b/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp index f762505a3..7f327b2fd 100644 --- a/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp +++ b/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp @@ -16,7 +16,7 @@ using namespace boost; #include #include #include -#include +#include #include #include #include @@ -63,8 +63,6 @@ double getGaussian() return sqrt(-2.0f * (double)log(S) / S) * V1; } -typedef NonlinearOptimizer Optimizer; - const SharedNoiseModel sigma1(noiseModel::Unit::Create(1)); /* ************************************************************************* */ @@ -177,11 +175,9 @@ TEST( GeneralSFMFactor, optimize_defaultK ) { // Create an ordering of the variables shared_ptr ordering = getOrdering(X,L); - 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(); - EXPECT(optimizer2.error() < 0.5 * 1e-5 * nMeasurements); + NonlinearOptimizer::auto_ptr optimizer = + LevenbergMarquardtOptimizer(graph, values, LevenbergMarquardtParams(), ordering).optimize(); + EXPECT(optimizer->error() < 0.5 * 1e-5 * nMeasurements); } /* ************************************************************************* */ @@ -222,11 +218,9 @@ TEST( GeneralSFMFactor, optimize_varK_SingleMeasurementError ) { const double reproj_error = 1e-5; shared_ptr ordering = getOrdering(X,L); - 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(); - EXPECT(optimizer2.error() < 0.5 * reproj_error * nMeasurements); + NonlinearOptimizer::auto_ptr optimizer = + LevenbergMarquardtOptimizer(graph, values, LevenbergMarquardtParams(), ordering).optimize(); + EXPECT(optimizer->error() < 0.5 * reproj_error * nMeasurements); } /* ************************************************************************* */ @@ -265,12 +259,9 @@ TEST( GeneralSFMFactor, optimize_varK_FixCameras ) { const double reproj_error = 1e-5 ; shared_ptr ordering = getOrdering(X,L); - NonlinearOptimizationParameters::shared_ptr params ( - new NonlinearOptimizationParameters(1e-5, 1e-5, 0.0, 100, 1e-3, 10, NonlinearOptimizationParameters::SILENT)); - Optimizer optimizer(graph, values, ordering, params); - - Optimizer optimizer2 = optimizer.levenbergMarquardt(); - EXPECT(optimizer2.error() < 0.5 * reproj_error * nMeasurements); + NonlinearOptimizer::auto_ptr optimizer = + LevenbergMarquardtOptimizer(graph, values, LevenbergMarquardtParams(), ordering).optimize(); + EXPECT(optimizer->error() < 0.5 * reproj_error * nMeasurements); } /* ************************************************************************* */ @@ -321,12 +312,9 @@ TEST( GeneralSFMFactor, optimize_varK_FixLandmarks ) { const double reproj_error = 1e-5 ; shared_ptr ordering = getOrdering(X,L); - NonlinearOptimizationParameters::shared_ptr params ( - new NonlinearOptimizationParameters(1e-5, 1e-5, 0.0, 100, 1e-3, 10, NonlinearOptimizationParameters::SILENT)); - Optimizer optimizer(graph, values, ordering, params); - - Optimizer optimizer2 = optimizer.levenbergMarquardt(); - EXPECT(optimizer2.error() < 0.5 * reproj_error * nMeasurements); + NonlinearOptimizer::auto_ptr optimizer = + LevenbergMarquardtOptimizer(graph, values, LevenbergMarquardtParams(), ordering).optimize(); + EXPECT(optimizer->error() < 0.5 * reproj_error * nMeasurements); } /* ************************************************************************* */ @@ -363,12 +351,9 @@ TEST( GeneralSFMFactor, optimize_varK_BA ) { const double reproj_error = 1e-5 ; shared_ptr ordering = getOrdering(X,L); - 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(); - EXPECT(optimizer2.error() < 0.5 * reproj_error * nMeasurements); + NonlinearOptimizer::auto_ptr optimizer = + LevenbergMarquardtOptimizer(graph, values, LevenbergMarquardtParams(), ordering).optimize(); + EXPECT(optimizer->error() < 0.5 * reproj_error * nMeasurements); } /* ************************************************************************* */ diff --git a/gtsam/slam/tests/testPose2SLAM.cpp b/gtsam/slam/tests/testPose2SLAM.cpp index 81ce20334..1abc8e476 100644 --- a/gtsam/slam/tests/testPose2SLAM.cpp +++ b/gtsam/slam/tests/testPose2SLAM.cpp @@ -160,17 +160,16 @@ TEST(Pose2Graph, optimize) { // Choose an ordering and optimize shared_ptr ordering(new Ordering); *ordering += kx0, kx1; - typedef NonlinearOptimizer Optimizer; - NonlinearOptimizationParameters::shared_ptr params = NonlinearOptimizationParameters::newDecreaseThresholds(1e-15, 1e-15); - Optimizer optimizer0(fg, initial, ordering, params); - Optimizer optimizer = optimizer0.levenbergMarquardt(); + LevenbergMarquardtParams params; + params.relativeErrorTol = 1e-15; + NonlinearOptimizer::auto_ptr optimizer = LevenbergMarquardtOptimizer(fg, initial, params, ordering).optimize(); // Check with expected config Values expected; expected.insert(pose2SLAM::PoseKey(0), Pose2(0,0,0)); expected.insert(pose2SLAM::PoseKey(1), Pose2(1,2,M_PI_2)); - CHECK(assert_equal(expected, *optimizer.values())); + CHECK(assert_equal(expected, *optimizer->values())); } /* ************************************************************************* */ @@ -200,11 +199,11 @@ TEST(Pose2Graph, optimizeThreePoses) { *ordering += kx0,kx1,kx2; // optimize - NonlinearOptimizationParameters::shared_ptr params = NonlinearOptimizationParameters::newDecreaseThresholds(1e-15, 1e-15); - pose2SLAM::Optimizer optimizer0(fg, initial, ordering, params); - pose2SLAM::Optimizer optimizer = optimizer0.levenbergMarquardt(); + LevenbergMarquardtParams params; + params.relativeErrorTol = 1e-15; + NonlinearOptimizer::auto_ptr optimizer = LevenbergMarquardtOptimizer(fg, initial, params, ordering).optimize(); - Values actual = *optimizer.values(); + Values actual = *optimizer->values(); // Check with ground truth CHECK(assert_equal((const Values&)hexagon, actual)); @@ -243,11 +242,11 @@ TEST_UNSAFE(Pose2SLAM, optimizeCircle) { *ordering += kx0,kx1,kx2,kx3,kx4,kx5; // optimize - NonlinearOptimizationParameters::shared_ptr params = NonlinearOptimizationParameters::newDecreaseThresholds(1e-15, 1e-15); - pose2SLAM::Optimizer optimizer0(fg, initial, ordering, params); - pose2SLAM::Optimizer optimizer = optimizer0.levenbergMarquardt(); + LevenbergMarquardtParams params; + params.relativeErrorTol = 1e-15; + NonlinearOptimizer::auto_ptr optimizer = LevenbergMarquardtOptimizer(fg, initial, params, ordering).optimize(); - Values actual = *optimizer.values(); + Values actual = *optimizer->values(); // Check with ground truth CHECK(assert_equal((const Values&)hexagon, actual)); diff --git a/gtsam/slam/tests/testPose3SLAM.cpp b/gtsam/slam/tests/testPose3SLAM.cpp index 2edf6c4f0..5759b1562 100644 --- a/gtsam/slam/tests/testPose3SLAM.cpp +++ b/gtsam/slam/tests/testPose3SLAM.cpp @@ -30,6 +30,7 @@ using namespace boost::assign; #include #include +#include using namespace std; using namespace gtsam; @@ -76,11 +77,9 @@ TEST(Pose3Graph, optimizeCircle) { // Choose an ordering and optimize shared_ptr ordering(new Ordering); *ordering += kx0,kx1,kx2,kx3,kx4,kx5; - NonlinearOptimizationParameters::shared_ptr params = NonlinearOptimizationParameters::newDecreaseThresholds(1e-15, 1e-15); - pose3SLAM::Optimizer optimizer0(fg, initial, ordering, params); - pose3SLAM::Optimizer optimizer = optimizer0.levenbergMarquardt(); - Values actual = *optimizer.values(); + Values actual = *LevenbergMarquardtOptimizer( + fg, initial, LevenbergMarquardtOptimizer::SharedLMParams(), ordering).optimize()->values(); // Check with ground truth CHECK(assert_equal(hexagon, actual,1e-4)); @@ -115,7 +114,7 @@ TEST(Pose3Graph, partial_prior_height) { // linearization EXPECT_DOUBLES_EQUAL(2.0, height.error(values), tol); - Values actual = *pose3SLAM::Optimizer::optimizeLM(graph, values); + Values actual = *LevenbergMarquardtOptimizer(graph, values).optimize()->values(); EXPECT(assert_equal(expected, actual.at(key), tol)); EXPECT_DOUBLES_EQUAL(0.0, graph.error(actual), tol); } @@ -167,7 +166,7 @@ TEST(Pose3Graph, partial_prior_xy) { Values values; values.insert(key, init); - Values actual = *pose3SLAM::Optimizer::optimizeLM(graph, values); + Values actual = *LevenbergMarquardtOptimizer(graph, values).optimize()->values(); EXPECT(assert_equal(expected, actual.at(key), tol)); EXPECT_DOUBLES_EQUAL(0.0, graph.error(actual), tol); } diff --git a/gtsam/slam/tests/testStereoFactor.cpp b/gtsam/slam/tests/testStereoFactor.cpp index fc026fb35..d101708b3 100644 --- a/gtsam/slam/tests/testStereoFactor.cpp +++ b/gtsam/slam/tests/testStereoFactor.cpp @@ -21,7 +21,7 @@ #include #include #include -#include +#include #include #include @@ -50,45 +50,34 @@ TEST( StereoFactor, singlePoint) { //Cal3_S2 K(625, 625, 0, 320, 240, 0.5); boost::shared_ptr K(new Cal3_S2Stereo(625, 625, 0, 320, 240, 0.5)); - boost::shared_ptr graph(new visualSLAM::Graph()); + NonlinearFactorGraph graph; - graph->add(visualSLAM::PoseConstraint(PoseKey(1),camera1)); + graph.add(visualSLAM::PoseConstraint(PoseKey(1),camera1)); StereoPoint2 z14(320,320.0-50, 240); // arguments: measurement, sigma, cam#, measurement #, K, baseline (m) - graph->add(visualSLAM::StereoFactor(z14,sigma, PoseKey(1), PointKey(1), K)); + graph.add(visualSLAM::StereoFactor(z14,sigma, PoseKey(1), PointKey(1), K)); // Create a configuration corresponding to the ground truth - boost::shared_ptr values(new Values()); - values->insert(PoseKey(1), camera1); // add camera at z=6.25m looking towards origin + Values values(new Values()); + values.insert(PoseKey(1), camera1); // add camera at z=6.25m looking towards origin Point3 l1(0, 0, 0); - values->insert(PointKey(1), l1); // add point at origin; + values.insert(PointKey(1), l1); // add point at origin; - Ordering::shared_ptr ordering = graph->orderingCOLAMD(*values); - - typedef gtsam::NonlinearOptimizer Optimizer; // optimization engine for this domain - - double absoluteThreshold = 1e-9; - double relativeThreshold = 1e-5; - int maxIterations = 100; - NonlinearOptimizationParameters::verbosityLevel verbose = NonlinearOptimizationParameters::SILENT; - NonlinearOptimizationParameters parameters(absoluteThreshold, relativeThreshold, 0, - maxIterations, 1.0, 10, verbose, NonlinearOptimizationParameters::BOUNDED); - - Optimizer optimizer(graph, values, ordering, make_shared(parameters)); + NonlinearOptimizer::auto_ptr optimizer(new GaussNewtonOptimizer(graph, values))); // We expect the initial to be zero because config is the ground truth - DOUBLES_EQUAL(0.0, optimizer.error(), 1e-9); + DOUBLES_EQUAL(0.0, optimizer->error(), 1e-9); // Iterate once, and the config should not have changed - Optimizer afterOneIteration = optimizer.iterate(); - DOUBLES_EQUAL(0.0, afterOneIteration.error(), 1e-9); + NonlinearOptimizer::auto_ptr afterOneIteration = optimizer->iterate(); + DOUBLES_EQUAL(0.0, afterOneIteration->error(), 1e-9); // Complete solution - Optimizer final = optimizer.gaussNewton(); + NonlinearOptimizer::auto_ptr final = optimizer->optimize(); - DOUBLES_EQUAL(0.0, final.error(), 1e-6); + DOUBLES_EQUAL(0.0, final->error(), 1e-6); } /* ************************************************************************* */ diff --git a/gtsam/slam/tests/testVSLAM.cpp b/gtsam/slam/tests/testVSLAM.cpp index 0188daa96..3a80752c2 100644 --- a/gtsam/slam/tests/testVSLAM.cpp +++ b/gtsam/slam/tests/testVSLAM.cpp @@ -23,7 +23,7 @@ using namespace boost; #include -#include +#include #include using namespace std; @@ -102,16 +102,16 @@ TEST( Graph, optimizeLM) // Create an optimizer and check its error // We expect the initial to be zero because config is the ground truth - visualSLAM::Optimizer optimizer(graph, initialEstimate, ordering); - DOUBLES_EQUAL(0.0, optimizer.error(), 1e-9); + NonlinearOptimizer::auto_ptr optimizer(new GaussNewtonOptimizer(graph, initialEstimate, GaussNewtonParams(), ordering)); + DOUBLES_EQUAL(0.0, optimizer->error(), 1e-9); // Iterate once, and the config should not have changed because we started // with the ground truth - visualSLAM::Optimizer afterOneIteration = optimizer.iterate(); - DOUBLES_EQUAL(0.0, optimizer.error(), 1e-9); + NonlinearOptimizer::auto_ptr afterOneIteration = optimizer->iterate(); + DOUBLES_EQUAL(0.0, optimizer->error(), 1e-9); // check if correct - CHECK(assert_equal(*initialEstimate,*(afterOneIteration.values()))); + CHECK(assert_equal(*initialEstimate,*(afterOneIteration->values()))); } @@ -139,16 +139,16 @@ TEST( Graph, optimizeLM2) // Create an optimizer and check its error // We expect the initial to be zero because config is the ground truth - visualSLAM::Optimizer optimizer(graph, initialEstimate, ordering); - DOUBLES_EQUAL(0.0, optimizer.error(), 1e-9); + NonlinearOptimizer::auto_ptr optimizer(new GaussNewtonOptimizer(graph, initialEstimate, GaussNewtonParams(), ordering)); + DOUBLES_EQUAL(0.0, optimizer->error(), 1e-9); // Iterate once, and the config should not have changed because we started // with the ground truth - visualSLAM::Optimizer afterOneIteration = optimizer.iterate(); - DOUBLES_EQUAL(0.0, optimizer.error(), 1e-9); + NonlinearOptimizer::auto_ptr afterOneIteration = optimizer->iterate(); + DOUBLES_EQUAL(0.0, optimizer->error(), 1e-9); // check if correct - CHECK(assert_equal(*initialEstimate,*(afterOneIteration.values()))); + CHECK(assert_equal(*initialEstimate,*(afterOneIteration->values()))); } @@ -170,20 +170,18 @@ TEST( Graph, CHECK_ORDERING) initialEstimate->insert(PointKey(3), landmark3); initialEstimate->insert(PointKey(4), landmark4); - Ordering::shared_ptr ordering = graph->orderingCOLAMD(*initialEstimate); - // Create an optimizer and check its error // We expect the initial to be zero because config is the ground truth - visualSLAM::Optimizer optimizer(graph, initialEstimate, ordering); - DOUBLES_EQUAL(0.0, optimizer.error(), 1e-9); + NonlinearOptimizer::auto_ptr optimizer(new GaussNewtonOptimizer(graph, initialEstimate)); + DOUBLES_EQUAL(0.0, optimizer->error(), 1e-9); // Iterate once, and the config should not have changed because we started // with the ground truth - visualSLAM::Optimizer afterOneIteration = optimizer.iterate(); - DOUBLES_EQUAL(0.0, optimizer.error(), 1e-9); + NonlinearOptimizer::auto_ptr afterOneIteration = optimizer->iterate(); + DOUBLES_EQUAL(0.0, optimizer->error(), 1e-9); // check if correct - CHECK(assert_equal(*initialEstimate,*(afterOneIteration.values()))); + CHECK(assert_equal(*initialEstimate,*(afterOneIteration->values()))); } /* ************************************************************************* */ diff --git a/gtsam/slam/visualSLAM.cpp b/gtsam/slam/visualSLAM.cpp index 67e77dc15..f6512e843 100644 --- a/gtsam/slam/visualSLAM.cpp +++ b/gtsam/slam/visualSLAM.cpp @@ -16,7 +16,6 @@ */ #include -#include #include namespace gtsam { diff --git a/gtsam/slam/visualSLAM.h b/gtsam/slam/visualSLAM.h index 5416e2142..d9be45eb3 100644 --- a/gtsam/slam/visualSLAM.h +++ b/gtsam/slam/visualSLAM.h @@ -145,7 +145,4 @@ namespace visualSLAM { }; // Graph - /// typedef for Optimizer. The current default will use the multi-frontal solver - typedef NonlinearOptimizer Optimizer; - } // namespaces diff --git a/tests/testBoundingConstraint.cpp b/tests/testBoundingConstraint.cpp index 09baec6e1..e553fb72b 100644 --- a/tests/testBoundingConstraint.cpp +++ b/tests/testBoundingConstraint.cpp @@ -31,11 +31,6 @@ SharedDiagonal soft_model2 = noiseModel::Unit::Create(2); SharedDiagonal soft_model2_alt = noiseModel::Isotropic::Sigma(2, 0.1); SharedDiagonal hard_model1 = noiseModel::Constrained::All(1); -typedef NonlinearFactorGraph Graph; -typedef boost::shared_ptr shared_graph; -typedef boost::shared_ptr shared_values; -typedef NonlinearOptimizer Optimizer; - // some simple inequality constraints Symbol key(simulated2D::PoseKey(1)); double mu = 10.0; @@ -150,19 +145,19 @@ TEST( testBoundingConstraint, unary_simple_optimization1) { Point2 goal_pt(1.0, 2.0); Point2 start_pt(0.0, 1.0); - shared_graph graph(new Graph()); + NonlinearFactorGraph graph; Symbol x1('x',1); - graph->add(iq2D::PoseXInequality(x1, 1.0, true)); - graph->add(iq2D::PoseYInequality(x1, 2.0, true)); - graph->add(simulated2D::Prior(start_pt, soft_model2, x1)); + graph.add(iq2D::PoseXInequality(x1, 1.0, true)); + graph.add(iq2D::PoseYInequality(x1, 2.0, true)); + graph.add(simulated2D::Prior(start_pt, soft_model2, x1)); - shared_values initValues(new Values()); - initValues->insert(x1, start_pt); + Values initValues; + initValues.insert(x1, start_pt); - Optimizer::shared_values actual = Optimizer::optimizeLM(graph, initValues); + Values actual = *LevenbergMarquardtOptimizer(graph, initValues).optimize()->values(); Values expected; expected.insert(x1, goal_pt); - CHECK(assert_equal(expected, *actual, tol)); + CHECK(assert_equal(expected, actual, tol)); } /* ************************************************************************* */ @@ -172,19 +167,19 @@ TEST( testBoundingConstraint, unary_simple_optimization2) { Point2 goal_pt(1.0, 2.0); Point2 start_pt(2.0, 3.0); - shared_graph graph(new Graph()); + NonlinearFactorGraph graph; Symbol x1('x',1); - graph->add(iq2D::PoseXInequality(x1, 1.0, false)); - graph->add(iq2D::PoseYInequality(x1, 2.0, false)); - graph->add(simulated2D::Prior(start_pt, soft_model2, x1)); + graph.add(iq2D::PoseXInequality(x1, 1.0, false)); + graph.add(iq2D::PoseYInequality(x1, 2.0, false)); + graph.add(simulated2D::Prior(start_pt, soft_model2, x1)); - shared_values initValues(new Values()); - initValues->insert(x1, start_pt); + Values initValues; + initValues.insert(x1, start_pt); - Optimizer::shared_values actual = Optimizer::optimizeLM(graph, initValues); + Values actual = *LevenbergMarquardtOptimizer(graph, initValues).optimize()->values(); Values expected; expected.insert(x1, goal_pt); - CHECK(assert_equal(expected, *actual, tol)); + CHECK(assert_equal(expected, actual, tol)); } /* ************************************************************************* */