New docs: SLAM

gtsam/geometry/SimpleCamera.h

@@ -31,10 +31,15 @@ namespace gtsam {
 
   /// Convenient aliases for Pinhole camera classes with different calibrations.
   /// Also needed as forward declarations in the wrapper.
+  using PinholePoseCal3_S2 = gtsam::PinholePose<gtsam::Cal3_S2>;
+  using PinholePoseCal3Bundler = gtsam::PinholePose<gtsam::Cal3Bundler>;
+  using PinholePoseCal3DS2 = gtsam::PinholePose<gtsam::Cal3DS2>;
+  using PinholePoseCal3Unified = gtsam::PinholePose<gtsam::Cal3Unified>;
+  using PinholePoseCal3Fisheye = gtsam::PinholePose<gtsam::Cal3Fisheye>;
   using PinholeCameraCal3_S2 = gtsam::PinholeCamera<gtsam::Cal3_S2>;
   using PinholeCameraCal3Bundler = gtsam::PinholeCamera<gtsam::Cal3Bundler>;
   using PinholeCameraCal3DS2 = gtsam::PinholeCamera<gtsam::Cal3DS2>;
   using PinholeCameraCal3Unified = gtsam::PinholeCamera<gtsam::Cal3Unified>;
   using PinholeCameraCal3Fisheye = gtsam::PinholeCamera<gtsam::Cal3Fisheye>;
-
+  
 }  // namespace gtsam

gtsam/geometry/geometry.i

@@ -694,6 +694,45 @@ class Unit3 {
   bool equals(const gtsam::Unit3& expected, double tol) const;
 };
 
