Skip to content
Draft
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
5 changes: 1 addition & 4 deletions gtsam/discrete/DiscreteBayesNet.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down
2 changes: 2 additions & 0 deletions gtsam/discrete/DiscreteConditional.h
Original file line number Diff line number Diff line change
Expand Up @@ -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.
Expand Down
20 changes: 10 additions & 10 deletions gtsam/gtsam.i
Original file line number Diff line number Diff line change
Expand Up @@ -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;
};
Expand All @@ -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;
};
Expand All @@ -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;
};
Expand All @@ -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<FactorIndex>
Expand Down
76 changes: 38 additions & 38 deletions gtsam/inference/inference.i
Original file line number Diff line number Diff line change
Expand Up @@ -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;

Expand All @@ -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 <gtsam/inference/LabeledSymbol.h>
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;
Expand All @@ -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 <gtsam/inference/Ordering.h>
class Ordering {
Expand Down Expand Up @@ -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;
Expand Down
81 changes: 46 additions & 35 deletions gtsam/linear/linear.i
Original file line number Diff line number Diff line change
Expand Up @@ -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);

Expand Down Expand Up @@ -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);
Expand Down Expand Up @@ -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);

Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -451,19 +451,24 @@ class GaussianFactorGraph {
#include <gtsam/hybrid/HybridValues.h>
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<std::pair<gtsam::Key, gtsam::Matrix>> 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,
Expand Down Expand Up @@ -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 <gtsam/linear/GaussianISAM.h>
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);
Expand Down
26 changes: 13 additions & 13 deletions gtsam/navigation/navigation.i
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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);

Expand Down Expand Up @@ -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 =
Expand All @@ -288,7 +288,7 @@ virtual class Pose3AttitudeFactor : gtsam::NonlinearFactor {

#include <gtsam/navigation/GPSFactor.h>
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
Expand All @@ -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
Expand Down
Loading
Loading