now throwing exception is params are incorrect
lcarlone
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