Cleaned up, made default constructor a working StereoCamera

gtsam/geometry/StereoCamera.h

@@ -28,10 +28,11 @@ namespace gtsam {
 
 class GTSAM_EXPORT StereoCheiralityException: public std::runtime_error {
 public:
-  StereoCheiralityException() : std::runtime_error("Stereo Cheirality Exception") {}
+  StereoCheiralityException() :
+      std::runtime_error("Stereo Cheirality Exception") {
+  }
 };
 
-
 /**
  * A stereo camera class, parameterize by left camera pose and stereo calibration
  * @addtogroup geometry
@@ -52,16 +53,22 @@ private:
 
 public:
 
-  enum { dimension = 6 };
+  enum {
+    dimension = 6
+  };
 
   /// @name Standard Constructors
   /// @{
 
-  StereoCamera() {
+  /// Default constructor allocates a calibration!
+  StereoCamera() :
+      K_(new Cal3_S2Stereo()) {
   }
 
+  /// Construct from pose and shared calibration
   StereoCamera(const Pose3& leftCamPose, const Cal3_S2Stereo::shared_ptr K);
 
+  /// Return shared pointer to calibration
   const Cal3_S2Stereo::shared_ptr calibration() const {
     return K_;
   }
@@ -70,26 +77,28 @@ public:
   /// @name Testable
   /// @{
 
+  /// print
   void print(const std::string& s = "") const {
     leftCamPose_.print(s + ".camera.");
     K_->print(s + ".calibration.");
   }
 
+  /// equals
   bool equals(const StereoCamera &camera, double tol = 1e-9) const {
-    return leftCamPose_.equals(camera.leftCamPose_, tol) && K_->equals(
+    return leftCamPose_.equals(camera.leftCamPose_, tol)
-        *camera.K_, tol);
+        && K_->equals(*camera.K_, tol);
   }
 
   /// @}
   /// @name Manifold
   /// @{
 
-  /** Dimensionality of the tangent space */
+  /// Dimensionality of the tangent space
   inline size_t dim() const {
     return 6;
   }
 
-  /** Dimensionality of the tangent space */
+  /// Dimensionality of the tangent space
   static inline size_t Dim() {
     return 6;
   }
@@ -108,10 +117,12 @@ public:
   /// @name Standard Interface
   /// @{
 
+  /// pose
   const Pose3& pose() const {
     return leftCamPose_;
   }
 
+  /// baseline
   double baseline() const {
     return K_->baseline();
   }
@@ -122,19 +133,16 @@ public:
    * @param H2 derivative with respect to point
    * @param H3 IGNORED (for calibration)
    */
-  StereoPoint2 project(const Point3& point,
+  StereoPoint2 project(const Point3& point, OptionalJacobian<3, 6> H1 =
-      OptionalJacobian<3, 6> H1 = boost::none,
+      boost::none, OptionalJacobian<3, 3> H2 = boost::none,
-      OptionalJacobian<3, 3> H2 = boost::none,
       OptionalJacobian<3, 0> H3 = boost::none) const;
 
-  /**
+  /// back-project a measurement
-   *
-   */
   Point3 backproject(const StereoPoint2& z) const {
     Vector measured = z.vector();
-    double Z = K_->baseline()*K_->fx()/(measured[0]-measured[1]);
+    double Z = K_->baseline() * K_->fx() / (measured[0] - measured[1]);
-    double X = Z *(measured[0]- K_->px()) / K_->fx();
+    double X = Z * (measured[0] - K_->px()) / K_->fx();
-    double Y = Z *(measured[2]- K_->py()) / K_->fy();
+    double Y = Z * (measured[2] - K_->py()) / K_->fy();
     Point3 world_point = leftCamPose_.transform_from(Point3(X, Y, Z));
     return world_point;
   }
@@ -142,7 +150,8 @@ public:
   /// @}
 
 private:
-  /** utility functions */
+
+  /// utility function
   Matrix3 Dproject_to_stereo_camera1(const Point3& P) const;
 
   friend class boost::serialization::access;
@@ -155,8 +164,10 @@ private:
 };
 
 template<>
-struct traits<StereoCamera> : public internal::Manifold<StereoCamera> {};
+struct traits<StereoCamera> : public internal::Manifold<StereoCamera> {
+};
 
 template<>
-struct traits<const StereoCamera> : public internal::Manifold<StereoCamera> {};
+struct traits<const StereoCamera> : public internal::Manifold<StereoCamera> {
+};
 }