Inherit constructors for CameraSets to switch to initializer lists.

gtsam/geometry/CameraSet.h

@@ -35,6 +35,7 @@ template<class CAMERA>
 class CameraSet : public std::vector<CAMERA, Eigen::aligned_allocator<CAMERA> > {
 
 protected:
+ using Base = std::vector<CAMERA, typename Eigen::aligned_allocator<CAMERA>>;
 
  /**
   * 2D measurement and noise model for each of the m views
@@ -47,9 +48,7 @@ protected:
  static const int ZDim = traits<Z>::dimension;    ///< Measurement dimension
 
  /// Make a vector of re-projection errors
-  static Vector ErrorVector(const ZVector& predicted,
+ static Vector ErrorVector(const ZVector& predicted, const ZVector& measured) {
-      const ZVector& measured) {
-
    // Check size
    size_t m = predicted.size();
    if (measured.size() != m)
@@ -59,7 +58,8 @@ protected:
    Vector b(ZDim * m);
    for (size_t i = 0, row = 0; i < m; i++, row += ZDim) {
      Vector bi = traits<Z>::Local(measured[i], predicted[i]);
-      if(ZDim==3 && std::isnan(bi(1))){ // if it is a stereo point and the right pixel is missing (nan)
+     if (ZDim == 3 && std::isnan(bi(1))) {  // if it is a stereo point and the
+                                            // right pixel is missing (nan)
        bi(1) = 0;
      }
      b.segment<ZDim>(row) = bi;
@@ -68,6 +68,7 @@ protected:
   }
 
 public:
+    using Base::Base;  // Inherit the vector constructors
 
     /// Destructor
     virtual ~CameraSet() = default;
@@ -83,8 +84,7 @@ public:
      */
     virtual void print(const std::string& s = "") const {
       std::cout << s << "CameraSet, cameras = \n";
-    for (size_t k = 0; k < this->size(); ++k)
+      for (size_t k = 0; k < this->size(); ++k) this->at(k).print(s);
-      this->at(k).print(s);
   }
 
   /// equals

gtsam/geometry/tests/testTriangulation.cpp

@@ -30,12 +30,8 @@
 #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
 #include <gtsam/slam/StereoFactor.h>
 
-#include <boost/assign.hpp>
-#include <boost/assign/std/vector.hpp>
-
 using namespace std;
 using namespace gtsam;
-using namespace boost::assign;
 
 // Some common constants
 
@@ -51,34 +47,34 @@ static const PinholeCamera<Cal3_S2> kCamera1(kPose1, *kSharedCal);
 static const Pose3 kPose2 = kPose1 * Pose3(Rot3(), Point3(1, 0, 0));
 static const PinholeCamera<Cal3_S2> kCamera2(kPose2, *kSharedCal);
 
-// landmark ~5 meters infront of camera
+static const std::vector<Pose3> kPoses = {kPose1, kPose2};
+
+
+// landmark ~5 meters in front of camera
 static const Point3 kLandmark(5, 0.5, 1.2);
 
 // 1. Project two landmarks into two cameras and triangulate
 static const Point2 kZ1 = kCamera1.project(kLandmark);
 static const Point2 kZ2 = kCamera2.project(kLandmark);
+static const Point2Vector kMeasurements{kZ1, kZ2};
 
 //******************************************************************************
 // Simple test with a well-behaved two camera situation
 TEST(triangulation, twoPoses) {
-  vector<Pose3> poses;
+  Point2Vector measurements = kMeasurements;
-  Point2Vector measurements;
-
-  poses += kPose1, kPose2;
-  measurements += kZ1, kZ2;
 
   double rank_tol = 1e-9;
 
   // 1. Test simple DLT, perfect in no noise situation
   bool optimize = false;
   boost::optional<Point3> actual1 =  //
-      triangulatePoint3<Cal3_S2>(poses, kSharedCal, measurements, rank_tol, optimize);
+      triangulatePoint3<Cal3_S2>(kPoses, kSharedCal, measurements, rank_tol, optimize);
   EXPECT(assert_equal(kLandmark, *actual1, 1e-7));
 
   // 2. test with optimization on, same answer
   optimize = true;
   boost::optional<Point3> actual2 =  //
-      triangulatePoint3<Cal3_S2>(poses, kSharedCal, measurements, rank_tol, optimize);
+      triangulatePoint3<Cal3_S2>(kPoses, kSharedCal, measurements, rank_tol, optimize);
   EXPECT(assert_equal(kLandmark, *actual2, 1e-7));
 
   // 3. Add some noise and try again: result should be ~ (4.995,
@@ -87,13 +83,13 @@ TEST(triangulation, twoPoses) {
   measurements.at(1) += Point2(-0.2, 0.3);
   optimize = false;
   boost::optional<Point3> actual3 =  //
-      triangulatePoint3<Cal3_S2>(poses, kSharedCal, measurements, rank_tol, optimize);
+      triangulatePoint3<Cal3_S2>(kPoses, kSharedCal, measurements, rank_tol, optimize);
   EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19814), *actual3, 1e-4));
 
   // 4. Now with optimization on
   optimize = true;
   boost::optional<Point3> actual4 =  //
