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

Merge pull request #1774 from borglab/fix-wrapper 

Update namespace for Vector and Matrix
release/4.3a0
Varun Agrawal 2024-06-30 09:22:58 -04:00 committed by GitHub
parent df57d9607f 6b14f22caf
commit 701ae40d8e
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
13 changed files with 503 additions and 503 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
.github/workflows/build-macos.yml vendored
View File

@ -25,16 +25,16 @@ jobs:
# Github Actions requires a single row to be added to the build matrix. # Github Actions requires a single row to be added to the build matrix.
# See https://help.github.com/en/articles/workflow-syntax-for-github-actions. # See https://help.github.com/en/articles/workflow-syntax-for-github-actions.
name: [ name: [
macos-11-xcode-13.4.1, macos-12-xcode-14.2,
] ]
build_type: [Debug, Release] build_type: [Debug, Release]
build_unstable: [ON] build_unstable: [ON]
include: include:
- name: macos-11-xcode-13.4.1 - name: macos-12-xcode-14.2
os: macos-11 os: macos-12
compiler: xcode compiler: xcode
version: "13.4.1" version: "14.2"
steps: steps:
- name: Checkout - name: Checkout

8
.github/workflows/build-python.yml vendored
View File

@ -31,7 +31,7 @@ jobs:
ubuntu-20.04-gcc-9, ubuntu-20.04-gcc-9,
ubuntu-20.04-gcc-9-tbb, ubuntu-20.04-gcc-9-tbb,
ubuntu-20.04-clang-9, ubuntu-20.04-clang-9,
macOS-11-xcode-13.4.1, macos-12-xcode-14.2,
windows-2022-msbuild, windows-2022-msbuild,
] ]
@ -54,10 +54,10 @@ jobs:
compiler: clang compiler: clang
version: "9" version: "9"
- name: macOS-11-xcode-13.4.1 - name: macos-12-xcode-14.2
os: macOS-11 os: macos-12
compiler: xcode compiler: xcode
version: "13.4.1" version: "14.2"
- name: windows-2022-msbuild - name: windows-2022-msbuild
os: windows-2022 os: windows-2022

4
gtsam/base/base.i
View File

