adjusted rolling shutter as well

gtsam/geometry/SphericalCamera.h

@@ -30,15 +30,34 @@
 namespace gtsam {
 
 class GTSAM_EXPORT EmptyCal {
+ protected:
+  Matrix3 K_;
  public:
-  EmptyCal(){}
+
+  ///< shared pointer to calibration object
+  EmptyCal()
+      : K_(Matrix3::Identity()) {
+  }
+  /// Default destructor
   virtual ~EmptyCal() = default;
   using shared_ptr = boost::shared_ptr<EmptyCal>;
   void print(const std::string& s) const {
     std::cout << "empty calibration: " <<  s << std::endl;
   }
+  Matrix3 K() const {return K_;}
 };
 
+//
+//class GTSAM_EXPORT EmptyCal {
+// public:
+//  EmptyCal(){}
+//  virtual ~EmptyCal() = default;
+//  using shared_ptr = boost::shared_ptr<EmptyCal>;
+//  void print(const std::string& s) const {
+//    std::cout << "empty calibration: " <<  s << std::endl;
+//  }
+//};
+
 /**
  * A spherical camera class that has a Pose3 and measures bearing vectors
  * @addtogroup geometry

gtsam/slam/SmartProjectionFactor.h

@@ -70,6 +70,7 @@ public:
   typedef boost::shared_ptr<This> shared_ptr;
 
   /// shorthand for a set of cameras
+  typedef CAMERA Camera;
   typedef CameraSet<CAMERA> Cameras;
 
   /**

gtsam/slam/SmartProjectionFactorP.h

@@ -60,7 +60,6 @@ class SmartProjectionFactorP : public SmartProjectionFactor<CAMERA> {
   static const int ZDim = 2;  ///< Measurement dimension (Point2)
 
  protected:
-
   /// vector of keys (one for each observation) with potentially repeated keys
   std::vector<Key> nonUniqueKeys_;
 

gtsam/slam/tests/smartFactorScenarios.h

@@ -110,6 +110,7 @@ Camera cam2(pose_right, K);
 Camera cam3(pose_above, K);
 typedef GeneralSFMFactor<Camera, Point3> SFMFactor;
 }
+
 /* *************************************************************************/
 // Cal3Bundler poses
 namespace bundlerPose {

gtsam/slam/tests/testSmartProjectionFactorP.cpp

@@ -1107,7 +1107,7 @@ TEST( SmartProjectionFactorP, optimization_3poses_sphericalCamera ) {
   emptyKs.push_back(emptyK);
 
   SmartProjectionParams params;
-  params.setRankTolerance(0.01);
+  params.setRankTolerance(0.1);
 
   SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(model,params));
   smartFactor1->add(measurements_lmk1, keys, emptyKs);

gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h

@@ -40,8 +40,16 @@ namespace gtsam {
 template<class CAMERA>
 class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor<CAMERA> {
 
- public:
+ private:
+  typedef SmartProjectionFactor<CAMERA> Base;
+  typedef SmartProjectionPoseFactorRollingShutter<CAMERA> This;
   typedef typename CAMERA::CalibrationType CALIBRATION;
+  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:
   /// shared pointer to calibration object (one for each observation)
@@ -57,22 +65,17 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor<CAM
   std::vector<Pose3> body_P_sensors_;
 
  public:
-  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  typedef CAMERA Camera;
-
+  typedef CameraSet<CAMERA> Cameras;
-  /// shorthand for base class type
+  typedef Eigen::Matrix<double, ZDim, DimBlock> MatrixZD;  // F blocks (derivatives wrt block of 2 poses)
-  typedef SmartProjectionFactor<PinholePose<CALIBRATION> > Base;
+  typedef std::vector<MatrixZD, Eigen::aligned_allocator<MatrixZD> > FBlocks;  // vector of F blocks
-
-  /// shorthand for this class
-  typedef SmartProjectionPoseFactorRollingShutter This;
 
   /// shorthand for a smart pointer to a factor
   typedef boost::shared_ptr<This> shared_ptr;
 
-  static const int DimBlock = 12;  ///< size of the variable stacking 2 poses from which the observation pose is interpolated
+  /// Default constructor, only for serialization
-  static const int DimPose = 6;  ///< Pose3 dimension
+  SmartProjectionPoseFactorRollingShutter() {
-  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
 
   /**
    * Constructor
@@ -83,6 +86,9 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor<CAM
       const SharedNoiseModel& sharedNoiseModel,
       const SmartProjectionParams& params = SmartProjectionParams())
   : Base(sharedNoiseModel, params) {
+    // use only configuration that works with this factor
+    Base::params_.degeneracyMode = gtsam::ZERO_ON_DEGENERACY;
+    Base::params_.linearizationMode = gtsam::HESSIAN;
   }
 
   /** Virtual destructor */
@@ -98,7 +104,7 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor<CAM
    * @param K (fixed) camera intrinsic calibration
    * @param body_P_sensor (fixed) camera extrinsic calibration
    */
-  void add(const Point2& measured, const Key& world_P_body_key1,
+  void add(const MEASUREMENT& measured, const Key& world_P_body_key1,
            const Key& world_P_body_key2, const double& alpha,
            const boost::shared_ptr<CALIBRATION>& K, const Pose3 body_P_sensor = Pose3::identity()) {
     // store measurements in base class
@@ -134,7 +140,7 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor<CAM
    * @param Ks vector of (fixed) intrinsic calibration objects
    * @param body_P_sensors vector of (fixed) extrinsic calibration objects
    */
-  void add(const Point2Vector& measurements,
+  void add(const MEASUREMENTS& measurements,
            const std::vector<std::pair<Key, Key>>& world_P_body_key_pairs,
            const std::vector<double>& alphas,
            const std::vector<boost::shared_ptr<CALIBRATION>>& Ks,
@@ -160,7 +166,7 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor<CAM
    * @param K (fixed) camera intrinsic calibration (same for all measurements)
    * @param body_P_sensor (fixed) camera extrinsic calibration (same for all measurements)
    */
-  void add(const Point2Vector& measurements,
+  void add(const MEASUREMENTS& measurements,
            const std::vector<std::pair<Key, Key>>& world_P_body_key_pairs,
            const std::vector<double>& alphas,
            const boost::shared_ptr<CALIBRATION>& K, const Pose3 body_P_sensor = Pose3::identity()) {
@@ -244,6 +250,43 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor<CAM
         && alphas_ == e->alphas() && keyPairsEqual && extrinsicCalibrationEqual;
   }
 
+  /**
+   * 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 {
+    size_t numViews = this->measured_.size();
+    assert(numViews == K_all_.size());
+    assert(numViews == alphas_.size());
+    assert(numViews == body_P_sensors_.size());
+    assert(numViews == world_P_body_key_pairs_.size());
+
+    typename Base::Cameras cameras;
+    for (size_t i = 0; i < numViews; 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 = body_P_sensors_[i];
+      const Pose3& w_P_cam = w_P_body.compose(body_P_cam);
+      cameras.emplace_back(w_P_cam, K_all_[i]);
+    }
+    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
@@ -274,12 +317,11 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor<CAM
         const Pose3& w_P_body = interpolate<Pose3>(w_P_body1, w_P_body2,interpolationFactor, dInterpPose_dPoseBody1, dInterpPose_dPoseBody2);
         const Pose3& body_P_cam = body_P_sensors_[i];
         const Pose3& w_P_cam = w_P_body.compose(body_P_cam, dPoseCam_dInterpPose);
-        PinholeCamera<CALIBRATION> camera(w_P_cam, *K_all_[i]);
+        typename Base::Camera camera(w_P_cam, K_all_[i]);
 
         // get jacobians and error vector for current measurement
-        Point2 reprojectionError_i = Point2(
+        Point2 reprojectionError_i = camera.reprojectionError(
-            camera.project(*this->result_, dProject_dPoseCam, Ei)
+            *this->result_, this->measured_.at(i), dProject_dPoseCam, Ei);
-            - this->measured_.at(i));
         Eigen::Matrix<double, ZDim, DimBlock> J;  // 2 x 12
         J.block(0, 0, ZDim, 6) = dProject_dPoseCam * dPoseCam_dInterpPose
             * dInterpPose_dPoseBody1;  // (2x6) * (6x6) * (6x6)
@@ -340,7 +382,7 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor<CAM
     for (size_t i = 0; i < Fs.size(); i++)
       Fs[i] = this->noiseModel_->Whiten(Fs[i]);
 
-    Matrix3 P = Base::Cameras::PointCov(E, lambda, diagonalDamping);
+    Matrix3 P = Cameras::PointCov(E, lambda, diagonalDamping);
 
     // Collect all the key pairs: these are the keys that correspond to the blocks in Fs (on which we apply the Schur Complement)
     KeyVector nonuniqueKeys;
@@ -352,50 +394,13 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor<CAM
     // Build augmented Hessian (with last row/column being the information vector)
     // Note: we need to get the augumented hessian wrt the unique keys in key_
     SymmetricBlockMatrix augmentedHessianUniqueKeys =
-        Base::Cameras::SchurComplementAndRearrangeBlocks_3_12_6(
+        Cameras::SchurComplementAndRearrangeBlocks_3_12_6(
             Fs, E, P, b, nonuniqueKeys, this->keys_);
 
     return boost::make_shared < RegularHessianFactor<DimPose>
         > (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 {
-    size_t numViews = this->measured_.size();
-    assert(numViews == K_all_.size());
-    assert(numViews == alphas_.size());
-    assert(numViews == body_P_sensors_.size());
-    assert(numViews == world_P_body_key_pairs_.size());
-
-    typename Base::Cameras cameras;
-    for (size_t i = 0; i < numViews; 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 = body_P_sensors_[i];
-      const Pose3& w_P_cam = w_P_body.compose(body_P_cam);
-      cameras.emplace_back(w_P_cam, K_all_[i]);
-    }
-    return cameras;
-  }
-
   /**
    * Linearize to Gaussian Factor (possibly adding a damping factor Lambda for LM)
    * @param values Values structure which must contain camera poses and extrinsic pose for this factor

gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp

@@ -19,6 +19,7 @@
 #include "gtsam/slam/tests/smartFactorScenarios.h"
 #include <gtsam/slam/ProjectionFactor.h>
 #include <gtsam/slam/PoseTranslationPrior.h>
+#include <gtsam/geometry/SphericalCamera.h>
 #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
 #include <gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h>
 #include <gtsam_unstable/slam/ProjectionFactorRollingShutter.h>
@@ -72,6 +73,20 @@ Camera cam2(interp_pose2, sharedK);
 Camera cam3(interp_pose3, sharedK);
 }
 
+/* ************************************************************************* */
+// default Cal3_S2 poses with rolling shutter effect
+namespace sphericalCameraRS {
+typedef SphericalCamera Camera;
+typedef SmartProjectionPoseFactorRollingShutter<Camera> SmartFactorRS_spherical;
+Pose3 interp_pose1 = interpolate<Pose3>(level_pose,pose_right,interp_factor1);
+Pose3 interp_pose2 = interpolate<Pose3>(pose_right,pose_above,interp_factor2);
+Pose3 interp_pose3 = interpolate<Pose3>(pose_above,level_pose,interp_factor3);
+static EmptyCal::shared_ptr emptyK;
+Camera cam1(interp_pose1, emptyK);
+Camera cam2(interp_pose2, emptyK);
+Camera cam3(interp_pose3, emptyK);
+}
+
 LevenbergMarquardtParams lmParams;
 typedef SmartProjectionPoseFactorRollingShutter< PinholePose<Cal3_S2> > SmartFactorRS;
 
@@ -1040,6 +1055,79 @@ TEST( SmartProjectionPoseFactorRollingShutter, timing ) {
 }
 #endif
 
+/* *************************************************************************/
+TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_sphericalCameras ) {
+
+  using namespace sphericalCameraRS;
+  std::vector<Unit3> measurements_lmk1, measurements_lmk2, measurements_lmk3;
+
+  // Project three landmarks into three cameras
+  projectToMultipleCameras<Camera>(cam1, cam2, cam3, landmark1, measurements_lmk1);
+  projectToMultipleCameras<Camera>(cam1, cam2, cam3, landmark2, measurements_lmk2);
+  projectToMultipleCameras<Camera>(cam1, cam2, cam3, landmark3, measurements_lmk3);
+
+  // create inputs
+  std::vector<std::pair<Key,Key>> key_pairs;
+  key_pairs.push_back(std::make_pair(x1,x2));
+  key_pairs.push_back(std::make_pair(x2,x3));
+  key_pairs.push_back(std::make_pair(x3,x1));
+
+  std::vector<double> interp_factors;
+  interp_factors.push_back(interp_factor1);
+  interp_factors.push_back(interp_factor2);
+  interp_factors.push_back(interp_factor3);
+
+  SmartProjectionParams params;
+  params.setRankTolerance(0.1);
+
+  SmartFactorRS_spherical::shared_ptr smartFactor1(
+      new SmartFactorRS_spherical(model,params));
+  smartFactor1->add(measurements_lmk1, key_pairs, interp_factors, emptyK);
+
+  SmartFactorRS_spherical::shared_ptr smartFactor2(
+      new SmartFactorRS_spherical(model,params));
+  smartFactor2->add(measurements_lmk2, key_pairs, interp_factors, emptyK);
+
+  SmartFactorRS_spherical::shared_ptr smartFactor3(
+      new SmartFactorRS_spherical(model,params));
+  smartFactor3->add(measurements_lmk3, key_pairs, interp_factors, emptyK);
+
+  const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
+
+  NonlinearFactorGraph graph;
+  graph.push_back(smartFactor1);
+  graph.push_back(smartFactor2);
+  graph.push_back(smartFactor3);
+  graph.addPrior(x1, level_pose, noisePrior);
+  graph.addPrior(x2, pose_right, noisePrior);
+
+  Values groundTruth;
+  groundTruth.insert(x1, level_pose);
+  groundTruth.insert(x2, pose_right);
+  groundTruth.insert(x3, pose_above);
+  DOUBLES_EQUAL(0, graph.error(groundTruth), 1e-9);
+
+  //  Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
+  Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
+                           Point3(0.1, 0.1, 0.1)); // smaller noise
+  Values values;
+  values.insert(x1, level_pose);
+  values.insert(x2, pose_right);
+  // initialize third pose with some noise, we expect it to move back to original pose_above
+  values.insert(x3, pose_above * noise_pose);
+  EXPECT( // check that the pose is actually noisy
+      assert_equal(
+          Pose3(
+              Rot3(0, -0.0314107591, 0.99950656, -0.99950656, -0.0313952598,
+                   -0.000986635786, 0.0314107591, -0.999013364, -0.0313952598),
+                   Point3(0.1, -0.1, 1.9)), values.at<Pose3>(x3)));
+
+  Values result;
+  LevenbergMarquardtOptimizer optimizer(graph, values, lmParams);
+  result = optimizer.optimize();
+  EXPECT(assert_equal(pose_above, result.at<Pose3>(x3), 1e-6));
+}
+
 /* ************************************************************************* */
 int main() {
   TestResult tr;