From 8c99d71de3107e65894b81292674296bc9eead65 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Thu, 5 Jun 2025 01:08:34 -0700 Subject: [PATCH 1/5] Fix misuses of `size_t` for `gtsam::Key` --- gtsam/gtsam.i | 20 +-- gtsam/inference/inference.i | 76 +++++------ gtsam/linear/linear.i | 67 ++++++---- gtsam/navigation/navigation.i | 26 ++-- gtsam/nonlinear/nonlinear.i | 12 +- gtsam/sam/sam.i | 8 +- gtsam/sfm/sfm.i | 12 +- gtsam/slam/slam.i | 45 +++---- gtsam/symbolic/symbolic.i | 40 +++--- gtsam_unstable/gtsam_unstable.i | 124 +++++++++--------- python/gtsam/tests/test_Cal3Fisheye.py | 14 +- python/gtsam/tests/test_Cal3Unified.py | 16 +-- python/gtsam/tests/test_GraphvizFormatting.py | 6 +- .../tests/test_HybridNonlinearFactorGraph.py | 4 +- 14 files changed, 241 insertions(+), 229 deletions(-) diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i index 00b4d05f87..ceb7b1efcc 100644 --- a/gtsam/gtsam.i +++ b/gtsam/gtsam.i @@ -31,12 +31,12 @@ class KeyList { // structure specific methods size_t front() const; size_t back() const; - void push_back(size_t key); - void push_front(size_t key); + void push_back(gtsam::Key key); + void push_front(gtsam::Key key); void pop_back(); void pop_front(); void sort(); - void remove(size_t key); + void remove(gtsam::Key key); void serialize() const; }; @@ -58,10 +58,10 @@ class KeySet { void clear(); // structure specific methods - void insert(size_t key); + void insert(gtsam::Key key); void merge(const gtsam::KeySet& other); - bool erase(size_t key); // returns true if value was removed - bool count(size_t key) const; // returns true if value exists + bool erase(gtsam::Key key); // returns true if value was removed + bool count(gtsam::Key key) const; // returns true if value exists void serialize() const; }; @@ -82,7 +82,7 @@ class KeyVector { size_t at(size_t i) const; size_t front() const; size_t back() const; - void push_back(size_t key) const; + void push_back(gtsam::Key key) const; void serialize() const; }; @@ -99,9 +99,9 @@ class KeyGroupMap { void clear(); // structure specific methods - size_t at(size_t key) const; - int erase(size_t key); - bool insert2(size_t key, int val); + size_t at(gtsam::Key key) const; + int erase(gtsam::Key key); + bool insert2(gtsam::Key key, int val); }; // Actually a FastSet diff --git a/gtsam/inference/inference.i b/gtsam/inference/inference.i index 39039926e6..fbf6242a9d 100644 --- a/gtsam/inference/inference.i +++ b/gtsam/inference/inference.i @@ -28,9 +28,9 @@ void PrintKeySet( class Symbol { Symbol(); Symbol(char c, uint64_t j); - Symbol(size_t key); + Symbol(gtsam::Key key); - size_t key() const; + gtsam::Key key() const; void print(const string& s = "") const; bool equals(const gtsam::Symbol& expected, double tol) const; @@ -39,46 +39,46 @@ class Symbol { string string() const; }; -size_t symbol(char chr, size_t index); -char symbolChr(size_t key); -size_t symbolIndex(size_t key); +gtsam::Key symbol(char chr, size_t index); +char symbolChr(gtsam::Key key); +size_t symbolIndex(gtsam::Key key); namespace symbol_shorthand { -size_t A(size_t j); -size_t B(size_t j); -size_t C(size_t j); -size_t D(size_t j); -size_t E(size_t j); -size_t F(size_t j); -size_t G(size_t j); -size_t H(size_t j); -size_t I(size_t j); -size_t J(size_t j); -size_t K(size_t j); -size_t L(size_t j); -size_t M(size_t j); -size_t N(size_t j); -size_t O(size_t j); -size_t P(size_t j); -size_t Q(size_t j); -size_t R(size_t j); -size_t S(size_t j); -size_t T(size_t j); -size_t U(size_t j); -size_t V(size_t j); -size_t W(size_t j); -size_t X(size_t j); -size_t Y(size_t j); -size_t Z(size_t j); +gtsam::Key A(size_t j); +gtsam::Key B(size_t j); +gtsam::Key C(size_t j); +gtsam::Key D(size_t j); +gtsam::Key E(size_t j); +gtsam::Key F(size_t j); +gtsam::Key G(size_t j); +gtsam::Key H(size_t j); +gtsam::Key I(size_t j); +gtsam::Key J(size_t j); +gtsam::Key K(size_t j); +gtsam::Key L(size_t j); +gtsam::Key M(size_t j); +gtsam::Key N(size_t j); +gtsam::Key O(size_t j); +gtsam::Key P(size_t j); +gtsam::Key Q(size_t j); +gtsam::Key R(size_t j); +gtsam::Key S(size_t j); +gtsam::Key T(size_t j); +gtsam::Key U(size_t j); +gtsam::Key V(size_t j); +gtsam::Key W(size_t j); +gtsam::Key X(size_t j); +gtsam::Key Y(size_t j); +gtsam::Key Z(size_t j); } // namespace symbol_shorthand #include class LabeledSymbol { - LabeledSymbol(size_t full_key); + LabeledSymbol(gtsam::Key full_key); LabeledSymbol(const gtsam::LabeledSymbol& key); LabeledSymbol(unsigned char valType, unsigned char label, size_t j); - size_t key() const; + gtsam::Key key() const; unsigned char label() const; unsigned char chr() const; size_t index() const; @@ -91,10 +91,10 @@ class LabeledSymbol { void print(string s = "") const; }; -size_t mrsymbol(unsigned char c, unsigned char label, size_t j); -unsigned char mrsymbolChr(size_t key); -unsigned char mrsymbolLabel(size_t key); -size_t mrsymbolIndex(size_t key); +gtsam::Key mrsymbol(unsigned char c, unsigned char label, size_t j); +unsigned char mrsymbolChr(gtsam::Key key); +unsigned char mrsymbolLabel(gtsam::Key key); +size_t mrsymbolIndex(gtsam::Key key); #include class Ordering { @@ -149,7 +149,7 @@ class Ordering { // Standard interface size_t size() const; size_t at(size_t i) const; - void push_back(size_t key); + void push_back(gtsam::Key key); // enabling serialization functionality void serialize() const; diff --git a/gtsam/linear/linear.i b/gtsam/linear/linear.i index cd5000d9f3..770ff0c53e 100644 --- a/gtsam/linear/linear.i +++ b/gtsam/linear/linear.i @@ -278,13 +278,13 @@ virtual class GaussianFactor : gtsam::Factor { virtual class JacobianFactor : gtsam::GaussianFactor { //Constructors JacobianFactor(); - JacobianFactor(Vector b_in); - JacobianFactor(size_t i1, Matrix A1, Vector b, + JacobianFactor(gtsam::Vector b_in); + JacobianFactor(gtsam::Key i1, gtsam::Matrix A1, gtsam::Vector b, const gtsam::noiseModel::Diagonal* model); - JacobianFactor(size_t i1, Matrix A1, size_t i2, Matrix A2, Vector b, + JacobianFactor(gtsam::Key i1, gtsam::Matrix A1, gtsam::Key i2, gtsam::Matrix A2, gtsam::Vector b, const gtsam::noiseModel::Diagonal* model); - JacobianFactor(size_t i1, Matrix A1, size_t i2, Matrix A2, size_t i3, Matrix A3, - Vector b, const gtsam::noiseModel::Diagonal* model); + JacobianFactor(gtsam::Key i1, gtsam::Matrix A1, gtsam::Key i2, gtsam::Matrix A2, gtsam::Key i3, gtsam::Matrix A3, + gtsam::Vector b, const gtsam::noiseModel::Diagonal* model); JacobianFactor(const gtsam::GaussianFactorGraph& graph); JacobianFactor(const gtsam::GaussianFactorGraph& graph, const gtsam::VariableSlots& p_variableSlots); @@ -330,12 +330,12 @@ virtual class HessianFactor : gtsam::GaussianFactor { //Constructors HessianFactor(); HessianFactor(const gtsam::GaussianFactor& factor); - HessianFactor(size_t j, Matrix G, Vector g, double f); - HessianFactor(size_t j, Vector mu, Matrix Sigma); - HessianFactor(size_t j1, size_t j2, Matrix G11, Matrix G12, Vector g1, Matrix G22, - Vector g2, double f); - HessianFactor(size_t j1, size_t j2, size_t j3, Matrix G11, Matrix G12, Matrix G13, - Vector g1, Matrix G22, Matrix G23, Vector g2, Matrix G33, Vector g3, + HessianFactor(gtsam::Key j, gtsam::Matrix G, gtsam::Vector g, double f); + HessianFactor(gtsam::Key j, gtsam::Vector mu, gtsam::Matrix Sigma); + HessianFactor(gtsam::Key j1, gtsam::Key j2, gtsam::Matrix G11, gtsam::Matrix G12, gtsam::Vector g1, gtsam::Matrix G22, + gtsam::Vector g2, double f); + HessianFactor(gtsam::Key j1, gtsam::Key j2, gtsam::Key j3, gtsam::Matrix G11, gtsam::Matrix G12, gtsam::Matrix G13, + gtsam::Vector g1, gtsam::Matrix G22, gtsam::Matrix G23, gtsam::Vector g2, gtsam::Matrix G33, gtsam::Vector g3, double f); HessianFactor(const gtsam::GaussianFactorGraph& factors); @@ -378,12 +378,12 @@ class GaussianFactorGraph { void push_back(const gtsam::GaussianBayesNet& bayesNet); void push_back(const gtsam::GaussianBayesTree& bayesTree); void add(const gtsam::GaussianFactor& factor); - void add(Vector b); - void add(size_t key1, Matrix A1, Vector b, const gtsam::noiseModel::Diagonal* model); - void add(size_t key1, Matrix A1, size_t key2, Matrix A2, Vector b, + void add(gtsam::Vector b); + void add(gtsam::Key key1, gtsam::Matrix A1, gtsam::Vector b, const gtsam::noiseModel::Diagonal* model); + void add(gtsam::Key key1, gtsam::Matrix A1, gtsam::Key key2, gtsam::Matrix A2, gtsam::Vector b, const gtsam::noiseModel::Diagonal* model); - void add(size_t key1, Matrix A1, size_t key2, Matrix A2, size_t key3, Matrix A3, - Vector b, const gtsam::noiseModel::Diagonal* model); + void add(gtsam::Key key1, gtsam::Matrix A1, gtsam::Key key2, gtsam::Matrix A2, gtsam::Key key3, gtsam::Matrix A3, + gtsam::Vector b, const gtsam::noiseModel::Diagonal* model); // error and probability double error(const gtsam::VectorValues& c) const; @@ -451,19 +451,24 @@ class GaussianFactorGraph { #include virtual class GaussianConditional : gtsam::JacobianFactor { // Constructors - GaussianConditional(size_t key, Vector d, Matrix R, + GaussianConditional(gtsam::Key key, gtsam::Vector d, gtsam::Matrix R, const gtsam::noiseModel::Diagonal* sigmas); - GaussianConditional(size_t key, Vector d, Matrix R, size_t name1, Matrix S, + GaussianConditional(gtsam::Key key, gtsam::Vector d, gtsam::Matrix R, gtsam::Key name1, gtsam::Matrix S, const gtsam::noiseModel::Diagonal* sigmas); - GaussianConditional(size_t key, Vector d, Matrix R, size_t name1, Matrix S, - size_t name2, Matrix T, + GaussianConditional(gtsam::Key key, gtsam::Vector d, gtsam::Matrix R, gtsam::Key name1, gtsam::Matrix S, + gtsam::Key name2, gtsam::Matrix T, + const gtsam::noiseModel::Diagonal* sigmas); + GaussianConditional(const std::vector> terms, + size_t nrFrontals, gtsam::Vector d, const gtsam::noiseModel::Diagonal* sigmas); // Constructors with no noise model - GaussianConditional(size_t key, Vector d, Matrix R); - GaussianConditional(size_t key, Vector d, Matrix R, size_t name1, Matrix S); - GaussianConditional(size_t key, Vector d, Matrix R, size_t name1, Matrix S, - size_t name2, Matrix T); + GaussianConditional(gtsam::Key key, gtsam::Vector d, gtsam::Matrix R); + GaussianConditional(gtsam::Key key, gtsam::Vector d, gtsam::Matrix R, gtsam::Key name1, gtsam::Matrix S); + GaussianConditional(gtsam::Key key, gtsam::Vector d, gtsam::Matrix R, gtsam::Key name1, gtsam::Matrix S, + gtsam::Key name2, gtsam::Matrix T); + GaussianConditional(const gtsam::KeyVector& keys, size_t nrFrontals, + const gtsam::VerticalBlockMatrix& augmentedMatrix); // Named constructors static gtsam::GaussianConditional FromMeanAndStddev(gtsam::Key key, @@ -621,16 +626,22 @@ virtual class GaussianBayesTree { double error(const gtsam::VectorValues& x) const; double determinant() const; double logDeterminant() const; - Matrix marginalCovariance(size_t key) const; - gtsam::GaussianConditional* marginalFactor(size_t key) const; - gtsam::GaussianFactorGraph* joint(size_t key1, size_t key2) const; - gtsam::GaussianBayesNet* jointBayesNet(size_t key1, size_t key2) const; + gtsam::Matrix marginalCovariance(gtsam::Key key) const; + gtsam::GaussianConditional* marginalFactor(gtsam::Key key) const; + gtsam::GaussianFactorGraph* joint(gtsam::Key key1, gtsam::Key key2) const; + gtsam::GaussianBayesNet* jointBayesNet(gtsam::Key key1, gtsam::Key key2) const; }; #include class GaussianISAM { //Constructor GaussianISAM(); + GaussianISAM(const gtsam::GaussianBayesTree& bayesTree); + + gtsam::VectorValues optimize() const; + gtsam::VectorValues optimizeGradientSearch() const; + + gtsam::GaussianConditional* marginalFactor(gtsam::Key key) const; //Standard Interface void update(const gtsam::GaussianFactorGraph& newFactors); diff --git a/gtsam/navigation/navigation.i b/gtsam/navigation/navigation.i index 7bbef9fc5a..5f8ea697f9 100644 --- a/gtsam/navigation/navigation.i +++ b/gtsam/navigation/navigation.i @@ -139,8 +139,8 @@ class PreintegratedImuMeasurements { }; virtual class ImuFactor: gtsam::NonlinearFactor { - ImuFactor(size_t pose_i, size_t vel_i, size_t pose_j, size_t vel_j, - size_t bias, + ImuFactor(gtsam::Key pose_i, gtsam::Key vel_i, gtsam::Key pose_j, gtsam::Key vel_j, + gtsam::Key bias, const gtsam::PreintegratedImuMeasurements& preintegratedMeasurements); // Standard Interface @@ -200,9 +200,9 @@ class PreintegratedCombinedMeasurements { const gtsam::imuBias::ConstantBias& bias) const; }; -virtual class CombinedImuFactor: gtsam::NonlinearFactor { - CombinedImuFactor(size_t pose_i, size_t vel_i, size_t pose_j, size_t vel_j, - size_t bias_i, size_t bias_j, +virtual class CombinedImuFactor: gtsam::NoiseModelFactor { + CombinedImuFactor(gtsam::Key pose_i, gtsam::Key vel_i, gtsam::Key pose_j, gtsam::Key vel_j, + gtsam::Key bias_i, gtsam::Key bias_j, const gtsam::PreintegratedCombinedMeasurements& CombinedPreintegratedMeasurements); // Standard Interface @@ -240,9 +240,9 @@ class PreintegratedAhrsMeasurements { }; virtual class AHRSFactor : gtsam::NonlinearFactor { - AHRSFactor(size_t rot_i, size_t rot_j,size_t bias, + AHRSFactor(gtsam::Key rot_i, gtsam::Key rot_j, gtsam::Key bias, const gtsam::PreintegratedAhrsMeasurements& preintegratedMeasurements, Vector omegaCoriolis); - AHRSFactor(size_t rot_i, size_t rot_j, size_t bias, + AHRSFactor(gtsam::Key rot_i, gtsam::Key rot_j, gtsam::Key bias, const gtsam::PreintegratedAhrsMeasurements& preintegratedMeasurements, Vector omegaCoriolis, const gtsam::Pose3& body_P_sensor); @@ -272,11 +272,11 @@ virtual class Rot3AttitudeFactor : gtsam::NonlinearFactor{ gtsam::Unit3 bRef() const; }; -virtual class Pose3AttitudeFactor : gtsam::NonlinearFactor { - Pose3AttitudeFactor(size_t key, const gtsam::Unit3& nZ, +virtual class Pose3AttitudeFactor : gtsam::NoiseModelFactor { + Pose3AttitudeFactor(gtsam::Key key, const gtsam::Unit3& nRef, const gtsam::noiseModel::Diagonal* model, - const gtsam::Unit3& bRef); - Pose3AttitudeFactor(size_t key, const gtsam::Unit3& nZ, + const gtsam::Unit3& bMeasured); + Pose3AttitudeFactor(gtsam::Key key, const gtsam::Unit3& nRef, const gtsam::noiseModel::Diagonal* model); Pose3AttitudeFactor(); void print(string s = "", const gtsam::KeyFormatter& keyFormatter = @@ -288,7 +288,7 @@ virtual class Pose3AttitudeFactor : gtsam::NonlinearFactor { #include virtual class GPSFactor : gtsam::NonlinearFactor{ - GPSFactor(size_t key, const gtsam::Point3& gpsIn, + GPSFactor(gtsam::Key key, const gtsam::Point3& gpsIn, const gtsam::noiseModel::Base* model); // Testable @@ -301,7 +301,7 @@ virtual class GPSFactor : gtsam::NonlinearFactor{ }; virtual class GPSFactor2 : gtsam::NonlinearFactor { - GPSFactor2(size_t key, const gtsam::Point3& gpsIn, + GPSFactor2(gtsam::Key key, const gtsam::Point3& gpsIn, const gtsam::noiseModel::Base* model); // Testable diff --git a/gtsam/nonlinear/nonlinear.i b/gtsam/nonlinear/nonlinear.i index 2fea0f9fd3..652eb8b856 100644 --- a/gtsam/nonlinear/nonlinear.i +++ b/gtsam/nonlinear/nonlinear.i @@ -83,7 +83,7 @@ class NonlinearFactorGraph { gtsam::PinholeCamera, gtsam::PinholeCamera, gtsam::imuBias::ConstantBias}> - void addPrior(size_t key, const T& prior, + void addPrior(gtsam::Key key, const T& prior, const gtsam::noiseModel::Base* noiseModel); // NonlinearFactorGraph @@ -757,9 +757,9 @@ class ISAM2 { gtsam::PinholeCamera, gtsam::PinholeCamera, gtsam::PinholeCamera, - gtsam::PinholeCamera, Vector, Matrix}> - VALUE calculateEstimate(size_t key) const; - Matrix marginalCovariance(size_t key) const; + gtsam::PinholeCamera, gtsam::Vector, gtsam::Matrix}> + VALUE calculateEstimate(gtsam::Key key) const; + gtsam::Matrix marginalCovariance(gtsam::Key key) const; gtsam::Values calculateBestEstimate() const; gtsam::VectorValues getDelta() const; double error(const gtsam::VectorValues& x) const; @@ -787,7 +787,7 @@ class NonlinearISAM { void printStats() const; void saveGraph(string s) const; gtsam::Values estimate() const; - Matrix marginalCovariance(size_t key) const; + gtsam::Matrix marginalCovariance(gtsam::Key key) const; int reorderInterval() const; int reorderCounter() const; void update(const gtsam::NonlinearFactorGraph& newFactors, @@ -830,7 +830,7 @@ template , gtsam::imuBias::ConstantBias}> virtual class PriorFactor : gtsam::NoiseModelFactor { - PriorFactor(size_t key, const T& prior, + PriorFactor(gtsam::Key key, const T& prior, const gtsam::noiseModel::Base* noiseModel); T prior() const; diff --git a/gtsam/sam/sam.i b/gtsam/sam/sam.i index a46a6de9e4..40b5502295 100644 --- a/gtsam/sam/sam.i +++ b/gtsam/sam/sam.i @@ -15,7 +15,7 @@ namespace gtsam { #include template virtual class RangeFactor : gtsam::NoiseModelFactor { - RangeFactor(size_t key1, size_t key2, double measured, + RangeFactor(gtsam::Key key1, gtsam::Key key2, double measured, const gtsam::noiseModel::Base* noiseModel); // enabling serialization functionality @@ -50,7 +50,7 @@ typedef gtsam::RangeFactor, #include template virtual class RangeFactorWithTransform : gtsam::NoiseModelFactor { - RangeFactorWithTransform(size_t key1, size_t key2, double measured, + RangeFactorWithTransform(gtsam::Key key1, gtsam::Key key2, double measured, const gtsam::noiseModel::Base* noiseModel, const POSE& body_T_sensor); @@ -73,7 +73,7 @@ typedef gtsam::RangeFactorWithTransform #include template virtual class BearingFactor : gtsam::NoiseModelFactor { - BearingFactor(size_t key1, size_t key2, const BEARING& measured, + BearingFactor(gtsam::Key key1, gtsam::Key key2, const BEARING& measured, const gtsam::noiseModel::Base* noiseModel); // enabling serialization functionality @@ -92,7 +92,7 @@ typedef gtsam::BearingFactor #include template virtual class BearingRangeFactor : gtsam::NoiseModelFactor { - BearingRangeFactor(size_t poseKey, size_t pointKey, + BearingRangeFactor(gtsam::Key poseKey, gtsam::Key pointKey, const BEARING& measuredBearing, const RANGE& measuredRange, const gtsam::noiseModel::Base* noiseModel); diff --git a/gtsam/sfm/sfm.i b/gtsam/sfm/sfm.i index c57e03174f..4fc2a2e412 100644 --- a/gtsam/sfm/sfm.i +++ b/gtsam/sfm/sfm.i @@ -79,8 +79,8 @@ gtsam::Values initialCamerasAndPointsEstimate(const gtsam::SfmData& db); #include virtual class ShonanFactor3 : gtsam::NoiseModelFactor { - ShonanFactor3(size_t key1, size_t key2, const gtsam::Rot3& R12, size_t p); - ShonanFactor3(size_t key1, size_t key2, const gtsam::Rot3& R12, size_t p, + ShonanFactor3(gtsam::Key key1, gtsam::Key key2, const gtsam::Rot3& R12, size_t p); + ShonanFactor3(gtsam::Key key1, gtsam::Key key2, const gtsam::Rot3& R12, size_t p, gtsam::noiseModel::Base* model); Vector evaluateError(const gtsam::SOn& Q1, const gtsam::SOn& Q2); }; @@ -88,10 +88,10 @@ virtual class ShonanFactor3 : gtsam::NoiseModelFactor { #include template class BinaryMeasurement { - BinaryMeasurement(size_t key1, size_t key2, const T& measured, + BinaryMeasurement(gtsam::Key key1, gtsam::Key key2, const T& measured, const gtsam::noiseModel::Base* model); - size_t key1() const; - size_t key2() const; + gtsam::Key key1() const; + gtsam::Key key2() const; T measured() const; gtsam::noiseModel::Base* noiseModel() const; }; @@ -274,7 +274,7 @@ class KeyPairDoubleMap { size_t size() const; bool empty() const; void clear(); - size_t at(const pair& keypair) const; + size_t at(const pair& keypair) const; }; class MFAS { diff --git a/gtsam/slam/slam.i b/gtsam/slam/slam.i index 8e1e06d5b4..ce72405655 100644 --- a/gtsam/slam/slam.i +++ b/gtsam/slam/slam.i @@ -15,7 +15,7 @@ template virtual class BetweenFactor : gtsam::NoiseModelFactor { - BetweenFactor(size_t key1, size_t key2, const T& relativePose, + BetweenFactor(gtsam::Key key1, gtsam::Key key2, const T& relativePose, const gtsam::noiseModel::Base* noiseModel); T measured() const; @@ -28,20 +28,20 @@ template virtual class GenericProjectionFactor : gtsam::NoiseModelFactor { GenericProjectionFactor(const gtsam::Point2& measured, const gtsam::noiseModel::Base* noiseModel, - size_t poseKey, size_t pointKey, + gtsam::Key poseKey, gtsam::Key pointKey, const CALIBRATION* k); GenericProjectionFactor(const gtsam::Point2& measured, const gtsam::noiseModel::Base* noiseModel, - size_t poseKey, size_t pointKey, const CALIBRATION* k, + gtsam::Key poseKey, gtsam::Key pointKey, const CALIBRATION* k, const POSE& body_P_sensor); GenericProjectionFactor(const gtsam::Point2& measured, const gtsam::noiseModel::Base* noiseModel, - size_t poseKey, size_t pointKey, const CALIBRATION* k, + gtsam::Key poseKey, gtsam::Key pointKey, const CALIBRATION* k, bool throwCheirality, bool verboseCheirality); GenericProjectionFactor(const gtsam::Point2& measured, const gtsam::noiseModel::Base* noiseModel, - size_t poseKey, size_t pointKey, const CALIBRATION* k, + gtsam::Key poseKey, gtsam::Key pointKey, const CALIBRATION* k, bool throwCheirality, bool verboseCheirality, const POSE& body_P_sensor); @@ -70,8 +70,8 @@ typedef gtsam::GenericProjectionFactor virtual class GeneralSFMFactor : gtsam::NoiseModelFactor { GeneralSFMFactor(const gtsam::Point2& measured, - const gtsam::noiseModel::Base* model, size_t cameraKey, - size_t landmarkKey); + const gtsam::noiseModel::Base* model, gtsam::Key cameraKey, + gtsam::Key landmarkKey); gtsam::Point2 measured() const; }; typedef gtsam::GeneralSFMFactor, @@ -110,8 +110,8 @@ template virtual class GeneralSFMFactor2 : gtsam::NoiseModelFactor { GeneralSFMFactor2(const gtsam::Point2& measured, - const gtsam::noiseModel::Base* model, size_t poseKey, - size_t landmarkKey, size_t calibKey); + const gtsam::noiseModel::Base* model, gtsam::Key poseKey, + gtsam::Key landmarkKey, gtsam::Key calibKey); gtsam::Point2 measured() const; // enabling serialization functionality @@ -152,7 +152,7 @@ virtual class SmartProjectionPoseFactor : gtsam::NonlinearFactor { const gtsam::Pose3& body_P_sensor, const gtsam::SmartProjectionParams& params); - void add(const gtsam::Point2& measured_i, size_t poseKey_i); + void add(const gtsam::Point2& measured_i, gtsam::Key poseKey_i); // enabling serialization functionality void serialize() const; @@ -165,8 +165,9 @@ typedef gtsam::SmartProjectionPoseFactor template virtual class GenericStereoFactor : gtsam::NoiseModelFactor { GenericStereoFactor(const gtsam::StereoPoint2& measured, - const gtsam::noiseModel::Base* noiseModel, size_t poseKey, - size_t landmarkKey, const gtsam::Cal3_S2Stereo* K); + const gtsam::noiseModel::Base* noiseModel, gtsam::Key poseKey, + gtsam::Key landmarkKey, const gtsam::Cal3_S2Stereo* K); + gtsam::StereoPoint2 measured() const; gtsam::Cal3_S2Stereo* calibration() const; @@ -179,8 +180,8 @@ typedef gtsam::GenericStereoFactor #include template virtual class PoseTranslationPrior : gtsam::NoiseModelFactor { - PoseTranslationPrior(size_t key, const POSE& pose_z, - const gtsam::noiseModel::Base* noiseModel); + PoseTranslationPrior(gtsam::Key key, const POSE& pose_z, + const gtsam::noiseModel::Base* model); POSE::Translation measured() const; // enabling serialization functionality @@ -193,8 +194,8 @@ typedef gtsam::PoseTranslationPrior PoseTranslationPrior3D; #include template virtual class PoseRotationPrior : gtsam::NoiseModelFactor { - PoseRotationPrior(size_t key, const POSE& pose_z, - const gtsam::noiseModel::Base* noiseModel); + PoseRotationPrior(gtsam::Key key, const POSE& pose_z, + const gtsam::noiseModel::Base* model); POSE::Rotation measured() const; }; @@ -203,7 +204,7 @@ typedef gtsam::PoseRotationPrior PoseRotationPrior3D; #include virtual class EssentialMatrixFactor : gtsam::NoiseModelFactor { - EssentialMatrixFactor(size_t key, const gtsam::Point2& pA, + EssentialMatrixFactor(gtsam::Key key, const gtsam::Point2& pA, const gtsam::Point2& pB, const gtsam::noiseModel::Base* noiseModel); void print(string s = "", const gtsam::KeyFormatter& keyFormatter = @@ -214,7 +215,7 @@ virtual class EssentialMatrixFactor : gtsam::NoiseModelFactor { #include virtual class EssentialMatrixConstraint : gtsam::NoiseModelFactor { - EssentialMatrixConstraint(size_t key1, size_t key2, const gtsam::EssentialMatrix &measuredE, + EssentialMatrixConstraint(gtsam::Key key1, gtsam::Key key2, const gtsam::EssentialMatrix &measuredE, const gtsam::noiseModel::Base *model); void print(string s = "", const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const; @@ -314,16 +315,16 @@ gtsam::noiseModel::Isotropic* ConvertNoiseModel(gtsam::noiseModel::Base* model, template virtual class FrobeniusFactor : gtsam::NoiseModelFactor { - FrobeniusFactor(size_t key1, size_t key2); - FrobeniusFactor(size_t key1, size_t key2, gtsam::noiseModel::Base* model); + FrobeniusFactor(gtsam::Key key1, gtsam::Key key2); + FrobeniusFactor(gtsam::Key j1, gtsam::Key j2, gtsam::noiseModel::Base* model); Vector evaluateError(const T& R1, const T& R2); }; template virtual class FrobeniusBetweenFactor : gtsam::NoiseModelFactor { - FrobeniusBetweenFactor(size_t key1, size_t key2, const T& R12); - FrobeniusBetweenFactor(size_t key1, size_t key2, const T& R12, + FrobeniusBetweenFactor(gtsam::Key j1, gtsam::Key j2, const T& T12); + FrobeniusBetweenFactor(gtsam::Key key1, gtsam::Key key2, const T& T12, gtsam::noiseModel::Base* model); Vector evaluateError(const T& R1, const T& R2); diff --git a/gtsam/symbolic/symbolic.i b/gtsam/symbolic/symbolic.i index 4da59dfa97..600f8a5872 100644 --- a/gtsam/symbolic/symbolic.i +++ b/gtsam/symbolic/symbolic.i @@ -8,14 +8,14 @@ virtual class SymbolicFactor : gtsam::Factor { // Standard Constructors and Named Constructors SymbolicFactor(const gtsam::SymbolicFactor& f); SymbolicFactor(); - SymbolicFactor(size_t j); - SymbolicFactor(size_t j1, size_t j2); - SymbolicFactor(size_t j1, size_t j2, size_t j3); - SymbolicFactor(size_t j1, size_t j2, size_t j3, size_t j4); - SymbolicFactor(size_t j1, size_t j2, size_t j3, size_t j4, size_t j5); - SymbolicFactor(size_t j1, size_t j2, size_t j3, size_t j4, size_t j5, - size_t j6); - static gtsam::SymbolicFactor FromKeys(const gtsam::KeyVector& js); + SymbolicFactor(gtsam::Key j); + SymbolicFactor(gtsam::Key j1, gtsam::Key j2); + SymbolicFactor(gtsam::Key j1, gtsam::Key j2, gtsam::Key j3); + SymbolicFactor(gtsam::Key j1, gtsam::Key j2, gtsam::Key j3, gtsam::Key j4); + SymbolicFactor(gtsam::Key j1, gtsam::Key j2, gtsam::Key j3, gtsam::Key j4, gtsam::Key j5); + SymbolicFactor(gtsam::Key j1, gtsam::Key j2, gtsam::Key j3, gtsam::Key j4, gtsam::Key j5, + gtsam::Key j6); + static gtsam::SymbolicFactor FromKeys(const gtsam::KeyVector& keys); // From Factor void print(string s = "SymbolicFactor", @@ -46,10 +46,10 @@ virtual class SymbolicFactorGraph { void push_back(const gtsam::SymbolicBayesTree& bayesTree); // Advanced Interface - void push_factor(size_t key); - void push_factor(size_t key1, size_t key2); - void push_factor(size_t key1, size_t key2, size_t key3); - void push_factor(size_t key1, size_t key2, size_t key3, size_t key4); + void push_factor(gtsam::Key key); + void push_factor(gtsam::Key key1, gtsam::Key key2); + void push_factor(gtsam::Key key1, gtsam::Key key2, gtsam::Key key3); + void push_factor(gtsam::Key key1, gtsam::Key key2, gtsam::Key key3, gtsam::Key key4); gtsam::SymbolicBayesNet* eliminateSequential(); gtsam::SymbolicBayesNet* eliminateSequential(const gtsam::Ordering& ordering); @@ -90,11 +90,11 @@ virtual class SymbolicConditional : gtsam::SymbolicFactor { // Standard Constructors and Named Constructors SymbolicConditional(); SymbolicConditional(const gtsam::SymbolicConditional& other); - SymbolicConditional(size_t key); - SymbolicConditional(size_t key, size_t parent); - SymbolicConditional(size_t key, size_t parent1, size_t parent2); - SymbolicConditional(size_t key, size_t parent1, size_t parent2, - size_t parent3); + SymbolicConditional(gtsam::Key key); + SymbolicConditional(gtsam::Key key, gtsam::Key parent); + SymbolicConditional(gtsam::Key key, gtsam::Key parent1, gtsam::Key parent2); + SymbolicConditional(gtsam::Key key, gtsam::Key parent1, gtsam::Key parent2, + gtsam::Key parent3); static gtsam::SymbolicConditional FromKeys(const gtsam::KeyVector& keys, size_t nrFrontals); @@ -214,9 +214,9 @@ class SymbolicBayesTree { void deleteCachedShortcuts(); size_t numCachedSeparatorMarginals() const; - gtsam::SymbolicConditional* marginalFactor(size_t key) const; - gtsam::SymbolicFactorGraph* joint(size_t key1, size_t key2) const; - gtsam::SymbolicBayesNet* jointBayesNet(size_t key1, size_t key2) const; + gtsam::SymbolicConditional* marginalFactor(gtsam::Key key) const; + gtsam::SymbolicFactorGraph* joint(gtsam::Key key1, gtsam::Key key2) const; + gtsam::SymbolicBayesNet* jointBayesNet(gtsam::Key key1, gtsam::Key key2) const; string dot(const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const; diff --git a/gtsam_unstable/gtsam_unstable.i b/gtsam_unstable/gtsam_unstable.i index 6ce7be20ae..41dac05569 100644 --- a/gtsam_unstable/gtsam_unstable.i +++ b/gtsam_unstable/gtsam_unstable.i @@ -279,7 +279,7 @@ class SimPolygon2D { #include template virtual class PriorFactor : gtsam::NoiseModelFactor { - PriorFactor(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel); + PriorFactor(gtsam::Key key, const T& prior, const gtsam::noiseModel::Base* noiseModel); void serializable() const; // enabling serialization functionality }; @@ -287,7 +287,7 @@ virtual class PriorFactor : gtsam::NoiseModelFactor { #include template virtual class BetweenFactor : gtsam::NoiseModelFactor { - BetweenFactor(size_t key1, size_t key2, const T& relativePose, const gtsam::noiseModel::Base* noiseModel); + BetweenFactor(gtsam::Key key1, gtsam::Key key2, const T& relativePose, const gtsam::noiseModel::Base* noiseModel); void serializable() const; // enabling serialization functionality }; @@ -295,11 +295,11 @@ virtual class BetweenFactor : gtsam::NoiseModelFactor { #include template virtual class BetweenFactorEM : gtsam::NonlinearFactor { - BetweenFactorEM(size_t key1, size_t key2, const T& relativePose, + BetweenFactorEM(gtsam::Key key1, gtsam::Key key2, const T& relativePose, const gtsam::noiseModel::Gaussian* model_inlier, const gtsam::noiseModel::Gaussian* model_outlier, double prior_inlier, double prior_outlier); - BetweenFactorEM(size_t key1, size_t key2, const T& relativePose, + BetweenFactorEM(gtsam::Key key1, gtsam::Key key2, const T& relativePose, const gtsam::noiseModel::Gaussian* model_inlier, const gtsam::noiseModel::Gaussian* model_outlier, double prior_inlier, double prior_outlier, bool flag_bump_up_near_zero_probs); @@ -321,12 +321,12 @@ virtual class BetweenFactorEM : gtsam::NonlinearFactor { #include template virtual class TransformBtwRobotsUnaryFactorEM : gtsam::NonlinearFactor { - TransformBtwRobotsUnaryFactorEM(size_t key, const T& relativePose, size_t keyA, size_t keyB, + TransformBtwRobotsUnaryFactorEM(gtsam::Key key, const T& relativePose, gtsam::Key keyA, gtsam::Key keyB, const gtsam::Values& valA, const gtsam::Values& valB, const gtsam::noiseModel::Gaussian* model_inlier, const gtsam::noiseModel::Gaussian* model_outlier, double prior_inlier, double prior_outlier); - TransformBtwRobotsUnaryFactorEM(size_t key, const T& relativePose, size_t keyA, size_t keyB, + TransformBtwRobotsUnaryFactorEM(gtsam::Key key, const T& relativePose, gtsam::Key keyA, gtsam::Key keyB, const gtsam::Values& valA, const gtsam::Values& valB, const gtsam::noiseModel::Gaussian* model_inlier, const gtsam::noiseModel::Gaussian* model_outlier, double prior_inlier, double prior_outlier, bool flag_bump_up_near_zero_probs, bool start_with_M_step); @@ -347,7 +347,7 @@ virtual class TransformBtwRobotsUnaryFactorEM : gtsam::NonlinearFactor { #include template virtual class TransformBtwRobotsUnaryFactor : gtsam::NonlinearFactor { - TransformBtwRobotsUnaryFactor(size_t key, const T& relativePose, size_t keyA, size_t keyB, + TransformBtwRobotsUnaryFactor(gtsam::Key key, const T& relativePose, gtsam::Key keyA, gtsam::Key keyB, const gtsam::Values& valA, const gtsam::Values& valB, const gtsam::noiseModel::Gaussian* model); @@ -362,7 +362,7 @@ virtual class TransformBtwRobotsUnaryFactor : gtsam::NonlinearFactor { virtual class SmartRangeFactor : gtsam::NoiseModelFactor { SmartRangeFactor(double s); - void addRange(size_t key, double measuredRange); + void addRange(gtsam::Key key, double measuredRange); gtsam::Point2 triangulate(const gtsam::Values& x) const; //void print(string s) const; @@ -371,7 +371,7 @@ virtual class SmartRangeFactor : gtsam::NoiseModelFactor { #include template virtual class RangeFactor : gtsam::NoiseModelFactor { - RangeFactor(size_t key1, size_t key2, double measured, const gtsam::noiseModel::Base* noiseModel); + RangeFactor(gtsam::Key key1, gtsam::Key key2, double measured, const gtsam::noiseModel::Base* noiseModel); void serializable() const; // enabling serialization functionality }; @@ -398,18 +398,18 @@ class TimeOfArrival { #include virtual class TOAFactor : gtsam::NonlinearFactor { // For now, because of overload issues, we only expose constructor with known sensor coordinates: - TOAFactor(size_t key1, gtsam::Point3 sensor, double measured, + TOAFactor(gtsam::Key key1, gtsam::Point3 sensor, double measured, const gtsam::noiseModel::Base* noiseModel); - static void InsertEvent(size_t key, const gtsam::Event& event, gtsam::Values* values); + static void InsertEvent(gtsam::Key key, const gtsam::Event& event, gtsam::Values* values); }; #include template virtual class NonlinearEquality : gtsam::NoiseModelFactor { // Constructor - forces exact evaluation - NonlinearEquality(size_t j, const T& feasible); + NonlinearEquality(gtsam::Key j, const T& feasible); // Constructor - allows inexact evaluation - NonlinearEquality(size_t j, const T& feasible, double error_gain); + NonlinearEquality(gtsam::Key j, const T& feasible, double error_gain); void serializable() const; // enabling serialization functionality }; @@ -418,67 +418,67 @@ virtual class NonlinearEquality : gtsam::NoiseModelFactor { template virtual class IMUFactor : gtsam::NoiseModelFactor { /** Standard constructor */ - IMUFactor(Vector accel, Vector gyro, - double dt, size_t key1, size_t key2, const gtsam::noiseModel::Base* model); + IMUFactor(gtsam::Vector accel, gtsam::Vector gyro, + double dt, gtsam::Key key1, gtsam::Key key2, const gtsam::noiseModel::Base* model); /** Full IMU vector specification */ - IMUFactor(Vector imu_vector, - double dt, size_t key1, size_t key2, const gtsam::noiseModel::Base* model); + IMUFactor(gtsam::Vector imu_vector, + double dt, gtsam::Key key1, gtsam::Key key2, const gtsam::noiseModel::Base* model); Vector gyro() const; Vector accel() const; Vector z() const; template - size_t key() const; + gtsam::Key key() const; }; #include template virtual class FullIMUFactor : gtsam::NoiseModelFactor { /** Standard constructor */ - FullIMUFactor(Vector accel, Vector gyro, - double dt, size_t key1, size_t key2, const gtsam::noiseModel::Base* model); + FullIMUFactor(gtsam::Vector accel, gtsam::Vector gyro, + double dt, gtsam::Key key1, gtsam::Key key2, const gtsam::noiseModel::Base* model); /** Single IMU vector - imu = [accel, gyro] */ - FullIMUFactor(Vector imu, - double dt, size_t key1, size_t key2, const gtsam::noiseModel::Base* model); + FullIMUFactor(gtsam::Vector imu, + double dt, gtsam::Key key1, gtsam::Key key2, const gtsam::noiseModel::Base* model); Vector gyro() const; Vector accel() const; Vector z() const; template - size_t key() const; + gtsam::Key key() const; }; #include virtual class DHeightPrior : gtsam::NonlinearFactor { - DHeightPrior(size_t key, double height, const gtsam::noiseModel::Base* model); + DHeightPrior(gtsam::Key key, double height, const gtsam::noiseModel::Base* model); }; virtual class DRollPrior : gtsam::NonlinearFactor { /** allows for explicit roll parameterization - uses canonical coordinate */ - DRollPrior(size_t key, double wx, const gtsam::noiseModel::Base* model); + DRollPrior(gtsam::Key key, double wx, const gtsam::noiseModel::Base* model); /** Forces roll to zero */ - DRollPrior(size_t key, const gtsam::noiseModel::Base* model); + DRollPrior(gtsam::Key key, const gtsam::noiseModel::Base* model); }; virtual class VelocityPrior : gtsam::NonlinearFactor { - VelocityPrior(size_t key, Vector vel, const gtsam::noiseModel::Base* model); + VelocityPrior(gtsam::Key key, gtsam::Vector vel, const gtsam::noiseModel::Base* model); }; virtual class DGroundConstraint : gtsam::NonlinearFactor { // Primary constructor allows for variable height of the "floor" - DGroundConstraint(size_t key, double height, const gtsam::noiseModel::Base* model); + DGroundConstraint(gtsam::Key key, double height, const gtsam::noiseModel::Base* model); // Fully specify vector - use only for debugging - DGroundConstraint(size_t key, Vector constraint, const gtsam::noiseModel::Base* model); + DGroundConstraint(gtsam::Key key, gtsam::Vector constraint, const gtsam::noiseModel::Base* model); }; #include virtual class VelocityConstraint3 : gtsam::NonlinearFactor { /** Standard constructor */ - VelocityConstraint3(size_t key1, size_t key2, size_t velKey, double dt); + VelocityConstraint3(gtsam::Key key1, gtsam::Key key2, gtsam::Key velKey, double dt); Vector evaluateError(const double& x1, const double& x2, const double& v) const; }; @@ -486,45 +486,45 @@ virtual class VelocityConstraint3 : gtsam::NonlinearFactor { #include virtual class PendulumFactor1 : gtsam::NonlinearFactor { /** Standard constructor */ - PendulumFactor1(size_t k1, size_t k, size_t velKey, double dt); + PendulumFactor1(gtsam::Key k1, gtsam::Key k, gtsam::Key velKey, double dt); - Vector evaluateError(const double& qk1, const double& qk, const double& v) const; + gtsam::Vector evaluateError(const double& qk1, const double& qk, const double& v) const; }; #include virtual class PendulumFactor2 : gtsam::NonlinearFactor { /** Standard constructor */ - PendulumFactor2(size_t vk1, size_t vk, size_t qKey, double dt, double L, double g); + PendulumFactor2(gtsam::Key vk1, gtsam::Key vk, gtsam::Key qKey, double dt, double L, double g); - Vector evaluateError(const double& vk1, const double& vk, const double& q) const; + gtsam::Vector evaluateError(const double& vk1, const double& vk, const double& q) const; }; virtual class PendulumFactorPk : gtsam::NonlinearFactor { /** Standard constructor */ - PendulumFactorPk(size_t pk, size_t qk, size_t qk1, double h, double m, double r, double g, double alpha); + PendulumFactorPk(gtsam::Key pk, gtsam::Key qk, gtsam::Key qk1, double h, double m, double r, double g, double alpha); - Vector evaluateError(const double& pk, const double& qk, const double& qk1) const; + gtsam::Vector evaluateError(const double& pk, const double& qk, const double& qk1) const; }; virtual class PendulumFactorPk1 : gtsam::NonlinearFactor { /** Standard constructor */ - PendulumFactorPk1(size_t pk1, size_t qk, size_t qk1, double h, double m, double r, double g, double alpha); + PendulumFactorPk1(gtsam::Key pk1, gtsam::Key qk, gtsam::Key qk1, double h, double m, double r, double g, double alpha); - Vector evaluateError(const double& pk1, const double& qk, const double& qk1) const; + gtsam::Vector evaluateError(const double& pk1, const double& qk, const double& qk1) const; }; #include virtual class Reconstruction : gtsam::NoiseModelFactor { - Reconstruction(size_t gKey1, size_t gKey, size_t xiKey, double h); + Reconstruction(gtsam::Key gKey1, gtsam::Key gKey, gtsam::Key xiKey, double h); - Vector evaluateError(const gtsam::Pose3& gK1, const gtsam::Pose3& gK, Vector xiK) const; + gtsam::Vector evaluateError(const gtsam::Pose3& gK1, const gtsam::Pose3& gK, Vector xiK) const; }; virtual class DiscreteEulerPoincareHelicopter : gtsam::NoiseModelFactor { - DiscreteEulerPoincareHelicopter(size_t xiKey, size_t xiKey_1, size_t gKey, - double h, Matrix Inertia, Vector Fu, double m); + DiscreteEulerPoincareHelicopter(gtsam::Key xiKey, gtsam::Key xiKey_1, gtsam::Key gKey, + double h, gtsam::Matrix Inertia, gtsam::Vector Fu, double m); - Vector evaluateError(Vector xiK, Vector xiK_1, const gtsam::Pose3& gK) const; + gtsam::Vector evaluateError(gtsam::Vector xiK, gtsam::Vector xiK_1, const gtsam::Pose3& gK) const; }; //************************************************************************* @@ -532,7 +532,7 @@ virtual class DiscreteEulerPoincareHelicopter : gtsam::NoiseModelFactor { //************************************************************************* #include class FixedLagSmootherKeyTimestampMapValue { - FixedLagSmootherKeyTimestampMapValue(size_t key, double timestamp); + FixedLagSmootherKeyTimestampMapValue(gtsam::Key key, double timestamp); FixedLagSmootherKeyTimestampMapValue(const gtsam::FixedLagSmootherKeyTimestampMapValue& other); }; @@ -547,7 +547,7 @@ class FixedLagSmootherKeyTimestampMap { bool empty() const; void clear(); - double at(const size_t key) const; + double at(const gtsam::Key key) const; void insert(const gtsam::FixedLagSmootherKeyTimestampMapValue& value); }; @@ -674,7 +674,7 @@ virtual class ConcurrentBatchSmoother : gtsam::ConcurrentSmoother { #include virtual class RelativeElevationFactor: gtsam::NoiseModelFactor { RelativeElevationFactor(); - RelativeElevationFactor(size_t poseKey, size_t pointKey, double measured, + RelativeElevationFactor(gtsam::Key poseKey, gtsam::Key pointKey, double measured, const gtsam::noiseModel::Base* model); double measured() const; @@ -683,25 +683,25 @@ virtual class RelativeElevationFactor: gtsam::NoiseModelFactor { #include virtual class DummyFactor : gtsam::NonlinearFactor { - DummyFactor(size_t key1, size_t dim1, size_t key2, size_t dim2); + DummyFactor(gtsam::Key key1, size_t dim1, gtsam::Key key2, size_t dim2); }; #include virtual class InvDepthFactorVariant1 : gtsam::NoiseModelFactor { - InvDepthFactorVariant1(size_t poseKey, size_t landmarkKey, const gtsam::Point2& measured, const gtsam::Cal3_S2* K, const gtsam::noiseModel::Base* model); + InvDepthFactorVariant1(gtsam::Key poseKey, gtsam::Key landmarkKey, const gtsam::Point2& measured, const gtsam::Cal3_S2* K, const gtsam::noiseModel::Base* model); }; #include virtual class InvDepthFactorVariant2 : gtsam::NoiseModelFactor { - InvDepthFactorVariant2(size_t poseKey, size_t landmarkKey, const gtsam::Point2& measured, const gtsam::Cal3_S2* K, const gtsam::Point3& referencePoint, const gtsam::noiseModel::Base* model); + InvDepthFactorVariant2(gtsam::Key poseKey, gtsam::Key landmarkKey, const gtsam::Point2& measured, const gtsam::Cal3_S2* K, const gtsam::Point3& referencePoint, const gtsam::noiseModel::Base* model); }; #include virtual class InvDepthFactorVariant3a : gtsam::NoiseModelFactor { - InvDepthFactorVariant3a(size_t poseKey, size_t landmarkKey, const gtsam::Point2& measured, const gtsam::Cal3_S2* K, const gtsam::noiseModel::Base* model); + InvDepthFactorVariant3a(gtsam::Key poseKey, gtsam::Key landmarkKey, const gtsam::Point2& measured, const gtsam::Cal3_S2* K, const gtsam::noiseModel::Base* model); }; virtual class InvDepthFactorVariant3b : gtsam::NoiseModelFactor { - InvDepthFactorVariant3b(size_t poseKey1, size_t poseKey2, size_t landmarkKey, const gtsam::Point2& measured, const gtsam::Cal3_S2* K, const gtsam::noiseModel::Base* model); + InvDepthFactorVariant3b(gtsam::Key poseKey1, gtsam::Key poseKey2, gtsam::Key landmarkKey, const gtsam::Point2& measured, const gtsam::Cal3_S2* K, const gtsam::noiseModel::Base* model); }; @@ -735,7 +735,7 @@ class AHRS { #include //typedef gtsam::NoiseModelFactorN NLPosePose; virtual class DeltaFactor : gtsam::NoiseModelFactor { - DeltaFactor(size_t i, size_t j, const gtsam::Point2& measured, + DeltaFactor(gtsam::Key i, gtsam::Key j, const gtsam::Point2& measured, const gtsam::noiseModel::Base* noiseModel); //void print(string s) const; }; @@ -743,7 +743,7 @@ virtual class DeltaFactor : gtsam::NoiseModelFactor { //typedef gtsam::NoiseModelFactorN NLPosePosePosePoint; virtual class DeltaFactorBase : gtsam::NoiseModelFactor { - DeltaFactorBase(size_t b1, size_t i, size_t b2, size_t j, + DeltaFactorBase(gtsam::Key b1, gtsam::Key i, gtsam::Key b2, gtsam::Key j, const gtsam::Point2& measured, const gtsam::noiseModel::Base* noiseModel); //void print(string s) const; }; @@ -751,7 +751,7 @@ virtual class DeltaFactorBase : gtsam::NoiseModelFactor { //typedef gtsam::NoiseModelFactorN NLPosePosePosePose; virtual class OdometryFactorBase : gtsam::NoiseModelFactor { - OdometryFactorBase(size_t b1, size_t i, size_t b2, size_t j, + OdometryFactorBase(gtsam::Key b1, gtsam::Key i, gtsam::Key b2, gtsam::Key j, const gtsam::Pose2& measured, const gtsam::noiseModel::Base* noiseModel); //void print(string s) const; }; @@ -761,10 +761,10 @@ virtual class OdometryFactorBase : gtsam::NoiseModelFactor { template virtual class ProjectionFactorPPP : gtsam::NoiseModelFactor { ProjectionFactorPPP(const gtsam::Point2& measured, const gtsam::noiseModel::Base* noiseModel, - size_t poseKey, size_t transformKey, size_t pointKey, const CALIBRATION* k); + gtsam::Key poseKey, gtsam::Key transformKey, gtsam::Key pointKey, const CALIBRATION* k); ProjectionFactorPPP(const gtsam::Point2& measured, const gtsam::noiseModel::Base* noiseModel, - size_t poseKey, size_t transformKey, size_t pointKey, const CALIBRATION* k, bool throwCheirality, bool verboseCheirality); + gtsam::Key poseKey, gtsam::Key transformKey, gtsam::Key pointKey, const CALIBRATION* k, bool throwCheirality, bool verboseCheirality); gtsam::Point2 measured() const; CALIBRATION* calibration() const; @@ -782,10 +782,10 @@ typedef gtsam::ProjectionFactorPPP template virtual class ProjectionFactorPPPC : gtsam::NoiseModelFactor { ProjectionFactorPPPC(const gtsam::Point2& measured, const gtsam::noiseModel::Base* noiseModel, - size_t poseKey, size_t transformKey, size_t pointKey, size_t calibKey); + gtsam::Key poseKey, gtsam::Key transformKey, gtsam::Key pointKey, gtsam::Key calibKey); ProjectionFactorPPPC(const gtsam::Point2& measured, const gtsam::noiseModel::Base* noiseModel, - size_t poseKey, size_t transformKey, size_t pointKey, size_t calibKey, bool throwCheirality, bool verboseCheirality); + gtsam::Key poseKey, gtsam::Key transformKey, gtsam::Key pointKey, gtsam::Key calibKey, bool throwCheirality, bool verboseCheirality); gtsam::Point2 measured() const; bool verboseCheirality() const; @@ -800,17 +800,17 @@ typedef gtsam::ProjectionFactorPPPC #include virtual class ProjectionFactorRollingShutter : gtsam::NoiseModelFactor { ProjectionFactorRollingShutter(const gtsam::Point2& measured, double alpha, const gtsam::noiseModel::Base* noiseModel, - size_t poseKey_a, size_t poseKey_b, size_t pointKey, const gtsam::Cal3_S2* K); + gtsam::Key poseKey_a, gtsam::Key poseKey_b, gtsam::Key pointKey, const gtsam::Cal3_S2* K); ProjectionFactorRollingShutter(const gtsam::Point2& measured, double alpha, const gtsam::noiseModel::Base* noiseModel, - size_t poseKey_a, size_t poseKey_b, size_t pointKey, const gtsam::Cal3_S2* K, gtsam::Pose3& body_P_sensor); + gtsam::Key poseKey_a, gtsam::Key poseKey_b, gtsam::Key pointKey, const gtsam::Cal3_S2* K, gtsam::Pose3& body_P_sensor); ProjectionFactorRollingShutter(const gtsam::Point2& measured, double alpha, const gtsam::noiseModel::Base* noiseModel, - size_t poseKey_a, size_t poseKey_b, size_t pointKey, const gtsam::Cal3_S2* K, bool throwCheirality, + gtsam::Key poseKey_a, gtsam::Key poseKey_b, gtsam::Key pointKey, const gtsam::Cal3_S2* K, bool throwCheirality, bool verboseCheirality); ProjectionFactorRollingShutter(const gtsam::Point2& measured, double alpha, const gtsam::noiseModel::Base* noiseModel, - size_t poseKey_a, size_t poseKey_b, size_t pointKey, const gtsam::Cal3_S2* K, bool throwCheirality, + gtsam::Key poseKey_a, gtsam::Key poseKey_b, gtsam::Key pointKey, const gtsam::Cal3_S2* K, bool throwCheirality, bool verboseCheirality, gtsam::Pose3& body_P_sensor); gtsam::Point2 measured() const; diff --git a/python/gtsam/tests/test_Cal3Fisheye.py b/python/gtsam/tests/test_Cal3Fisheye.py index e54afc7574..857c70836f 100644 --- a/python/gtsam/tests/test_Cal3Fisheye.py +++ b/python/gtsam/tests/test_Cal3Fisheye.py @@ -91,8 +91,8 @@ def test_generic_factor(self): noise_model = gtsam.noiseModel.Isotropic.Sigma(2, 1) pose_key, point_key = P(0), L(0) k = gtsam.Cal3Fisheye() - state.insert_pose3(pose_key, gtsam.Pose3()) - state.insert_point3(point_key, self.obj_point) + state.insert(pose_key, gtsam.Pose3()) + state.insertPoint3(point_key, self.obj_point) factor = gtsam.GenericProjectionFactorCal3Fisheye(measured, noise_model, pose_key, point_key, k) graph.add(factor) score = graph.error(state) @@ -106,9 +106,9 @@ def test_sfm_factor2(self): noise_model = gtsam.noiseModel.Isotropic.Sigma(2, 1) camera_key, pose_key, landmark_key = K(0), P(0), L(0) k = gtsam.Cal3Fisheye() - state.insert_cal3fisheye(camera_key, k) - state.insert_pose3(pose_key, gtsam.Pose3()) - state.insert_point3(landmark_key, gtsam.Point3(self.obj_point)) + state.insert(camera_key, k) + state.insert(pose_key, gtsam.Pose3()) + state.insertPoint3(landmark_key, gtsam.Point3(self.obj_point)) factor = gtsam.GeneralSFMFactor2Cal3Fisheye(measured, noise_model, pose_key, landmark_key, camera_key) graph.add(factor) score = graph.error(state) @@ -168,8 +168,8 @@ def evaluate_jacobian(obj_point, img_point): camera = gtsam.Cal3Fisheye() state = gtsam.Values() camera_key, pose_key, landmark_key = K(0), P(0), L(0) - state.insert_point3(landmark_key, obj_point) - state.insert_pose3(pose_key, pose) + state.insertPoint3(landmark_key, obj_point) + state.insert(pose_key, pose) g = gtsam.NonlinearFactorGraph() noise_model = gtsam.noiseModel.Unit.Create(2) factor = gtsam.GenericProjectionFactorCal3Fisheye(img_point, noise_model, pose_key, landmark_key, camera) diff --git a/python/gtsam/tests/test_Cal3Unified.py b/python/gtsam/tests/test_Cal3Unified.py index 630109d667..326bec3bd9 100644 --- a/python/gtsam/tests/test_Cal3Unified.py +++ b/python/gtsam/tests/test_Cal3Unified.py @@ -93,8 +93,8 @@ def test_generic_factor(self): noise_model = gtsam.noiseModel.Isotropic.Sigma(2, 1) pose_key, point_key = P(0), L(0) k = self.stereographic - state.insert_pose3(pose_key, gtsam.Pose3()) - state.insert_point3(point_key, self.obj_point) + state.insert(pose_key, gtsam.Pose3()) + state.insertPoint3(point_key, self.obj_point) factor = gtsam.GenericProjectionFactorCal3Unified(measured, noise_model, pose_key, point_key, k) graph.add(factor) score = graph.error(state) @@ -109,9 +109,9 @@ def test_sfm_factor2(self): noise_model = gtsam.noiseModel.Isotropic.Sigma(2, 1) camera_key, pose_key, landmark_key = K(0), P(0), L(0) k = self.stereographic - state.insert_cal3unified(camera_key, k) - state.insert_pose3(pose_key, gtsam.Pose3()) - state.insert_point3(landmark_key, self.obj_point) + state.insert(camera_key, k) + state.insert(pose_key, gtsam.Pose3()) + state.insertPoint3(landmark_key, self.obj_point) factor = gtsam.GeneralSFMFactor2Cal3Unified(measured, noise_model, pose_key, landmark_key, camera_key) graph.add(factor) score = graph.error(state) @@ -125,9 +125,9 @@ def test_jacobian(self): camera = gtsam.Cal3Unified() state = gtsam.Values() camera_key, pose_key, landmark_key = K(0), P(0), L(0) - state.insert_cal3unified(camera_key, camera) - state.insert_point3(landmark_key, obj_point_on_axis) - state.insert_pose3(pose_key, pose) + state.insert(camera_key, camera) + state.insertPoint3(landmark_key, obj_point_on_axis) + state.insert(pose_key, pose) g = gtsam.NonlinearFactorGraph() noise_model = gtsam.noiseModel.Unit.Create(2) factor = gtsam.GeneralSFMFactor2Cal3Unified(img_point, noise_model, pose_key, landmark_key, camera_key) diff --git a/python/gtsam/tests/test_GraphvizFormatting.py b/python/gtsam/tests/test_GraphvizFormatting.py index ecdc23b450..5a26afb560 100644 --- a/python/gtsam/tests/test_GraphvizFormatting.py +++ b/python/gtsam/tests/test_GraphvizFormatting.py @@ -29,9 +29,9 @@ def setUp(self): self.graph.add(gtsam.BetweenFactorPose2(1, 2, odometry, odometryNoise)) self.values = gtsam.Values() - self.values.insert_pose2(0, gtsam.Pose2(0., 0., 0.)) - self.values.insert_pose2(1, gtsam.Pose2(2., 0., 0.)) - self.values.insert_pose2(2, gtsam.Pose2(4., 0., 0.)) + self.values.insert(0, gtsam.Pose2(0., 0., 0.)) + self.values.insert(1, gtsam.Pose2(2., 0., 0.)) + self.values.insert(2, gtsam.Pose2(4., 0., 0.)) def test_default(self): """Test with default GraphvizFormatting""" diff --git a/python/gtsam/tests/test_HybridNonlinearFactorGraph.py b/python/gtsam/tests/test_HybridNonlinearFactorGraph.py index ed26a0c813..4b6af0a9ab 100644 --- a/python/gtsam/tests/test_HybridNonlinearFactorGraph.py +++ b/python/gtsam/tests/test_HybridNonlinearFactorGraph.py @@ -42,8 +42,8 @@ def test_nonlinear_hybrid(self): ])) nlfg.push_back(gtsam.DecisionTreeFactor((10, 2), "1 3")) values = gtsam.Values() - values.insert_point3(1, Point3(0, 0, 0)) - values.insert_point3(2, Point3(2, 3, 1)) + values.insertPoint3(1, Point3(0, 0, 0)) + values.insertPoint3(2, Point3(2, 3, 1)) hfg = nlfg.linearize(values) hbn = hfg.eliminateSequential() hbv = hbn.optimize() From e400b23b8ad7fb72555c1165c142e166a791c541 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Fri, 6 Jun 2025 23:50:58 -0700 Subject: [PATCH 2/5] Two more fixes of 32-bit gtsam::Key vs size_t --- gtsam/linear/linear.i | 14 +++++++------- gtsam/nonlinear/nonlinear.i | 4 ++-- 2 files changed, 9 insertions(+), 9 deletions(-) diff --git a/gtsam/linear/linear.i b/gtsam/linear/linear.i index 770ff0c53e..d88a8755e0 100644 --- a/gtsam/linear/linear.i +++ b/gtsam/linear/linear.i @@ -228,16 +228,16 @@ class VectorValues { //Standard Interface size_t size() const; - size_t dim(size_t j) const; - bool exists(size_t j) const; + size_t dim(gtsam::Key j) const; + bool exists(gtsam::Key j) const; void print(string s = "VectorValues", const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const; - bool equals(const gtsam::VectorValues& expected, double tol) const; - void insert(size_t j, Vector value); - Vector vector() const; - Vector vector(const gtsam::KeyVector& keys) const; - Vector at(size_t j) const; + bool equals(const gtsam::VectorValues& x, double tol) const; + void insert(gtsam::Key j, gtsam::Vector value); + gtsam::Vector vector() const; + gtsam::Vector vector(const gtsam::KeyVector& keys) const; + gtsam::Vector at(gtsam::Key j) const; void insert(const gtsam::VectorValues& values); void update(const gtsam::VectorValues& values); diff --git a/gtsam/nonlinear/nonlinear.i b/gtsam/nonlinear/nonlinear.i index 652eb8b856..cc4a22fc6f 100644 --- a/gtsam/nonlinear/nonlinear.i +++ b/gtsam/nonlinear/nonlinear.i @@ -367,8 +367,8 @@ class Marginals { void print(string s = "Marginals: ", const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const; - Matrix marginalCovariance(size_t variable) const; - Matrix marginalInformation(size_t variable) const; + gtsam::Matrix marginalCovariance(gtsam::Key variable) const; + gtsam::Matrix marginalInformation(gtsam::Key variable) const; gtsam::JointMarginal jointMarginalCovariance( const gtsam::KeyVector& variables) const; gtsam::JointMarginal jointMarginalInformation( From c9bf4228813cfb711de531cc3d169f22a4d85e6e Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Sat, 7 Jun 2025 00:59:28 -0700 Subject: [PATCH 3/5] Fix usage of GCC extension before C++23 --- gtsam/discrete/DiscreteBayesNet.cpp | 5 +---- gtsam/slam/BearingFactor.h | 4 ---- gtsam/slam/BearingRangeFactor.h | 5 +---- gtsam/slam/RangeFactor.h | 5 ----- 4 files changed, 2 insertions(+), 17 deletions(-) diff --git a/gtsam/discrete/DiscreteBayesNet.cpp b/gtsam/discrete/DiscreteBayesNet.cpp index 7cd190cc26..42a8bd6ab8 100644 --- a/gtsam/discrete/DiscreteBayesNet.cpp +++ b/gtsam/discrete/DiscreteBayesNet.cpp @@ -60,11 +60,8 @@ DiscreteValues DiscreteBayesNet::optimize() const { DiscreteValues DiscreteBayesNet::optimize(DiscreteValues result) const { // solve each node in turn in topological sort order (parents first) -#ifdef _MSC_VER #pragma message("DiscreteBayesNet::optimize (deprecated) does not compute MPE!") -#else -#warning "DiscreteBayesNet::optimize (deprecated) does not compute MPE!" -#endif + for (auto conditional : boost::adaptors::reverse(*this)) conditional->solveInPlace(&result); return result; diff --git a/gtsam/slam/BearingFactor.h b/gtsam/slam/BearingFactor.h index 7383690f49..1d24edf362 100644 --- a/gtsam/slam/BearingFactor.h +++ b/gtsam/slam/BearingFactor.h @@ -11,11 +11,7 @@ #pragma once -#ifdef _MSC_VER #pragma message("BearingFactor is now an ExpressionFactor in SAM directory") -#else -#warning "BearingFactor is now an ExpressionFactor in SAM directory" -#endif #include diff --git a/gtsam/slam/BearingRangeFactor.h b/gtsam/slam/BearingRangeFactor.h index 901860015c..ec995957a7 100644 --- a/gtsam/slam/BearingRangeFactor.h +++ b/gtsam/slam/BearingRangeFactor.h @@ -11,11 +11,8 @@ #pragma once -#ifdef _MSC_VER #pragma message( \ "BearingRangeFactor is now an ExpressionFactor in SAM directory") -#else -#warning "BearingRangeFactor is now an ExpressionFactor in SAM directory" -#endif + #include diff --git a/gtsam/slam/RangeFactor.h b/gtsam/slam/RangeFactor.h index 3fa5c1a21c..a7779ec504 100644 --- a/gtsam/slam/RangeFactor.h +++ b/gtsam/slam/RangeFactor.h @@ -11,12 +11,7 @@ #pragma once -#ifdef _MSC_VER #pragma message("RangeFactor is now an ExpressionFactor in SAM directory") -#else -#warning "RangeFactor is now an ExpressionFactor in SAM directory" -#endif - #include From 845096ff394eda3fa04c997f11e3a97279cb8cc6 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Sun, 1 Jun 2025 21:22:29 -0700 Subject: [PATCH 4/5] Fix compiler warning of operator being shadowed --- gtsam/discrete/DiscreteConditional.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/gtsam/discrete/DiscreteConditional.h b/gtsam/discrete/DiscreteConditional.h index a4e1f011b7..67180a3f36 100644 --- a/gtsam/discrete/DiscreteConditional.h +++ b/gtsam/discrete/DiscreteConditional.h @@ -111,6 +111,8 @@ class GTSAM_EXPORT DiscreteConditional const DecisionTreeFactor& marginal, const Ordering& orderedKeys); + using DecisionTreeFactor::operator*; + /** * @brief Combine two conditionals, yielding a new conditional with the union * of the frontal keys, ordered by gtsam::Key. From 19ef6ff5d872aee8834599f222b80f7e25724332 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Sat, 7 Jun 2025 01:38:03 -0700 Subject: [PATCH 5/5] More fixes to the nonlinear.i --- gtsam/nonlinear/nonlinear.i | 280 ++++++++++++++++++------------------ 1 file changed, 140 insertions(+), 140 deletions(-) diff --git a/gtsam/nonlinear/nonlinear.i b/gtsam/nonlinear/nonlinear.i index cc4a22fc6f..c58bf9b29a 100644 --- a/gtsam/nonlinear/nonlinear.i +++ b/gtsam/nonlinear/nonlinear.i @@ -149,10 +149,10 @@ class Values { void insert(const gtsam::Values& values); void update(const gtsam::Values& values); void insert_or_assign(const gtsam::Values& values); - void erase(size_t j); + void erase(gtsam::Key j); void swap(gtsam::Values& values); - bool exists(size_t j) const; + bool exists(gtsam::Key j) const; gtsam::KeyVector keys() const; gtsam::VectorValues zeroVectors() const; @@ -170,144 +170,144 @@ class Values { // The order is important: Vector has to precede Point2/Point3 so `atVector` // can work for those fixed-size vectors. - void insert(size_t j, Vector vector); - void insert(size_t j, Matrix matrix); - void insert(size_t j, const gtsam::Point2& point2); - void insert(size_t j, const gtsam::Point3& point3); - void insert(size_t j, const gtsam::Rot2& rot2); - void insert(size_t j, const gtsam::Pose2& pose2); - void insert(size_t j, const gtsam::SO3& R); - void insert(size_t j, const gtsam::SO4& Q); - void insert(size_t j, const gtsam::SOn& P); - void insert(size_t j, const gtsam::Rot3& rot3); - void insert(size_t j, const gtsam::Pose3& pose3); - void insert(size_t j, const gtsam::Unit3& unit3); - void insert(size_t j, const gtsam::Cal3_S2& cal3_s2); - void insert(size_t j, const gtsam::Cal3DS2& cal3ds2); - void insert(size_t j, const gtsam::Cal3Bundler& cal3bundler); - void insert(size_t j, const gtsam::Cal3Fisheye& cal3fisheye); - void insert(size_t j, const gtsam::Cal3Unified& cal3unified); - void insert(size_t j, const gtsam::EssentialMatrix& essential_matrix); - void insert(size_t j, const gtsam::PinholeCamera& camera); - void insert(size_t j, const gtsam::PinholeCamera& camera); - void insert(size_t j, const gtsam::PinholeCamera& camera); - void insert(size_t j, const gtsam::PinholeCamera& camera); - void insert(size_t j, const gtsam::PinholePose& camera); - void insert(size_t j, const gtsam::PinholePose& camera); - void insert(size_t j, const gtsam::PinholePose& camera); - void insert(size_t j, const gtsam::PinholePose& camera); - void insert(size_t j, const gtsam::imuBias::ConstantBias& constant_bias); - void insert(size_t j, const gtsam::NavState& nav_state); - void insert(size_t j, double c); - void insert(size_t j, const gtsam::ParameterMatrix<1>& X); - void insert(size_t j, const gtsam::ParameterMatrix<2>& X); - void insert(size_t j, const gtsam::ParameterMatrix<3>& X); - void insert(size_t j, const gtsam::ParameterMatrix<4>& X); - void insert(size_t j, const gtsam::ParameterMatrix<5>& X); - void insert(size_t j, const gtsam::ParameterMatrix<6>& X); - void insert(size_t j, const gtsam::ParameterMatrix<7>& X); - void insert(size_t j, const gtsam::ParameterMatrix<8>& X); - void insert(size_t j, const gtsam::ParameterMatrix<9>& X); - void insert(size_t j, const gtsam::ParameterMatrix<10>& X); - void insert(size_t j, const gtsam::ParameterMatrix<11>& X); - void insert(size_t j, const gtsam::ParameterMatrix<12>& X); - void insert(size_t j, const gtsam::ParameterMatrix<13>& X); - void insert(size_t j, const gtsam::ParameterMatrix<14>& X); - void insert(size_t j, const gtsam::ParameterMatrix<15>& X); + void insert(gtsam::Key j, Vector vector); + void insert(gtsam::Key j, Matrix matrix); + void insert(gtsam::Key j, const gtsam::Point2& point2); + void insert(gtsam::Key j, const gtsam::Point3& point3); + void insert(gtsam::Key j, const gtsam::Rot2& rot2); + void insert(gtsam::Key j, const gtsam::Pose2& pose2); + void insert(gtsam::Key j, const gtsam::SO3& R); + void insert(gtsam::Key j, const gtsam::SO4& Q); + void insert(gtsam::Key j, const gtsam::SOn& P); + void insert(gtsam::Key j, const gtsam::Rot3& rot3); + void insert(gtsam::Key j, const gtsam::Pose3& pose3); + void insert(gtsam::Key j, const gtsam::Unit3& unit3); + void insert(gtsam::Key j, const gtsam::Cal3_S2& cal3_s2); + void insert(gtsam::Key j, const gtsam::Cal3DS2& cal3ds2); + void insert(gtsam::Key j, const gtsam::Cal3Bundler& cal3bundler); + void insert(gtsam::Key j, const gtsam::Cal3Fisheye& cal3fisheye); + void insert(gtsam::Key j, const gtsam::Cal3Unified& cal3unified); + void insert(gtsam::Key j, const gtsam::EssentialMatrix& essential_matrix); + void insert(gtsam::Key j, const gtsam::PinholeCamera& camera); + void insert(gtsam::Key j, const gtsam::PinholeCamera& camera); + void insert(gtsam::Key j, const gtsam::PinholeCamera& camera); + void insert(gtsam::Key j, const gtsam::PinholeCamera& camera); + void insert(gtsam::Key j, const gtsam::PinholePose& camera); + void insert(gtsam::Key j, const gtsam::PinholePose& camera); + void insert(gtsam::Key j, const gtsam::PinholePose& camera); + void insert(gtsam::Key j, const gtsam::PinholePose& camera); + void insert(gtsam::Key j, const gtsam::imuBias::ConstantBias& constant_bias); + void insert(gtsam::Key j, const gtsam::NavState& nav_state); + void insert(gtsam::Key j, double c); + void insert(gtsam::Key j, const gtsam::ParameterMatrix<1>& X); + void insert(gtsam::Key j, const gtsam::ParameterMatrix<2>& X); + void insert(gtsam::Key j, const gtsam::ParameterMatrix<3>& X); + void insert(gtsam::Key j, const gtsam::ParameterMatrix<4>& X); + void insert(gtsam::Key j, const gtsam::ParameterMatrix<5>& X); + void insert(gtsam::Key j, const gtsam::ParameterMatrix<6>& X); + void insert(gtsam::Key j, const gtsam::ParameterMatrix<7>& X); + void insert(gtsam::Key j, const gtsam::ParameterMatrix<8>& X); + void insert(gtsam::Key j, const gtsam::ParameterMatrix<9>& X); + void insert(gtsam::Key j, const gtsam::ParameterMatrix<10>& X); + void insert(gtsam::Key j, const gtsam::ParameterMatrix<11>& X); + void insert(gtsam::Key j, const gtsam::ParameterMatrix<12>& X); + void insert(gtsam::Key j, const gtsam::ParameterMatrix<13>& X); + void insert(gtsam::Key j, const gtsam::ParameterMatrix<14>& X); + void insert(gtsam::Key j, const gtsam::ParameterMatrix<15>& X); template - void insert(size_t j, const T& val); - - void update(size_t j, const gtsam::Point2& point2); - void update(size_t j, const gtsam::Point3& point3); - void update(size_t j, const gtsam::Rot2& rot2); - void update(size_t j, const gtsam::Pose2& pose2); - void update(size_t j, const gtsam::SO3& R); - void update(size_t j, const gtsam::SO4& Q); - void update(size_t j, const gtsam::SOn& P); - void update(size_t j, const gtsam::Rot3& rot3); - void update(size_t j, const gtsam::Pose3& pose3); - void update(size_t j, const gtsam::Unit3& unit3); - void update(size_t j, const gtsam::Cal3_S2& cal3_s2); - void update(size_t j, const gtsam::Cal3DS2& cal3ds2); - void update(size_t j, const gtsam::Cal3Bundler& cal3bundler); - void update(size_t j, const gtsam::Cal3Fisheye& cal3fisheye); - void update(size_t j, const gtsam::Cal3Unified& cal3unified); - void update(size_t j, const gtsam::EssentialMatrix& essential_matrix); - void update(size_t j, const gtsam::PinholeCamera& camera); - void update(size_t j, const gtsam::PinholeCamera& camera); - void update(size_t j, const gtsam::PinholeCamera& camera); - void update(size_t j, const gtsam::PinholeCamera& camera); - void update(size_t j, const gtsam::PinholePose& camera); - void update(size_t j, const gtsam::PinholePose& camera); - void update(size_t j, const gtsam::PinholePose& camera); - void update(size_t j, const gtsam::PinholePose& camera); - void update(size_t j, const gtsam::imuBias::ConstantBias& constant_bias); - void update(size_t j, const gtsam::NavState& nav_state); - void update(size_t j, Vector vector); - void update(size_t j, Matrix matrix); - void update(size_t j, double c); - void update(size_t j, const gtsam::ParameterMatrix<1>& X); - void update(size_t j, const gtsam::ParameterMatrix<2>& X); - void update(size_t j, const gtsam::ParameterMatrix<3>& X); - void update(size_t j, const gtsam::ParameterMatrix<4>& X); - void update(size_t j, const gtsam::ParameterMatrix<5>& X); - void update(size_t j, const gtsam::ParameterMatrix<6>& X); - void update(size_t j, const gtsam::ParameterMatrix<7>& X); - void update(size_t j, const gtsam::ParameterMatrix<8>& X); - void update(size_t j, const gtsam::ParameterMatrix<9>& X); - void update(size_t j, const gtsam::ParameterMatrix<10>& X); - void update(size_t j, const gtsam::ParameterMatrix<11>& X); - void update(size_t j, const gtsam::ParameterMatrix<12>& X); - void update(size_t j, const gtsam::ParameterMatrix<13>& X); - void update(size_t j, const gtsam::ParameterMatrix<14>& X); - void update(size_t j, const gtsam::ParameterMatrix<15>& X); - - void insert_or_assign(size_t j, const gtsam::Point2& point2); - void insert_or_assign(size_t j, const gtsam::Point3& point3); - void insert_or_assign(size_t j, const gtsam::Rot2& rot2); - void insert_or_assign(size_t j, const gtsam::Pose2& pose2); - void insert_or_assign(size_t j, const gtsam::SO3& R); - void insert_or_assign(size_t j, const gtsam::SO4& Q); - void insert_or_assign(size_t j, const gtsam::SOn& P); - void insert_or_assign(size_t j, const gtsam::Rot3& rot3); - void insert_or_assign(size_t j, const gtsam::Pose3& pose3); - void insert_or_assign(size_t j, const gtsam::Unit3& unit3); - void insert_or_assign(size_t j, const gtsam::Cal3_S2& cal3_s2); - void insert_or_assign(size_t j, const gtsam::Cal3DS2& cal3ds2); - void insert_or_assign(size_t j, const gtsam::Cal3Bundler& cal3bundler); - void insert_or_assign(size_t j, const gtsam::Cal3Fisheye& cal3fisheye); - void insert_or_assign(size_t j, const gtsam::Cal3Unified& cal3unified); - void insert_or_assign(size_t j, const gtsam::EssentialMatrix& essential_matrix); - void insert_or_assign(size_t j, const gtsam::PinholeCamera& camera); - void insert_or_assign(size_t j, const gtsam::PinholeCamera& camera); - void insert_or_assign(size_t j, const gtsam::PinholeCamera& camera); - void insert_or_assign(size_t j, const gtsam::PinholeCamera& camera); - void insert_or_assign(size_t j, const gtsam::PinholePose& camera); - void insert_or_assign(size_t j, const gtsam::PinholePose& camera); - void insert_or_assign(size_t j, const gtsam::PinholePose& camera); - void insert_or_assign(size_t j, const gtsam::PinholePose& camera); - void insert_or_assign(size_t j, const gtsam::imuBias::ConstantBias& constant_bias); - void insert_or_assign(size_t j, const gtsam::NavState& nav_state); - void insert_or_assign(size_t j, Vector vector); - void insert_or_assign(size_t j, Matrix matrix); - void insert_or_assign(size_t j, double c); - void insert_or_assign(size_t j, const gtsam::ParameterMatrix<1>& X); - void insert_or_assign(size_t j, const gtsam::ParameterMatrix<2>& X); - void insert_or_assign(size_t j, const gtsam::ParameterMatrix<3>& X); - void insert_or_assign(size_t j, const gtsam::ParameterMatrix<4>& X); - void insert_or_assign(size_t j, const gtsam::ParameterMatrix<5>& X); - void insert_or_assign(size_t j, const gtsam::ParameterMatrix<6>& X); - void insert_or_assign(size_t j, const gtsam::ParameterMatrix<7>& X); - void insert_or_assign(size_t j, const gtsam::ParameterMatrix<8>& X); - void insert_or_assign(size_t j, const gtsam::ParameterMatrix<9>& X); - void insert_or_assign(size_t j, const gtsam::ParameterMatrix<10>& X); - void insert_or_assign(size_t j, const gtsam::ParameterMatrix<11>& X); - void insert_or_assign(size_t j, const gtsam::ParameterMatrix<12>& X); - void insert_or_assign(size_t j, const gtsam::ParameterMatrix<13>& X); - void insert_or_assign(size_t j, const gtsam::ParameterMatrix<14>& X); - void insert_or_assign(size_t j, const gtsam::ParameterMatrix<15>& X); + void insert(gtsam::Key j, const T& val); + + void update(gtsam::Key j, const gtsam::Point2& point2); + void update(gtsam::Key j, const gtsam::Point3& point3); + void update(gtsam::Key j, const gtsam::Rot2& rot2); + void update(gtsam::Key j, const gtsam::Pose2& pose2); + void update(gtsam::Key j, const gtsam::SO3& R); + void update(gtsam::Key j, const gtsam::SO4& Q); + void update(gtsam::Key j, const gtsam::SOn& P); + void update(gtsam::Key j, const gtsam::Rot3& rot3); + void update(gtsam::Key j, const gtsam::Pose3& pose3); + void update(gtsam::Key j, const gtsam::Unit3& unit3); + void update(gtsam::Key j, const gtsam::Cal3_S2& cal3_s2); + void update(gtsam::Key j, const gtsam::Cal3DS2& cal3ds2); + void update(gtsam::Key j, const gtsam::Cal3Bundler& cal3bundler); + void update(gtsam::Key j, const gtsam::Cal3Fisheye& cal3fisheye); + void update(gtsam::Key j, const gtsam::Cal3Unified& cal3unified); + void update(gtsam::Key j, const gtsam::EssentialMatrix& essential_matrix); + void update(gtsam::Key j, const gtsam::PinholeCamera& camera); + void update(gtsam::Key j, const gtsam::PinholeCamera& camera); + void update(gtsam::Key j, const gtsam::PinholeCamera& camera); + void update(gtsam::Key j, const gtsam::PinholeCamera& camera); + void update(gtsam::Key j, const gtsam::PinholePose& camera); + void update(gtsam::Key j, const gtsam::PinholePose& camera); + void update(gtsam::Key j, const gtsam::PinholePose& camera); + void update(gtsam::Key j, const gtsam::PinholePose& camera); + void update(gtsam::Key j, const gtsam::imuBias::ConstantBias& constant_bias); + void update(gtsam::Key j, const gtsam::NavState& nav_state); + void update(gtsam::Key j, Vector vector); + void update(gtsam::Key j, Matrix matrix); + void update(gtsam::Key j, double c); + void update(gtsam::Key j, const gtsam::ParameterMatrix<1>& X); + void update(gtsam::Key j, const gtsam::ParameterMatrix<2>& X); + void update(gtsam::Key j, const gtsam::ParameterMatrix<3>& X); + void update(gtsam::Key j, const gtsam::ParameterMatrix<4>& X); + void update(gtsam::Key j, const gtsam::ParameterMatrix<5>& X); + void update(gtsam::Key j, const gtsam::ParameterMatrix<6>& X); + void update(gtsam::Key j, const gtsam::ParameterMatrix<7>& X); + void update(gtsam::Key j, const gtsam::ParameterMatrix<8>& X); + void update(gtsam::Key j, const gtsam::ParameterMatrix<9>& X); + void update(gtsam::Key j, const gtsam::ParameterMatrix<10>& X); + void update(gtsam::Key j, const gtsam::ParameterMatrix<11>& X); + void update(gtsam::Key j, const gtsam::ParameterMatrix<12>& X); + void update(gtsam::Key j, const gtsam::ParameterMatrix<13>& X); + void update(gtsam::Key j, const gtsam::ParameterMatrix<14>& X); + void update(gtsam::Key j, const gtsam::ParameterMatrix<15>& X); + + void insert_or_assign(gtsam::Key j, const gtsam::Point2& point2); + void insert_or_assign(gtsam::Key j, const gtsam::Point3& point3); + void insert_or_assign(gtsam::Key j, const gtsam::Rot2& rot2); + void insert_or_assign(gtsam::Key j, const gtsam::Pose2& pose2); + void insert_or_assign(gtsam::Key j, const gtsam::SO3& R); + void insert_or_assign(gtsam::Key j, const gtsam::SO4& Q); + void insert_or_assign(gtsam::Key j, const gtsam::SOn& P); + void insert_or_assign(gtsam::Key j, const gtsam::Rot3& rot3); + void insert_or_assign(gtsam::Key j, const gtsam::Pose3& pose3); + void insert_or_assign(gtsam::Key j, const gtsam::Unit3& unit3); + void insert_or_assign(gtsam::Key j, const gtsam::Cal3_S2& cal3_s2); + void insert_or_assign(gtsam::Key j, const gtsam::Cal3DS2& cal3ds2); + void insert_or_assign(gtsam::Key j, const gtsam::Cal3Bundler& cal3bundler); + void insert_or_assign(gtsam::Key j, const gtsam::Cal3Fisheye& cal3fisheye); + void insert_or_assign(gtsam::Key j, const gtsam::Cal3Unified& cal3unified); + void insert_or_assign(gtsam::Key j, const gtsam::EssentialMatrix& essential_matrix); + void insert_or_assign(gtsam::Key j, const gtsam::PinholeCamera& camera); + void insert_or_assign(gtsam::Key j, const gtsam::PinholeCamera& camera); + void insert_or_assign(gtsam::Key j, const gtsam::PinholeCamera& camera); + void insert_or_assign(gtsam::Key j, const gtsam::PinholeCamera& camera); + void insert_or_assign(gtsam::Key j, const gtsam::PinholePose& camera); + void insert_or_assign(gtsam::Key j, const gtsam::PinholePose& camera); + void insert_or_assign(gtsam::Key j, const gtsam::PinholePose& camera); + void insert_or_assign(gtsam::Key j, const gtsam::PinholePose& camera); + void insert_or_assign(gtsam::Key j, const gtsam::imuBias::ConstantBias& constant_bias); + void insert_or_assign(gtsam::Key j, const gtsam::NavState& nav_state); + void insert_or_assign(gtsam::Key j, Vector vector); + void insert_or_assign(gtsam::Key j, Matrix matrix); + void insert_or_assign(gtsam::Key j, double c); + void insert_or_assign(gtsam::Key j, const gtsam::ParameterMatrix<1>& X); + void insert_or_assign(gtsam::Key j, const gtsam::ParameterMatrix<2>& X); + void insert_or_assign(gtsam::Key j, const gtsam::ParameterMatrix<3>& X); + void insert_or_assign(gtsam::Key j, const gtsam::ParameterMatrix<4>& X); + void insert_or_assign(gtsam::Key j, const gtsam::ParameterMatrix<5>& X); + void insert_or_assign(gtsam::Key j, const gtsam::ParameterMatrix<6>& X); + void insert_or_assign(gtsam::Key j, const gtsam::ParameterMatrix<7>& X); + void insert_or_assign(gtsam::Key j, const gtsam::ParameterMatrix<8>& X); + void insert_or_assign(gtsam::Key j, const gtsam::ParameterMatrix<9>& X); + void insert_or_assign(gtsam::Key j, const gtsam::ParameterMatrix<10>& X); + void insert_or_assign(gtsam::Key j, const gtsam::ParameterMatrix<11>& X); + void insert_or_assign(gtsam::Key j, const gtsam::ParameterMatrix<12>& X); + void insert_or_assign(gtsam::Key j, const gtsam::ParameterMatrix<13>& X); + void insert_or_assign(gtsam::Key j, const gtsam::ParameterMatrix<14>& X); + void insert_or_assign(gtsam::Key j, const gtsam::ParameterMatrix<15>& X); template , gtsam::ParameterMatrix<14>, gtsam::ParameterMatrix<15>}> - T at(size_t j); + T at(gtsam::Key j); }; #include @@ -849,9 +849,9 @@ template virtual class NonlinearEquality : gtsam::NoiseModelFactor { // Constructor - forces exact evaluation - NonlinearEquality(size_t j, const T& feasible); + NonlinearEquality(gtsam::Key j, const T& feasible); // Constructor - allows inexact evaluation - NonlinearEquality(size_t j, const T& feasible, double error_gain); + NonlinearEquality(gtsam::Key j, const T& feasible, double error_gain); // enabling serialization functionality void serialize() const;