Skip to content

Commit

Permalink
Merge pull request #941 from borglab/wrap/isam2
Browse files Browse the repository at this point in the history 
Wrap additional ISAM2 methods
  • Loading branch information
@varunagrawal
varunagrawal authored Nov 30, 2021
2 parents 29ccbe2 + 0b89d2d commit 7bd4ebf
Show file tree
Hide file tree
Showing 5 changed files with 73 additions and 26 deletions.
42 changes: 20 additions & 22 deletions gtsam/linear/NoiseModel.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -177,17 +177,16 @@ namespace gtsam {
return *sqrt_information_;
}

protected:

/** protected constructor takes square root information matrix */
Gaussian(size_t dim = 1, const boost::optional<Matrix>& sqrt_information = boost::none) :
Base(dim), sqrt_information_(sqrt_information) {
}

public:

typedef boost::shared_ptr<Gaussian> shared_ptr;

/** constructor takes square root information matrix */
Gaussian(size_t dim = 1,
const boost::optional<Matrix>& sqrt_information = boost::none)
: Base(dim), sqrt_information_(sqrt_information) {}

~Gaussian() override {}

/**
Expand Down Expand Up @@ -290,13 +289,13 @@ namespace gtsam {
Vector sigmas_, invsigmas_, precisions_;

protected:
/** protected constructor - no initializations */
Diagonal();

/** constructor to allow for disabling initialization of invsigmas */
Diagonal(const Vector& sigmas);

public:
/** constructor - no initializations, for serialization */
Diagonal();

typedef boost::shared_ptr<Diagonal> shared_ptr;