-      triangulatePoint3<Cal3_S2>(poses, kSharedCal, measurements, rank_tol, optimize);
+      triangulatePoint3<Cal3_S2>(kPoses, kSharedCal, measurements, rank_tol, optimize);
   EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19814), *actual4, 1e-4));
 }
 
@@ -102,7 +98,7 @@ TEST(triangulation, twoCamerasUsingLOST) {
   cameras.push_back(kCamera1);
   cameras.push_back(kCamera2);
 
-  Point2Vector measurements = {kZ1, kZ2};
+  Point2Vector measurements = kMeasurements;
   SharedNoiseModel measurementNoise = noiseModel::Isotropic::Sigma(2, 1e-4);
   double rank_tol = 1e-9;
 
@@ -175,25 +171,21 @@ TEST(triangulation, twoPosesCal3DS2) {
   Point2 z1Distorted = camera1Distorted.project(kLandmark);
   Point2 z2Distorted = camera2Distorted.project(kLandmark);
 
-  vector<Pose3> poses;
+  Point2Vector measurements{z1Distorted, z2Distorted};
-  Point2Vector measurements;
-
-  poses += kPose1, kPose2;
-  measurements += z1Distorted, z2Distorted;
 
   double rank_tol = 1e-9;
 
   // 1. Test simple DLT, perfect in no noise situation
   bool optimize = false;
   boost::optional<Point3> actual1 =  //
-      triangulatePoint3<Cal3DS2>(poses, sharedDistortedCal, measurements,
+      triangulatePoint3<Cal3DS2>(kPoses, sharedDistortedCal, measurements,
                                  rank_tol, optimize);
   EXPECT(assert_equal(kLandmark, *actual1, 1e-7));
 
   // 2. test with optimization on, same answer
   optimize = true;
   boost::optional<Point3> actual2 =  //
-      triangulatePoint3<Cal3DS2>(poses, sharedDistortedCal, measurements,
+      triangulatePoint3<Cal3DS2>(kPoses, sharedDistortedCal, measurements,
                                  rank_tol, optimize);
   EXPECT(assert_equal(kLandmark, *actual2, 1e-7));
 
@@ -203,14 +195,14 @@ TEST(triangulation, twoPosesCal3DS2) {
   measurements.at(1) += Point2(-0.2, 0.3);
   optimize = false;
   boost::optional<Point3> actual3 =  //
-      triangulatePoint3<Cal3DS2>(poses, sharedDistortedCal, measurements,
+      triangulatePoint3<Cal3DS2>(kPoses, sharedDistortedCal, measurements,
                                  rank_tol, optimize);
   EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19814), *actual3, 1e-3));
 
   // 4. Now with optimization on
   optimize = true;
   boost::optional<Point3> actual4 =  //
-      triangulatePoint3<Cal3DS2>(poses, sharedDistortedCal, measurements,
+      triangulatePoint3<Cal3DS2>(kPoses, sharedDistortedCal, measurements,
                                  rank_tol, optimize);
   EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19814), *actual4, 1e-3));
 }
