diff --git a/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp b/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp index 1b1ae5390..21f5f2008 100644 --- a/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp +++ b/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp @@ -366,7 +366,7 @@ TEST( GeneralSFMFactor_Cal3Bundler, optimize_varK_BA ) { } // Constrain position of system with the first camera constrained to the origin - graph.addCameraConstraint(X(0), cameras[0]); + graph.addCameraConstraint(0, cameras[0]); // Constrain the scale of the problem with a soft range factor of 1m between the cameras graph.add(RangeFactor(X(0), X(1), 2.0, noiseModel::Isotropic::Sigma(1, 10.0)));