12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Merge pull request #941 from borglab/wrap/isam2 

Wrap additional ISAM2 methods
release/4.3a0
Varun Agrawal 2021-11-30 06:57:42 -05:00 committed by GitHub
parent 29ccbe2130 0b89d2d7ab
commit 7bd4ebfb9c
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
5 changed files with 73 additions and 26 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

42
gtsam/linear/NoiseModel.h
View File

@ -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 {}
/**
@ -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;
@ -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.
@ -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
@ -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;
@ -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 {}
/**

15
gtsam/linear/linear.i
View File

@ -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 {
@ -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 {
@ -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 {
@ -78,6 +87,9 @@ 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 {
@ -85,6 +97,9 @@ virtual class Unit : gtsam::noiseModel::Isotropic {
// enabling serialization functionality
void serializable() const;
// enable pickling in python
void pickle() const;
};
namespace mEstimator {

24
gtsam/navigation/navigation.i
View File

@ -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
@ -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>
@ -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>
@ -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 {

6
gtsam/nonlinear/LinearContainerFactor.h
View File

@ -29,9 +29,6 @@ protected:
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);
@ -43,6 +40,9 @@ public:
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());

12
gtsam/nonlinear/nonlinear.i
View File

@ -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,
@ -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>