Merge branch 'feature/cameraTemplateForAllSmartFactors' into feature/sphericalCamera

# Conflicts:
#	gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h

gtsam/slam/SmartProjectionRigFactor.h

@@ -73,6 +73,8 @@ class SmartProjectionRigFactor : public SmartProjectionFactor<CAMERA> {
   FastVector<size_t> cameraIds_;
 
  public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
   typedef CAMERA Camera;
   typedef CameraSet<CAMERA> Cameras;
 
@@ -129,7 +131,7 @@ class SmartProjectionRigFactor : public SmartProjectionFactor<CAMERA> {
   }
 
   /** Virtual destructor */
-  ~SmartProjectionRigFactor() override {}
+  ~SmartProjectionRigFactor() override = default;
 
   /**
    * add a new measurement, corresponding to an observation from pose "poseKey"
@@ -178,8 +180,8 @@ class SmartProjectionRigFactor : public SmartProjectionFactor<CAMERA> {
     if (cameraIds.size() == 0 && cameraRig_.size() > 1) {
       throw std::runtime_error(
           "SmartProjectionRigFactor: "
-          "camera rig includes multiple camera but add did not input "
+          "camera rig includes multiple camera "
-          "cameraIds");
+          "but add did not input cameraIds");
     }
     for (size_t i = 0; i < measurements.size(); i++) {
       add(measurements[i], poseKeys[i],
@@ -378,9 +380,9 @@ class SmartProjectionRigFactor : public SmartProjectionFactor<CAMERA> {
   template <class ARCHIVE>
   void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
     ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
-    ar& BOOST_SERIALIZATION_NVP(nonUniqueKeys_);
+    //ar& BOOST_SERIALIZATION_NVP(nonUniqueKeys_);
-    ar& BOOST_SERIALIZATION_NVP(cameraRig_);
+    // ar& BOOST_SERIALIZATION_NVP(cameraRig_);
-    ar& BOOST_SERIALIZATION_NVP(cameraIds_);
+    //ar& BOOST_SERIALIZATION_NVP(cameraIds_);
   }
 };
 // end of class declaration

gtsam/slam/tests/testSmartProjectionRigFactor.cpp

@@ -1225,16 +1225,16 @@ TEST(SmartProjectionRigFactor, timing) {
   size_t nrTests = 10000;
 
   for (size_t i = 0; i < nrTests; i++) {
-    SmartRigFactor::shared_ptr smartFactorP(
+    SmartRigFactor::shared_ptr smartRigFactor(
         new SmartRigFactor(model, cameraRig, params));
-    smartFactorP->add(measurements_lmk1[0], x1, cameraId1);
+    smartRigFactor->add(measurements_lmk1[0], x1, cameraId1);
-    smartFactorP->add(measurements_lmk1[1], x1, cameraId1);
+    smartRigFactor->add(measurements_lmk1[1], x1, cameraId1);
 
     Values values;
     values.insert(x1, pose1);
     values.insert(x2, pose2);
     gttic_(SmartRigFactor_LINEARIZE);
-    smartFactorP->linearize(values);
+    smartRigFactor->linearize(values);
     gttoc_(SmartRigFactor_LINEARIZE);
   }
 

gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h

@@ -50,10 +50,6 @@ class SmartProjectionPoseFactorRollingShutter
   typedef typename CAMERA::Measurement MEASUREMENT;
   typedef typename CAMERA::MeasurementVector MEASUREMENTS;
 
-  static const int DimBlock = 12;  ///< size of the variable stacking 2 poses from which the observation pose is interpolated
-  static const int DimPose = 6;  ///< Pose3 dimension
-  static const int ZDim = 2;  ///< Measurement dimension (Point2)
-
  protected:
   /// The keys of the pose of the body (with respect to an external world
   /// frame): two consecutive poses for each observation
@@ -72,12 +68,20 @@ class SmartProjectionPoseFactorRollingShutter
   FastVector<size_t> cameraIds_;
 
  public:
- EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
- 
+
+  static const int DimBlock =
+      12;  ///< size of the variable stacking 2 poses from which the observation
+           ///< pose is interpolated
+  static const int DimPose = 6;  ///< Pose3 dimension
+  static const int ZDim = 2;     ///< Measurement dimension (Point2)
+  typedef Eigen::Matrix<double, ZDim, DimBlock>
+      MatrixZD;  // F blocks (derivatives wrt block of 2 poses)
+  typedef std::vector<MatrixZD, Eigen::aligned_allocator<MatrixZD>>
+      FBlocks;  // vector of F blocks
+
   typedef CAMERA Camera;
   typedef CameraSet<CAMERA> Cameras;
-  typedef Eigen::Matrix<double, ZDim, DimBlock> MatrixZD;  // F blocks (derivatives wrt block of 2 poses)
-  typedef std::vector<MatrixZD, Eigen::aligned_allocator<MatrixZD> > FBlocks;  // vector of F blocks
 
   /// shorthand for a smart pointer to a factor
   typedef boost::shared_ptr<This> shared_ptr;
@@ -198,8 +202,8 @@ class SmartProjectionPoseFactorRollingShutter
     if (cameraIds.size() == 0 && cameraRig_.size() > 1) {
       throw std::runtime_error(
           "SmartProjectionPoseFactorRollingShutter: "
-          "camera rig includes multiple camera but add did not input "
+          "camera rig includes multiple camera "
-          "cameraIds");
+          "but add did not input cameraIds");
     }
     for (size_t i = 0; i < measurements.size(); i++) {
       add(measurements[i], world_P_body_key_pairs[i].first,
@@ -276,6 +280,44 @@ class SmartProjectionPoseFactorRollingShutter
                       e->cameraIds().begin());
   }
 
+  /**
+   * Collect all cameras involved in this factor
+   * @param values Values structure which must contain camera poses
+   * corresponding to keys involved in this factor
+   * @return Cameras
+   */
+  typename Base::Cameras cameras(const Values& values) const override {
+    typename Base::Cameras cameras;
+    for (size_t i = 0; i < this->measured_.size();
+         i++) {  // for each measurement
+      const Pose3& w_P_body1 =
+          values.at<Pose3>(world_P_body_key_pairs_[i].first);
+      const Pose3& w_P_body2 =
+          values.at<Pose3>(world_P_body_key_pairs_[i].second);
+      double interpolationFactor = alphas_[i];
+      const Pose3& w_P_body =
+          interpolate<Pose3>(w_P_body1, w_P_body2, interpolationFactor);
+      const typename Base::Camera& camera_i = cameraRig_[cameraIds_[i]];
+      const Pose3& body_P_cam = camera_i.pose();
+      const Pose3& w_P_cam = w_P_body.compose(body_P_cam);
+      cameras.emplace_back(w_P_cam,
+                           make_shared<typename CAMERA::CalibrationType>(
+                               camera_i.calibration()));
+    }
+    return cameras;
+  }
+
+  /**
+   * error calculates the error of the factor.
+   */
+  double error(const Values& values) const override {
+    if (this->active(values)) {
+      return this->totalReprojectionError(this->cameras(values));
+    } else {  // else of active flag
+      return 0.0;
+    }
+  }
+
   /**
    * Compute jacobian F, E and error vector at a given linearization point
    * @param values Values structure which must contain camera poses
@@ -406,43 +448,6 @@ class SmartProjectionPoseFactorRollingShutter
         this->keys_, augmentedHessianUniqueKeys);
   }
 
-  /**
-   * error calculates the error of the factor.
-   */
-  double error(const Values& values) const override {
-    if (this->active(values)) {
-      return this->totalReprojectionError(this->cameras(values));
-    } else {  // else of active flag
-      return 0.0;
-    }
-  }
-
-  /**
-   * Collect all cameras involved in this factor
-   * @param values Values structure which must contain camera poses
-   * corresponding to keys involved in this factor
-   * @return Cameras
-   */
-  typename Base::Cameras cameras(const Values& values) const override {
-    typename Base::Cameras cameras;
-    for (size_t i = 0; i < this->measured_.size();
-         i++) {  // for each measurement
-      const Pose3& w_P_body1 =
-          values.at<Pose3>(world_P_body_key_pairs_[i].first);
-      const Pose3& w_P_body2 =
-          values.at<Pose3>(world_P_body_key_pairs_[i].second);
-      double interpolationFactor = alphas_[i];
-      const Pose3& w_P_body =
-          interpolate<Pose3>(w_P_body1, w_P_body2, interpolationFactor);
-      const Pose3& body_P_cam = cameraRig_[cameraIds_[i]].pose();
-      const Pose3& w_P_cam = w_P_body.compose(body_P_cam);
-      cameras.emplace_back(w_P_cam,
-                           make_shared<typename CAMERA::CalibrationType>(
-                               cameraRig_[cameraIds_[i]].calibration()));
-    }
-    return cameras;
-  }
-
   /**
    * Linearize to Gaussian Factor (possibly adding a damping factor Lambda for
    * LM)
@@ -459,8 +464,8 @@ class SmartProjectionPoseFactorRollingShutter
         return this->createHessianFactor(values, lambda);
       default:
         throw std::runtime_error(
-            "SmartProjectionPoseFactorRollingShutter: unknown linearization "
+            "SmartProjectionPoseFactorRollingShutter: "
-            "mode");
+            "unknown linearization mode");
     }
   }