spherical camera

gtsam/geometry/PinholeCamera.h

@@ -312,6 +312,17 @@ class GTSAM_EXPORT PinholeCamera: public PinholeBaseK<Calibration> {
     return range(camera.pose(), Dcamera, Dother);
   }
 
+  /// for Linear Triangulation
+  Matrix34 getCameraProjectionMatrix() const {
+    Matrix34 P = Matrix34(PinholeBase::pose().inverse().matrix().block(0, 0, 3, 4));
+    return K_.K() * P;
+  }
+
+  /// for Nonlinear Triangulation
+  Vector defaultErrorWhenTriangulatingBehindCamera() const {
+    return Eigen::Matrix<double,traits<Point2>::dimension,1>::Constant(2.0 * K_.fx());;
+  }
+
 private:
 
   /** Serialization function */

gtsam/geometry/PinholePose.h

@@ -121,6 +121,13 @@ class GTSAM_EXPORT PinholeBaseK: public PinholeBase {
     return _project(pw, Dpose, Dpoint, Dcal);
   }
 
+  /// project a 3D point from world coordinates into the image
+  Point2 reprojectionError(const Point3& pw, const Point2& measured, OptionalJacobian<2, 6> Dpose = boost::none,
+      OptionalJacobian<2, 3> Dpoint = boost::none,
+      OptionalJacobian<2, DimK> Dcal = boost::none) const {
+    return Point2(_project(pw, Dpose, Dpoint, Dcal) - measured);
+  }
+
   /// project a point at infinity from world coordinates into the image
   Point2 project(const Unit3& pw, OptionalJacobian<2, 6> Dpose = boost::none,
       OptionalJacobian<2, 2> Dpoint = boost::none,
@@ -159,7 +166,6 @@ class GTSAM_EXPORT PinholeBaseK: public PinholeBase {
     return result;
   }
 
-
   /// backproject a 2-dimensional point to a 3-dimensional point at infinity
   Unit3 backprojectPointAtInfinity(const Point2& p) const {
     const Point2 pn = calibration().calibrate(p);
@@ -410,6 +416,16 @@ class PinholePose: public PinholeBaseK<CALIBRATION> {
     return PinholePose(); // assumes that the default constructor is valid
   }
 
+  /// for Linear Triangulation
+  Matrix34 getCameraProjectionMatrix() const {
+    Matrix34 P = Matrix34(PinholeBase::pose().inverse().matrix().block(0, 0, 3, 4));
+    return K_->K() * P;
+  }
+
+  /// for Nonlinear Triangulation
+  Vector defaultErrorWhenTriangulatingBehindCamera() const {
+    return Eigen::Matrix<double,traits<Point2>::dimension,1>::Constant(2.0 * K_->fx());;
+  }
   /// @}
 
 private:

gtsam/geometry/SphericalCamera.cpp

@@ -0,0 +1,109 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file SphericalCamera.h
+ * @brief Calibrated camera with spherical projection
+ * @date Aug 26, 2021
+ * @author Luca Carlone
+ */
+
+#include <gtsam/geometry/SphericalCamera.h>
+
+using namespace std;
+
+namespace gtsam {
+
+/* ************************************************************************* */
+bool SphericalCamera::equals(const SphericalCamera& camera, double tol) const {
+  return pose_.equals(camera.pose(), tol);
+}
+
+/* ************************************************************************* */
+void SphericalCamera::print(const string& s) const { pose_.print(s + ".pose"); }
+
+/* ************************************************************************* */
+pair<Unit3, bool> SphericalCamera::projectSafe(const Point3& pw) const {
+  const Point3 pc = pose().transformTo(pw);
+  Unit3 pu = Unit3::FromPoint3(pc);
+  return make_pair(pu, pc.norm() > 1e-8);
+}
+
+/* ************************************************************************* */
+Unit3 SphericalCamera::project2(const Point3& pw, OptionalJacobian<2, 6> Dpose,
+                                OptionalJacobian<2, 3> Dpoint) const {
+  Matrix36 Dtf_pose;
+  Matrix3 Dtf_point;  // calculated by transformTo if needed
+  const Point3 pc =
+      pose().transformTo(pw, Dpose ? &Dtf_pose : 0, Dpoint ? &Dtf_point : 0);
+
+  if (pc.norm() <= 1e-8) throw("point cannot be at the center of the camera");
+
+  Matrix23 Dunit;  // calculated by FromPoint3 if needed
+  Unit3 pu = Unit3::FromPoint3(Point3(pc), Dpoint ? &Dunit : 0);
+
+  if (Dpose) *Dpose = Dunit * Dtf_pose;     // 2x3 * 3x6 = 2x6
+  if (Dpoint) *Dpoint = Dunit * Dtf_point;  // 2x3 * 3x3 = 2x3
+  return pu;
+}
+
+/* ************************************************************************* */
+Unit3 SphericalCamera::project2(const Unit3& pwu, OptionalJacobian<2, 6> Dpose,
+                                OptionalJacobian<2, 2> Dpoint) const {
+  Matrix23 Dtf_rot;
+  Matrix2 Dtf_point;  // calculated by transformTo if needed
+  const Unit3 pu = pose().rotation().unrotate(pwu, Dpose ? &Dtf_rot : 0,
+                                              Dpoint ? &Dtf_point : 0);
+
+  if (Dpose)
+    *Dpose << Dtf_rot, Matrix::Zero(2, 3);  // 2x6 (translation part is zero)
+  if (Dpoint) *Dpoint = Dtf_point;          // 2x2
+  return pu;
+}
+
+/* ************************************************************************* */
+Point3 SphericalCamera::backproject(const Unit3& pu, const double depth) const {
+  return pose().transformFrom(depth * pu);
+}
+
+/* ************************************************************************* */
+Unit3 SphericalCamera::backprojectPointAtInfinity(const Unit3& p) const {
+  return pose().rotation().rotate(p);
+}
+
+/* ************************************************************************* */
+Unit3 SphericalCamera::project(const Point3& point,
+                               OptionalJacobian<2, 6> Dcamera,
+                               OptionalJacobian<2, 3> Dpoint) const {
+  return project2(point, Dcamera, Dpoint);
+}
+
+/* ************************************************************************* */
+Vector2 SphericalCamera::reprojectionError(
+    const Point3& point, const Unit3& measured, OptionalJacobian<2, 6> Dpose,
+    OptionalJacobian<2, 3> Dpoint) const {
+  // project point
+  if (Dpose || Dpoint) {
+    Matrix26 H_project_pose;
+    Matrix23 H_project_point;
+    Matrix22 H_error;
+    Unit3 projected = project2(point, H_project_pose, H_project_point);
+    Vector2 error = measured.errorVector(projected, boost::none, H_error);
+    if (Dpose) *Dpose = H_error * H_project_pose;
+    if (Dpoint) *Dpoint = H_error * H_project_point;
+    return error;
+  } else {
+    return measured.errorVector(project2(point, Dpose, Dpoint));
+  }
+}
+
+/* ************************************************************************* */
+}  // namespace gtsam

gtsam/geometry/SphericalCamera.h

@@ -0,0 +1,217 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file SphericalCamera.h
+ * @brief Calibrated camera with spherical projection
+ * @date Aug 26, 2021
+ * @author Luca Carlone
+ */
+
+#pragma once
+
+#include <gtsam/base/Manifold.h>
+#include <gtsam/base/ThreadsafeException.h>
+#include <gtsam/base/concepts.h>
+#include <gtsam/dllexport.h>
+#include <gtsam/geometry/BearingRange.h>
+#include <gtsam/geometry/Pose3.h>
+#include <gtsam/geometry/Unit3.h>
+
+#include <boost/serialization/nvp.hpp>
+
+namespace gtsam {
+
+class GTSAM_EXPORT EmptyCal {
+ public:
+  enum { dimension = 0 };
+  EmptyCal() {}
+  virtual ~EmptyCal() = default;
+  using shared_ptr = boost::shared_ptr<EmptyCal>;
+  void print(const std::string& s) const {
+    std::cout << "empty calibration: " << s << std::endl;
+  }
+};
+
+/**
+ * A spherical camera class that has a Pose3 and measures bearing vectors
+ * @addtogroup geometry
+ * \nosubgrouping
+ */
+class GTSAM_EXPORT SphericalCamera {
+ public:
+  enum { dimension = 6 };
+
+  typedef Unit3 Measurement;
+  typedef std::vector<Unit3> MeasurementVector;
+  typedef EmptyCal CalibrationType;
+
+ private:
+  Pose3 pose_;  ///< 3D pose of camera
+
+ protected:
+  EmptyCal::shared_ptr emptyCal_;
+
+ public:
+  /// @}
+  /// @name Standard Constructors
+  /// @{
+
+  /// Default constructor
+  SphericalCamera()
+      : pose_(Pose3::identity()), emptyCal_(boost::make_shared<EmptyCal>()) {}
+
+  /// Constructor with pose
+  explicit SphericalCamera(const Pose3& pose)
+      : pose_(pose), emptyCal_(boost::make_shared<EmptyCal>()) {}
+
+  /// Constructor with empty intrinsics (needed for smart factors)
+  explicit SphericalCamera(const Pose3& pose,
+                           const boost::shared_ptr<EmptyCal>& cal)
+      : pose_(pose), emptyCal_(boost::make_shared<EmptyCal>()) {}
+
+  /// @}
+  /// @name Advanced Constructors
+  /// @{
+  explicit SphericalCamera(const Vector& v) : pose_(Pose3::Expmap(v)) {}
+
+  /// Default destructor
+  virtual ~SphericalCamera() = default;
+
+  /// return shared pointer to calibration
+  const boost::shared_ptr<EmptyCal>& sharedCalibration() const {
+    return emptyCal_;
+  }
+
+  /// return calibration
+  const EmptyCal& calibration() const { return *emptyCal_; }
+
+  /// @}
+  /// @name Testable
+  /// @{
+
+  /// assert equality up to a tolerance
+  bool equals(const SphericalCamera& camera, double tol = 1e-9) const;
+
+  /// print
+  virtual void print(const std::string& s = "SphericalCamera") const;
+
+  /// @}
+  /// @name Standard Interface
+  /// @{
+
+  /// return pose, constant version
+  const Pose3& pose() const { return pose_; }
+
+  /// get rotation
+  const Rot3& rotation() const { return pose_.rotation(); }
+
+  /// get translation
+  const Point3& translation() const { return pose_.translation(); }
+
+  //  /// return pose, with derivative
+  //  const Pose3& getPose(OptionalJacobian<6, 6> H) const;
+
+  /// @}
+  /// @name Transformations and measurement functions
+  /// @{
+
+  /// Project a point into the image and check depth
+  std::pair<Unit3, bool> projectSafe(const Point3& pw) const;
+
+  /** Project point into the image
+   * (note: there is no CheiralityException for a spherical camera)
+   * @param point 3D point in world coordinates
+   * @return the intrinsic coordinates of the projected point
+   */
+  Unit3 project2(const Point3& pw, OptionalJacobian<2, 6> Dpose = boost::none,
+                 OptionalJacobian<2, 3> Dpoint = boost::none) const;
+
+  /** Project point into the image
+   * (note: there is no CheiralityException for a spherical camera)
+   * @param point 3D direction in world coordinates
+   * @return the intrinsic coordinates of the projected point
+   */
+  Unit3 project2(const Unit3& pwu, OptionalJacobian<2, 6> Dpose = boost::none,
+                 OptionalJacobian<2, 2> Dpoint = boost::none) const;
+
+  /// backproject a 2-dimensional point to a 3-dimensional point at given depth
+  Point3 backproject(const Unit3& p, const double depth) const;
+
+  /// backproject point at infinity
+  Unit3 backprojectPointAtInfinity(const Unit3& p) const;
+
+  /** Project point into the image
+   * (note: there is no CheiralityException for a spherical camera)
+   * @param point 3D point in world coordinates
+   * @return the intrinsic coordinates of the projected point
+   */
+  Unit3 project(const Point3& point, OptionalJacobian<2, 6> Dpose = boost::none,
+                OptionalJacobian<2, 3> Dpoint = boost::none) const;
+
+  /** Compute reprojection error for a given 3D point in world coordinates
+   * @param point 3D point in world coordinates
+   * @return the tangent space error between the projection and the measurement
+   */
+  Vector2 reprojectionError(const Point3& point, const Unit3& measured,
+                            OptionalJacobian<2, 6> Dpose = boost::none,
+                            OptionalJacobian<2, 3> Dpoint = boost::none) const;
+  /// @}
+
+  /// move a cameras according to d
+  SphericalCamera retract(const Vector6& d) const {
+    return SphericalCamera(pose().retract(d));
+  }
+
+  /// return canonical coordinate
+  Vector6 localCoordinates(const SphericalCamera& p) const {
+    return pose().localCoordinates(p.pose());
+  }
+
+  /// for Canonical
+  static SphericalCamera identity() {
+    return SphericalCamera(
+        Pose3::identity());  // assumes that the default constructor is valid
+  }
+
+  /// for Linear Triangulation
+  Matrix34 getCameraProjectionMatrix() const {
+    return Matrix34(pose_.inverse().matrix().block(0, 0, 3, 4));
+  }
+
+  /// for Nonlinear Triangulation
+  Vector defaultErrorWhenTriangulatingBehindCamera() const {
+    return Eigen::Matrix<double, traits<Point2>::dimension, 1>::Constant(0.0);
+  }
+
+  /// @deprecated
+  size_t dim() const { return 6; }
+
+  /// @deprecated
+  static size_t Dim() { return 6; }
+
+ private:
+  /** Serialization function */
+  friend class boost::serialization::access;
+  template <class Archive>
+  void serialize(Archive& ar, const unsigned int /*version*/) {
+    ar& BOOST_SERIALIZATION_NVP(pose_);
+  }
+};
+// end of class SphericalCamera
+
+template <>
+struct traits<SphericalCamera> : public internal::LieGroup<Pose3> {};
+
+template <>
+struct traits<const SphericalCamera> : public internal::LieGroup<Pose3> {};
+
+}  // namespace gtsam

gtsam/geometry/StereoCamera.h

@@ -170,6 +170,11 @@ class GTSAM_EXPORT StereoCamera {
       OptionalJacobian<3, 3> H2 = boost::none, OptionalJacobian<3, 0> H3 =
           boost::none) const;
 
+  /// for Nonlinear Triangulation
+  Vector defaultErrorWhenTriangulatingBehindCamera() const {
+    return Eigen::Matrix<double,traits<Measurement>::dimension,1>::Constant(2.0 * K_->fx());;
+  }
+
   /// @}
 
 private: