final cosmetics

gtsam/slam/SmartProjectionRigFactor.h

@@ -71,6 +71,8 @@ class SmartProjectionRigFactor : public SmartProjectionFactor<CAMERA> {
   FastVector<size_t> cameraIds_;
 
  public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
   typedef CAMERA Camera;
   typedef CameraSet<CAMERA> Cameras;
 
@@ -127,7 +129,7 @@ class SmartProjectionRigFactor : public SmartProjectionFactor<CAMERA> {
   }
 
   /** Virtual destructor */
-  ~SmartProjectionRigFactor() override {}
+  ~SmartProjectionRigFactor() override = default;
 
   /**
    * add a new measurement, corresponding to an observation from pose "poseKey"
@@ -176,8 +178,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],
@@ -376,9 +378,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

@@ -1229,16 +1229,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

@@ -84,6 +84,9 @@ class SmartProjectionPoseFactorRollingShutter
   typedef std::vector<MatrixZD, Eigen::aligned_allocator<MatrixZD>>
       FBlocks;  // vector of F blocks
 
+  /// Default constructor, only for serialization
+  SmartProjectionPoseFactorRollingShutter() {}
+
   /**
    * Constructor
    * @param Isotropic measurement noise
@@ -197,8 +200,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,
@@ -275,6 +278,43 @@ 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 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;
+  }
+
+  /**
+   * 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
@@ -404,43 +444,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)
@@ -457,8 +460,8 @@ class SmartProjectionPoseFactorRollingShutter
         return this->createHessianFactor(values, lambda);
       default:
         throw std::runtime_error(
-            "SmartProjectionPoseFactorRollingShutter: unknown linearization "
+            "SmartProjectionPoseFactorRollingShutter: "
-            "mode");
+            "unknown linearization mode");
     }
   }