@@ -232,25 +224,21 @@ TEST(triangulation, twoPosesFisheye) {
   Point2 z1Distorted = camera1Distorted.project(kLandmark);
   Point2 z2Distorted = camera2Distorted.project(kLandmark);
 
-  vector<Pose3> poses;
+  Point2Vector measurements{z1Distorted, z2Distorted};
-  Point2Vector measurements;
-
-  poses += kPose1, kPose2;
-  measurements += z1Distorted, z2Distorted;
 
   double rank_tol = 1e-9;
 
   // 1. Test simple DLT, perfect in no noise situation
   bool optimize = false;
   boost::optional<Point3> actual1 =  //
-      triangulatePoint3<Calibration>(poses, sharedDistortedCal, measurements,
+      triangulatePoint3<Calibration>(kPoses, sharedDistortedCal, measurements,
                                      rank_tol, optimize);
   EXPECT(assert_equal(kLandmark, *actual1, 1e-7));
 
   // 2. test with optimization on, same answer
   optimize = true;
   boost::optional<Point3> actual2 =  //
-      triangulatePoint3<Calibration>(poses, sharedDistortedCal, measurements,
+      triangulatePoint3<Calibration>(kPoses, sharedDistortedCal, measurements,
                                      rank_tol, optimize);
   EXPECT(assert_equal(kLandmark, *actual2, 1e-7));
 
@@ -260,14 +248,14 @@ TEST(triangulation, twoPosesFisheye) {
   measurements.at(1) += Point2(-0.2, 0.3);
   optimize = false;
   boost::optional<Point3> actual3 =  //
-      triangulatePoint3<Calibration>(poses, sharedDistortedCal, measurements,
+      triangulatePoint3<Calibration>(kPoses, sharedDistortedCal, measurements,
                                      rank_tol, optimize);
   EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19814), *actual3, 1e-3));
 
   // 4. Now with optimization on
   optimize = true;
   boost::optional<Point3> actual4 =  //
-      triangulatePoint3<Calibration>(poses, sharedDistortedCal, measurements,
+      triangulatePoint3<Calibration>(kPoses, sharedDistortedCal, measurements,
                                      rank_tol, optimize);
   EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19814), *actual4, 1e-3));
 }
