Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Changes to expose code to python for nerf-slam #1

Open
wants to merge 2 commits into
base: develop
Choose a base branch
from
Open

Changes to expose code to python for nerf-slam #1

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
139c156
Changes to expose code to python for nerf-slam
ToniRV Dec 12, 2022
a0240d6
Update gtsam_unstable.i
ToniRV Jan 1, 2023
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
27 changes: 19 additions & 8 deletions gtsam/linear/linear.i
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -236,6 +236,7 @@ class VectorValues {
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;
void update(const gtsam::VectorValues& values);

Expand Down Expand Up @@ -341,6 +342,15 @@ virtual class HessianFactor : gtsam::GaussianFactor {
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,
double f);
// TODO(Toni): somehow the parser converts this to
// const std::vector<const gtsam::Matrix&>&, and I need to remove manually in
// /home/tonirv/Code/gtsam/build/python/linear.cpp
// the inner const X& ..., and also add <pybind11/stl.h> because:
// """ Did you forget to `#include <pybind11/stl.h>`? """
HessianFactor(const gtsam::KeyVector& js,
const std::vector<gtsam::Matrix>& Gs,
const std::vector<gtsam::Vector>& gs,
double f);
HessianFactor(const gtsam::GaussianFactorGraph& factors);

//Testable
Expand Down Expand Up @@ -401,6 +411,7 @@ class GaussianFactorGraph {

// Optimizing and linear algebra
gtsam::VectorValues optimize() const;
gtsam::VectorValues optimizeDensely() const;
gtsam::VectorValues optimize(const gtsam::Ordering& ordering) const;
gtsam::VectorValues optimizeGradientSearch() const;
gtsam::VectorValues gradient(const gtsam::VectorValues& x0) const;
Expand Down Expand Up @@ -470,25 +481,25 @@ virtual class GaussianConditional : gtsam::JacobianFactor {
size_t name2, Matrix T);

// Named constructors
static gtsam::GaussianConditional FromMeanAndStddev(gtsam::Key key,
static gtsam::GaussianConditional FromMeanAndStddev(gtsam::Key key,
const Matrix& A,
gtsam::Key parent,
const Vector& b,
double sigma);

static gtsam::GaussianConditional FromMeanAndStddev(gtsam::Key key,
const Matrix& A1,
gtsam::Key parent1,
gtsam::Key parent1,
const Matrix& A2,
gtsam::Key parent2,
gtsam::Key parent2,
const Vector& b,
double sigma);
// Testable
void print(string s = "GaussianConditional",
const gtsam::KeyFormatter& keyFormatter =
gtsam::DefaultKeyFormatter) const;
bool equals(const gtsam::GaussianConditional& cg, double tol) const;

// Standard Interface
gtsam::Key firstFrontalKey() const;
gtsam::VectorValues solve(const gtsam::VectorValues& parents) const;
Expand All @@ -497,7 +508,7 @@ virtual class GaussianConditional : gtsam::JacobianFactor {
gtsam::JacobianFactor* likelihood(Vector frontal) const;
gtsam::VectorValues sample(const gtsam::VectorValues& parents) const;
gtsam::VectorValues sample() const;

// Advanced Interface
gtsam::VectorValues solveOtherRHS(const gtsam::VectorValues& parents,
const gtsam::VectorValues& rhs) const;
Expand Down Expand Up @@ -552,7 +563,7 @@ virtual class GaussianBayesNet {
gtsam::VectorValues optimize() const;
gtsam::VectorValues optimize(gtsam::VectorValues given) const;
gtsam::VectorValues optimizeGradientSearch() const;

gtsam::VectorValues sample(gtsam::VectorValues given) const;
gtsam::VectorValues sample() const;
gtsam::VectorValues backSubstitute(const gtsam::VectorValues& gx) const;
Expand All @@ -566,7 +577,7 @@ virtual class GaussianBayesNet {

void saveGraph(const string& s) const;

std::pair<Matrix, Vector> matrix() const;
std::pair<Matrix, Vector> matrix() const;
gtsam::VectorValues gradient(const gtsam::VectorValues& x0) const;
gtsam::VectorValues gradientAtZero() const;
double error(const gtsam::VectorValues& x) const;
Expand Down Expand Up @@ -712,4 +723,4 @@ class KalmanFilter {
Vector z, Matrix Q);
};

}
}
2 changes: 2 additions & 0 deletions gtsam/navigation/navigation.i
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -120,6 +120,8 @@ class PreintegratedImuMeasurements {
// Standard Interface
void integrateMeasurement(Vector measuredAcc, Vector measuredOmega,
double deltaT);
void integrateMeasurements(Matrix measuredAccs, Matrix measuredOmegas,
Vector dts);
void resetIntegration();
void resetIntegrationAndSetBias(const gtsam::imuBias::ConstantBias& biasHat);

Expand Down
10 changes: 10 additions & 0 deletions gtsam/nonlinear/nonlinear.i
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -94,6 +94,11 @@ class NonlinearFactorGraph {
// Ordering* orderingCOLAMDConstrained(const gtsam::Values& c, const
// std::map<gtsam::Key,int>& constraints) const;
gtsam::GaussianFactorGraph* linearize(const gtsam::Values& values) const;
// typedef std::function< void(const boost::shared_ptr<gtsam::HessianFactor>& hessianFactor)> Dampen;
// gtsam::HessianFactor* linearizeToHessianFactor(
// const gtsam::Values& values,
// boost::optional<gtsam::Ordering&> ordering = boost::none,
// const Dampen& dampen = nullptr) const;
gtsam::NonlinearFactorGraph clone() const;

string dot(
Expand Down Expand Up @@ -561,6 +566,9 @@ virtual class NonlinearOptimizer {
gtsam::Values values() const;
gtsam::NonlinearFactorGraph graph() const;
gtsam::GaussianFactorGraph* iterate() const;
gtsam::VectorValues solve(
gtsam::GaussianFactorGraph &gfg,
const gtsam::NonlinearOptimizerParams& params) const;
};

#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
Expand Down Expand Up @@ -705,6 +713,8 @@ class ISAM2Result {
size_t getCliques() const;
double getErrorBefore() const;
double getErrorAfter() const;

gtsam::FactorIndices newFactorsIndices;
};

class ISAM2 {
Expand Down
22 changes: 21 additions & 1 deletion gtsam_unstable/gtsam_unstable.i
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -589,20 +589,28 @@ virtual class BatchFixedLagSmoother : gtsam::FixedLagSmoother {
gtsam::Rot3, gtsam::Pose3, gtsam::Cal3_S2, gtsam::Cal3DS2,
Vector, Matrix}>
VALUE calculateEstimate(size_t key) const;
gtsam::VectorValues getDelta() const;
Matrix marginalCovariance (gtsam::Key key) const;
};

#include <gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h>
virtual class IncrementalFixedLagSmoother : gtsam::FixedLagSmoother {
IncrementalFixedLagSmoother();
IncrementalFixedLagSmoother(double smootherLag);
IncrementalFixedLagSmoother(double smootherLag, const gtsam::ISAM2Params& params);

void print(string s = "IncrementalFixedLagSmoother:\n") const;

gtsam::ISAM2Params params() const;

// gtsam::FixedLagSmootherResult update(const gtsam::NonlinearFactorGraph& newFactors,
// const gtsam::Values& newTheta,
// const gtsam::FixedLagSmootherKeyTimestampMap& timestamps,
// const gtsam::FactorIndices& factorsToRemove);

gtsam::VectorValues getDelta() const;
gtsam::NonlinearFactorGraph getFactors() const;
gtsam::ISAM2 getISAM2() const;
gtsam::ISAM2Result getISAM2Result() const;
};

#include <gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.h>
Expand Down Expand Up @@ -797,6 +805,18 @@ virtual class ProjectionFactorPPPC : gtsam::NoiseModelFactor {
typedef gtsam::ProjectionFactorPPPC<gtsam::Pose3, gtsam::Point3, gtsam::Cal3_S2> ProjectionFactorPPPCCal3_S2;
typedef gtsam::ProjectionFactorPPPC<gtsam::Pose3, gtsam::Point3, gtsam::Cal3DS2> ProjectionFactorPPPCCal3DS2;

#include <gtsam_unstable/nonlinear/LinearizedFactor.h>

virtual class LinearizedGaussianFactor : gtsam::NonlinearFactor {
// LinearizedGaussianFactor();
// LinearizedGaussianFactor(const boost::shared_ptr<gtsam::GaussianFactor>& gaussian, const gtsam::Values& lin_points);
};

virtual class LinearizedHessianFactor : gtsam::LinearizedGaussianFactor {
LinearizedHessianFactor();
LinearizedHessianFactor(const boost::shared_ptr<gtsam::HessianFactor>& hessian, const gtsam::Values& lin_points);
Copy link

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

class not closed here leading to build failure

Copy link
Owner Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

ouch, thanks alex! Fixed.

};

#include <gtsam_unstable/slam/ProjectionFactorRollingShutter.h>
virtual class ProjectionFactorRollingShutter : gtsam::NoiseModelFactor {
ProjectionFactorRollingShutter(const gtsam::Point2& measured, double alpha, const gtsam::noiseModel::Base* noiseModel,
Expand Down