now throwing exception is params are incorrect
							parent
							
								
									1e384686a1
								
							
						
					
					
						commit
						710a64fed4
					
				|  | @ -92,9 +92,15 @@ class SmartProjectionRigFactor : public SmartProjectionFactor<CAMERA> { | |||
|       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<CAMERA> { | |||
|       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); | ||||
|   } | ||||
| 
 | ||||
|  |  | |||
|  | @ -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<size_t> 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<size_t> cameraIds{0, 0}; | ||||
| 
 | ||||
|   SmartRigFactor::shared_ptr smartFactor1 = boost::make_shared<SmartRigFactor>( | ||||
|       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<size_t> 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<size_t> 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<size_t> 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<size_t> 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);
 | ||||
|  |  | |||
|  | @ -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); | ||||
|   } | ||||
| 
 | ||||
|  |  | |||
|  | @ -75,6 +75,9 @@ Pose3 interp_pose3 = interpolate<Pose3>(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<PinholePose<Cal3_S2>> | |||
| 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 <gtsam/base/timing.h> | ||||
| //-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); | ||||
| 
 | ||||
|  |  | |||
		Loading…
	
		Reference in New Issue