From 29ad9478f7543a875c2dcd5af1f81843d8c6a483 Mon Sep 17 00:00:00 2001 From: cbeall3 Date: Wed, 26 Aug 2015 13:02:39 -0400 Subject: [PATCH 1/2] Move noise model initialization from add function into constructor. Can only have one noise model per factor anyway --- gtsam/slam/SmartFactorBase.h | 59 ++--- gtsam/slam/SmartProjectionFactor.h | 4 +- gtsam/slam/SmartProjectionPoseFactor.h | 6 +- gtsam/slam/tests/testSmartFactorBase.cpp | 25 ++- .../tests/testSmartProjectionCameraFactor.cpp | 160 +++++++------- .../tests/testSmartProjectionPoseFactor.cpp | 206 +++++++++--------- .../SmartStereoProjectionFactorExample.cpp | 6 +- .../slam/SmartStereoProjectionFactor.h | 4 +- .../slam/SmartStereoProjectionPoseFactor.h | 33 ++- .../testSmartStereoProjectionPoseFactor.cpp | 116 +++++----- 10 files changed, 298 insertions(+), 321 deletions(-) diff --git a/gtsam/slam/SmartFactorBase.h b/gtsam/slam/SmartFactorBase.h index f4ef51d9a..3f043a469 100644 --- a/gtsam/slam/SmartFactorBase.h +++ b/gtsam/slam/SmartFactorBase.h @@ -15,6 +15,7 @@ * @author Luca Carlone * @author Zsolt Kira * @author Frank Dellaert + * @author Chris Beall */ #pragma once @@ -79,22 +80,6 @@ protected: typedef Eigen::Matrix VectorD; typedef Eigen::Matrix Matrix2; - // check that noise model is isotropic and the same - // TODO, this is hacky, we should just do this via constructor, not add - void maybeSetNoiseModel(const SharedNoiseModel& sharedNoiseModel) { - if (!sharedNoiseModel) - return; - SharedIsotropic sharedIsotropic = boost::dynamic_pointer_cast< - noiseModel::Isotropic>(sharedNoiseModel); - if (!sharedIsotropic) - throw std::runtime_error("SmartFactorBase: needs isotropic"); - if (!noiseModel_) - noiseModel_ = sharedIsotropic; - else if (!sharedIsotropic->equals(*noiseModel_)) - throw std::runtime_error( - "SmartFactorBase: cannot add measurements with different noise model"); - } - public: // Definitions for blocks of F, externally visible @@ -109,8 +94,21 @@ public: typedef CameraSet Cameras; /// Constructor - SmartFactorBase(boost::optional body_P_sensor = boost::none) : - body_P_sensor_(body_P_sensor){} + SmartFactorBase(const SharedNoiseModel& sharedNoiseModel, + boost::optional body_P_sensor = boost::none) : + body_P_sensor_(body_P_sensor){ + + if (!sharedNoiseModel) + throw std::runtime_error("SmartFactorBase: sharedNoiseModel is required"); + + SharedIsotropic sharedIsotropic = boost::dynamic_pointer_cast< + noiseModel::Isotropic>(sharedNoiseModel); + + if (!sharedIsotropic) + throw std::runtime_error("SmartFactorBase: needs isotropic"); + + noiseModel_ = sharedIsotropic; + } /// Virtual destructor, subclasses from NonlinearFactor virtual ~SmartFactorBase() { @@ -122,34 +120,18 @@ public: * @param poseKey is the index corresponding to the camera observing the landmark * @param sharedNoiseModel is the measurement noise */ - void add(const Z& measured_i, const Key& cameraKey_i, - const SharedNoiseModel& sharedNoiseModel) { + void add(const Z& measured_i, const Key& cameraKey_i) { this->measured_.push_back(measured_i); this->keys_.push_back(cameraKey_i); - maybeSetNoiseModel(sharedNoiseModel); } /** - * Add a bunch of measurements, together with the camera keys and noises + * Add a bunch of measurements, together with the camera keys */ - void add(std::vector& measurements, std::vector& cameraKeys, - std::vector& noises) { + void add(std::vector& measurements, std::vector& cameraKeys) { for (size_t i = 0; i < measurements.size(); i++) { this->measured_.push_back(measurements.at(i)); this->keys_.push_back(cameraKeys.at(i)); - maybeSetNoiseModel(noises.at(i)); - } - } - - /** - * Add a bunch of measurements and use the same noise model for all of them - */ - void add(std::vector& measurements, std::vector& cameraKeys, - const SharedNoiseModel& noise) { - for (size_t i = 0; i < measurements.size(); i++) { - this->measured_.push_back(measurements.at(i)); - this->keys_.push_back(cameraKeys.at(i)); - maybeSetNoiseModel(noise); } } @@ -158,11 +140,10 @@ public: * The noise is assumed to be the same for all measurements */ template - void add(const SFM_TRACK& trackToAdd, const SharedNoiseModel& noise) { + void add(const SFM_TRACK& trackToAdd) { for (size_t k = 0; k < trackToAdd.number_measurements(); k++) { this->measured_.push_back(trackToAdd.measurements[k].second); this->keys_.push_back(trackToAdd.measurements[k].first); - maybeSetNoiseModel(noise); } } diff --git a/gtsam/slam/SmartProjectionFactor.h b/gtsam/slam/SmartProjectionFactor.h index 120b0bbce..5146c5a32 100644 --- a/gtsam/slam/SmartProjectionFactor.h +++ b/gtsam/slam/SmartProjectionFactor.h @@ -159,10 +159,10 @@ public: * @param body_P_sensor pose of the camera in the body frame * @param params internal parameters of the smart factors */ - SmartProjectionFactor( + SmartProjectionFactor(const SharedNoiseModel& sharedNoiseModel, const boost::optional body_P_sensor = boost::none, const SmartProjectionParams& params = SmartProjectionParams()) : - Base(body_P_sensor), params_(params), // + Base(sharedNoiseModel, body_P_sensor), params_(params), // result_(TriangulationResult::Degenerate()) { } diff --git a/gtsam/slam/SmartProjectionPoseFactor.h b/gtsam/slam/SmartProjectionPoseFactor.h index 834c9c9b6..c091ff79d 100644 --- a/gtsam/slam/SmartProjectionPoseFactor.h +++ b/gtsam/slam/SmartProjectionPoseFactor.h @@ -61,14 +61,16 @@ public: /** * Constructor + * @param Isotropic measurement noise * @param K (fixed) calibration, assumed to be the same for all cameras * @param body_P_sensor pose of the camera in the body frame * @param params internal parameters of the smart factors */ - SmartProjectionPoseFactor(const boost::shared_ptr K, + SmartProjectionPoseFactor(const SharedNoiseModel& sharedNoiseModel, + const boost::shared_ptr K, const boost::optional body_P_sensor = boost::none, const SmartProjectionParams& params = SmartProjectionParams()) : - Base(body_P_sensor, params), K_(K) { + Base(sharedNoiseModel, body_P_sensor, params), K_(K) { } /** Virtual destructor */ diff --git a/gtsam/slam/tests/testSmartFactorBase.cpp b/gtsam/slam/tests/testSmartFactorBase.cpp index 373d482fe..bf5969d4d 100644 --- a/gtsam/slam/tests/testSmartFactorBase.cpp +++ b/gtsam/slam/tests/testSmartFactorBase.cpp @@ -16,17 +16,24 @@ * @date Feb 2015 */ -#include "../SmartFactorBase.h" +#include #include #include using namespace std; using namespace gtsam; +static SharedNoiseModel unit2(noiseModel::Unit::Create(2)); +static SharedNoiseModel unit3(noiseModel::Unit::Create(3)); + /* ************************************************************************* */ #include #include class PinholeFactor: public SmartFactorBase > { +public: + typedef SmartFactorBase > Base; + PinholeFactor(const SharedNoiseModel& sharedNoiseModel): Base(sharedNoiseModel) { + } virtual double error(const Values& values) const { return 0.0; } @@ -37,15 +44,19 @@ class PinholeFactor: public SmartFactorBase > { }; TEST(SmartFactorBase, Pinhole) { - PinholeFactor f; - f.add(Point2(), 1, SharedNoiseModel()); - f.add(Point2(), 2, SharedNoiseModel()); + PinholeFactor f= PinholeFactor(unit2); + f.add(Point2(), 1); + f.add(Point2(), 2); EXPECT_LONGS_EQUAL(2 * 2, f.dim()); } /* ************************************************************************* */ #include class StereoFactor: public SmartFactorBase { +public: + typedef SmartFactorBase Base; + StereoFactor(const SharedNoiseModel& sharedNoiseModel): Base(sharedNoiseModel) { + } virtual double error(const Values& values) const { return 0.0; } @@ -56,9 +67,9 @@ class StereoFactor: public SmartFactorBase { }; TEST(SmartFactorBase, Stereo) { - StereoFactor f; - f.add(StereoPoint2(), 1, SharedNoiseModel()); - f.add(StereoPoint2(), 2, SharedNoiseModel()); + StereoFactor f(unit3); + f.add(StereoPoint2(), 1); + f.add(StereoPoint2(), 2); EXPECT_LONGS_EQUAL(2 * 3, f.dim()); } diff --git a/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp b/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp index 9838d65e5..d4f60b085 100644 --- a/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp @@ -72,39 +72,39 @@ TEST( SmartProjectionCameraFactor, perturbCameraPose) { /* ************************************************************************* */ TEST( SmartProjectionCameraFactor, Constructor) { using namespace vanilla; - SmartFactor::shared_ptr factor1(new SmartFactor()); + SmartFactor::shared_ptr factor1(new SmartFactor(unit2)); } /* ************************************************************************* */ TEST( SmartProjectionCameraFactor, Constructor2) { using namespace vanilla; params.setRankTolerance(rankTol); - SmartFactor factor1(boost::none, params); + SmartFactor factor1(unit2, boost::none, params); } /* ************************************************************************* */ TEST( SmartProjectionCameraFactor, Constructor3) { using namespace vanilla; - SmartFactor::shared_ptr factor1(new SmartFactor()); - factor1->add(measurement1, x1, unit2); + SmartFactor::shared_ptr factor1(new SmartFactor(unit2)); + factor1->add(measurement1, x1); } /* ************************************************************************* */ TEST( SmartProjectionCameraFactor, Constructor4) { using namespace vanilla; params.setRankTolerance(rankTol); - SmartFactor factor1(boost::none, params); - factor1.add(measurement1, x1, unit2); + SmartFactor factor1(unit2, boost::none, params); + factor1.add(measurement1, x1); } /* ************************************************************************* */ TEST( SmartProjectionCameraFactor, Equals ) { using namespace vanilla; - SmartFactor::shared_ptr factor1(new SmartFactor()); - factor1->add(measurement1, x1, unit2); + SmartFactor::shared_ptr factor1(new SmartFactor(unit2)); + factor1->add(measurement1, x1); - SmartFactor::shared_ptr factor2(new SmartFactor()); - factor2->add(measurement1, x1, unit2); + SmartFactor::shared_ptr factor2(new SmartFactor(unit2)); + factor2->add(measurement1, x1); } /* *************************************************************************/ @@ -115,9 +115,9 @@ TEST( SmartProjectionCameraFactor, noiseless ) { values.insert(c1, level_camera); values.insert(c2, level_camera_right); - SmartFactor::shared_ptr factor1(new SmartFactor()); - factor1->add(level_uv, c1, unit2); - factor1->add(level_uv_right, c2, unit2); + SmartFactor::shared_ptr factor1(new SmartFactor(unit2)); + factor1->add(level_uv, c1); + factor1->add(level_uv_right, c2); double expectedError = 0.0; DOUBLES_EQUAL(expectedError, factor1->error(values), 1e-7); @@ -140,9 +140,9 @@ TEST( SmartProjectionCameraFactor, noisy ) { Camera perturbed_level_camera_right = perturbCameraPose(level_camera_right); values.insert(c2, perturbed_level_camera_right); - SmartFactor::shared_ptr factor1(new SmartFactor()); - factor1->add(level_uv, c1, unit2); - factor1->add(level_uv_right, c2, unit2); + SmartFactor::shared_ptr factor1(new SmartFactor(unit2)); + factor1->add(level_uv, c1); + factor1->add(level_uv_right, c2); // Point is now uninitialized before a triangulation event EXPECT(!factor1->point()); @@ -164,20 +164,16 @@ TEST( SmartProjectionCameraFactor, noisy ) { Vector actual = factor1->whitenedError(cameras1, point1); EXPECT(assert_equal(expected, actual, 1)); - SmartFactor::shared_ptr factor2(new SmartFactor()); + SmartFactor::shared_ptr factor2(new SmartFactor(unit2)); vector measurements; measurements.push_back(level_uv); measurements.push_back(level_uv_right); - vector noises; - noises.push_back(unit2); - noises.push_back(unit2); - vector views; views.push_back(c1); views.push_back(c2); - factor2->add(measurements, views, noises); + factor2->add(measurements, views); double actualError2 = factor2->error(values); EXPECT_DOUBLES_EQUAL(expectedError, actualError2, 1); @@ -195,16 +191,16 @@ TEST( SmartProjectionCameraFactor, perturbPoseAndOptimize ) { projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); // Create and fill smartfactors - SmartFactor::shared_ptr smartFactor1(new SmartFactor()); - SmartFactor::shared_ptr smartFactor2(new SmartFactor()); - SmartFactor::shared_ptr smartFactor3(new SmartFactor()); + SmartFactor::shared_ptr smartFactor1(new SmartFactor(unit2)); + SmartFactor::shared_ptr smartFactor2(new SmartFactor(unit2)); + SmartFactor::shared_ptr smartFactor3(new SmartFactor(unit2)); vector views; views.push_back(c1); views.push_back(c2); views.push_back(c3); - smartFactor1->add(measurements_cam1, views, unit2); - smartFactor2->add(measurements_cam2, views, unit2); - smartFactor3->add(measurements_cam3, views, unit2); + smartFactor1->add(measurements_cam1, views); + smartFactor2->add(measurements_cam2, views); + smartFactor3->add(measurements_cam3, views); // Create factor graph and add priors on two cameras NonlinearFactorGraph graph; @@ -308,8 +304,8 @@ TEST( SmartProjectionCameraFactor, perturbPoseAndOptimizeFromSfM_tracks ) { measures.second = measurements_cam1.at(i); track1.measurements.push_back(measures); } - SmartFactor::shared_ptr smartFactor1(new SmartFactor()); - smartFactor1->add(track1, unit2); + SmartFactor::shared_ptr smartFactor1(new SmartFactor(unit2)); + smartFactor1->add(track1); SfM_Track track2; for (size_t i = 0; i < 3; ++i) { @@ -318,11 +314,11 @@ TEST( SmartProjectionCameraFactor, perturbPoseAndOptimizeFromSfM_tracks ) { measures.second = measurements_cam2.at(i); track2.measurements.push_back(measures); } - SmartFactor::shared_ptr smartFactor2(new SmartFactor()); - smartFactor2->add(track2, unit2); + SmartFactor::shared_ptr smartFactor2(new SmartFactor(unit2)); + smartFactor2->add(track2); - SmartFactor::shared_ptr smartFactor3(new SmartFactor()); - smartFactor3->add(measurements_cam3, views, unit2); + SmartFactor::shared_ptr smartFactor3(new SmartFactor(unit2)); + smartFactor3->add(measurements_cam3, views); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6 + 5, 1e-5); @@ -384,20 +380,20 @@ TEST( SmartProjectionCameraFactor, perturbCamerasAndOptimize ) { views.push_back(c2); views.push_back(c3); - SmartFactor::shared_ptr smartFactor1(new SmartFactor()); - smartFactor1->add(measurements_cam1, views, unit2); + SmartFactor::shared_ptr smartFactor1(new SmartFactor(unit2)); + smartFactor1->add(measurements_cam1, views); - SmartFactor::shared_ptr smartFactor2(new SmartFactor()); - smartFactor2->add(measurements_cam2, views, unit2); + SmartFactor::shared_ptr smartFactor2(new SmartFactor(unit2)); + smartFactor2->add(measurements_cam2, views); - SmartFactor::shared_ptr smartFactor3(new SmartFactor()); - smartFactor3->add(measurements_cam3, views, unit2); + SmartFactor::shared_ptr smartFactor3(new SmartFactor(unit2)); + smartFactor3->add(measurements_cam3, views); - SmartFactor::shared_ptr smartFactor4(new SmartFactor()); - smartFactor4->add(measurements_cam4, views, unit2); + SmartFactor::shared_ptr smartFactor4(new SmartFactor(unit2)); + smartFactor4->add(measurements_cam4, views); - SmartFactor::shared_ptr smartFactor5(new SmartFactor()); - smartFactor5->add(measurements_cam5, views, unit2); + SmartFactor::shared_ptr smartFactor5(new SmartFactor(unit2)); + smartFactor5->add(measurements_cam5, views); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6 + 5, 1e-5); @@ -464,20 +460,20 @@ TEST( SmartProjectionCameraFactor, Cal3Bundler ) { views.push_back(c2); views.push_back(c3); - SmartFactor::shared_ptr smartFactor1(new SmartFactor()); - smartFactor1->add(measurements_cam1, views, unit2); + SmartFactor::shared_ptr smartFactor1(new SmartFactor(unit2)); + smartFactor1->add(measurements_cam1, views); - SmartFactor::shared_ptr smartFactor2(new SmartFactor()); - smartFactor2->add(measurements_cam2, views, unit2); + SmartFactor::shared_ptr smartFactor2(new SmartFactor(unit2)); + smartFactor2->add(measurements_cam2, views); - SmartFactor::shared_ptr smartFactor3(new SmartFactor()); - smartFactor3->add(measurements_cam3, views, unit2); + SmartFactor::shared_ptr smartFactor3(new SmartFactor(unit2)); + smartFactor3->add(measurements_cam3, views); - SmartFactor::shared_ptr smartFactor4(new SmartFactor()); - smartFactor4->add(measurements_cam4, views, unit2); + SmartFactor::shared_ptr smartFactor4(new SmartFactor(unit2)); + smartFactor4->add(measurements_cam4, views); - SmartFactor::shared_ptr smartFactor5(new SmartFactor()); - smartFactor5->add(measurements_cam5, views, unit2); + SmartFactor::shared_ptr smartFactor5(new SmartFactor(unit2)); + smartFactor5->add(measurements_cam5, views); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(9, 1e-6); @@ -540,20 +536,20 @@ TEST( SmartProjectionCameraFactor, Cal3Bundler2 ) { views.push_back(c2); views.push_back(c3); - SmartFactor::shared_ptr smartFactor1(new SmartFactor()); - smartFactor1->add(measurements_cam1, views, unit2); + SmartFactor::shared_ptr smartFactor1(new SmartFactor(unit2)); + smartFactor1->add(measurements_cam1, views); - SmartFactor::shared_ptr smartFactor2(new SmartFactor()); - smartFactor2->add(measurements_cam2, views, unit2); + SmartFactor::shared_ptr smartFactor2(new SmartFactor(unit2)); + smartFactor2->add(measurements_cam2, views); - SmartFactor::shared_ptr smartFactor3(new SmartFactor()); - smartFactor3->add(measurements_cam3, views, unit2); + SmartFactor::shared_ptr smartFactor3(new SmartFactor(unit2)); + smartFactor3->add(measurements_cam3, views); - SmartFactor::shared_ptr smartFactor4(new SmartFactor()); - smartFactor4->add(measurements_cam4, views, unit2); + SmartFactor::shared_ptr smartFactor4(new SmartFactor(unit2)); + smartFactor4->add(measurements_cam4, views); - SmartFactor::shared_ptr smartFactor5(new SmartFactor()); - smartFactor5->add(measurements_cam5, views, unit2); + SmartFactor::shared_ptr smartFactor5(new SmartFactor(unit2)); + smartFactor5->add(measurements_cam5, views); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(9, 1e-6); @@ -604,9 +600,9 @@ TEST( SmartProjectionCameraFactor, noiselessBundler ) { values.insert(c1, level_camera); values.insert(c2, level_camera_right); - SmartFactor::shared_ptr factor1(new SmartFactor()); - factor1->add(level_uv, c1, unit2); - factor1->add(level_uv_right, c2, unit2); + SmartFactor::shared_ptr factor1(new SmartFactor(unit2)); + factor1->add(level_uv, c1); + factor1->add(level_uv_right, c2); double actualError = factor1->error(values); @@ -633,9 +629,9 @@ TEST( SmartProjectionCameraFactor, comparisonGeneralSfMFactor ) { values.insert(c2, level_camera_right); NonlinearFactorGraph smartGraph; - SmartFactor::shared_ptr factor1(new SmartFactor()); - factor1->add(level_uv, c1, unit2); - factor1->add(level_uv_right, c2, unit2); + SmartFactor::shared_ptr factor1(new SmartFactor(unit2)); + factor1->add(level_uv, c1); + factor1->add(level_uv_right, c2); smartGraph.push_back(factor1); double expectedError = factor1->error(values); double expectedErrorGraph = smartGraph.error(values); @@ -674,9 +670,9 @@ TEST( SmartProjectionCameraFactor, comparisonGeneralSfMFactor1 ) { values.insert(c2, level_camera_right); NonlinearFactorGraph smartGraph; - SmartFactor::shared_ptr factor1(new SmartFactor()); - factor1->add(level_uv, c1, unit2); - factor1->add(level_uv_right, c2, unit2); + SmartFactor::shared_ptr factor1(new SmartFactor(unit2)); + factor1->add(level_uv, c1); + factor1->add(level_uv_right, c2); smartGraph.push_back(factor1); Matrix expectedHessian = smartGraph.linearize(values)->hessian().first; Vector expectedInfoVector = smartGraph.linearize(values)->hessian().second; @@ -765,9 +761,9 @@ TEST( SmartProjectionCameraFactor, computeImplicitJacobian ) { values.insert(c1, level_camera); values.insert(c2, level_camera_right); - SmartFactor::shared_ptr factor1(new SmartFactor()); - factor1->add(level_uv, c1, unit2); - factor1->add(level_uv_right, c2, unit2); + SmartFactor::shared_ptr factor1(new SmartFactor(unit2)); + factor1->add(level_uv, c1); + factor1->add(level_uv_right, c2); Matrix expectedE; Vector expectedb; @@ -814,9 +810,9 @@ TEST( SmartProjectionCameraFactor, implicitJacobianFactor ) { params.setEnableEPI(useEPI); SmartFactor::shared_ptr explicitFactor( - new SmartFactor(boost::none, params)); - explicitFactor->add(level_uv, c1, unit2); - explicitFactor->add(level_uv_right, c2, unit2); + new SmartFactor(unit2, boost::none, params)); + explicitFactor->add(level_uv, c1); + explicitFactor->add(level_uv_right, c2); GaussianFactor::shared_ptr gaussianHessianFactor = explicitFactor->linearize( values); @@ -826,9 +822,9 @@ TEST( SmartProjectionCameraFactor, implicitJacobianFactor ) { // Implicit Schur version params.setLinearizationMode(gtsam::IMPLICIT_SCHUR); SmartFactor::shared_ptr implicitFactor( - new SmartFactor(boost::none, params)); - implicitFactor->add(level_uv, c1, unit2); - implicitFactor->add(level_uv_right, c2, unit2); + new SmartFactor(unit2, boost::none, params)); + implicitFactor->add(level_uv, c1); + implicitFactor->add(level_uv_right, c2); GaussianFactor::shared_ptr gaussianImplicitSchurFactor = implicitFactor->linearize(values); CHECK(gaussianImplicitSchurFactor); diff --git a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp index 72147ff35..8796dfe65 100644 --- a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp @@ -53,7 +53,7 @@ LevenbergMarquardtParams lmParams; /* ************************************************************************* */ TEST( SmartProjectionPoseFactor, Constructor) { using namespace vanillaPose; - SmartFactor::shared_ptr factor1(new SmartFactor(sharedK)); + SmartFactor::shared_ptr factor1(new SmartFactor(model, sharedK)); } /* ************************************************************************* */ @@ -61,14 +61,14 @@ TEST( SmartProjectionPoseFactor, Constructor2) { using namespace vanillaPose; SmartProjectionParams params; params.setRankTolerance(rankTol); - SmartFactor factor1(sharedK, boost::none, params); + SmartFactor factor1(model, sharedK, boost::none, params); } /* ************************************************************************* */ TEST( SmartProjectionPoseFactor, Constructor3) { using namespace vanillaPose; - SmartFactor::shared_ptr factor1(new SmartFactor(sharedK)); - factor1->add(measurement1, x1, model); + SmartFactor::shared_ptr factor1(new SmartFactor(model, sharedK)); + factor1->add(measurement1, x1); } /* ************************************************************************* */ @@ -76,18 +76,18 @@ TEST( SmartProjectionPoseFactor, Constructor4) { using namespace vanillaPose; SmartProjectionParams params; params.setRankTolerance(rankTol); - SmartFactor factor1(sharedK, boost::none, params); - factor1.add(measurement1, x1, model); + SmartFactor factor1(model, sharedK, boost::none, params); + factor1.add(measurement1, x1); } /* ************************************************************************* */ TEST( SmartProjectionPoseFactor, Equals ) { using namespace vanillaPose; - SmartFactor::shared_ptr factor1(new SmartFactor(sharedK)); - factor1->add(measurement1, x1, model); + SmartFactor::shared_ptr factor1(new SmartFactor(model, sharedK)); + factor1->add(measurement1, x1); - SmartFactor::shared_ptr factor2(new SmartFactor(sharedK)); - factor2->add(measurement1, x1, model); + SmartFactor::shared_ptr factor2(new SmartFactor(model,sharedK)); + factor2->add(measurement1, x1); CHECK(assert_equal(*factor1, *factor2)); } @@ -101,9 +101,9 @@ TEST( SmartProjectionPoseFactor, noiseless ) { Point2 level_uv = level_camera.project(landmark1); Point2 level_uv_right = level_camera_right.project(landmark1); - SmartFactor factor(sharedK); - factor.add(level_uv, x1, model); - factor.add(level_uv_right, x2, model); + SmartFactor factor(model, sharedK); + factor.add(level_uv, x1); + factor.add(level_uv_right, x2); Values values; // it's a pose factor, hence these are poses values.insert(x1, cam1.pose()); @@ -166,26 +166,22 @@ TEST( SmartProjectionPoseFactor, noisy ) { Point3(0.5, 0.1, 0.3)); values.insert(x2, pose_right.compose(noise_pose)); - SmartFactor::shared_ptr factor(new SmartFactor((sharedK))); - factor->add(level_uv, x1, model); - factor->add(level_uv_right, x2, model); + SmartFactor::shared_ptr factor(new SmartFactor(model, sharedK)); + factor->add(level_uv, x1); + factor->add(level_uv_right, x2); double actualError1 = factor->error(values); - SmartFactor::shared_ptr factor2(new SmartFactor((sharedK))); + SmartFactor::shared_ptr factor2(new SmartFactor(model, sharedK)); vector measurements; measurements.push_back(level_uv); measurements.push_back(level_uv_right); - vector noises; - noises.push_back(model); - noises.push_back(model); - vector views; views.push_back(x1); views.push_back(x2); - factor2->add(measurements, views, noises); + factor2->add(measurements, views); double actualError2 = factor2->error(values); DOUBLES_EQUAL(actualError1, actualError2, 1e-7); } @@ -238,14 +234,14 @@ TEST( SmartProjectionPoseFactor, smartFactorWithSensorBodyTransform ){ params.setDegeneracyMode(gtsam::IGNORE_DEGENERACY); params.setEnableEPI(false); - SmartProjectionPoseFactor smartFactor1(K, sensor_to_body, params); - smartFactor1.add(measurements_cam1, views, model); + SmartProjectionPoseFactor smartFactor1(model, K, sensor_to_body, params); + smartFactor1.add(measurements_cam1, views); - SmartProjectionPoseFactor smartFactor2(K, sensor_to_body, params); - smartFactor2.add(measurements_cam2, views, model); + SmartProjectionPoseFactor smartFactor2(model, K, sensor_to_body, params); + smartFactor2.add(measurements_cam2, views); - SmartProjectionPoseFactor smartFactor3(K, sensor_to_body, params); - smartFactor3.add(measurements_cam3, views, model); + SmartProjectionPoseFactor smartFactor3(model, K, sensor_to_body, params); + smartFactor3.add(measurements_cam3, views); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -296,14 +292,14 @@ TEST( SmartProjectionPoseFactor, 3poses_smart_projection_factor ) { views.push_back(x2); views.push_back(x3); - SmartFactor::shared_ptr smartFactor1(new SmartFactor(sharedK2)); - smartFactor1->add(measurements_cam1, views, model); + SmartFactor::shared_ptr smartFactor1(new SmartFactor(model, sharedK2)); + smartFactor1->add(measurements_cam1, views); - SmartFactor::shared_ptr smartFactor2(new SmartFactor(sharedK2)); - smartFactor2->add(measurements_cam2, views, model); + SmartFactor::shared_ptr smartFactor2(new SmartFactor(model, sharedK2)); + smartFactor2->add(measurements_cam2, views); - SmartFactor::shared_ptr smartFactor3(new SmartFactor(sharedK2)); - smartFactor3->add(measurements_cam3, views, model); + SmartFactor::shared_ptr smartFactor3(new SmartFactor(model, sharedK2)); + smartFactor3->add(measurements_cam3, views); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -366,8 +362,8 @@ TEST( SmartProjectionPoseFactor, Factors ) { views.push_back(x1); views.push_back(x2); - SmartFactor::shared_ptr smartFactor1 = boost::make_shared(sharedK); - smartFactor1->add(measurements_cam1, views, model); + SmartFactor::shared_ptr smartFactor1 = boost::make_shared(model, sharedK); + smartFactor1->add(measurements_cam1, views); SmartFactor::Cameras cameras; cameras.push_back(cam1); @@ -524,14 +520,14 @@ TEST( SmartProjectionPoseFactor, 3poses_iterative_smart_projection_factor ) { projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); - SmartFactor::shared_ptr smartFactor1(new SmartFactor(sharedK)); - smartFactor1->add(measurements_cam1, views, model); + SmartFactor::shared_ptr smartFactor1(new SmartFactor(model, sharedK)); + smartFactor1->add(measurements_cam1, views); - SmartFactor::shared_ptr smartFactor2(new SmartFactor(sharedK)); - smartFactor2->add(measurements_cam2, views, model); + SmartFactor::shared_ptr smartFactor2(new SmartFactor(model, sharedK)); + smartFactor2->add(measurements_cam2, views); - SmartFactor::shared_ptr smartFactor3(new SmartFactor(sharedK)); - smartFactor3->add(measurements_cam3, views, model); + SmartFactor::shared_ptr smartFactor3(new SmartFactor(model, sharedK)); + smartFactor3->add(measurements_cam3, views); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -586,19 +582,19 @@ TEST( SmartProjectionPoseFactor, jacobianSVD ) { params.setLinearizationMode(gtsam::JACOBIAN_SVD); params.setDegeneracyMode(gtsam::IGNORE_DEGENERACY); params.setEnableEPI(false); - SmartFactor factor1(sharedK, boost::none, params); + SmartFactor factor1(model, sharedK, boost::none, params); SmartFactor::shared_ptr smartFactor1( - new SmartFactor(sharedK, boost::none, params)); - smartFactor1->add(measurements_cam1, views, model); + new SmartFactor(model, sharedK, boost::none, params)); + smartFactor1->add(measurements_cam1, views); SmartFactor::shared_ptr smartFactor2( - new SmartFactor(sharedK, boost::none, params)); - smartFactor2->add(measurements_cam2, views, model); + new SmartFactor(model, sharedK, boost::none, params)); + smartFactor2->add(measurements_cam2, views); SmartFactor::shared_ptr smartFactor3( - new SmartFactor(sharedK, boost::none, params)); - smartFactor3->add(measurements_cam3, views, model); + new SmartFactor(model, sharedK, boost::none, params)); + smartFactor3->add(measurements_cam3, views); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -650,16 +646,16 @@ TEST( SmartProjectionPoseFactor, landmarkDistance ) { params.setEnableEPI(false); SmartFactor::shared_ptr smartFactor1( - new SmartFactor(sharedK, boost::none, params)); - smartFactor1->add(measurements_cam1, views, model); + new SmartFactor(model, sharedK, boost::none, params)); + smartFactor1->add(measurements_cam1, views); SmartFactor::shared_ptr smartFactor2( - new SmartFactor(sharedK, boost::none, params)); - smartFactor2->add(measurements_cam2, views, model); + new SmartFactor(model, sharedK, boost::none, params)); + smartFactor2->add(measurements_cam2, views); SmartFactor::shared_ptr smartFactor3( - new SmartFactor(sharedK, boost::none, params)); - smartFactor3->add(measurements_cam3, views, model); + new SmartFactor(model, sharedK, boost::none, params)); + smartFactor3->add(measurements_cam3, views); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -717,20 +713,20 @@ TEST( SmartProjectionPoseFactor, dynamicOutlierRejection ) { params.setDynamicOutlierRejectionThreshold(dynamicOutlierRejectionThreshold); SmartFactor::shared_ptr smartFactor1( - new SmartFactor(sharedK, boost::none, params)); - smartFactor1->add(measurements_cam1, views, model); + new SmartFactor(model, sharedK, boost::none, params)); + smartFactor1->add(measurements_cam1, views); SmartFactor::shared_ptr smartFactor2( - new SmartFactor(sharedK, boost::none, params)); - smartFactor2->add(measurements_cam2, views, model); + new SmartFactor(model, sharedK, boost::none, params)); + smartFactor2->add(measurements_cam2, views); SmartFactor::shared_ptr smartFactor3( - new SmartFactor(sharedK, boost::none, params)); - smartFactor3->add(measurements_cam3, views, model); + new SmartFactor(model, sharedK, boost::none, params)); + smartFactor3->add(measurements_cam3, views); SmartFactor::shared_ptr smartFactor4( - new SmartFactor(sharedK, boost::none, params)); - smartFactor4->add(measurements_cam4, views, model); + new SmartFactor(model, sharedK, boost::none, params)); + smartFactor4->add(measurements_cam4, views); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -775,16 +771,16 @@ TEST( SmartProjectionPoseFactor, jacobianQ ) { params.setLinearizationMode(gtsam::JACOBIAN_Q); SmartFactor::shared_ptr smartFactor1( - new SmartFactor(sharedK, boost::none, params)); - smartFactor1->add(measurements_cam1, views, model); + new SmartFactor(model, sharedK, boost::none, params)); + smartFactor1->add(measurements_cam1, views); SmartFactor::shared_ptr smartFactor2( - new SmartFactor(sharedK, boost::none, params)); - smartFactor2->add(measurements_cam2, views, model); + new SmartFactor(model, sharedK, boost::none, params)); + smartFactor2->add(measurements_cam2, views); SmartFactor::shared_ptr smartFactor3( - new SmartFactor(sharedK, boost::none, params)); - smartFactor3->add(measurements_cam3, views, model); + new SmartFactor(model, sharedK, boost::none, params)); + smartFactor3->add(measurements_cam3, views); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -895,16 +891,16 @@ TEST( SmartProjectionPoseFactor, CheckHessian) { params.setRankTolerance(10); SmartFactor::shared_ptr smartFactor1( - new SmartFactor(sharedK, boost::none, params)); // HESSIAN, by default - smartFactor1->add(measurements_cam1, views, model); + new SmartFactor(model, sharedK, boost::none, params)); // HESSIAN, by default + smartFactor1->add(measurements_cam1, views); SmartFactor::shared_ptr smartFactor2( - new SmartFactor(sharedK, boost::none, params)); // HESSIAN, by default - smartFactor2->add(measurements_cam2, views, model); + new SmartFactor(model, sharedK, boost::none, params)); // HESSIAN, by default + smartFactor2->add(measurements_cam2, views); SmartFactor::shared_ptr smartFactor3( - new SmartFactor(sharedK, boost::none, params)); // HESSIAN, by default - smartFactor3->add(measurements_cam3, views, model); + new SmartFactor(model, sharedK, boost::none, params)); // HESSIAN, by default + smartFactor3->add(measurements_cam3, views); NonlinearFactorGraph graph; graph.push_back(smartFactor1); @@ -978,12 +974,12 @@ TEST( SmartProjectionPoseFactor, 3poses_2land_rotation_only_smart_projection_fac params.setDegeneracyMode(gtsam::HANDLE_INFINITY); SmartFactor::shared_ptr smartFactor1( - new SmartFactor(sharedK2, boost::none, params)); - smartFactor1->add(measurements_cam1, views, model); + new SmartFactor(model, sharedK2, boost::none, params)); + smartFactor1->add(measurements_cam1, views); SmartFactor::shared_ptr smartFactor2( - new SmartFactor(sharedK2, boost::none, params)); - smartFactor2->add(measurements_cam2, views, model); + new SmartFactor(model, sharedK2, boost::none, params)); + smartFactor2->add(measurements_cam2, views); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); const SharedDiagonal noisePriorTranslation = noiseModel::Isotropic::Sigma(3, 0.10); @@ -1040,16 +1036,16 @@ TEST( SmartProjectionPoseFactor, 3poses_rotation_only_smart_projection_factor ) params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY); SmartFactor::shared_ptr smartFactor1( - new SmartFactor(sharedK, boost::none, params)); - smartFactor1->add(measurements_cam1, views, model); + new SmartFactor(model, sharedK, boost::none, params)); + smartFactor1->add(measurements_cam1, views); SmartFactor::shared_ptr smartFactor2( - new SmartFactor(sharedK, boost::none, params)); - smartFactor2->add(measurements_cam2, views, model); + new SmartFactor(model, sharedK, boost::none, params)); + smartFactor2->add(measurements_cam2, views); SmartFactor::shared_ptr smartFactor3( - new SmartFactor(sharedK, boost::none, params)); - smartFactor3->add(measurements_cam3, views, model); + new SmartFactor(model, sharedK, boost::none, params)); + smartFactor3->add(measurements_cam3, views); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); const SharedDiagonal noisePriorTranslation = noiseModel::Isotropic::Sigma(3, @@ -1108,8 +1104,8 @@ TEST( SmartProjectionPoseFactor, Hessian ) { measurements_cam1.push_back(cam1_uv1); measurements_cam1.push_back(cam2_uv1); - SmartFactor::shared_ptr smartFactor1(new SmartFactor(sharedK2)); - smartFactor1->add(measurements_cam1, views, model); + SmartFactor::shared_ptr smartFactor1(new SmartFactor(model, sharedK2)); + smartFactor1->add(measurements_cam1, views); Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 10, 0., -M_PI / 10), Point3(0.5, 0.1, 0.3)); @@ -1140,8 +1136,8 @@ TEST( SmartProjectionPoseFactor, HessianWithRotation ) { projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); - SmartFactor::shared_ptr smartFactorInstance(new SmartFactor(sharedK)); - smartFactorInstance->add(measurements_cam1, views, model); + SmartFactor::shared_ptr smartFactorInstance(new SmartFactor(model, sharedK)); + smartFactorInstance->add(measurements_cam1, views); Values values; values.insert(x1, cam1.pose()); @@ -1196,8 +1192,8 @@ TEST( SmartProjectionPoseFactor, HessianWithRotationDegenerate ) { vector measurements_cam1; projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); - SmartFactor::shared_ptr smartFactor(new SmartFactor(sharedK2)); - smartFactor->add(measurements_cam1, views, model); + SmartFactor::shared_ptr smartFactor(new SmartFactor(model, sharedK2)); + smartFactor->add(measurements_cam1, views); Values values; values.insert(x1, cam1.pose()); @@ -1239,8 +1235,8 @@ TEST( SmartProjectionPoseFactor, ConstructorWithCal3Bundler) { using namespace bundlerPose; SmartProjectionParams params; params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY); - SmartFactor factor(sharedBundlerK, boost::none, params); - factor.add(measurement1, x1, model); + SmartFactor factor(model, sharedBundlerK, boost::none, params); + factor.add(measurement1, x1); } /* *************************************************************************/ @@ -1263,14 +1259,14 @@ TEST( SmartProjectionPoseFactor, Cal3Bundler ) { views.push_back(x2); views.push_back(x3); - SmartFactor::shared_ptr smartFactor1(new SmartFactor(sharedBundlerK)); - smartFactor1->add(measurements_cam1, views, model); + SmartFactor::shared_ptr smartFactor1(new SmartFactor(model, sharedBundlerK)); + smartFactor1->add(measurements_cam1, views); - SmartFactor::shared_ptr smartFactor2(new SmartFactor(sharedBundlerK)); - smartFactor2->add(measurements_cam2, views, model); + SmartFactor::shared_ptr smartFactor2(new SmartFactor(model, sharedBundlerK)); + smartFactor2->add(measurements_cam2, views); - SmartFactor::shared_ptr smartFactor3(new SmartFactor(sharedBundlerK)); - smartFactor3->add(measurements_cam3, views, model); + SmartFactor::shared_ptr smartFactor3(new SmartFactor(model, sharedBundlerK)); + smartFactor3->add(measurements_cam3, views); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -1334,16 +1330,16 @@ TEST( SmartProjectionPoseFactor, Cal3BundlerRotationOnly ) { params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY); SmartFactor::shared_ptr smartFactor1( - new SmartFactor(sharedBundlerK, boost::none, params)); - smartFactor1->add(measurements_cam1, views, model); + new SmartFactor(model, sharedBundlerK, boost::none, params)); + smartFactor1->add(measurements_cam1, views); SmartFactor::shared_ptr smartFactor2( - new SmartFactor(sharedBundlerK, boost::none, params)); - smartFactor2->add(measurements_cam2, views, model); + new SmartFactor(model, sharedBundlerK, boost::none, params)); + smartFactor2->add(measurements_cam2, views); SmartFactor::shared_ptr smartFactor3( - new SmartFactor(sharedBundlerK, boost::none, params)); - smartFactor3->add(measurements_cam3, views, model); + new SmartFactor(model, sharedBundlerK, boost::none, params)); + smartFactor3->add(measurements_cam3, views); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); const SharedDiagonal noisePriorTranslation = noiseModel::Isotropic::Sigma(3, diff --git a/gtsam_unstable/examples/SmartStereoProjectionFactorExample.cpp b/gtsam_unstable/examples/SmartStereoProjectionFactorExample.cpp index 5ba8ec913..d3680460f 100644 --- a/gtsam_unstable/examples/SmartStereoProjectionFactorExample.cpp +++ b/gtsam_unstable/examples/SmartStereoProjectionFactorExample.cpp @@ -109,17 +109,17 @@ int main(int argc, char** argv){ //read stereo measurements and construct smart factors - SmartFactor::shared_ptr factor(new SmartFactor()); + SmartFactor::shared_ptr factor(new SmartFactor(model)); size_t current_l = 3; // hardcoded landmark ID from first measurement while (factor_file >> x >> l >> uL >> uR >> v >> X >> Y >> Z) { if(current_l != l) { graph.push_back(factor); - factor = SmartFactor::shared_ptr(new SmartFactor()); + factor = SmartFactor::shared_ptr(new SmartFactor(model)); current_l = l; } - factor->add(StereoPoint2(uL,uR,v), Symbol('x',x), model, K); + factor->add(StereoPoint2(uL,uR,v), Symbol('x',x), K); } Pose3 first_pose = initial_estimate.at(Symbol('x',1)); diff --git a/gtsam_unstable/slam/SmartStereoProjectionFactor.h b/gtsam_unstable/slam/SmartStereoProjectionFactor.h index c12bf8d94..e7118a36c 100644 --- a/gtsam_unstable/slam/SmartStereoProjectionFactor.h +++ b/gtsam_unstable/slam/SmartStereoProjectionFactor.h @@ -159,8 +159,10 @@ public: * Constructor * @param params internal parameters of the smart factors */ - SmartStereoProjectionFactor(const SmartStereoProjectionParams& params = + SmartStereoProjectionFactor(const SharedNoiseModel& sharedNoiseModel, + const SmartStereoProjectionParams& params = SmartStereoProjectionParams()) : + Base(sharedNoiseModel), // params_(params), // result_(TriangulationResult::Degenerate()) { } diff --git a/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h b/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h index c9d8ec285..c2526fd74 100644 --- a/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h +++ b/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h @@ -62,15 +62,13 @@ public: /** * Constructor - * @param rankTol tolerance used to check if point triangulation is degenerate - * @param linThreshold threshold on relative pose changes used to decide whether to relinearize (selective relinearization) - * @param manageDegeneracy is true, in presence of degenerate triangulation, the factor is converted to a rotation-only constraint, - * otherwise the factor is simply neglected - * @param enableEPI if set to true linear triangulation is refined with embedded LM iterations + * @param Isotropic measurement noise + * @param params internal parameters of the smart factors */ - SmartStereoProjectionPoseFactor(const SmartStereoProjectionParams& params = + SmartStereoProjectionPoseFactor(const SharedNoiseModel& sharedNoiseModel, + const SmartStereoProjectionParams& params = SmartStereoProjectionParams()) : - Base(params) { + Base(sharedNoiseModel, params) { } /** Virtual destructor */ @@ -80,27 +78,23 @@ public: * add a new measurement and pose key * @param measured is the 2m dimensional location of the projection of a single landmark in the m view (the measurement) * @param poseKey is key corresponding to the camera observing the same landmark - * @param noise_i is the measurement noise - * @param K_i is the (known) camera calibration + * @param K is the (fixed) camera calibration */ - void add(const StereoPoint2 measured_i, const Key poseKey_i, - const SharedNoiseModel noise_i, - const boost::shared_ptr K_i) { - Base::add(measured_i, poseKey_i, noise_i); - K_all_.push_back(K_i); + void add(const StereoPoint2 measured, const Key poseKey, + const boost::shared_ptr K) { + Base::add(measured, poseKey); + K_all_.push_back(K); } /** * Variant of the previous one in which we include a set of measurements * @param measurements vector of the 2m dimensional location of the projection of a single landmark in the m view (the measurement) * @param poseKeys vector of keys corresponding to the camera observing the same landmark - * @param noises vector of measurement noises * @param Ks vector of calibration objects */ void add(std::vector measurements, std::vector poseKeys, - std::vector noises, std::vector > Ks) { - Base::add(measurements, poseKeys, noises); + Base::add(measurements, poseKeys); for (size_t i = 0; i < measurements.size(); i++) { K_all_.push_back(Ks.at(i)); } @@ -110,13 +104,12 @@ public: * Variant of the previous one in which we include a set of measurements with the same noise and calibration * @param mmeasurements vector of the 2m dimensional location of the projection of a single landmark in the m view (the measurement) * @param poseKeys vector of keys corresponding to the camera observing the same landmark - * @param noise measurement noise (same for all measurements) * @param K the (known) camera calibration (same for all measurements) */ void add(std::vector measurements, std::vector poseKeys, - const SharedNoiseModel noise, const boost::shared_ptr K) { + const boost::shared_ptr K) { for (size_t i = 0; i < measurements.size(); i++) { - Base::add(measurements.at(i), poseKeys.at(i), noise); + Base::add(measurements.at(i), poseKeys.at(i)); K_all_.push_back(K); } } diff --git a/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp b/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp index 3b16a9db8..2981f675d 100644 --- a/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp +++ b/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp @@ -81,33 +81,33 @@ LevenbergMarquardtParams lm_params; /* ************************************************************************* */ TEST( SmartStereoProjectionPoseFactor, Constructor) { - SmartStereoProjectionPoseFactor::shared_ptr factor1(new SmartStereoProjectionPoseFactor()); + SmartStereoProjectionPoseFactor::shared_ptr factor1(new SmartStereoProjectionPoseFactor(model)); } /* ************************************************************************* */ TEST( SmartStereoProjectionPoseFactor, Constructor2) { - SmartStereoProjectionPoseFactor factor1(params); + SmartStereoProjectionPoseFactor factor1(model, params); } /* ************************************************************************* */ TEST( SmartStereoProjectionPoseFactor, Constructor3) { - SmartStereoProjectionPoseFactor::shared_ptr factor1(new SmartStereoProjectionPoseFactor()); - factor1->add(measurement1, poseKey1, model, K); + SmartStereoProjectionPoseFactor::shared_ptr factor1(new SmartStereoProjectionPoseFactor(model)); + factor1->add(measurement1, poseKey1, K); } /* ************************************************************************* */ TEST( SmartStereoProjectionPoseFactor, Constructor4) { - SmartStereoProjectionPoseFactor factor1(params); - factor1.add(measurement1, poseKey1, model, K); + SmartStereoProjectionPoseFactor factor1(model, params); + factor1.add(measurement1, poseKey1, K); } /* ************************************************************************* */ TEST( SmartStereoProjectionPoseFactor, Equals ) { - SmartStereoProjectionPoseFactor::shared_ptr factor1(new SmartStereoProjectionPoseFactor()); - factor1->add(measurement1, poseKey1, model, K); + SmartStereoProjectionPoseFactor::shared_ptr factor1(new SmartStereoProjectionPoseFactor(model)); + factor1->add(measurement1, poseKey1, K); - SmartStereoProjectionPoseFactor::shared_ptr factor2(new SmartStereoProjectionPoseFactor()); - factor2->add(measurement1, poseKey1, model, K); + SmartStereoProjectionPoseFactor::shared_ptr factor2(new SmartStereoProjectionPoseFactor(model)); + factor2->add(measurement1, poseKey1, K); CHECK(assert_equal(*factor1, *factor2)); } @@ -134,9 +134,9 @@ TEST_UNSAFE( SmartStereoProjectionPoseFactor, noiseless ) { values.insert(x1, level_pose); values.insert(x2, level_pose_right); - SmartStereoProjectionPoseFactor factor1; - factor1.add(level_uv, x1, model, K2); - factor1.add(level_uv_right, x2, model, K2); + SmartStereoProjectionPoseFactor factor1(model); + factor1.add(level_uv, x1, K2); + factor1.add(level_uv_right, x2, K2); double actualError = factor1.error(values); double expectedError = 0.0; @@ -176,21 +176,17 @@ TEST( SmartStereoProjectionPoseFactor, noisy ) { Point3(0.5, 0.1, 0.3)); values.insert(x2, level_pose_right.compose(noise_pose)); - SmartStereoProjectionPoseFactor::shared_ptr factor1(new SmartStereoProjectionPoseFactor()); - factor1->add(level_uv, x1, model, K); - factor1->add(level_uv_right, x2, model, K); + SmartStereoProjectionPoseFactor::shared_ptr factor1(new SmartStereoProjectionPoseFactor(model)); + factor1->add(level_uv, x1, K); + factor1->add(level_uv_right, x2, K); double actualError1 = factor1->error(values); - SmartStereoProjectionPoseFactor::shared_ptr factor2(new SmartStereoProjectionPoseFactor()); + SmartStereoProjectionPoseFactor::shared_ptr factor2(new SmartStereoProjectionPoseFactor(model)); vector measurements; measurements.push_back(level_uv); measurements.push_back(level_uv_right); - vector noises; - noises.push_back(model); - noises.push_back(model); - vector > Ks; ///< shared pointer to calibration object (one for each camera) Ks.push_back(K); Ks.push_back(K); @@ -199,7 +195,7 @@ TEST( SmartStereoProjectionPoseFactor, noisy ) { views.push_back(x1); views.push_back(x2); - factor2->add(measurements, views, noises, Ks); + factor2->add(measurements, views, Ks); double actualError2 = factor2->error(values); @@ -241,14 +237,14 @@ TEST( SmartStereoProjectionPoseFactor, 3poses_smart_projection_factor ) { SmartStereoProjectionParams smart_params; smart_params.triangulation.enableEPI = true; - SmartStereoProjectionPoseFactor::shared_ptr smartFactor1(new SmartStereoProjectionPoseFactor(smart_params)); - smartFactor1->add(measurements_l1, views, model, K2); + SmartStereoProjectionPoseFactor::shared_ptr smartFactor1(new SmartStereoProjectionPoseFactor(model, smart_params)); + smartFactor1->add(measurements_l1, views, K2); - SmartStereoProjectionPoseFactor::shared_ptr smartFactor2(new SmartStereoProjectionPoseFactor(smart_params)); - smartFactor2->add(measurements_l2, views, model, K2); + SmartStereoProjectionPoseFactor::shared_ptr smartFactor2(new SmartStereoProjectionPoseFactor(model, smart_params)); + smartFactor2->add(measurements_l2, views, K2); - SmartStereoProjectionPoseFactor::shared_ptr smartFactor3(new SmartStereoProjectionPoseFactor(smart_params)); - smartFactor3->add(measurements_l3, views, model, K2); + SmartStereoProjectionPoseFactor::shared_ptr smartFactor3(new SmartStereoProjectionPoseFactor(model, smart_params)); + smartFactor3->add(measurements_l3, views, K2); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -383,14 +379,14 @@ TEST( SmartStereoProjectionPoseFactor, jacobianSVD ) { SmartStereoProjectionParams params; params.setLinearizationMode(JACOBIAN_SVD); - SmartStereoProjectionPoseFactor::shared_ptr smartFactor1( new SmartStereoProjectionPoseFactor(params)); - smartFactor1->add(measurements_cam1, views, model, K); + SmartStereoProjectionPoseFactor::shared_ptr smartFactor1( new SmartStereoProjectionPoseFactor(model, params)); + smartFactor1->add(measurements_cam1, views, K); - SmartStereoProjectionPoseFactor::shared_ptr smartFactor2(new SmartStereoProjectionPoseFactor(params)); - smartFactor2->add(measurements_cam2, views, model, K); + SmartStereoProjectionPoseFactor::shared_ptr smartFactor2(new SmartStereoProjectionPoseFactor(model, params)); + smartFactor2->add(measurements_cam2, views, K); - SmartStereoProjectionPoseFactor::shared_ptr smartFactor3(new SmartStereoProjectionPoseFactor(params)); - smartFactor3->add(measurements_cam3, views, model, K); + SmartStereoProjectionPoseFactor::shared_ptr smartFactor3(new SmartStereoProjectionPoseFactor(model, params)); + smartFactor3->add(measurements_cam3, views, K); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -452,14 +448,14 @@ TEST( SmartStereoProjectionPoseFactor, landmarkDistance ) { params.setLinearizationMode(JACOBIAN_SVD); params.setLandmarkDistanceThreshold(2); - SmartStereoProjectionPoseFactor::shared_ptr smartFactor1(new SmartStereoProjectionPoseFactor(params)); - smartFactor1->add(measurements_cam1, views, model, K); + SmartStereoProjectionPoseFactor::shared_ptr smartFactor1(new SmartStereoProjectionPoseFactor(model, params)); + smartFactor1->add(measurements_cam1, views, K); - SmartStereoProjectionPoseFactor::shared_ptr smartFactor2(new SmartStereoProjectionPoseFactor(params)); - smartFactor2->add(measurements_cam2, views, model, K); + SmartStereoProjectionPoseFactor::shared_ptr smartFactor2(new SmartStereoProjectionPoseFactor(model, params)); + smartFactor2->add(measurements_cam2, views, K); - SmartStereoProjectionPoseFactor::shared_ptr smartFactor3(new SmartStereoProjectionPoseFactor(params)); - smartFactor3->add(measurements_cam3, views, model, K); + SmartStereoProjectionPoseFactor::shared_ptr smartFactor3(new SmartStereoProjectionPoseFactor(model, params)); + smartFactor3->add(measurements_cam3, views, K); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -526,21 +522,21 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ) { params.setDynamicOutlierRejectionThreshold(1); - SmartStereoProjectionPoseFactor::shared_ptr smartFactor1(new SmartStereoProjectionPoseFactor(params)); - smartFactor1->add(measurements_cam1, views, model, K); + SmartStereoProjectionPoseFactor::shared_ptr smartFactor1(new SmartStereoProjectionPoseFactor(model, params)); + smartFactor1->add(measurements_cam1, views, K); - SmartStereoProjectionPoseFactor::shared_ptr smartFactor2(new SmartStereoProjectionPoseFactor(params)); - smartFactor2->add(measurements_cam2, views, model, K); + SmartStereoProjectionPoseFactor::shared_ptr smartFactor2(new SmartStereoProjectionPoseFactor(model, params)); + smartFactor2->add(measurements_cam2, views, K); - SmartStereoProjectionPoseFactor::shared_ptr smartFactor3(new SmartStereoProjectionPoseFactor(params)); - smartFactor3->add(measurements_cam3, views, model, K); + SmartStereoProjectionPoseFactor::shared_ptr smartFactor3(new SmartStereoProjectionPoseFactor(model, params)); + smartFactor3->add(measurements_cam3, views, K); - SmartStereoProjectionPoseFactor::shared_ptr smartFactor4(new SmartStereoProjectionPoseFactor(params)); - smartFactor4->add(measurements_cam4, views, model, K); + SmartStereoProjectionPoseFactor::shared_ptr smartFactor4(new SmartStereoProjectionPoseFactor(model, params)); + smartFactor4->add(measurements_cam4, views, K); // same as factor 4, but dynamic outlier rejection is off - SmartStereoProjectionPoseFactor::shared_ptr smartFactor4b(new SmartStereoProjectionPoseFactor()); - smartFactor4b->add(measurements_cam4, views, model, K); + SmartStereoProjectionPoseFactor::shared_ptr smartFactor4b(new SmartStereoProjectionPoseFactor(model)); + smartFactor4b->add(measurements_cam4, views, K); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -739,14 +735,14 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian) { SmartStereoProjectionParams params; params.setRankTolerance(10); - SmartStereoProjectionPoseFactor::shared_ptr smartFactor1(new SmartStereoProjectionPoseFactor(params)); - smartFactor1->add(measurements_cam1, views, model, K); + SmartStereoProjectionPoseFactor::shared_ptr smartFactor1(new SmartStereoProjectionPoseFactor(model, params)); + smartFactor1->add(measurements_cam1, views, K); - SmartStereoProjectionPoseFactor::shared_ptr smartFactor2(new SmartStereoProjectionPoseFactor(params)); - smartFactor2->add(measurements_cam2, views, model, K); + SmartStereoProjectionPoseFactor::shared_ptr smartFactor2(new SmartStereoProjectionPoseFactor(model, params)); + smartFactor2->add(measurements_cam2, views, K); - SmartStereoProjectionPoseFactor::shared_ptr smartFactor3(new SmartStereoProjectionPoseFactor(params)); - smartFactor3->add(measurements_cam3, views, model, K); + SmartStereoProjectionPoseFactor::shared_ptr smartFactor3(new SmartStereoProjectionPoseFactor(model, params)); + smartFactor3->add(measurements_cam3, views, K); // Create graph to optimize NonlinearFactorGraph graph; @@ -997,8 +993,8 @@ TEST( SmartStereoProjectionPoseFactor, HessianWithRotation ) { vector measurements_cam1 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark1); - SmartStereoProjectionPoseFactor::shared_ptr smartFactorInstance(new SmartStereoProjectionPoseFactor()); - smartFactorInstance->add(measurements_cam1, views, model, K); + SmartStereoProjectionPoseFactor::shared_ptr smartFactorInstance(new SmartStereoProjectionPoseFactor(model)); + smartFactorInstance->add(measurements_cam1, views, K); Values values; values.insert(x1, pose1); @@ -1065,8 +1061,8 @@ TEST( SmartStereoProjectionPoseFactor, HessianWithRotationDegenerate ) { vector measurements_cam1 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark1); - SmartStereoProjectionPoseFactor::shared_ptr smartFactor(new SmartStereoProjectionPoseFactor()); - smartFactor->add(measurements_cam1, views, model, K2); + SmartStereoProjectionPoseFactor::shared_ptr smartFactor(new SmartStereoProjectionPoseFactor(model)); + smartFactor->add(measurements_cam1, views, K2); Values values; values.insert(x1, pose1); From 92e210b8939ad5687a1f5a44a5594352ff183364 Mon Sep 17 00:00:00 2001 From: cbeall3 Date: Wed, 26 Aug 2015 13:25:12 -0400 Subject: [PATCH 2/2] Fix examples and Matlab wrapper --- examples/SFMExample_SmartFactor.cpp | 4 ++-- examples/SFMExample_SmartFactorPCG.cpp | 4 ++-- gtsam.h | 12 +++++++----- .../examples/SmartProjectionFactorExample.cpp | 6 +++--- 4 files changed, 14 insertions(+), 12 deletions(-) diff --git a/examples/SFMExample_SmartFactor.cpp b/examples/SFMExample_SmartFactor.cpp index bb8935439..e7c0aa696 100644 --- a/examples/SFMExample_SmartFactor.cpp +++ b/examples/SFMExample_SmartFactor.cpp @@ -58,7 +58,7 @@ int main(int argc, char* argv[]) { for (size_t j = 0; j < points.size(); ++j) { // every landmark represent a single landmark, we use shared pointer to init the factor, and then insert measurements. - SmartFactor::shared_ptr smartfactor(new SmartFactor(K)); + SmartFactor::shared_ptr smartfactor(new SmartFactor(measurementNoise, K)); for (size_t i = 0; i < poses.size(); ++i) { @@ -71,7 +71,7 @@ int main(int argc, char* argv[]) { // 2. the corresponding camera's key // 3. camera noise model // 4. camera calibration - smartfactor->add(measurement, i, measurementNoise); + smartfactor->add(measurement, i); } // insert the smart factor in the graph diff --git a/examples/SFMExample_SmartFactorPCG.cpp b/examples/SFMExample_SmartFactorPCG.cpp index 7ec5f8268..743934c7c 100644 --- a/examples/SFMExample_SmartFactorPCG.cpp +++ b/examples/SFMExample_SmartFactorPCG.cpp @@ -54,7 +54,7 @@ int main(int argc, char* argv[]) { for (size_t j = 0; j < points.size(); ++j) { // every landmark represent a single landmark, we use shared pointer to init the factor, and then insert measurements. - SmartFactor::shared_ptr smartfactor(new SmartFactor(K)); + SmartFactor::shared_ptr smartfactor(new SmartFactor(measurementNoise, K)); for (size_t i = 0; i < poses.size(); ++i) { @@ -63,7 +63,7 @@ int main(int argc, char* argv[]) { Point2 measurement = camera.project(points[j]); // call add() function to add measurement into a single factor, here we need to add: - smartfactor->add(measurement, i, measurementNoise); + smartfactor->add(measurement, i); } // insert the smart factor in the graph diff --git a/gtsam.h b/gtsam.h index 6fb73e0ca..a75dbf056 100644 --- a/gtsam.h +++ b/gtsam.h @@ -2364,15 +2364,17 @@ class SmartProjectionParams { template virtual class SmartProjectionPoseFactor: gtsam::NonlinearFactor { - SmartProjectionPoseFactor(const CALIBRATION* K); - SmartProjectionPoseFactor(const CALIBRATION* K, + SmartProjectionPoseFactor(const gtsam::noiseModel::Base* noise, + const CALIBRATION* K); + SmartProjectionPoseFactor(const gtsam::noiseModel::Base* noise, + const CALIBRATION* K, const gtsam::Pose3& body_P_sensor); - SmartProjectionPoseFactor(const CALIBRATION* K, + SmartProjectionPoseFactor(const gtsam::noiseModel::Base* noise, + const CALIBRATION* K, const gtsam::Pose3& body_P_sensor, const gtsam::SmartProjectionParams& params); - void add(const gtsam::Point2& measured_i, size_t poseKey_i, - const gtsam::noiseModel::Base* noise_i); + void add(const gtsam::Point2& measured_i, size_t poseKey_i); // enabling serialization functionality //void serialize() const; diff --git a/gtsam_unstable/examples/SmartProjectionFactorExample.cpp b/gtsam_unstable/examples/SmartProjectionFactorExample.cpp index e00dcee57..1423ef113 100644 --- a/gtsam_unstable/examples/SmartProjectionFactorExample.cpp +++ b/gtsam_unstable/examples/SmartProjectionFactorExample.cpp @@ -87,17 +87,17 @@ int main(int argc, char** argv){ //read stereo measurements and construct smart factors - SmartFactor::shared_ptr factor(new SmartFactor(K)); + SmartFactor::shared_ptr factor(new SmartFactor(model, K)); size_t current_l = 3; // hardcoded landmark ID from first measurement while (factor_file >> i >> l >> uL >> uR >> v >> X >> Y >> Z) { if(current_l != l) { graph.push_back(factor); - factor = SmartFactor::shared_ptr(new SmartFactor(K)); + factor = SmartFactor::shared_ptr(new SmartFactor(model, K)); current_l = l; } - factor->add(Point2(uL,v), i, model); + factor->add(Point2(uL,v), i); } Pose3 firstPose = initial_estimate.at(1);