diff --git a/gtsam/basis/basis.i b/gtsam/basis/basis.i index 384797ed3f..f733aca732 100644 --- a/gtsam/basis/basis.i +++ b/gtsam/basis/basis.i @@ -193,10 +193,10 @@ class FitBasis { static gtsam::NonlinearFactorGraph NonlinearGraph( const std::map& sequence, - const std::shared_ptr& model, size_t N); - static gtsam::GaussianFactorGraph::shared_ptr LinearGraph( + const gtsam::noiseModel::Base* model, size_t N); + static gtsam::GaussianFactorGraph* LinearGraph( const std::map& sequence, - const std::shared_ptr& model, size_t N); + const gtsam::noiseModel::Base* model, size_t N); gtsam::This::Parameters parameters() const; }; diff --git a/gtsam/constrained/constrained.i b/gtsam/constrained/constrained.i index 8f955946c9..636892281f 100644 --- a/gtsam/constrained/constrained.i +++ b/gtsam/constrained/constrained.i @@ -107,9 +107,9 @@ class LpProblem : gtsam::ConstrainedOptProblem { double objective(const gtsam::Values& values) const; gtsam::Values optimize( const gtsam::Values& initialValues, - std::shared_ptr params = nullptr) const; + gtsam::ActiveSetSolverParams* params = nullptr) const; gtsam::Values optimize( - std::shared_ptr params = nullptr) const; + gtsam::ActiveSetSolverParams* params = nullptr) const; }; #include @@ -224,7 +224,7 @@ virtual class AugmentedLagrangianOptimizer { AugmentedLagrangianOptimizer( const gtsam::ConstrainedOptProblem& problem, const gtsam::Values& initialValues, - gtsam::AugmentedLagrangianParams::shared_ptr p); + gtsam::AugmentedLagrangianParams* p); gtsam::Values optimize() const; }; diff --git a/gtsam/discrete/discrete.i b/gtsam/discrete/discrete.i index 9420603699..51496fad97 100644 --- a/gtsam/discrete/discrete.i +++ b/gtsam/discrete/discrete.i @@ -236,7 +236,7 @@ class DiscreteBayesNet { class DiscreteBayesTreeClique { DiscreteBayesTreeClique(); DiscreteBayesTreeClique(const gtsam::DiscreteConditional* conditional); - const gtsam::DiscreteConditional::shared_ptr& conditional() const; + const gtsam::DiscreteConditional* conditional() const; bool isRoot() const; size_t nrChildren() const; const gtsam::DiscreteBayesTreeClique* operator[](size_t i) const; @@ -253,11 +253,11 @@ class DiscreteBayesTreeClique { class DiscreteBayesTree { DiscreteBayesTree(); void insertRoot( - const std::shared_ptr& subtree); + const gtsam::DiscreteBayesTreeClique* subtree); void addClique( - const std::shared_ptr& clique, - const std::shared_ptr& parent_clique = - std::shared_ptr()); + const gtsam::DiscreteBayesTreeClique* clique, + const gtsam::DiscreteBayesTreeClique* parent_clique = + nullptr); void print(string s = "DiscreteBayesTree\n", const gtsam::KeyFormatter& keyFormatter = @@ -267,23 +267,23 @@ class DiscreteBayesTree { size_t size() const; bool empty() const; const DiscreteBayesTreeClique* operator[](gtsam::Key j) const; - const std::shared_ptr& clique( + const gtsam::DiscreteBayesTreeClique* clique( gtsam::Key j) const; size_t numCachedSeparatorMarginals() const; - std::shared_ptr marginalFactor( + gtsam::DiscreteConditional* marginalFactor( gtsam::Key j, const gtsam::DiscreteFactorGraph::Eliminate& function = gtsam::DiscreteFactorGraph::Eliminate( gtsam::DiscreteFactorGraph::EliminationTraitsType::DefaultEliminate)) const; - std::shared_ptr joint( + gtsam::DiscreteFactorGraph* joint( gtsam::Key j1, gtsam::Key j2, const gtsam::DiscreteFactorGraph::Eliminate& function = gtsam::DiscreteFactorGraph::Eliminate( gtsam::DiscreteFactorGraph::EliminationTraitsType::DefaultEliminate)) const; - std::shared_ptr jointBayesNet( + gtsam::DiscreteBayesNet* jointBayesNet( gtsam::Key j1, gtsam::Key j2, const gtsam::DiscreteFactorGraph::Eliminate& function = gtsam::DiscreteFactorGraph::Eliminate( @@ -342,22 +342,22 @@ EliminateForMPE(const gtsam::DiscreteFactorGraph& factors, #include class DiscreteFactorGraph { - std::shared_ptr eliminateSequential( + gtsam::DiscreteBayesNet* eliminateSequential( gtsam::DiscreteFactorGraph::OptionalOrderingType orderingType = std::nullopt, const gtsam::DiscreteFactorGraph::Eliminate& function = gtsam::DiscreteFactorGraph::Eliminate( gtsam::DiscreteFactorGraph::EliminationTraitsType::DefaultEliminate), gtsam::DiscreteFactorGraph::OptionalVariableIndex variableIndex = std::nullopt) const; - std::shared_ptr eliminateSequential( + gtsam::DiscreteBayesNet* eliminateSequential( const gtsam::Ordering& ordering, const gtsam::DiscreteFactorGraph::Eliminate& function = gtsam::DiscreteFactorGraph::Eliminate( gtsam::DiscreteFactorGraph::EliminationTraitsType::DefaultEliminate), gtsam::DiscreteFactorGraph::OptionalVariableIndex variableIndex = std::nullopt) const; - pair, - std::shared_ptr> + pair eliminatePartialSequential( const gtsam::Ordering& ordering, const gtsam::DiscreteFactorGraph::Eliminate& function = @@ -365,8 +365,8 @@ class DiscreteFactorGraph { gtsam::DiscreteFactorGraph::EliminationTraitsType::DefaultEliminate), gtsam::DiscreteFactorGraph::OptionalVariableIndex variableIndex = std::nullopt) const; - pair, - std::shared_ptr> + pair eliminatePartialSequential( const gtsam::KeyVector& variables, const gtsam::DiscreteFactorGraph::Eliminate& function = @@ -374,22 +374,22 @@ class DiscreteFactorGraph { gtsam::DiscreteFactorGraph::EliminationTraitsType::DefaultEliminate), gtsam::DiscreteFactorGraph::OptionalVariableIndex variableIndex = std::nullopt) const; - std::shared_ptr eliminateMultifrontal( + gtsam::DiscreteBayesTree* eliminateMultifrontal( gtsam::DiscreteFactorGraph::OptionalOrderingType orderingType = std::nullopt, const gtsam::DiscreteFactorGraph::Eliminate& function = gtsam::DiscreteFactorGraph::Eliminate( gtsam::DiscreteFactorGraph::EliminationTraitsType::DefaultEliminate), gtsam::DiscreteFactorGraph::OptionalVariableIndex variableIndex = std::nullopt) const; - std::shared_ptr eliminateMultifrontal( + gtsam::DiscreteBayesTree* eliminateMultifrontal( const gtsam::Ordering& ordering, const gtsam::DiscreteFactorGraph::Eliminate& function = gtsam::DiscreteFactorGraph::Eliminate( gtsam::DiscreteFactorGraph::EliminationTraitsType::DefaultEliminate), gtsam::DiscreteFactorGraph::OptionalVariableIndex variableIndex = std::nullopt) const; - pair, - std::shared_ptr> + pair eliminatePartialMultifrontal( const gtsam::Ordering& ordering, const gtsam::DiscreteFactorGraph::Eliminate& function = @@ -397,8 +397,8 @@ class DiscreteFactorGraph { gtsam::DiscreteFactorGraph::EliminationTraitsType::DefaultEliminate), gtsam::DiscreteFactorGraph::OptionalVariableIndex variableIndex = std::nullopt) const; - pair, - std::shared_ptr> + pair eliminatePartialMultifrontal( const gtsam::KeyVector& variables, const gtsam::DiscreteFactorGraph::Eliminate& function = diff --git a/gtsam/geometry/cameras.i b/gtsam/geometry/cameras.i index 0961f33522..039d59b744 100644 --- a/gtsam/geometry/cameras.i +++ b/gtsam/geometry/cameras.i @@ -453,7 +453,7 @@ class PinholePose { static This Level(const gtsam::Pose2& pose, double height); static This Lookat(const gtsam::Point3& eye, const gtsam::Point3& target, const gtsam::Point3& upVector, - const std::shared_ptr& K); + const CALIBRATION* K); // Testable void print(string s = "PinholePose") const; @@ -508,7 +508,7 @@ class SphericalCamera { SphericalCamera(); SphericalCamera(const gtsam::Pose3& pose); SphericalCamera(const gtsam::Pose3& pose, - const gtsam::EmptyCal::shared_ptr& cal); + const gtsam::EmptyCal* cal); SphericalCamera(const gtsam::Vector& v); // Testable @@ -621,13 +621,13 @@ class TriangulationParameters { double landmarkDistanceThreshold; double dynamicOutlierRejectionThreshold; bool useLOST; - gtsam::SharedNoiseModel noiseModel; + gtsam::noiseModel::Base* noiseModel; TriangulationParameters(const double rankTolerance = 1.0, const bool enableEPI = false, double landmarkDistanceThreshold = -1, double dynamicOutlierRejectionThreshold = -1, const bool useLOST = false, - const gtsam::SharedNoiseModel& noiseModel = nullptr); + const gtsam::noiseModel::Base* noiseModel = nullptr); }; // Can be templated but overloaded for convenience. @@ -639,22 +639,22 @@ gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses, const gtsam::Point2Vector& measurements, double rank_tol = 1e-9, bool optimize = false, - const gtsam::SharedNoiseModel& model = nullptr, + const gtsam::noiseModel::Base* model = nullptr, const bool useLOST = false); gtsam::Point3 triangulatePoint3(const gtsam::CameraSetCal3_S2& cameras, const gtsam::Point2Vector& measurements, double rank_tol, bool optimize, - const gtsam::SharedNoiseModel& model = nullptr, + const gtsam::noiseModel::Base* model = nullptr, const bool useLOST = false); gtsam::Point3 triangulateNonlinear(const gtsam::Pose3Vector& poses, gtsam::Cal3_S2* sharedCal, const gtsam::Point2Vector& measurements, const gtsam::Point3& initialEstimate, - const gtsam::SharedNoiseModel& model = nullptr); + const gtsam::noiseModel::Base* model = nullptr); gtsam::Point3 triangulateNonlinear(const gtsam::CameraSetCal3_S2& cameras, const gtsam::Point2Vector& measurements, const gtsam::Point3& initialEstimate, - const gtsam::SharedNoiseModel& model = nullptr); + const gtsam::noiseModel::Base* model = nullptr); gtsam::TriangulationResult triangulateSafe( const gtsam::CameraSetCal3_S2& cameras, const gtsam::Point2Vector& measurements, @@ -670,22 +670,22 @@ gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses, const gtsam::Point2Vector& measurements, double rank_tol = 1e-9, bool optimize = false, - const gtsam::SharedNoiseModel& model = nullptr, + const gtsam::noiseModel::Base* model = nullptr, const bool useLOST = false); gtsam::Point3 triangulatePoint3(const gtsam::CameraSetCal3DS2& cameras, const gtsam::Point2Vector& measurements, double rank_tol, bool optimize, - const gtsam::SharedNoiseModel& model = nullptr, + const gtsam::noiseModel::Base* model = nullptr, const bool useLOST = false); gtsam::Point3 triangulateNonlinear(const gtsam::Pose3Vector& poses, gtsam::Cal3DS2* sharedCal, const gtsam::Point2Vector& measurements, const gtsam::Point3& initialEstimate, - const gtsam::SharedNoiseModel& model = nullptr); + const gtsam::noiseModel::Base* model = nullptr); gtsam::Point3 triangulateNonlinear(const gtsam::CameraSetCal3DS2& cameras, const gtsam::Point2Vector& measurements, const gtsam::Point3& initialEstimate, - const gtsam::SharedNoiseModel& model = nullptr); + const gtsam::noiseModel::Base* model = nullptr); gtsam::TriangulationResult triangulateSafe( const gtsam::CameraSetCal3DS2& cameras, const gtsam::Point2Vector& measurements, @@ -701,22 +701,22 @@ gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses, const gtsam::Point2Vector& measurements, double rank_tol = 1e-9, bool optimize = false, - const gtsam::SharedNoiseModel& model = nullptr, + const gtsam::noiseModel::Base* model = nullptr, const bool useLOST = false); gtsam::Point3 triangulatePoint3(const gtsam::CameraSetCal3Bundler& cameras, const gtsam::Point2Vector& measurements, double rank_tol, bool optimize, - const gtsam::SharedNoiseModel& model = nullptr, + const gtsam::noiseModel::Base* model = nullptr, const bool useLOST = false); gtsam::Point3 triangulateNonlinear(const gtsam::Pose3Vector& poses, gtsam::Cal3Bundler* sharedCal, const gtsam::Point2Vector& measurements, const gtsam::Point3& initialEstimate, - const gtsam::SharedNoiseModel& model = nullptr); + const gtsam::noiseModel::Base* model = nullptr); gtsam::Point3 triangulateNonlinear(const gtsam::CameraSetCal3Bundler& cameras, const gtsam::Point2Vector& measurements, const gtsam::Point3& initialEstimate, - const gtsam::SharedNoiseModel& model = nullptr); + const gtsam::noiseModel::Base* model = nullptr); gtsam::TriangulationResult triangulateSafe( const gtsam::CameraSetCal3Bundler& cameras, const gtsam::Point2Vector& measurements, @@ -732,22 +732,22 @@ gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses, const gtsam::Point2Vector& measurements, double rank_tol = 1e-9, bool optimize = false, - const gtsam::SharedNoiseModel& model = nullptr, + const gtsam::noiseModel::Base* model = nullptr, const bool useLOST = false); gtsam::Point3 triangulatePoint3(const gtsam::CameraSetCal3Fisheye& cameras, const gtsam::Point2Vector& measurements, double rank_tol, bool optimize, - const gtsam::SharedNoiseModel& model = nullptr, + const gtsam::noiseModel::Base* model = nullptr, const bool useLOST = false); gtsam::Point3 triangulateNonlinear(const gtsam::Pose3Vector& poses, gtsam::Cal3Fisheye* sharedCal, const gtsam::Point2Vector& measurements, const gtsam::Point3& initialEstimate, - const gtsam::SharedNoiseModel& model = nullptr); + const gtsam::noiseModel::Base* model = nullptr); gtsam::Point3 triangulateNonlinear(const gtsam::CameraSetCal3Fisheye& cameras, const gtsam::Point2Vector& measurements, const gtsam::Point3& initialEstimate, - const gtsam::SharedNoiseModel& model = nullptr); + const gtsam::noiseModel::Base* model = nullptr); gtsam::TriangulationResult triangulateSafe( const gtsam::CameraSetCal3Fisheye& cameras, const gtsam::Point2Vector& measurements, @@ -763,22 +763,22 @@ gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses, const gtsam::Point2Vector& measurements, double rank_tol = 1e-9, bool optimize = false, - const gtsam::SharedNoiseModel& model = nullptr, + const gtsam::noiseModel::Base* model = nullptr, const bool useLOST = false); gtsam::Point3 triangulatePoint3(const gtsam::CameraSetCal3Unified& cameras, const gtsam::Point2Vector& measurements, double rank_tol, bool optimize, - const gtsam::SharedNoiseModel& model = nullptr, + const gtsam::noiseModel::Base* model = nullptr, const bool useLOST = false); gtsam::Point3 triangulateNonlinear(const gtsam::Pose3Vector& poses, gtsam::Cal3Unified* sharedCal, const gtsam::Point2Vector& measurements, const gtsam::Point3& initialEstimate, - const gtsam::SharedNoiseModel& model = nullptr); + const gtsam::noiseModel::Base* model = nullptr); gtsam::Point3 triangulateNonlinear(const gtsam::CameraSetCal3Unified& cameras, const gtsam::Point2Vector& measurements, const gtsam::Point3& initialEstimate, - const gtsam::SharedNoiseModel& model = nullptr); + const gtsam::noiseModel::Base* model = nullptr); gtsam::TriangulationResult triangulateSafe( const gtsam::CameraSetCal3Unified& cameras, const gtsam::Point2Vector& measurements, @@ -793,13 +793,13 @@ gtsam::Point3 triangulatePoint3( const gtsam::CameraSet& cameras, const gtsam::SphericalCamera::MeasurementVector& measurements, double rank_tol, bool optimize, - const gtsam::SharedNoiseModel& model = nullptr, + const gtsam::noiseModel::Base* model = nullptr, const bool useLOST = false); gtsam::Point3 triangulateNonlinear( const gtsam::CameraSet& cameras, const gtsam::SphericalCamera::MeasurementVector& measurements, const gtsam::Point3& initialEstimate, - const gtsam::SharedNoiseModel& model = nullptr); + const gtsam::noiseModel::Base* model = nullptr); gtsam::TriangulationResult triangulateSafe( const gtsam::CameraSet& cameras, const gtsam::SphericalCamera::MeasurementVector& measurements, diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i index 6a221edac1..2972899687 100644 --- a/gtsam/gtsam.i +++ b/gtsam/gtsam.i @@ -187,8 +187,8 @@ void insertBackprojections(gtsam::Values& values, void insertProjectionFactors( gtsam::NonlinearFactorGraph& graph, gtsam::Key i, const gtsam::Vector& J, gtsam::ConstMatrixView Z, - const std::shared_ptr& model, - const gtsam::Cal3_S2::shared_ptr K, + const gtsam::noiseModel::Base* model, + const gtsam::Cal3_S2* K, const gtsam::Pose3& body_P_sensor = gtsam::Pose3()); gtsam::Matrix reprojectionErrors(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& values); diff --git a/gtsam/hybrid/hybrid.i b/gtsam/hybrid/hybrid.i index e0a7f8d52c..0cbf8d6795 100644 --- a/gtsam/hybrid/hybrid.i +++ b/gtsam/hybrid/hybrid.i @@ -134,7 +134,7 @@ virtual class HybridFactor : gtsam::Factor { double error(const gtsam::HybridValues& hybridValues) const; gtsam::AlgebraicDecisionTreeKey errorTree( const gtsam::VectorValues& continuousValues) const; - std::shared_ptr restrict( + gtsam::Factor* restrict( const gtsam::DiscreteValues& discreteValues) const; }; @@ -164,19 +164,19 @@ virtual class HybridConditional : gtsam::HybridFactor { gtsam::GaussianConditional* asGaussian() const; gtsam::DiscreteConditional* asDiscrete() const; - std::shared_ptr inner() const; + gtsam::Factor* inner() const; }; #include class HybridGaussianFactor : gtsam::HybridFactor { HybridGaussianFactor( const gtsam::DiscreteKey& discreteKey, - const std::vector& factors); + const std::vector& factors); HybridGaussianFactor( const gtsam::DiscreteKey& discreteKey, - const std::vector>& + const std::vector>& factorPairs); - std::pair operator()( + std::pair operator()( const gtsam::DiscreteValues& assignment) const; }; @@ -188,7 +188,7 @@ class HybridGaussianConditional : gtsam::HybridGaussianFactor { const gtsam::HybridGaussianConditional::Conditionals& conditionals); HybridGaussianConditional( const gtsam::DiscreteKey& discreteParent, - const std::vector& conditionals); + const std::vector& conditionals); HybridGaussianConditional( const gtsam::DiscreteKey& discreteParent, gtsam::Key key, const gtsam::Matrix& A, gtsam::Key parent, @@ -214,7 +214,7 @@ class HybridGaussianConditional : gtsam::HybridGaussianFactor { double evaluate(const gtsam::HybridValues& values) const; // double operator()(const gtsam::HybridValues &values) const; - gtsam::HybridGaussianConditional::shared_ptr prune( + gtsam::HybridGaussianConditional* prune( const gtsam::DiscreteConditional &discreteProbs) const; bool pruned() const; @@ -224,7 +224,7 @@ class HybridGaussianConditional : gtsam::HybridGaussianFactor { class HybridBayesTreeClique { HybridBayesTreeClique(); HybridBayesTreeClique(const gtsam::HybridConditional* conditional); - const gtsam::HybridConditional::shared_ptr& conditional() const; + const gtsam::HybridConditional* conditional() const; bool isRoot() const; // double evaluate(const gtsam::HybridValues& values) const; }; @@ -255,12 +255,12 @@ virtual class HybridBayesTree { class HybridBayesNet { HybridBayesNet(); void push_back( - const std::shared_ptr& conditional); + const gtsam::HybridGaussianConditional* conditional); void push_back( - const std::shared_ptr& conditional); + const gtsam::GaussianConditional* conditional); void push_back( - const std::shared_ptr& conditional); - void push_back(gtsam::HybridConditional::shared_ptr conditional); + const gtsam::DiscreteConditional* conditional); + void push_back(gtsam::HybridConditional* conditional); bool empty() const; size_t size() const; @@ -392,22 +392,22 @@ virtual class HybridGaussianFactorGraph : gtsam::HybridFactorGraph { const gtsam::VectorValues& continuousValues) const; // Sequential Elimination - std::shared_ptr eliminateSequential( + gtsam::HybridBayesNet* eliminateSequential( gtsam::HybridGaussianFactorGraph::OptionalOrderingType orderingType = std::nullopt, const gtsam::HybridGaussianFactorGraph::Eliminate& function = gtsam::HybridGaussianFactorGraph::Eliminate( gtsam::HybridGaussianFactorGraph::EliminationTraitsType::DefaultEliminate), gtsam::HybridGaussianFactorGraph::OptionalVariableIndex variableIndex = std::nullopt) const; - std::shared_ptr eliminateSequential( + gtsam::HybridBayesNet* eliminateSequential( const gtsam::Ordering& ordering, const gtsam::HybridGaussianFactorGraph::Eliminate& function = gtsam::HybridGaussianFactorGraph::Eliminate( gtsam::HybridGaussianFactorGraph::EliminationTraitsType::DefaultEliminate), gtsam::HybridGaussianFactorGraph::OptionalVariableIndex variableIndex = std::nullopt) const; - pair, - std::shared_ptr> + pair eliminatePartialSequential( const gtsam::Ordering& ordering, const gtsam::HybridGaussianFactorGraph::Eliminate& function = @@ -415,8 +415,8 @@ virtual class HybridGaussianFactorGraph : gtsam::HybridFactorGraph { gtsam::HybridGaussianFactorGraph::EliminationTraitsType::DefaultEliminate), gtsam::HybridGaussianFactorGraph::OptionalVariableIndex variableIndex = std::nullopt) const; - pair, - std::shared_ptr> + pair eliminatePartialSequential( const gtsam::KeyVector& variables, const gtsam::HybridGaussianFactorGraph::Eliminate& function = @@ -426,22 +426,22 @@ virtual class HybridGaussianFactorGraph : gtsam::HybridFactorGraph { const; // Multifrontal Elimination - std::shared_ptr eliminateMultifrontal( + gtsam::HybridBayesTree* eliminateMultifrontal( gtsam::HybridGaussianFactorGraph::OptionalOrderingType orderingType = std::nullopt, const gtsam::HybridGaussianFactorGraph::Eliminate& function = gtsam::HybridGaussianFactorGraph::Eliminate( gtsam::HybridGaussianFactorGraph::EliminationTraitsType::DefaultEliminate), gtsam::HybridGaussianFactorGraph::OptionalVariableIndex variableIndex = std::nullopt) const; - std::shared_ptr eliminateMultifrontal( + gtsam::HybridBayesTree* eliminateMultifrontal( const gtsam::Ordering& ordering, const gtsam::HybridGaussianFactorGraph::Eliminate& function = gtsam::HybridGaussianFactorGraph::Eliminate( gtsam::HybridGaussianFactorGraph::EliminationTraitsType::DefaultEliminate), gtsam::HybridGaussianFactorGraph::OptionalVariableIndex variableIndex = std::nullopt) const; - pair, - std::shared_ptr> + pair eliminatePartialMultifrontal( const gtsam::Ordering& ordering, const gtsam::HybridGaussianFactorGraph::Eliminate& function = @@ -449,8 +449,8 @@ virtual class HybridGaussianFactorGraph : gtsam::HybridFactorGraph { gtsam::HybridGaussianFactorGraph::EliminationTraitsType::DefaultEliminate), gtsam::HybridGaussianFactorGraph::OptionalVariableIndex variableIndex = std::nullopt) const; - pair, - std::shared_ptr> + pair eliminatePartialMultifrontal( const gtsam::KeyVector& variables, const gtsam::HybridGaussianFactorGraph::Eliminate& function = @@ -484,7 +484,7 @@ virtual class HybridNonlinearFactorGraph : gtsam::HybridFactorGraph { gtsam::AlgebraicDecisionTreeKey errorTree(const gtsam::Values& continuousValues) const; - std::shared_ptr linearize( + gtsam::HybridGaussianFactorGraph* linearize( const gtsam::Values& continuousValues) const; gtsam::AlgebraicDecisionTreeKey discretePosterior( @@ -514,7 +514,7 @@ class HybridNonlinearFactor : gtsam::HybridFactor { gtsam::AlgebraicDecisionTreeKey errorTree( const gtsam::Values& continuousValues) const; - std::shared_ptr linearize( + gtsam::HybridGaussianFactor* linearize( const gtsam::Values& continuousValues) const; }; diff --git a/gtsam/linear/linear.i b/gtsam/linear/linear.i index 023aad5845..0307f2cd6c 100644 --- a/gtsam/linear/linear.i +++ b/gtsam/linear/linear.i @@ -406,12 +406,12 @@ virtual class Custom: gtsam::noiseModel::mEstimator::Base { virtual class Robust : gtsam::noiseModel::Base { Robust(const gtsam::noiseModel::mEstimator::Base* robust, const gtsam::noiseModel::Base* noise); static gtsam::noiseModel::Robust* Create( - const std::shared_ptr& robust, - const std::shared_ptr noise); + const gtsam::noiseModel::mEstimator::Base* robust, + const gtsam::noiseModel::Base* noise); // Access to the contained models. - const std::shared_ptr& robust() const; - const std::shared_ptr& noise() const; + const gtsam::noiseModel::mEstimator::Base* robust() const; + const gtsam::noiseModel::Base* noise() const; // enabling serialization functionality void serializable() const; @@ -428,7 +428,7 @@ class Sampler { // Standard Interface size_t dim() const; gtsam::Vector sigmas() const; - const std::shared_ptr& model() const; + const gtsam::noiseModel::Diagonal* model() const; gtsam::Vector sample() const; }; @@ -535,7 +535,7 @@ virtual class JacobianFactor : gtsam::GaussianFactor { void setModel(bool anyConstrained, const gtsam::Vector& sigmas); - const std::shared_ptr& get_model() const; + const gtsam::noiseModel::Diagonal* get_model() const; // enabling serialization functionality void serialize() const; @@ -570,36 +570,36 @@ virtual class HessianFactor : gtsam::GaussianFactor { #include class GaussianFactorGraph { - std::shared_ptr eliminateSequential( + gtsam::GaussianBayesNet* eliminateSequential( gtsam::GaussianFactorGraph::OptionalOrderingType orderingType = std::nullopt, const gtsam::GaussianFactorGraph::Eliminate& function = gtsam::GaussianFactorGraph::Eliminate( gtsam::GaussianFactorGraph::EliminationTraitsType::DefaultEliminate), gtsam::GaussianFactorGraph::OptionalVariableIndex variableIndex = std::nullopt) const; - std::shared_ptr eliminateSequential( + gtsam::GaussianBayesNet* eliminateSequential( const gtsam::Ordering& ordering, const gtsam::GaussianFactorGraph::Eliminate& function = gtsam::GaussianFactorGraph::Eliminate( gtsam::GaussianFactorGraph::EliminationTraitsType::DefaultEliminate), gtsam::GaussianFactorGraph::OptionalVariableIndex variableIndex = std::nullopt) const; - std::shared_ptr eliminateMultifrontal( + gtsam::GaussianBayesTree* eliminateMultifrontal( gtsam::GaussianFactorGraph::OptionalOrderingType orderingType = std::nullopt, const gtsam::GaussianFactorGraph::Eliminate& function = gtsam::GaussianFactorGraph::Eliminate( gtsam::GaussianFactorGraph::EliminationTraitsType::DefaultEliminate), gtsam::GaussianFactorGraph::OptionalVariableIndex variableIndex = std::nullopt) const; - std::shared_ptr eliminateMultifrontal( + gtsam::GaussianBayesTree* eliminateMultifrontal( const gtsam::Ordering& ordering, const gtsam::GaussianFactorGraph::Eliminate& function = gtsam::GaussianFactorGraph::Eliminate( gtsam::GaussianFactorGraph::EliminationTraitsType::DefaultEliminate), gtsam::GaussianFactorGraph::OptionalVariableIndex variableIndex = std::nullopt) const; - pair, - std::shared_ptr> + pair eliminatePartialSequential( const gtsam::Ordering& ordering, const gtsam::GaussianFactorGraph::Eliminate& function = @@ -607,8 +607,8 @@ class GaussianFactorGraph { gtsam::GaussianFactorGraph::EliminationTraitsType::DefaultEliminate), gtsam::GaussianFactorGraph::OptionalVariableIndex variableIndex = std::nullopt) const; - pair, - std::shared_ptr> + pair eliminatePartialSequential( const gtsam::KeyVector& variables, const gtsam::GaussianFactorGraph::Eliminate& function = @@ -616,8 +616,8 @@ class GaussianFactorGraph { gtsam::GaussianFactorGraph::EliminationTraitsType::DefaultEliminate), gtsam::GaussianFactorGraph::OptionalVariableIndex variableIndex = std::nullopt) const; - pair, - std::shared_ptr> + pair eliminatePartialMultifrontal( const gtsam::Ordering& ordering, const gtsam::GaussianFactorGraph::Eliminate& function = @@ -625,8 +625,8 @@ class GaussianFactorGraph { gtsam::GaussianFactorGraph::EliminationTraitsType::DefaultEliminate), gtsam::GaussianFactorGraph::OptionalVariableIndex variableIndex = std::nullopt) const; - pair, - std::shared_ptr> + pair eliminatePartialMultifrontal( const gtsam::KeyVector& variables, const gtsam::GaussianFactorGraph::Eliminate& function = @@ -634,21 +634,21 @@ class GaussianFactorGraph { gtsam::GaussianFactorGraph::EliminationTraitsType::DefaultEliminate), gtsam::GaussianFactorGraph::OptionalVariableIndex variableIndex = std::nullopt) const; - std::shared_ptr marginalMultifrontalBayesNet( + gtsam::GaussianBayesNet* marginalMultifrontalBayesNet( const gtsam::Ordering& variables, const gtsam::GaussianFactorGraph::Eliminate& function = gtsam::GaussianFactorGraph::Eliminate( gtsam::GaussianFactorGraph::EliminationTraitsType::DefaultEliminate), gtsam::GaussianFactorGraph::OptionalVariableIndex variableIndex = std::nullopt) const; - std::shared_ptr marginalMultifrontalBayesNet( + gtsam::GaussianBayesNet* marginalMultifrontalBayesNet( const gtsam::KeyVector& variables, const gtsam::GaussianFactorGraph::Eliminate& function = gtsam::GaussianFactorGraph::Eliminate( gtsam::GaussianFactorGraph::EliminationTraitsType::DefaultEliminate), gtsam::GaussianFactorGraph::OptionalVariableIndex variableIndex = std::nullopt) const; - std::shared_ptr marginalMultifrontalBayesNet( + gtsam::GaussianBayesNet* marginalMultifrontalBayesNet( const gtsam::Ordering& variables, const gtsam::Ordering& marginalizedVariableOrdering, const gtsam::GaussianFactorGraph::Eliminate& function = @@ -656,7 +656,7 @@ class GaussianFactorGraph { gtsam::GaussianFactorGraph::EliminationTraitsType::DefaultEliminate), gtsam::GaussianFactorGraph::OptionalVariableIndex variableIndex = std::nullopt) const; - std::shared_ptr marginalMultifrontalBayesNet( + gtsam::GaussianBayesNet* marginalMultifrontalBayesNet( const gtsam::KeyVector& variables, const gtsam::Ordering& marginalizedVariableOrdering, const gtsam::GaussianFactorGraph::Eliminate& function = @@ -664,7 +664,7 @@ class GaussianFactorGraph { gtsam::GaussianFactorGraph::EliminationTraitsType::DefaultEliminate), gtsam::GaussianFactorGraph::OptionalVariableIndex variableIndex = std::nullopt) const; - std::shared_ptr marginal( + gtsam::GaussianFactorGraph* marginal( const gtsam::KeyVector& variables, const gtsam::GaussianFactorGraph::Eliminate& function = gtsam::GaussianFactorGraph::Eliminate( @@ -680,7 +680,7 @@ class GaussianFactorGraph { gtsam::DefaultKeyFormatter) const; bool equals(const gtsam::GaussianFactorGraph& fg, double tol) const; size_t size() const; - const std::shared_ptr at(size_t idx) const; + const gtsam::GaussianFactor* at(size_t idx) const; gtsam::KeySet keys() const; gtsam::KeyVector keyVector() const; bool exists(size_t idx) const; @@ -695,19 +695,19 @@ class GaussianFactorGraph { void add(const gtsam::Vector& b); void add( gtsam::Key key1, const gtsam::Matrix& A1, const gtsam::Vector& b, - const std::shared_ptr& model = - gtsam::SharedDiagonal()); + const gtsam::noiseModel::Diagonal* model = + nullptr); void add( gtsam::Key key1, const gtsam::Matrix& A1, gtsam::Key key2, const gtsam::Matrix& A2, const gtsam::Vector& b, - const std::shared_ptr& model = - gtsam::SharedDiagonal()); + const gtsam::noiseModel::Diagonal* model = + nullptr); void add( gtsam::Key key1, const gtsam::Matrix& A1, gtsam::Key key2, const gtsam::Matrix& A2, gtsam::Key key3, const gtsam::Matrix& A3, const gtsam::Vector& b, - const std::shared_ptr& model = - gtsam::SharedDiagonal()); + const gtsam::noiseModel::Diagonal* model = + nullptr); // error and probability double error(const gtsam::VectorValues& x) const; @@ -883,7 +883,7 @@ virtual class GaussianBayesNet { gtsam::VectorValues backSubstituteTranspose(const gtsam::VectorValues& gx) const; // FactorGraph derived interface - const std::shared_ptr at(size_t idx) const; + const gtsam::GaussianConditional* at(size_t idx) const; gtsam::KeySet keys() const; gtsam::KeyVector keyVector() const; bool exists(size_t idx) const; @@ -911,7 +911,7 @@ class GaussianBayesTreeClique { bool equals(const gtsam::GaussianBayesTreeClique& other, double tol) const; void print(string s = "", const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter); - const gtsam::GaussianConditional::shared_ptr& conditional() const; + const gtsam::GaussianConditional* conditional() const; bool isRoot() const; gtsam::GaussianBayesTreeClique* parent() const; size_t nrChildren() const; @@ -952,31 +952,31 @@ virtual class GaussianBayesTree { const gtsam::KeyVector& queryKeys) const; gtsam::JointMarginal jointMarginalInformation( const gtsam::KeyVector& queryKeys) const; - std::shared_ptr marginalFactor( + gtsam::GaussianConditional* marginalFactor( gtsam::Key j, const gtsam::GaussianFactorGraph::Eliminate& function = gtsam::GaussianFactorGraph::Eliminate( gtsam::GaussianFactorGraph::EliminationTraitsType::DefaultEliminate)) const; - std::shared_ptr joint( + gtsam::GaussianFactorGraph* joint( gtsam::Key j1, gtsam::Key j2, const gtsam::GaussianFactorGraph::Eliminate& function = gtsam::GaussianFactorGraph::Eliminate( gtsam::GaussianFactorGraph::EliminationTraitsType::DefaultEliminate)) const; - std::shared_ptr joint( + gtsam::GaussianFactorGraph* joint( const gtsam::KeyVector& keys, const gtsam::GaussianFactorGraph::Eliminate& function = gtsam::GaussianFactorGraph::Eliminate( gtsam::GaussianFactorGraph::EliminationTraitsType::DefaultEliminate)) const; - std::shared_ptr jointBayesNet( + gtsam::GaussianBayesNet* jointBayesNet( gtsam::Key j1, gtsam::Key j2, const gtsam::GaussianFactorGraph::Eliminate& function = gtsam::GaussianFactorGraph::Eliminate( gtsam::GaussianFactorGraph::EliminationTraitsType::DefaultEliminate)) const; - std::shared_ptr jointBayesNet( + gtsam::GaussianBayesNet* jointBayesNet( const gtsam::KeyVector& keys, const gtsam::GaussianFactorGraph::Eliminate& function = gtsam::GaussianFactorGraph::Eliminate( @@ -1057,7 +1057,7 @@ virtual class PCGSolverParameters : gtsam::ConjugateGradientParameters { PCGSolverParameters(const gtsam::PreconditionerParameters* preconditioner); void print(string s = ""); - std::shared_ptr preconditioner; + gtsam::PreconditionerParameters* preconditioner; bool parallel; size_t numThreads; }; @@ -1076,7 +1076,6 @@ virtual class SubgraphSolver { #include class KalmanFilter { KalmanFilter(size_t n); - // gtsam::GaussianDensity* init(gtsam::Vector x0, const gtsam::SharedDiagonal& P0); gtsam::GaussianDensity* init( const gtsam::Vector& x0, const gtsam::Matrix& P0) const; void print(string s = "") const; diff --git a/gtsam/navigation/navigation.i b/gtsam/navigation/navigation.i index c2075b6268..f32b0a504e 100644 --- a/gtsam/navigation/navigation.i +++ b/gtsam/navigation/navigation.i @@ -1201,7 +1201,7 @@ class NavStateImuEKF : gtsam::LeftLinearEKF { // Accessors const gtsam::Matrix9& processNoise() const; const gtsam::Vector3& gravity() const; - const std::shared_ptr& params() const; + const gtsam::PreintegrationParams* params() const; // Static methods static gtsam::NavState Gravity(const gtsam::Vector3& n_gravity, double dt); @@ -1232,7 +1232,7 @@ class Gal3ImuEKF : gtsam::InvariantEKF { // Accessors const gtsam::Gal3ImuEKF::Covariance& processNoise() const; const gtsam::Vector3& gravity() const; - const std::shared_ptr& params() const; + const gtsam::PreintegrationParams* params() const; // Static methods static gtsam::Gal3 Gravity(const gtsam::Vector3& g_n, double dt); @@ -1263,7 +1263,7 @@ class ContactMeasurement { class LeggedEstimatorParams { LeggedEstimatorParams(); - std::shared_ptr preintegrationParams; + gtsam::PreintegrationParams* preintegrationParams; gtsam::Pose3 body_P_imu; double footholdProcessSigma; double footholdInitSigma; diff --git a/gtsam/nonlinear/custom.i b/gtsam/nonlinear/custom.i index 0d195a91b6..ecf6ffd645 100644 --- a/gtsam/nonlinear/custom.i +++ b/gtsam/nonlinear/custom.i @@ -27,7 +27,7 @@ virtual class CustomFactor : gtsam::NoiseModelFactor { * cf = CustomFactor(noise_model, keys, error_func) * ``` */ - CustomFactor(const gtsam::SharedNoiseModel& noiseModel, + CustomFactor(const gtsam::noiseModel::Base* noiseModel, const gtsam::KeyVector& keys, const gtsam::CustomErrorFunction& errorFunction); diff --git a/gtsam/nonlinear/nonlinear.i b/gtsam/nonlinear/nonlinear.i index 86b3c001ac..d102ac0426 100644 --- a/gtsam/nonlinear/nonlinear.i +++ b/gtsam/nonlinear/nonlinear.i @@ -59,7 +59,7 @@ virtual class NonlinearFactorGraph { void replace(size_t i, gtsam::NonlinearFactor* factors); void resize(size_t size); size_t nrFactors() const; - const std::shared_ptr at(size_t idx) const; + const gtsam::NonlinearFactor* at(size_t idx) const; void push_back(const gtsam::NonlinearFactorGraph& factors); void push_back(gtsam::NonlinearFactor* factor); void add(gtsam::NonlinearFactor* factor); @@ -151,7 +151,7 @@ virtual class NonlinearFactor : gtsam::Factor { #include virtual class NoiseModelFactor : gtsam::NonlinearFactor { - const std::shared_ptr& noiseModel() const; + const gtsam::noiseModel::Base* noiseModel() const; gtsam::NoiseModelFactor* cloneWithNewNoiseModel( gtsam::noiseModel::Base* newNoise) const; gtsam::Vector unwhitenedError( @@ -183,10 +183,10 @@ class Marginals { #include virtual class LinearContainerFactor : gtsam::NonlinearFactor { LinearContainerFactor( - const std::shared_ptr& factor, + const gtsam::GaussianFactor* factor, const gtsam::Values& linearizationPoint = gtsam::Values()); - const std::shared_ptr& factor() const; + const gtsam::GaussianFactor* factor() const; // const std::optional& linearizationPoint() const; bool isJacobian() const; @@ -546,31 +546,31 @@ class ISAM2 { const string& filename, const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const; - std::shared_ptr marginalFactor( + gtsam::GaussianConditional* marginalFactor( gtsam::Key j, const gtsam::GaussianFactorGraph::Eliminate& function = gtsam::GaussianFactorGraph::Eliminate( gtsam::GaussianFactorGraph::EliminationTraitsType::DefaultEliminate)) const; - std::shared_ptr joint( + gtsam::GaussianFactorGraph* joint( gtsam::Key j1, gtsam::Key j2, const gtsam::GaussianFactorGraph::Eliminate& function = gtsam::GaussianFactorGraph::Eliminate( gtsam::GaussianFactorGraph::EliminationTraitsType::DefaultEliminate)) const; - std::shared_ptr joint( + gtsam::GaussianFactorGraph* joint( const gtsam::KeyVector& keys, const gtsam::GaussianFactorGraph::Eliminate& function = gtsam::GaussianFactorGraph::Eliminate( gtsam::GaussianFactorGraph::EliminationTraitsType::DefaultEliminate)) const; - std::shared_ptr jointBayesNet( + gtsam::GaussianBayesNet* jointBayesNet( gtsam::Key j1, gtsam::Key j2, const gtsam::GaussianFactorGraph::Eliminate& function = gtsam::GaussianFactorGraph::Eliminate( gtsam::GaussianFactorGraph::EliminationTraitsType::DefaultEliminate)) const; - std::shared_ptr jointBayesNet( + gtsam::GaussianBayesNet* jointBayesNet( const gtsam::KeyVector& keys, const gtsam::GaussianFactorGraph::Eliminate& function = gtsam::GaussianFactorGraph::Eliminate( @@ -712,7 +712,7 @@ virtual class WnoaMotionFactor : gtsam::NoiseModelFactor { template virtual class WnoaInterpFactor : gtsam::NoiseModelFactor { - WnoaInterpFactor(const gtsam::NoiseModelFactor::shared_ptr inner_factor, + WnoaInterpFactor(const gtsam::NoiseModelFactor* inner_factor, const std::set estimated_states, const std::set interp_states, const gtsam::Vector q_psd_diag, @@ -852,10 +852,10 @@ template virtual class ExtendedPriorFactor : gtsam::NoiseModelFactor { ExtendedPriorFactor(gtsam::Key key, const T& origin, - const gtsam::SharedNoiseModel& noiseModel); + const gtsam::noiseModel::Base* noiseModel); ExtendedPriorFactor(gtsam::Key key, const T& origin, const gtsam::Vector& mean, - const gtsam::SharedNoiseModel& noiseModel); + const gtsam::noiseModel::Base* noiseModel); ExtendedPriorFactor(gtsam::Key key, const T& origin, const gtsam::Matrix& covariance); ExtendedPriorFactor(gtsam::Key key, const T& origin, @@ -887,10 +887,10 @@ virtual class ConcentratedGaussian : gtsam::ExtendedPriorFactor { // Constructors mirroring header (origin terminology) ConcentratedGaussian( gtsam::Key key, const T& origin, - const gtsam::noiseModel::Gaussian::shared_ptr& noiseModel); + const gtsam::noiseModel::Gaussian* noiseModel); ConcentratedGaussian( gtsam::Key key, const T& origin, const gtsam::Vector& mean, - const gtsam::noiseModel::Gaussian::shared_ptr& noiseModel); + const gtsam::noiseModel::Gaussian* noiseModel); ConcentratedGaussian(gtsam::Key key, const T& origin, const gtsam::Matrix& covariance); ConcentratedGaussian(gtsam::Key key, const T& origin, @@ -1124,7 +1124,7 @@ virtual class ExtendedKalmanFilter { T predict(const gtsam::NoiseModelFactor& motionFactor); T update(const gtsam::NoiseModelFactor& measurementFactor); - const gtsam::JacobianFactor::shared_ptr Density() const; + const gtsam::JacobianFactor* Density() const; }; } // namespace gtsam diff --git a/gtsam/sfm/sfm.i b/gtsam/sfm/sfm.i index 2a3f50d722..5a19c6b64c 100644 --- a/gtsam/sfm/sfm.i +++ b/gtsam/sfm/sfm.i @@ -58,10 +58,10 @@ class SfmData { const gtsam::PinholeCamera& camera(size_t idx) const; gtsam::NonlinearFactorGraph generalSfmFactors( - const gtsam::SharedNoiseModel& model = + const gtsam::noiseModel::Base* model = gtsam::noiseModel::Isotropic::Sigma(2, 1.0)) const; gtsam::NonlinearFactorGraph sfmFactorGraph( - const gtsam::SharedNoiseModel& model = + const gtsam::noiseModel::Base* model = gtsam::noiseModel::Isotropic::Sigma(2, 1.0), std::optional fixedCamera = 0, std::optional fixedPoint = 0) const; @@ -133,7 +133,7 @@ class UnaryMeasurement { const gtsam::noiseModel::Base* model); gtsam::Key key() const; const T& measured() const; - const std::shared_ptr& noiseModel() const; + const gtsam::noiseModel::Base* noiseModel() const; }; typedef gtsam::UnaryMeasurement UnaryMeasurementPose3; @@ -148,7 +148,7 @@ class BinaryMeasurement { gtsam::Key key1() const; gtsam::Key key2() const; const T& measured() const; - const std::shared_ptr& noiseModel() const; + const gtsam::noiseModel::Base* noiseModel() const; }; typedef gtsam::BinaryMeasurement BinaryMeasurementUnit3; @@ -376,7 +376,7 @@ class LocationRecovery { bool bilinear = true) const; void addAnchorPrior(gtsam::Key anchorKey, gtsam::NonlinearFactorGraph @graph, - const gtsam::SharedNoiseModel& priorNoiseModel = + const gtsam::noiseModel::Base* priorNoiseModel = gtsam::noiseModel::Isotropic::Sigma(3, 0.01)) const; gtsam::Values initializeRandomly( const std::set& keys, size_t numEdges, bool bilinear, @@ -397,7 +397,7 @@ class TranslationRecovery : gtsam::LocationRecovery { const std::vector>& betweenTranslations, gtsam::NonlinearFactorGraph @graph, - const gtsam::SharedNoiseModel& priorNoiseModel = + const gtsam::noiseModel::Base* priorNoiseModel = gtsam::noiseModel::Isotropic::Sigma(3, 0.01)) const; gtsam::NonlinearFactorGraph buildGraph( const std::vector>& diff --git a/gtsam/slam/slam.i b/gtsam/slam/slam.i index 3e665432ee..b38f2e4a18 100644 --- a/gtsam/slam/slam.i +++ b/gtsam/slam/slam.i @@ -88,7 +88,7 @@ virtual class GenericProjectionFactor : gtsam::NoiseModelFactor { const POSE& body_P_sensor); const gtsam::Point2& measured() const; - const std::shared_ptr calibration() const; + const CALIBRATION* calibration() const; bool verboseCheirality() const; bool throwCheirality() const; @@ -231,18 +231,18 @@ virtual class SmartProjectionFactor : gtsam::SmartFactorBase { gtsam::TriangulationResult triangulateSafe(const gtsam::CameraSet& cameras) const; bool triangulateForLinearize(const gtsam::CameraSet& cameras) const; - gtsam::This::SharedHessianFactor createHessianFactor( + gtsam::HessianFactor* createHessianFactor( const gtsam::CameraSet& cameras, const double _lambda = 0.0, bool diagonalDamping = false) const; - gtsam::This::SharedJacobianFactor createJacobianQFactor( + gtsam::JacobianFactor* createJacobianQFactor( const gtsam::CameraSet& cameras, double _lambda) const; - gtsam::This::SharedJacobianFactor createJacobianQFactor( + gtsam::JacobianFactor* createJacobianQFactor( const gtsam::Values& values, double _lambda) const; gtsam::JacobianFactor* createJacobianSVDFactor( const gtsam::CameraSet& cameras, double _lambda) const; - gtsam::This::SharedHessianFactor linearizeToHessian( + gtsam::HessianFactor* linearizeToHessian( const gtsam::Values& values, double _lambda = 0.0) const; - gtsam::This::SharedJacobianFactor linearizeToJacobian( + gtsam::JacobianFactor* linearizeToJacobian( const gtsam::Values& values, double _lambda = 0.0) const; gtsam::GaussianFactor* linearizeDamped(const gtsam::CameraSet& cameras, @@ -321,7 +321,7 @@ virtual class SmartProjectionRigFactor : gtsam::SmartProjectionFactor { const gtsam::FastVector& cameraIds = gtsam::FastVector()); const gtsam::KeyVector& nonUniqueKeys() const; - const std::shared_ptr& cameraRig() const; + const gtsam::This::Cameras* cameraRig() const; const gtsam::FastVector& cameraIds() const; }; @@ -346,7 +346,7 @@ virtual class GenericStereoFactor : gtsam::NoiseModelFactor { bool throwCheirality, bool verboseCheirality, POSE body_P_sensor); const gtsam::StereoPoint2& measured() const; - const gtsam::Cal3_S2Stereo::shared_ptr calibration() const; + const gtsam::Cal3_S2Stereo* calibration() const; // enabling serialization functionality void serialize() const; @@ -498,7 +498,7 @@ enum KernelFunctionType { pair load2D( const string& filename, - std::shared_ptr model = nullptr, + gtsam::noiseModel::Base* model = nullptr, size_t maxIndex = 0, bool addNoise = false, bool smart = true, gtsam::NoiseFormat noiseFormat = gtsam::NoiseFormat::NoiseFormatAUTO, gtsam::KernelFunctionType kernelFunctionType = @@ -506,10 +506,10 @@ pair load2D( void save2D(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& config, - const std::shared_ptr model, + const gtsam::noiseModel::Diagonal* model, const string& filename); -// std::vector::shared_ptr> +// Shared BetweenFactor container used by the MATLAB wrapper. // Used in Matlab wrapper class BetweenFactorPose2s { BetweenFactorPose2s(); @@ -519,10 +519,10 @@ class BetweenFactorPose2s { }; gtsam::BetweenFactorPose2s parse2DFactors( const string& filename, - const std::shared_ptr& model = nullptr, + const gtsam::noiseModel::Diagonal* model = nullptr, size_t maxIndex = 0); -// std::vector::shared_ptr> +// Shared BetweenFactor container used by the MATLAB wrapper. // Used in Matlab wrapper class BetweenFactorPose3s { BetweenFactorPose3s(); @@ -531,7 +531,7 @@ class BetweenFactorPose3s { void push_back(const gtsam::BetweenFactor* factor); }; -// std::vector::shared_ptr> +// Shared BetweenFactor container used by the MATLAB wrapper. // Used in Matlab wrapper class BetweenFactorSL4s { BetweenFactorSL4s(); @@ -542,7 +542,7 @@ class BetweenFactorSL4s { gtsam::BetweenFactorPose3s parse3DFactors( const string& filename, - const std::shared_ptr& model = nullptr, + const gtsam::noiseModel::Diagonal* model = nullptr, size_t maxIndex = 0); pair load3D( @@ -602,8 +602,8 @@ template & elements); #include -std::shared_ptr ConvertNoiseModel( - const std::shared_ptr& model, size_t n, +gtsam::noiseModel::Base* ConvertNoiseModel( + const gtsam::noiseModel::Base* model, size_t n, bool defaultToUnit = true); template #include virtual class SymbolicFactorGraph { - std::shared_ptr eliminateSequential( + gtsam::SymbolicBayesNet* eliminateSequential( gtsam::SymbolicFactorGraph::OptionalOrderingType orderingType = std::nullopt, const gtsam::SymbolicFactorGraph::Eliminate& function = gtsam::SymbolicFactorGraph::Eliminate( gtsam::SymbolicFactorGraph::EliminationTraitsType::DefaultEliminate), gtsam::SymbolicFactorGraph::OptionalVariableIndex variableIndex = std::nullopt) const; - std::shared_ptr eliminateSequential( + gtsam::SymbolicBayesNet* eliminateSequential( const gtsam::Ordering& ordering, const gtsam::SymbolicFactorGraph::Eliminate& function = gtsam::SymbolicFactorGraph::Eliminate( gtsam::SymbolicFactorGraph::EliminationTraitsType::DefaultEliminate), gtsam::SymbolicFactorGraph::OptionalVariableIndex variableIndex = std::nullopt) const; - std::shared_ptr eliminateMultifrontal( + gtsam::SymbolicBayesTree* eliminateMultifrontal( gtsam::SymbolicFactorGraph::OptionalOrderingType orderingType = std::nullopt, const gtsam::SymbolicFactorGraph::Eliminate& function = gtsam::SymbolicFactorGraph::Eliminate( gtsam::SymbolicFactorGraph::EliminationTraitsType::DefaultEliminate), gtsam::SymbolicFactorGraph::OptionalVariableIndex variableIndex = std::nullopt) const; - std::shared_ptr eliminateMultifrontal( + gtsam::SymbolicBayesTree* eliminateMultifrontal( const gtsam::Ordering& ordering, const gtsam::SymbolicFactorGraph::Eliminate& function = gtsam::SymbolicFactorGraph::Eliminate( gtsam::SymbolicFactorGraph::EliminationTraitsType::DefaultEliminate), gtsam::SymbolicFactorGraph::OptionalVariableIndex variableIndex = std::nullopt) const; - pair, - std::shared_ptr> + pair eliminatePartialSequential( const gtsam::Ordering& ordering, const gtsam::SymbolicFactorGraph::Eliminate& function = @@ -61,8 +61,8 @@ virtual class SymbolicFactorGraph { gtsam::SymbolicFactorGraph::EliminationTraitsType::DefaultEliminate), gtsam::SymbolicFactorGraph::OptionalVariableIndex variableIndex = std::nullopt) const; - pair, - std::shared_ptr> + pair eliminatePartialSequential( const gtsam::KeyVector& variables, const gtsam::SymbolicFactorGraph::Eliminate& function = @@ -70,8 +70,8 @@ virtual class SymbolicFactorGraph { gtsam::SymbolicFactorGraph::EliminationTraitsType::DefaultEliminate), gtsam::SymbolicFactorGraph::OptionalVariableIndex variableIndex = std::nullopt) const; - pair, - std::shared_ptr> + pair eliminatePartialMultifrontal( const gtsam::Ordering& ordering, const gtsam::SymbolicFactorGraph::Eliminate& function = @@ -79,8 +79,8 @@ virtual class SymbolicFactorGraph { gtsam::SymbolicFactorGraph::EliminationTraitsType::DefaultEliminate), gtsam::SymbolicFactorGraph::OptionalVariableIndex variableIndex = std::nullopt) const; - pair, - std::shared_ptr> + pair eliminatePartialMultifrontal( const gtsam::KeyVector& variables, const gtsam::SymbolicFactorGraph::Eliminate& function = @@ -88,21 +88,21 @@ virtual class SymbolicFactorGraph { gtsam::SymbolicFactorGraph::EliminationTraitsType::DefaultEliminate), gtsam::SymbolicFactorGraph::OptionalVariableIndex variableIndex = std::nullopt) const; - std::shared_ptr marginalMultifrontalBayesNet( + gtsam::SymbolicBayesNet* marginalMultifrontalBayesNet( const gtsam::Ordering& variables, const gtsam::SymbolicFactorGraph::Eliminate& function = gtsam::SymbolicFactorGraph::Eliminate( gtsam::SymbolicFactorGraph::EliminationTraitsType::DefaultEliminate), gtsam::SymbolicFactorGraph::OptionalVariableIndex variableIndex = std::nullopt) const; - std::shared_ptr marginalMultifrontalBayesNet( + gtsam::SymbolicBayesNet* marginalMultifrontalBayesNet( const gtsam::KeyVector& variables, const gtsam::SymbolicFactorGraph::Eliminate& function = gtsam::SymbolicFactorGraph::Eliminate( gtsam::SymbolicFactorGraph::EliminationTraitsType::DefaultEliminate), gtsam::SymbolicFactorGraph::OptionalVariableIndex variableIndex = std::nullopt) const; - std::shared_ptr marginalMultifrontalBayesNet( + gtsam::SymbolicBayesNet* marginalMultifrontalBayesNet( const gtsam::Ordering& variables, const gtsam::Ordering& marginalizedVariableOrdering, const gtsam::SymbolicFactorGraph::Eliminate& function = @@ -110,7 +110,7 @@ virtual class SymbolicFactorGraph { gtsam::SymbolicFactorGraph::EliminationTraitsType::DefaultEliminate), gtsam::SymbolicFactorGraph::OptionalVariableIndex variableIndex = std::nullopt) const; - std::shared_ptr marginalMultifrontalBayesNet( + gtsam::SymbolicBayesNet* marginalMultifrontalBayesNet( const gtsam::KeyVector& variables, const gtsam::Ordering& marginalizedVariableOrdering, const gtsam::SymbolicFactorGraph::Eliminate& function = @@ -118,7 +118,7 @@ virtual class SymbolicFactorGraph { gtsam::SymbolicFactorGraph::EliminationTraitsType::DefaultEliminate), gtsam::SymbolicFactorGraph::OptionalVariableIndex variableIndex = std::nullopt) const; - std::shared_ptr marginal( + gtsam::SymbolicFactorGraph* marginal( const gtsam::KeyVector& variables, const gtsam::SymbolicFactorGraph::Eliminate& function = gtsam::SymbolicFactorGraph::Eliminate( @@ -193,7 +193,7 @@ class SymbolicBayesNet { // Standard interface size_t size() const; - const std::shared_ptr at(size_t idx) const; + const gtsam::SymbolicConditional* at(size_t idx) const; gtsam::SymbolicConditional* front() const; gtsam::SymbolicConditional* back() const; void push_back(gtsam::SymbolicConditional* conditional); @@ -253,7 +253,7 @@ class SymbolicBayesTreeClique { bool equals(const gtsam::SymbolicBayesTreeClique& other, double tol) const; void print(string s = "", const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter); - const gtsam::SymbolicConditional::shared_ptr& conditional() const; + const gtsam::SymbolicConditional* conditional() const; bool isRoot() const; gtsam::SymbolicBayesTreeClique* parent() const; size_t nrChildren() const; @@ -287,19 +287,19 @@ class SymbolicBayesTree { void deleteCachedShortcuts(); size_t numCachedSeparatorMarginals() const; - std::shared_ptr marginalFactor( + gtsam::SymbolicConditional* marginalFactor( gtsam::Key j, const gtsam::SymbolicFactorGraph::Eliminate& function = gtsam::SymbolicFactorGraph::Eliminate( gtsam::SymbolicFactorGraph::EliminationTraitsType::DefaultEliminate)) const; - std::shared_ptr joint( + gtsam::SymbolicFactorGraph* joint( gtsam::Key j1, gtsam::Key j2, const gtsam::SymbolicFactorGraph::Eliminate& function = gtsam::SymbolicFactorGraph::Eliminate( gtsam::SymbolicFactorGraph::EliminationTraitsType::DefaultEliminate)) const; - std::shared_ptr jointBayesNet( + gtsam::SymbolicBayesNet* jointBayesNet( gtsam::Key j1, gtsam::Key j2, const gtsam::SymbolicFactorGraph::Eliminate& function = gtsam::SymbolicFactorGraph::Eliminate( diff --git a/gtsam_unstable/gtsam_unstable.i b/gtsam_unstable/gtsam_unstable.i index 9ffa85a9dc..97e2efc6e9 100644 --- a/gtsam_unstable/gtsam_unstable.i +++ b/gtsam_unstable/gtsam_unstable.i @@ -576,7 +576,7 @@ virtual class ProjectionFactorPPP : gtsam::NoiseModelFactor { gtsam::Key poseKey, gtsam::Key transformKey, gtsam::Key pointKey, const CALIBRATION* k, bool throwCheirality, bool verboseCheirality); const gtsam::Point2& measured() const; - const std::shared_ptr calibration() const; + const CALIBRATION* calibration() const; bool verboseCheirality() const; bool throwCheirality() const; @@ -635,6 +635,26 @@ virtual class SmartStereoProjectionFactor : gtsam::NonlinearFactor { }; #include +// Stereo measurement container used by the MATLAB wrapper. +class StereoPoint2Vector { + StereoPoint2Vector(); + size_t size() const; + bool empty() const; + void clear(); + gtsam::StereoPoint2 at(size_t i) const; + void push_back(const gtsam::StereoPoint2& measurement); +}; + +// Shared Cal3_S2Stereo container used by the MATLAB wrapper. +class Cal3_S2StereoVector { + Cal3_S2StereoVector(); + size_t size() const; + bool empty() const; + void clear(); + gtsam::Cal3_S2Stereo* at(size_t i) const; + void push_back(const gtsam::Cal3_S2Stereo* calibration); +}; + virtual class SmartStereoProjectionPoseFactor : gtsam::SmartStereoProjectionFactor { SmartStereoProjectionPoseFactor(const gtsam::noiseModel::Base* sharedNoiseModel, const gtsam::SmartProjectionParams& params, @@ -644,15 +664,15 @@ virtual class SmartStereoProjectionPoseFactor : gtsam::SmartStereoProjectionFact SmartStereoProjectionPoseFactor(const gtsam::noiseModel::Base* sharedNoiseModel); void add(const gtsam::StereoPoint2& measured, const gtsam::Key& poseKey, - const std::shared_ptr& K); - void add(const std::vector& measurements, + const gtsam::Cal3_S2Stereo* K); + void add(const gtsam::StereoPoint2Vector& measurements, const gtsam::KeyVector& poseKeys, - const std::vector>& Ks); - void add(const std::vector& measurements, + const gtsam::Cal3_S2StereoVector& Ks); + void add(const gtsam::StereoPoint2Vector& measurements, const gtsam::KeyVector& poseKeys, - const std::shared_ptr& K); + const gtsam::Cal3_S2Stereo* K); - std::vector> calibration() const; + gtsam::Cal3_S2StereoVector calibration() const; }; #include @@ -673,7 +693,7 @@ virtual class ProjectionFactorRollingShutter : gtsam::NoiseModelFactor { const gtsam::Point2& measured() const; double alpha() const; - const std::shared_ptr calibration() const; + const gtsam::Cal3_S2* calibration() const; bool verboseCheirality() const; bool throwCheirality() const; @@ -733,7 +753,7 @@ class EqVIOFilter { void initializeFromIMU(const gtsam::eqvio::IMUInput& imu); void predict(const gtsam::eqvio::IMUInput& imu, double dt); void update(const std::map& measurement, - const std::shared_ptr& camera, + const gtsam::eqvio::CameraModel* camera, const gtsam::Matrix& R); bool isInitialized() const; diff --git a/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.cpp b/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.cpp index 52129daa87..64c2e6eb5c 100644 --- a/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.cpp +++ b/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.cpp @@ -39,8 +39,8 @@ void SmartStereoProjectionPoseFactor::add( } void SmartStereoProjectionPoseFactor::add( - const std::vector& measurements, const KeyVector& poseKeys, - const std::vector>& Ks) { + const StereoPoint2Vector& measurements, const KeyVector& poseKeys, + const Cal3_S2StereoVector& Ks) { assert(measurements.size() == poseKeys.size()); assert(poseKeys.size() == Ks.size()); Base::add(measurements, poseKeys); @@ -48,7 +48,7 @@ void SmartStereoProjectionPoseFactor::add( } void SmartStereoProjectionPoseFactor::add( - const std::vector& measurements, const KeyVector& poseKeys, + const StereoPoint2Vector& measurements, const KeyVector& poseKeys, const std::shared_ptr& K) { assert(poseKeys.size() == measurements.size()); for (size_t i = 0; i < measurements.size(); i++) { diff --git a/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h b/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h index 2aec583c93..8cf592c8ee 100644 --- a/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h +++ b/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h @@ -24,6 +24,10 @@ #include namespace gtsam { + +/// Collection of shared stereo camera calibrations. +using Cal3_S2StereoVector = std::vector>; + /** * * @ingroup slam @@ -47,7 +51,7 @@ class GTSAM_UNSTABLE_EXPORT SmartStereoProjectionPoseFactor : public SmartStereoProjectionFactor { protected: /// shared pointer to calibration object (one for each camera) - std::vector> K_all_; + Cal3_S2StereoVector K_all_; public: /// shorthand for base class type @@ -88,9 +92,8 @@ class GTSAM_UNSTABLE_EXPORT SmartStereoProjectionPoseFactor * the same landmark * @param Ks vector of calibration objects */ - void add(const std::vector& measurements, - const KeyVector& poseKeys, - const std::vector>& Ks); + void add(const StereoPoint2Vector& measurements, const KeyVector& poseKeys, + const Cal3_S2StereoVector& Ks); /** * Variant of the previous one in which we include a set of measurements with @@ -101,8 +104,7 @@ class GTSAM_UNSTABLE_EXPORT SmartStereoProjectionPoseFactor * same landmark * @param K the (known) camera calibration (same for all measurements) */ - void add(const std::vector& measurements, - const KeyVector& poseKeys, + void add(const StereoPoint2Vector& measurements, const KeyVector& poseKeys, const std::shared_ptr& K); /** @@ -122,9 +124,7 @@ class GTSAM_UNSTABLE_EXPORT SmartStereoProjectionPoseFactor double error(const Values& values) const override; /** return the calibration object */ - inline std::vector> calibration() const { - return K_all_; - } + inline Cal3_S2StereoVector calibration() const { return K_all_; } /** * Collect all cameras involved in this factor diff --git a/matlab/custom.i b/matlab/custom.i index 54b4ab0d81..4218b8c8fc 100644 --- a/matlab/custom.i +++ b/matlab/custom.i @@ -6,7 +6,7 @@ namespace gtsam { // // The constructor intentionally accepts gtsam::noiseModel::Base* here because // that matches the generated MATLAB wrapper surface. The corresponding C++ -// constructor takes noiseModel::Base::shared_ptr after unwrap. +// constructor receives the unwrapped shared-ownership handle. #include virtual class MatlabCustomFactor : gtsam::NoiseModelFactor { MatlabCustomFactor(gtsam::noiseModel::Base* noiseModel, diff --git a/matlab/gtsam_tests/testSharedPointers.m b/matlab/gtsam_tests/testSharedPointers.m new file mode 100644 index 0000000000..957bac42c4 --- /dev/null +++ b/matlab/gtsam_tests/testSharedPointers.m @@ -0,0 +1,46 @@ +% Regression coverage for shared-pointer declarations written with the +% wrapper DSL's T* spelling. + +noise = gtsam.noiseModel.Isotropic.Sigma(2, 1.0); + +triangulation = gtsam.TriangulationParameters(); +triangulation.noiseModel = noise; +clear noise; +recoveredNoise = triangulation.noiseModel; +gtsam.EXPECT('triangulation noise type', ... + isa(recoveredNoise, 'gtsam.noiseModel.Isotropic')); +gtsam.EXPECT('triangulation noise value', recoveredNoise.dim() == 2); +clear triangulation; +gtsam.EXPECT('triangulation noise lifetime', recoveredNoise.dim() == 2); + +preconditioner = gtsam.DummyPreconditionerParameters(); +pcg = gtsam.PCGSolverParameters(); +pcg.preconditioner = preconditioner; +clear preconditioner; +recoveredPreconditioner = pcg.preconditioner; +gtsam.EXPECT('preconditioner type', ... + isa(recoveredPreconditioner, 'gtsam.DummyPreconditionerParameters')); +clear pcg; +gtsam.EXPECT('preconditioner lifetime', ... + isa(recoveredPreconditioner, 'gtsam.DummyPreconditionerParameters')); + +preintegration = gtsam.PreintegrationParams.MakeSharedU(9.81); +legged = gtsam.LeggedEstimatorParams(); +legged.preintegrationParams = preintegration; +clear preintegration; +recoveredPreintegration = legged.preintegrationParams; +gtsam.EXPECT('preintegration type', ... + isa(recoveredPreintegration, 'gtsam.PreintegrationParams')); +clear legged; +gtsam.EXPECT('preintegration lifetime', ... + isa(recoveredPreintegration, 'gtsam.PreintegrationParams')); + +kernel = gtsam.noiseModel.mEstimator.Huber.Create(1.345); +robust = gtsam.noiseModel.Robust.Create(kernel, recoveredNoise); +clear kernel recoveredNoise; +gtsam.EXPECT('robust kernel type', ... + isa(robust.robust(), 'gtsam.noiseModel.mEstimator.Huber')); +gtsam.EXPECT('robust noise type', ... + isa(robust.noise(), 'gtsam.noiseModel.Isotropic')); + +clear robust recoveredPreconditioner recoveredPreintegration; diff --git a/matlab/gtsam_tests/testUnstableSharedPointers.m b/matlab/gtsam_tests/testUnstableSharedPointers.m new file mode 100644 index 0000000000..c3d7c34b60 --- /dev/null +++ b/matlab/gtsam_tests/testUnstableSharedPointers.m @@ -0,0 +1,43 @@ +% Regression coverage for the named vector of shared stereo calibrations. + +firstCalibration = gtsam.Cal3_S2Stereo(500, 500, 0, 320, 240, 0.2); +secondCalibration = gtsam.Cal3_S2Stereo(510, 505, 0, 320, 240, 0.3); + +measurements = gtsam.StereoPoint2Vector(); +measurements.push_back(gtsam.StereoPoint2(320, 300, 240)); +measurements.push_back(gtsam.StereoPoint2(330, 310, 245)); +gtsam.EXPECT('stereo measurement vector size', measurements.size() == 2); +gtsam.EXPECT('stereo measurement vector element', ... + abs(measurements.at(1).uL() - 330) < 1e-12); + +calibrations = gtsam.Cal3_S2StereoVector(); +calibrations.push_back(firstCalibration); +calibrations.push_back(secondCalibration); +clear firstCalibration secondCalibration; + +gtsam.EXPECT('calibration vector size', calibrations.size() == 2); +storedCalibration = calibrations.at(0); +gtsam.EXPECT('calibration vector element type', ... + isa(storedCalibration, 'gtsam.Cal3_S2Stereo')); +gtsam.EXPECT('calibration vector element lifetime', ... + abs(storedCalibration.baseline() - 0.2) < 1e-12); + +noise = gtsam.noiseModel.Isotropic.Sigma(3, 1.0); +factor = gtsam.SmartStereoProjectionPoseFactor(noise); +poseKeys = gtsam.KeyVector(); +poseKeys.push_back(uint64(1)); +poseKeys.push_back(uint64(2)); +factor.add(measurements, poseKeys, calibrations); +clear noise measurements poseKeys calibrations storedCalibration; + +recovered = factor.calibration(); +gtsam.EXPECT('recovered calibration vector type', ... + isa(recovered, 'gtsam.Cal3_S2StereoVector')); +gtsam.EXPECT('recovered calibration vector size', recovered.size() == 2); +clear factor; +gtsam.EXPECT('recovered first calibration lifetime', ... + abs(recovered.at(0).baseline() - 0.2) < 1e-12); +gtsam.EXPECT('recovered second calibration lifetime', ... + abs(recovered.at(1).baseline() - 0.3) < 1e-12); + +clear recovered; diff --git a/matlab/gtsam_tests/test_gtsam.m b/matlab/gtsam_tests/test_gtsam.m index 0d2b042d2e..62ca2f4f21 100644 --- a/matlab/gtsam_tests/test_gtsam.m +++ b/matlab/gtsam_tests/test_gtsam.m @@ -24,6 +24,14 @@ display 'Starting: testCustomFactor' testCustomFactor +display 'Starting: testSharedPointers' +testSharedPointers + +if ~isempty(which('gtsam.SmartStereoProjectionPoseFactor')) + display 'Starting: testUnstableSharedPointers' + testUnstableSharedPointers +end + %% SLAM display 'Starting: testPriorFactor' testPriorFactor diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index 9efd60fdb0..c2d89ffaf4 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -254,6 +254,8 @@ if(GTSAM_UNSTABLE_BUILD_PYTHON) gtsam::Point2Vector gtsam::Pose3Vector gtsam::KeyVector + gtsam::StereoPoint2Vector + gtsam::Cal3_S2StereoVector gtsam::BinaryMeasurementsPoint3 gtsam::BinaryMeasurementsUnit3 gtsam::BinaryMeasurementsRot3 diff --git a/python/gtsam/tests/test_SharedPointerProperties.py b/python/gtsam/tests/test_SharedPointerProperties.py new file mode 100644 index 0000000000..aa7d40013f --- /dev/null +++ b/python/gtsam/tests/test_SharedPointerProperties.py @@ -0,0 +1,39 @@ +"""Compatibility tests for shared-pointer-backed Python properties.""" + +import unittest + +import gtsam + + +class TestSharedPointerProperties(unittest.TestCase): + def test_triangulation_noise_model_property(self): + params = gtsam.TriangulationParameters() + model = gtsam.noiseModel.Isotropic.Sigma(2, 0.5) + + params.noiseModel = model + + self.assertIsInstance(params.noiseModel, gtsam.noiseModel.Base) + + def test_pcg_preconditioner_property(self): + params = gtsam.PCGSolverParameters() + preconditioner = gtsam.DummyPreconditionerParameters() + + params.preconditioner = preconditioner + + self.assertIsInstance( + params.preconditioner, gtsam.DummyPreconditionerParameters + ) + + def test_legged_preintegration_params_property(self): + params = gtsam.LeggedEstimatorParams() + preintegration = gtsam.PreintegrationParams.MakeSharedU(9.81) + + params.preintegrationParams = preintegration + + self.assertIsInstance( + params.preintegrationParams, gtsam.PreintegrationParams + ) + + +if __name__ == "__main__": + unittest.main() diff --git a/python/gtsam_unstable/gtsam_unstable.tpl b/python/gtsam_unstable/gtsam_unstable.tpl index 957e04d031..21bcb33e72 100644 --- a/python/gtsam_unstable/gtsam_unstable.tpl +++ b/python/gtsam_unstable/gtsam_unstable.tpl @@ -64,8 +64,8 @@ PYBIND11_MODULE({module_name}, m_) {{ // Note here we need to import the dependent library py::module::import("gtsam"); -{wrapped_namespace} - #include "python/gtsam_unstable/specializations/gtsam_unstable.h" +{wrapped_namespace} + }} diff --git a/python/gtsam_unstable/preamble.h b/python/gtsam_unstable/preamble.h index e69de29bb2..dc79b644f6 100644 --- a/python/gtsam_unstable/preamble.h +++ b/python/gtsam_unstable/preamble.h @@ -0,0 +1,4 @@ +/* Keep these vectors as bound C++ containers instead of allowing pybind11's + * automatic STL conversion to copy them to and from Python lists. */ +PYBIND11_MAKE_OPAQUE(gtsam::StereoPoint2Vector); +PYBIND11_MAKE_OPAQUE(gtsam::Cal3_S2StereoVector); diff --git a/python/gtsam_unstable/specializations/gtsam_unstable.h b/python/gtsam_unstable/specializations/gtsam_unstable.h index e69de29bb2..7d83c2a859 100644 --- a/python/gtsam_unstable/specializations/gtsam_unstable.h +++ b/python/gtsam_unstable/specializations/gtsam_unstable.h @@ -0,0 +1,6 @@ +py::bind_vector(m_, "StereoPoint2Vector"); +py::bind_vector(m_, "Cal3_S2StereoVector"); + +// Preserve the iterable inputs accepted by pybind11's automatic STL caster. +py::implicitly_convertible(); +py::implicitly_convertible(); diff --git a/python/gtsam_unstable/tests/test_SmartStereoProjectionPoseFactor.py b/python/gtsam_unstable/tests/test_SmartStereoProjectionPoseFactor.py new file mode 100644 index 0000000000..f0753449f6 --- /dev/null +++ b/python/gtsam_unstable/tests/test_SmartStereoProjectionPoseFactor.py @@ -0,0 +1,61 @@ +""" +GTSAM Copyright 2010-2026, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information. + +SmartStereoProjectionPoseFactor Python binding tests. +""" + +# pylint: disable=no-member + +import gc +import unittest + +import gtsam +import gtsam_unstable +from gtsam.utils.test_case import GtsamTestCase + + +class TestSmartStereoProjectionPoseFactor(GtsamTestCase): + """Tests the unstable smart stereo factor's container bindings.""" + + def test_calibration_vector_round_trip(self): + """The named vector preserves shared calibration ownership.""" + noise = gtsam.noiseModel.Isotropic.Sigma(3, 1.0) + factor = gtsam_unstable.SmartStereoProjectionPoseFactor(noise) + + first = gtsam.Cal3_S2Stereo(500.0, 500.0, 0.0, 320.0, 240.0, 0.2) + second = gtsam.Cal3_S2Stereo(510.0, 505.0, 0.0, 320.0, 240.0, 0.3) + measurement_values = [ + gtsam.StereoPoint2(320.0, 300.0, 240.0), + gtsam.StereoPoint2(330.0, 310.0, 245.0), + ] + measurements = gtsam_unstable.StereoPoint2Vector(measurement_values) + calibrations = gtsam_unstable.Cal3_S2StereoVector([first, second]) + factor.add(measurements, [1, 2], calibrations) + + # Existing Python list inputs remain implicitly convertible. + factor_from_lists = gtsam_unstable.SmartStereoProjectionPoseFactor( + noise) + factor_from_lists.add(measurement_values, [1, 2], [first, second]) + self.assertEqual(len(factor_from_lists.calibration()), 2) + del factor_from_lists + + del calibrations, measurements, measurement_values, first, second + gc.collect() + recovered = factor.calibration() + self.assertIsInstance(recovered, + gtsam_unstable.Cal3_S2StereoVector) + self.assertEqual(len(recovered), 2) + self.assertAlmostEqual(recovered[0].baseline(), 0.2) + self.assertAlmostEqual(recovered[1].baseline(), 0.3) + + del factor + gc.collect() + self.assertAlmostEqual(recovered[0].baseline(), 0.2) + + +if __name__ == "__main__": + unittest.main()