Only now it really works with StereoCamera

gtsam/geometry/CameraSet.h

@@ -30,12 +30,10 @@ namespace gtsam {
  * Assumes that a camera is laid out as 6 Pose3 parameters then calibration
  */
 template<class CAMERA>
-class CameraSet {
+class CameraSet: public std::vector<CAMERA> {
 
 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.
@@ -45,25 +43,8 @@ private:
   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() {
-  }
-
-  /// Add a new camera
-  void add(const CAMERA& camera) {
-    cameras_.push_back(camera);
-  }
-
-  /// Retrieve ith camera
-  const CAMERA& operator[](size_t i) const {
-    return cameras_[i];
-  }
-
   /**
    * print
    * @param s optional string naming the factor
@@ -71,15 +52,17 @@ public:
    */
   void print(const std::string& s = "") const {
     std::cout << s << "CameraSet, cameras = \n";
-    for (size_t k = 0; k < cameras_.size(); ++k)
+    for (size_t k = 0; k < this->size(); ++k)
-      cameras_[k].print();
+      this->at(k).print();
   }
 
   /// equals
   virtual bool equals(const CameraSet& p, double tol = 1e-9) const {
+    if (this->size() != p.size())
+      return false;
     bool camerasAreEqual = true;
-    for (size_t i = 0; i < cameras_.size(); i++) {
+    for (size_t i = 0; i < this->size(); i++) {
-      if (cameras_.at(i).equals(p.cameras_.at(i), tol) == false)
+      if (this->at(i).equals(p.at(i), tol) == false)
         camerasAreEqual = false;
       break;
     }
@@ -94,26 +77,20 @@ public:
       boost::none, boost::optional<Matrix&> E = boost::none,
       boost::optional<Matrix&> H = boost::none) const {
 
-    size_t nrCameras = cameras_.size();
+    size_t nrCameras = this->size();
-    if (F)
+    if (F) F->resize(ZDim * nrCameras, 6);
-      F->resize(ZDim * nrCameras, 6);
+    if (E) E->resize(ZDim * nrCameras, 3);
-    if (E)
+    if (H && Dim > 6) H->resize(ZDim * nrCameras, Dim - 6);
-      E->resize(ZDim * nrCameras, 3);
-    if (H && Dim > 6)
-      H->resize(ZDim * nrCameras, Dim - 6);
     std::vector<Z> z(nrCameras);
 
-    for (size_t i = 0; i < cameras_.size(); i++) {
+    for (size_t i = 0; i < nrCameras; i++) {
       Eigen::Matrix<double, ZDim, 6> Fi;
       Eigen::Matrix<double, ZDim, 3> Ei;
       Eigen::Matrix<double, ZDim, Dim - 6> Hi;
-      z[i] = cameras_[i].project(point, F ? &Fi : 0, E ? &Ei : 0, H ? &Hi : 0);
+      z[i] = this->at(i).project(point, F ? &Fi : 0, E ? &Ei : 0, H ? &Hi : 0);
-      if (F)
+      if (F) F->block<ZDim, 6>(ZDim * i, 0) = Fi;
-        F->block<ZDim, 6>(ZDim * i, 0) = Fi;
+      if (E) E->block<ZDim, 3>(ZDim * i, 0) = Ei;
-      if (E)
+      if (H) H->block<ZDim, Dim - 6>(ZDim * i, 0) = Hi;
-        E->block<ZDim, 3>(ZDim * i, 0) = Ei;
-      if (H)
-        H->block<ZDim, Dim - 6>(ZDim * i, 0) = Hi;
     }
     return z;
   }
@@ -124,7 +101,7 @@ private:
   friend class boost::serialization::access;
   template<class ARCHIVE>
   void serialize(ARCHIVE & ar, const unsigned int version) {
-    ar & cameras_;
+    ar & (*this);
   }
 };
 

gtsam/geometry/tests/testCameraSet.cpp

@@ -34,13 +34,13 @@ TEST(CameraSet, Pinhole) {
   typedef vector<Point2> ZZ;
   CameraSet<Camera> set;
   Camera camera;
-  set.add(camera);
+  set.push_back(camera);
-  set.add(camera);
+  set.push_back(camera);
   Point3 p(0, 0, 1);
   CHECK(assert_equal(set, set));
   CameraSet<Camera> set2 = set;
-  set2.add(camera);
+  set2.push_back(camera);
-  CHECK(!assert_equal(set,set2));
+  CHECK(!set.equals(set2));
 
   // Check measurements
   Point2 expected;
@@ -76,20 +76,20 @@ TEST(CameraSet, Stereo) {
   typedef vector<StereoPoint2> ZZ;
   CameraSet<StereoCamera> set;
   StereoCamera camera;
-  set.add(camera);
+  set.push_back(camera);
-  set.add(camera);
+  set.push_back(camera);
   Point3 p(0, 0, 1);
   EXPECT_LONGS_EQUAL(6, traits<StereoCamera>::dimension);
 
   // Check measurements
-  StereoPoint2 expected;
+  StereoPoint2 expected(0, -1, 0);
   ZZ z = set.project(p);
   CHECK(assert_equal(expected, z[0]));
   CHECK(assert_equal(expected, z[1]));
 
   // Calculate expected derivatives using Pinhole
-  Matrix46 actualF;
+  Matrix66 actualF;
-  Matrix43 actualE;
+  Matrix63 actualE;
   {
     Matrix36 F1;
     Matrix33 E1;