Expand Down Expand Up @@ -387,14 +386,6 @@ namespace gtsam {
// Sigmas are contained in the base class
Vector mu_; ///< Penalty function weight - needs to be large enough to dominate soft constraints

/**
* protected constructor takes sigmas.
* prevents any inf values
* from appearing in invsigmas or precisions.
* mu set to large default value (1000.0)
*/
Constrained(const Vector& sigmas = Z_1x1);

/**
* Constructor that prevents any inf values
* from appearing in invsigmas or precisions.
Expand All @@ -406,6 +397,14 @@ namespace gtsam {

typedef boost::shared_ptr<Constrained> shared_ptr;

/**
* protected constructor takes sigmas.
* prevents any inf values
* from appearing in invsigmas or precisions.
* mu set to large default value (1000.0)
*/
Constrained(const Vector& sigmas = Z_1x1);

~Constrained() override {}

/// true if a constrained noise mode, saves slow/clumsy dynamic casting
Expand Down Expand Up @@ -531,11 +530,11 @@ namespace gtsam {
Isotropic(size_t dim, double sigma) :
Diagonal(Vector::Constant(dim, sigma)),sigma_(sigma),invsigma_(1.0/sigma) {}

public:

/* dummy constructor to allow for serialization */
Isotropic() : Diagonal(Vector1::Constant(1.0)),sigma_(1.0),invsigma_(1.0) {}

public:

~Isotropic() override {}

typedef boost::shared_ptr<Isotropic> shared_ptr;
Expand Down Expand Up @@ -592,14 +591,13 @@ namespace gtsam {
* Unit: i.i.d. unit-variance noise on all m dimensions.
*/
class GTSAM_EXPORT Unit : public Isotropic {
protected:

Unit(size_t dim=1): Isotropic(dim,1.0) {}

public:

typedef boost::shared_ptr<Unit> shared_ptr;

/** constructor for serialization */
Unit(size_t dim=1): Isotropic(dim,1.0) {}

~Unit() override {}

/**
Expand Down
15 changes: 15 additions & 0 deletions gtsam/linear/linear.i
View file
Original file line number Diff line number Diff line change
Expand Up @@ -34,6 +34,9 @@ virtual class Gaussian : gtsam::noiseModel::Base {

// enabling serialization functionality
void serializable() const;

// enable pickling in python
void pickle() const;
};

virtual class Diagonal : gtsam::noiseModel::Gaussian {
Expand All @@ -49,6 +52,9 @@ virtual class Diagonal : gtsam::noiseModel::Gaussian {

// enabling serialization functionality
void serializable() const;

// enable pickling in python
void pickle() const;
};

virtual class Constrained : gtsam::noiseModel::Diagonal {
Expand All @@ -66,6 +72,9 @@ virtual class Constrained : gtsam::noiseModel::Diagonal {

// enabling serialization functionality
void serializable() const;

// enable pickling in python
void pickle() const;
};

virtual class Isotropic : gtsam::noiseModel::Diagonal {
Expand All @@ -78,13 +87,19 @@ virtual class Isotropic : gtsam::noiseModel::Diagonal {

// enabling serialization functionality
void serializable() const;

// enable pickling in python
void pickle() const;
};

virtual class Unit : gtsam::noiseModel::Isotropic {
static gtsam::noiseModel::Unit* Create(size_t dim);

// enabling serialization functionality
void serializable() const;

// enable pickling in python
void pickle() const;
};

namespace mEstimator {
Expand Down
24 changes: 24 additions & 0 deletions gtsam/navigation/navigation.i
View file
Original file line number Diff line number Diff line change
Expand Up @@ -41,6 +41,12 @@ class ConstantBias {
Vector gyroscope() const;
Vector correctAccelerometer(Vector measurement) const;
Vector correctGyroscope(Vector measurement) const;

// enabling serialization functionality
void serialize() const;

// enable pickling in python
void pickle() const;
};

}///\namespace imuBias
Expand All @@ -64,6 +70,12 @@ class NavState {

gtsam::NavState retract(const Vector& x) const;
Vector localCoordinates(const gtsam::NavState& g) const;

// enabling serialization functionality
void serialize() const;

// enable pickling in python
void pickle() const;
};

#include <gtsam/navigation/PreintegratedRotation.h>
Expand Down Expand Up @@ -106,6 +118,12 @@ virtual class PreintegrationParams : gtsam::PreintegratedRotationParams {
Matrix getAccelerometerCovariance() const;
Matrix getIntegrationCovariance() const;
bool getUse2ndOrderCoriolis() const;

// enabling serialization functionality
void serialize() const;

// enable pickling in python
void pickle() const;
};

#include <gtsam/navigation/ImuFactor.h>
Expand Down Expand Up @@ -135,6 +153,12 @@ class PreintegratedImuMeasurements {
Vector biasHatVector() const;
gtsam::NavState predict(const gtsam::NavState& state_i,
const gtsam::imuBias::ConstantBias& bias) const;

// enabling serialization functionality
void serialize() const;

// enable pickling in python
void pickle() const;
};

virtual class ImuFactor: gtsam::NonlinearFactor {
Expand Down
6 changes: 3 additions & 3 deletions gtsam/nonlinear/LinearContainerFactor.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -29,9 +29,6 @@ class LinearContainerFactor : public NonlinearFactor {
GaussianFactor::shared_ptr factor_;
boost::optional<Values> linearizationPoint_;

/** Default constructor - necessary for serialization */
LinearContainerFactor() {}

/** direct copy constructor */
GTSAM_EXPORT LinearContainerFactor(const GaussianFactor::shared_ptr& factor, const boost::optional<Values>& linearizationPoint);

Expand All @@ -43,6 +40,9 @@ class LinearContainerFactor : public NonlinearFactor {

typedef boost::shared_ptr<This> shared_ptr;

/** Default constructor - necessary for serialization */
LinearContainerFactor() {}

/** Primary constructor: store a linear factor with optional linearization point */
GTSAM_EXPORT LinearContainerFactor(const JacobianFactor& factor, const Values& linearizationPoint = Values());

Expand Down
12 changes: 11 additions & 1 deletion gtsam/nonlinear/nonlinear.i
View file
Original file line number Diff line number Diff line change
Expand Up @@ -738,7 +738,12 @@ class ISAM2 {
const gtsam::KeyList& extraReelimKeys,
bool force_relinearize);

gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors,
const gtsam::Values& newTheta,
const gtsam::ISAM2UpdateParams& updateParams);

gtsam::Values getLinearizationPoint() const;
bool valueExists(gtsam::Key key) const;
gtsam::Values calculateEstimate() const;
template <VALUE = {gtsam::Point2, gtsam::Rot2, gtsam::Pose2, gtsam::Point3,
gtsam::Rot3, gtsam::Pose3, gtsam::Cal3_S2, gtsam::Cal3DS2,
Expand All @@ -748,12 +753,17 @@ class ISAM2 {
gtsam::PinholeCamera<gtsam::Cal3Fisheye>,
gtsam::PinholeCamera<gtsam::Cal3Unified>, Vector, Matrix}>
VALUE calculateEstimate(size_t key) const;
gtsam::Values calculateBestEstimate() const;
Matrix marginalCovariance(size_t key) const;
gtsam::Values calculateBestEstimate() const;
gtsam::VectorValues getDelta() const;
double error(const gtsam::VectorValues& x) const;
gtsam::NonlinearFactorGraph getFactorsUnsafe() const;
gtsam::VariableIndex getVariableIndex() const;
const gtsam::KeySet& getFixedVariables() const;
gtsam::ISAM2Params params() const;

void printStats() const;
gtsam::VectorValues gradientAtZero() const;
};

#include <gtsam/nonlinear/NonlinearISAM.h>
Expand Down

0 comments on commit 7bd4ebf

Please sign in to comment.