12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

now throwing exception is params are incorrect

release/4.3a0
lcarlone 2021-11-06 19:22:28 -04:00
parent 1e384686a1
commit 710a64fed4
4 changed files with 206 additions and 111 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
gtsam/slam/SmartProjectionRigFactor.h
View File

@ -92,9 +92,15 @@ class SmartProjectionRigFactor : public SmartProjectionFactor<CAMERA> {
const SharedNoiseModel& sharedNoiseModel, const Cameras& cameraRig, const SharedNoiseModel& sharedNoiseModel, const Cameras& cameraRig,
const SmartProjectionParams& params = SmartProjectionParams()) const SmartProjectionParams& params = SmartProjectionParams())
: Base(sharedNoiseModel, params), cameraRig_(cameraRig) { : Base(sharedNoiseModel, params), cameraRig_(cameraRig) {
// use only configuration that works with this factor // throw exception if configuration is not supported by this factor
Base::params_.degeneracyMode = gtsam::ZERO_ON_DEGENERACY; if (Base::params_.degeneracyMode != gtsam::ZERO_ON_DEGENERACY)
Base::params_.linearizationMode = gtsam::HESSIAN; 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 SharedNoiseModel& sharedNoiseModel, const Camera& camera,
const SmartProjectionParams& params = SmartProjectionParams()) const SmartProjectionParams& params = SmartProjectionParams())
: Base(sharedNoiseModel, params) { : Base(sharedNoiseModel, params) {
// use only configuration that works with this factor // throw exception if configuration is not supported by this factor
Base::params_.degeneracyMode = gtsam::ZERO_ON_DEGENERACY; if (Base::params_.degeneracyMode != gtsam::ZERO_ON_DEGENERACY)
Base::params_.linearizationMode = gtsam::HESSIAN; 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); cameraRig_.push_back(camera);
} }

170
gtsam/slam/tests/testSmartProjectionRigFactor.cpp
View File