@ -86,7 +86,7 @@ class IndexPairSetMap {
#include <gtsam/base/Matrix.h> #include <gtsam/base/Matrix.h>
#include <gtsam/base/MatrixSerialization.h> #include <gtsam/base/MatrixSerialization.h>
bool linear_independent(Matrix A, Matrix B, double tol); bool linear_independent(gtsam::Matrix A, gtsam::Matrix B, double tol);
#include <gtsam/base/Value.h> #include <gtsam/base/Value.h>
virtual class Value { virtual class Value {
@ -100,7 +100,7 @@ virtual class Value {
}; };
#include <gtsam/base/GenericValue.h> #include <gtsam/base/GenericValue.h>
template <T = {Vector, Matrix, gtsam::Point2, gtsam::Point3, gtsam::Rot2, template <T = {gtsam::Vector, gtsam::Matrix, gtsam::Point2, gtsam::Point3, gtsam::Rot2,
gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::StereoPoint2, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::StereoPoint2,
gtsam::Cal3_S2, gtsam::Cal3DS2, gtsam::Cal3Bundler, gtsam::Cal3_S2, gtsam::Cal3DS2, gtsam::Cal3Bundler,
gtsam::Cal3Fisheye, gtsam::Cal3Unified, gtsam::EssentialMatrix, gtsam::Cal3Fisheye, gtsam::Cal3Unified, gtsam::EssentialMatrix,

40
gtsam/basis/basis.i
View File

@ -10,23 +10,23 @@ namespace gtsam {
#include <gtsam/basis/Fourier.h> #include <gtsam/basis/Fourier.h>
class FourierBasis { class FourierBasis {
static Vector CalculateWeights(size_t N, double x); static gtsam::Vector CalculateWeights(size_t N, double x);
static Matrix WeightMatrix(size_t N, Vector x); static gtsam::Matrix WeightMatrix(size_t N, gtsam::Vector x);
static Matrix DifferentiationMatrix(size_t N); static gtsam::Matrix DifferentiationMatrix(size_t N);
static Vector DerivativeWeights(size_t N, double x); static gtsam::Vector DerivativeWeights(size_t N, double x);
}; };
#include <gtsam/basis/Chebyshev.h> #include <gtsam/basis/Chebyshev.h>
class Chebyshev1Basis { class Chebyshev1Basis {
static Matrix CalculateWeights(size_t N, double x); static gtsam::Matrix CalculateWeights(size_t N, double x);
static Matrix WeightMatrix(size_t N, Vector X); static gtsam::Matrix WeightMatrix(size_t N, gtsam::Vector X);
}; };
class Chebyshev2Basis { class Chebyshev2Basis {
static Matrix CalculateWeights(size_t N, double x); static gtsam::Matrix CalculateWeights(size_t N, double x);
static Matrix WeightMatrix(size_t N, Vector x); static gtsam::Matrix WeightMatrix(size_t N, gtsam::Vector x);
}; };
#include <gtsam/basis/Chebyshev2.h> #include <gtsam/basis/Chebyshev2.h>
@ -34,16 +34,16 @@ class Chebyshev2 {
static double Point(size_t N, int j); static double Point(size_t N, int j);
static double Point(size_t N, int j, double a, double b); static double Point(size_t N, int j, double a, double b);
static Vector Points(size_t N); static gtsam::Vector Points(size_t N);
static Vector Points(size_t N, double a, double b); static gtsam::Vector Points(size_t N, double a, double b);
static Matrix WeightMatrix(size_t N, Vector X); static gtsam::Matrix WeightMatrix(size_t N, gtsam::Vector X);
static Matrix WeightMatrix(size_t N, Vector X, double a, double b); static gtsam::Matrix WeightMatrix(size_t N, gtsam::Vector X, double a, double b);
static Matrix CalculateWeights(size_t N, double x, double a, double b); static gtsam::Matrix CalculateWeights(size_t N, double x, double a, double b);
static Matrix DerivativeWeights(size_t N, double x, double a, double b); static gtsam::Matrix DerivativeWeights(size_t N, double x, double a, double b);
static Matrix IntegrationWeights(size_t N, double a, double b); static gtsam::Matrix IntegrationWeights(size_t N, double a, double b);
static Matrix DifferentiationMatrix(size_t N, double a, double b); static gtsam::Matrix DifferentiationMatrix(size_t N, double a, double b);
}; };
#include <gtsam/basis/BasisFactors.h> #include <gtsam/basis/BasisFactors.h>
@ -64,10 +64,10 @@ template <BASIS = {gtsam::FourierBasis, gtsam::Chebyshev1Basis,
gtsam::Chebyshev2Basis, gtsam::Chebyshev2}> gtsam::Chebyshev2Basis, gtsam::Chebyshev2}>
virtual class VectorEvaluationFactor : gtsam::NoiseModelFactor { virtual class VectorEvaluationFactor : gtsam::NoiseModelFactor {
VectorEvaluationFactor(); VectorEvaluationFactor();
VectorEvaluationFactor(gtsam::Key key, const Vector& z, VectorEvaluationFactor(gtsam::Key key, const gtsam::Vector& z,
const gtsam::noiseModel::Base* model, const size_t M, const gtsam::noiseModel::Base* model, const size_t M,
const size_t N, double x); const size_t N, double x);
VectorEvaluationFactor(gtsam::Key key, const Vector& z, VectorEvaluationFactor(gtsam::Key key, const gtsam::Vector& z,
const gtsam::noiseModel::Base* model, const size_t M, const gtsam::noiseModel::Base* model, const size_t M,
const size_t N, double x, double a, double b); const size_t N, double x, double a, double b);
}; };
@ -116,10 +116,10 @@ template <BASIS = {gtsam::FourierBasis, gtsam::Chebyshev1Basis,
gtsam::Chebyshev2Basis, gtsam::Chebyshev2}> gtsam::Chebyshev2Basis, gtsam::Chebyshev2}>
virtual class VectorDerivativeFactor : gtsam::NoiseModelFactor { virtual class VectorDerivativeFactor : gtsam::NoiseModelFactor {
VectorDerivativeFactor(); VectorDerivativeFactor();
VectorDerivativeFactor(gtsam::Key key, const Vector& z, VectorDerivativeFactor(gtsam::Key key, const gtsam::Vector& z,
const gtsam::noiseModel::Base* model, const size_t M, const gtsam::noiseModel::Base* model, const size_t M,
const size_t N, double x); const size_t N, double x);
VectorDerivativeFactor(gtsam::Key key, const Vector& z, VectorDerivativeFactor(gtsam::Key key, const gtsam::Vector& z,
const gtsam::noiseModel::Base* model, const size_t M, const gtsam::noiseModel::Base* model, const size_t M,
const size_t N, double x, double a, double b); const size_t N, double x, double a, double b);
}; };

328
gtsam/geometry/geometry.i
View File

@ -9,7 +9,7 @@ class Point2 {
// Standard Constructors // Standard Constructors
Point2(); Point2();
Point2(double x, double y); Point2(double x, double y);
Point2(Vector v); Point2(gtsam::Vector v);
// Testable // Testable
void print(string s = "") const; void print(string s = "") const;
@ -21,7 +21,7 @@ class Point2 {
// Standard Interface // Standard Interface
double x() const; double x() const;
double y() const; double y() const;
Vector vector() const; gtsam::Vector vector() const;
double distance(const gtsam::Point2& p2) const; double distance(const gtsam::Point2& p2) const;
double norm() const; double norm() const;
@ -83,21 +83,21 @@ class StereoPoint2 {
// Operator Overloads // Operator Overloads
gtsam::StereoPoint2 operator-() const; gtsam::StereoPoint2 operator-() const;
// gtsam::StereoPoint2 operator+(Vector b) const; //TODO Mixed types not yet // gtsam::StereoPoint2 operator+(gtsam::Vector b) const; //TODO Mixed types not yet
// supported // supported
gtsam::StereoPoint2 operator+(const gtsam::StereoPoint2& p2) const; gtsam::StereoPoint2 operator+(const gtsam::StereoPoint2& p2) const;
gtsam::StereoPoint2 operator-(const gtsam::StereoPoint2& p2) const; gtsam::StereoPoint2 operator-(const gtsam::StereoPoint2& p2) const;
// Manifold // Manifold
gtsam::StereoPoint2 retract(Vector v) const; gtsam::StereoPoint2 retract(gtsam::Vector v) const;
Vector localCoordinates(const gtsam::StereoPoint2& p) const; gtsam::Vector localCoordinates(const gtsam::StereoPoint2& p) const;
// Lie Group // Lie Group
static gtsam::StereoPoint2 Expmap(Vector v); static gtsam::StereoPoint2 Expmap(gtsam::Vector v);
static Vector Logmap(const gtsam::StereoPoint2& p); static gtsam::Vector Logmap(const gtsam::StereoPoint2& p);
// Standard Interface // Standard Interface
Vector vector() const; gtsam::Vector vector() const;
double uL() const; double uL() const;
double uR() const; double uR() const;
double v() const; double v() const;
@ -111,7 +111,7 @@ class Point3 {
// Standard Constructors // Standard Constructors
Point3(); Point3();
Point3(double x, double y, double z); Point3(double x, double y, double z);
Point3(Vector v); Point3(gtsam::Vector v);
// Testable // Testable
void print(string s = "") const; void print(string s = "") const;
@ -121,7 +121,7 @@ class Point3 {
static gtsam::Point3 Identity(); static gtsam::Point3 Identity();
// Standard Interface // Standard Interface
Vector vector() const; gtsam::Vector vector() const;
double x() const; double x() const;
double y() const; double y() const;
double z() const; double z() const;
@ -166,15 +166,15 @@ class Rot2 {
gtsam::Rot2 operator*(const gtsam::Rot2& p2) const; gtsam::Rot2 operator*(const gtsam::Rot2& p2) const;
// Manifold // Manifold
gtsam::Rot2 retract(Vector v) const; gtsam::Rot2 retract(gtsam::Vector v) const;
gtsam::Rot2 retract(Vector v, Eigen::Ref<Eigen::MatrixXd> H1, Eigen::Ref<Eigen::MatrixXd> H2) const; gtsam::Rot2 retract(gtsam::Vector v, Eigen::Ref<Eigen::MatrixXd> H1, Eigen::Ref<Eigen::MatrixXd> H2) const;
Vector localCoordinates(const gtsam::Rot2& p) const; gtsam::Vector localCoordinates(const gtsam::Rot2& p) const;
Vector localCoordinates(const gtsam::Rot2& p, Eigen::Ref<Eigen::MatrixXd> H1, Eigen::Ref<Eigen::MatrixXd> H2) const; gtsam::Vector localCoordinates(const gtsam::Rot2& p, Eigen::Ref<Eigen::MatrixXd> H1, Eigen::Ref<Eigen::MatrixXd> H2) const;
// Lie Group // Lie Group
static gtsam::Rot2 Expmap(Vector v); static gtsam::Rot2 Expmap(gtsam::Vector v);
static Vector Logmap(const gtsam::Rot2& p); static gtsam::Vector Logmap(const gtsam::Rot2& p);
Vector logmap(const gtsam::Rot2& p); gtsam::Vector logmap(const gtsam::Rot2& p);
// Group Action on Point2 // Group Action on Point2
gtsam::Point2 rotate(const gtsam::Point2& point) const; gtsam::Point2 rotate(const gtsam::Point2& point) const;
@ -188,7 +188,7 @@ class Rot2 {
double degrees() const; double degrees() const;
double c() const; double c() const;
double s() const; double s() const;
Matrix matrix() const; gtsam::Matrix matrix() const;
// enabling serialization functionality // enabling serialization functionality
void serialize() const; void serialize() const;
@ -198,10 +198,10 @@ class Rot2 {
class SO3 { class SO3 {
// Standard Constructors // Standard Constructors
SO3(); SO3();
SO3(Matrix R); SO3(gtsam::Matrix R);
static gtsam::SO3 FromMatrix(Matrix R); static gtsam::SO3 FromMatrix(gtsam::Matrix R);
static gtsam::SO3 AxisAngle(const Vector axis, double theta); static gtsam::SO3 AxisAngle(const gtsam::Vector axis, double theta);
static gtsam::SO3 ClosestTo(const Matrix M); static gtsam::SO3 ClosestTo(const gtsam::Matrix M);
// Testable // Testable
void print(string s = "") const; void print(string s = "") const;
@ -217,21 +217,21 @@ class SO3 {
gtsam::SO3 operator*(const gtsam::SO3& R) const; gtsam::SO3 operator*(const gtsam::SO3& R) const;
// Manifold // Manifold
gtsam::SO3 retract(Vector v) const; gtsam::SO3 retract(gtsam::Vector v) const;
Vector localCoordinates(const gtsam::SO3& R) const; gtsam::Vector localCoordinates(const gtsam::SO3& R) const;
static gtsam::SO3 Expmap(Vector v); static gtsam::SO3 Expmap(gtsam::Vector v);
// Other methods // Other methods
Vector vec() const; gtsam::Vector vec() const;
Matrix matrix() const; gtsam::Matrix matrix() const;
}; };
#include <gtsam/geometry/SO4.h> #include <gtsam/geometry/SO4.h>
class SO4 { class SO4 {
// Standard Constructors // Standard Constructors
SO4(); SO4();
SO4(Matrix R); SO4(gtsam::Matrix R);
static gtsam::SO4 FromMatrix(Matrix R); static gtsam::SO4 FromMatrix(gtsam::Matrix R);
// Testable // Testable
void print(string s = "") const; void print(string s = "") const;
@ -247,21 +247,21 @@ class SO4 {
gtsam::SO4 operator*(const gtsam::SO4& Q) const; gtsam::SO4 operator*(const gtsam::SO4& Q) const;
// Manifold // Manifold
gtsam::SO4 retract(Vector v) const; gtsam::SO4 retract(gtsam::Vector v) const;
Vector localCoordinates(const gtsam::SO4& Q) const; gtsam::Vector localCoordinates(const gtsam::SO4& Q) const;
static gtsam::SO4 Expmap(Vector v); static gtsam::SO4 Expmap(gtsam::Vector v);
// Other methods // Other methods
Vector vec() const; gtsam::Vector vec() const;
Matrix matrix() const; gtsam::Matrix matrix() const;
}; };
#include <gtsam/geometry/SOn.h> #include <gtsam/geometry/SOn.h>
class SOn { class SOn {
// Standard Constructors // Standard Constructors
SOn(size_t n); SOn(size_t n);
static gtsam::SOn FromMatrix(Matrix R); static gtsam::SOn FromMatrix(gtsam::Matrix R);
static gtsam::SOn Lift(size_t n, Matrix R); static gtsam::SOn Lift(size_t n, gtsam::Matrix R);
// Testable // Testable
void print(string s = "") const; void print(string s = "") const;
@ -277,13 +277,13 @@ class SOn {
gtsam::SOn operator*(const gtsam::SOn& Q) const; gtsam::SOn operator*(const gtsam::SOn& Q) const;
// Manifold // Manifold
gtsam::SOn retract(Vector v) const; gtsam::SOn retract(gtsam::Vector v) const;
Vector localCoordinates(const gtsam::SOn& Q) const; gtsam::Vector localCoordinates(const gtsam::SOn& Q) const;
static gtsam::SOn Expmap(Vector v); static gtsam::SOn Expmap(gtsam::Vector v);
// Other methods // Other methods
Vector vec() const; gtsam::Vector vec() const;
Matrix matrix() const; gtsam::Matrix matrix() const;
// enabling serialization functionality // enabling serialization functionality
void serialize() const; void serialize() const;
@ -295,14 +295,14 @@ class Quaternion {
double x() const; double x() const;
double y() const; double y() const;
double z() const; double z() const;
Vector coeffs() const; gtsam::Vector coeffs() const;
}; };
#include <gtsam/geometry/Rot3.h> #include <gtsam/geometry/Rot3.h>
class Rot3 { class Rot3 {
// Standard Constructors and Named Constructors // Standard Constructors and Named Constructors
Rot3(); Rot3();
Rot3(Matrix R); Rot3(gtsam::Matrix R);
Rot3(const gtsam::Point3& col1, const gtsam::Point3& col2, Rot3(const gtsam::Point3& col1, const gtsam::Point3& col2,
const gtsam::Point3& col3); const gtsam::Point3& col3);
Rot3(double R11, double R12, double R13, double R21, double R22, double R23, Rot3(double R11, double R12, double R13, double R21, double R22, double R23,
@ -313,7 +313,7 @@ class Rot3 {
static gtsam::Rot3 Ry(double t); static gtsam::Rot3 Ry(double t);
static gtsam::Rot3 Rz(double t); static gtsam::Rot3 Rz(double t);
static gtsam::Rot3 RzRyRx(double x, double y, double z); static gtsam::Rot3 RzRyRx(double x, double y, double z);
static gtsam::Rot3 RzRyRx(Vector xyz); static gtsam::Rot3 RzRyRx(gtsam::Vector xyz);
static gtsam::Rot3 Yaw( static gtsam::Rot3 Yaw(
double t); // positive yaw is to right (as in aircraft heading) double t); // positive yaw is to right (as in aircraft heading)
static gtsam::Rot3 Pitch( static gtsam::Rot3 Pitch(
@ -323,9 +323,9 @@ class Rot3 {
static gtsam::Rot3 Ypr(double y, double p, double r); static gtsam::Rot3 Ypr(double y, double p, double r);
static gtsam::Rot3 Quaternion(double w, double x, double y, double z); static gtsam::Rot3 Quaternion(double w, double x, double y, double z);
static gtsam::Rot3 AxisAngle(const gtsam::Point3& axis, double angle); static gtsam::Rot3 AxisAngle(const gtsam::Point3& axis, double angle);
static gtsam::Rot3 Rodrigues(Vector v); static gtsam::Rot3 Rodrigues(gtsam::Vector v);
static gtsam::Rot3 Rodrigues(double wx, double wy, double wz); static gtsam::Rot3 Rodrigues(double wx, double wy, double wz);
static gtsam::Rot3 ClosestTo(const Matrix M); static gtsam::Rot3 ClosestTo(const gtsam::Matrix M);
// Testable // Testable
void print(string s = "") const; void print(string s = "") const;
@ -341,10 +341,10 @@ class Rot3 {
gtsam::Rot3 operator*(const gtsam::Rot3& p2) const; gtsam::Rot3 operator*(const gtsam::Rot3& p2) const;
// Manifold // Manifold
// gtsam::Rot3 retractCayley(Vector v) const; // TODO, does not exist in both // gtsam::Rot3 retractCayley(gtsam::Vector v) const; // TODO, does not exist in both
// Matrix and Quaternion options // gtsam::Matrix and Quaternion options
gtsam::Rot3 retract(Vector v) const; gtsam::Rot3 retract(gtsam::Vector v) const;
Vector localCoordinates(const gtsam::Rot3& p) const; gtsam::Vector localCoordinates(const gtsam::Rot3& p) const;
// Group Action on Point3 // Group Action on Point3
gtsam::Point3 rotate(const gtsam::Point3& p) const; gtsam::Point3 rotate(const gtsam::Point3& p) const;
@ -358,21 +358,21 @@ class Rot3 {
gtsam::Unit3 unrotate(const gtsam::Unit3& p) const; gtsam::Unit3 unrotate(const gtsam::Unit3& p) const;
// Standard Interface // Standard Interface
static gtsam::Rot3 Expmap(Vector v); static gtsam::Rot3 Expmap(gtsam::Vector v);
static Vector Logmap(const gtsam::Rot3& p); static gtsam::Vector Logmap(const gtsam::Rot3& p);
Vector logmap(const gtsam::Rot3& p); gtsam::Vector logmap(const gtsam::Rot3& p);
Matrix matrix() const; gtsam::Matrix matrix() const;
Matrix transpose() const; gtsam::Matrix transpose() const;
gtsam::Point3 column(size_t index) const; gtsam::Point3 column(size_t index) const;
Vector xyz() const; gtsam::Vector xyz() const;
Vector ypr() const; gtsam::Vector ypr() const;
Vector rpy() const; gtsam::Vector rpy() const;
double roll() const; double roll() const;
double pitch() const; double pitch() const;
double yaw() const; double yaw() const;
pair<gtsam::Unit3, double> axisAngle() const; pair<gtsam::Unit3, double> axisAngle() const;
gtsam::Quaternion toQuaternion() const; gtsam::Quaternion toQuaternion() const;
// Vector quaternion() const; // @deprecated, see https://github.com/borglab/gtsam/pull/1219 // gtsam::Vector quaternion() const; // @deprecated, see https://github.com/borglab/gtsam/pull/1219
gtsam::Rot3 slerp(double t, const gtsam::Rot3& other) const; gtsam::Rot3 slerp(double t, const gtsam::Rot3& other) const;
// enabling serialization functionality // enabling serialization functionality
@ -387,7 +387,7 @@ class Pose2 {
Pose2(double x, double y, double theta); Pose2(double x, double y, double theta);
Pose2(double theta, const gtsam::Point2& t); Pose2(double theta, const gtsam::Point2& t);
Pose2(const gtsam::Rot2& r, const gtsam::Point2& t); Pose2(const gtsam::Rot2& r, const gtsam::Point2& t);
Pose2(Vector v); Pose2(gtsam::Vector v);
static std::optional<gtsam::Pose2> Align(const gtsam::Point2Pairs& abPointPairs); static std::optional<gtsam::Pose2> Align(const gtsam::Point2Pairs& abPointPairs);
static std::optional<gtsam::Pose2> Align(const gtsam::Matrix& a, const gtsam::Matrix& b); static std::optional<gtsam::Pose2> Align(const gtsam::Matrix& a, const gtsam::Matrix& b);
@ -408,32 +408,32 @@ class Pose2 {
gtsam::Pose2 operator*(const gtsam::Pose2& p2) const; gtsam::Pose2 operator*(const gtsam::Pose2& p2) const;
// Manifold // Manifold
gtsam::Pose2 retract(Vector v) const; gtsam::Pose2 retract(gtsam::Vector v) const;
gtsam::Pose2 retract(Vector v, Eigen::Ref<Eigen::MatrixXd> H1, Eigen::Ref<Eigen::MatrixXd> H2) const; gtsam::Pose2 retract(gtsam::Vector v, Eigen::Ref<Eigen::MatrixXd> H1, Eigen::Ref<Eigen::MatrixXd> H2) const;
Vector localCoordinates(const gtsam::Pose2& p) const; gtsam::Vector localCoordinates(const gtsam::Pose2& p) const;
Vector localCoordinates(const gtsam::Pose2& p, Eigen::Ref<Eigen::MatrixXd> H1, Eigen::Ref<Eigen::MatrixXd> H2) const; gtsam::Vector localCoordinates(const gtsam::Pose2& p, Eigen::Ref<Eigen::MatrixXd> H1, Eigen::Ref<Eigen::MatrixXd> H2) const;
// Lie Group // Lie Group
static gtsam::Pose2 Expmap(Vector v); static gtsam::Pose2 Expmap(gtsam::Vector v);
static Vector Logmap(const gtsam::Pose2& p); static gtsam::Vector Logmap(const gtsam::Pose2& p);
Vector logmap(const gtsam::Pose2& p); gtsam::Vector logmap(const gtsam::Pose2& p);
Vector logmap(const gtsam::Pose2& p, Eigen::Ref<Eigen::MatrixXd> H); gtsam::Vector logmap(const gtsam::Pose2& p, Eigen::Ref<Eigen::MatrixXd> H);
static Matrix ExpmapDerivative(Vector v); static gtsam::Matrix ExpmapDerivative(gtsam::Vector v);
static Matrix LogmapDerivative(const gtsam::Pose2& v); static gtsam::Matrix LogmapDerivative(const gtsam::Pose2& v);
Matrix AdjointMap() const; gtsam::Matrix AdjointMap() const;
Vector Adjoint(Vector xi) const; gtsam::Vector Adjoint(gtsam::Vector xi) const;
static Matrix adjointMap_(Vector v); static gtsam::Matrix adjointMap_(gtsam::Vector v);
static Vector adjoint_(Vector xi, Vector y); static gtsam::Vector adjoint_(gtsam::Vector xi, gtsam::Vector y);
static Vector adjointTranspose(Vector xi, Vector y); static gtsam::Vector adjointTranspose(gtsam::Vector xi, gtsam::Vector y);
static Matrix wedge(double vx, double vy, double w); static gtsam::Matrix wedge(double vx, double vy, double w);
// Group Actions on Point2 // Group Actions on Point2
gtsam::Point2 transformFrom(const gtsam::Point2& p) const; gtsam::Point2 transformFrom(const gtsam::Point2& p) const;
gtsam::Point2 transformTo(const gtsam::Point2& p) const; gtsam::Point2 transformTo(const gtsam::Point2& p) const;
// Matrix versions // gtsam::Matrix versions
Matrix transformFrom(const Matrix& points) const; gtsam::Matrix transformFrom(const gtsam::Matrix& points) const;
Matrix transformTo(const Matrix& points) const; gtsam::Matrix transformTo(const gtsam::Matrix& points) const;
// Standard Interface // Standard Interface
double x() const; double x() const;
@ -445,7 +445,7 @@ class Pose2 {
gtsam::Point2 translation(Eigen::Ref<Eigen::MatrixXd> Hself) const; gtsam::Point2 translation(Eigen::Ref<Eigen::MatrixXd> Hself) const;
gtsam::Rot2 rotation() const; gtsam::Rot2 rotation() const;
gtsam::Rot2 rotation(Eigen::Ref<Eigen::MatrixXd> Hself) const; gtsam::Rot2 rotation(Eigen::Ref<Eigen::MatrixXd> Hself) const;
Matrix matrix() const; gtsam::Matrix matrix() const;
// enabling serialization functionality // enabling serialization functionality
void serialize() const; void serialize() const;
@ -458,7 +458,7 @@ class Pose3 {
Pose3(const gtsam::Pose3& other); Pose3(const gtsam::Pose3& other);
Pose3(const gtsam::Rot3& r, const gtsam::Point3& t); Pose3(const gtsam::Rot3& r, const gtsam::Point3& t);
Pose3(const gtsam::Pose2& pose2); Pose3(const gtsam::Pose2& pose2);
Pose3(Matrix mat); Pose3(gtsam::Matrix mat);
static std::optional<gtsam::Pose3> Align(const gtsam::Point3Pairs& abPointPairs); static std::optional<gtsam::Pose3> Align(const gtsam::Point3Pairs& abPointPairs);
static std::optional<gtsam::Pose3> Align(const gtsam::Matrix& a, const gtsam::Matrix& b); static std::optional<gtsam::Pose3> Align(const gtsam::Matrix& a, const gtsam::Matrix& b);
@ -488,33 +488,33 @@ class Pose3 {
gtsam::Pose3 operator*(const gtsam::Pose3& pose) const; gtsam::Pose3 operator*(const gtsam::Pose3& pose) const;
// Manifold // Manifold
gtsam::Pose3 retract(Vector v) const; gtsam::Pose3 retract(gtsam::Vector v) const;
gtsam::Pose3 retract(Vector v, Eigen::Ref<Eigen::MatrixXd> Hxi) const; gtsam::Pose3 retract(gtsam::Vector v, Eigen::Ref<Eigen::MatrixXd> Hxi) const;
Vector localCoordinates(const gtsam::Pose3& pose) const; gtsam::Vector localCoordinates(const gtsam::Pose3& pose) const;
Vector localCoordinates(const gtsam::Pose3& pose, Eigen::Ref<Eigen::MatrixXd> Hxi) const; gtsam::Vector localCoordinates(const gtsam::Pose3& pose, Eigen::Ref<Eigen::MatrixXd> Hxi) const;
// Lie Group // Lie Group
static gtsam::Pose3 Expmap(Vector v); static gtsam::Pose3 Expmap(gtsam::Vector v);
static gtsam::Pose3 Expmap(Vector v, Eigen::Ref<Eigen::MatrixXd> Hxi); static gtsam::Pose3 Expmap(gtsam::Vector v, Eigen::Ref<Eigen::MatrixXd> Hxi);
static Vector Logmap(const gtsam::Pose3& pose); static gtsam::Vector Logmap(const gtsam::Pose3& pose);
static Vector Logmap(const gtsam::Pose3& pose, Eigen::Ref<Eigen::MatrixXd> Hpose); static gtsam::Vector Logmap(const gtsam::Pose3& pose, Eigen::Ref<Eigen::MatrixXd> Hpose);
gtsam::Pose3 expmap(Vector v); gtsam::Pose3 expmap(gtsam::Vector v);
Vector logmap(const gtsam::Pose3& pose); gtsam::Vector logmap(const gtsam::Pose3& pose);
Matrix AdjointMap() const; gtsam::Matrix AdjointMap() const;
Vector Adjoint(Vector xi) const; gtsam::Vector Adjoint(gtsam::Vector xi) const;
Vector Adjoint(Vector xi, Eigen::Ref<Eigen::MatrixXd> H_this, gtsam::Vector Adjoint(gtsam::Vector xi, Eigen::Ref<Eigen::MatrixXd> H_this,
Eigen::Ref<Eigen::MatrixXd> H_xib) const; Eigen::Ref<Eigen::MatrixXd> H_xib) const;
Vector AdjointTranspose(Vector xi) const; gtsam::Vector AdjointTranspose(gtsam::Vector xi) const;
Vector AdjointTranspose(Vector xi, Eigen::Ref<Eigen::MatrixXd> H_this, gtsam::Vector AdjointTranspose(gtsam::Vector xi, Eigen::Ref<Eigen::MatrixXd> H_this,
Eigen::Ref<Eigen::MatrixXd> H_x) const; Eigen::Ref<Eigen::MatrixXd> H_x) const;
static Matrix adjointMap(Vector xi); static gtsam::Matrix adjointMap(gtsam::Vector xi);
static Vector adjoint(Vector xi, Vector y); static gtsam::Vector adjoint(gtsam::Vector xi, gtsam::Vector y);
static Matrix adjointMap_(Vector xi); static gtsam::Matrix adjointMap_(gtsam::Vector xi);
static Vector adjoint_(Vector xi, Vector y); static gtsam::Vector adjoint_(gtsam::Vector xi, gtsam::Vector y);
static Vector adjointTranspose(Vector xi, Vector y); static gtsam::Vector adjointTranspose(gtsam::Vector xi, gtsam::Vector y);
static Matrix ExpmapDerivative(Vector xi); static gtsam::Matrix ExpmapDerivative(gtsam::Vector xi);
static Matrix LogmapDerivative(const gtsam::Pose3& xi); static gtsam::Matrix LogmapDerivative(const gtsam::Pose3& xi);
static Matrix wedge(double wx, double wy, double wz, double vx, double vy, static gtsam::Matrix wedge(double wx, double wy, double wz, double vx, double vy,
double vz); double vz);
// Group Action on Point3 // Group Action on Point3
@ -525,9 +525,9 @@ class Pose3 {
gtsam::Point3 transformTo(const gtsam::Point3& point, Eigen::Ref<Eigen::MatrixXd> Hself, gtsam::Point3 transformTo(const gtsam::Point3& point, Eigen::Ref<Eigen::MatrixXd> Hself,
Eigen::Ref<Eigen::MatrixXd> Hpoint) const; Eigen::Ref<Eigen::MatrixXd> Hpoint) const;
// Matrix versions // gtsam::Matrix versions
Matrix transformFrom(const Matrix& points) const; gtsam::Matrix transformFrom(const gtsam::Matrix& points) const;
Matrix transformTo(const Matrix& points) const; gtsam::Matrix transformTo(const gtsam::Matrix& points) const;
// Standard Interface // Standard Interface
gtsam::Rot3 rotation() const; gtsam::Rot3 rotation() const;
@ -537,7 +537,7 @@ class Pose3 {
double x() const; double x() const;
double y() const; double y() const;
double z() const; double z() const;
Matrix matrix() const; gtsam::Matrix matrix() const;
gtsam::Pose3 transformPoseFrom(const gtsam::Pose3& pose) const; gtsam::Pose3 transformPoseFrom(const gtsam::Pose3& pose) const;
gtsam::Pose3 transformPoseFrom(const gtsam::Pose3& pose, Eigen::Ref<Eigen::MatrixXd> Hself, gtsam::Pose3 transformPoseFrom(const gtsam::Pose3& pose, Eigen::Ref<Eigen::MatrixXd> Hself,
Eigen::Ref<Eigen::MatrixXd> HaTb) const; Eigen::Ref<Eigen::MatrixXd> HaTb) const;
@ -584,9 +584,9 @@ class Unit3 {
bool equals(const gtsam::Unit3& pose, double tol) const; bool equals(const gtsam::Unit3& pose, double tol) const;
// Other functionality // Other functionality
Matrix basis() const; gtsam::Matrix basis() const;
Matrix basis(Eigen::Ref<Eigen::MatrixXd> H) const; gtsam::Matrix basis(Eigen::Ref<Eigen::MatrixXd> H) const;
Matrix skew() const; gtsam::Matrix skew() const;
gtsam::Point3 point3() const; gtsam::Point3 point3() const;
gtsam::Point3 point3(Eigen::Ref<Eigen::MatrixXd> H) const; gtsam::Point3 point3(Eigen::Ref<Eigen::MatrixXd> H) const;
@ -602,8 +602,8 @@ class Unit3 {
// Manifold // Manifold
static size_t Dim(); static size_t Dim();
size_t dim() const; size_t dim() const;
gtsam::Unit3 retract(Vector v) const; gtsam::Unit3 retract(gtsam::Vector v) const;
Vector localCoordinates(const gtsam::Unit3& s) const; gtsam::Vector localCoordinates(const gtsam::Unit3& s) const;
gtsam::Unit3 FromPoint3(const gtsam::Point3& point) const; gtsam::Unit3 FromPoint3(const gtsam::Point3& point) const;
gtsam::Unit3 FromPoint3(const gtsam::Point3& point, Eigen::Ref<Eigen::MatrixXd> H) const; gtsam::Unit3 FromPoint3(const gtsam::Point3& point, Eigen::Ref<Eigen::MatrixXd> H) const;
@ -632,14 +632,14 @@ class EssentialMatrix {
// Manifold // Manifold
static size_t Dim(); static size_t Dim();
size_t dim() const; size_t dim() const;
gtsam::EssentialMatrix retract(Vector v) const; gtsam::EssentialMatrix retract(gtsam::Vector v) const;
Vector localCoordinates(const gtsam::EssentialMatrix& s) const; gtsam::Vector localCoordinates(const gtsam::EssentialMatrix& s) const;
// Other methods: // Other methods:
gtsam::Rot3 rotation() const; gtsam::Rot3 rotation() const;
gtsam::Unit3 direction() const; gtsam::Unit3 direction() const;
Matrix matrix() const; gtsam::Matrix matrix() const;
double error(Vector vA, Vector vB); double error(gtsam::Vector vA, gtsam::Vector vB);
}; };
#include <gtsam/geometry/Cal3_S2.h> #include <gtsam/geometry/Cal3_S2.h>
@ -647,7 +647,7 @@ class Cal3_S2 {
// Standard Constructors // Standard Constructors
Cal3_S2(); Cal3_S2();
Cal3_S2(double fx, double fy, double s, double u0, double v0); Cal3_S2(double fx, double fy, double s, double u0, double v0);
Cal3_S2(Vector v); Cal3_S2(gtsam::Vector v);
Cal3_S2(double fov, int w, int h); Cal3_S2(double fov, int w, int h);
// Testable // Testable
@ -657,8 +657,8 @@ class Cal3_S2 {
// Manifold // Manifold
static size_t Dim(); static size_t Dim();
size_t dim() const; size_t dim() const;
gtsam::Cal3_S2 retract(Vector v) const; gtsam::Cal3_S2 retract(gtsam::Vector v) const;
Vector localCoordinates(const gtsam::Cal3_S2& c) const; gtsam::Vector localCoordinates(const gtsam::Cal3_S2& c) const;
// Action on Point2 // Action on Point2
gtsam::Point2 calibrate(const gtsam::Point2& p) const; gtsam::Point2 calibrate(const gtsam::Point2& p) const;
@ -677,9 +677,9 @@ class Cal3_S2 {
double px() const; double px() const;
double py() const; double py() const;
gtsam::Point2 principalPoint() const; gtsam::Point2 principalPoint() const;
Vector vector() const; gtsam::Vector vector() const;
Matrix K() const; gtsam::Matrix K() const;
Matrix inverse() const; gtsam::Matrix inverse() const;
// enabling serialization functionality // enabling serialization functionality
void serialize() const; void serialize() const;
@ -701,9 +701,9 @@ virtual class Cal3DS2_Base {
double py() const; double py() const;
double k1() const; double k1() const;
double k2() const; double k2() const;
Matrix K() const; gtsam::Matrix K() const;
Vector k() const; gtsam::Vector k() const;
Vector vector() const; gtsam::Vector vector() const;
// Action on Point2 // Action on Point2
gtsam::Point2 uncalibrate(const gtsam::Point2& p) const; gtsam::Point2 uncalibrate(const gtsam::Point2& p) const;
@ -727,7 +727,7 @@ virtual class Cal3DS2 : gtsam::Cal3DS2_Base {
double k2); double k2);
Cal3DS2(double fx, double fy, double s, double u0, double v0, double k1, Cal3DS2(double fx, double fy, double s, double u0, double v0, double k1,
double k2, double p1, double p2); double k2, double p1, double p2);
Cal3DS2(Vector v); Cal3DS2(gtsam::Vector v);
// Testable // Testable
bool equals(const gtsam::Cal3DS2& rhs, double tol) const; bool equals(const gtsam::Cal3DS2& rhs, double tol) const;
@ -735,8 +735,8 @@ virtual class Cal3DS2 : gtsam::Cal3DS2_Base {
// Manifold // Manifold
size_t dim() const; size_t dim() const;
static size_t Dim(); static size_t Dim();
gtsam::Cal3DS2 retract(Vector v) const; gtsam::Cal3DS2 retract(gtsam::Vector v) const;
Vector localCoordinates(const gtsam::Cal3DS2& c) const; gtsam::Vector localCoordinates(const gtsam::Cal3DS2& c) const;
// enabling serialization functionality // enabling serialization functionality
void serialize() const; void serialize() const;
@ -750,7 +750,7 @@ virtual class Cal3Unified : gtsam::Cal3DS2_Base {
double k2); double k2);
Cal3Unified(double fx, double fy, double s, double u0, double v0, double k1, Cal3Unified(double fx, double fy, double s, double u0, double v0, double k1,
double k2, double p1, double p2, double xi); double k2, double p1, double p2, double xi);
Cal3Unified(Vector v); Cal3Unified(gtsam::Vector v);
// Testable // Testable
bool equals(const gtsam::Cal3Unified& rhs, double tol) const; bool equals(const gtsam::Cal3Unified& rhs, double tol) const;
@ -763,8 +763,8 @@ virtual class Cal3Unified : gtsam::Cal3DS2_Base {
// Manifold // Manifold
size_t dim() const; size_t dim() const;
static size_t Dim(); static size_t Dim();
gtsam::Cal3Unified retract(Vector v) const; gtsam::Cal3Unified retract(gtsam::Vector v) const;
Vector localCoordinates(const gtsam::Cal3Unified& c) const; gtsam::Vector localCoordinates(const gtsam::Cal3Unified& c) const;
// Action on Point2 // Action on Point2
// Note: the signature of this functions differ from the functions // Note: the signature of this functions differ from the functions
@ -788,7 +788,7 @@ class Cal3Fisheye {
Cal3Fisheye(); Cal3Fisheye();
Cal3Fisheye(double fx, double fy, double s, double u0, double v0, double k1, Cal3Fisheye(double fx, double fy, double s, double u0, double v0, double k1,
double k2, double k3, double k4, double tol = 1e-5); double k2, double k3, double k4, double tol = 1e-5);
Cal3Fisheye(Vector v); Cal3Fisheye(gtsam::Vector v);
// Testable // Testable
void print(string s = "Cal3Fisheye") const; void print(string s = "Cal3Fisheye") const;
@ -797,8 +797,8 @@ class Cal3Fisheye {
// Manifold // Manifold
static size_t Dim(); static size_t Dim();
size_t dim() const; size_t dim() const;
gtsam::Cal3Fisheye retract(Vector v) const; gtsam::Cal3Fisheye retract(gtsam::Vector v) const;
Vector localCoordinates(const gtsam::Cal3Fisheye& c) const; gtsam::Vector localCoordinates(const gtsam::Cal3Fisheye& c) const;
// Action on Point2 // Action on Point2
gtsam::Point2 calibrate(const gtsam::Point2& p) const; gtsam::Point2 calibrate(const gtsam::Point2& p) const;
@ -821,10 +821,10 @@ class Cal3Fisheye {
double px() const; double px() const;
double py() const; double py() const;
gtsam::Point2 principalPoint() const; gtsam::Point2 principalPoint() const;
Vector vector() const; gtsam::Vector vector() const;
Vector k() const; gtsam::Vector k() const;
Matrix K() const; gtsam::Matrix K() const;
Matrix inverse() const; gtsam::Matrix inverse() const;
// enabling serialization functionality // enabling serialization functionality
void serialize() const; void serialize() const;
@ -835,13 +835,13 @@ class Cal3_S2Stereo {
// Standard Constructors // Standard Constructors
Cal3_S2Stereo(); Cal3_S2Stereo();
Cal3_S2Stereo(double fx, double fy, double s, double u0, double v0, double b); Cal3_S2Stereo(double fx, double fy, double s, double u0, double v0, double b);
Cal3_S2Stereo(Vector v); Cal3_S2Stereo(gtsam::Vector v);
// Manifold // Manifold
static size_t Dim(); static size_t Dim();
size_t dim() const; size_t dim() const;
gtsam::Cal3_S2Stereo retract(Vector v) const; gtsam::Cal3_S2Stereo retract(gtsam::Vector v) const;
Vector localCoordinates(const gtsam::Cal3_S2Stereo& c) const; gtsam::Vector localCoordinates(const gtsam::Cal3_S2Stereo& c) const;
// Testable // Testable
void print(string s = "") const; void print(string s = "") const;
@ -853,11 +853,11 @@ class Cal3_S2Stereo {
double skew() const; double skew() const;
double px() const; double px() const;
double py() const; double py() const;
Matrix K() const; gtsam::Matrix K() const;
gtsam::Point2 principalPoint() const; gtsam::Point2 principalPoint() const;
double baseline() const; double baseline() const;
Vector6 vector() const; Vector6 vector() const;
Matrix inverse() const; gtsam::Matrix inverse() const;
}; };
#include <gtsam/geometry/Cal3Bundler.h> #include <gtsam/geometry/Cal3Bundler.h>
@ -875,8 +875,8 @@ class Cal3Bundler {
// Manifold // Manifold
static size_t Dim(); static size_t Dim();
size_t dim() const; size_t dim() const;
gtsam::Cal3Bundler retract(Vector v) const; gtsam::Cal3Bundler retract(gtsam::Vector v) const;
Vector localCoordinates(const gtsam::Cal3Bundler& c) const; gtsam::Vector localCoordinates(const gtsam::Cal3Bundler& c) const;
// Action on Point2 // Action on Point2
gtsam::Point2 calibrate(const gtsam::Point2& p) const; gtsam::Point2 calibrate(const gtsam::Point2& p) const;
@ -895,9 +895,9 @@ class Cal3Bundler {
double k2() const; double k2() const;
double px() const; double px() const;
double py() const; double py() const;
Vector vector() const; gtsam::Vector vector() const;
Vector k() const; gtsam::Vector k() const;
Matrix K() const; gtsam::Matrix K() const;
// enabling serialization functionality // enabling serialization functionality
void serialize() const; void serialize() const;
@ -908,7 +908,7 @@ class CalibratedCamera {
// Standard Constructors and Named Constructors // Standard Constructors and Named Constructors
CalibratedCamera(); CalibratedCamera();
CalibratedCamera(const gtsam::Pose3& pose); CalibratedCamera(const gtsam::Pose3& pose);
CalibratedCamera(Vector v); CalibratedCamera(gtsam::Vector v);
static gtsam::CalibratedCamera Level(const gtsam::Pose2& pose2, static gtsam::CalibratedCamera Level(const gtsam::Pose2& pose2,
double height); double height);
@ -919,8 +919,8 @@ class CalibratedCamera {
// Manifold // Manifold
static size_t Dim(); static size_t Dim();
size_t dim() const; size_t dim() const;
gtsam::CalibratedCamera retract(Vector d) const; gtsam::CalibratedCamera retract(gtsam::Vector d) const;
Vector localCoordinates(const gtsam::CalibratedCamera& T2) const; gtsam::Vector localCoordinates(const gtsam::CalibratedCamera& T2) const;
// Action on Point3 // Action on Point3
gtsam::Point2 project(const gtsam::Point3& point) const; gtsam::Point2 project(const gtsam::Point3& point) const;
@ -972,8 +972,8 @@ class PinholeCamera {
CALIBRATION calibration() const; CALIBRATION calibration() const;
// Manifold // Manifold
This retract(Vector d) const; This retract(gtsam::Vector d) const;
Vector localCoordinates(const This& T2) const; gtsam::Vector localCoordinates(const This& T2) const;
size_t dim() const; size_t dim() const;
static size_t Dim(); static size_t Dim();
@ -1039,8 +1039,8 @@ class PinholePose {
CALIBRATION calibration() const; CALIBRATION calibration() const;
// Manifold // Manifold
This retract(Vector d) const; This retract(gtsam::Vector d) const;
Vector localCoordinates(const This& T2) const; gtsam::Vector localCoordinates(const This& T2) const;
size_t dim() const; size_t dim() const;
static size_t Dim(); static size_t Dim();
@ -1081,8 +1081,8 @@ class Similarity2 {
Similarity2(); Similarity2();
Similarity2(double s); Similarity2(double s);
Similarity2(const gtsam::Rot2& R, const gtsam::Point2& t, double s); Similarity2(const gtsam::Rot2& R, const gtsam::Point2& t, double s);
Similarity2(const Matrix& R, const Vector& t, double s); Similarity2(const gtsam::Matrix& R, const gtsam::Vector& t, double s);
Similarity2(const Matrix& T); Similarity2(const gtsam::Matrix& T);
gtsam::Point2 transformFrom(const gtsam::Point2& p) const; gtsam::Point2 transformFrom(const gtsam::Point2& p) const;
gtsam::Pose2 transformFrom(const gtsam::Pose2& T); gtsam::Pose2 transformFrom(const gtsam::Pose2& T);
@ -1093,7 +1093,7 @@ class Similarity2 {
// Standard Interface // Standard Interface
bool equals(const gtsam::Similarity2& sim, double tol) const; bool equals(const gtsam::Similarity2& sim, double tol) const;
void print(const std::string& s = "") const; void print(const std::string& s = "") const;
Matrix matrix() const; gtsam::Matrix matrix() const;
gtsam::Rot2& rotation(); gtsam::Rot2& rotation();
gtsam::Point2& translation(); gtsam::Point2& translation();
double scale() const; double scale() const;
@ -1105,8 +1105,8 @@ class Similarity3 {
Similarity3(); Similarity3();
Similarity3(double s); Similarity3(double s);
Similarity3(const gtsam::Rot3& R, const gtsam::Point3& t, double s); Similarity3(const gtsam::Rot3& R, const gtsam::Point3& t, double s);
Similarity3(const Matrix& R, const Vector& t, double s); Similarity3(const gtsam::Matrix& R, const gtsam::Vector& t, double s);
Similarity3(const Matrix& T); Similarity3(const gtsam::Matrix& T);
gtsam::Point3 transformFrom(const gtsam::Point3& p) const; gtsam::Point3 transformFrom(const gtsam::Point3& p) const;
gtsam::Pose3 transformFrom(const gtsam::Pose3& T); gtsam::Pose3 transformFrom(const gtsam::Pose3& T);
@ -1117,7 +1117,7 @@ class Similarity3 {
// Standard Interface // Standard Interface
bool equals(const gtsam::Similarity3& sim, double tol) const; bool equals(const gtsam::Similarity3& sim, double tol) const;
void print(const std::string& s = "") const; void print(const std::string& s = "") const;
Matrix matrix() const; gtsam::Matrix matrix() const;
gtsam::Rot3& rotation(); gtsam::Rot3& rotation();
gtsam::Point3& translation(); gtsam::Point3& translation();
double scale() const; double scale() const;
@ -1148,8 +1148,8 @@ class StereoCamera {
gtsam::Cal3_S2Stereo calibration() const; gtsam::Cal3_S2Stereo calibration() const;
// Manifold // Manifold
gtsam::StereoCamera retract(Vector d) const; gtsam::StereoCamera retract(gtsam::Vector d) const;
Vector localCoordinates(const gtsam::StereoCamera& T2) const; gtsam::Vector localCoordinates(const gtsam::StereoCamera& T2) const;
size_t dim() const; size_t dim() const;
static size_t Dim(); static size_t Dim();

28
gtsam/gtsam.i
View File

@ -161,31 +161,31 @@ class FactorIndices {
namespace utilities { namespace utilities {
#include <gtsam/nonlinear/utilities.h> #include <gtsam/nonlinear/utilities.h>
gtsam::KeyList createKeyList(Vector I); gtsam::KeyList createKeyList(gtsam::Vector I);
gtsam::KeyList createKeyList(string s, Vector I); gtsam::KeyList createKeyList(string s, gtsam::Vector I);
gtsam::KeyVector createKeyVector(Vector I); gtsam::KeyVector createKeyVector(gtsam::Vector I);
gtsam::KeyVector createKeyVector(string s, Vector I); gtsam::KeyVector createKeyVector(string s, gtsam::Vector I);
gtsam::KeySet createKeySet(Vector I); gtsam::KeySet createKeySet(gtsam::Vector I);
gtsam::KeySet createKeySet(string s, Vector I); gtsam::KeySet createKeySet(string s, gtsam::Vector I);
Matrix extractPoint2(const gtsam::Values& values); gtsam::Matrix extractPoint2(const gtsam::Values& values);
Matrix extractPoint3(const gtsam::Values& values); gtsam::Matrix extractPoint3(const gtsam::Values& values);
gtsam::Values allPose2s(gtsam::Values& values); gtsam::Values allPose2s(gtsam::Values& values);
Matrix extractPose2(const gtsam::Values& values); gtsam::Matrix extractPose2(const gtsam::Values& values);
gtsam::Values allPose3s(gtsam::Values& values); gtsam::Values allPose3s(gtsam::Values& values);
Matrix extractPose3(const gtsam::Values& values); gtsam::Matrix extractPose3(const gtsam::Values& values);
Matrix extractVectors(const gtsam::Values& values, char c); gtsam::Matrix extractVectors(const gtsam::Values& values, char c);
void perturbPoint2(gtsam::Values& values, double sigma, int seed = 42u); void perturbPoint2(gtsam::Values& values, double sigma, int seed = 42u);
void perturbPose2(gtsam::Values& values, double sigmaT, double sigmaR, void perturbPose2(gtsam::Values& values, double sigmaT, double sigmaR,
int seed = 42u); int seed = 42u);
void perturbPoint3(gtsam::Values& values, double sigma, int seed = 42u); void perturbPoint3(gtsam::Values& values, double sigma, int seed = 42u);
void insertBackprojections(gtsam::Values& values, void insertBackprojections(gtsam::Values& values,
const gtsam::PinholeCamera<gtsam::Cal3_S2>& c, const gtsam::PinholeCamera<gtsam::Cal3_S2>& c,
Vector J, Matrix Z, double depth); gtsam::Vector J, gtsam::Matrix Z, double depth);
void insertProjectionFactors( void insertProjectionFactors(
gtsam::NonlinearFactorGraph& graph, size_t i, Vector J, Matrix Z, gtsam::NonlinearFactorGraph& graph, size_t i, gtsam::Vector J, gtsam::Matrix Z,
const gtsam::noiseModel::Base* model, const gtsam::Cal3_S2* K, const gtsam::noiseModel::Base* model, const gtsam::Cal3_S2* K,
const gtsam::Pose3& body_P_sensor = gtsam::Pose3()); const gtsam::Pose3& body_P_sensor = gtsam::Pose3());
Matrix reprojectionErrors(const gtsam::NonlinearFactorGraph& graph, gtsam::Matrix reprojectionErrors(const gtsam::NonlinearFactorGraph& graph,
const gtsam::Values& values); const gtsam::Values& values);
gtsam::Values localToWorld(const gtsam::Values& local, gtsam::Values localToWorld(const gtsam::Values& local,
const gtsam::Pose2& base); const gtsam::Pose2& base);

210
gtsam/linear/linear.i
View File

@ -12,52 +12,52 @@ virtual class Base {
// bool isConstrained() const; // bool isConstrained() const;
// bool isUnit() const; // bool isUnit() const;
// size_t dim() const; // size_t dim() const;
// Vector sigmas() const; // gtsam::Vector sigmas() const;
}; };
virtual class Gaussian : gtsam::noiseModel::Base { virtual class Gaussian : gtsam::noiseModel::Base {
static gtsam::noiseModel::Gaussian* Information(Matrix R, bool smart = true); static gtsam::noiseModel::Gaussian* Information(gtsam::Matrix R, bool smart = true);
static gtsam::noiseModel::Gaussian* SqrtInformation(Matrix R, bool smart = true); static gtsam::noiseModel::Gaussian* SqrtInformation(gtsam::Matrix R, bool smart = true);
static gtsam::noiseModel::Gaussian* Covariance(Matrix R, bool smart = true); static gtsam::noiseModel::Gaussian* Covariance(gtsam::Matrix R, bool smart = true);
bool equals(gtsam::noiseModel::Base& expected, double tol); bool equals(gtsam::noiseModel::Base& expected, double tol);
// access to noise model // access to noise model
Matrix R() const; gtsam::Matrix R() const;
Matrix information() const; gtsam::Matrix information() const;
Matrix covariance() const; gtsam::Matrix covariance() const;
// Whitening operations // Whitening operations
Vector whiten(Vector v) const; gtsam::Vector whiten(gtsam::Vector v) const;
Vector unwhiten(Vector v) const; gtsam::Vector unwhiten(gtsam::Vector v) const;
Matrix Whiten(Matrix H) const; gtsam::Matrix Whiten(gtsam::Matrix H) const;
// enabling serialization functionality // enabling serialization functionality
void serializable() const; void serializable() const;
}; };
virtual class Diagonal : gtsam::noiseModel::Gaussian { virtual class Diagonal : gtsam::noiseModel::Gaussian {
static gtsam::noiseModel::Diagonal* Sigmas(Vector sigmas, bool smart = true); static gtsam::noiseModel::Diagonal* Sigmas(gtsam::Vector sigmas, bool smart = true);
static gtsam::noiseModel::Diagonal* Variances(Vector variances, bool smart = true); static gtsam::noiseModel::Diagonal* Variances(gtsam::Vector variances, bool smart = true);
static gtsam::noiseModel::Diagonal* Precisions(Vector precisions, bool smart = true); static gtsam::noiseModel::Diagonal* Precisions(gtsam::Vector precisions, bool smart = true);
Matrix R() const; gtsam::Matrix R() const;
// access to noise model // access to noise model
Vector sigmas() const; gtsam::Vector sigmas() const;
Vector invsigmas() const; gtsam::Vector invsigmas() const;
Vector precisions() const; gtsam::Vector precisions() const;
// enabling serialization functionality // enabling serialization functionality
void serializable() const; void serializable() const;
}; };
virtual class Constrained : gtsam::noiseModel::Diagonal { virtual class Constrained : gtsam::noiseModel::Diagonal {
static gtsam::noiseModel::Constrained* MixedSigmas(Vector mu, Vector sigmas); static gtsam::noiseModel::Constrained* MixedSigmas(gtsam::Vector mu, gtsam::Vector sigmas);
static gtsam::noiseModel::Constrained* MixedSigmas(double m, Vector sigmas); static gtsam::noiseModel::Constrained* MixedSigmas(double m, gtsam::Vector sigmas);
static gtsam::noiseModel::Constrained* MixedVariances(Vector mu, Vector variances); static gtsam::noiseModel::Constrained* MixedVariances(gtsam::Vector mu, gtsam::Vector variances);
static gtsam::noiseModel::Constrained* MixedVariances(Vector variances); static gtsam::noiseModel::Constrained* MixedVariances(gtsam::Vector variances);
static gtsam::noiseModel::Constrained* MixedPrecisions(Vector mu, Vector precisions); static gtsam::noiseModel::Constrained* MixedPrecisions(gtsam::Vector mu, gtsam::Vector precisions);
static gtsam::noiseModel::Constrained* MixedPrecisions(Vector precisions); static gtsam::noiseModel::Constrained* MixedPrecisions(gtsam::Vector precisions);
static gtsam::noiseModel::Constrained* All(size_t dim); static gtsam::noiseModel::Constrained* All(size_t dim);
static gtsam::noiseModel::Constrained* All(size_t dim, double mu); static gtsam::noiseModel::Constrained* All(size_t dim, double mu);
@ -257,13 +257,13 @@ virtual class Robust : gtsam::noiseModel::Base {
class Sampler { class Sampler {
// Constructors // Constructors
Sampler(gtsam::noiseModel::Diagonal* model, int seed); Sampler(gtsam::noiseModel::Diagonal* model, int seed);
Sampler(Vector sigmas, int seed); Sampler(gtsam::Vector sigmas, int seed);
// Standard Interface // Standard Interface
size_t dim() const; size_t dim() const;
Vector sigmas() const; gtsam::Vector sigmas() const;
gtsam::noiseModel::Diagonal* model() const; gtsam::noiseModel::Diagonal* model() const;
Vector sample(); gtsam::Vector sample();
}; };
#include <gtsam/linear/VectorValues.h> #include <gtsam/linear/VectorValues.h>
@ -284,10 +284,10 @@ class VectorValues {
const gtsam::KeyFormatter& keyFormatter = const gtsam::KeyFormatter& keyFormatter =
gtsam::DefaultKeyFormatter) const; gtsam::DefaultKeyFormatter) const;
bool equals(const gtsam::VectorValues& expected, double tol) const; bool equals(const gtsam::VectorValues& expected, double tol) const;
void insert(size_t j, Vector value); void insert(size_t j, gtsam::Vector value);
Vector vector() const; gtsam::Vector vector() const;
Vector vector(const gtsam::KeyVector& keys) const; gtsam::Vector vector(const gtsam::KeyVector& keys) const;
Vector at(size_t j) const; gtsam::Vector at(size_t j) const;
void insert(const gtsam::VectorValues& values); void insert(const gtsam::VectorValues& values);
void update(const gtsam::VectorValues& values); void update(const gtsam::VectorValues& values);
@ -318,23 +318,23 @@ virtual class GaussianFactor : gtsam::Factor {
double error(const gtsam::VectorValues& c) const; double error(const gtsam::VectorValues& c) const;
gtsam::GaussianFactor* clone() const; gtsam::GaussianFactor* clone() const;
gtsam::GaussianFactor* negate() const; gtsam::GaussianFactor* negate() const;
Matrix augmentedInformation() const; gtsam::Matrix augmentedInformation() const;
Matrix information() const; gtsam::Matrix information() const;
Matrix augmentedJacobian() const; gtsam::Matrix augmentedJacobian() const;
pair<Matrix, Vector> jacobian() const; pair<gtsam::Matrix, gtsam::Vector> jacobian() const;
}; };
#include <gtsam/linear/JacobianFactor.h> #include <gtsam/linear/JacobianFactor.h>
virtual class JacobianFactor : gtsam::GaussianFactor { virtual class JacobianFactor : gtsam::GaussianFactor {
//Constructors //Constructors
JacobianFactor(); JacobianFactor();
JacobianFactor(Vector b_in); JacobianFactor(gtsam::Vector b_in);
JacobianFactor(size_t i1, Matrix A1, Vector b, JacobianFactor(size_t i1, gtsam::Matrix A1, gtsam::Vector b,
const gtsam::noiseModel::Diagonal* model); const gtsam::noiseModel::Diagonal* model);
JacobianFactor(size_t i1, Matrix A1, size_t i2, Matrix A2, Vector b, JacobianFactor(size_t i1, gtsam::Matrix A1, size_t i2, gtsam::Matrix A2, gtsam::Vector b,
const gtsam::noiseModel::Diagonal* model); const gtsam::noiseModel::Diagonal* model);
JacobianFactor(size_t i1, Matrix A1, size_t i2, Matrix A2, size_t i3, Matrix A3, JacobianFactor(size_t i1, gtsam::Matrix A1, size_t i2, gtsam::Matrix A2, size_t i3, gtsam::Matrix A3,
Vector b, const gtsam::noiseModel::Diagonal* model); gtsam::Vector b, const gtsam::noiseModel::Diagonal* model);
JacobianFactor(const gtsam::GaussianFactorGraph& graph); JacobianFactor(const gtsam::GaussianFactorGraph& graph);
JacobianFactor(const gtsam::GaussianFactorGraph& graph, JacobianFactor(const gtsam::GaussianFactorGraph& graph,
const gtsam::VariableSlots& p_variableSlots); const gtsam::VariableSlots& p_variableSlots);
@ -349,25 +349,25 @@ virtual class JacobianFactor : gtsam::GaussianFactor {
void print(string s = "", const gtsam::KeyFormatter& keyFormatter = void print(string s = "", const gtsam::KeyFormatter& keyFormatter =
gtsam::DefaultKeyFormatter) const; gtsam::DefaultKeyFormatter) const;
bool equals(const gtsam::GaussianFactor& lf, double tol) const; bool equals(const gtsam::GaussianFactor& lf, double tol) const;
Vector unweighted_error(const gtsam::VectorValues& c) const; gtsam::Vector unweighted_error(const gtsam::VectorValues& c) const;
Vector error_vector(const gtsam::VectorValues& c) const; gtsam::Vector error_vector(const gtsam::VectorValues& c) const;
double error(const gtsam::VectorValues& c) const; double error(const gtsam::VectorValues& c) const;
//Standard Interface //Standard Interface
Matrix getA() const; gtsam::Matrix getA() const;
Vector getb() const; gtsam::Vector getb() const;
size_t rows() const; size_t rows() const;
size_t cols() const; size_t cols() const;
bool isConstrained() const; bool isConstrained() const;
pair<Matrix, Vector> jacobianUnweighted() const; pair<gtsam::Matrix, gtsam::Vector> jacobianUnweighted() const;
Matrix augmentedJacobianUnweighted() const; gtsam::Matrix augmentedJacobianUnweighted() const;
void transposeMultiplyAdd(double alpha, Vector e, gtsam::VectorValues& x) const; void transposeMultiplyAdd(double alpha, gtsam::Vector e, gtsam::VectorValues& x) const;
gtsam::JacobianFactor whiten() const; gtsam::JacobianFactor whiten() const;
pair<gtsam::GaussianConditional*, gtsam::JacobianFactor*> eliminate(const gtsam::Ordering& keys) const; pair<gtsam::GaussianConditional*, gtsam::JacobianFactor*> eliminate(const gtsam::Ordering& keys) const;
void setModel(bool anyConstrained, Vector sigmas); void setModel(bool anyConstrained, gtsam::Vector sigmas);
gtsam::noiseModel::Diagonal* get_model() const; gtsam::noiseModel::Diagonal* get_model() const;
@ -382,12 +382,12 @@ virtual class HessianFactor : gtsam::GaussianFactor {
//Constructors //Constructors
HessianFactor(); HessianFactor();
HessianFactor(const gtsam::GaussianFactor& factor); HessianFactor(const gtsam::GaussianFactor& factor);
HessianFactor(size_t j, Matrix G, Vector g, double f); HessianFactor(size_t j, gtsam::Matrix G, gtsam::Vector g, double f);
HessianFactor(size_t j, Vector mu, Matrix Sigma); HessianFactor(size_t j, gtsam::Vector mu, gtsam::Matrix Sigma);
HessianFactor(size_t j1, size_t j2, Matrix G11, Matrix G12, Vector g1, Matrix G22, HessianFactor(size_t j1, size_t j2, gtsam::Matrix G11, gtsam::Matrix G12, gtsam::Vector g1, gtsam::Matrix G22,
Vector g2, double f); gtsam::Vector g2, double f);
HessianFactor(size_t j1, size_t j2, size_t j3, Matrix G11, Matrix G12, Matrix G13, HessianFactor(size_t j1, size_t j2, size_t j3, gtsam::Matrix G11, gtsam::Matrix G12, gtsam::Matrix G13,
Vector g1, Matrix G22, Matrix G23, Vector g2, Matrix G33, Vector g3, gtsam::Vector g1, gtsam::Matrix G22, gtsam::Matrix G23, gtsam::Vector g2, gtsam::Matrix G33, gtsam::Vector g3,
double f); double f);
HessianFactor(const gtsam::GaussianFactorGraph& factors); HessianFactor(const gtsam::GaussianFactorGraph& factors);
@ -399,9 +399,9 @@ virtual class HessianFactor : gtsam::GaussianFactor {
//Standard Interface //Standard Interface
size_t rows() const; size_t rows() const;
Matrix information() const; gtsam::Matrix information() const;
double constantTerm() const; double constantTerm() const;
Vector linearTerm() const; gtsam::Vector linearTerm() const;
// enabling serialization functionality // enabling serialization functionality
void serialize() const; void serialize() const;
@ -430,12 +430,12 @@ class GaussianFactorGraph {
void push_back(const gtsam::GaussianBayesNet& bayesNet); void push_back(const gtsam::GaussianBayesNet& bayesNet);
void push_back(const gtsam::GaussianBayesTree& bayesTree); void push_back(const gtsam::GaussianBayesTree& bayesTree);
void add(const gtsam::GaussianFactor& factor); void add(const gtsam::GaussianFactor& factor);
void add(Vector b); void add(gtsam::Vector b);
void add(size_t key1, Matrix A1, Vector b, const gtsam::noiseModel::Diagonal* model); void add(size_t key1, gtsam::Matrix A1, gtsam::Vector b, const gtsam::noiseModel::Diagonal* model);
void add(size_t key1, Matrix A1, size_t key2, Matrix A2, Vector b, void add(size_t key1, gtsam::Matrix A1, size_t key2, gtsam::Matrix A2, gtsam::Vector b,
const gtsam::noiseModel::Diagonal* model); const gtsam::noiseModel::Diagonal* model);
void add(size_t key1, Matrix A1, size_t key2, Matrix A2, size_t key3, Matrix A3, void add(size_t key1, gtsam::Matrix A1, size_t key2, gtsam::Matrix A2, size_t key3, gtsam::Matrix A3,
Vector b, const gtsam::noiseModel::Diagonal* model); gtsam::Vector b, const gtsam::noiseModel::Diagonal* model);
// error and probability // error and probability
double error(const gtsam::VectorValues& c) const; double error(const gtsam::VectorValues& c) const;
@ -477,15 +477,15 @@ class GaussianFactorGraph {
gtsam::GaussianFactorGraph* marginal(const gtsam::KeyVector& key_vector); gtsam::GaussianFactorGraph* marginal(const gtsam::KeyVector& key_vector);
// Conversion to matrices // Conversion to matrices
Matrix sparseJacobian_() const; gtsam::Matrix sparseJacobian_() const;
Matrix augmentedJacobian() const; gtsam::Matrix augmentedJacobian() const;
Matrix augmentedJacobian(const gtsam::Ordering& ordering) const; gtsam::Matrix augmentedJacobian(const gtsam::Ordering& ordering) const;
pair<Matrix,Vector> jacobian() const; pair<gtsam::Matrix,gtsam::Vector> jacobian() const;
pair<Matrix,Vector> jacobian(const gtsam::Ordering& ordering) const; pair<gtsam::Matrix,gtsam::Vector> jacobian(const gtsam::Ordering& ordering) const;
Matrix augmentedHessian() const; gtsam::Matrix augmentedHessian() const;
Matrix augmentedHessian(const gtsam::Ordering& ordering) const; gtsam::Matrix augmentedHessian(const gtsam::Ordering& ordering) const;
pair<Matrix,Vector> hessian() const; pair<gtsam::Matrix,gtsam::Vector> hessian() const;
pair<Matrix,Vector> hessian(const gtsam::Ordering& ordering) const; pair<gtsam::Matrix,gtsam::Vector> hessian(const gtsam::Ordering& ordering) const;
string dot( string dot(
const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter, const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter,
@ -503,37 +503,37 @@ class GaussianFactorGraph {
#include <gtsam/hybrid/HybridValues.h> #include <gtsam/hybrid/HybridValues.h>
virtual class GaussianConditional : gtsam::JacobianFactor { virtual class GaussianConditional : gtsam::JacobianFactor {
// Constructors // Constructors
GaussianConditional(size_t key, Vector d, Matrix R, GaussianConditional(size_t key, gtsam::Vector d, gtsam::Matrix R,
const gtsam::noiseModel::Diagonal* sigmas); const gtsam::noiseModel::Diagonal* sigmas);
GaussianConditional(size_t key, Vector d, Matrix R, size_t name1, Matrix S, GaussianConditional(size_t key, gtsam::Vector d, gtsam::Matrix R, size_t name1, gtsam::Matrix S,
const gtsam::noiseModel::Diagonal* sigmas); const gtsam::noiseModel::Diagonal* sigmas);
GaussianConditional(size_t key, Vector d, Matrix R, size_t name1, Matrix S, GaussianConditional(size_t key, gtsam::Vector d, gtsam::Matrix R, size_t name1, gtsam::Matrix S,
size_t name2, Matrix T, size_t name2, gtsam::Matrix T,
const gtsam::noiseModel::Diagonal* sigmas); const gtsam::noiseModel::Diagonal* sigmas);
// Constructors with no noise model // Constructors with no noise model
GaussianConditional(size_t key, Vector d, Matrix R); GaussianConditional(size_t key, gtsam::Vector d, gtsam::Matrix R);
GaussianConditional(size_t key, Vector d, Matrix R, size_t name1, Matrix S); GaussianConditional(size_t key, gtsam::Vector d, gtsam::Matrix R, size_t name1, gtsam::Matrix S);
GaussianConditional(size_t key, Vector d, Matrix R, size_t name1, Matrix S, GaussianConditional(size_t key, gtsam::Vector d, gtsam::Matrix R, size_t name1, gtsam::Matrix S,
size_t name2, Matrix T); size_t name2, gtsam::Matrix T);
// Named constructors // Named constructors
static gtsam::GaussianConditional FromMeanAndStddev(gtsam::Key key, static gtsam::GaussianConditional FromMeanAndStddev(gtsam::Key key,
const Vector& mu, const gtsam::Vector& mu,
double sigma); double sigma);
static gtsam::GaussianConditional FromMeanAndStddev(gtsam::Key key, static gtsam::GaussianConditional FromMeanAndStddev(gtsam::Key key,
const Matrix& A, const gtsam::Matrix& A,
gtsam::Key parent, gtsam::Key parent,
const Vector& b, const gtsam::Vector& b,
double sigma); double sigma);
static gtsam::GaussianConditional FromMeanAndStddev(gtsam::Key key, static gtsam::GaussianConditional FromMeanAndStddev(gtsam::Key key,
const Matrix& A1, const gtsam::Matrix& A1,
gtsam::Key parent1, gtsam::Key parent1,
const Matrix& A2, const gtsam::Matrix& A2,
gtsam::Key parent2, gtsam::Key parent2,
const Vector& b, const gtsam::Vector& b,
double sigma); double sigma);
// Testable // Testable
void print(string s = "GaussianConditional", void print(string s = "GaussianConditional",
@ -550,7 +550,7 @@ virtual class GaussianConditional : gtsam::JacobianFactor {
gtsam::VectorValues solve(const gtsam::VectorValues& parents) const; gtsam::VectorValues solve(const gtsam::VectorValues& parents) const;
gtsam::JacobianFactor* likelihood( gtsam::JacobianFactor* likelihood(
const gtsam::VectorValues& frontalValues) const; const gtsam::VectorValues& frontalValues) const;
gtsam::JacobianFactor* likelihood(Vector frontal) const; gtsam::JacobianFactor* likelihood(gtsam::Vector frontal) const;
gtsam::VectorValues sample(const gtsam::VectorValues& parents) const; gtsam::VectorValues sample(const gtsam::VectorValues& parents) const;
gtsam::VectorValues sample() const; gtsam::VectorValues sample() const;
@ -558,9 +558,9 @@ virtual class GaussianConditional : gtsam::JacobianFactor {
gtsam::VectorValues solveOtherRHS(const gtsam::VectorValues& parents, gtsam::VectorValues solveOtherRHS(const gtsam::VectorValues& parents,
const gtsam::VectorValues& rhs) const; const gtsam::VectorValues& rhs) const;
void solveTransposeInPlace(gtsam::VectorValues& gy) const; void solveTransposeInPlace(gtsam::VectorValues& gy) const;
Matrix R() const; gtsam::Matrix R() const;
Matrix S() const; gtsam::Matrix S() const;
Vector d() const; gtsam::Vector d() const;
// enabling serialization functionality // enabling serialization functionality
void serialize() const; void serialize() const;
@ -574,11 +574,11 @@ virtual class GaussianConditional : gtsam::JacobianFactor {
#include <gtsam/linear/GaussianDensity.h> #include <gtsam/linear/GaussianDensity.h>
virtual class GaussianDensity : gtsam::GaussianConditional { virtual class GaussianDensity : gtsam::GaussianConditional {
// Constructors // Constructors
GaussianDensity(gtsam::Key key, Vector d, Matrix R, GaussianDensity(gtsam::Key key, gtsam::Vector d, gtsam::Matrix R,
const gtsam::noiseModel::Diagonal* sigmas); const gtsam::noiseModel::Diagonal* sigmas);
static gtsam::GaussianDensity FromMeanAndStddev(gtsam::Key key, static gtsam::GaussianDensity FromMeanAndStddev(gtsam::Key key,
const Vector& mean, const gtsam::Vector& mean,
double sigma); double sigma);
// Testable // Testable
@ -588,8 +588,8 @@ virtual class GaussianDensity : gtsam::GaussianConditional {
bool equals(const gtsam::GaussianDensity& cg, double tol) const; bool equals(const gtsam::GaussianDensity& cg, double tol) const;
// Standard Interface // Standard Interface
Vector mean() const; gtsam::Vector mean() const;
Matrix covariance() const; gtsam::Matrix covariance() const;
}; };
#include <gtsam/linear/GaussianBayesNet.h> #include <gtsam/linear/GaussianBayesNet.h>
@ -632,7 +632,7 @@ virtual class GaussianBayesNet {
void saveGraph(const string& s) const; void saveGraph(const string& s) const;
std::pair<Matrix, Vector> matrix() const; std::pair<gtsam::Matrix, gtsam::Vector> matrix() const;
gtsam::VectorValues gradient(const gtsam::VectorValues& x0) const; gtsam::VectorValues gradient(const gtsam::VectorValues& x0) const;
gtsam::VectorValues gradientAtZero() const; gtsam::VectorValues gradientAtZero() const;
double error(const gtsam::VectorValues& x) const; double error(const gtsam::VectorValues& x) const;
@ -673,7 +673,7 @@ virtual class GaussianBayesTree {
double error(const gtsam::VectorValues& x) const; double error(const gtsam::VectorValues& x) const;
double determinant() const; double determinant() const;
double logDeterminant() const; double logDeterminant() const;
Matrix marginalCovariance(size_t key) const; gtsam::Matrix marginalCovariance(size_t key) const;
gtsam::GaussianConditional* marginalFactor(size_t key) const; gtsam::GaussianConditional* marginalFactor(size_t key) const;
gtsam::GaussianFactorGraph* joint(size_t key1, size_t key2) const; gtsam::GaussianFactorGraph* joint(size_t key1, size_t key2) const;
gtsam::GaussianBayesNet* jointBayesNet(size_t key1, size_t key2) const; gtsam::GaussianBayesNet* jointBayesNet(size_t key1, size_t key2) const;
@ -751,20 +751,20 @@ virtual class SubgraphSolver {
#include <gtsam/linear/KalmanFilter.h> #include <gtsam/linear/KalmanFilter.h>
class KalmanFilter { class KalmanFilter {
KalmanFilter(size_t n); KalmanFilter(size_t n);
// gtsam::GaussianDensity* init(Vector x0, const gtsam::SharedDiagonal& P0); // gtsam::GaussianDensity* init(gtsam::Vector x0, const gtsam::SharedDiagonal& P0);
gtsam::GaussianDensity* init(Vector x0, Matrix P0); gtsam::GaussianDensity* init(gtsam::Vector x0, gtsam::Matrix P0);
void print(string s = "") const; void print(string s = "") const;
static size_t step(gtsam::GaussianDensity* p); static size_t step(gtsam::GaussianDensity* p);
gtsam::GaussianDensity* predict(gtsam::GaussianDensity* p, Matrix F, gtsam::GaussianDensity* predict(gtsam::GaussianDensity* p, gtsam::Matrix F,
Matrix B, Vector u, const gtsam::noiseModel::Diagonal* modelQ); gtsam::Matrix B, gtsam::Vector u, const gtsam::noiseModel::Diagonal* modelQ);
gtsam::GaussianDensity* predictQ(gtsam::GaussianDensity* p, Matrix F, gtsam::GaussianDensity* predictQ(gtsam::GaussianDensity* p, gtsam::Matrix F,
Matrix B, Vector u, Matrix Q); gtsam::Matrix B, gtsam::Vector u, gtsam::Matrix Q);
gtsam::GaussianDensity* predict2(gtsam::GaussianDensity* p, Matrix A0, gtsam::GaussianDensity* predict2(gtsam::GaussianDensity* p, gtsam::Matrix A0,
Matrix A1, Vector b, const gtsam::noiseModel::Diagonal* model); gtsam::Matrix A1, gtsam::Vector b, const gtsam::noiseModel::Diagonal* model);
gtsam::GaussianDensity* update(gtsam::GaussianDensity* p, Matrix H, gtsam::GaussianDensity* update(gtsam::GaussianDensity* p, gtsam::Matrix H,
Vector z, const gtsam::noiseModel::Diagonal* model); gtsam::Vector z, const gtsam::noiseModel::Diagonal* model);
gtsam::GaussianDensity* updateQ(gtsam::GaussianDensity* p, Matrix H, gtsam::GaussianDensity* updateQ(gtsam::GaussianDensity* p, gtsam::Matrix H,
Vector z, Matrix Q); gtsam::Vector z, gtsam::Matrix Q);
}; };
} }

140
gtsam/navigation/navigation.i
View File

@ -10,7 +10,7 @@ namespace imuBias {
class ConstantBias { class ConstantBias {
// Constructors // Constructors
ConstantBias(); ConstantBias();
ConstantBias(Vector biasAcc, Vector biasGyro); ConstantBias(gtsam::Vector biasAcc, gtsam::Vector biasGyro);
// Testable // Testable
void print(string s = "") const; void print(string s = "") const;
@ -25,11 +25,11 @@ class ConstantBias {
gtsam::imuBias::ConstantBias operator-(const gtsam::imuBias::ConstantBias& b) const; gtsam::imuBias::ConstantBias operator-(const gtsam::imuBias::ConstantBias& b) const;
// Standard Interface // Standard Interface
Vector vector() const; gtsam::Vector vector() const;
Vector accelerometer() const; gtsam::Vector accelerometer() const;
Vector gyroscope() const; gtsam::Vector gyroscope() const;
Vector correctAccelerometer(Vector measurement) const; gtsam::Vector correctAccelerometer(gtsam::Vector measurement) const;
Vector correctGyroscope(Vector measurement) const; gtsam::Vector correctGyroscope(gtsam::Vector measurement) const;
// enabling serialization functionality // enabling serialization functionality
void serialize() const; void serialize() const;
@ -41,8 +41,8 @@ class ConstantBias {
class NavState { class NavState {
// Constructors // Constructors
NavState(); NavState();
NavState(const gtsam::Rot3& R, const gtsam::Point3& t, Vector v); NavState(const gtsam::Rot3& R, const gtsam::Point3& t, gtsam::Vector v);
NavState(const gtsam::Pose3& pose, Vector v); NavState(const gtsam::Pose3& pose, gtsam::Vector v);
// Testable // Testable
void print(string s = "") const; void print(string s = "") const;
@ -51,11 +51,11 @@ class NavState {
// Access // Access
gtsam::Rot3 attitude() const; gtsam::Rot3 attitude() const;
gtsam::Point3 position() const; gtsam::Point3 position() const;
Vector velocity() const; gtsam::Vector velocity() const;
gtsam::Pose3 pose() const; gtsam::Pose3 pose() const;
gtsam::NavState retract(const Vector& x) const; gtsam::NavState retract(const gtsam::Vector& x) const;
Vector localCoordinates(const gtsam::NavState& g) const; gtsam::Vector localCoordinates(const gtsam::NavState& g) const;
// enabling serialization functionality // enabling serialization functionality
void serialize() const; void serialize() const;
@ -69,19 +69,19 @@ virtual class PreintegratedRotationParams {
void print(string s = "") const; void print(string s = "") const;
bool equals(const gtsam::PreintegratedRotationParams& expected, double tol); bool equals(const gtsam::PreintegratedRotationParams& expected, double tol);
void setGyroscopeCovariance(Matrix cov); void setGyroscopeCovariance(gtsam::Matrix cov);
void setOmegaCoriolis(Vector omega); void setOmegaCoriolis(gtsam::Vector omega);
void setBodyPSensor(const gtsam::Pose3& pose); void setBodyPSensor(const gtsam::Pose3& pose);
Matrix getGyroscopeCovariance() const; gtsam::Matrix getGyroscopeCovariance() const;
std::optional<Vector> getOmegaCoriolis() const; std::optional<gtsam::Vector> getOmegaCoriolis() const;
std::optional<gtsam::Pose3> getBodyPSensor() const; std::optional<gtsam::Pose3> getBodyPSensor() const;
}; };
#include <gtsam/navigation/PreintegrationParams.h> #include <gtsam/navigation/PreintegrationParams.h>
virtual class PreintegrationParams : gtsam::PreintegratedRotationParams { virtual class PreintegrationParams : gtsam::PreintegratedRotationParams {
PreintegrationParams(Vector n_gravity); PreintegrationParams(gtsam::Vector n_gravity);
gtsam::Vector n_gravity; gtsam::Vector n_gravity;
@ -94,12 +94,12 @@ virtual class PreintegrationParams : gtsam::PreintegratedRotationParams {
void print(string s = "") const; void print(string s = "") const;
bool equals(const gtsam::PreintegrationParams& expected, double tol); bool equals(const gtsam::PreintegrationParams& expected, double tol);
void setAccelerometerCovariance(Matrix cov); void setAccelerometerCovariance(gtsam::Matrix cov);
void setIntegrationCovariance(Matrix cov); void setIntegrationCovariance(gtsam::Matrix cov);
void setUse2ndOrderCoriolis(bool flag); void setUse2ndOrderCoriolis(bool flag);
Matrix getAccelerometerCovariance() const; gtsam::Matrix getAccelerometerCovariance() const;
Matrix getIntegrationCovariance() const; gtsam::Matrix getIntegrationCovariance() const;
bool getUse2ndOrderCoriolis() const; bool getUse2ndOrderCoriolis() const;
// enabling serialization functionality // enabling serialization functionality
@ -118,19 +118,19 @@ class PreintegratedImuMeasurements {
bool equals(const gtsam::PreintegratedImuMeasurements& expected, double tol); bool equals(const gtsam::PreintegratedImuMeasurements& expected, double tol);
// Standard Interface // Standard Interface
void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, void integrateMeasurement(gtsam::Vector measuredAcc, gtsam::Vector measuredOmega,
double deltaT); double deltaT);
void resetIntegration(); void resetIntegration();
void resetIntegrationAndSetBias(const gtsam::imuBias::ConstantBias& biasHat); void resetIntegrationAndSetBias(const gtsam::imuBias::ConstantBias& biasHat);
Matrix preintMeasCov() const; gtsam::Matrix preintMeasCov() const;
Vector preintegrated() const; gtsam::Vector preintegrated() const;
double deltaTij() const; double deltaTij() const;
gtsam::Rot3 deltaRij() const; gtsam::Rot3 deltaRij() const;
Vector deltaPij() const; gtsam::Vector deltaPij() const;
Vector deltaVij() const; gtsam::Vector deltaVij() const;
gtsam::imuBias::ConstantBias biasHat() const; gtsam::imuBias::ConstantBias biasHat() const;
Vector biasHatVector() const; gtsam::Vector biasHatVector() const;
gtsam::NavState predict(const gtsam::NavState& state_i, gtsam::NavState predict(const gtsam::NavState& state_i,
const gtsam::imuBias::ConstantBias& bias) const; const gtsam::imuBias::ConstantBias& bias) const;
@ -145,14 +145,14 @@ virtual class ImuFactor: gtsam::NonlinearFactor {
// Standard Interface // Standard Interface
gtsam::PreintegratedImuMeasurements preintegratedMeasurements() const; gtsam::PreintegratedImuMeasurements preintegratedMeasurements() const;
Vector evaluateError(const gtsam::Pose3& pose_i, Vector vel_i, gtsam::Vector evaluateError(const gtsam::Pose3& pose_i, gtsam::Vector vel_i,
const gtsam::Pose3& pose_j, Vector vel_j, const gtsam::Pose3& pose_j, gtsam::Vector vel_j,
const gtsam::imuBias::ConstantBias& bias); const gtsam::imuBias::ConstantBias& bias);
}; };
#include <gtsam/navigation/CombinedImuFactor.h> #include <gtsam/navigation/CombinedImuFactor.h>
virtual class PreintegrationCombinedParams : gtsam::PreintegrationParams { virtual class PreintegrationCombinedParams : gtsam::PreintegrationParams {
PreintegrationCombinedParams(Vector n_gravity); PreintegrationCombinedParams(gtsam::Vector n_gravity);
static gtsam::PreintegrationCombinedParams* MakeSharedD(double g); static gtsam::PreintegrationCombinedParams* MakeSharedD(double g);
static gtsam::PreintegrationCombinedParams* MakeSharedU(double g); static gtsam::PreintegrationCombinedParams* MakeSharedU(double g);
@ -163,13 +163,13 @@ virtual class PreintegrationCombinedParams : gtsam::PreintegrationParams {
void print(string s = "") const; void print(string s = "") const;
bool equals(const gtsam::PreintegrationCombinedParams& expected, double tol); bool equals(const gtsam::PreintegrationCombinedParams& expected, double tol);
void setBiasAccCovariance(Matrix cov); void setBiasAccCovariance(gtsam::Matrix cov);
void setBiasOmegaCovariance(Matrix cov); void setBiasOmegaCovariance(gtsam::Matrix cov);
void setBiasAccOmegaInit(Matrix cov); void setBiasAccOmegaInit(gtsam::Matrix cov);
Matrix getBiasAccCovariance() const ; gtsam::Matrix getBiasAccCovariance() const ;
Matrix getBiasOmegaCovariance() const ; gtsam::Matrix getBiasOmegaCovariance() const ;
Matrix getBiasAccOmegaInit() const; gtsam::Matrix getBiasAccOmegaInit() const;
}; };
@ -184,18 +184,18 @@ class PreintegratedCombinedMeasurements {
double tol); double tol);
// Standard Interface // Standard Interface
void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, void integrateMeasurement(gtsam::Vector measuredAcc, gtsam::Vector measuredOmega,
double deltaT); double deltaT);
void resetIntegration(); void resetIntegration();
void resetIntegrationAndSetBias(const gtsam::imuBias::ConstantBias& biasHat); void resetIntegrationAndSetBias(const gtsam::imuBias::ConstantBias& biasHat);
Matrix preintMeasCov() const; gtsam::Matrix preintMeasCov() const;
double deltaTij() const; double deltaTij() const;
gtsam::Rot3 deltaRij() const; gtsam::Rot3 deltaRij() const;
Vector deltaPij() const; gtsam::Vector deltaPij() const;
Vector deltaVij() const; gtsam::Vector deltaVij() const;
gtsam::imuBias::ConstantBias biasHat() const; gtsam::imuBias::ConstantBias biasHat() const;
Vector biasHatVector() const; gtsam::Vector biasHatVector() const;
gtsam::NavState predict(const gtsam::NavState& state_i, gtsam::NavState predict(const gtsam::NavState& state_i,
const gtsam::imuBias::ConstantBias& bias) const; const gtsam::imuBias::ConstantBias& bias) const;
}; };
@ -207,8 +207,8 @@ virtual class CombinedImuFactor: gtsam::NoiseModelFactor {
// Standard Interface // Standard Interface
gtsam::PreintegratedCombinedMeasurements preintegratedMeasurements() const; gtsam::PreintegratedCombinedMeasurements preintegratedMeasurements() const;
Vector evaluateError(const gtsam::Pose3& pose_i, Vector vel_i, gtsam::Vector evaluateError(const gtsam::Pose3& pose_i, gtsam::Vector vel_i,
const gtsam::Pose3& pose_j, Vector vel_j, const gtsam::Pose3& pose_j, gtsam::Vector vel_j,
const gtsam::imuBias::ConstantBias& bias_i, const gtsam::imuBias::ConstantBias& bias_i,
const gtsam::imuBias::ConstantBias& bias_j); const gtsam::imuBias::ConstantBias& bias_j);
}; };
@ -217,12 +217,12 @@ virtual class CombinedImuFactor: gtsam::NoiseModelFactor {
class PreintegratedAhrsMeasurements { class PreintegratedAhrsMeasurements {
// Standard Constructor // Standard Constructor
PreintegratedAhrsMeasurements(const gtsam::PreintegrationParams* params, PreintegratedAhrsMeasurements(const gtsam::PreintegrationParams* params,
const Vector& biasHat); const gtsam::Vector& biasHat);
PreintegratedAhrsMeasurements(const gtsam::PreintegrationParams* p, PreintegratedAhrsMeasurements(const gtsam::PreintegrationParams* p,
const Vector& bias_hat, double deltaTij, const gtsam::Vector& bias_hat, double deltaTij,
const gtsam::Rot3& deltaRij, const gtsam::Rot3& deltaRij,
const Matrix& delRdelBiasOmega, const gtsam::Matrix& delRdelBiasOmega,
const Matrix& preint_meas_cov); const gtsam::Matrix& preint_meas_cov);
PreintegratedAhrsMeasurements(const gtsam::PreintegratedAhrsMeasurements& rhs); PreintegratedAhrsMeasurements(const gtsam::PreintegratedAhrsMeasurements& rhs);
// Testable // Testable
@ -232,27 +232,27 @@ class PreintegratedAhrsMeasurements {
// get Data // get Data
gtsam::Rot3 deltaRij() const; gtsam::Rot3 deltaRij() const;
double deltaTij() const; double deltaTij() const;
Vector biasHat() const; gtsam::Vector biasHat() const;
// Standard Interface // Standard Interface
void integrateMeasurement(Vector measuredOmega, double deltaT); void integrateMeasurement(gtsam::Vector measuredOmega, double deltaT);
void resetIntegration() ; void resetIntegration() ;
}; };
virtual class AHRSFactor : gtsam::NonlinearFactor { virtual class AHRSFactor : gtsam::NonlinearFactor {
AHRSFactor(size_t rot_i, size_t rot_j,size_t bias, AHRSFactor(size_t rot_i, size_t rot_j,size_t bias,
const gtsam::PreintegratedAhrsMeasurements& preintegratedMeasurements, Vector omegaCoriolis); const gtsam::PreintegratedAhrsMeasurements& preintegratedMeasurements, gtsam::Vector omegaCoriolis);
AHRSFactor(size_t rot_i, size_t rot_j, size_t bias, AHRSFactor(size_t rot_i, size_t rot_j, size_t bias,
const gtsam::PreintegratedAhrsMeasurements& preintegratedMeasurements, Vector omegaCoriolis, const gtsam::PreintegratedAhrsMeasurements& preintegratedMeasurements, gtsam::Vector omegaCoriolis,
const gtsam::Pose3& body_P_sensor); const gtsam::Pose3& body_P_sensor);
// Standard Interface // Standard Interface
gtsam::PreintegratedAhrsMeasurements preintegratedMeasurements() const; gtsam::PreintegratedAhrsMeasurements preintegratedMeasurements() const;
Vector evaluateError(const gtsam::Rot3& rot_i, const gtsam::Rot3& rot_j, gtsam::Vector evaluateError(const gtsam::Rot3& rot_i, const gtsam::Rot3& rot_j,
Vector bias) const; gtsam::Vector bias) const;
gtsam::Rot3 predict(const gtsam::Rot3& rot_i, Vector bias, gtsam::Rot3 predict(const gtsam::Rot3& rot_i, gtsam::Vector bias,
const gtsam::PreintegratedAhrsMeasurements& preintegratedMeasurements, const gtsam::PreintegratedAhrsMeasurements& preintegratedMeasurements,
Vector omegaCoriolis) const; gtsam::Vector omegaCoriolis) const;
}; };
#include <gtsam/navigation/AttitudeFactor.h> #include <gtsam/navigation/AttitudeFactor.h>
@ -333,25 +333,25 @@ virtual class BarometricFactor : gtsam::NonlinearFactor {
#include <gtsam/navigation/Scenario.h> #include <gtsam/navigation/Scenario.h>
virtual class Scenario { virtual class Scenario {
gtsam::Pose3 pose(double t) const; gtsam::Pose3 pose(double t) const;
Vector omega_b(double t) const; gtsam::Vector omega_b(double t) const;
Vector velocity_n(double t) const; gtsam::Vector velocity_n(double t) const;
Vector acceleration_n(double t) const; gtsam::Vector acceleration_n(double t) const;
gtsam::Rot3 rotation(double t) const; gtsam::Rot3 rotation(double t) const;
gtsam::NavState navState(double t) const; gtsam::NavState navState(double t) const;
Vector velocity_b(double t) const; gtsam::Vector velocity_b(double t) const;
Vector acceleration_b(double t) const; gtsam::Vector acceleration_b(double t) const;
}; };
virtual class ConstantTwistScenario : gtsam::Scenario { virtual class ConstantTwistScenario : gtsam::Scenario {
ConstantTwistScenario(Vector w, Vector v); ConstantTwistScenario(gtsam::Vector w, gtsam::Vector v);
ConstantTwistScenario(Vector w, Vector v, ConstantTwistScenario(gtsam::Vector w, gtsam::Vector v,
const gtsam::Pose3& nTb0); const gtsam::Pose3& nTb0);
}; };
virtual class AcceleratingScenario : gtsam::Scenario { virtual class AcceleratingScenario : gtsam::Scenario {
AcceleratingScenario(const gtsam::Rot3& nRb, const gtsam::Point3& p0, AcceleratingScenario(const gtsam::Rot3& nRb, const gtsam::Point3& p0,
Vector v0, Vector a_n, gtsam::Vector v0, gtsam::Vector a_n,
Vector omega_b); gtsam::Vector omega_b);
}; };
#include <gtsam/navigation/ScenarioRunner.h> #include <gtsam/navigation/ScenarioRunner.h>
@ -360,11 +360,11 @@ class ScenarioRunner {
const gtsam::PreintegrationParams* p, const gtsam::PreintegrationParams* p,
double imuSampleTime, double imuSampleTime,
const gtsam::imuBias::ConstantBias& bias); const gtsam::imuBias::ConstantBias& bias);
Vector gravity_n() const; gtsam::Vector gravity_n() const;
Vector actualAngularVelocity(double t) const; gtsam::Vector actualAngularVelocity(double t) const;
Vector actualSpecificForce(double t) const; gtsam::Vector actualSpecificForce(double t) const;
Vector measuredAngularVelocity(double t) const; gtsam::Vector measuredAngularVelocity(double t) const;
Vector measuredSpecificForce(double t) const; gtsam::Vector measuredSpecificForce(double t) const;
double imuSampleTime() const; double imuSampleTime() const;
gtsam::PreintegratedImuMeasurements integrate( gtsam::PreintegratedImuMeasurements integrate(
double T, const gtsam::imuBias::ConstantBias& estimatedBias, double T, const gtsam::imuBias::ConstantBias& estimatedBias,
@ -372,10 +372,10 @@ class ScenarioRunner {
gtsam::NavState predict( gtsam::NavState predict(
const gtsam::PreintegratedImuMeasurements& pim, const gtsam::PreintegratedImuMeasurements& pim,
const gtsam::imuBias::ConstantBias& estimatedBias) const; const gtsam::imuBias::ConstantBias& estimatedBias) const;
Matrix estimateCovariance( gtsam::Matrix estimateCovariance(
double T, size_t N, double T, size_t N,
const gtsam::imuBias::ConstantBias& estimatedBias) const; const gtsam::imuBias::ConstantBias& estimatedBias) const;
Matrix estimateNoiseCovariance(size_t N) const; gtsam::Matrix estimateNoiseCovariance(size_t N) const;
}; };
} }

34
gtsam/nonlinear/nonlinear.i
View File

@ -63,7 +63,7 @@ class NonlinearFactorGraph {
gtsam::KeyVector keyVector() const; gtsam::KeyVector keyVector() const;
template <T = {double, template <T = {double,
Vector, gtsam::Vector,
gtsam::Point2, gtsam::Point2,
gtsam::StereoPoint2, gtsam::StereoPoint2,
gtsam::Point3, gtsam::Point3,
@ -131,8 +131,8 @@ virtual class NoiseModelFactor : gtsam::NonlinearFactor {
bool equals(const gtsam::NoiseModelFactor& other, double tol) const; bool equals(const gtsam::NoiseModelFactor& other, double tol) const;
gtsam::noiseModel::Base* noiseModel() const; gtsam::noiseModel::Base* noiseModel() const;
gtsam::NoiseModelFactor* cloneWithNewNoiseModel(gtsam::noiseModel::Base* newNoise) const; gtsam::NoiseModelFactor* cloneWithNewNoiseModel(gtsam::noiseModel::Base* newNoise) const;
Vector unwhitenedError(const gtsam::Values& x) const; gtsam::Vector unwhitenedError(const gtsam::Values& x) const;
Vector whitenedError(const gtsam::Values& x) const; gtsam::Vector whitenedError(const gtsam::Values& x) const;
}; };
#include <gtsam/nonlinear/Marginals.h> #include <gtsam/nonlinear/Marginals.h>
@ -146,8 +146,8 @@ class Marginals {
void print(string s = "Marginals: ", const gtsam::KeyFormatter& keyFormatter = void print(string s = "Marginals: ", const gtsam::KeyFormatter& keyFormatter =
gtsam::DefaultKeyFormatter) const; gtsam::DefaultKeyFormatter) const;
Matrix marginalCovariance(size_t variable) const; gtsam::Matrix marginalCovariance(size_t variable) const;
Matrix marginalInformation(size_t variable) const; gtsam::Matrix marginalInformation(size_t variable) const;
gtsam::JointMarginal jointMarginalCovariance( gtsam::JointMarginal jointMarginalCovariance(
const gtsam::KeyVector& variables) const; const gtsam::KeyVector& variables) const;
gtsam::JointMarginal jointMarginalInformation( gtsam::JointMarginal jointMarginalInformation(
@ -155,8 +155,8 @@ class Marginals {
}; };
class JointMarginal { class JointMarginal {
Matrix at(size_t iVariable, size_t jVariable) const; gtsam::Matrix at(size_t iVariable, size_t jVariable) const;
Matrix fullMatrix() const; gtsam::Matrix fullMatrix() const;
void print(string s = "", gtsam::KeyFormatter keyFormatter = void print(string s = "", gtsam::KeyFormatter keyFormatter =
gtsam::DefaultKeyFormatter) const; gtsam::DefaultKeyFormatter) const;
}; };
@ -368,10 +368,10 @@ virtual class GncOptimizer {
const gtsam::Values& initialValues, const gtsam::Values& initialValues,
const PARAMS& params); const PARAMS& params);
void setInlierCostThresholds(const double inth); void setInlierCostThresholds(const double inth);
const Vector& getInlierCostThresholds(); const gtsam::Vector& getInlierCostThresholds();
void setInlierCostThresholdsAtProbability(const double alpha); void setInlierCostThresholdsAtProbability(const double alpha);
void setWeights(const Vector w); void setWeights(const gtsam::Vector w);
const Vector& getWeights(); const gtsam::Vector& getWeights();
gtsam::Values optimize(); gtsam::Values optimize();
}; };
@ -417,7 +417,7 @@ class ISAM2DoglegParams {
}; };
class ISAM2ThresholdMapValue { class ISAM2ThresholdMapValue {
ISAM2ThresholdMapValue(char c, Vector thresholds); ISAM2ThresholdMapValue(char c, gtsam::Vector thresholds);
ISAM2ThresholdMapValue(const gtsam::ISAM2ThresholdMapValue& other); ISAM2ThresholdMapValue(const gtsam::ISAM2ThresholdMapValue& other);
}; };
@ -467,7 +467,7 @@ class ISAM2Clique {
ISAM2Clique(); ISAM2Clique();
// Standard Interface // Standard Interface
Vector gradientContribution() const; gtsam::Vector gradientContribution() const;
void print(string s = "", void print(string s = "",
gtsam::KeyFormatter keyFormatter = gtsam::DefaultKeyFormatter); gtsam::KeyFormatter keyFormatter = gtsam::DefaultKeyFormatter);
}; };
@ -535,9 +535,9 @@ class ISAM2 {
gtsam::PinholeCamera<gtsam::Cal3_S2>, gtsam::PinholeCamera<gtsam::Cal3_S2>,
gtsam::PinholeCamera<gtsam::Cal3Bundler>, gtsam::PinholeCamera<gtsam::Cal3Bundler>,
gtsam::PinholeCamera<gtsam::Cal3Fisheye>, gtsam::PinholeCamera<gtsam::Cal3Fisheye>,
gtsam::PinholeCamera<gtsam::Cal3Unified>, Vector, Matrix}> gtsam::PinholeCamera<gtsam::Cal3Unified>, gtsam::Vector, gtsam::Matrix}>
VALUE calculateEstimate(size_t key) const; VALUE calculateEstimate(size_t key) const;
Matrix marginalCovariance(size_t key) const; gtsam::Matrix marginalCovariance(size_t key) const;
gtsam::Values calculateBestEstimate() const; gtsam::Values calculateBestEstimate() const;
gtsam::VectorValues getDelta() const; gtsam::VectorValues getDelta() const;
double error(const gtsam::VectorValues& x) const; double error(const gtsam::VectorValues& x) const;
@ -564,7 +564,7 @@ class NonlinearISAM {
void printStats() const; void printStats() const;
void saveGraph(string s) const; void saveGraph(string s) const;
gtsam::Values estimate() const; gtsam::Values estimate() const;
Matrix marginalCovariance(size_t key) const; gtsam::Matrix marginalCovariance(size_t key) const;
int reorderInterval() const; int reorderInterval() const;
int reorderCounter() const; int reorderCounter() const;
void update(const gtsam::NonlinearFactorGraph& newFactors, void update(const gtsam::NonlinearFactorGraph& newFactors,
@ -583,7 +583,7 @@ class NonlinearISAM {
//************************************************************************* //*************************************************************************
#include <gtsam/nonlinear/PriorFactor.h> #include <gtsam/nonlinear/PriorFactor.h>
template <T = {double, template <T = {double,
Vector, gtsam::Vector,
gtsam::Point2, gtsam::Point2,
gtsam::StereoPoint2, gtsam::StereoPoint2,
gtsam::Point3, gtsam::Point3,
@ -703,7 +703,7 @@ virtual class BatchFixedLagSmoother : gtsam::FixedLagSmoother {
gtsam::LevenbergMarquardtParams params() const; gtsam::LevenbergMarquardtParams params() const;
template <VALUE = {gtsam::Point2, gtsam::Rot2, gtsam::Pose2, gtsam::Point3, template <VALUE = {gtsam::Point2, gtsam::Rot2, gtsam::Pose2, gtsam::Point3,
gtsam::Rot3, gtsam::Pose3, gtsam::Cal3_S2, gtsam::Cal3DS2, gtsam::Rot3, gtsam::Pose3, gtsam::Cal3_S2, gtsam::Cal3DS2,
Vector, Matrix}> gtsam::Vector, gtsam::Matrix}>
VALUE calculateEstimate(size_t key) const; VALUE calculateEstimate(size_t key) const;
}; };

22
gtsam/nonlinear/values.i
View File

@ -66,10 +66,10 @@ class Values {
// void update(size_t j, const gtsam::Value& val); // void update(size_t j, const gtsam::Value& val);
// gtsam::Value at(size_t j) const; // gtsam::Value at(size_t j) const;
// The order is important: Vector has to precede Point2/Point3 so `atVector` // The order is important: gtsam::Vector has to precede Point2/Point3 so `atVector`
// can work for those fixed-size vectors. // can work for those fixed-size vectors.
void insert(size_t j, Vector vector); void insert(size_t j, gtsam::Vector vector);
void insert(size_t j, Matrix matrix); void insert(size_t j, gtsam::Matrix matrix);
void insert(size_t j, const gtsam::Point2& point2); void insert(size_t j, const gtsam::Point2& point2);
void insert(size_t j, const gtsam::Point3& point3); void insert(size_t j, const gtsam::Point3& point3);
void insert(size_t j, const gtsam::Rot2& rot2); void insert(size_t j, const gtsam::Rot2& rot2);
@ -101,10 +101,10 @@ class Values {
template <T = {gtsam::Point2, gtsam::Point3}> template <T = {gtsam::Point2, gtsam::Point3}>
void insert(size_t j, const T& val); void insert(size_t j, const T& val);
// The order is important: Vector has to precede Point2/Point3 so `atVector` // The order is important: gtsam::Vector has to precede Point2/Point3 so `atVector`
// can work for those fixed-size vectors. // can work for those fixed-size vectors.
void update(size_t j, Vector vector); void update(size_t j, gtsam::Vector vector);
void update(size_t j, Matrix matrix); void update(size_t j, gtsam::Matrix matrix);
void update(size_t j, const gtsam::Point2& point2); void update(size_t j, const gtsam::Point2& point2);
void update(size_t j, const gtsam::Point3& point3); void update(size_t j, const gtsam::Point3& point3);
void update(size_t j, const gtsam::Rot2& rot2); void update(size_t j, const gtsam::Rot2& rot2);
@ -133,10 +133,10 @@ class Values {
void update(size_t j, const gtsam::NavState& nav_state); void update(size_t j, const gtsam::NavState& nav_state);
void update(size_t j, double c); void update(size_t j, double c);
// The order is important: Vector has to precede Point2/Point3 so `atVector` // The order is important: gtsam::Vector has to precede Point2/Point3 so `atVector`
// can work for those fixed-size vectors. // can work for those fixed-size vectors.
void insert_or_assign(size_t j, Vector vector); void insert_or_assign(size_t j, gtsam::Vector vector);
void insert_or_assign(size_t j, Matrix matrix); void insert_or_assign(size_t j, gtsam::Matrix matrix);
void insert_or_assign(size_t j, const gtsam::Point2& point2); void insert_or_assign(size_t j, const gtsam::Point2& point2);
void insert_or_assign(size_t j, const gtsam::Point3& point3); void insert_or_assign(size_t j, const gtsam::Point3& point3);
void insert_or_assign(size_t j, const gtsam::Rot2& rot2); void insert_or_assign(size_t j, const gtsam::Rot2& rot2);
@ -201,8 +201,8 @@ class Values {
gtsam::PinholePose<gtsam::Cal3Unified>, gtsam::PinholePose<gtsam::Cal3Unified>,
gtsam::imuBias::ConstantBias, gtsam::imuBias::ConstantBias,
gtsam::NavState, gtsam::NavState,
Vector, gtsam::Vector,
Matrix, gtsam::Matrix,
double}> double}>
T at(size_t j); T at(size_t j);
}; };

38
gtsam/sfm/sfm.i
View File

@ -81,7 +81,7 @@ virtual class ShonanFactor3 : gtsam::NoiseModelFactor {
ShonanFactor3(size_t key1, size_t key2, const gtsam::Rot3& R12, size_t p); ShonanFactor3(size_t key1, size_t key2, const gtsam::Rot3& R12, size_t p);
ShonanFactor3(size_t key1, size_t key2, const gtsam::Rot3& R12, size_t p, ShonanFactor3(size_t key1, size_t key2, const gtsam::Rot3& R12, size_t p,
gtsam::noiseModel::Base* model); gtsam::noiseModel::Base* model);
Vector evaluateError(const gtsam::SOn& Q1, const gtsam::SOn& Q2); gtsam::Vector evaluateError(const gtsam::SOn& Q1, const gtsam::SOn& Q2);
}; };
#include <gtsam/sfm/BinaryMeasurement.h> #include <gtsam/sfm/BinaryMeasurement.h>
@ -162,23 +162,23 @@ class ShonanAveraging2 {
gtsam::Rot2 measured(size_t i); gtsam::Rot2 measured(size_t i);
gtsam::KeyVector keys(size_t i); gtsam::KeyVector keys(size_t i);
// Matrix API (advanced use, debugging) // gtsam::Matrix API (advanced use, debugging)
Matrix denseD() const; gtsam::Matrix denseD() const;
Matrix denseQ() const; gtsam::Matrix denseQ() const;
Matrix denseL() const; gtsam::Matrix denseL() const;
// Matrix computeLambda_(Matrix S) const; // gtsam::Matrix computeLambda_(gtsam::Matrix S) const;
Matrix computeLambda_(const gtsam::Values& values) const; gtsam::Matrix computeLambda_(const gtsam::Values& values) const;
Matrix computeA_(const gtsam::Values& values) const; gtsam::Matrix computeA_(const gtsam::Values& values) const;
double computeMinEigenValue(const gtsam::Values& values) const; double computeMinEigenValue(const gtsam::Values& values) const;
gtsam::Values initializeWithDescent(size_t p, const gtsam::Values& values, gtsam::Values initializeWithDescent(size_t p, const gtsam::Values& values,
const Vector& minEigenVector, const gtsam::Vector& minEigenVector,
double minEigenValue) const; double minEigenValue) const;
// Advanced API // Advanced API
gtsam::NonlinearFactorGraph buildGraphAt(size_t p) const; gtsam::NonlinearFactorGraph buildGraphAt(size_t p) const;
gtsam::Values initializeRandomlyAt(size_t p) const; gtsam::Values initializeRandomlyAt(size_t p) const;
double costAt(size_t p, const gtsam::Values& values) const; double costAt(size_t p, const gtsam::Values& values) const;
pair<double, Vector> computeMinEigenVector(const gtsam::Values& values) const; pair<double, gtsam::Vector> computeMinEigenVector(const gtsam::Values& values) const;
bool checkOptimality(const gtsam::Values& values) const; bool checkOptimality(const gtsam::Values& values) const;
gtsam::LevenbergMarquardtOptimizer* createOptimizerAt( gtsam::LevenbergMarquardtOptimizer* createOptimizerAt(
size_t p, const gtsam::Values& initial); size_t p, const gtsam::Values& initial);
@ -212,23 +212,23 @@ class ShonanAveraging3 {
gtsam::Rot3 measured(size_t i); gtsam::Rot3 measured(size_t i);
gtsam::KeyVector keys(size_t i); gtsam::KeyVector keys(size_t i);
// Matrix API (advanced use, debugging) // gtsam::Matrix API (advanced use, debugging)
Matrix denseD() const; gtsam::Matrix denseD() const;
Matrix denseQ() const; gtsam::Matrix denseQ() const;
Matrix denseL() const; gtsam::Matrix denseL() const;
// Matrix computeLambda_(Matrix S) const; // gtsam::Matrix computeLambda_(gtsam::Matrix S) const;
Matrix computeLambda_(const gtsam::Values& values) const; gtsam::Matrix computeLambda_(const gtsam::Values& values) const;
Matrix computeA_(const gtsam::Values& values) const; gtsam::Matrix computeA_(const gtsam::Values& values) const;
double computeMinEigenValue(const gtsam::Values& values) const; double computeMinEigenValue(const gtsam::Values& values) const;
gtsam::Values initializeWithDescent(size_t p, const gtsam::Values& values, gtsam::Values initializeWithDescent(size_t p, const gtsam::Values& values,
const Vector& minEigenVector, const gtsam::Vector& minEigenVector,
double minEigenValue) const; double minEigenValue) const;
// Advanced API // Advanced API
gtsam::NonlinearFactorGraph buildGraphAt(size_t p) const; gtsam::NonlinearFactorGraph buildGraphAt(size_t p) const;
gtsam::Values initializeRandomlyAt(size_t p) const; gtsam::Values initializeRandomlyAt(size_t p) const;
double costAt(size_t p, const gtsam::Values& values) const; double costAt(size_t p, const gtsam::Values& values) const;
pair<double, Vector> computeMinEigenVector(const gtsam::Values& values) const; pair<double, gtsam::Vector> computeMinEigenVector(const gtsam::Values& values) const;
bool checkOptimality(const gtsam::Values& values) const; bool checkOptimality(const gtsam::Values& values) const;
gtsam::LevenbergMarquardtOptimizer* createOptimizerAt( gtsam::LevenbergMarquardtOptimizer* createOptimizerAt(
size_t p, const gtsam::Values& initial); size_t p, const gtsam::Values& initial);

10
gtsam/slam/slam.i
View File

@ -12,7 +12,7 @@ namespace gtsam {
// ###### // ######
#include <gtsam/slam/BetweenFactor.h> #include <gtsam/slam/BetweenFactor.h>
template <T = {double, Vector, gtsam::Point2, gtsam::Point3, gtsam::Rot2, gtsam::SO3, template <T = {double, gtsam::Vector, gtsam::Point2, gtsam::Point3, gtsam::Rot2, gtsam::SO3,
gtsam::SO4, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::Similarity3, gtsam::SO4, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::Similarity3,
gtsam::imuBias::ConstantBias}> gtsam::imuBias::ConstantBias}>
virtual class BetweenFactor : gtsam::NoiseModelFactor { virtual class BetweenFactor : gtsam::NoiseModelFactor {
@ -227,7 +227,7 @@ virtual class EssentialMatrixFactor : gtsam::NoiseModelFactor {
void print(string s = "", const gtsam::KeyFormatter& keyFormatter = void print(string s = "", const gtsam::KeyFormatter& keyFormatter =
gtsam::DefaultKeyFormatter) const; gtsam::DefaultKeyFormatter) const;
bool equals(const gtsam::EssentialMatrixFactor& other, double tol) const; bool equals(const gtsam::EssentialMatrixFactor& other, double tol) const;
Vector evaluateError(const gtsam::EssentialMatrix& E) const; gtsam::Vector evaluateError(const gtsam::EssentialMatrix& E) const;
}; };
#include <gtsam/slam/EssentialMatrixConstraint.h> #include <gtsam/slam/EssentialMatrixConstraint.h>
@ -237,7 +237,7 @@ virtual class EssentialMatrixConstraint : gtsam::NoiseModelFactor {
void print(string s = "", const gtsam::KeyFormatter& keyFormatter = void print(string s = "", const gtsam::KeyFormatter& keyFormatter =
gtsam::DefaultKeyFormatter) const; gtsam::DefaultKeyFormatter) const;
bool equals(const gtsam::EssentialMatrixConstraint& other, double tol) const; bool equals(const gtsam::EssentialMatrixConstraint& other, double tol) const;
Vector evaluateError(const gtsam::Pose3& p1, const gtsam::Pose3& p2) const; gtsam::Vector evaluateError(const gtsam::Pose3& p1, const gtsam::Pose3& p2) const;
const gtsam::EssentialMatrix& measured() const; const gtsam::EssentialMatrix& measured() const;
}; };
@ -336,7 +336,7 @@ virtual class FrobeniusFactor : gtsam::NoiseModelFactor {
FrobeniusFactor(size_t key1, size_t key2); FrobeniusFactor(size_t key1, size_t key2);
FrobeniusFactor(size_t key1, size_t key2, gtsam::noiseModel::Base* model); FrobeniusFactor(size_t key1, size_t key2, gtsam::noiseModel::Base* model);
Vector evaluateError(const T& R1, const T& R2); gtsam::Vector evaluateError(const T& R1, const T& R2);
}; };
template <T = {gtsam::SO3, gtsam::SO4}> template <T = {gtsam::SO3, gtsam::SO4}>
@ -345,7 +345,7 @@ virtual class FrobeniusBetweenFactor : gtsam::NoiseModelFactor {
FrobeniusBetweenFactor(size_t key1, size_t key2, const T& R12, FrobeniusBetweenFactor(size_t key1, size_t key2, const T& R12,
gtsam::noiseModel::Base* model); gtsam::noiseModel::Base* model);
Vector evaluateError(const T& R1, const T& R2); gtsam::Vector evaluateError(const T& R1, const T& R2);
}; };
#include <gtsam/slam/lago.h> #include <gtsam/slam/lago.h>

136
gtsam_unstable/gtsam_unstable.i
View File

@ -54,10 +54,10 @@ class Dummy {
#include <gtsam_unstable/dynamics/PoseRTV.h> #include <gtsam_unstable/dynamics/PoseRTV.h>
class PoseRTV { class PoseRTV {
PoseRTV(); PoseRTV();
PoseRTV(Vector rtv); PoseRTV(gtsam::Vector rtv);
PoseRTV(const gtsam::Point3& pt, const gtsam::Rot3& rot, const Vector& vel); PoseRTV(const gtsam::Point3& pt, const gtsam::Rot3& rot, const gtsam::Vector& vel);
PoseRTV(const gtsam::Rot3& rot, const gtsam::Point3& pt, const Vector& vel); PoseRTV(const gtsam::Rot3& rot, const gtsam::Point3& pt, const gtsam::Vector& vel);
PoseRTV(const gtsam::Pose3& pose, const Vector& vel); PoseRTV(const gtsam::Pose3& pose, const gtsam::Vector& vel);
PoseRTV(const gtsam::Pose3& pose); PoseRTV(const gtsam::Pose3& pose);
PoseRTV(double roll, double pitch, double yaw, double x, double y, double z, double vx, double vy, double vz); PoseRTV(double roll, double pitch, double yaw, double x, double y, double z, double vx, double vy, double vz);
@ -68,21 +68,21 @@ class PoseRTV {
// access // access
gtsam::Point3 translation() const; gtsam::Point3 translation() const;
gtsam::Rot3 rotation() const; gtsam::Rot3 rotation() const;
Vector velocity() const; gtsam::Vector velocity() const;
gtsam::Pose3 pose() const; gtsam::Pose3 pose() const;
// Vector interfaces // gtsam::Vector interfaces
Vector vector() const; gtsam::Vector vector() const;
Vector translationVec() const; gtsam::Vector translationVec() const;
Vector velocityVec() const; gtsam::Vector velocityVec() const;
// manifold/Lie // manifold/Lie
static size_t Dim(); static size_t Dim();
size_t dim() const; size_t dim() const;
gtsam::PoseRTV retract(Vector v) const; gtsam::PoseRTV retract(gtsam::Vector v) const;
Vector localCoordinates(const gtsam::PoseRTV& p) const; gtsam::Vector localCoordinates(const gtsam::PoseRTV& p) const;
static gtsam::PoseRTV Expmap(Vector v); static gtsam::PoseRTV Expmap(gtsam::Vector v);
static Vector Logmap(const gtsam::PoseRTV& p); static gtsam::Vector Logmap(const gtsam::PoseRTV& p);
gtsam::PoseRTV inverse() const; gtsam::PoseRTV inverse() const;
gtsam::PoseRTV compose(const gtsam::PoseRTV& p) const; gtsam::PoseRTV compose(const gtsam::PoseRTV& p) const;
gtsam::PoseRTV between(const gtsam::PoseRTV& p) const; gtsam::PoseRTV between(const gtsam::PoseRTV& p) const;
@ -94,10 +94,10 @@ class PoseRTV {
// IMU/dynamics // IMU/dynamics
gtsam::PoseRTV planarDynamics(double vel_rate, double heading_rate, double max_accel, double dt) const; gtsam::PoseRTV planarDynamics(double vel_rate, double heading_rate, double max_accel, double dt) const;
gtsam::PoseRTV flyingDynamics(double pitch_rate, double heading_rate, double lift_control, double dt) const; gtsam::PoseRTV flyingDynamics(double pitch_rate, double heading_rate, double lift_control, double dt) const;
gtsam::PoseRTV generalDynamics(Vector accel, Vector gyro, double dt) const; gtsam::PoseRTV generalDynamics(gtsam::Vector accel, gtsam::Vector gyro, double dt) const;
Vector imuPrediction(const gtsam::PoseRTV& x2, double dt) const; gtsam::Vector imuPrediction(const gtsam::PoseRTV& x2, double dt) const;
gtsam::Point3 translationIntegration(const gtsam::PoseRTV& x2, double dt) const; gtsam::Point3 translationIntegration(const gtsam::PoseRTV& x2, double dt) const;
Vector translationIntegrationVec(const gtsam::PoseRTV& x2, double dt) const; gtsam::Vector translationIntegrationVec(const gtsam::PoseRTV& x2, double dt) const;
void serializable() const; // enabling serialization functionality void serializable() const; // enabling serialization functionality
}; };
@ -126,16 +126,16 @@ class Pose3Upright {
gtsam::Pose3 pose() const; gtsam::Pose3 pose() const;
size_t dim() const; size_t dim() const;
gtsam::Pose3Upright retract(Vector v) const; gtsam::Pose3Upright retract(gtsam::Vector v) const;
Vector localCoordinates(const gtsam::Pose3Upright& p2) const; gtsam::Vector localCoordinates(const gtsam::Pose3Upright& p2) const;
static gtsam::Pose3Upright Identity(); static gtsam::Pose3Upright Identity();
gtsam::Pose3Upright inverse() const; gtsam::Pose3Upright inverse() const;
gtsam::Pose3Upright compose(const gtsam::Pose3Upright& p2) const; gtsam::Pose3Upright compose(const gtsam::Pose3Upright& p2) const;
gtsam::Pose3Upright between(const gtsam::Pose3Upright& p2) const; gtsam::Pose3Upright between(const gtsam::Pose3Upright& p2) const;
static gtsam::Pose3Upright Expmap(Vector xi); static gtsam::Pose3Upright Expmap(gtsam::Vector xi);
static Vector Logmap(const gtsam::Pose3Upright& p); static gtsam::Vector Logmap(const gtsam::Pose3Upright& p);
void serializable() const; // enabling serialization functionality void serializable() const; // enabling serialization functionality
}; // \class Pose3Upright }; // \class Pose3Upright
@ -156,8 +156,8 @@ class BearingS2 {
bool equals(const gtsam::BearingS2& x, double tol) const; bool equals(const gtsam::BearingS2& x, double tol) const;
size_t dim() const; size_t dim() const;
gtsam::BearingS2 retract(Vector v) const; gtsam::BearingS2 retract(gtsam::Vector v) const;
Vector localCoordinates(const gtsam::BearingS2& p2) const; gtsam::Vector localCoordinates(const gtsam::BearingS2& p2) const;
void serializable() const; // enabling serialization functionality void serializable() const; // enabling serialization functionality
}; };
@ -304,17 +304,17 @@ virtual class BetweenFactorEM : gtsam::NonlinearFactor {
const gtsam::noiseModel::Gaussian* model_inlier, const gtsam::noiseModel::Gaussian* model_outlier, const gtsam::noiseModel::Gaussian* model_inlier, const gtsam::noiseModel::Gaussian* model_outlier,
double prior_inlier, double prior_outlier, bool flag_bump_up_near_zero_probs); double prior_inlier, double prior_outlier, bool flag_bump_up_near_zero_probs);
Vector whitenedError(const gtsam::Values& x); gtsam::Vector whitenedError(const gtsam::Values& x);
Vector unwhitenedError(const gtsam::Values& x); gtsam::Vector unwhitenedError(const gtsam::Values& x);
Vector calcIndicatorProb(const gtsam::Values& x); gtsam::Vector calcIndicatorProb(const gtsam::Values& x);
gtsam::Pose2 measured(); // TODO: figure out how to output a template instead gtsam::Pose2 measured(); // TODO: figure out how to output a template instead
void set_flag_bump_up_near_zero_probs(bool flag); void set_flag_bump_up_near_zero_probs(bool flag);
bool get_flag_bump_up_near_zero_probs() const; bool get_flag_bump_up_near_zero_probs() const;
void updateNoiseModels(const gtsam::Values& values, const gtsam::NonlinearFactorGraph& graph); void updateNoiseModels(const gtsam::Values& values, const gtsam::NonlinearFactorGraph& graph);
void updateNoiseModels_givenCovs(const gtsam::Values& values, Matrix cov1, Matrix cov2, Matrix cov12); void updateNoiseModels_givenCovs(const gtsam::Values& values, gtsam::Matrix cov1, gtsam::Matrix cov2, gtsam::Matrix cov12);
Matrix get_model_inlier_cov(); gtsam::Matrix get_model_inlier_cov();
Matrix get_model_outlier_cov(); gtsam::Matrix get_model_outlier_cov();
void serializable() const; // enabling serialization functionality void serializable() const; // enabling serialization functionality
}; };
@ -332,15 +332,15 @@ virtual class TransformBtwRobotsUnaryFactorEM : gtsam::NonlinearFactor {
const gtsam::noiseModel::Gaussian* model_inlier, const gtsam::noiseModel::Gaussian* model_outlier, const gtsam::noiseModel::Gaussian* model_inlier, const gtsam::noiseModel::Gaussian* model_outlier,
double prior_inlier, double prior_outlier, bool flag_bump_up_near_zero_probs, bool start_with_M_step); double prior_inlier, double prior_outlier, bool flag_bump_up_near_zero_probs, bool start_with_M_step);
Vector whitenedError(const gtsam::Values& x); gtsam::Vector whitenedError(const gtsam::Values& x);
Vector unwhitenedError(const gtsam::Values& x); gtsam::Vector unwhitenedError(const gtsam::Values& x);
Vector calcIndicatorProb(const gtsam::Values& x); gtsam::Vector calcIndicatorProb(const gtsam::Values& x);
void setValAValB(const gtsam::Values valA, const gtsam::Values valB); void setValAValB(const gtsam::Values valA, const gtsam::Values valB);
void updateNoiseModels(const gtsam::Values& values, const gtsam::NonlinearFactorGraph& graph); void updateNoiseModels(const gtsam::Values& values, const gtsam::NonlinearFactorGraph& graph);
void updateNoiseModels_givenCovs(const gtsam::Values& values, Matrix cov1, Matrix cov2, Matrix cov12); void updateNoiseModels_givenCovs(const gtsam::Values& values, gtsam::Matrix cov1, gtsam::Matrix cov2, gtsam::Matrix cov12);
Matrix get_model_inlier_cov(); gtsam::Matrix get_model_inlier_cov();
Matrix get_model_outlier_cov(); gtsam::Matrix get_model_outlier_cov();
void serializable() const; // enabling serialization functionality void serializable() const; // enabling serialization functionality
}; };
@ -352,8 +352,8 @@ virtual class TransformBtwRobotsUnaryFactor : gtsam::NonlinearFactor {
const gtsam::Values& valA, const gtsam::Values& valB, const gtsam::Values& valA, const gtsam::Values& valB,
const gtsam::noiseModel::Gaussian* model); const gtsam::noiseModel::Gaussian* model);
Vector whitenedError(const gtsam::Values& x); gtsam::Vector whitenedError(const gtsam::Values& x);
Vector unwhitenedError(const gtsam::Values& x); gtsam::Vector unwhitenedError(const gtsam::Values& x);
void setValAValB(const gtsam::Values valA, const gtsam::Values valB); void setValAValB(const gtsam::Values valA, const gtsam::Values valB);
void serializable() const; // enabling serialization functionality void serializable() const; // enabling serialization functionality
@ -419,16 +419,16 @@ virtual class NonlinearEquality : gtsam::NoiseModelFactor {
template<POSE = {gtsam::PoseRTV}> template<POSE = {gtsam::PoseRTV}>
virtual class IMUFactor : gtsam::NoiseModelFactor { virtual class IMUFactor : gtsam::NoiseModelFactor {
/** Standard constructor */ /** Standard constructor */
IMUFactor(Vector accel, Vector gyro, IMUFactor(gtsam::Vector accel, gtsam::Vector gyro,
double dt, size_t key1, size_t key2, const gtsam::noiseModel::Base* model); double dt, size_t key1, size_t key2, const gtsam::noiseModel::Base* model);
/** Full IMU vector specification */ /** Full IMU vector specification */
IMUFactor(Vector imu_vector, IMUFactor(gtsam::Vector imu_vector,
double dt, size_t key1, size_t key2, const gtsam::noiseModel::Base* model); double dt, size_t key1, size_t key2, const gtsam::noiseModel::Base* model);
Vector gyro() const; gtsam::Vector gyro() const;
Vector accel() const; gtsam::Vector accel() const;
Vector z() const; gtsam::Vector z() const;
template <I = {1, 2}> template <I = {1, 2}>
size_t key() const; size_t key() const;
@ -438,16 +438,16 @@ virtual class IMUFactor : gtsam::NoiseModelFactor {
template<POSE = {gtsam::PoseRTV}> template<POSE = {gtsam::PoseRTV}>
virtual class FullIMUFactor : gtsam::NoiseModelFactor { virtual class FullIMUFactor : gtsam::NoiseModelFactor {
/** Standard constructor */ /** Standard constructor */
FullIMUFactor(Vector accel, Vector gyro, FullIMUFactor(gtsam::Vector accel, gtsam::Vector gyro,
double dt, size_t key1, size_t key2, const gtsam::noiseModel::Base* model); double dt, size_t key1, size_t key2, const gtsam::noiseModel::Base* model);
/** Single IMU vector - imu = [accel, gyro] */ /** Single IMU vector - imu = [accel, gyro] */
FullIMUFactor(Vector imu, FullIMUFactor(gtsam::Vector imu,
double dt, size_t key1, size_t key2, const gtsam::noiseModel::Base* model); double dt, size_t key1, size_t key2, const gtsam::noiseModel::Base* model);
Vector gyro() const; gtsam::Vector gyro() const;
Vector accel() const; gtsam::Vector accel() const;
Vector z() const; gtsam::Vector z() const;
template <I = {1, 2}> template <I = {1, 2}>
size_t key() const; size_t key() const;
@ -466,14 +466,14 @@ virtual class DRollPrior : gtsam::NonlinearFactor {
}; };
virtual class VelocityPrior : gtsam::NonlinearFactor { virtual class VelocityPrior : gtsam::NonlinearFactor {
VelocityPrior(size_t key, Vector vel, const gtsam::noiseModel::Base* model); VelocityPrior(size_t key, gtsam::Vector vel, const gtsam::noiseModel::Base* model);
}; };
virtual class DGroundConstraint : gtsam::NonlinearFactor { virtual class DGroundConstraint : gtsam::NonlinearFactor {
// Primary constructor allows for variable height of the "floor" // Primary constructor allows for variable height of the "floor"
DGroundConstraint(size_t key, double height, const gtsam::noiseModel::Base* model); DGroundConstraint(size_t key, double height, const gtsam::noiseModel::Base* model);
// Fully specify vector - use only for debugging // Fully specify vector - use only for debugging
DGroundConstraint(size_t key, Vector constraint, const gtsam::noiseModel::Base* model); DGroundConstraint(size_t key, gtsam::Vector constraint, const gtsam::noiseModel::Base* model);
}; };
#include <gtsam_unstable/dynamics/VelocityConstraint3.h> #include <gtsam_unstable/dynamics/VelocityConstraint3.h>
@ -481,7 +481,7 @@ virtual class VelocityConstraint3 : gtsam::NonlinearFactor {
/** Standard constructor */ /** Standard constructor */
VelocityConstraint3(size_t key1, size_t key2, size_t velKey, double dt); VelocityConstraint3(size_t key1, size_t key2, size_t velKey, double dt);
Vector evaluateError(const double& x1, const double& x2, const double& v) const; gtsam::Vector evaluateError(const double& x1, const double& x2, const double& v) const;
}; };
#include <gtsam_unstable/dynamics/Pendulum.h> #include <gtsam_unstable/dynamics/Pendulum.h>
@ -489,7 +489,7 @@ virtual class PendulumFactor1 : gtsam::NonlinearFactor {
/** Standard constructor */ /** Standard constructor */
PendulumFactor1(size_t k1, size_t k, size_t velKey, double dt); PendulumFactor1(size_t k1, size_t k, size_t velKey, double dt);
Vector evaluateError(const double& qk1, const double& qk, const double& v) const; gtsam::Vector evaluateError(const double& qk1, const double& qk, const double& v) const;
}; };
#include <gtsam_unstable/dynamics/Pendulum.h> #include <gtsam_unstable/dynamics/Pendulum.h>
@ -497,35 +497,35 @@ virtual class PendulumFactor2 : gtsam::NonlinearFactor {
/** Standard constructor */ /** Standard constructor */
PendulumFactor2(size_t vk1, size_t vk, size_t qKey, double dt, double L, double g); PendulumFactor2(size_t vk1, size_t vk, size_t qKey, double dt, double L, double g);
Vector evaluateError(const double& vk1, const double& vk, const double& q) const; gtsam::Vector evaluateError(const double& vk1, const double& vk, const double& q) const;
}; };
virtual class PendulumFactorPk : gtsam::NonlinearFactor { virtual class PendulumFactorPk : gtsam::NonlinearFactor {
/** Standard constructor */ /** Standard constructor */
PendulumFactorPk(size_t pk, size_t qk, size_t qk1, double h, double m, double r, double g, double alpha); PendulumFactorPk(size_t pk, size_t qk, size_t qk1, double h, double m, double r, double g, double alpha);
Vector evaluateError(const double& pk, const double& qk, const double& qk1) const; gtsam::Vector evaluateError(const double& pk, const double& qk, const double& qk1) const;
}; };
virtual class PendulumFactorPk1 : gtsam::NonlinearFactor { virtual class PendulumFactorPk1 : gtsam::NonlinearFactor {
/** Standard constructor */ /** Standard constructor */
PendulumFactorPk1(size_t pk1, size_t qk, size_t qk1, double h, double m, double r, double g, double alpha); PendulumFactorPk1(size_t pk1, size_t qk, size_t qk1, double h, double m, double r, double g, double alpha);
Vector evaluateError(const double& pk1, const double& qk, const double& qk1) const; gtsam::Vector evaluateError(const double& pk1, const double& qk, const double& qk1) const;
}; };
#include <gtsam_unstable/dynamics/SimpleHelicopter.h> #include <gtsam_unstable/dynamics/SimpleHelicopter.h>
virtual class Reconstruction : gtsam::NoiseModelFactor { virtual class Reconstruction : gtsam::NoiseModelFactor {
Reconstruction(size_t gKey1, size_t gKey, size_t xiKey, double h); Reconstruction(size_t gKey1, size_t gKey, size_t xiKey, double h);
Vector evaluateError(const gtsam::Pose3& gK1, const gtsam::Pose3& gK, Vector xiK) const; gtsam::Vector evaluateError(const gtsam::Pose3& gK1, const gtsam::Pose3& gK, gtsam::Vector xiK) const;
}; };
virtual class DiscreteEulerPoincareHelicopter : gtsam::NoiseModelFactor { virtual class DiscreteEulerPoincareHelicopter : gtsam::NoiseModelFactor {
DiscreteEulerPoincareHelicopter(size_t xiKey, size_t xiKey_1, size_t gKey, DiscreteEulerPoincareHelicopter(size_t xiKey, size_t xiKey_1, size_t gKey,
double h, Matrix Inertia, Vector Fu, double m); double h, gtsam::Matrix Inertia, gtsam::Vector Fu, double m);
Vector evaluateError(Vector xiK, Vector xiK_1, const gtsam::Pose3& gK) const; gtsam::Vector evaluateError(gtsam::Vector xiK, gtsam::Vector xiK_1, const gtsam::Pose3& gK) const;
}; };
//************************************************************************* //*************************************************************************
@ -649,25 +649,25 @@ virtual class InvDepthFactorVariant3b : gtsam::NoiseModelFactor {
#include <gtsam_unstable/slam/Mechanization_bRn2.h> #include <gtsam_unstable/slam/Mechanization_bRn2.h>
class Mechanization_bRn2 { class Mechanization_bRn2 {
Mechanization_bRn2(); Mechanization_bRn2();
Mechanization_bRn2(gtsam::Rot3& initial_bRn, Vector initial_x_g, Mechanization_bRn2(gtsam::Rot3& initial_bRn, gtsam::Vector initial_x_g,
Vector initial_x_a); gtsam::Vector initial_x_a);
Vector b_g(double g_e); gtsam::Vector b_g(double g_e);
gtsam::Rot3 bRn(); gtsam::Rot3 bRn();
Vector x_g(); gtsam::Vector x_g();
Vector x_a(); gtsam::Vector x_a();
static gtsam::Mechanization_bRn2 initialize(Matrix U, Matrix F, double g_e); static gtsam::Mechanization_bRn2 initialize(gtsam::Matrix U, gtsam::Matrix F, double g_e);
gtsam::Mechanization_bRn2 correct(Vector dx) const; gtsam::Mechanization_bRn2 correct(gtsam::Vector dx) const;
gtsam::Mechanization_bRn2 integrate(Vector u, double dt) const; gtsam::Mechanization_bRn2 integrate(gtsam::Vector u, double dt) const;
//void print(string s) const; //void print(string s) const;
}; };
#include <gtsam_unstable/slam/AHRS.h> #include <gtsam_unstable/slam/AHRS.h>
class AHRS { class AHRS {
AHRS(Matrix U, Matrix F, double g_e); AHRS(gtsam::Matrix U, gtsam::Matrix F, double g_e);
pair<gtsam::Mechanization_bRn2, gtsam::GaussianDensity*> initialize(double g_e); pair<gtsam::Mechanization_bRn2, gtsam::GaussianDensity*> initialize(double g_e);
pair<gtsam::Mechanization_bRn2, gtsam::GaussianDensity*> integrate(const gtsam::Mechanization_bRn2& mech, gtsam::GaussianDensity* state, Vector u, double dt); pair<gtsam::Mechanization_bRn2, gtsam::GaussianDensity*> integrate(const gtsam::Mechanization_bRn2& mech, gtsam::GaussianDensity* state, gtsam::Vector u, double dt);
pair<gtsam::Mechanization_bRn2, gtsam::GaussianDensity*> aid(const gtsam::Mechanization_bRn2& mech, gtsam::GaussianDensity* state, Vector f, bool Farrel); pair<gtsam::Mechanization_bRn2, gtsam::GaussianDensity*> aid(const gtsam::Mechanization_bRn2& mech, gtsam::GaussianDensity* state, gtsam::Vector f, bool Farrel);
pair<gtsam::Mechanization_bRn2, gtsam::GaussianDensity*> aidGeneral(const gtsam::Mechanization_bRn2& mech, gtsam::GaussianDensity* state, Vector f, Vector f_expected, const gtsam::Rot3& increment); pair<gtsam::Mechanization_bRn2, gtsam::GaussianDensity*> aidGeneral(const gtsam::Mechanization_bRn2& mech, gtsam::GaussianDensity* state, gtsam::Vector f, gtsam::Vector f_expected, const gtsam::Rot3& increment);
//void print(string s) const; //void print(string s) const;
}; };