refactoring: step 1

gtsam_unstable/slam/SmartStereoProjectionFactor.h

@@ -33,6 +33,12 @@
 
 namespace gtsam {
 
+/// Linearization mode: what factor to linearize to
+ enum LinearizationMode {
+   HESSIAN, IMPLICIT_SCHUR, JACOBIAN_Q, JACOBIAN_SVD
+ };
+
+
 /**
  * SmartStereoProjectionFactor: triangulates point
  */
@@ -82,11 +88,8 @@ public:
   /// shorthand for a smart pointer to a factor
   typedef boost::shared_ptr<This> shared_ptr;
 
-  /// shorthand for a StereoCamera // TODO: Get rid of this?
-  typedef StereoCamera Camera;
-
   /// Vector of cameras
-  typedef CameraSet<Camera> Cameras;
+  typedef CameraSet<StereoCamera> Cameras;
 
   /**
    * Constructor
@@ -195,7 +198,7 @@ public:
         }
 
         std::vector<PinholeCamera<Cal3_S2> > mono_cameras;
-        BOOST_FOREACH(const Camera& camera, cameras) {
+        BOOST_FOREACH(const StereoCamera& camera, cameras) {
           const Pose3& pose = camera.pose();
           const Cal3_S2& K = camera.calibration()->calibration();
           mono_cameras.push_back(PinholeCamera<Cal3_S2>(pose, K));
@@ -215,7 +218,7 @@ public:
         // Check landmark distance and reprojection errors to avoid outliers
         double totalReprojError = 0.0;
         size_t i = 0;
-        BOOST_FOREACH(const Camera& camera, cameras) {
+        BOOST_FOREACH(const StereoCamera& camera, cameras) {
           Point3 cameraTranslation = camera.pose().translation();
           // we discard smart factors corresponding to points that are far away
           if (cameraTranslation.distance(point_) > parameters_.landmarkDistanceThreshold) {
@@ -578,9 +581,6 @@ public:
     }
   }
 
-  /// Cameras are computed in derived class
-  virtual Cameras cameras(const Values& values) const = 0;
-
   /** return the landmark */
   boost::optional<Point3> point() const {
     return point_;

gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h

@@ -42,10 +42,7 @@ template<class CALIBRATION>
 class SmartStereoProjectionPoseFactor: public SmartStereoProjectionFactor<CALIBRATION> {
 
 public:
-  /// Linearization mode: what factor to linearize to
+
-   enum LinearizationMode {
-     HESSIAN, IMPLICIT_SCHUR, JACOBIAN_Q, JACOBIAN_SVD
-   };
 
 protected:
 
@@ -163,7 +160,7 @@ public:
     size_t i=0;
     BOOST_FOREACH(const Key& k, this->keys_) {
       Pose3 pose = values.at<Pose3>(k);
-      typename Base::Camera camera(pose, K_all_[i++]);
+      StereoCamera camera(pose, K_all_[i++]);
       cameras.push_back(camera);
     }
     return cameras;

gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp

@@ -326,15 +326,15 @@ TEST( SmartStereoProjectionPoseFactor, jacobianSVD ) {
       cam2, cam3, landmark3);
 
   SmartFactor::shared_ptr smartFactor1(
-      new SmartFactor(1, -1, false, false, SmartFactor::JACOBIAN_SVD));
+      new SmartFactor(1, -1, false, false, JACOBIAN_SVD));
   smartFactor1->add(measurements_cam1, views, model, K);
 
   SmartFactor::shared_ptr smartFactor2(
-      new SmartFactor(1, -1, false, false, SmartFactor::JACOBIAN_SVD));
+      new SmartFactor(1, -1, false, false, JACOBIAN_SVD));
   smartFactor2->add(measurements_cam2, views, model, K);
 
   SmartFactor::shared_ptr smartFactor3(
-      new SmartFactor(1, -1, false, false, SmartFactor::JACOBIAN_SVD));
+      new SmartFactor(1, -1, false, false, JACOBIAN_SVD));
   smartFactor3->add(measurements_cam3, views, model, K);
 
   const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
@@ -394,17 +394,17 @@ TEST( SmartStereoProjectionPoseFactor, landmarkDistance ) {
       cam2, cam3, landmark3);
 
   SmartFactor::shared_ptr smartFactor1(
-      new SmartFactor(1, -1, false, false, SmartFactor::JACOBIAN_SVD,
+      new SmartFactor(1, -1, false, false, JACOBIAN_SVD,
           excludeLandmarksFutherThanDist));
   smartFactor1->add(measurements_cam1, views, model, K);
 
   SmartFactor::shared_ptr smartFactor2(
-      new SmartFactor(1, -1, false, false, SmartFactor::JACOBIAN_SVD,
+      new SmartFactor(1, -1, false, false, JACOBIAN_SVD,
           excludeLandmarksFutherThanDist));
   smartFactor2->add(measurements_cam2, views, model, K);
 
   SmartFactor::shared_ptr smartFactor3(
-      new SmartFactor(1, -1, false, false, SmartFactor::JACOBIAN_SVD,
+      new SmartFactor(1, -1, false, false, JACOBIAN_SVD,
           excludeLandmarksFutherThanDist));
   smartFactor3->add(measurements_cam3, views, model, K);
 
@@ -472,22 +472,22 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ) {
   measurements_cam4.at(0) = measurements_cam4.at(0) + StereoPoint2(10, 10, 1); // add outlier
 
   SmartFactor::shared_ptr smartFactor1(
-      new SmartFactor(1, -1, false, false, SmartFactor::JACOBIAN_SVD,
+      new SmartFactor(1, -1, false, false, JACOBIAN_SVD,
           excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold));
   smartFactor1->add(measurements_cam1, views, model, K);
 
   SmartFactor::shared_ptr smartFactor2(
-      new SmartFactor(1, -1, false, false, SmartFactor::JACOBIAN_SVD,
+      new SmartFactor(1, -1, false, false, JACOBIAN_SVD,
           excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold));
   smartFactor2->add(measurements_cam2, views, model, K);
 
   SmartFactor::shared_ptr smartFactor3(
-      new SmartFactor(1, -1, false, false, SmartFactor::JACOBIAN_SVD,
+      new SmartFactor(1, -1, false, false, JACOBIAN_SVD,
           excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold));
   smartFactor3->add(measurements_cam3, views, model, K);
 
   SmartFactor::shared_ptr smartFactor4(
-      new SmartFactor(1, -1, false, false, SmartFactor::JACOBIAN_SVD,
+      new SmartFactor(1, -1, false, false, JACOBIAN_SVD,
           excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold));
   smartFactor4->add(measurements_cam4, views, model, K);