diff --git a/gtsam/slam/SmartProjectionRigFactor.h b/gtsam/slam/SmartProjectionRigFactor.h index d7e802658..4bfd56695 100644 --- a/gtsam/slam/SmartProjectionRigFactor.h +++ b/gtsam/slam/SmartProjectionRigFactor.h @@ -92,9 +92,15 @@ class SmartProjectionRigFactor : public SmartProjectionFactor { const SharedNoiseModel& sharedNoiseModel, const Cameras& cameraRig, const SmartProjectionParams& params = SmartProjectionParams()) : Base(sharedNoiseModel, params), cameraRig_(cameraRig) { - // use only configuration that works with this factor - Base::params_.degeneracyMode = gtsam::ZERO_ON_DEGENERACY; - Base::params_.linearizationMode = gtsam::HESSIAN; + // throw exception if configuration is not supported by this factor + if (Base::params_.degeneracyMode != gtsam::ZERO_ON_DEGENERACY) + throw std::runtime_error( + "SmartProjectionRigFactor: " + "degeneracyMode must be set to ZERO_ON_DEGENERACY"); + if (Base::params_.linearizationMode != gtsam::HESSIAN) + throw std::runtime_error( + "SmartProjectionRigFactor: " + "linearizationMode must be set to HESSIAN"); } /** @@ -108,9 +114,15 @@ class SmartProjectionRigFactor : public SmartProjectionFactor { const SharedNoiseModel& sharedNoiseModel, const Camera& camera, const SmartProjectionParams& params = SmartProjectionParams()) : Base(sharedNoiseModel, params) { - // use only configuration that works with this factor - Base::params_.degeneracyMode = gtsam::ZERO_ON_DEGENERACY; - Base::params_.linearizationMode = gtsam::HESSIAN; + // throw exception if configuration is not supported by this factor + if (Base::params_.degeneracyMode != gtsam::ZERO_ON_DEGENERACY) + throw std::runtime_error( + "SmartProjectionRigFactor: " + "degeneracyMode must be set to ZERO_ON_DEGENERACY"); + if (Base::params_.linearizationMode != gtsam::HESSIAN) + throw std::runtime_error( + "SmartProjectionRigFactor: " + "linearizationMode must be set to HESSIAN"); cameraRig_.push_back(camera); } diff --git a/gtsam/slam/tests/testSmartProjectionRigFactor.cpp b/gtsam/slam/tests/testSmartProjectionRigFactor.cpp index 8e6735dbd..ec3d5ddf0 100644 --- a/gtsam/slam/tests/testSmartProjectionRigFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionRigFactor.cpp @@ -57,68 +57,87 @@ LevenbergMarquardtParams lmParams; // Make more verbose like so (in tests): // params.verbosityLM = LevenbergMarquardtParams::SUMMARY; +/* ************************************************************************* */ +// default Cal3_S2 poses with rolling shutter effect +namespace vanillaRig { +using namespace vanillaPose; +SmartProjectionParams params( + gtsam::HESSIAN, + gtsam::ZERO_ON_DEGENERACY); // only config that works with rig factors +} // namespace vanillaRig + /* ************************************************************************* */ TEST(SmartProjectionRigFactor, Constructor) { - using namespace vanillaPose; + using namespace vanillaRig; Cameras cameraRig; cameraRig.push_back(Camera(Pose3::identity(), sharedK)); - SmartRigFactor::shared_ptr factor1(new SmartRigFactor(model, cameraRig)); + SmartRigFactor::shared_ptr factor1( + new SmartRigFactor(model, cameraRig, params)); } /* ************************************************************************* */ TEST(SmartProjectionRigFactor, Constructor2) { - using namespace vanillaPose; + using namespace vanillaRig; Cameras cameraRig; - SmartProjectionParams params; - params.setRankTolerance(rankTol); - SmartRigFactor factor1(model, cameraRig, params); + SmartProjectionParams params2( + gtsam::HESSIAN, + gtsam::ZERO_ON_DEGENERACY); // only config that works with rig factors + params2.setRankTolerance(rankTol); + SmartRigFactor factor1(model, cameraRig, params2); } /* ************************************************************************* */ TEST(SmartProjectionRigFactor, Constructor3) { - using namespace vanillaPose; + using namespace vanillaRig; Cameras cameraRig; cameraRig.push_back(Camera(Pose3::identity(), sharedK)); - SmartRigFactor::shared_ptr factor1(new SmartRigFactor(model, cameraRig)); + SmartRigFactor::shared_ptr factor1( + new SmartRigFactor(model, cameraRig, params)); factor1->add(measurement1, x1, cameraId1); } /* ************************************************************************* */ TEST(SmartProjectionRigFactor, Constructor4) { - using namespace vanillaPose; + using namespace vanillaRig; Cameras cameraRig; cameraRig.push_back(Camera(Pose3::identity(), sharedK)); - SmartProjectionParams params; - params.setRankTolerance(rankTol); - SmartRigFactor factor1(model, cameraRig, params); + SmartProjectionParams params2( + gtsam::HESSIAN, + gtsam::ZERO_ON_DEGENERACY); // only config that works with rig factors + params2.setRankTolerance(rankTol); + SmartRigFactor factor1(model, cameraRig, params2); factor1.add(measurement1, x1, cameraId1); } /* ************************************************************************* */ TEST(SmartProjectionRigFactor, Constructor5) { - using namespace vanillaPose; - SmartProjectionParams params; - params.setRankTolerance(rankTol); - SmartRigFactor factor1(model, Camera(Pose3::identity(), sharedK), params); + using namespace vanillaRig; + SmartProjectionParams params2( + gtsam::HESSIAN, + gtsam::ZERO_ON_DEGENERACY); // only config that works with rig factors + params2.setRankTolerance(rankTol); + SmartRigFactor factor1(model, Camera(Pose3::identity(), sharedK), params2); factor1.add(measurement1, x1, cameraId1); } /* ************************************************************************* */ TEST(SmartProjectionRigFactor, Equals) { - using namespace vanillaPose; + using namespace vanillaRig; Cameras cameraRig; // single camera in the rig cameraRig.push_back(Camera(Pose3::identity(), sharedK)); - SmartRigFactor::shared_ptr factor1(new SmartRigFactor(model, cameraRig)); + SmartRigFactor::shared_ptr factor1( + new SmartRigFactor(model, cameraRig, params)); factor1->add(measurement1, x1, cameraId1); - SmartRigFactor::shared_ptr factor2(new SmartRigFactor(model, cameraRig)); + SmartRigFactor::shared_ptr factor2( + new SmartRigFactor(model, cameraRig, params)); factor2->add(measurement1, x1, cameraId1); CHECK(assert_equal(*factor1, *factor2)); SmartRigFactor::shared_ptr factor3( - new SmartRigFactor(model, Camera(Pose3::identity(), sharedK))); + new SmartRigFactor(model, Camera(Pose3::identity(), sharedK), params)); factor3->add(measurement1, x1); // now use default CHECK(assert_equal(*factor1, *factor3)); @@ -126,13 +145,13 @@ TEST(SmartProjectionRigFactor, Equals) { /* *************************************************************************/ TEST(SmartProjectionRigFactor, noiseless) { - using namespace vanillaPose; + using namespace vanillaRig; // Project two landmarks into two cameras Point2 level_uv = level_camera.project(landmark1); Point2 level_uv_right = level_camera_right.project(landmark1); - SmartRigFactor factor(model, Camera(Pose3::identity(), sharedK)); + SmartRigFactor factor(model, Camera(Pose3::identity(), sharedK), params); factor.add(level_uv, x1); // both taken from the same camera factor.add(level_uv_right, x2); @@ -184,7 +203,7 @@ TEST(SmartProjectionRigFactor, noiseless) { /* *************************************************************************/ TEST(SmartProjectionRigFactor, noisy) { - using namespace vanillaPose; + using namespace vanillaRig; Cameras cameraRig; // single camera in the rig cameraRig.push_back(Camera(Pose3::identity(), sharedK)); @@ -200,14 +219,16 @@ TEST(SmartProjectionRigFactor, noisy) { Pose3(Rot3::Ypr(-M_PI / 10, 0., -M_PI / 10), Point3(0.5, 0.1, 0.3)); values.insert(x2, pose_right.compose(noise_pose)); - SmartRigFactor::shared_ptr factor(new SmartRigFactor(model, cameraRig)); + SmartRigFactor::shared_ptr factor( + new SmartRigFactor(model, cameraRig, params)); factor->add(level_uv, x1, cameraId1); factor->add(level_uv_right, x2, cameraId1); double actualError1 = factor->error(values); // create other factor by passing multiple measurements - SmartRigFactor::shared_ptr factor2(new SmartRigFactor(model, cameraRig)); + SmartRigFactor::shared_ptr factor2( + new SmartRigFactor(model, cameraRig, params)); Point2Vector measurements; measurements.push_back(level_uv); @@ -223,7 +244,7 @@ TEST(SmartProjectionRigFactor, noisy) { /* *************************************************************************/ TEST(SmartProjectionRigFactor, smartFactorWithSensorBodyTransform) { - using namespace vanillaPose; + using namespace vanillaRig; // create arbitrary body_T_sensor (transforms from sensor to body) Pose3 body_T_sensor = @@ -253,7 +274,7 @@ TEST(SmartProjectionRigFactor, smartFactorWithSensorBodyTransform) { SmartProjectionParams params; params.setRankTolerance(1.0); - params.setDegeneracyMode(IGNORE_DEGENERACY); + params.setDegeneracyMode(ZERO_ON_DEGENERACY); params.setEnableEPI(false); SmartRigFactor smartFactor1(model, cameraRig, params); @@ -304,7 +325,7 @@ TEST(SmartProjectionRigFactor, smartFactorWithSensorBodyTransform) { /* *************************************************************************/ TEST(SmartProjectionRigFactor, smartFactorWithMultipleCameras) { - using namespace vanillaPose; + using namespace vanillaRig; // create arbitrary body_T_sensor (transforms from sensor to body) Pose3 body_T_sensor1 = @@ -342,7 +363,7 @@ TEST(SmartProjectionRigFactor, smartFactorWithMultipleCameras) { SmartProjectionParams params; params.setRankTolerance(1.0); - params.setDegeneracyMode(IGNORE_DEGENERACY); + params.setDegeneracyMode(ZERO_ON_DEGENERACY); params.setEnableEPI(false); SmartRigFactor smartFactor1(model, cameraRig, params); @@ -406,13 +427,20 @@ TEST(SmartProjectionRigFactor, 3poses_smart_projection_factor) { FastVector cameraIds{ 0, 0, 0}; // 3 measurements from the same camera in the rig - SmartRigFactor::shared_ptr smartFactor1(new SmartRigFactor(model, cameraRig)); + SmartProjectionParams params( + gtsam::HESSIAN, + gtsam::ZERO_ON_DEGENERACY); // only config that works with rig factors + + SmartRigFactor::shared_ptr smartFactor1( + new SmartRigFactor(model, cameraRig, params)); smartFactor1->add(measurements_cam1, views, cameraIds); - SmartRigFactor::shared_ptr smartFactor2(new SmartRigFactor(model, cameraRig)); + SmartRigFactor::shared_ptr smartFactor2( + new SmartRigFactor(model, cameraRig, params)); smartFactor2->add(measurements_cam2, views, cameraIds); - SmartRigFactor::shared_ptr smartFactor3(new SmartRigFactor(model, cameraRig)); + SmartRigFactor::shared_ptr smartFactor3( + new SmartRigFactor(model, cameraRig, params)); smartFactor3->add(measurements_cam3, views, cameraIds); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -454,7 +482,7 @@ TEST(SmartProjectionRigFactor, 3poses_smart_projection_factor) { /* *************************************************************************/ TEST(SmartProjectionRigFactor, Factors) { - using namespace vanillaPose; + using namespace vanillaRig; // Default cameras for simple derivatives Rot3 R; @@ -476,7 +504,7 @@ TEST(SmartProjectionRigFactor, Factors) { FastVector cameraIds{0, 0}; SmartRigFactor::shared_ptr smartFactor1 = boost::make_shared( - model, Camera(Pose3::identity(), sharedK)); + model, Camera(Pose3::identity(), sharedK), params); smartFactor1->add(measurements_cam1, views); // we have a single camera so use default cameraIds @@ -543,7 +571,7 @@ TEST(SmartProjectionRigFactor, Factors) { /* *************************************************************************/ TEST(SmartProjectionRigFactor, 3poses_iterative_smart_projection_factor) { - using namespace vanillaPose; + using namespace vanillaRig; KeyVector views{x1, x2, x3}; @@ -563,13 +591,16 @@ TEST(SmartProjectionRigFactor, 3poses_iterative_smart_projection_factor) { Cameras cameraRig; // single camera in the rig cameraRig.push_back(Camera(Pose3::identity(), sharedK)); FastVector cameraIds{0, 0, 0}; - SmartRigFactor::shared_ptr smartFactor1(new SmartRigFactor(model, cameraRig)); + SmartRigFactor::shared_ptr smartFactor1( + new SmartRigFactor(model, cameraRig, params)); smartFactor1->add(measurements_cam1, views, cameraIds); - SmartRigFactor::shared_ptr smartFactor2(new SmartRigFactor(model, cameraRig)); + SmartRigFactor::shared_ptr smartFactor2( + new SmartRigFactor(model, cameraRig, params)); smartFactor2->add(measurements_cam2, views, cameraIds); - SmartRigFactor::shared_ptr smartFactor3(new SmartRigFactor(model, cameraRig)); + SmartRigFactor::shared_ptr smartFactor3( + new SmartRigFactor(model, cameraRig, params)); smartFactor3->add(measurements_cam3, views, cameraIds); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -605,7 +636,7 @@ TEST(SmartProjectionRigFactor, 3poses_iterative_smart_projection_factor) { /* *************************************************************************/ TEST(SmartProjectionRigFactor, landmarkDistance) { - using namespace vanillaPose; + using namespace vanillaRig; double excludeLandmarksFutherThanDist = 2; @@ -620,8 +651,8 @@ TEST(SmartProjectionRigFactor, landmarkDistance) { SmartProjectionParams params; params.setRankTolerance(1.0); - params.setLinearizationMode(gtsam::JACOBIAN_SVD); - params.setDegeneracyMode(gtsam::IGNORE_DEGENERACY); + params.setLinearizationMode(gtsam::HESSIAN); + params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY); params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist); params.setEnableEPI(false); @@ -668,7 +699,7 @@ TEST(SmartProjectionRigFactor, landmarkDistance) { /* *************************************************************************/ TEST(SmartProjectionRigFactor, dynamicOutlierRejection) { - using namespace vanillaPose; + using namespace vanillaRig; double excludeLandmarksFutherThanDist = 1e10; double dynamicOutlierRejectionThreshold = @@ -742,7 +773,7 @@ TEST(SmartProjectionRigFactor, dynamicOutlierRejection) { TEST(SmartProjectionRigFactor, CheckHessian) { KeyVector views{x1, x2, x3}; - using namespace vanillaPose; + using namespace vanillaRig; // Two slightly different cameras Pose3 pose2 = @@ -842,7 +873,12 @@ TEST(SmartProjectionRigFactor, Hessian) { cameraRig.push_back(Camera(Pose3::identity(), sharedK2)); FastVector cameraIds{0, 0}; - SmartRigFactor::shared_ptr smartFactor1(new SmartRigFactor(model, cameraRig)); + SmartProjectionParams params( + gtsam::HESSIAN, + gtsam::ZERO_ON_DEGENERACY); // only config that works with rig factors + + SmartRigFactor::shared_ptr smartFactor1( + new SmartRigFactor(model, cameraRig, params)); smartFactor1->add(measurements_cam1, views, cameraIds); Pose3 noise_pose = @@ -875,6 +911,9 @@ TEST(SmartProjectionRigFactor, ConstructorWithCal3Bundler) { /* *************************************************************************/ TEST(SmartProjectionRigFactor, Cal3Bundler) { using namespace bundlerPose; + SmartProjectionParams params( + gtsam::HESSIAN, + gtsam::ZERO_ON_DEGENERACY); // only config that works with rig factors // three landmarks ~5 meters in front of camera Point3 landmark3(3, 0, 3.0); @@ -892,13 +931,16 @@ TEST(SmartProjectionRigFactor, Cal3Bundler) { cameraRig.push_back(Camera(Pose3::identity(), sharedBundlerK)); FastVector cameraIds{0, 0, 0}; - SmartRigFactor::shared_ptr smartFactor1(new SmartRigFactor(model, cameraRig)); + SmartRigFactor::shared_ptr smartFactor1( + new SmartRigFactor(model, cameraRig, params)); smartFactor1->add(measurements_cam1, views, cameraIds); - SmartRigFactor::shared_ptr smartFactor2(new SmartRigFactor(model, cameraRig)); + SmartRigFactor::shared_ptr smartFactor2( + new SmartRigFactor(model, cameraRig, params)); smartFactor2->add(measurements_cam2, views, cameraIds); - SmartRigFactor::shared_ptr smartFactor3(new SmartRigFactor(model, cameraRig)); + SmartRigFactor::shared_ptr smartFactor3( + new SmartRigFactor(model, cameraRig, params)); smartFactor3->add(measurements_cam3, views, cameraIds); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -942,7 +984,7 @@ TEST(SmartProjectionRigFactor, // measurements of the same landmark at a single pose, a setup that occurs in // multi-camera systems - using namespace vanillaPose; + using namespace vanillaRig; Point2Vector measurements_lmk1; // Project three landmarks into three cameras @@ -960,7 +1002,8 @@ TEST(SmartProjectionRigFactor, cameraRig.push_back(Camera(Pose3::identity(), sharedK)); FastVector cameraIds{0, 0, 0, 0}; - SmartRigFactor::shared_ptr smartFactor1(new SmartRigFactor(model, cameraRig)); + SmartRigFactor::shared_ptr smartFactor1( + new SmartRigFactor(model, cameraRig, params)); smartFactor1->add(measurements_lmk1_redundant, keys, cameraIds); Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), @@ -1073,7 +1116,7 @@ TEST(SmartProjectionRigFactor, /* *************************************************************************/ TEST(SmartProjectionRigFactor, optimization_3poses_measurementsFromSamePose) { - using namespace vanillaPose; + using namespace vanillaRig; Point2Vector measurements_lmk1, measurements_lmk2, measurements_lmk3; // Project three landmarks into three cameras @@ -1097,14 +1140,17 @@ TEST(SmartProjectionRigFactor, optimization_3poses_measurementsFromSamePose) { KeyVector keys_redundant = keys; keys_redundant.push_back(keys.at(0)); // we readd the first key - SmartRigFactor::shared_ptr smartFactor1(new SmartRigFactor(model, cameraRig)); + SmartRigFactor::shared_ptr smartFactor1( + new SmartRigFactor(model, cameraRig, params)); smartFactor1->add(measurements_lmk1_redundant, keys_redundant, cameraIdsRedundant); - SmartRigFactor::shared_ptr smartFactor2(new SmartRigFactor(model, cameraRig)); + SmartRigFactor::shared_ptr smartFactor2( + new SmartRigFactor(model, cameraRig, params)); smartFactor2->add(measurements_lmk2, keys, cameraIds); - SmartRigFactor::shared_ptr smartFactor3(new SmartRigFactor(model, cameraRig)); + SmartRigFactor::shared_ptr smartFactor3( + new SmartRigFactor(model, cameraRig, params)); smartFactor3->add(measurements_lmk3, keys, cameraIds); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -1150,12 +1196,13 @@ TEST(SmartProjectionRigFactor, optimization_3poses_measurementsFromSamePose) { // this factor is slightly slower (but comparable) to original // SmartProjectionPoseFactor //-Total: 0 CPU (0 times, 0 wall, 0.17 children, min: 0 max: 0) -//| -SmartRigFactor LINEARIZE: 0.11 CPU (10000 times, 0.086311 wall, 0.11 -// children, min: 0 max: 0) | -SmartPoseFactor LINEARIZE: 0.06 CPU (10000 -// times, 0.065103 wall, 0.06 children, min: 0 max: 0) +//| -SmartRigFactor LINEARIZE: 0.06 CPU +//(10000 times, 0.061226 wall, 0.06 children, min: 0 max: 0) +//| -SmartPoseFactor LINEARIZE: 0.06 CPU +//(10000 times, 0.073037 wall, 0.06 children, min: 0 max: 0) /* *************************************************************************/ TEST(SmartProjectionRigFactor, timing) { - using namespace vanillaPose; + using namespace vanillaRig; // Default cameras for simple derivatives static Cal3_S2::shared_ptr sharedKSimple(new Cal3_S2(100, 100, 0, 0, 0)); @@ -1182,7 +1229,7 @@ TEST(SmartProjectionRigFactor, timing) { for (size_t i = 0; i < nrTests; i++) { SmartRigFactor::shared_ptr smartFactorP( - new SmartRigFactor(model, cameraRig)); + new SmartRigFactor(model, cameraRig, params)); smartFactorP->add(measurements_lmk1[0], x1, cameraId1); smartFactorP->add(measurements_lmk1[1], x1, cameraId1); @@ -1195,7 +1242,8 @@ TEST(SmartProjectionRigFactor, timing) { } for (size_t i = 0; i < nrTests; i++) { - SmartFactor::shared_ptr smartFactor(new SmartFactor(model, sharedKSimple)); + SmartFactor::shared_ptr smartFactor( + new SmartFactor(model, sharedKSimple, params)); smartFactor->add(measurements_lmk1[0], x1); smartFactor->add(measurements_lmk1[1], x2); @@ -1225,7 +1273,7 @@ TEST(SmartProjectionRigFactor, timing) { // BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal"); // // TEST(SmartProjectionRigFactor, serialize) { -// using namespace vanillaPose; +// using namespace vanillaRig; // using namespace gtsam::serializationTestHelpers; // SmartProjectionParams params; // params.setRankTolerance(rankTol); @@ -1242,7 +1290,7 @@ TEST(SmartProjectionRigFactor, timing) { // //// SERIALIZATION TEST FAILS: to be fixed ////TEST(SmartProjectionRigFactor, serialize2) { -//// using namespace vanillaPose; +//// using namespace vanillaRig; //// using namespace gtsam::serializationTestHelpers; //// SmartProjectionParams params; //// params.setRankTolerance(rankTol); diff --git a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h index d16cfa2da..4a9481d6b 100644 --- a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h +++ b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h @@ -95,9 +95,15 @@ class SmartProjectionPoseFactorRollingShutter const SharedNoiseModel& sharedNoiseModel, const Cameras& cameraRig, const SmartProjectionParams& params = SmartProjectionParams()) : Base(sharedNoiseModel, params), cameraRig_(cameraRig) { - // use only configuration that works with this factor - Base::params_.degeneracyMode = gtsam::ZERO_ON_DEGENERACY; - Base::params_.linearizationMode = gtsam::HESSIAN; + // throw exception if configuration is not supported by this factor + if (Base::params_.degeneracyMode != gtsam::ZERO_ON_DEGENERACY) + throw std::runtime_error( + "SmartProjectionRigFactor: " + "degeneracyMode must be set to ZERO_ON_DEGENERACY"); + if (Base::params_.linearizationMode != gtsam::HESSIAN) + throw std::runtime_error( + "SmartProjectionRigFactor: " + "linearizationMode must be set to HESSIAN"); } /** @@ -110,9 +116,15 @@ class SmartProjectionPoseFactorRollingShutter const SharedNoiseModel& sharedNoiseModel, const Camera& camera, const SmartProjectionParams& params = SmartProjectionParams()) : Base(sharedNoiseModel, params) { - // use only configuration that works with this factor - Base::params_.degeneracyMode = gtsam::ZERO_ON_DEGENERACY; - Base::params_.linearizationMode = gtsam::HESSIAN; + // throw exception if configuration is not supported by this factor + if (Base::params_.degeneracyMode != gtsam::ZERO_ON_DEGENERACY) + throw std::runtime_error( + "SmartProjectionRigFactor: " + "degeneracyMode must be set to ZERO_ON_DEGENERACY"); + if (Base::params_.linearizationMode != gtsam::HESSIAN) + throw std::runtime_error( + "SmartProjectionRigFactor: " + "linearizationMode must be set to HESSIAN"); cameraRig_.push_back(camera); } diff --git a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp index 09a16e6fb..1d32eccca 100644 --- a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp +++ b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp @@ -75,6 +75,9 @@ Pose3 interp_pose3 = interpolate(pose_above, level_pose, interp_factor3); Camera cam1(interp_pose1, sharedK); Camera cam2(interp_pose2, sharedK); Camera cam3(interp_pose3, sharedK); +SmartProjectionParams params( + gtsam::HESSIAN, + gtsam::ZERO_ON_DEGENERACY); // only config that works with RS factors } // namespace vanillaPoseRS LevenbergMarquardtParams lmParams; @@ -85,13 +88,12 @@ typedef SmartProjectionPoseFactorRollingShutter> TEST(SmartProjectionPoseFactorRollingShutter, Constructor) { using namespace vanillaPoseRS; SmartFactorRS::shared_ptr factor1( - new SmartFactorRS(model, Camera(Pose3::identity(), sharedK))); + new SmartFactorRS(model, Camera(Pose3::identity(), sharedK), params)); } /* ************************************************************************* */ TEST(SmartProjectionPoseFactorRollingShutter, Constructor2) { using namespace vanillaPoseRS; - SmartProjectionParams params; params.setRankTolerance(rankTol); SmartFactorRS factor1(model, Camera(Pose3::identity(), sharedK), params); } @@ -100,13 +102,13 @@ TEST(SmartProjectionPoseFactorRollingShutter, Constructor2) { TEST(SmartProjectionPoseFactorRollingShutter, add) { using namespace vanillaPoseRS; SmartFactorRS::shared_ptr factor1( - new SmartFactorRS(model, Camera(Pose3::identity(), sharedK))); + new SmartFactorRS(model, Camera(Pose3::identity(), sharedK), params)); factor1->add(measurement1, x1, x2, interp_factor); } /* ************************************************************************* */ TEST(SmartProjectionPoseFactorRollingShutter, Equals) { - using namespace vanillaPose; + using namespace vanillaPoseRS; // create fake measurements Point2Vector measurements; @@ -130,15 +132,18 @@ TEST(SmartProjectionPoseFactorRollingShutter, Equals) { cameraRig.push_back(Camera(body_P_sensor, sharedK)); // create by adding a batch of measurements with a bunch of calibrations - SmartFactorRS::shared_ptr factor2(new SmartFactorRS(model, cameraRig)); + SmartFactorRS::shared_ptr factor2( + new SmartFactorRS(model, cameraRig, params)); factor2->add(measurements, key_pairs, interp_factors, cameraIds); // create by adding a batch of measurements with a single calibrations - SmartFactorRS::shared_ptr factor3(new SmartFactorRS(model, cameraRig)); + SmartFactorRS::shared_ptr factor3( + new SmartFactorRS(model, cameraRig, params)); factor3->add(measurements, key_pairs, interp_factors, cameraIds); { // create equal factors and show equal returns true - SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model, cameraRig)); + SmartFactorRS::shared_ptr factor1( + new SmartFactorRS(model, cameraRig, params)); factor1->add(measurement1, x1, x2, interp_factor1, cameraId1); factor1->add(measurement2, x2, x3, interp_factor2, cameraId1); factor1->add(measurement3, x3, x4, interp_factor3, cameraId1); @@ -147,7 +152,8 @@ TEST(SmartProjectionPoseFactorRollingShutter, Equals) { EXPECT(factor1->equals(*factor3)); } { // create equal factors and show equal returns true (use default cameraId) - SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model, cameraRig)); + SmartFactorRS::shared_ptr factor1( + new SmartFactorRS(model, cameraRig, params)); factor1->add(measurement1, x1, x2, interp_factor1); factor1->add(measurement2, x2, x3, interp_factor2); factor1->add(measurement3, x3, x4, interp_factor3); @@ -156,7 +162,8 @@ TEST(SmartProjectionPoseFactorRollingShutter, Equals) { EXPECT(factor1->equals(*factor3)); } { // create equal factors and show equal returns true (use default cameraId) - SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model, cameraRig)); + SmartFactorRS::shared_ptr factor1( + new SmartFactorRS(model, cameraRig, params)); factor1->add(measurements, key_pairs, interp_factors); EXPECT(factor1->equals(*factor2)); @@ -164,7 +171,8 @@ TEST(SmartProjectionPoseFactorRollingShutter, Equals) { } { // create slightly different factors (different keys) and show equal // returns false (use default cameraIds) - SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model, cameraRig)); + SmartFactorRS::shared_ptr factor1( + new SmartFactorRS(model, cameraRig, params)); factor1->add(measurement1, x1, x2, interp_factor1, cameraId1); factor1->add(measurement2, x2, x2, interp_factor2, cameraId1); // different! @@ -177,7 +185,8 @@ TEST(SmartProjectionPoseFactorRollingShutter, Equals) { // returns false Cameras cameraRig2; cameraRig2.push_back(Camera(body_P_sensor * body_P_sensor, sharedK)); - SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model, cameraRig2)); + SmartFactorRS::shared_ptr factor1( + new SmartFactorRS(model, cameraRig2, params)); factor1->add(measurement1, x1, x2, interp_factor1, cameraId1); factor1->add(measurement2, x2, x3, interp_factor2, cameraId1); // different! @@ -188,7 +197,8 @@ TEST(SmartProjectionPoseFactorRollingShutter, Equals) { } { // create slightly different factors (different interp factors) and show // equal returns false - SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model, cameraRig)); + SmartFactorRS::shared_ptr factor1( + new SmartFactorRS(model, cameraRig, params)); factor1->add(measurement1, x1, x2, interp_factor1, cameraId1); factor1->add(measurement2, x2, x3, interp_factor1, cameraId1); // different! @@ -216,7 +226,7 @@ TEST(SmartProjectionPoseFactorRollingShutter, noiselessErrorAndJacobians) { Point2 level_uv_right = cam2.project(landmark1); Pose3 body_P_sensorId = Pose3::identity(); - SmartFactorRS factor(model, Camera(body_P_sensorId, sharedK)); + SmartFactorRS factor(model, Camera(body_P_sensorId, sharedK), params); factor.add(level_uv, x1, x2, interp_factor1); factor.add(level_uv_right, x2, x3, interp_factor2); @@ -291,7 +301,7 @@ TEST(SmartProjectionPoseFactorRollingShutter, noisyErrorAndJacobians) { Point2 level_uv_right = cam2.project(landmark1); Pose3 body_P_sensorNonId = body_P_sensor; - SmartFactorRS factor(model, Camera(body_P_sensorNonId, sharedK)); + SmartFactorRS factor(model, Camera(body_P_sensorNonId, sharedK), params); factor.add(level_uv, x1, x2, interp_factor1); factor.add(level_uv_right, x2, x3, interp_factor2); @@ -383,15 +393,15 @@ TEST(SmartProjectionPoseFactorRollingShutter, optimization_3poses) { interp_factors.push_back(interp_factor3); SmartFactorRS::shared_ptr smartFactor1( - new SmartFactorRS(model, Camera(Pose3::identity(), sharedK))); + new SmartFactorRS(model, Camera(Pose3::identity(), sharedK), params)); smartFactor1->add(measurements_lmk1, key_pairs, interp_factors); SmartFactorRS::shared_ptr smartFactor2( - new SmartFactorRS(model, Camera(Pose3::identity(), sharedK))); + new SmartFactorRS(model, Camera(Pose3::identity(), sharedK), params)); smartFactor2->add(measurements_lmk2, key_pairs, interp_factors); SmartFactorRS::shared_ptr smartFactor3( - new SmartFactorRS(model, Camera(Pose3::identity(), sharedK))); + new SmartFactorRS(model, Camera(Pose3::identity(), sharedK), params)); smartFactor3->add(measurements_lmk3, key_pairs, interp_factors); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -457,13 +467,16 @@ TEST(SmartProjectionPoseFactorRollingShutter, optimization_3poses_multiCam) { cameraRig.push_back(Camera(body_P_sensor, sharedK)); cameraRig.push_back(Camera(Pose3::identity(), sharedK)); - SmartFactorRS::shared_ptr smartFactor1(new SmartFactorRS(model, cameraRig)); + SmartFactorRS::shared_ptr smartFactor1( + new SmartFactorRS(model, cameraRig, params)); smartFactor1->add(measurements_lmk1, key_pairs, interp_factors, {1, 1, 1}); - SmartFactorRS::shared_ptr smartFactor2(new SmartFactorRS(model, cameraRig)); + SmartFactorRS::shared_ptr smartFactor2( + new SmartFactorRS(model, cameraRig, params)); smartFactor2->add(measurements_lmk2, key_pairs, interp_factors, {1, 1, 1}); - SmartFactorRS::shared_ptr smartFactor3(new SmartFactorRS(model, cameraRig)); + SmartFactorRS::shared_ptr smartFactor3( + new SmartFactorRS(model, cameraRig, params)); smartFactor3->add(measurements_lmk3, key_pairs, interp_factors, {1, 1, 1}); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -543,13 +556,16 @@ TEST(SmartProjectionPoseFactorRollingShutter, optimization_3poses_multiCam2) { cameraRig.push_back(Camera(body_T_sensor2, sharedK)); cameraRig.push_back(Camera(body_T_sensor3, sharedK)); - SmartFactorRS::shared_ptr smartFactor1(new SmartFactorRS(model, cameraRig)); + SmartFactorRS::shared_ptr smartFactor1( + new SmartFactorRS(model, cameraRig, params)); smartFactor1->add(measurements_lmk1, key_pairs, interp_factors, {0, 1, 2}); - SmartFactorRS::shared_ptr smartFactor2(new SmartFactorRS(model, cameraRig)); + SmartFactorRS::shared_ptr smartFactor2( + new SmartFactorRS(model, cameraRig, params)); smartFactor2->add(measurements_lmk2, key_pairs, interp_factors, {0, 1, 2}); - SmartFactorRS::shared_ptr smartFactor3(new SmartFactorRS(model, cameraRig)); + SmartFactorRS::shared_ptr smartFactor3( + new SmartFactorRS(model, cameraRig, params)); smartFactor3->add(measurements_lmk3, key_pairs, interp_factors, {0, 1, 2}); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -597,7 +613,7 @@ TEST(SmartProjectionPoseFactorRollingShutter, hessian_simple_2poses) { // falls back to standard pixel measurements) Note: this is a quite extreme // test since in typical camera you would not have more than 1 measurement per // landmark at each interpolated pose - using namespace vanillaPose; + using namespace vanillaPoseRS; // Default cameras for simple derivatives static Cal3_S2::shared_ptr sharedKSimple(new Cal3_S2(100, 100, 0, 0, 0)); @@ -618,13 +634,13 @@ TEST(SmartProjectionPoseFactorRollingShutter, hessian_simple_2poses) { measurements_lmk1.push_back(cam2.project(landmark1)); SmartFactorRS::shared_ptr smartFactor1( - new SmartFactorRS(model, Camera(body_P_sensorId, sharedKSimple))); + new SmartFactorRS(model, Camera(body_P_sensorId, sharedKSimple), params)); double interp_factor = 0; // equivalent to measurement taken at pose 1 smartFactor1->add(measurements_lmk1[0], x1, x2, interp_factor); interp_factor = 1; // equivalent to measurement taken at pose 2 smartFactor1->add(measurements_lmk1[1], x1, x2, interp_factor); - SmartFactor::Cameras cameras; + SmartFactorRS::Cameras cameras; cameras.push_back(cam1); cameras.push_back(cam2); @@ -772,7 +788,9 @@ TEST(SmartProjectionPoseFactorRollingShutter, SmartProjectionParams params; params.setRankTolerance(1.0); params.setLinearizationMode(gtsam::HESSIAN); - params.setDegeneracyMode(gtsam::IGNORE_DEGENERACY); + // params.setDegeneracyMode(gtsam::IGNORE_DEGENERACY); // this would give an + // exception as expected + params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY); params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist); params.setEnableEPI(false); @@ -916,7 +934,7 @@ TEST(SmartProjectionPoseFactorRollingShutter, interp_factors.push_back(interp_factor3); SmartFactorRS::shared_ptr smartFactor1( - new SmartFactorRS(model, Camera(Pose3::identity(), sharedK))); + new SmartFactorRS(model, Camera(Pose3::identity(), sharedK), params)); smartFactor1->add(measurements_lmk1, key_pairs, interp_factors); Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), @@ -1054,7 +1072,7 @@ TEST(SmartProjectionPoseFactorRollingShutter, interp_factors.push_back(interp_factor1); SmartFactorRS::shared_ptr smartFactor1( - new SmartFactorRS(model, Camera(Pose3::identity(), sharedK))); + new SmartFactorRS(model, Camera(Pose3::identity(), sharedK), params)); smartFactor1->add(measurements_lmk1_redundant, key_pairs, interp_factors); Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), @@ -1210,16 +1228,16 @@ TEST(SmartProjectionPoseFactorRollingShutter, interp_factors.at(0)); // we readd the first interp factor SmartFactorRS::shared_ptr smartFactor1( - new SmartFactorRS(model, Camera(Pose3::identity(), sharedK))); + new SmartFactorRS(model, Camera(Pose3::identity(), sharedK), params)); smartFactor1->add(measurements_lmk1_redundant, key_pairs_redundant, interp_factors_redundant); SmartFactorRS::shared_ptr smartFactor2( - new SmartFactorRS(model, Camera(Pose3::identity(), sharedK))); + new SmartFactorRS(model, Camera(Pose3::identity(), sharedK), params)); smartFactor2->add(measurements_lmk2, key_pairs, interp_factors); SmartFactorRS::shared_ptr smartFactor3( - new SmartFactorRS(model, Camera(Pose3::identity(), sharedK))); + new SmartFactorRS(model, Camera(Pose3::identity(), sharedK), params)); smartFactor3->add(measurements_lmk3, key_pairs, interp_factors); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -1263,15 +1281,19 @@ TEST(SmartProjectionPoseFactorRollingShutter, #ifndef DISABLE_TIMING #include //-Total: 0 CPU (0 times, 0 wall, 0.21 children, min: 0 max: 0) -//| -SF RS LINEARIZE: 0.15 CPU (10000 times, 0.125521 wall, 0.15 children, -// min: 0 max: 0) | -RS LINEARIZE: 0.06 CPU (10000 times, 0.06311 wall, 0.06 -// children, min: 0 max: 0) +//| -SF RS LINEARIZE: 0.09 CPU +// (10000 times, 0.124106 wall, 0.09 children, min: 0 max: 0) +//| -RS LINEARIZE: 0.09 CPU +// (10000 times, 0.068719 wall, 0.09 children, min: 0 max: 0) /* *************************************************************************/ TEST(SmartProjectionPoseFactorRollingShutter, timing) { using namespace vanillaPose; // Default cameras for simple derivatives static Cal3_S2::shared_ptr sharedKSimple(new Cal3_S2(100, 100, 0, 0, 0)); + SmartProjectionParams params( + gtsam::HESSIAN, + gtsam::ZERO_ON_DEGENERACY); // only config that works with RS factors Rot3 R = Rot3::identity(); Pose3 pose1 = Pose3(R, Point3(0, 0, 0)); @@ -1291,8 +1313,8 @@ TEST(SmartProjectionPoseFactorRollingShutter, timing) { size_t nrTests = 10000; for (size_t i = 0; i < nrTests; i++) { - SmartFactorRS::shared_ptr smartFactorRS( - new SmartFactorRS(model, Camera(body_P_sensorId, sharedKSimple))); + SmartFactorRS::shared_ptr smartFactorRS(new SmartFactorRS( + model, Camera(body_P_sensorId, sharedKSimple), params)); double interp_factor = 0; // equivalent to measurement taken at pose 1 smartFactorRS->add(measurements_lmk1[0], x1, x2, interp_factor); interp_factor = 1; // equivalent to measurement taken at pose 2 @@ -1307,7 +1329,8 @@ TEST(SmartProjectionPoseFactorRollingShutter, timing) { } for (size_t i = 0; i < nrTests; i++) { - SmartFactor::shared_ptr smartFactor(new SmartFactor(model, sharedKSimple)); + SmartFactor::shared_ptr smartFactor( + new SmartFactor(model, sharedKSimple, params)); smartFactor->add(measurements_lmk1[0], x1); smartFactor->add(measurements_lmk1[1], x2);