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);