Merge branch 'develop' into feature/smartFactorsWithExtrinsicCalibration

borglab · Apr 7, 2021 · 14f8998 · 14f8998
2 parents 0a08c19 + d2549c6
commit 14f8998
Show file tree

Hide file tree

Showing 41 changed files with 1,163 additions and 400 deletions.
diff --git a/.github/workflows/build-python.yml b/.github/workflows/build-python.yml
@@ -19,7 +19,7 @@ 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: [
-          ubuntu-18.04-gcc-5,
+          # ubuntu-18.04-gcc-5,  #TODO Enable once the Python wrapper is optimized for memory
           ubuntu-18.04-gcc-9,
           ubuntu-18.04-clang-9,
           macOS-10.15-xcode-11.3.1,
@@ -31,10 +31,10 @@ jobs:
         build_type: [Release]
         python_version: [3]
         include:
-          - name: ubuntu-18.04-gcc-5
-            os: ubuntu-18.04
-            compiler: gcc
-            version: "5"
+          # - name: ubuntu-18.04-gcc-5
+          #   os: ubuntu-18.04
+          #   compiler: gcc
+          #   version: "5"
 
           - name: ubuntu-18.04-gcc-9
             os: ubuntu-18.04

diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i
@@ -266,7 +266,6 @@ class Point2 {
 };
 
 // std::vector<gtsam::Point2>
-#include <gtsam/geometry/Point2.h>
 class Point2Vector
 {
   // Constructors
@@ -308,6 +307,12 @@ class StereoPoint2 {
   gtsam::StereoPoint2 compose(const gtsam::StereoPoint2& p2) const;
   gtsam::StereoPoint2 between(const gtsam::StereoPoint2& p2) const;
 
+  // Operator Overloads
+  gtsam::StereoPoint2 operator-() const;
+  // gtsam::StereoPoint2 operator+(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;
@@ -356,7 +361,6 @@ class Point3 {
   void pickle() const;
 };
 
-#include <gtsam/geometry/Point3.h>
 class Point3Pairs {
   Point3Pairs();
   size_t size() const;
@@ -384,6 +388,9 @@ class Rot2 {
   gtsam::Rot2 compose(const gtsam::Rot2& p2) const;
   gtsam::Rot2 between(const gtsam::Rot2& p2) const;
 
+  // Operator Overloads
+  gtsam::Rot2 operator*(const gtsam::Rot2& p2) const;
+
   // Manifold
   gtsam::Rot2 retract(Vector v) const;
   Vector localCoordinates(const gtsam::Rot2& p) const;
@@ -432,6 +439,9 @@ class SO3 {
   gtsam::SO3 between(const gtsam::SO3& R) const;
   gtsam::SO3 compose(const gtsam::SO3& R) const;
 
+  // Operator Overloads
+  gtsam::SO3 operator*(const gtsam::SO3& R) const;
+
   // Manifold
   gtsam::SO3 retract(Vector v) const;
   Vector localCoordinates(const gtsam::SO3& R) const;
@@ -459,6 +469,9 @@ class SO4 {
   gtsam::SO4 between(const gtsam::SO4& Q) const;
   gtsam::SO4 compose(const gtsam::SO4& Q) const;
 
+  // Operator Overloads
+  gtsam::SO4 operator*(const gtsam::SO4& Q) const;
+
   // Manifold
   gtsam::SO4 retract(Vector v) const;
   Vector localCoordinates(const gtsam::SO4& Q) const;
@@ -486,6 +499,9 @@ class SOn {
   gtsam::SOn between(const gtsam::SOn& Q) const;
   gtsam::SOn compose(const gtsam::SOn& Q) const;
 
+  // Operator Overloads
+  gtsam::SOn operator*(const gtsam::SOn& Q) const;
+
   // Manifold
   gtsam::SOn retract(Vector v) const;
   Vector localCoordinates(const gtsam::SOn& Q) const;
@@ -544,6 +560,9 @@ class Rot3 {
   gtsam::Rot3 compose(const gtsam::Rot3& p2) const;
   gtsam::Rot3 between(const gtsam::Rot3& p2) const;
 
+  // Operator Overloads
+  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;
@@ -598,6 +617,9 @@ class Pose2 {
   gtsam::Pose2 compose(const gtsam::Pose2& p2) const;
   gtsam::Pose2 between(const gtsam::Pose2& p2) const;
 
+  // Operator Overloads
+  gtsam::Pose2 operator*(const gtsam::Pose2& p2) const;
+
   // Manifold
   gtsam::Pose2 retract(Vector v) const;
   Vector localCoordinates(const gtsam::Pose2& p) const;
@@ -655,6 +677,9 @@ class Pose3 {
   gtsam::Pose3 compose(const gtsam::Pose3& pose) const;
   gtsam::Pose3 between(const gtsam::Pose3& pose) const;
 
+  // Operator Overloads
+  gtsam::Pose3 operator*(const gtsam::Pose3& pose) const;
+
   // Manifold
   gtsam::Pose3 retract(Vector v) const;
   Vector localCoordinates(const gtsam::Pose3& pose) const;
@@ -695,7 +720,6 @@ class Pose3 {
   void pickle() const;
 };
 
-#include <gtsam/geometry/Pose3.h>
 class Pose3Pairs {
   Pose3Pairs();
   size_t size() const;
@@ -704,8 +728,6 @@ class Pose3Pairs {
   void push_back(const gtsam::Pose3Pair& pose_pair);
 };
 
-// std::vector<gtsam::Pose3>
-#include <gtsam/geometry/Pose3.h>
 class Pose3Vector
 {
   Pose3Vector();
@@ -976,7 +998,9 @@ class CalibratedCamera {
 
   // Standard Interface
   gtsam::Pose3 pose() const;
-  double range(const gtsam::Point3& p) const; // TODO: Other overloaded range methods
+  double range(const gtsam::Point3& point) const;
+  double range(const gtsam::Pose3& pose) const;
+  double range(const gtsam::CalibratedCamera& camera) const;
 
   // enabling serialization functionality
   void serialize() const;
@@ -1048,7 +1072,6 @@ class Similarity3 {
 };
 
 
-
 // Forward declaration of PinholeCameraCalX is defined here.
 #include <gtsam/geometry/SimpleCamera.h>
 // Some typedefs for common camera types
@@ -1251,9 +1274,9 @@ class SymbolicBayesTree {
 };
 
 // class SymbolicBayesTreeClique {
-//   BayesTreeClique();
-//   BayesTreeClique(CONDITIONAL* conditional);
-// //  BayesTreeClique(const pair<typename ConditionalType::shared_ptr, typename ConditionalType::FactorType::shared_ptr>& result) : Base(result) {}
+//   SymbolicBayesTreeClique();
+//   SymbolicBayesTreeClique(CONDITIONAL* conditional);
+//   SymbolicBayesTreeClique(const pair<typename ConditionalType::shared_ptr, typename ConditionalType::FactorType::shared_ptr>& result) : Base(result) {}
 //
 //   bool equals(const This& other, double tol) const;
 //   void print(string s) const;
@@ -1264,13 +1287,13 @@ class SymbolicBayesTree {
 //   CONDITIONAL* conditional() const;
 //   bool isRoot() const;
 //   size_t treeSize() const;
-// //  const std::list<derived_ptr>& children() const { return children_; }
-// //  derived_ptr parent() const { return parent_.lock(); }
+//  const std::list<derived_ptr>& children() const { return children_; }
+//  derived_ptr parent() const { return parent_.lock(); }
 //
 //   // TODO: need wrapped versions graphs, BayesNet
-// //  BayesNet<ConditionalType> shortcut(derived_ptr root, Eliminate function) const;
-// //  FactorGraph<FactorType> marginal(derived_ptr root, Eliminate function) const;
-// //  FactorGraph<FactorType> joint(derived_ptr C2, derived_ptr root, Eliminate function) const;
+//  BayesNet<ConditionalType> shortcut(derived_ptr root, Eliminate function) const;
+//  FactorGraph<FactorType> marginal(derived_ptr root, Eliminate function) const;
+//  FactorGraph<FactorType> joint(derived_ptr C2, derived_ptr root, Eliminate function) const;
 //
 //   void deleteCachedShortcuts();
 // };
@@ -2068,8 +2091,14 @@ class NonlinearFactorGraph {
   gtsam::KeySet keys() const;
   gtsam::KeyVector keyVector() const;
 
-  template<T = {Vector, gtsam::Point2, gtsam::StereoPoint2, gtsam::Point3, gtsam::Rot2, gtsam::SO3, gtsam::SO4, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::Cal3_S2,gtsam::CalibratedCamera, gtsam::PinholeCameraCal3_S2, gtsam::PinholeCamera<gtsam::Cal3Bundler>, gtsam::imuBias::ConstantBias}>
-  void addPrior(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel);
+  template <T = {double, Vector, gtsam::Point2, gtsam::StereoPoint2,
+                 gtsam::Point3, gtsam::Rot2, gtsam::SO3, gtsam::SO4,
+                 gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::Cal3_S2,
+                 gtsam::CalibratedCamera, gtsam::PinholeCameraCal3_S2,
+                 gtsam::PinholeCamera<gtsam::Cal3Bundler>,
+                 gtsam::imuBias::ConstantBias}>
+  void addPrior(size_t key, const T& prior,
+                const gtsam::noiseModel::Base* noiseModel);
 
   // NonlinearFactorGraph
   void printErrors(const gtsam::Values& values) const;
@@ -2538,7 +2567,26 @@ class NonlinearISAM {
 #include <gtsam/geometry/StereoPoint2.h>
 
 #include <gtsam/nonlinear/PriorFactor.h>
-template<T = {Vector, gtsam::Point2, gtsam::StereoPoint2, gtsam::Point3, gtsam::Rot2, gtsam::SO3, gtsam::SO4, gtsam::SOn, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::Unit3, gtsam::Cal3_S2, gtsam::Cal3DS2, gtsam::Cal3Bundler, gtsam::CalibratedCamera, gtsam::PinholeCameraCal3_S2, gtsam::imuBias::ConstantBias, gtsam::PinholeCamera<gtsam::Cal3Bundler>}>
+template <T = {double,
+               Vector,
+               gtsam::Point2,
+               gtsam::StereoPoint2,
+               gtsam::Point3,
+               gtsam::Rot2,
+               gtsam::SO3,
+               gtsam::SO4,
+               gtsam::SOn,
+               gtsam::Rot3,
+               gtsam::Pose2,
+               gtsam::Pose3,
+               gtsam::Unit3,
+               gtsam::Cal3_S2,
+               gtsam::Cal3DS2,
+               gtsam::Cal3Bundler,
+               gtsam::CalibratedCamera,
+               gtsam::PinholeCameraCal3_S2,
+               gtsam::imuBias::ConstantBias,
+               gtsam::PinholeCamera<gtsam::Cal3Bundler>}>
 virtual class PriorFactor : gtsam::NoiseModelFactor {
   PriorFactor(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel);
   T prior() const;
@@ -2550,7 +2598,6 @@ virtual class PriorFactor : gtsam::NoiseModelFactor {
   void pickle() const;
 };
 
-
 #include <gtsam/slam/BetweenFactor.h>
 template<T = {Vector, gtsam::Point2, gtsam::Point3, gtsam::Rot2, gtsam::SO3, gtsam::SO4, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::imuBias::ConstantBias}>
 virtual class BetweenFactor : gtsam::NoiseModelFactor {
@@ -2734,7 +2781,7 @@ virtual class SmartProjectionPoseFactor: gtsam::NonlinearFactor {
   void add(const gtsam::Point2& measured_i, size_t poseKey_i);
 
   // enabling serialization functionality
-  //void serialize() const;
+  void serialize() const;
 };
 
 typedef gtsam::SmartProjectionPoseFactor<gtsam::Cal3_S2> SmartProjectionPose3Factor;
@@ -3040,7 +3087,7 @@ class ShonanAveraging3 {
   ShonanAveraging3(string g2oFile);
   ShonanAveraging3(string g2oFile,
                    const gtsam::ShonanAveragingParameters3 &parameters);
-  
+
   // TODO(frank): deprecate once we land pybind wrapper
   ShonanAveraging3(const gtsam::BetweenFactorPose3s &factors);
   ShonanAveraging3(const gtsam::BetweenFactorPose3s &factors,
@@ -3132,6 +3179,11 @@ class ConstantBias {
   gtsam::imuBias::ConstantBias compose(const gtsam::imuBias::ConstantBias& b) const;
   gtsam::imuBias::ConstantBias between(const gtsam::imuBias::ConstantBias& b) const;
 
+  // Operator Overloads
+  gtsam::imuBias::ConstantBias operator-() const;
+  gtsam::imuBias::ConstantBias operator+(const gtsam::imuBias::ConstantBias& b) const;
+  gtsam::imuBias::ConstantBias operator-(const gtsam::imuBias::ConstantBias& b) const;
+
   // Manifold
   gtsam::imuBias::ConstantBias retract(Vector v) const;
   Vector localCoordinates(const gtsam::imuBias::ConstantBias& b) const;
@@ -3166,6 +3218,9 @@ class NavState {
   gtsam::Point3 position() const;
   Vector velocity() const;
   gtsam::Pose3 pose() const;
+
+  gtsam::NavState retract(const Vector& x) const;
+  Vector localCoordinates(const gtsam::NavState& g) const;
 };
 
 #include <gtsam/navigation/PreintegratedRotation.h>
@@ -3182,9 +3237,8 @@ virtual class PreintegratedRotationParams {
 
   Matrix getGyroscopeCovariance() const;
 
-  // TODO(frank): allow optional
-  //  boost::optional<Vector> getOmegaCoriolis() const;
-  //  boost::optional<Pose3>   getBodyPSensor()   const;
+  boost::optional<Vector> getOmegaCoriolis() const;
+  boost::optional<gtsam::Pose3> getBodyPSensor() const;
 };
 
 #include <gtsam/navigation/PreintegrationParams.h>

diff --git a/wrap/.gitignore b/wrap/.gitignore
@@ -1,6 +1,7 @@
 __pycache__/
 .vscode/
 *build*
+*install*
 *dist*
 *.egg-info