@ -57,68 +57,87 @@ LevenbergMarquardtParams lmParams;
// Make more verbose like so (in tests): // Make more verbose like so (in tests):
// params.verbosityLM = LevenbergMarquardtParams::SUMMARY; // 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) { TEST(SmartProjectionRigFactor, Constructor) {
using namespace vanillaPose; using namespace vanillaRig;
Cameras cameraRig; Cameras cameraRig;
cameraRig.push_back(Camera(Pose3::identity(), sharedK)); 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) { TEST(SmartProjectionRigFactor, Constructor2) {
using namespace vanillaPose; using namespace vanillaRig;
Cameras cameraRig; Cameras cameraRig;
SmartProjectionParams params; SmartProjectionParams params2(
params.setRankTolerance(rankTol); gtsam::HESSIAN,
SmartRigFactor factor1(model, cameraRig, params); gtsam::ZERO_ON_DEGENERACY); // only config that works with rig factors
params2.setRankTolerance(rankTol);
SmartRigFactor factor1(model, cameraRig, params2);
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST(SmartProjectionRigFactor, Constructor3) { TEST(SmartProjectionRigFactor, Constructor3) {
using namespace vanillaPose; using namespace vanillaRig;
Cameras cameraRig; Cameras cameraRig;
cameraRig.push_back(Camera(Pose3::identity(), sharedK)); 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); factor1->add(measurement1, x1, cameraId1);
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST(SmartProjectionRigFactor, Constructor4) { TEST(SmartProjectionRigFactor, Constructor4) {
using namespace vanillaPose; using namespace vanillaRig;
Cameras cameraRig; Cameras cameraRig;
cameraRig.push_back(Camera(Pose3::identity(), sharedK)); cameraRig.push_back(Camera(Pose3::identity(), sharedK));
SmartProjectionParams params; SmartProjectionParams params2(
params.setRankTolerance(rankTol); gtsam::HESSIAN,
SmartRigFactor factor1(model, cameraRig, params); gtsam::ZERO_ON_DEGENERACY); // only config that works with rig factors
params2.setRankTolerance(rankTol);
SmartRigFactor factor1(model, cameraRig, params2);
factor1.add(measurement1, x1, cameraId1); factor1.add(measurement1, x1, cameraId1);
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST(SmartProjectionRigFactor, Constructor5) { TEST(SmartProjectionRigFactor, Constructor5) {
using namespace vanillaPose; using namespace vanillaRig;
SmartProjectionParams params; SmartProjectionParams params2(
params.setRankTolerance(rankTol); gtsam::HESSIAN,
SmartRigFactor factor1(model, Camera(Pose3::identity(), sharedK), params); 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); factor1.add(measurement1, x1, cameraId1);
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST(SmartProjectionRigFactor, Equals) { TEST(SmartProjectionRigFactor, Equals) {
using namespace vanillaPose; using namespace vanillaRig;
Cameras cameraRig; // single camera in the rig Cameras cameraRig; // single camera in the rig
cameraRig.push_back(Camera(Pose3::identity(), sharedK)); 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); 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); factor2->add(measurement1, x1, cameraId1);
CHECK(assert_equal(*factor1, *factor2)); CHECK(assert_equal(*factor1, *factor2));
SmartRigFactor::shared_ptr factor3( 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 factor3->add(measurement1, x1); // now use default
CHECK(assert_equal(*factor1, *factor3)); CHECK(assert_equal(*factor1, *factor3));
@ -126,13 +145,13 @@ TEST(SmartProjectionRigFactor, Equals) {
/* *************************************************************************/ /* *************************************************************************/
TEST(SmartProjectionRigFactor, noiseless) { TEST(SmartProjectionRigFactor, noiseless) {
using namespace vanillaPose; using namespace vanillaRig;
// Project two landmarks into two cameras // Project two landmarks into two cameras
Point2 level_uv = level_camera.project(landmark1); Point2 level_uv = level_camera.project(landmark1);
Point2 level_uv_right = level_camera_right.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, x1); // both taken from the same camera
factor.add(level_uv_right, x2); factor.add(level_uv_right, x2);
@ -184,7 +203,7 @@ TEST(SmartProjectionRigFactor, noiseless) {
/* *************************************************************************/ /* *************************************************************************/
TEST(SmartProjectionRigFactor, noisy) { TEST(SmartProjectionRigFactor, noisy) {
using namespace vanillaPose; using namespace vanillaRig;
Cameras cameraRig; // single camera in the rig Cameras cameraRig; // single camera in the rig
cameraRig.push_back(Camera(Pose3::identity(), sharedK)); 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)); 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)); 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, x1, cameraId1);
factor->add(level_uv_right, x2, cameraId1); factor->add(level_uv_right, x2, cameraId1);
double actualError1 = factor->error(values); double actualError1 = factor->error(values);
// create other factor by passing multiple measurements // 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; Point2Vector measurements;
measurements.push_back(level_uv); measurements.push_back(level_uv);
@ -223,7 +244,7 @@ TEST(SmartProjectionRigFactor, noisy) {
/* *************************************************************************/ /* *************************************************************************/
TEST(SmartProjectionRigFactor, smartFactorWithSensorBodyTransform) { TEST(SmartProjectionRigFactor, smartFactorWithSensorBodyTransform) {
using namespace vanillaPose; using namespace vanillaRig;
// create arbitrary body_T_sensor (transforms from sensor to body) // create arbitrary body_T_sensor (transforms from sensor to body)
Pose3 body_T_sensor = Pose3 body_T_sensor =
@ -253,7 +274,7 @@ TEST(SmartProjectionRigFactor, smartFactorWithSensorBodyTransform) {
SmartProjectionParams params; SmartProjectionParams params;
params.setRankTolerance(1.0); params.setRankTolerance(1.0);
params.setDegeneracyMode(IGNORE_DEGENERACY); params.setDegeneracyMode(ZERO_ON_DEGENERACY);
params.setEnableEPI(false); params.setEnableEPI(false);
SmartRigFactor smartFactor1(model, cameraRig, params); SmartRigFactor smartFactor1(model, cameraRig, params);
@ -304,7 +325,7 @@ TEST(SmartProjectionRigFactor, smartFactorWithSensorBodyTransform) {
/* *************************************************************************/ /* *************************************************************************/
TEST(SmartProjectionRigFactor, smartFactorWithMultipleCameras) { TEST(SmartProjectionRigFactor, smartFactorWithMultipleCameras) {
using namespace vanillaPose; using namespace vanillaRig;
// create arbitrary body_T_sensor (transforms from sensor to body) // create arbitrary body_T_sensor (transforms from sensor to body)
Pose3 body_T_sensor1 = Pose3 body_T_sensor1 =
@ -342,7 +363,7 @@ TEST(SmartProjectionRigFactor, smartFactorWithMultipleCameras) {
SmartProjectionParams params; SmartProjectionParams params;
params.setRankTolerance(1.0); params.setRankTolerance(1.0);
params.setDegeneracyMode(IGNORE_DEGENERACY); params.setDegeneracyMode(ZERO_ON_DEGENERACY);
params.setEnableEPI(false); params.setEnableEPI(false);
SmartRigFactor smartFactor1(model, cameraRig, params); SmartRigFactor smartFactor1(model, cameraRig, params);
@ -406,13 +427,20 @@ TEST(SmartProjectionRigFactor, 3poses_smart_projection_factor) {
FastVector<size_t> cameraIds{ FastVector<size_t> cameraIds{
0, 0, 0}; // 3 measurements from the same camera in the rig 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); 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); 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); smartFactor3->add(measurements_cam3, views, cameraIds);
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
@ -454,7 +482,7 @@ TEST(SmartProjectionRigFactor, 3poses_smart_projection_factor) {
/* *************************************************************************/ /* *************************************************************************/
TEST(SmartProjectionRigFactor, Factors) { TEST(SmartProjectionRigFactor, Factors) {
using namespace vanillaPose; using namespace vanillaRig;
// Default cameras for simple derivatives // Default cameras for simple derivatives
Rot3 R; Rot3 R;
@ -476,7 +504,7 @@ TEST(SmartProjectionRigFactor, Factors) {
FastVector<size_t> cameraIds{0, 0}; FastVector<size_t> cameraIds{0, 0};
SmartRigFactor::shared_ptr smartFactor1 = boost::make_shared<SmartRigFactor>( SmartRigFactor::shared_ptr smartFactor1 = boost::make_shared<SmartRigFactor>(
model, Camera(Pose3::identity(), sharedK)); model, Camera(Pose3::identity(), sharedK), params);
smartFactor1->add(measurements_cam1, smartFactor1->add(measurements_cam1,
views); // we have a single camera so use default cameraIds views); // we have a single camera so use default cameraIds
@ -543,7 +571,7 @@ TEST(SmartProjectionRigFactor, Factors) {
/* *************************************************************************/ /* *************************************************************************/
TEST(SmartProjectionRigFactor, 3poses_iterative_smart_projection_factor) { TEST(SmartProjectionRigFactor, 3poses_iterative_smart_projection_factor) {
using namespace vanillaPose; using namespace vanillaRig;
KeyVector views{x1, x2, x3}; KeyVector views{x1, x2, x3};
@ -563,13 +591,16 @@ TEST(SmartProjectionRigFactor, 3poses_iterative_smart_projection_factor) {
Cameras cameraRig; // single camera in the rig Cameras cameraRig; // single camera in the rig
cameraRig.push_back(Camera(Pose3::identity(), sharedK)); cameraRig.push_back(Camera(Pose3::identity(), sharedK));
FastVector<size_t> cameraIds{0, 0, 0}; 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); 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); 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); smartFactor3->add(measurements_cam3, views, cameraIds);
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
@ -605,7 +636,7 @@ TEST(SmartProjectionRigFactor, 3poses_iterative_smart_projection_factor) {
/* *************************************************************************/ /* *************************************************************************/
TEST(SmartProjectionRigFactor, landmarkDistance) { TEST(SmartProjectionRigFactor, landmarkDistance) {
using namespace vanillaPose; using namespace vanillaRig;
double excludeLandmarksFutherThanDist = 2; double excludeLandmarksFutherThanDist = 2;
@ -620,8 +651,8 @@ TEST(SmartProjectionRigFactor, landmarkDistance) {
SmartProjectionParams params; SmartProjectionParams params;
params.setRankTolerance(1.0); params.setRankTolerance(1.0);
params.setLinearizationMode(gtsam::JACOBIAN_SVD); params.setLinearizationMode(gtsam::HESSIAN);
params.setDegeneracyMode(gtsam::IGNORE_DEGENERACY); params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY);
params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist); params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist);
params.setEnableEPI(false); params.setEnableEPI(false);
@ -668,7 +699,7 @@ TEST(SmartProjectionRigFactor, landmarkDistance) {
/* *************************************************************************/ /* *************************************************************************/
TEST(SmartProjectionRigFactor, dynamicOutlierRejection) { TEST(SmartProjectionRigFactor, dynamicOutlierRejection) {
using namespace vanillaPose; using namespace vanillaRig;
double excludeLandmarksFutherThanDist = 1e10; double excludeLandmarksFutherThanDist = 1e10;
double dynamicOutlierRejectionThreshold = double dynamicOutlierRejectionThreshold =
@ -742,7 +773,7 @@ TEST(SmartProjectionRigFactor, dynamicOutlierRejection) {
TEST(SmartProjectionRigFactor, CheckHessian) { TEST(SmartProjectionRigFactor, CheckHessian) {
KeyVector views{x1, x2, x3}; KeyVector views{x1, x2, x3};
using namespace vanillaPose; using namespace vanillaRig;
// Two slightly different cameras // Two slightly different cameras
Pose3 pose2 = Pose3 pose2 =
@ -842,7 +873,12 @@ TEST(SmartProjectionRigFactor, Hessian) {
cameraRig.push_back(Camera(Pose3::identity(), sharedK2)); cameraRig.push_back(Camera(Pose3::identity(), sharedK2));
FastVector<size_t> cameraIds{0, 0}; 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); smartFactor1->add(measurements_cam1, views, cameraIds);
Pose3 noise_pose = Pose3 noise_pose =
@ -875,6 +911,9 @@ TEST(SmartProjectionRigFactor, ConstructorWithCal3Bundler) {
/* *************************************************************************/ /* *************************************************************************/
TEST(SmartProjectionRigFactor, Cal3Bundler) { TEST(SmartProjectionRigFactor, Cal3Bundler) {
using namespace bundlerPose; 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 // three landmarks ~5 meters in front of camera
Point3 landmark3(3, 0, 3.0); Point3 landmark3(3, 0, 3.0);
@ -892,13 +931,16 @@ TEST(SmartProjectionRigFactor, Cal3Bundler) {
cameraRig.push_back(Camera(Pose3::identity(), sharedBundlerK)); cameraRig.push_back(Camera(Pose3::identity(), sharedBundlerK));
FastVector<size_t> cameraIds{0, 0, 0}; 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); 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); 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); smartFactor3->add(measurements_cam3, views, cameraIds);
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); 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 // measurements of the same landmark at a single pose, a setup that occurs in
// multi-camera systems // multi-camera systems
using namespace vanillaPose; using namespace vanillaRig;
Point2Vector measurements_lmk1; Point2Vector measurements_lmk1;
// Project three landmarks into three cameras // Project three landmarks into three cameras
@ -960,7 +1002,8 @@ TEST(SmartProjectionRigFactor,
cameraRig.push_back(Camera(Pose3::identity(), sharedK)); cameraRig.push_back(Camera(Pose3::identity(), sharedK));
FastVector<size_t> cameraIds{0, 0, 0, 0}; 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); smartFactor1->add(measurements_lmk1_redundant, keys, cameraIds);
Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
@ -1073,7 +1116,7 @@ TEST(SmartProjectionRigFactor,
/* *************************************************************************/ /* *************************************************************************/
TEST(SmartProjectionRigFactor, optimization_3poses_measurementsFromSamePose) { TEST(SmartProjectionRigFactor, optimization_3poses_measurementsFromSamePose) {
using namespace vanillaPose; using namespace vanillaRig;
Point2Vector measurements_lmk1, measurements_lmk2, measurements_lmk3; Point2Vector measurements_lmk1, measurements_lmk2, measurements_lmk3;
// Project three landmarks into three cameras // Project three landmarks into three cameras
@ -1097,14 +1140,17 @@ TEST(SmartProjectionRigFactor, optimization_3poses_measurementsFromSamePose) {
KeyVector keys_redundant = keys; KeyVector keys_redundant = keys;
keys_redundant.push_back(keys.at(0)); // we readd the first key 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, smartFactor1->add(measurements_lmk1_redundant, keys_redundant,
cameraIdsRedundant); cameraIdsRedundant);
SmartRigFactor::shared_ptr smartFactor2(new SmartRigFactor(model, cameraRig)); SmartRigFactor::shared_ptr smartFactor2(
new SmartRigFactor(model, cameraRig, params));
smartFactor2->add(measurements_lmk2, keys, cameraIds); 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); smartFactor3->add(measurements_lmk3, keys, cameraIds);
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); 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 // this factor is slightly slower (but comparable) to original
// SmartProjectionPoseFactor // SmartProjectionPoseFactor
//-Total: 0 CPU (0 times, 0 wall, 0.17 children, min: 0 max: 0) //-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 //| -SmartRigFactor LINEARIZE: 0.06 CPU
// children, min: 0 max: 0) | -SmartPoseFactor LINEARIZE: 0.06 CPU (10000 //(10000 times, 0.061226 wall, 0.06 children, min: 0 max: 0)
// times, 0.065103 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) { TEST(SmartProjectionRigFactor, timing) {
using namespace vanillaPose; using namespace vanillaRig;
// Default cameras for simple derivatives // Default cameras for simple derivatives
static Cal3_S2::shared_ptr sharedKSimple(new Cal3_S2(100, 100, 0, 0, 0)); 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++) { for (size_t i = 0; i < nrTests; i++) {
SmartRigFactor::shared_ptr smartFactorP( SmartRigFactor::shared_ptr smartFactorP(
new SmartRigFactor(model, cameraRig)); new SmartRigFactor(model, cameraRig, params));
smartFactorP->add(measurements_lmk1[0], x1, cameraId1); smartFactorP->add(measurements_lmk1[0], x1, cameraId1);
smartFactorP->add(measurements_lmk1[1], x1, cameraId1); smartFactorP->add(measurements_lmk1[1], x1, cameraId1);
@ -1195,7 +1242,8 @@ TEST(SmartProjectionRigFactor, timing) {
} }
for (size_t i = 0; i < nrTests; i++) { 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[0], x1);
smartFactor->add(measurements_lmk1[1], x2); smartFactor->add(measurements_lmk1[1], x2);
@ -1225,7 +1273,7 @@ TEST(SmartProjectionRigFactor, timing) {
// BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal"); // BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal");
// //
// TEST(SmartProjectionRigFactor, serialize) { // TEST(SmartProjectionRigFactor, serialize) {
// using namespace vanillaPose; // using namespace vanillaRig;
// using namespace gtsam::serializationTestHelpers; // using namespace gtsam::serializationTestHelpers;
// SmartProjectionParams params; // SmartProjectionParams params;
// params.setRankTolerance(rankTol); // params.setRankTolerance(rankTol);
@ -1242,7 +1290,7 @@ TEST(SmartProjectionRigFactor, timing) {
// //
//// SERIALIZATION TEST FAILS: to be fixed //// SERIALIZATION TEST FAILS: to be fixed
////TEST(SmartProjectionRigFactor, serialize2) { ////TEST(SmartProjectionRigFactor, serialize2) {
//// using namespace vanillaPose; //// using namespace vanillaRig;
//// using namespace gtsam::serializationTestHelpers; //// using namespace gtsam::serializationTestHelpers;
//// SmartProjectionParams params; //// SmartProjectionParams params;
//// params.setRankTolerance(rankTol); //// params.setRankTolerance(rankTol);

24
gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h
View File

@ -95,9 +95,15 @@ class SmartProjectionPoseFactorRollingShutter
const SharedNoiseModel& sharedNoiseModel, const Cameras& cameraRig, const SharedNoiseModel& sharedNoiseModel, const Cameras& cameraRig,
const SmartProjectionParams& params = SmartProjectionParams()) const SmartProjectionParams& params = SmartProjectionParams())
: Base(sharedNoiseModel, params), cameraRig_(cameraRig) { : Base(sharedNoiseModel, params), cameraRig_(cameraRig) {
// use only configuration that works with this factor // throw exception if configuration is not supported by this factor
Base::params_.degeneracyMode = gtsam::ZERO_ON_DEGENERACY; if (Base::params_.degeneracyMode != gtsam::ZERO_ON_DEGENERACY)
Base::params_.linearizationMode = gtsam::HESSIAN; 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 SharedNoiseModel& sharedNoiseModel, const Camera& camera,
const SmartProjectionParams& params = SmartProjectionParams()) const SmartProjectionParams& params = SmartProjectionParams())
: Base(sharedNoiseModel, params) { : Base(sharedNoiseModel, params) {
// use only configuration that works with this factor // throw exception if configuration is not supported by this factor
Base::params_.degeneracyMode = gtsam::ZERO_ON_DEGENERACY; if (Base::params_.degeneracyMode != gtsam::ZERO_ON_DEGENERACY)
Base::params_.linearizationMode = gtsam::HESSIAN; 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); cameraRig_.push_back(camera);
} }

99
gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp
View File

@ -75,6 +75,9 @@ Pose3 interp_pose3 = interpolate<Pose3>(pose_above, level_pose, interp_factor3);
Camera cam1(interp_pose1, sharedK); Camera cam1(interp_pose1, sharedK);
Camera cam2(interp_pose2, sharedK); Camera cam2(interp_pose2, sharedK);
Camera cam3(interp_pose3, sharedK); Camera cam3(interp_pose3, sharedK);
SmartProjectionParams params(
gtsam::HESSIAN,
gtsam::ZERO_ON_DEGENERACY); // only config that works with RS factors
} // namespace vanillaPoseRS } // namespace vanillaPoseRS
LevenbergMarquardtParams lmParams; LevenbergMarquardtParams lmParams;
@ -85,13 +88,12 @@ typedef SmartProjectionPoseFactorRollingShutter<PinholePose<Cal3_S2>>
TEST(SmartProjectionPoseFactorRollingShutter, Constructor) { TEST(SmartProjectionPoseFactorRollingShutter, Constructor) {
using namespace vanillaPoseRS; using namespace vanillaPoseRS;
SmartFactorRS::shared_ptr factor1( SmartFactorRS::shared_ptr factor1(
new SmartFactorRS(model, Camera(Pose3::identity(), sharedK))); new SmartFactorRS(model, Camera(Pose3::identity(), sharedK), params));
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST(SmartProjectionPoseFactorRollingShutter, Constructor2) { TEST(SmartProjectionPoseFactorRollingShutter, Constructor2) {
using namespace vanillaPoseRS; using namespace vanillaPoseRS;
SmartProjectionParams params;
params.setRankTolerance(rankTol); params.setRankTolerance(rankTol);
SmartFactorRS factor1(model, Camera(Pose3::identity(), sharedK), params); SmartFactorRS factor1(model, Camera(Pose3::identity(), sharedK), params);
} }
@ -100,13 +102,13 @@ TEST(SmartProjectionPoseFactorRollingShutter, Constructor2) {
TEST(SmartProjectionPoseFactorRollingShutter, add) { TEST(SmartProjectionPoseFactorRollingShutter, add) {
using namespace vanillaPoseRS; using namespace vanillaPoseRS;
SmartFactorRS::shared_ptr factor1( 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); factor1->add(measurement1, x1, x2, interp_factor);
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST(SmartProjectionPoseFactorRollingShutter, Equals) { TEST(SmartProjectionPoseFactorRollingShutter, Equals) {
using namespace vanillaPose; using namespace vanillaPoseRS;
// create fake measurements // create fake measurements
Point2Vector measurements; Point2Vector measurements;
@ -130,15 +132,18 @@ TEST(SmartProjectionPoseFactorRollingShutter, Equals) {
cameraRig.push_back(Camera(body_P_sensor, sharedK)); cameraRig.push_back(Camera(body_P_sensor, sharedK));
// create by adding a batch of measurements with a bunch of calibrations // 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); factor2->add(measurements, key_pairs, interp_factors, cameraIds);
// create by adding a batch of measurements with a single calibrations // 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); factor3->add(measurements, key_pairs, interp_factors, cameraIds);
{ // create equal factors and show equal returns true { // 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(measurement1, x1, x2, interp_factor1, cameraId1);
factor1->add(measurement2, x2, x3, interp_factor2, cameraId1); factor1->add(measurement2, x2, x3, interp_factor2, cameraId1);
factor1->add(measurement3, x3, x4, interp_factor3, cameraId1); factor1->add(measurement3, x3, x4, interp_factor3, cameraId1);
@ -147,7 +152,8 @@ TEST(SmartProjectionPoseFactorRollingShutter, Equals) {
EXPECT(factor1->equals(*factor3)); EXPECT(factor1->equals(*factor3));
} }
{ // create equal factors and show equal returns true (use default cameraId) { // 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(measurement1, x1, x2, interp_factor1);
factor1->add(measurement2, x2, x3, interp_factor2); factor1->add(measurement2, x2, x3, interp_factor2);
factor1->add(measurement3, x3, x4, interp_factor3); factor1->add(measurement3, x3, x4, interp_factor3);
@ -156,7 +162,8 @@ TEST(SmartProjectionPoseFactorRollingShutter, Equals) {
EXPECT(factor1->equals(*factor3)); EXPECT(factor1->equals(*factor3));
} }
{ // create equal factors and show equal returns true (use default cameraId) { // 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); factor1->add(measurements, key_pairs, interp_factors);
EXPECT(factor1->equals(*factor2)); EXPECT(factor1->equals(*factor2));
@ -164,7 +171,8 @@ TEST(SmartProjectionPoseFactorRollingShutter, Equals) {
} }
{ // create slightly different factors (different keys) and show equal { // create slightly different factors (different keys) and show equal
// returns false (use default cameraIds) // 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(measurement1, x1, x2, interp_factor1, cameraId1);
factor1->add(measurement2, x2, x2, interp_factor2, factor1->add(measurement2, x2, x2, interp_factor2,
cameraId1); // different! cameraId1); // different!
@ -177,7 +185,8 @@ TEST(SmartProjectionPoseFactorRollingShutter, Equals) {
// returns false // returns false
Cameras cameraRig2; Cameras cameraRig2;
cameraRig2.push_back(Camera(body_P_sensor * body_P_sensor, sharedK)); 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(measurement1, x1, x2, interp_factor1, cameraId1);
factor1->add(measurement2, x2, x3, interp_factor2, factor1->add(measurement2, x2, x3, interp_factor2,
cameraId1); // different! cameraId1); // different!
@ -188,7 +197,8 @@ TEST(SmartProjectionPoseFactorRollingShutter, Equals) {
} }
{ // create slightly different factors (different interp factors) and show { // create slightly different factors (different interp factors) and show
// equal returns false // 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(measurement1, x1, x2, interp_factor1, cameraId1);
factor1->add(measurement2, x2, x3, interp_factor1, factor1->add(measurement2, x2, x3, interp_factor1,
cameraId1); // different! cameraId1); // different!
@ -216,7 +226,7 @@ TEST(SmartProjectionPoseFactorRollingShutter, noiselessErrorAndJacobians) {
Point2 level_uv_right = cam2.project(landmark1); Point2 level_uv_right = cam2.project(landmark1);
Pose3 body_P_sensorId = Pose3::identity(); 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, x1, x2, interp_factor1);
factor.add(level_uv_right, x2, x3, interp_factor2); factor.add(level_uv_right, x2, x3, interp_factor2);
@ -291,7 +301,7 @@ TEST(SmartProjectionPoseFactorRollingShutter, noisyErrorAndJacobians) {
Point2 level_uv_right = cam2.project(landmark1); Point2 level_uv_right = cam2.project(landmark1);
Pose3 body_P_sensorNonId = body_P_sensor; 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, x1, x2, interp_factor1);
factor.add(level_uv_right, x2, x3, interp_factor2); factor.add(level_uv_right, x2, x3, interp_factor2);
@ -383,15 +393,15 @@ TEST(SmartProjectionPoseFactorRollingShutter, optimization_3poses) {
interp_factors.push_back(interp_factor3); interp_factors.push_back(interp_factor3);
SmartFactorRS::shared_ptr smartFactor1( 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); smartFactor1->add(measurements_lmk1, key_pairs, interp_factors);
SmartFactorRS::shared_ptr smartFactor2( 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); smartFactor2->add(measurements_lmk2, key_pairs, interp_factors);
SmartFactorRS::shared_ptr smartFactor3( 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); smartFactor3->add(measurements_lmk3, key_pairs, interp_factors);
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); 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(body_P_sensor, sharedK));
cameraRig.push_back(Camera(Pose3::identity(), 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}); 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}); 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}); smartFactor3->add(measurements_lmk3, key_pairs, interp_factors, {1, 1, 1});
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); 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_sensor2, sharedK));
cameraRig.push_back(Camera(body_T_sensor3, 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}); 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}); 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}); smartFactor3->add(measurements_lmk3, key_pairs, interp_factors, {0, 1, 2});
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); 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 // 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 // test since in typical camera you would not have more than 1 measurement per
// landmark at each interpolated pose // landmark at each interpolated pose
using namespace vanillaPose; using namespace vanillaPoseRS;
// Default cameras for simple derivatives // Default cameras for simple derivatives
static Cal3_S2::shared_ptr sharedKSimple(new Cal3_S2(100, 100, 0, 0, 0)); 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)); measurements_lmk1.push_back(cam2.project(landmark1));
SmartFactorRS::shared_ptr smartFactor1( 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 double interp_factor = 0; // equivalent to measurement taken at pose 1
smartFactor1->add(measurements_lmk1[0], x1, x2, interp_factor); smartFactor1->add(measurements_lmk1[0], x1, x2, interp_factor);
interp_factor = 1; // equivalent to measurement taken at pose 2 interp_factor = 1; // equivalent to measurement taken at pose 2
smartFactor1->add(measurements_lmk1[1], x1, x2, interp_factor); smartFactor1->add(measurements_lmk1[1], x1, x2, interp_factor);
SmartFactor::Cameras cameras; SmartFactorRS::Cameras cameras;
cameras.push_back(cam1); cameras.push_back(cam1);
cameras.push_back(cam2); cameras.push_back(cam2);
@ -772,7 +788,9 @@ TEST(SmartProjectionPoseFactorRollingShutter,
SmartProjectionParams params; SmartProjectionParams params;
params.setRankTolerance(1.0); params.setRankTolerance(1.0);
params.setLinearizationMode(gtsam::HESSIAN); 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.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist);
params.setEnableEPI(false); params.setEnableEPI(false);
@ -916,7 +934,7 @@ TEST(SmartProjectionPoseFactorRollingShutter,
interp_factors.push_back(interp_factor3); interp_factors.push_back(interp_factor3);
SmartFactorRS::shared_ptr smartFactor1( 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); smartFactor1->add(measurements_lmk1, key_pairs, interp_factors);
Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), 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); interp_factors.push_back(interp_factor1);
SmartFactorRS::shared_ptr smartFactor1( 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); smartFactor1->add(measurements_lmk1_redundant, key_pairs, interp_factors);
Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), 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 interp_factors.at(0)); // we readd the first interp factor
SmartFactorRS::shared_ptr smartFactor1( 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, smartFactor1->add(measurements_lmk1_redundant, key_pairs_redundant,
interp_factors_redundant); interp_factors_redundant);
SmartFactorRS::shared_ptr smartFactor2( 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); smartFactor2->add(measurements_lmk2, key_pairs, interp_factors);
SmartFactorRS::shared_ptr smartFactor3( 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); smartFactor3->add(measurements_lmk3, key_pairs, interp_factors);
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
@ -1263,15 +1281,19 @@ TEST(SmartProjectionPoseFactorRollingShutter,
#ifndef DISABLE_TIMING #ifndef DISABLE_TIMING
#include <gtsam/base/timing.h> #include <gtsam/base/timing.h>
//-Total: 0 CPU (0 times, 0 wall, 0.21 children, min: 0 max: 0) //-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, //| -SF RS LINEARIZE: 0.09 CPU
// min: 0 max: 0) | -RS LINEARIZE: 0.06 CPU (10000 times, 0.06311 wall, 0.06 // (10000 times, 0.124106 wall, 0.09 children, min: 0 max: 0)
// 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) { TEST(SmartProjectionPoseFactorRollingShutter, timing) {
using namespace vanillaPose; using namespace vanillaPose;
// Default cameras for simple derivatives // Default cameras for simple derivatives
static Cal3_S2::shared_ptr sharedKSimple(new Cal3_S2(100, 100, 0, 0, 0)); 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(); Rot3 R = Rot3::identity();
Pose3 pose1 = Pose3(R, Point3(0, 0, 0)); Pose3 pose1 = Pose3(R, Point3(0, 0, 0));
@ -1291,8 +1313,8 @@ TEST(SmartProjectionPoseFactorRollingShutter, timing) {
size_t nrTests = 10000; size_t nrTests = 10000;
for (size_t i = 0; i < nrTests; i++) { for (size_t i = 0; i < nrTests; i++) {
SmartFactorRS::shared_ptr smartFactorRS( SmartFactorRS::shared_ptr smartFactorRS(new SmartFactorRS(
new SmartFactorRS(model, Camera(body_P_sensorId, sharedKSimple))); model, Camera(body_P_sensorId, sharedKSimple), params));
double interp_factor = 0; // equivalent to measurement taken at pose 1 double interp_factor = 0; // equivalent to measurement taken at pose 1
smartFactorRS->add(measurements_lmk1[0], x1, x2, interp_factor); smartFactorRS->add(measurements_lmk1[0], x1, x2, interp_factor);
interp_factor = 1; // equivalent to measurement taken at pose 2 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++) { 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[0], x1);
smartFactor->add(measurements_lmk1[1], x2); smartFactor->add(measurements_lmk1[1], x2);