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.
# See https://help.github.com/en/articles/workflow-syntax-for-github-actions.
name: [
macos-11-xcode-13.4.1,
macos-12-xcode-14.2,
]
build_type: [Debug, Release]
build_unstable: [ON]
include:
- name: macos-11-xcode-13.4.1
os: macos-11
- name: macos-12-xcode-14.2
os: macos-12
compiler: xcode
version: "13.4.1"
version: "14.2"
steps:
- 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-tbb,
ubuntu-20.04-clang-9,
macOS-11-xcode-13.4.1,
macos-12-xcode-14.2,
windows-2022-msbuild,
]
@ -54,10 +54,10 @@ jobs:
compiler: clang
version: "9"
- name: macOS-11-xcode-13.4.1
os: macOS-11
- name: macos-12-xcode-14.2
os: macos-12
compiler: xcode
version: "13.4.1"
version: "14.2"
- name: windows-2022-msbuild
os: windows-2022

4
gtsam/base/base.i
View File

@ -86,7 +86,7 @@ class IndexPairSetMap {
#include <gtsam/base/Matrix.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>
virtual class Value {
@ -100,7 +100,7 @@ virtual class Value {
};
#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::Cal3_S2, gtsam::Cal3DS2, gtsam::Cal3Bundler,
gtsam::Cal3Fisheye, gtsam::Cal3Unified, gtsam::EssentialMatrix,

40
gtsam/basis/basis.i
View File

@ -10,23 +10,23 @@ namespace gtsam {
#include <gtsam/basis/Fourier.h>
class FourierBasis {
static Vector CalculateWeights(size_t N, double x);
static Matrix WeightMatrix(size_t N, Vector x);
static gtsam::Vector CalculateWeights(size_t N, double x);
static gtsam::Matrix WeightMatrix(size_t N, gtsam::Vector x);
static Matrix DifferentiationMatrix(size_t N);
static Vector DerivativeWeights(size_t N, double x);
static gtsam::Matrix DifferentiationMatrix(size_t N);
static gtsam::Vector DerivativeWeights(size_t N, double x);
};
#include <gtsam/basis/Chebyshev.h>
class Chebyshev1Basis {
static Matrix CalculateWeights(size_t N, double x);
static Matrix WeightMatrix(size_t N, Vector X);
static gtsam::Matrix CalculateWeights(size_t N, double x);
static gtsam::Matrix WeightMatrix(size_t N, gtsam::Vector X);
};
class Chebyshev2Basis {
static Matrix CalculateWeights(size_t N, double x);
static Matrix WeightMatrix(size_t N, Vector x);
static gtsam::Matrix CalculateWeights(size_t N, double x);
static gtsam::Matrix WeightMatrix(size_t N, gtsam::Vector x);
};
#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, double a, double b);
static Vector Points(size_t N);
static Vector Points(size_t N, double a, double b);
static gtsam::Vector Points(size_t N);
static gtsam::Vector Points(size_t N, double a, double b);
static Matrix WeightMatrix(size_t N, Vector X);
static Matrix WeightMatrix(size_t N, Vector X, double a, double b);
static gtsam::Matrix WeightMatrix(size_t N, gtsam::Vector X);
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 Matrix DerivativeWeights(size_t N, double x, double a, double b);
static Matrix IntegrationWeights(size_t N, double a, double b);
static Matrix DifferentiationMatrix(size_t N, double a, double b);
static gtsam::Matrix CalculateWeights(size_t N, double x, double a, double b);
static gtsam::Matrix DerivativeWeights(size_t N, double x, double a, double b);
static gtsam::Matrix IntegrationWeights(size_t N, double a, double b);
static gtsam::Matrix DifferentiationMatrix(size_t N, double a, double b);
};
#include <gtsam/basis/BasisFactors.h>
@ -64,10 +64,10 @@ template <BASIS = {gtsam::FourierBasis, gtsam::Chebyshev1Basis,
gtsam::Chebyshev2Basis, gtsam::Chebyshev2}>
virtual class VectorEvaluationFactor : gtsam::NoiseModelFactor {
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 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 size_t N, double x, double a, double b);
};
@ -116,10 +116,10 @@ template <BASIS = {gtsam::FourierBasis, gtsam::Chebyshev1Basis,
gtsam::Chebyshev2Basis, gtsam::Chebyshev2}>
virtual class VectorDerivativeFactor : gtsam::NoiseModelFactor {
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 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 size_t N, double x, double a, double b);
};

328
gtsam/geometry/geometry.i
View File

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

28
gtsam/gtsam.i
View File

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

210
gtsam/linear/linear.i
View File

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

140
gtsam/navigation/navigation.i
View File

@ -10,7 +10,7 @@ namespace imuBias {
class ConstantBias {
// Constructors
ConstantBias();
ConstantBias(Vector biasAcc, Vector biasGyro);
ConstantBias(gtsam::Vector biasAcc, gtsam::Vector biasGyro);
// Testable
void print(string s = "") const;
@ -25,11 +25,11 @@ class ConstantBias {
gtsam::imuBias::ConstantBias operator-(const gtsam::imuBias::ConstantBias& b) const;
// Standard Interface
Vector vector() const;
Vector accelerometer() const;
Vector gyroscope() const;
Vector correctAccelerometer(Vector measurement) const;
Vector correctGyroscope(Vector measurement) const;
gtsam::Vector vector() const;
gtsam::Vector accelerometer() const;
gtsam::Vector gyroscope() const;
gtsam::Vector correctAccelerometer(gtsam::Vector measurement) const;
gtsam::Vector correctGyroscope(gtsam::Vector measurement) const;
// enabling serialization functionality
void serialize() const;
@ -41,8 +41,8 @@ class ConstantBias {
class NavState {
// Constructors
NavState();
NavState(const gtsam::Rot3& R, const gtsam::Point3& t, Vector v);
NavState(const gtsam::Pose3& pose, Vector v);
NavState(const gtsam::Rot3& R, const gtsam::Point3& t, gtsam::Vector v);
NavState(const gtsam::Pose3& pose, gtsam::Vector v);
// Testable
void print(string s = "") const;
@ -51,11 +51,11 @@ class NavState {
// Access
gtsam::Rot3 attitude() const;
gtsam::Point3 position() const;
Vector velocity() const;
gtsam::Vector velocity() const;
gtsam::Pose3 pose() const;
gtsam::NavState retract(const Vector& x) const;
Vector localCoordinates(const gtsam::NavState& g) const;
gtsam::NavState retract(const gtsam::Vector& x) const;
gtsam::Vector localCoordinates(const gtsam::NavState& g) const;
// enabling serialization functionality
void serialize() const;
@ -69,19 +69,19 @@ virtual class PreintegratedRotationParams {
void print(string s = "") const;
bool equals(const gtsam::PreintegratedRotationParams& expected, double tol);
void setGyroscopeCovariance(Matrix cov);
void setOmegaCoriolis(Vector omega);
void setGyroscopeCovariance(gtsam::Matrix cov);
void setOmegaCoriolis(gtsam::Vector omega);
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;
};
#include <gtsam/navigation/PreintegrationParams.h>
virtual class PreintegrationParams : gtsam::PreintegratedRotationParams {
PreintegrationParams(Vector n_gravity);
PreintegrationParams(gtsam::Vector n_gravity);
gtsam::Vector n_gravity;
@ -94,12 +94,12 @@ virtual class PreintegrationParams : gtsam::PreintegratedRotationParams {
void print(string s = "") const;
bool equals(const gtsam::PreintegrationParams& expected, double tol);
void setAccelerometerCovariance(Matrix cov);
void setIntegrationCovariance(Matrix cov);
void setAccelerometerCovariance(gtsam::Matrix cov);
void setIntegrationCovariance(gtsam::Matrix cov);
void setUse2ndOrderCoriolis(bool flag);
Matrix getAccelerometerCovariance() const;
Matrix getIntegrationCovariance() const;
gtsam::Matrix getAccelerometerCovariance() const;
gtsam::Matrix getIntegrationCovariance() const;
bool getUse2ndOrderCoriolis() const;
// enabling serialization functionality
@ -118,19 +118,19 @@ class PreintegratedImuMeasurements {
bool equals(const gtsam::PreintegratedImuMeasurements& expected, double tol);
// Standard Interface
void integrateMeasurement(Vector measuredAcc, Vector measuredOmega,
void integrateMeasurement(gtsam::Vector measuredAcc, gtsam::Vector measuredOmega,
double deltaT);
void resetIntegration();
void resetIntegrationAndSetBias(const gtsam::imuBias::ConstantBias& biasHat);
Matrix preintMeasCov() const;
Vector preintegrated() const;
gtsam::Matrix preintMeasCov() const;
gtsam::Vector preintegrated() const;
double deltaTij() const;
gtsam::Rot3 deltaRij() const;
Vector deltaPij() const;
Vector deltaVij() const;
gtsam::Vector deltaPij() const;
gtsam::Vector deltaVij() const;
gtsam::imuBias::ConstantBias biasHat() const;
Vector biasHatVector() const;
gtsam::Vector biasHatVector() const;
gtsam::NavState predict(const gtsam::NavState& state_i,
const gtsam::imuBias::ConstantBias& bias) const;
@ -145,14 +145,14 @@ virtual class ImuFactor: gtsam::NonlinearFactor {
// Standard Interface
gtsam::PreintegratedImuMeasurements preintegratedMeasurements() const;
Vector evaluateError(const gtsam::Pose3& pose_i, Vector vel_i,
const gtsam::Pose3& pose_j, Vector vel_j,
gtsam::Vector evaluateError(const gtsam::Pose3& pose_i, gtsam::Vector vel_i,
const gtsam::Pose3& pose_j, gtsam::Vector vel_j,
const gtsam::imuBias::ConstantBias& bias);
};
#include <gtsam/navigation/CombinedImuFactor.h>
virtual class PreintegrationCombinedParams : gtsam::PreintegrationParams {
PreintegrationCombinedParams(Vector n_gravity);
PreintegrationCombinedParams(gtsam::Vector n_gravity);
static gtsam::PreintegrationCombinedParams* MakeSharedD(double g);
static gtsam::PreintegrationCombinedParams* MakeSharedU(double g);
@ -163,13 +163,13 @@ virtual class PreintegrationCombinedParams : gtsam::PreintegrationParams {
void print(string s = "") const;
bool equals(const gtsam::PreintegrationCombinedParams& expected, double tol);
void setBiasAccCovariance(Matrix cov);
void setBiasOmegaCovariance(Matrix cov);
void setBiasAccOmegaInit(Matrix cov);
void setBiasAccCovariance(gtsam::Matrix cov);
void setBiasOmegaCovariance(gtsam::Matrix cov);
void setBiasAccOmegaInit(gtsam::Matrix cov);
Matrix getBiasAccCovariance() const ;
Matrix getBiasOmegaCovariance() const ;
Matrix getBiasAccOmegaInit() const;
gtsam::Matrix getBiasAccCovariance() const ;
gtsam::Matrix getBiasOmegaCovariance() const ;
gtsam::Matrix getBiasAccOmegaInit() const;
};
@ -184,18 +184,18 @@ class PreintegratedCombinedMeasurements {
double tol);
// Standard Interface
void integrateMeasurement(Vector measuredAcc, Vector measuredOmega,
void integrateMeasurement(gtsam::Vector measuredAcc, gtsam::Vector measuredOmega,
double deltaT);
void resetIntegration();
void resetIntegrationAndSetBias(const gtsam::imuBias::ConstantBias& biasHat);
Matrix preintMeasCov() const;
gtsam::Matrix preintMeasCov() const;
double deltaTij() const;
gtsam::Rot3 deltaRij() const;
Vector deltaPij() const;
Vector deltaVij() const;
gtsam::Vector deltaPij() const;
gtsam::Vector deltaVij() const;
gtsam::imuBias::ConstantBias biasHat() const;
Vector biasHatVector() const;
gtsam::Vector biasHatVector() const;
gtsam::NavState predict(const gtsam::NavState& state_i,
const gtsam::imuBias::ConstantBias& bias) const;
};
@ -207,8 +207,8 @@ virtual class CombinedImuFactor: gtsam::NoiseModelFactor {
// Standard Interface
gtsam::PreintegratedCombinedMeasurements preintegratedMeasurements() const;
Vector evaluateError(const gtsam::Pose3& pose_i, Vector vel_i,
const gtsam::Pose3& pose_j, Vector vel_j,
gtsam::Vector evaluateError(const gtsam::Pose3& pose_i, gtsam::Vector vel_i,
const gtsam::Pose3& pose_j, gtsam::Vector vel_j,
const gtsam::imuBias::ConstantBias& bias_i,
const gtsam::imuBias::ConstantBias& bias_j);
};
@ -217,12 +217,12 @@ virtual class CombinedImuFactor: gtsam::NoiseModelFactor {
class PreintegratedAhrsMeasurements {
// Standard Constructor
PreintegratedAhrsMeasurements(const gtsam::PreintegrationParams* params,
const Vector& biasHat);
const gtsam::Vector& biasHat);
PreintegratedAhrsMeasurements(const gtsam::PreintegrationParams* p,
const Vector& bias_hat, double deltaTij,
const gtsam::Vector& bias_hat, double deltaTij,
const gtsam::Rot3& deltaRij,
const Matrix& delRdelBiasOmega,
const Matrix& preint_meas_cov);
const gtsam::Matrix& delRdelBiasOmega,
const gtsam::Matrix& preint_meas_cov);
PreintegratedAhrsMeasurements(const gtsam::PreintegratedAhrsMeasurements& rhs);
// Testable
@ -232,27 +232,27 @@ class PreintegratedAhrsMeasurements {
// get Data
gtsam::Rot3 deltaRij() const;
double deltaTij() const;
Vector biasHat() const;
gtsam::Vector biasHat() const;
// Standard Interface
void integrateMeasurement(Vector measuredOmega, double deltaT);
void integrateMeasurement(gtsam::Vector measuredOmega, double deltaT);
void resetIntegration() ;
};
virtual class AHRSFactor : gtsam::NonlinearFactor {
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,
const gtsam::PreintegratedAhrsMeasurements& preintegratedMeasurements, Vector omegaCoriolis,
const gtsam::PreintegratedAhrsMeasurements& preintegratedMeasurements, gtsam::Vector omegaCoriolis,
const gtsam::Pose3& body_P_sensor);
// Standard Interface
gtsam::PreintegratedAhrsMeasurements preintegratedMeasurements() const;
Vector evaluateError(const gtsam::Rot3& rot_i, const gtsam::Rot3& rot_j,
Vector bias) const;
gtsam::Rot3 predict(const gtsam::Rot3& rot_i, Vector bias,
gtsam::Vector evaluateError(const gtsam::Rot3& rot_i, const gtsam::Rot3& rot_j,
gtsam::Vector bias) const;
gtsam::Rot3 predict(const gtsam::Rot3& rot_i, gtsam::Vector bias,
const gtsam::PreintegratedAhrsMeasurements& preintegratedMeasurements,
Vector omegaCoriolis) const;
gtsam::Vector omegaCoriolis) const;
};
#include <gtsam/navigation/AttitudeFactor.h>
@ -333,25 +333,25 @@ virtual class BarometricFactor : gtsam::NonlinearFactor {
#include <gtsam/navigation/Scenario.h>
virtual class Scenario {
gtsam::Pose3 pose(double t) const;
Vector omega_b(double t) const;
Vector velocity_n(double t) const;
Vector acceleration_n(double t) const;
gtsam::Vector omega_b(double t) const;
gtsam::Vector velocity_n(double t) const;
gtsam::Vector acceleration_n(double t) const;
gtsam::Rot3 rotation(double t) const;
gtsam::NavState navState(double t) const;
Vector velocity_b(double t) const;
Vector acceleration_b(double t) const;
gtsam::Vector velocity_b(double t) const;
gtsam::Vector acceleration_b(double t) const;
};
virtual class ConstantTwistScenario : gtsam::Scenario {
ConstantTwistScenario(Vector w, Vector v);
ConstantTwistScenario(Vector w, Vector v,
ConstantTwistScenario(gtsam::Vector w, gtsam::Vector v);
ConstantTwistScenario(gtsam::Vector w, gtsam::Vector v,
const gtsam::Pose3& nTb0);
};
virtual class AcceleratingScenario : gtsam::Scenario {
AcceleratingScenario(const gtsam::Rot3& nRb, const gtsam::Point3& p0,
Vector v0, Vector a_n,
Vector omega_b);
gtsam::Vector v0, gtsam::Vector a_n,
gtsam::Vector omega_b);
};
#include <gtsam/navigation/ScenarioRunner.h>
@ -360,11 +360,11 @@ class ScenarioRunner {
const gtsam::PreintegrationParams* p,
double imuSampleTime,
const gtsam::imuBias::ConstantBias& bias);
Vector gravity_n() const;
Vector actualAngularVelocity(double t) const;
Vector actualSpecificForce(double t) const;
Vector measuredAngularVelocity(double t) const;
Vector measuredSpecificForce(double t) const;
gtsam::Vector gravity_n() const;
gtsam::Vector actualAngularVelocity(double t) const;
gtsam::Vector actualSpecificForce(double t) const;
gtsam::Vector measuredAngularVelocity(double t) const;
gtsam::Vector measuredSpecificForce(double t) const;
double imuSampleTime() const;
gtsam::PreintegratedImuMeasurements integrate(
double T, const gtsam::imuBias::ConstantBias& estimatedBias,
@ -372,10 +372,10 @@ class ScenarioRunner {
gtsam::NavState predict(
const gtsam::PreintegratedImuMeasurements& pim,
const gtsam::imuBias::ConstantBias& estimatedBias) const;
Matrix estimateCovariance(
gtsam::Matrix estimateCovariance(
double T, size_t N,
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;
template <T = {double,
Vector,
gtsam::Vector,
gtsam::Point2,
gtsam::StereoPoint2,
gtsam::Point3,
@ -131,8 +131,8 @@ virtual class NoiseModelFactor : gtsam::NonlinearFactor {
bool equals(const gtsam::NoiseModelFactor& other, double tol) const;
gtsam::noiseModel::Base* noiseModel() const;
gtsam::NoiseModelFactor* cloneWithNewNoiseModel(gtsam::noiseModel::Base* newNoise) const;
Vector unwhitenedError(const gtsam::Values& x) const;
Vector whitenedError(const gtsam::Values& x) const;
gtsam::Vector unwhitenedError(const gtsam::Values& x) const;
gtsam::Vector whitenedError(const gtsam::Values& x) const;
};
#include <gtsam/nonlinear/Marginals.h>
@ -146,8 +146,8 @@ class Marginals {
void print(string s = "Marginals: ", const gtsam::KeyFormatter& keyFormatter =
gtsam::DefaultKeyFormatter) const;
Matrix marginalCovariance(size_t variable) const;
Matrix marginalInformation(size_t variable) const;
gtsam::Matrix marginalCovariance(size_t variable) const;
gtsam::Matrix marginalInformation(size_t variable) const;
gtsam::JointMarginal jointMarginalCovariance(
const gtsam::KeyVector& variables) const;
gtsam::JointMarginal jointMarginalInformation(
@ -155,8 +155,8 @@ class Marginals {
};
class JointMarginal {
Matrix at(size_t iVariable, size_t jVariable) const;
Matrix fullMatrix() const;
gtsam::Matrix at(size_t iVariable, size_t jVariable) const;
gtsam::Matrix fullMatrix() const;
void print(string s = "", gtsam::KeyFormatter keyFormatter =
gtsam::DefaultKeyFormatter) const;
};
@ -368,10 +368,10 @@ virtual class GncOptimizer {
const gtsam::Values& initialValues,
const PARAMS& params);
void setInlierCostThresholds(const double inth);
const Vector& getInlierCostThresholds();
const gtsam::Vector& getInlierCostThresholds();
void setInlierCostThresholdsAtProbability(const double alpha);
void setWeights(const Vector w);
const Vector& getWeights();
void setWeights(const gtsam::Vector w);
const gtsam::Vector& getWeights();
gtsam::Values optimize();
};
@ -417,7 +417,7 @@ class ISAM2DoglegParams {
};
class ISAM2ThresholdMapValue {
ISAM2ThresholdMapValue(char c, Vector thresholds);
ISAM2ThresholdMapValue(char c, gtsam::Vector thresholds);
ISAM2ThresholdMapValue(const gtsam::ISAM2ThresholdMapValue& other);
};
@ -467,7 +467,7 @@ class ISAM2Clique {
ISAM2Clique();
// Standard Interface
Vector gradientContribution() const;
gtsam::Vector gradientContribution() const;
void print(string s = "",
gtsam::KeyFormatter keyFormatter = gtsam::DefaultKeyFormatter);
};
@ -535,9 +535,9 @@ class ISAM2 {
gtsam::PinholeCamera<gtsam::Cal3_S2>,
gtsam::PinholeCamera<gtsam::Cal3Bundler>,
gtsam::PinholeCamera<gtsam::Cal3Fisheye>,
gtsam::PinholeCamera<gtsam::Cal3Unified>, Vector, Matrix}>
gtsam::PinholeCamera<gtsam::Cal3Unified>, gtsam::Vector, gtsam::Matrix}>
VALUE calculateEstimate(size_t key) const;
Matrix marginalCovariance(size_t key) const;
gtsam::Matrix marginalCovariance(size_t key) const;
gtsam::Values calculateBestEstimate() const;
gtsam::VectorValues getDelta() const;
double error(const gtsam::VectorValues& x) const;
@ -564,7 +564,7 @@ class NonlinearISAM {
void printStats() const;
void saveGraph(string s) const;
gtsam::Values estimate() const;
Matrix marginalCovariance(size_t key) const;
gtsam::Matrix marginalCovariance(size_t key) const;
int reorderInterval() const;
int reorderCounter() const;
void update(const gtsam::NonlinearFactorGraph& newFactors,
@ -583,7 +583,7 @@ class NonlinearISAM {
//*************************************************************************
#include <gtsam/nonlinear/PriorFactor.h>
template <T = {double,
Vector,
gtsam::Vector,
gtsam::Point2,
gtsam::StereoPoint2,
gtsam::Point3,
@ -703,7 +703,7 @@ virtual class BatchFixedLagSmoother : gtsam::FixedLagSmoother {
gtsam::LevenbergMarquardtParams params() const;
template <VALUE = {gtsam::Point2, gtsam::Rot2, gtsam::Pose2, gtsam::Point3,
gtsam::Rot3, gtsam::Pose3, gtsam::Cal3_S2, gtsam::Cal3DS2,
Vector, Matrix}>
gtsam::Vector, gtsam::Matrix}>
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);
// 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.
void insert(size_t j, Vector vector);
void insert(size_t j, Matrix matrix);
void insert(size_t j, gtsam::Vector vector);
void insert(size_t j, gtsam::Matrix matrix);
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::Rot2& rot2);
@ -101,10 +101,10 @@ class Values {
template <T = {gtsam::Point2, gtsam::Point3}>
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.
void update(size_t j, Vector vector);
void update(size_t j, Matrix matrix);
void update(size_t j, gtsam::Vector vector);
void update(size_t j, gtsam::Matrix matrix);
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::Rot2& rot2);
@ -133,10 +133,10 @@ class Values {
void update(size_t j, const gtsam::NavState& nav_state);
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.
void insert_or_assign(size_t j, Vector vector);
void insert_or_assign(size_t j, Matrix matrix);
void insert_or_assign(size_t j, gtsam::Vector vector);
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::Point3& point3);
void insert_or_assign(size_t j, const gtsam::Rot2& rot2);
@ -201,8 +201,8 @@ class Values {
gtsam::PinholePose<gtsam::Cal3Unified>,
gtsam::imuBias::ConstantBias,
gtsam::NavState,
Vector,
Matrix,
gtsam::Vector,
gtsam::Matrix,
double}>
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,
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>
@ -162,23 +162,23 @@ class ShonanAveraging2 {
gtsam::Rot2 measured(size_t i);
gtsam::KeyVector keys(size_t i);
// Matrix API (advanced use, debugging)
Matrix denseD() const;
Matrix denseQ() const;
Matrix denseL() const;
// Matrix computeLambda_(Matrix S) const;
Matrix computeLambda_(const gtsam::Values& values) const;
Matrix computeA_(const gtsam::Values& values) const;
// gtsam::Matrix API (advanced use, debugging)
gtsam::Matrix denseD() const;
gtsam::Matrix denseQ() const;
gtsam::Matrix denseL() const;
// gtsam::Matrix computeLambda_(gtsam::Matrix S) const;
gtsam::Matrix computeLambda_(const gtsam::Values& values) const;
gtsam::Matrix computeA_(const gtsam::Values& values) const;
double computeMinEigenValue(const gtsam::Values& values) const;
gtsam::Values initializeWithDescent(size_t p, const gtsam::Values& values,
const Vector& minEigenVector,
const gtsam::Vector& minEigenVector,
double minEigenValue) const;
// Advanced API
gtsam::NonlinearFactorGraph buildGraphAt(size_t p) const;
gtsam::Values initializeRandomlyAt(size_t p) 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;
gtsam::LevenbergMarquardtOptimizer* createOptimizerAt(
size_t p, const gtsam::Values& initial);
@ -212,23 +212,23 @@ class ShonanAveraging3 {
gtsam::Rot3 measured(size_t i);
gtsam::KeyVector keys(size_t i);
// Matrix API (advanced use, debugging)
Matrix denseD() const;
Matrix denseQ() const;
Matrix denseL() const;
// Matrix computeLambda_(Matrix S) const;
Matrix computeLambda_(const gtsam::Values& values) const;
Matrix computeA_(const gtsam::Values& values) const;
// gtsam::Matrix API (advanced use, debugging)
gtsam::Matrix denseD() const;
gtsam::Matrix denseQ() const;
gtsam::Matrix denseL() const;
// gtsam::Matrix computeLambda_(gtsam::Matrix S) const;
gtsam::Matrix computeLambda_(const gtsam::Values& values) const;
gtsam::Matrix computeA_(const gtsam::Values& values) const;
double computeMinEigenValue(const gtsam::Values& values) const;
gtsam::Values initializeWithDescent(size_t p, const gtsam::Values& values,
const Vector& minEigenVector,
const gtsam::Vector& minEigenVector,
double minEigenValue) const;
// Advanced API
gtsam::NonlinearFactorGraph buildGraphAt(size_t p) const;
gtsam::Values initializeRandomlyAt(size_t p) 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;
gtsam::LevenbergMarquardtOptimizer* createOptimizerAt(
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>
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::imuBias::ConstantBias}>
virtual class BetweenFactor : gtsam::NoiseModelFactor {
@ -227,7 +227,7 @@ virtual class EssentialMatrixFactor : gtsam::NoiseModelFactor {
void print(string s = "", const gtsam::KeyFormatter& keyFormatter =
gtsam::DefaultKeyFormatter) 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>
@ -237,7 +237,7 @@ virtual class EssentialMatrixConstraint : gtsam::NoiseModelFactor {
void print(string s = "", const gtsam::KeyFormatter& keyFormatter =
gtsam::DefaultKeyFormatter) 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;
};
@ -336,7 +336,7 @@ virtual class FrobeniusFactor : gtsam::NoiseModelFactor {
FrobeniusFactor(size_t key1, size_t key2);
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}>
@ -345,7 +345,7 @@ virtual class FrobeniusBetweenFactor : gtsam::NoiseModelFactor {
FrobeniusBetweenFactor(size_t key1, size_t key2, const T& R12,
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>

136
gtsam_unstable/gtsam_unstable.i
View File

@ -54,10 +54,10 @@ class Dummy {
#include <gtsam_unstable/dynamics/PoseRTV.h>
class PoseRTV {
PoseRTV();
PoseRTV(Vector rtv);
PoseRTV(const gtsam::Point3& pt, const gtsam::Rot3& rot, const Vector& vel);
PoseRTV(const gtsam::Rot3& rot, const gtsam::Point3& pt, const Vector& vel);
PoseRTV(const gtsam::Pose3& pose, const Vector& vel);
PoseRTV(gtsam::Vector rtv);
PoseRTV(const gtsam::Point3& pt, const gtsam::Rot3& rot, const gtsam::Vector& vel);
PoseRTV(const gtsam::Rot3& rot, const gtsam::Point3& pt, const gtsam::Vector& vel);
PoseRTV(const gtsam::Pose3& pose, const gtsam::Vector& vel);
PoseRTV(const gtsam::Pose3& pose);
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
gtsam::Point3 translation() const;
gtsam::Rot3 rotation() const;
Vector velocity() const;
gtsam::Vector velocity() const;
gtsam::Pose3 pose() const;
// Vector interfaces
Vector vector() const;
Vector translationVec() const;
Vector velocityVec() const;
// gtsam::Vector interfaces
gtsam::Vector vector() const;
gtsam::Vector translationVec() const;
gtsam::Vector velocityVec() const;
// manifold/Lie
static size_t Dim();
size_t dim() const;
gtsam::PoseRTV retract(Vector v) const;
Vector localCoordinates(const gtsam::PoseRTV& p) const;
static gtsam::PoseRTV Expmap(Vector v);
static Vector Logmap(const gtsam::PoseRTV& p);
gtsam::PoseRTV retract(gtsam::Vector v) const;
gtsam::Vector localCoordinates(const gtsam::PoseRTV& p) const;
static gtsam::PoseRTV Expmap(gtsam::Vector v);
static gtsam::Vector Logmap(const gtsam::PoseRTV& p);
gtsam::PoseRTV inverse() const;
gtsam::PoseRTV compose(const gtsam::PoseRTV& p) const;
gtsam::PoseRTV between(const gtsam::PoseRTV& p) const;
@ -94,10 +94,10 @@ class PoseRTV {
// IMU/dynamics
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 generalDynamics(Vector accel, Vector gyro, double dt) const;
Vector imuPrediction(const gtsam::PoseRTV& x2, double dt) const;
gtsam::PoseRTV generalDynamics(gtsam::Vector accel, gtsam::Vector gyro, double dt) const;
gtsam::Vector imuPrediction(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
};
@ -126,16 +126,16 @@ class Pose3Upright {
gtsam::Pose3 pose() const;
size_t dim() const;
gtsam::Pose3Upright retract(Vector v) const;
Vector localCoordinates(const gtsam::Pose3Upright& p2) const;
gtsam::Pose3Upright retract(gtsam::Vector v) const;
gtsam::Vector localCoordinates(const gtsam::Pose3Upright& p2) const;
static gtsam::Pose3Upright Identity();
gtsam::Pose3Upright inverse() const;
gtsam::Pose3Upright compose(const gtsam::Pose3Upright& p2) const;
gtsam::Pose3Upright between(const gtsam::Pose3Upright& p2) const;
static gtsam::Pose3Upright Expmap(Vector xi);
static Vector Logmap(const gtsam::Pose3Upright& p);
static gtsam::Pose3Upright Expmap(gtsam::Vector xi);
static gtsam::Vector Logmap(const gtsam::Pose3Upright& p);
void serializable() const; // enabling serialization functionality
}; // \class Pose3Upright
@ -156,8 +156,8 @@ class BearingS2 {
bool equals(const gtsam::BearingS2& x, double tol) const;
size_t dim() const;
gtsam::BearingS2 retract(Vector v) const;
Vector localCoordinates(const gtsam::BearingS2& p2) const;
gtsam::BearingS2 retract(gtsam::Vector v) const;
gtsam::Vector localCoordinates(const gtsam::BearingS2& p2) const;
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,
double prior_inlier, double prior_outlier, bool flag_bump_up_near_zero_probs);
Vector whitenedError(const gtsam::Values& x);
Vector unwhitenedError(const gtsam::Values& x);
Vector calcIndicatorProb(const gtsam::Values& x);
gtsam::Vector whitenedError(const gtsam::Values& x);
gtsam::Vector unwhitenedError(const gtsam::Values& x);
gtsam::Vector calcIndicatorProb(const gtsam::Values& x);
gtsam::Pose2 measured(); // TODO: figure out how to output a template instead
void set_flag_bump_up_near_zero_probs(bool flag);
bool get_flag_bump_up_near_zero_probs() const;
void updateNoiseModels(const gtsam::Values& values, const gtsam::NonlinearFactorGraph& graph);
void updateNoiseModels_givenCovs(const gtsam::Values& values, Matrix cov1, Matrix cov2, Matrix cov12);
Matrix get_model_inlier_cov();
Matrix get_model_outlier_cov();
void updateNoiseModels_givenCovs(const gtsam::Values& values, gtsam::Matrix cov1, gtsam::Matrix cov2, gtsam::Matrix cov12);
gtsam::Matrix get_model_inlier_cov();
gtsam::Matrix get_model_outlier_cov();
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,
double prior_inlier, double prior_outlier, bool flag_bump_up_near_zero_probs, bool start_with_M_step);
Vector whitenedError(const gtsam::Values& x);
Vector unwhitenedError(const gtsam::Values& x);
Vector calcIndicatorProb(const gtsam::Values& x);
gtsam::Vector whitenedError(const gtsam::Values& x);
gtsam::Vector unwhitenedError(const gtsam::Values& x);
gtsam::Vector calcIndicatorProb(const gtsam::Values& x);
void setValAValB(const gtsam::Values valA, const gtsam::Values valB);
void updateNoiseModels(const gtsam::Values& values, const gtsam::NonlinearFactorGraph& graph);
void updateNoiseModels_givenCovs(const gtsam::Values& values, Matrix cov1, Matrix cov2, Matrix cov12);
Matrix get_model_inlier_cov();
Matrix get_model_outlier_cov();
void updateNoiseModels_givenCovs(const gtsam::Values& values, gtsam::Matrix cov1, gtsam::Matrix cov2, gtsam::Matrix cov12);
gtsam::Matrix get_model_inlier_cov();
gtsam::Matrix get_model_outlier_cov();
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::noiseModel::Gaussian* model);
Vector whitenedError(const gtsam::Values& x);
Vector unwhitenedError(const gtsam::Values& x);
gtsam::Vector whitenedError(const gtsam::Values& x);
gtsam::Vector unwhitenedError(const gtsam::Values& x);
void setValAValB(const gtsam::Values valA, const gtsam::Values valB);
void serializable() const; // enabling serialization functionality
@ -419,16 +419,16 @@ virtual class NonlinearEquality : gtsam::NoiseModelFactor {
template<POSE = {gtsam::PoseRTV}>
virtual class IMUFactor : gtsam::NoiseModelFactor {
/** 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);
/** 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);
Vector gyro() const;
Vector accel() const;
Vector z() const;
gtsam::Vector gyro() const;
gtsam::Vector accel() const;
gtsam::Vector z() const;
template <I = {1, 2}>
size_t key() const;
@ -438,16 +438,16 @@ virtual class IMUFactor : gtsam::NoiseModelFactor {
template<POSE = {gtsam::PoseRTV}>
virtual class FullIMUFactor : gtsam::NoiseModelFactor {
/** 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);
/** 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);
Vector gyro() const;
Vector accel() const;
Vector z() const;
gtsam::Vector gyro() const;
gtsam::Vector accel() const;
gtsam::Vector z() const;
template <I = {1, 2}>
size_t key() const;
@ -466,14 +466,14 @@ virtual class DRollPrior : 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 {
// Primary constructor allows for variable height of the "floor"
DGroundConstraint(size_t key, double height, const gtsam::noiseModel::Base* model);
// 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>
@ -481,7 +481,7 @@ virtual class VelocityConstraint3 : gtsam::NonlinearFactor {
/** Standard constructor */
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>
@ -489,7 +489,7 @@ virtual class PendulumFactor1 : gtsam::NonlinearFactor {
/** Standard constructor */
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>
@ -497,35 +497,35 @@ virtual class PendulumFactor2 : gtsam::NonlinearFactor {
/** Standard constructor */
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 {
/** Standard constructor */
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 {
/** Standard constructor */
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>
virtual class Reconstruction : gtsam::NoiseModelFactor {
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 {
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>
class Mechanization_bRn2 {
Mechanization_bRn2();
Mechanization_bRn2(gtsam::Rot3& initial_bRn, Vector initial_x_g,
Vector initial_x_a);
Vector b_g(double g_e);
Mechanization_bRn2(gtsam::Rot3& initial_bRn, gtsam::Vector initial_x_g,
gtsam::Vector initial_x_a);
gtsam::Vector b_g(double g_e);
gtsam::Rot3 bRn();
Vector x_g();
Vector x_a();
static gtsam::Mechanization_bRn2 initialize(Matrix U, Matrix F, double g_e);
gtsam::Mechanization_bRn2 correct(Vector dx) const;
gtsam::Mechanization_bRn2 integrate(Vector u, double dt) const;
gtsam::Vector x_g();
gtsam::Vector x_a();
static gtsam::Mechanization_bRn2 initialize(gtsam::Matrix U, gtsam::Matrix F, double g_e);
gtsam::Mechanization_bRn2 correct(gtsam::Vector dx) const;
gtsam::Mechanization_bRn2 integrate(gtsam::Vector u, double dt) const;
//void print(string s) const;
};
#include <gtsam_unstable/slam/AHRS.h>
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*> integrate(const gtsam::Mechanization_bRn2& mech, gtsam::GaussianDensity* state, 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*> aidGeneral(const gtsam::Mechanization_bRn2& mech, gtsam::GaussianDensity* state, Vector f, Vector f_expected, const gtsam::Rot3& increment);
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, gtsam::Vector f, bool Farrel);
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;
};