+#include <gtsam/geometry/OrientedPlane3.h>
+class OrientedPlane3 {
+  // Standard constructors
+  OrientedPlane3();
+  OrientedPlane3(const gtsam::Unit3& n, double d);
+  OrientedPlane3(const gtsam::Vector& vec);
+  OrientedPlane3(double a, double b, double c, double d);
+
+  // Testable
+  void print(string s = "") const;
+  bool equals(const gtsam::OrientedPlane3& s, double tol = 1e-9) const;
+
+  gtsam::OrientedPlane3 transform(const gtsam::Pose3& xr) const;
+  gtsam::OrientedPlane3 transform(const gtsam::Pose3& xr,
+                           Eigen::Ref<Eigen::MatrixXd> Hp,
+                           Eigen::Ref<Eigen::MatrixXd> Hr) const;
+
+  gtsam::Vector3 errorVector(const gtsam::OrientedPlane3& other) const;
+  gtsam::Vector3 errorVector(const gtsam::OrientedPlane3& other,
+                      Eigen::Ref<Eigen::MatrixXd> H1,
+                      Eigen::Ref<Eigen::MatrixXd> H2) const;
+
+  static size_t Dim();
+  size_t dim() const;
+
+  gtsam::OrientedPlane3 retract(const gtsam::Vector3& v) const;
+  gtsam::OrientedPlane3 retract(const gtsam::Vector3& v,
+                        Eigen::Ref<Eigen::MatrixXd> H) const;
+
+  gtsam::Vector3 localCoordinates(const gtsam::OrientedPlane3& s) const;
+
+  gtsam::Vector planeCoefficients() const;
+
+  gtsam::Unit3 normal() const;
+  gtsam::Unit3 normal(Eigen::Ref<Eigen::MatrixXd> H) const;
+  double distance() const;
+  double distance(Eigen::Ref<Eigen::MatrixXd> H) const;
+};
+
 #include <gtsam/geometry/EssentialMatrix.h>
 class EssentialMatrix {
   // Standard Constructors
@@ -1278,7 +1317,7 @@ class Similarity3 {
   double scale() const;
 };
 
-template <T>
+template <T = {gtsam::PinholePoseCal3_S2}>
 class CameraSet {
   CameraSet();
 

gtsam/geometry/triangulation.h

@@ -758,10 +758,17 @@ TriangulationResult triangulateSafe(const CameraSet<CAMERA>& cameras,
 }
 
 // Vector of Cameras - used by the Python/MATLAB wrapper
+using CameraSetPinholePoseCal3Bundler = CameraSet<PinholePose<Cal3Bundler>>;
+using CameraSetPinholePoseCal3_S2 = CameraSet<PinholePose<Cal3_S2>>;
+using CameraSetPinholePoseCal3DS2 = CameraSet<PinholePose<Cal3DS2>>;
+using CameraSetPinholePoseCal3Fisheye = CameraSet<PinholePose<Cal3Fisheye>>;
+using CameraSetPinholePoseCal3Unified = CameraSet<PinholePose<Cal3Unified>>;
+
 using CameraSetCal3Bundler = CameraSet<PinholeCamera<Cal3Bundler>>;
 using CameraSetCal3_S2 = CameraSet<PinholeCamera<Cal3_S2>>;
 using CameraSetCal3DS2 = CameraSet<PinholeCamera<Cal3DS2>>;
 using CameraSetCal3Fisheye = CameraSet<PinholeCamera<Cal3Fisheye>>;
 using CameraSetCal3Unified = CameraSet<PinholeCamera<Cal3Unified>>;
+
 using CameraSetSpherical = CameraSet<SphericalCamera>;
 } // \namespace gtsam

gtsam/linear/linear.i

@@ -355,7 +355,7 @@ virtual class JacobianFactor : gtsam::GaussianFactor {
   gtsam::Vector error_vector(const gtsam::VectorValues& c) const;
   double error(const gtsam::VectorValues& c) const;
 
-  //Standard Interface
+  // Standard Interface
   gtsam::Matrix getA() const;
   gtsam::Vector getb() const;
   size_t rows() const;

gtsam/nonlinear/values.i

@@ -12,6 +12,7 @@ namespace gtsam {
 #include <gtsam/geometry/Cal3Unified.h>
 #include <gtsam/geometry/EssentialMatrix.h>
 #include <gtsam/geometry/FundamentalMatrix.h>
+#include <gtsam/geometry/OrientedPlane3.h>
 #include <gtsam/geometry/PinholeCamera.h>
 #include <gtsam/geometry/Point2.h>
 #include <gtsam/geometry/Point3.h>
@@ -94,6 +95,7 @@ class Values {
   void insert(size_t j, const gtsam::EssentialMatrix& E);
   void insert(size_t j, const gtsam::FundamentalMatrix& F);
   void insert(size_t j, const gtsam::SimpleFundamentalMatrix& F);
+  void insert(size_t j, const gtsam::OrientedPlane3& plane);
   void insert(size_t j, const gtsam::PinholeCamera<gtsam::Cal3Bundler>& camera);
   void insert(size_t j, const gtsam::PinholeCamera<gtsam::Cal3f>& camera);
   void insert(size_t j, const gtsam::PinholeCamera<gtsam::Cal3_S2>& camera);
@@ -138,6 +140,7 @@ class Values {
   void update(size_t j, const gtsam::EssentialMatrix& E);
   void update(size_t j, const gtsam::FundamentalMatrix& F);
   void update(size_t j, const gtsam::SimpleFundamentalMatrix& F);
+  void update(size_t j, const gtsam::OrientedPlane3& plane);
   void update(size_t j, const gtsam::PinholeCamera<gtsam::Cal3Bundler>& camera);
   void update(size_t j, const gtsam::PinholeCamera<gtsam::Cal3f>& camera);
   void update(size_t j, const gtsam::PinholeCamera<gtsam::Cal3_S2>& camera);
@@ -179,6 +182,7 @@ class Values {
   void insert_or_assign(size_t j, const gtsam::EssentialMatrix& E);
   void insert_or_assign(size_t j, const gtsam::FundamentalMatrix& F);
   void insert_or_assign(size_t j, const gtsam::SimpleFundamentalMatrix& F);
+  void insert_or_assign(size_t j, const gtsam::OrientedPlane3& plane);
   void insert_or_assign(size_t j, const gtsam::PinholeCamera<gtsam::Cal3Bundler>& camera);
   void insert_or_assign(size_t j, const gtsam::PinholeCamera<gtsam::Cal3f>& camera);
   void insert_or_assign(size_t j, const gtsam::PinholeCamera<gtsam::Cal3_S2>& camera);
@@ -216,6 +220,7 @@ class Values {
                  gtsam::EssentialMatrix, 
                  gtsam::FundamentalMatrix, 
                  gtsam::SimpleFundamentalMatrix,
+                 gtsam::OrientedPlane3,
                  gtsam::PinholeCamera<gtsam::Cal3Bundler>,
                  gtsam::PinholeCamera<gtsam::Cal3f>,
                  gtsam::PinholeCamera<gtsam::Cal3_S2>,

gtsam/slam/KarcherMeanFactor.h

@@ -27,13 +27,17 @@
 namespace gtsam {
 /**
  * Optimize for the Karcher mean, minimizing the geodesic distance to each of
- * the given rotations, by constructing a factor graph out of simple
+ * the given Lie groups elements, by constructing a factor graph out of simple
  * PriorFactors.
  */
 template <class T>
-T FindKarcherMean(const std::vector<T, Eigen::aligned_allocator<T>> &rotations);
+typename std::enable_if<traits<T>::IsLieGroup, T>::type 
+FindKarcherMean(const std::vector<T, Eigen::aligned_allocator<T>> &elements);
 
-template <class T> T FindKarcherMean(std::initializer_list<T> &&rotations);
+/// FindKarcherMean version from initializer list
+template <class T>
+typename std::enable_if<traits<T>::IsLieGroup, T>::type 
+FindKarcherMean(std::initializer_list<T> &&elements);
 
 /**
  * The KarcherMeanFactor creates a constraint on all SO(n) variables with

gtsam/slam/PlanarProjectionFactor.h

@@ -201,7 +201,7 @@ namespace gtsam {
             const Cal3DS2& calib,
             const SharedNoiseModel& model = {})
             : PlanarProjectionFactorBase(measured),
-            NoiseModelFactorN(model, landmarkKey, poseKey),
+            NoiseModelFactorN(model, poseKey, landmarkKey),
             bTc_(bTc),
             calib_(calib) {}
 

gtsam/slam/SmartProjectionRigFactor.h

@@ -46,6 +46,30 @@ namespace gtsam {
  * calibration (i.e., are from the same camera), use SmartProjectionPoseFactor
  * instead! If the calibration should be optimized, as well, use
  * SmartProjectionFactor instead!
+ *
+ * <b>Note on Template Parameter `CAMERA`:</b>
+ * While this factor is templated on `CAMERA` to allow for generality (e.g.,
+ * `SphericalCamera`), the current internal implementation for linearization
+ * (specifically, methods like `createHessianFactor` involving Schur complement
+ * calculations inherited or adapted from base classes) has limitations. It
+ * implicitly assumes that the CAMERA only has a Pose3 unknown.
+ *
+ * Consequently:
+ * - This factor works correctly with `CAMERA` types where this is the case,
+ *   such as `PinholePose<CALIBRATION>` or `SphericalCamera`.
+ * - Using `CAMERA` types where `dimension != 6`, such as
+ *   `PinholeCamera<CALIBRATION>` (which has dimension `6 + CalDim`), will lead
+ *   to compilation errors due to matrix dimension mismatches.
+ *
+ * Therefore, for standard pinhole cameras within a fixed rig, use
+ * `PinholePose<CALIBRATION>` as the `CAMERA` template parameter when defining
+ * the `CameraSet` passed to this factor's constructor.
+ *
+ * TODO(dellaert): Refactor the internal linearization logic (e.g., in
+ * `createHessianFactor`) to explicitly compute Jacobians with respect to the
+ * 6-DoF body pose by correctly applying the chain rule, rather than relying on
+ * `traits<CAMERA>::dimension` for downstream calculations.
+ *
  * @ingroup slam
  */
 template <class CAMERA>