diff --git a/.cproject b/.cproject index de3ea1faf..66d59d91a 100644 --- a/.cproject +++ b/.cproject @@ -2053,6 +2053,14 @@ true true + + make + -j7 + testNonlinearOptimizer.run + true + true + true + make -j2 diff --git a/gtsam/nonlinear/GaussNewtonOptimizer.h b/gtsam/nonlinear/GaussNewtonOptimizer.h index e445a4e00..0c85e35d1 100644 --- a/gtsam/nonlinear/GaussNewtonOptimizer.h +++ b/gtsam/nonlinear/GaussNewtonOptimizer.h @@ -210,7 +210,7 @@ private: // error, and increments the iteration count. GaussNewtonOptimizer(const GaussNewtonOptimizer& original, const SharedValues& newValues, double newError) : - NonlinearOptimizer(graph_, newValues, params_, newError, iterations_+1), + NonlinearOptimizer(original.graph_, newValues, original.params_, newError, original.iterations_+1), gnParams_(original.gnParams_), colamdOrdering_(original.colamdOrdering_), ordering_(original.ordering_) {} diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp index dd84bff1c..177c72c36 100644 --- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp +++ b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp @@ -46,7 +46,7 @@ NonlinearOptimizer::auto_ptr LevenbergMarquardtOptimizer::iterate() const { const double lambdaFactor = lmParams_->lambdaFactor; // Variables to update during try_lambda loop - double lambda = lmParams_->lambdaInitial; + double lambda = lambda_; double next_error = error(); SharedValues next_values = values(); diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.h b/gtsam/nonlinear/LevenbergMarquardtOptimizer.h index 48a7fd532..e3a8331be 100644 --- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.h +++ b/gtsam/nonlinear/LevenbergMarquardtOptimizer.h @@ -263,7 +263,7 @@ private: // error, and increments the iteration count. LevenbergMarquardtOptimizer(const LevenbergMarquardtOptimizer& original, const SharedValues& newValues, double newError, double newLambda) : - NonlinearOptimizer(graph_, newValues, params_, newError, iterations_+1), + NonlinearOptimizer(original.graph_, newValues, original.params_, newError, original.iterations_+1), lmParams_(original.lmParams_), colamdOrdering_(original.colamdOrdering_), ordering_(original.ordering_), diff --git a/tests/testNonlinearOptimizer.cpp b/tests/testNonlinearOptimizer.cpp index ddf0c4ab5..5f228f3e7 100644 --- a/tests/testNonlinearOptimizer.cpp +++ b/tests/testNonlinearOptimizer.cpp @@ -163,7 +163,6 @@ TEST( NonlinearOptimizer, optimization_method ) paramsQR.factorization = LevenbergMarquardtParams::QR; LevenbergMarquardtParams paramsLDL; paramsLDL.factorization = LevenbergMarquardtParams::LDL; - EXPECT(false); example::Graph fg = example::createReallyNonlinearFactorGraph(); @@ -202,7 +201,7 @@ TEST( NonlinearOptimizer, Factorization ) } /* ************************************************************************* */ -TEST_UNSAFE(NonlinearOptimizer, NullFactor) { +TEST(NonlinearOptimizer, NullFactor) { example::Graph fg = example::createReallyNonlinearFactorGraph(); @@ -234,66 +233,6 @@ TEST_UNSAFE(NonlinearOptimizer, NullFactor) { DOUBLES_EQUAL(0,fg.error(*actual2->values()),tol); } -///* ************************************************************************* */ -// SL-FIX TEST( NonlinearOptimizer, SubgraphSolver ) -//{ -// using namespace pose2SLAM; -// typedef SubgraphSolver Solver; -// typedef NonlinearOptimizer Optimizer; -// -// // Create a graph -// boost::shared_ptr graph(new Graph); -// graph->addPrior(1, Pose2(0., 0., 0.), noiseModel::Isotropic::Sigma(3, 1e-10)); -// graph->addConstraint(1, 2, Pose2(1., 0., 0.), noiseModel::Isotropic::Sigma(3, 1)); -// -// // Create an initial config -// boost::shared_ptr config(new Values); -// config->insert(1, Pose2(0., 0., 0.)); -// config->insert(2, Pose2(1.5, 0., 0.)); -// -// // Create solver and optimizer -// Optimizer::shared_solver solver -// (new SubgraphSolver (*graph, *config)); -// Optimizer optimizer(graph, config, solver); -// -// // Optimize !!!! -// double relativeThreshold = 1e-5; -// double absoluteThreshold = 1e-5; -// Optimizer optimized = optimizer.gaussNewton(relativeThreshold, -// absoluteThreshold, Optimizer::SILENT); -// -// // Check solution -// Values expected; -// expected.insert(1, Pose2(0., 0., 0.)); -// expected.insert(2, Pose2(1., 0., 0.)); -// CHECK(assert_equal(expected, *optimized.values(), 1e-5)); -//} - -/* ************************************************************************* */ -// SL-FIX TEST( NonlinearOptimizer, MultiFrontalSolver ) -//{ -// shared_ptr fg(new example::Graph( -// example::createNonlinearFactorGraph())); -// Optimizer::shared_values initial = example::sharedNoisyValues(); -// -// Values expected; -// expected.insert(simulated2D::PoseKey(1), Point2(0.0, 0.0)); -// expected.insert(simulated2D::PoseKey(2), Point2(1.5, 0.0)); -// expected.insert(simulated2D::PointKey(1), Point2(0.0, -1.0)); -// -// Optimizer::shared_solver solver; -// -// // Check one ordering -// shared_ptr ord1(new Ordering()); -// *ord1 += kx(2),kl(1),kx(1); -// solver = Optimizer::shared_solver(new Optimizer::solver(ord1)); -// Optimizer optimizer1(fg, initial, solver); -// -// Values actual = optimizer1.levenbergMarquardt(); -// CHECK(assert_equal(actual,expected)); -//} - - /* ************************************************************************* */ int main() { TestResult tr;