First draft of CameraSet

.cproject

@@ -1011,6 +1011,14 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
+			<target name="testCameraSet.run" path="build/gtsam/geometry/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j4</buildArguments>
+				<buildTarget>testCameraSet.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
 			<target name="all" path="spqr_mini" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>

gtsam/geometry/CameraSet.h

@@ -0,0 +1,126 @@
+/* ----------------------------------------------------------------------------
+
+ * 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   CameraSet.h
+ * @brief  Base class to create smart factors on poses or cameras
+ * @author Frank Dellaert
+ * @date   Feb 19, 2015
+ */
+
+#pragma once
+
+#include <gtsam/geometry/Point3.h>
+#include <gtsam/geometry/CalibratedCamera.h> // for Cheirality exception
+#include <boost/foreach.hpp>
+
+#include <vector>
+
+namespace gtsam {
+
+/**
+ * @brief A set of cameras, all with their own calibration
+ */
+template<class CAMERA>
+class CameraSet {
+
+private:
+
+  std::vector<CAMERA> cameras_;
+
+  /**
+   * 2D measurement and noise model for each of the m views
+   * The order is kept the same as the keys that we use to create the factor.
+   */
+  typedef typename CAMERA::Measurement Z;
+
+  static const int ZDim = traits<Z>::dimension; ///< Measurement dimension
+  static const int Dim = traits<CAMERA>::dimension; ///< Camera dimension
+
+  /// shorthand for this class
+  typedef CameraSet<CAMERA> This;
+
+public:
+
+  /// Default Constructor
+  CameraSet() {
+  }
+
+  /** Virtual destructor */
+  virtual ~CameraSet() {
+  }
+
+  /**
+   * Add a new camera
+   */
+  void add(const CAMERA& camera) {
+    cameras_.push_back(camera);
+  }
+
+  /**
+   * print
+   * @param s optional string naming the factor
+   * @param keyFormatter optional formatter useful for printing Symbols
+   */
+  void print(const std::string& s = "") const {
+    std::cout << s << "CameraSet, cameras = \n";
+    for (size_t k = 0; k < cameras_.size(); ++k)
+      cameras_[k]->print();
+  }
+
+  /// equals
+  virtual bool equals(const CameraSet& p, double tol = 1e-9) const {
+    bool camerasAreEqual = true;
+    for (size_t i = 0; i < cameras_.size(); i++) {
+      if (cameras_.at(i).equals(p.cameras_.at(i), tol) == false)
+        camerasAreEqual = false;
+      break;
+    }
+    return camerasAreEqual;
+  }
+
+  /**
+   *  project, with derivatives in this, point, and calibration
+   * throws CheiralityException
+   */
+  std::vector<Z> project(const Point3& point, boost::optional<Matrix&> F,
+      boost::optional<Matrix&> E, boost::optional<Matrix&> H) const {
+
+    size_t nrCameras = cameras_.size();
+    if (F) F->resize(ZDim * nrCameras, 6);
+    if (E) E->resize(ZDim * nrCameras, 3);
+    if (H) H->resize(ZDim * nrCameras, Dim - 6);
+    std::vector<Z> z(nrCameras);
+
+    for (size_t i = 0; i < cameras_.size(); i++) {
+      Matrix Fi(ZDim, 6), Ei(ZDim, 3), Hi(ZDim, Dim - 6);
+      z[i] = cameras_[i].project(point, F ? &Fi : 0, E ? &Ei : 0, H ? &Hi : 0);
+      if (F) F->block<ZDim, 6>(ZDim * i, 0) = Fi;
+      if (E) E->block<ZDim, 3>(ZDim * i, 0) = Ei;
+      if (H) H->block<ZDim, Dim - 6>(ZDim * i, 0) = Hi;
+    }
+    return z;
+  }
+
+private:
+
+  /// Serialization function
+  friend class boost::serialization::access;
+  template<class ARCHIVE>
+  void serialize(ARCHIVE & ar, const unsigned int version) {
+    ar & cameras_;
+  }
+};
+
+template<class CAMERA>
+const int CameraSet<CAMERA>::ZDim;
+
+} // \ namespace gtsam

gtsam/geometry/tests/testCameraSet.cpp

@@ -0,0 +1,51 @@
+/* ----------------------------------------------------------------------------
+
+ * 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   testCameraSet.cpp
+ *  @brief  Unit tests for testCameraSet Class
+ *  @author Frank Dellaert
+ *  @date   Feb 19, 2015
+ */
+
+#include <gtsam/geometry/CameraSet.h>
+#include <gtsam/geometry/Pose3.h>
+#include <CppUnitLite/TestHarness.h>
+
+using namespace std;
+using namespace gtsam;
+
+/* ************************************************************************* */
+#include <gtsam/geometry/PinholeCamera.h>
+#include <gtsam/geometry/Cal3Bundler.h>
+class PinholeSet: public CameraSet<PinholeCamera<Cal3Bundler> > {
+};
+
+TEST(CameraSet, Pinhole) {
+  PinholeSet f;
+}
+
+/* ************************************************************************* */
+#include <gtsam/geometry/StereoCamera.h>
+class StereoSet: public CameraSet<StereoCamera> {
+};
+
+TEST(CameraSet, Stereo) {
+  StereoSet f;
+}
+
+/* ************************************************************************* */
+int main() {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}
+/* ************************************************************************* */
+