Add SphericalCamera Python bindings

gtsam/geometry/geometry.i

@@ -1472,6 +1472,43 @@ typedef gtsam::PinholePose<gtsam::Cal3Unified> PinholePoseCal3Unified;
 typedef gtsam::PinholePose<gtsam::Cal3Bundler> PinholePoseCal3Bundler;
 typedef gtsam::PinholePose<gtsam::Cal3Fisheye> PinholePoseCal3Fisheye;
 
+#include <gtsam/geometry/SphericalCamera.h>
+class SphericalCamera {
+  // Standard Constructors
+  SphericalCamera();
+  SphericalCamera(const gtsam::Pose3& pose);
+  SphericalCamera(const gtsam::Pose3& pose,
+                  const gtsam::EmptyCal::shared_ptr& cal);
+  SphericalCamera(const gtsam::Vector& v);
+
+  // Testable
+  bool equals(const gtsam::SphericalCamera& camera, double tol = 1e-9) const;
+  void print(const std::string& s = "SphericalCamera") const;
+
+  // Standard Interface
+  gtsam::Pose3 pose() const;
+  const gtsam::Rot3& rotation() const;
+  const gtsam::Point3& translation() const;
+
+  // Transformations and measurement functions
+  pair<gtsam::Unit3, bool> projectSafe(const gtsam::Point3& pw) const;
+  gtsam::Unit3 project(const gtsam::Point3& point) const;
+  gtsam::Unit3 project2(const gtsam::Point3& point) const;
+  gtsam::Unit3 project2(const gtsam::Unit3& pwu) const;
+  gtsam::Point3 backproject(const gtsam::Unit3& p, double depth) const;
+  gtsam::Unit3 backprojectPointAtInfinity(const gtsam::Unit3& p) const;
+  gtsam::Vector2 reprojectionError(const gtsam::Point3& point,
+                                   const gtsam::Unit3& measured) const;
+
+  // Manifold
+  gtsam::SphericalCamera retract(const gtsam::Vector6& d) const;
+  gtsam::Vector6 localCoordinates(const gtsam::SphericalCamera& p) const;
+  static gtsam::SphericalCamera Identity();
+
+  // enabling serialization functionality
+  void serialize() const;
+};
+
 #include <gtsam/geometry/Similarity2.h>
 class Similarity2 {
   // Standard Constructors
@@ -1851,6 +1888,22 @@ gtsam::TriangulationResult triangulateSafe(
     const gtsam::Point2Vector& measurements,
     const gtsam::TriangulationParameters& params);
 
+// Spherical versions
+gtsam::Point3 triangulatePoint3(
+    const gtsam::CameraSet<gtsam::SphericalCamera>& cameras,
+    const gtsam::SphericalCamera::MeasurementVector& measurements,
+    double rank_tol, bool optimize,
+    const gtsam::SharedNoiseModel& model = nullptr,
+    const bool useLOST = false);
+gtsam::Point3 triangulateNonlinear(
+    const gtsam::CameraSet<gtsam::SphericalCamera>& cameras,
+    const gtsam::SphericalCamera::MeasurementVector& measurements,
+    const gtsam::Point3& initialEstimate);
+gtsam::TriangulationResult triangulateSafe(
+    const gtsam::CameraSet<gtsam::SphericalCamera>& cameras,
+    const gtsam::SphericalCamera::MeasurementVector& measurements,
+    const gtsam::TriangulationParameters& params);
+
 
 #include <gtsam/geometry/BearingRange.h>
 template <POSE, POINT, BEARING, RANGE>

gtsam/nonlinear/nonlinear.i

@@ -20,6 +20,7 @@ namespace gtsam {
 #include <gtsam/geometry/Pose3.h>
 #include <gtsam/geometry/Similarity2.h>
 #include <gtsam/geometry/Similarity3.h>
+#include <gtsam/geometry/SphericalCamera.h>
 #include <gtsam/geometry/Rot2.h>
 #include <gtsam/geometry/Rot3.h>
 #include <gtsam/geometry/SO3.h>
@@ -630,6 +631,7 @@ template <T = {double,
                gtsam::PinholeCamera<gtsam::Cal3Bundler>,
                gtsam::PinholeCamera<gtsam::Cal3Fisheye>,
                gtsam::PinholeCamera<gtsam::Cal3Unified>,
+               gtsam::SphericalCamera,
                gtsam::NavState,
                gtsam::imuBias::ConstantBias}>
 virtual class PriorFactor : gtsam::NoiseModelFactor {
@@ -738,6 +740,7 @@ template <T = {gtsam::Point2, gtsam::StereoPoint2, gtsam::Point3, gtsam::Rot2,
                gtsam::PinholeCamera<gtsam::Cal3Bundler>,
                gtsam::PinholeCamera<gtsam::Cal3Fisheye>,
                gtsam::PinholeCamera<gtsam::Cal3Unified>,
+               gtsam::SphericalCamera,
                gtsam::imuBias::ConstantBias}>
 virtual class NonlinearEquality : gtsam::NoiseModelFactor {
   // Constructor - forces exact evaluation

gtsam/nonlinear/values.i

@@ -30,6 +30,7 @@ namespace gtsam {
 #include <gtsam/geometry/SL4.h>
 #include <gtsam/geometry/StereoPoint2.h>
 #include <gtsam/geometry/Unit3.h>
+#include <gtsam/geometry/SphericalCamera.h>
 #include <gtsam/navigation/ImuBias.h>
 #include <gtsam/navigation/NavState.h>
 
@@ -113,6 +114,7 @@ class Values {
   void insert(gtsam::Key j, const gtsam::PinholePose<gtsam::Cal3DS2>& camera);
   void insert(gtsam::Key j, const gtsam::PinholePose<gtsam::Cal3Fisheye>& camera);
   void insert(gtsam::Key j, const gtsam::PinholePose<gtsam::Cal3Unified>& camera);
+  void insert(gtsam::Key j, const gtsam::SphericalCamera& camera);
   void insert(gtsam::Key j, const gtsam::imuBias::ConstantBias& constant_bias);
   void insert(gtsam::Key j, const gtsam::NavState& nav_state);
   void insert(gtsam::Key j, double c);
@@ -157,6 +159,7 @@ class Values {
   void update(gtsam::Key j, const gtsam::PinholePose<gtsam::Cal3DS2>& camera);
   void update(gtsam::Key j, const gtsam::PinholePose<gtsam::Cal3Fisheye>& camera);
   void update(gtsam::Key j, const gtsam::PinholePose<gtsam::Cal3Unified>& camera);
+  void update(gtsam::Key j, const gtsam::SphericalCamera& camera);
   void update(gtsam::Key j, const gtsam::imuBias::ConstantBias& constant_bias);
   void update(gtsam::Key j, const gtsam::NavState& nav_state);
   void update(gtsam::Key j, double c);
@@ -201,6 +204,7 @@ class Values {
   void insert_or_assign(gtsam::Key j, const gtsam::PinholePose<gtsam::Cal3DS2>& camera);
   void insert_or_assign(gtsam::Key j, const gtsam::PinholePose<gtsam::Cal3Fisheye>& camera);
   void insert_or_assign(gtsam::Key j, const gtsam::PinholePose<gtsam::Cal3Unified>& camera);
+  void insert_or_assign(gtsam::Key j, const gtsam::SphericalCamera& camera);
   void insert_or_assign(gtsam::Key j, const gtsam::imuBias::ConstantBias& constant_bias);
   void insert_or_assign(gtsam::Key j, const gtsam::NavState& nav_state);
   void insert_or_assign(gtsam::Key j, double c);
@@ -243,6 +247,7 @@ class Values {
                  gtsam::PinholePose<gtsam::Cal3DS2>,
                  gtsam::PinholePose<gtsam::Cal3Fisheye>,
                  gtsam::PinholePose<gtsam::Cal3Unified>,
+                 gtsam::SphericalCamera,
                  gtsam::imuBias::ConstantBias,
                  gtsam::NavState,
                  double}>

gtsam/slam/GeneralSFMFactor.h

@@ -42,6 +42,7 @@
 #endif
 #include <iostream>
 #include <string>
+#include <type_traits>
 
 namespace boost {
 namespace serialization {
@@ -62,14 +63,16 @@ class GeneralSFMFactor: public NoiseModelFactorN<CAMERA, LANDMARK> {
   GTSAM_CONCEPT_MANIFOLD_TYPE(CAMERA)
   GTSAM_CONCEPT_MANIFOLD_TYPE(LANDMARK)
 
+  using Measurement = typename CAMERA::Measurement;
   static const int DimC = FixedDimension<CAMERA>::value;
   static const int DimL = FixedDimension<LANDMARK>::value;
-  typedef Eigen::Matrix<double, 2, DimC> JacobianC;
-  typedef Eigen::Matrix<double, 2, DimL> JacobianL;
+  static const int ZDim = traits<Measurement>::dimension;
+  typedef Eigen::Matrix<double, ZDim, DimC> JacobianC;
+  typedef Eigen::Matrix<double, ZDim, DimL> JacobianL;
 
 protected:
 
-  Point2 measured_; ///< the 2D measurement
+  Measurement measured_; ///< the measurement
 
 public:
 
@@ -89,14 +92,18 @@ class GeneralSFMFactor: public NoiseModelFactorN<CAMERA, LANDMARK> {
    * @param cameraKey is the index of the camera
    * @param landmarkKey is the index of the landmark
    */
-  GeneralSFMFactor(const Point2& measured, const SharedNoiseModel& model,
+  GeneralSFMFactor(const Measurement& measured, const SharedNoiseModel& model,
                    Key cameraKey, Key landmarkKey)
       : Base(model, cameraKey, landmarkKey), measured_(measured) {}
 
-  GeneralSFMFactor() : measured_(0.0, 0.0) {}  ///< default constructor
-  ///< constructor that takes a Point2
+  GeneralSFMFactor() : measured_(Measurement()) {}  ///< default constructor
+  ///< constructor that takes a Point2 (only enabled for Point2 measurements)
+  template <typename M = Measurement,
+            typename std::enable_if<std::is_same<M, Point2>::value, int>::type = 0>
   GeneralSFMFactor(const Point2& p) : measured_(p) {}
   ///< constructor that takes doubles x,y to make a Point2
+  template <typename M = Measurement,
+            typename std::enable_if<std::is_same<M, Point2>::value, int>::type = 0>
   GeneralSFMFactor(double x, double y) : measured_(x, y) {}
 
   ~GeneralSFMFactor() override {} ///< destructor
@@ -113,28 +120,29 @@ class GeneralSFMFactor: public NoiseModelFactorN<CAMERA, LANDMARK> {
    */
   void print(const std::string& s = "SFMFactor", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
     Base::print(s, keyFormatter);
-    traits<Point2>::Print(measured_, s + ".z");
+    traits<Measurement>::Print(measured_, s + ".z");
   }
 
   /**
    * equals
    */
   bool equals(const NonlinearFactor &p, double tol = 1e-9) const override {
     const This* e = dynamic_cast<const This*>(&p);
-    return e && Base::equals(p, tol) && traits<Point2>::Equals(this->measured_, e->measured_, tol);
+    return e && Base::equals(p, tol) &&
+           traits<Measurement>::Equals(this->measured_, e->measured_, tol);
   }
 
   /** h(x)-z */
   Vector evaluateError(const CAMERA& camera, const LANDMARK& point,
       OptionalMatrixType H1, OptionalMatrixType H2) const override {
     try {
-      return camera.project2(point,H1,H2) - measured_;
-    }
-    catch( CheiralityException& e [[maybe_unused]]) {
+      Measurement predicted = camera.project2(point, H1, H2);
+      return traits<Measurement>::Local(measured_, predicted);
+    } catch (CheiralityException& e [[maybe_unused]]) {
       if (H1) *H1 = JacobianC::Zero();
       if (H2) *H2 = JacobianL::Zero();
       //TODO Print the exception via logging
-      return Z_2x1;
+      return Vector::Zero(ZDim);
     }
   }
 
@@ -144,17 +152,19 @@ class GeneralSFMFactor: public NoiseModelFactorN<CAMERA, LANDMARK> {
     if (!this->active(values)) return std::shared_ptr<JacobianFactor>();
 
     const Key key1 = this->key1(), key2 = this->key2();
-    JacobianC H1;
-    JacobianL H2;
-    Vector2 b;
+    JacobianC Dcamera;
+    JacobianL Dlandmark;
+    Vector b;
     try {
-      const CAMERA& camera = values.atRef<CAMERA>(key1);
-      const LANDMARK& point = values.atRef<LANDMARK>(key2);
-      b = measured() - camera.project2(point, H1, H2);
+      const CAMERA& camera = values.at<CAMERA>(key1);
+      const LANDMARK& point = values.at<LANDMARK>(key2);
+      Measurement predicted = camera.project2(point, &Dcamera, &Dlandmark);
+
+      b = -Vector(traits<Measurement>::Local(measured_, predicted));
     } catch (CheiralityException& e [[maybe_unused]]) {
-      H1.setZero();
-      H2.setZero();
-      b.setZero();
+      Dcamera.setZero();
+      Dlandmark.setZero();
+      b = Vector::Zero(ZDim);
       //TODO Print the exception via logging
     }
 
@@ -163,8 +173,8 @@ class GeneralSFMFactor: public NoiseModelFactorN<CAMERA, LANDMARK> {
     if (noiseModel && !noiseModel->isUnit()) {
       // TODO: implement WhitenSystem for fixed size matrices and include
       // above
-      H1 = noiseModel->Whiten(H1);
-      H2 = noiseModel->Whiten(H2);
+      Dcamera = noiseModel->Whiten(Dcamera);
+      Dlandmark = noiseModel->Whiten(Dlandmark);
       b = noiseModel->Whiten(b);
     }
 
@@ -174,11 +184,13 @@ class GeneralSFMFactor: public NoiseModelFactorN<CAMERA, LANDMARK> {
       model = std::static_pointer_cast<noiseModel::Constrained>(noiseModel)->unit();
     }
 
-    return std::make_shared<BinaryJacobianFactor<2, DimC, DimL> >(key1, H1, key2, H2, b, model);
+    return std::make_shared<BinaryJacobianFactor<ZDim, DimC, DimL>>(key1, Dcamera,
+                                                                    key2, Dlandmark,
+                                                                    b, model);
   }
 
   /** return the measured */
-  inline const Point2 measured() const {
+  inline const Measurement measured() const {
     return measured_;
   }
 

gtsam/slam/slam.i

@@ -10,6 +10,7 @@ namespace gtsam {
 #include <gtsam/geometry/Similarity2.h>
 #include <gtsam/geometry/Similarity3.h>
 #include <gtsam/geometry/Gal3.h>
+#include <gtsam/geometry/SphericalCamera.h>
 // Following header defines PinholeCamera{Cal3_S2|Cal3DS2|Cal3Bundler|Cal3Fisheye|Cal3Unified}
 #include <gtsam/geometry/SimpleCamera.h>
 
@@ -106,10 +107,10 @@ typedef gtsam::GenericProjectionFactor<gtsam::Pose3, gtsam::Point3,
 #include <gtsam/slam/GeneralSFMFactor.h>
 template <CAMERA, LANDMARK>
 virtual class GeneralSFMFactor : gtsam::NoiseModelFactor {
-  GeneralSFMFactor(const gtsam::Point2& measured,
+  GeneralSFMFactor(const CAMERA::Measurement& measured,
                    const gtsam::noiseModel::Base* model, gtsam::Key cameraKey,
                    gtsam::Key landmarkKey);
-  gtsam::Point2 measured() const;
+  CAMERA::Measurement measured() const;
 };
 typedef gtsam::GeneralSFMFactor<gtsam::PinholeCamera<gtsam::Cal3_S2>,
                                 gtsam::Point3>
@@ -142,6 +143,8 @@ typedef gtsam::GeneralSFMFactor<gtsam::PinholePose<gtsam::Cal3Fisheye>,
 typedef gtsam::GeneralSFMFactor<gtsam::PinholePose<gtsam::Cal3Unified>,
                                 gtsam::Point3>
     GeneralSFMFactorPoseCal3Unified;
+typedef gtsam::GeneralSFMFactor<gtsam::SphericalCamera, gtsam::Point3>
+    GeneralSFMFactorSphericalCamera;
 
 template <CALIBRATION = {gtsam::Cal3_S2, gtsam::Cal3DS2, gtsam::Cal3f, gtsam::Cal3Bundler,
                          gtsam::Cal3Fisheye, gtsam::Cal3Unified}>
@@ -157,19 +160,20 @@ virtual class GeneralSFMFactor2 : gtsam::NoiseModelFactor {
 
 #include <gtsam/slam/SmartFactorBase.h>
 
-// Currently not wrapping SphericalCamera, since measurement type is not Point2 but Unit3
 template <
     CAMERA = {gtsam::PinholeCameraCal3_S2, gtsam::PinholeCameraCal3DS2,
               gtsam::PinholeCameraCal3Bundler, gtsam::PinholeCameraCal3Fisheye,
               gtsam::PinholeCameraCal3Unified, gtsam::PinholePoseCal3_S2,
               gtsam::PinholePoseCal3DS2, gtsam::PinholePoseCal3Bundler,
-              gtsam::PinholePoseCal3Fisheye, gtsam::PinholePoseCal3Unified}>
+              gtsam::PinholePoseCal3Fisheye, gtsam::PinholePoseCal3Unified,
+              gtsam::SphericalCamera}>
 virtual class SmartFactorBase : gtsam::NonlinearFactor {
-  void add(const gtsam::Point2& measured, gtsam::Key key);
-  void add(const gtsam::Point2Vector& measurements, const gtsam::KeyVector& cameraKeys);
+  void add(const CAMERA::Measurement& measured, gtsam::Key key);
+  void add(const CAMERA::MeasurementVector& measurements,
+           const gtsam::KeyVector& cameraKeys);
   size_t dim() const;
-  const gtsam::Point2Vector& measured() const;
-  gtsam::CameraSet<CAMERA> cameras(const gtsam::Values& values) const;
+  const CAMERA::MeasurementVector& measured() const;
+  std::vector<CAMERA> cameras(const gtsam::Values& values) const;
 
   void print(const std::string& s = "", const gtsam::KeyFormatter& keyFormatter =
     gtsam::DefaultKeyFormatter) const;
@@ -205,7 +209,8 @@ template <
               gtsam::PinholeCameraCal3Bundler, gtsam::PinholeCameraCal3Fisheye,
               gtsam::PinholeCameraCal3Unified, gtsam::PinholePoseCal3_S2,
               gtsam::PinholePoseCal3DS2, gtsam::PinholePoseCal3Bundler,
-              gtsam::PinholePoseCal3Fisheye, gtsam::PinholePoseCal3Unified}>
+              gtsam::PinholePoseCal3Fisheye, gtsam::PinholePoseCal3Unified,
+              gtsam::SphericalCamera}>
 virtual class SmartProjectionFactor : gtsam::SmartFactorBase<CAMERA> {
   SmartProjectionFactor();
 
@@ -290,11 +295,12 @@ virtual class SmartProjectionPoseFactor : gtsam::NonlinearFactor {
 };
 
 #include <gtsam/slam/SmartProjectionRigFactor.h>
-// Only for PinholePose cameras -> PinholeCamera is not supported
+// Only for pose-only cameras (e.g., PinholePose or SphericalCamera)
 template <CAMERA = {gtsam::PinholePoseCal3_S2, gtsam::PinholePoseCal3DS2,
   gtsam::PinholePoseCal3Bundler,
   gtsam::PinholePoseCal3Fisheye,
-  gtsam::PinholePoseCal3Unified}>
+  gtsam::PinholePoseCal3Unified,
+  gtsam::SphericalCamera}>
 virtual class SmartProjectionRigFactor : gtsam::SmartProjectionFactor<CAMERA> {
   SmartProjectionRigFactor();
 
@@ -303,10 +309,10 @@ virtual class SmartProjectionRigFactor : gtsam::SmartProjectionFactor<CAMERA> {
       const gtsam::CameraSet<CAMERA>* cameraRig,
       const gtsam::SmartProjectionParams& params = gtsam::SmartProjectionParams());
 
-  void add(const gtsam::Point2& measured, const gtsam::Key& poseKey,
+  void add(const CAMERA::Measurement& measured, const gtsam::Key& poseKey,
            const size_t& cameraId = 0);
 
-  void add(const gtsam::Point2Vector& measurements, const gtsam::KeyVector& poseKeys,
+  void add(const CAMERA::MeasurementVector& measurements, const gtsam::KeyVector& poseKeys,
            const gtsam::FastVector<size_t>& cameraIds = gtsam::FastVector<size_t>());
 
   const gtsam::KeyVector& nonUniqueKeys() const;

gtsam_unstable/slam/serialization.cpp

@@ -25,6 +25,7 @@
 #include <gtsam/geometry/Pose3.h>
 #include <gtsam/geometry/SL4.h>
 #include <gtsam/geometry/Cal3DS2.h>
+#include <gtsam/geometry/SphericalCamera.h>
 //#include <gtsam/geometry/Cal3_S2Stereo.h>
 
 using namespace gtsam;
@@ -82,6 +83,8 @@ typedef GenericProjectionFactor<Pose3, Point3, Cal3DS2> GenericProjectionFactorC
 
 typedef gtsam::GeneralSFMFactor<gtsam::PinholeCameraCal3_S2, gtsam::Point3> GeneralSFMFactorCal3_S2;
 typedef gtsam::GeneralSFMFactor<gtsam::PinholeCameraCal3DS2, gtsam::Point3> GeneralSFMFactorCal3DS2;
+typedef gtsam::GeneralSFMFactor<gtsam::SphericalCamera, gtsam::Point3>
+    GeneralSFMFactorSphericalCamera;
 
 typedef gtsam::GeneralSFMFactor2<gtsam::Cal3_S2> GeneralSFMFactor2Cal3_S2;
 
@@ -173,6 +176,8 @@ BOOST_CLASS_EXPORT_GUID(GenericProjectionFactorCal3DS2, "gtsam::GenericProjectio
 
 BOOST_CLASS_EXPORT_GUID(GeneralSFMFactorCal3_S2, "gtsam::GeneralSFMFactorCal3_S2");
 BOOST_CLASS_EXPORT_GUID(GeneralSFMFactorCal3DS2, "gtsam::GeneralSFMFactorCal3DS2");
+BOOST_CLASS_EXPORT_GUID(GeneralSFMFactorSphericalCamera,
+                        "gtsam::GeneralSFMFactorSphericalCamera");
 
 BOOST_CLASS_EXPORT_GUID(GeneralSFMFactor2Cal3_S2, "gtsam::GeneralSFMFactor2Cal3_S2");
 
@@ -288,4 +293,3 @@ Values::shared_ptr gtsam::deserializeValuesFromXMLFile(const std::string& fname,
 
 /* ************************************************************************* */
 
-