@@ -284,17 +272,13 @@ TEST(triangulation, twoPosesBundler) {
   Point2 z1 = camera1.project(kLandmark);
   Point2 z2 = camera2.project(kLandmark);
 
-  vector<Pose3> poses;
+  Point2Vector measurements{z1, z2};
-  Point2Vector measurements;
-
-  poses += kPose1, kPose2;
-  measurements += z1, z2;
 
   bool optimize = true;
   double rank_tol = 1e-9;
 
   boost::optional<Point3> actual =  //
-      triangulatePoint3<Cal3Bundler>(poses, bundlerCal, measurements, rank_tol,
+      triangulatePoint3<Cal3Bundler>(kPoses, bundlerCal, measurements, rank_tol,
                                      optimize);
   EXPECT(assert_equal(kLandmark, *actual, 1e-7));
 
@@ -303,19 +287,15 @@ TEST(triangulation, twoPosesBundler) {
   measurements.at(1) += Point2(-0.2, 0.3);
 
   boost::optional<Point3> actual2 =  //
-      triangulatePoint3<Cal3Bundler>(poses, bundlerCal, measurements, rank_tol,
+      triangulatePoint3<Cal3Bundler>(kPoses, bundlerCal, measurements, rank_tol,
                                      optimize);
   EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19847), *actual2, 1e-3));
 }
 
 //******************************************************************************
 TEST(triangulation, fourPoses) {
-  vector<Pose3> poses;
+  Pose3Vector poses = kPoses;
-  Point2Vector measurements;
+  Point2Vector measurements = kMeasurements;
-
-  poses += kPose1, kPose2;
-  measurements += kZ1, kZ2;
-
   boost::optional<Point3> actual =
       triangulatePoint3<Cal3_S2>(poses, kSharedCal, measurements);
   EXPECT(assert_equal(kLandmark, *actual, 1e-2));
@@ -334,8 +314,8 @@ TEST(triangulation, fourPoses) {
   PinholeCamera<Cal3_S2> camera3(pose3, *kSharedCal);
   Point2 z3 = camera3.project(kLandmark);
 
-  poses += pose3;
+  poses.push_back(pose3);
-  measurements += z3 + Point2(0.1, -0.1);
+  measurements.push_back(z3 + Point2(0.1, -0.1));
 
   boost::optional<Point3> triangulated_3cameras =  //
       triangulatePoint3<Cal3_S2>(poses, kSharedCal, measurements);
@@ -353,8 +333,8 @@ TEST(triangulation, fourPoses) {
 #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
   CHECK_EXCEPTION(camera4.project(kLandmark), CheiralityException);
 
-  poses += pose4;
+  poses.push_back(pose4);
-  measurements += Point2(400, 400);
+  measurements.emplace_back(400, 400);
 
   CHECK_EXCEPTION(triangulatePoint3<Cal3_S2>(poses, kSharedCal, measurements),
                   TriangulationCheiralityException);
@@ -368,10 +348,8 @@ TEST(triangulation, threePoses_robustNoiseModel) {
   PinholeCamera<Cal3_S2> camera3(pose3, *kSharedCal);
   Point2 z3 = camera3.project(kLandmark);
 
-  vector<Pose3> poses;
+  const vector<Pose3> poses{kPose1, kPose2, pose3};
-  Point2Vector measurements;
+  Point2Vector measurements{kZ1, kZ2, z3};
-  poses += kPose1, kPose2, pose3;
-  measurements += kZ1, kZ2, z3;
 
   // noise free, so should give exactly the landmark
   boost::optional<Point3> actual =
@@ -410,10 +388,9 @@ TEST(triangulation, fourPoses_robustNoiseModel) {
   PinholeCamera<Cal3_S2> camera3(pose3, *kSharedCal);
   Point2 z3 = camera3.project(kLandmark);
 
-  vector<Pose3> poses;
+  const vector<Pose3> poses{kPose1, kPose1, kPose2, pose3};
-  Point2Vector measurements;
+  // 2 measurements from pose 1:
-  poses += kPose1, kPose1, kPose2, pose3; // 2 measurements from pose 1
+  Point2Vector measurements{kZ1, kZ1, kZ2, z3};
-  measurements += kZ1, kZ1, kZ2, z3;
 
   // noise free, so should give exactly the landmark
   boost::optional<Point3> actual =
@@ -463,11 +440,8 @@ TEST(triangulation, fourPoses_distinct_Ks) {
   Point2 z1 = camera1.project(kLandmark);
   Point2 z2 = camera2.project(kLandmark);
 
-  CameraSet<PinholeCamera<Cal3_S2>> cameras;
+  CameraSet<PinholeCamera<Cal3_S2>> cameras{camera1, camera2};
-  Point2Vector measurements;
+  Point2Vector measurements{z1, z2};
-
-  cameras += camera1, camera2;
-  measurements += z1, z2;
 
   boost::optional<Point3> actual =  //
       triangulatePoint3<Cal3_S2>(cameras, measurements);
@@ -488,8 +462,8 @@ TEST(triangulation, fourPoses_distinct_Ks) {
   PinholeCamera<Cal3_S2> camera3(pose3, K3);
   Point2 z3 = camera3.project(kLandmark);
 
-  cameras += camera3;
+  cameras.push_back(camera3);
-  measurements += z3 + Point2(0.1, -0.1);
+  measurements.push_back(z3 + Point2(0.1, -0.1));
 
   boost::optional<Point3> triangulated_3cameras =  //
       triangulatePoint3<Cal3_S2>(cameras, measurements);
@@ -508,8 +482,8 @@ TEST(triangulation, fourPoses_distinct_Ks) {
 #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
   CHECK_EXCEPTION(camera4.project(kLandmark), CheiralityException);
 
-  cameras += camera4;
+  cameras.push_back(camera4);
-  measurements += Point2(400, 400);
+  measurements.emplace_back(400, 400);
   CHECK_EXCEPTION(triangulatePoint3<Cal3_S2>(cameras, measurements),
                   TriangulationCheiralityException);
 #endif
@@ -529,11 +503,8 @@ TEST(triangulation, fourPoses_distinct_Ks_distortion) {
   Point2 z1 = camera1.project(kLandmark);
   Point2 z2 = camera2.project(kLandmark);
 
-  CameraSet<PinholeCamera<Cal3DS2>> cameras;
+  const CameraSet<PinholeCamera<Cal3DS2>> cameras{camera1, camera2};
-  Point2Vector measurements;
+  const Point2Vector measurements{z1, z2};
-
-  cameras += camera1, camera2;
-  measurements += z1, z2;
 
   boost::optional<Point3> actual =  //
           triangulatePoint3<Cal3DS2>(cameras, measurements);
@@ -554,11 +525,8 @@ TEST(triangulation, outliersAndFarLandmarks) {
   Point2 z1 = camera1.project(kLandmark);
   Point2 z2 = camera2.project(kLandmark);
 
-  CameraSet<PinholeCamera<Cal3_S2>> cameras;
+  CameraSet<PinholeCamera<Cal3_S2>> cameras{camera1, camera2};
-  Point2Vector measurements;
+  Point2Vector measurements{z1, z2};
-
-  cameras += camera1, camera2;
-  measurements += z1, z2;
 
   double landmarkDistanceThreshold = 10;  // landmark is closer than that
   TriangulationParameters params(
@@ -582,8 +550,8 @@ TEST(triangulation, outliersAndFarLandmarks) {
   PinholeCamera<Cal3_S2> camera3(pose3, K3);
   Point2 z3 = camera3.project(kLandmark);
 
-  cameras += camera3;
+  cameras.push_back(camera3);
-  measurements += z3 + Point2(10, -10);
+  measurements.push_back(z3 + Point2(10, -10));
 
   landmarkDistanceThreshold = 10;  // landmark is closer than that
   double outlierThreshold = 100;   // loose, the outlier is going to pass
@@ -608,11 +576,8 @@ TEST(triangulation, twoIdenticalPoses) {
   // 1. Project two landmarks into two cameras and triangulate
   Point2 z1 = camera1.project(kLandmark);
 
-  vector<Pose3> poses;
+  const vector<Pose3> poses{kPose1, kPose1};
-  Point2Vector measurements;
+  const Point2Vector measurements{z1, z1};
-
-  poses += kPose1, kPose1;
-  measurements += z1, z1;
 
   CHECK_EXCEPTION(triangulatePoint3<Cal3_S2>(poses, kSharedCal, measurements),
                   TriangulationUnderconstrainedException);
@@ -623,22 +588,19 @@ TEST(triangulation, onePose) {
   // we expect this test to fail with a TriangulationUnderconstrainedException
   // because there's only one camera observation
 
-  vector<Pose3> poses;
+  const vector<Pose3> poses{Pose3()};
-  Point2Vector measurements;
+  const Point2Vector measurements {{0,0}};
-
-  poses += Pose3();
-  measurements += Point2(0, 0);
 
   CHECK_EXCEPTION(triangulatePoint3<Cal3_S2>(poses, kSharedCal, measurements),
                   TriangulationUnderconstrainedException);
 }
 
 //******************************************************************************
-TEST(triangulation, StereotriangulateNonlinear) {
+TEST(triangulation, StereoTriangulateNonlinear) {
   auto stereoK = boost::make_shared<Cal3_S2Stereo>(1733.75, 1733.75, 0, 689.645,
                                                    508.835, 0.0699612);
 
-  // two camera poses m1, m2
+  // two camera kPoses m1, m2
   Matrix4 m1, m2;
   m1 << 0.796888717, 0.603404026, -0.0295271487, 46.6673779, 0.592783835,
       -0.77156583, 0.230856632, 66.2186159, 0.116517574, -0.201470143,
@@ -648,14 +610,12 @@ TEST(triangulation, StereotriangulateNonlinear) {
       0.947083213, 0.131587097, 65.843136, -0.0206094928, 0.131334858,
       -0.991123524, -4.3525033, 0, 0, 0, 1;
 
-  typedef CameraSet<StereoCamera> Cameras;
+  typedef CameraSet<StereoCamera> StereoCameras;
-  Cameras cameras;
+  const StereoCameras cameras{{Pose3(m1), stereoK}, {Pose3(m2), stereoK}};
-  cameras.push_back(StereoCamera(Pose3(m1), stereoK));
-  cameras.push_back(StereoCamera(Pose3(m2), stereoK));
 
   StereoPoint2Vector measurements;
-  measurements += StereoPoint2(226.936, 175.212, 424.469);
+  measurements.push_back(StereoPoint2(226.936, 175.212, 424.469));
-  measurements += StereoPoint2(339.571, 285.547, 669.973);
+  measurements.push_back(StereoPoint2(339.571, 285.547, 669.973));
 
   Point3 initial =
       Point3(46.0536958, 66.4621179, -6.56285929);  // error: 96.5715555191
@@ -741,8 +701,6 @@ TEST(triangulation, StereotriangulateNonlinear) {
 //******************************************************************************
 // Simple test with a well-behaved two camera situation
 TEST(triangulation, twoPoses_sphericalCamera) {
-  vector<Pose3> poses;
-  std::vector<Unit3> measurements;
 
   // Project landmark into two cameras and triangulate
   SphericalCamera cam1(kPose1);
@@ -750,8 +708,7 @@ TEST(triangulation, twoPoses_sphericalCamera) {
   Unit3 u1 = cam1.project(kLandmark);
   Unit3 u2 = cam2.project(kLandmark);
 
-  poses += kPose1, kPose2;
+  std::vector<Unit3> measurements{u1, u2};
-  measurements += u1, u2;
 
   CameraSet<SphericalCamera> cameras;
   cameras.push_back(cam1);
@@ -803,9 +760,6 @@ TEST(triangulation, twoPoses_sphericalCamera) {
 
 //******************************************************************************
 TEST(triangulation, twoPoses_sphericalCamera_extremeFOV) {
-  vector<Pose3> poses;
-  std::vector<Unit3> measurements;
-
   // Project landmark into two cameras and triangulate
   Pose3 poseA = Pose3(
       Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2),
@@ -825,8 +779,7 @@ TEST(triangulation, twoPoses_sphericalCamera_extremeFOV) {
   EXPECT(assert_equal(Unit3(Point3(1.0, 0.0, -1.0)), u2,
                       1e-7));  // behind and to the right of PoseB
 
-  poses += kPose1, kPose2;
+  const std::vector<Unit3> measurements{u1, u2};
-  measurements += u1, u2;
 
   CameraSet<SphericalCamera> cameras;
   cameras.push_back(cam1);