diff --git a/gtsam/geometry/GeneralCameraT.h b/gtsam/geometry/GeneralCameraT.h index cb790627e..92c93365f 100644 --- a/gtsam/geometry/GeneralCameraT.h +++ b/gtsam/geometry/GeneralCameraT.h @@ -104,7 +104,7 @@ class GeneralCameraT { } Matrix H1_k, H1_pose, H2_pt; - Point2 projection = project(P, H1_k, H1_pose, H2_pt); + Point2 projection = project(P, H1_pose, H1_k, H2_pt); if ( H1 ) *H1 = collect(2, &H1_pose, &H1_k); if ( H2 ) *H2 = H2_pt; diff --git a/gtsam/slam/tests/testGeneralSFMFactor.cpp b/gtsam/slam/tests/testGeneralSFMFactor.cpp index 7f4aed1c6..94993b187 100644 --- a/gtsam/slam/tests/testGeneralSFMFactor.cpp +++ b/gtsam/slam/tests/testGeneralSFMFactor.cpp @@ -190,7 +190,7 @@ TEST( GeneralSFMFactor, optimize_defaultK ) { shared_ptr ordering = getOrdering(X,L); //graph->print("graph") ; values->print("values") ; NonlinearOptimizationParameters::sharedThis params ( - new NonlinearOptimizationParameters(1e-7, 1e-7, 0.0, 100, 1e-5, 10, NonlinearOptimizationParameters::SILENT)); + new NonlinearOptimizationParameters(1e-5, 1e-5, 0.0, 100, 1e-5, 10, NonlinearOptimizationParameters::SILENT)); Optimizer optimizer(graph, values, ordering, params); cout << "optimize_defaultK::" << endl ; cout << "before optimization, error is " << optimizer.error() << endl; @@ -241,7 +241,7 @@ TEST( GeneralSFMFactor, optimize_varK_SingleMeasurementError ) { shared_ptr ordering = getOrdering(X,L); NonlinearOptimizationParameters::sharedThis params ( - new NonlinearOptimizationParameters(1e-7, 1e-7, 0.0, 100, 1e-5, 10, NonlinearOptimizationParameters::SILENT)); + new NonlinearOptimizationParameters(1e-5, 1e-5, 0.0, 100, 1e-5, 10, NonlinearOptimizationParameters::SILENT)); Optimizer optimizer(graph, values, ordering, params); cout << "optimize_varK_SingleMeasurementError::" << endl ; cout << "before optimization, error is " << optimizer.error() << endl; @@ -286,7 +286,7 @@ TEST( GeneralSFMFactor, optimize_varK_FixCameras ) { shared_ptr ordering = getOrdering(X,L); NonlinearOptimizationParameters::sharedThis params ( - new NonlinearOptimizationParameters(1e-7, 1e-7, 0.0, 100, 1e-3, 10, NonlinearOptimizationParameters::SILENT)); + new NonlinearOptimizationParameters(1e-5, 1e-5, 0.0, 100, 1e-3, 10, NonlinearOptimizationParameters::SILENT)); Optimizer optimizer(graph, values, ordering, params); cout << "optimize_varK_FixCameras::" << endl ; @@ -349,16 +349,15 @@ TEST( GeneralSFMFactor, optimize_varK_FixLandmarks ) { shared_ptr ordering = getOrdering(X,L); NonlinearOptimizationParameters::sharedThis params ( - new NonlinearOptimizationParameters(1e-7, 1e-7, 0.0, 100, 1e-3, 10, NonlinearOptimizationParameters::SILENT)); -// new NonlinearOptimizationParameters(1e-7, 1e-7, 0.0, 100, 1e-3, 10, NonlinearOptimizationParameters::TRYDELTA)); + new NonlinearOptimizationParameters(1e-5, 1e-5, 0.0, 100, 1e-3, 10, NonlinearOptimizationParameters::SILENT)); +// new NonlinearOptimizationParameters(1e-7, 1e-7, 0.0, 100, 1e-5, 10, NonlinearOptimizationParameters::TRYDELTA)); Optimizer optimizer(graph, values, ordering, params); cout << "optimize_varK_FixLandmarks::" << endl ; cout << "before optimization, error is " << optimizer.error() << endl; Optimizer optimizer2 = optimizer.levenbergMarquardt(); cout << "after optimization, error is " << optimizer2.error() << endl; - // CHECK(optimizer2.error() < 0.5 * reproj_error * nMeasurements); - CHECK(1); + CHECK(optimizer2.error() < 0.5 * reproj_error * nMeasurements); } /* ************************************************************************* */ @@ -396,15 +395,14 @@ TEST( GeneralSFMFactor, optimize_varK_BA ) { shared_ptr ordering = getOrdering(X,L); NonlinearOptimizationParameters::sharedThis params ( - new NonlinearOptimizationParameters(1e-7, 1e-7, 0.0, 100, 1e-5, 10, NonlinearOptimizationParameters::SILENT)); + new NonlinearOptimizationParameters(1e-5, 1e-5, 0.0, 100, 1e-5, 10, NonlinearOptimizationParameters::ERROR)); Optimizer optimizer(graph, values, ordering, params); cout << "optimize_varK_BA::" << endl ; cout << "before optimization, error is " << optimizer.error() << endl; Optimizer optimizer2 = optimizer.levenbergMarquardt(); cout << "after optimization, error is " << optimizer2.error() << endl; - //CHECK(optimizer2.error() < 0.5 * reproj_error * nMeasurements); - CHECK(1); + CHECK(optimizer2.error() < 0.5 * reproj_error * nMeasurements); }