Merged in fix/matlab_wrapper (pull request #137)

Matlab wrapper and serialization

gtsam.h

@@ -383,11 +383,6 @@ class Point3 {
   void serialize() const;
 };
 
-class OptionalPoint3 {
-  bool is_initialized() const;
-  gtsam::Point3 value();
-};
-
 class Rot2 {
   // Standard Constructors and Named Constructors
   Rot2();
@@ -866,8 +861,45 @@ class PinholeCamera {
   void serialize() const;
 };
 
-// Do typedefs here so we can also define SimpleCamera
-typedef gtsam::PinholeCamera<gtsam::Cal3_S2> SimpleCamera;
+virtual class SimpleCamera {
+  // Standard Constructors and Named Constructors
+  SimpleCamera();
+  SimpleCamera(const gtsam::Pose3& pose);
+  SimpleCamera(const gtsam::Pose3& pose, const gtsam::Cal3_S2& K);
+  static gtsam::SimpleCamera Level(const gtsam::Cal3_S2& K, const gtsam::Pose2& pose, double height);
+  static gtsam::SimpleCamera Level(const gtsam::Pose2& pose, double height);
+  static gtsam::SimpleCamera Lookat(const gtsam::Point3& eye, const gtsam::Point3& target,
+      const gtsam::Point3& upVector, const gtsam::Cal3_S2& K);
+
+  // Testable
+  void print(string s) const;
+  bool equals(const gtsam::SimpleCamera& camera, double tol) const;
+
+  // Standard Interface
+  gtsam::Pose3 pose() const;
+  gtsam::Cal3_S2 calibration() const;
+
+  // Manifold
+  gtsam::SimpleCamera retract(const Vector& d) const;
+  Vector localCoordinates(const gtsam::SimpleCamera& T2) const;
+  size_t dim() const;
+  static size_t Dim();
+
+  // Transformations and measurement functions
+  static gtsam::Point2 project_to_camera(const gtsam::Point3& cameraPoint);
+  pair<gtsam::Point2,bool> projectSafe(const gtsam::Point3& pw) const;
+  gtsam::Point2 project(const gtsam::Point3& point);
+  gtsam::Point3 backproject(const gtsam::Point2& p, double depth) const;
+  double range(const gtsam::Point3& point);
+  double range(const gtsam::Pose3& point);
+
+  // enabling serialization functionality
+  void serialize() const;
+
+};
+
+// Some typedefs for common camera types
+// PinholeCameraCal3_S2 is the same as SimpleCamera above
 typedef gtsam::PinholeCamera<gtsam::Cal3_S2> PinholeCameraCal3_S2;
 typedef gtsam::PinholeCamera<gtsam::Cal3DS2> PinholeCameraCal3DS2;
 typedef gtsam::PinholeCamera<gtsam::Cal3Unified> PinholeCameraCal3Unified;

gtsam/geometry/PinholePose.h

@@ -37,6 +37,8 @@ class GTSAM_EXPORT PinholeBaseK: public PinholeBase {
 
 public  :
 
+  typedef Calibration CalibrationType;
+
   /// @name Standard Constructors
   /// @{
 

gtsam/geometry/Point3.h

@@ -194,9 +194,6 @@ namespace gtsam {
 /// Syntactic sugar for multiplying coordinates by a scalar s*p
 inline Point3 operator*(double s, const Point3& p) { return p*s;}
 
-// For MATLAB wrapper
-typedef boost::optional<Point3> OptionalPoint3;
-
 template<>
 struct traits<Point3> : public internal::VectorSpace<Point3> {};
 

gtsam/geometry/SimpleCamera.h

@@ -24,7 +24,114 @@
 namespace gtsam {
 
   /// A simple camera class with a Cal3_S2 calibration
-  typedef PinholeCamera<Cal3_S2> SimpleCamera;
+typedef gtsam::PinholeCamera<gtsam::Cal3_S2> PinholeCameraCal3_S2;
+
+/**
+ * @deprecated: SimpleCamera for backwards compatability with GTSAM 3.x
+ * Use PinholeCameraCal3_S2 instead
+ */
+class SimpleCamera : public PinholeCameraCal3_S2 {
+
+  typedef PinholeCamera<Cal3_S2> Base;
+  typedef boost::shared_ptr<SimpleCamera> shared_ptr;
+
+public:
+
+  /// @name Standard Constructors
+  /// @{
+
+  /** default constructor */
+  SimpleCamera() :
+      Base() {
+  }
+
+  /** constructor with pose */
+  explicit SimpleCamera(const Pose3& pose) :
+      Base(pose) {
+  }
+
+  /** constructor with pose and calibration */
+  SimpleCamera(const Pose3& pose, const Cal3_S2& K) :
+      Base(pose, K) {
+  }
+
+  /// @}
+   /// @name Named Constructors
+   /// @{
+
+   /**
+    * Create a level camera at the given 2D pose and height
+    * @param K the calibration
+    * @param pose2 specifies the location and viewing direction
+    * (theta 0 = looking in direction of positive X axis)
+    * @param height camera height
+    */
+   static SimpleCamera Level(const Cal3_S2 &K, const Pose2& pose2,
+       double height) {
+     return SimpleCamera(Base::LevelPose(pose2, height), K);
+   }
+
+   /// PinholeCamera::level with default calibration
+   static SimpleCamera Level(const Pose2& pose2, double height) {
+    return SimpleCamera::Level(Cal3_S2(), pose2, height);
+   }
+
+   /**
+    * Create a camera at the given eye position looking at a target point in the scene
+    * with the specified up direction vector.
+    * @param eye specifies the camera position
+    * @param target the point to look at
+    * @param upVector specifies the camera up direction vector,
+    *        doesn't need to be on the image plane nor orthogonal to the viewing axis
+    * @param K optional calibration parameter
+    */
+   static SimpleCamera Lookat(const Point3& eye, const Point3& target,
+       const Point3& upVector, const Cal3_S2& K = Cal3_S2()) {
+    return SimpleCamera(Base::LookatPose(eye, target, upVector), K);
+   }
+
+   /// @}
+   /// @name Advanced Constructors
+   /// @{
+
+   /// Init from vector, can be 6D (default calibration) or dim
+   explicit SimpleCamera(const Vector &v) :
+       Base(v) {
+   }
+
+   /// Init from Vector and calibration
+   SimpleCamera(const Vector &v, const Vector &K) :
+       Base(v, K) {
+   }
+
+   /// Copy this object as its actual derived type.
+   SimpleCamera::shared_ptr clone() const { return boost::make_shared<SimpleCamera>(*this); }
+
+
+   /// @}
+   /// @name Manifold
+   /// @{
+
+   /// move a cameras according to d
+   SimpleCamera retract(const Vector& d) const {
+     if ((size_t) d.size() == 6)
+       return SimpleCamera(this->pose().retract(d), calibration());
+     else
+       return SimpleCamera(this->pose().retract(d.head(6)),
+           calibration().retract(d.tail(calibration().dim())));
+   }
+
+   /// @}
+
+};
+
+template<>
+struct traits<SimpleCamera> : public internal::Manifold<SimpleCamera> {
+};
+
+template<>
+struct traits<const SimpleCamera> : public internal::Manifold<SimpleCamera> {
+};
 
   /// Recover camera from 3*4 camera matrix
   GTSAM_EXPORT SimpleCamera simpleCamera(const Matrix34& P);

gtsam/geometry/triangulation.h

@@ -102,9 +102,9 @@ std::pair<NonlinearFactorGraph, Values> triangulationGraph(
  * @param initialEstimate
  * @return graph and initial values
  */
-template<class CALIBRATION>
+template<class CAMERA>
 std::pair<NonlinearFactorGraph, Values> triangulationGraph(
-    const std::vector<PinholeCamera<CALIBRATION> >& cameras,
+    const std::vector<CAMERA>& cameras,
     const std::vector<Point2>& measurements, Key landmarkKey,
     const Point3& initialEstimate) {
   Values values;
@@ -113,8 +113,8 @@ std::pair<NonlinearFactorGraph, Values> triangulationGraph(
   static SharedNoiseModel unit2(noiseModel::Unit::Create(2));
   static SharedNoiseModel prior_model(noiseModel::Isotropic::Sigma(6, 1e-6));
   for (size_t i = 0; i < measurements.size(); i++) {
-    const PinholeCamera<CALIBRATION>& camera_i = cameras[i];
-    graph.push_back(TriangulationFactor<CALIBRATION> //
+    const CAMERA& camera_i = cameras[i];
+    graph.push_back(TriangulationFactor<typename CAMERA::CalibrationType> //
         (camera_i, measurements[i], unit2, landmarkKey));
   }
   return std::make_pair(graph, values);
@@ -160,9 +160,9 @@ Point3 triangulateNonlinear(const std::vector<Pose3>& poses,
  * @param initialEstimate
  * @return refined Point3
  */
-template<class CALIBRATION>
+template<class CAMERA>
 Point3 triangulateNonlinear(
-    const std::vector<PinholeCamera<CALIBRATION> >& cameras,
+    const std::vector<CAMERA>& cameras,
     const std::vector<Point2>& measurements, const Point3& initialEstimate) {
 
   // Create a factor graph and initial values
@@ -252,9 +252,9 @@ Point3 triangulatePoint3(const std::vector<Pose3>& poses,
  * @param optimize Flag to turn on nonlinear refinement of triangulation
  * @return Returns a Point3
  */
-template<class CALIBRATION>
+template<class CAMERA>
 Point3 triangulatePoint3(
-    const std::vector<PinholeCamera<CALIBRATION> >& cameras,
+    const std::vector<CAMERA>& cameras,
     const std::vector<Point2>& measurements, double rank_tol = 1e-9,
     bool optimize = false) {
 
@@ -265,11 +265,11 @@ Point3 triangulatePoint3(
     throw(TriangulationUnderconstrainedException());
 
   // construct projection matrices from poses & calibration
-  typedef PinholeCamera<CALIBRATION> Camera;
+  typedef PinholeCamera<typename CAMERA::CalibrationType> Camera;
   std::vector<Matrix34> projection_matrices;
   BOOST_FOREACH(const Camera& camera, cameras)
     projection_matrices.push_back(
-        CameraProjectionMatrix<CALIBRATION>(camera.calibration())(
+        CameraProjectionMatrix<typename CAMERA::CalibrationType>(camera.calibration())(
             camera.pose()));
   Point3 point = triangulateDLT(projection_matrices, measurements, rank_tol);
 

gtsam/slam/SmartProjectionFactor.h

@@ -243,7 +243,7 @@ public:
       // We triangulate the 3D position of the landmark
       try {
         // std::cout << "triangulatePoint3 i \n" << rankTolerance << std::endl;
-        point_ = triangulatePoint3<CALIBRATION>(cameras, this->measured_,
+        point_ = triangulatePoint3<Camera>(cameras, this->measured_,
             rankTolerance_, enableEPI_);
         degenerate_ = false;
         cheiralityException_ = false;

gtsam_unstable/slam/SmartStereoProjectionFactor.h

@@ -267,7 +267,7 @@ public:
           const Cal3_S2& K = camera.calibration()->calibration();
           mono_cameras.push_back(PinholeCamera<Cal3_S2>(pose, K));
         }
-        point_ = triangulatePoint3<Cal3_S2>(mono_cameras, mono_measurements,
+        point_ = triangulatePoint3<PinholeCamera<Cal3_S2> >(mono_cameras, mono_measurements,
             rankTolerance_, enableEPI_);
 
         // // // End temporary hack