diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i index 01161817b..3cf78989c 100644 --- a/gtsam/geometry/geometry.i +++ b/gtsam/geometry/geometry.i @@ -69,6 +69,7 @@ class StereoPoint2 { // Standard Constructors StereoPoint2(); StereoPoint2(double uL, double uR, double v); + StereoPoint2(const gtsam::Vector3 &v); // Testable void print(string s = "") const; @@ -836,6 +837,12 @@ class Cal3_S2Stereo { Cal3_S2Stereo(double fx, double fy, double s, double u0, double v0, double b); Cal3_S2Stereo(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; + // Testable void print(string s = "") const; bool equals(const gtsam::Cal3_S2Stereo& K, double tol) const; @@ -846,8 +853,11 @@ class Cal3_S2Stereo { double skew() const; double px() const; double py() const; + Matrix K() const; gtsam::Point2 principalPoint() const; double baseline() const; + Vector6 vector() const; + Matrix inverse() const; }; #include diff --git a/gtsam/navigation/navigation.i b/gtsam/navigation/navigation.i index 92864c18a..aad6f9851 100644 --- a/gtsam/navigation/navigation.i +++ b/gtsam/navigation/navigation.i @@ -200,7 +200,7 @@ class PreintegratedCombinedMeasurements { const gtsam::imuBias::ConstantBias& bias) const; }; -virtual class CombinedImuFactor: gtsam::NonlinearFactor { +virtual class CombinedImuFactor: gtsam::NoiseModelFactor { CombinedImuFactor(size_t pose_i, size_t vel_i, size_t pose_j, size_t vel_j, size_t bias_i, size_t bias_j, const gtsam::PreintegratedCombinedMeasurements& CombinedPreintegratedMeasurements); diff --git a/gtsam/nonlinear/nonlinear.i b/gtsam/nonlinear/nonlinear.i index 3f5fb1dd5..c39cfd22a 100644 --- a/gtsam/nonlinear/nonlinear.i +++ b/gtsam/nonlinear/nonlinear.i @@ -130,6 +130,7 @@ virtual class NonlinearFactor : gtsam::Factor { 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; }; @@ -320,6 +321,8 @@ virtual class GncParams { enum Verbosity { SILENT, SUMMARY, + MU, + WEIGHTS, VALUES }; }; diff --git a/gtsam/slam/slam.i b/gtsam/slam/slam.i index 64977a2a5..8dc10e51c 100644 --- a/gtsam/slam/slam.i +++ b/gtsam/slam/slam.i @@ -170,7 +170,8 @@ template virtual class GenericStereoFactor : gtsam::NoiseModelFactor { GenericStereoFactor(const gtsam::StereoPoint2& measured, const gtsam::noiseModel::Base* noiseModel, size_t poseKey, - size_t landmarkKey, const gtsam::Cal3_S2Stereo* K); + size_t landmarkKey, const gtsam::Cal3_S2Stereo* K, + POSE body_P_sensor); gtsam::StereoPoint2 measured() const; gtsam::Cal3_S2Stereo* calibration() const;