all set here!

gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h

@@ -49,9 +49,6 @@ class SmartProjectionPoseFactorRollingShutter
   typedef typename CAMERA::CalibrationType CALIBRATION;
 
  protected:
-  /// shared pointer to calibration object (one for each observation)
-  std::vector<boost::shared_ptr<CALIBRATION>> K_all_;
-
   /// The keys of the pose of the body (with respect to an external world
   /// frame): two consecutive poses for each observation
   std::vector<std::pair<Key, Key>> world_P_body_key_pairs_;
@@ -60,10 +57,10 @@ class SmartProjectionPoseFactorRollingShutter
   /// pair of consecutive poses
   std::vector<double> alphas_;
 
-  /// one or more cameras in the rig (fixed poses wrt body + fixed intrinsics)
+  /// one or more cameras taking observations (fixed poses wrt body + fixed intrinsics)
   typename Base::Cameras cameraRig_;
 
-  /// vector of camera Ids (one for each observation, in the same order), identifying which camera in the rig took the measurement
+  /// vector of camera Ids (one for each observation, in the same order), identifying which camera took the measurement
   FastVector<size_t> cameraIds_;
 
  public:
@@ -87,6 +84,7 @@ class SmartProjectionPoseFactorRollingShutter
   /**
    * Constructor
    * @param Isotropic measurement noise
+   * @param cameraRig set of cameras (fixed poses wrt body and intrinsics) taking the measurements
    * @param params internal parameters of the smart factors
    */
   SmartProjectionPoseFactorRollingShutter(
@@ -102,6 +100,7 @@ class SmartProjectionPoseFactorRollingShutter
   /**
    * Constructor
    * @param Isotropic measurement noise
+   * @param camera single camera (fixed poses wrt body and intrinsics)
    * @param params internal parameters of the smart factors
    */
   SmartProjectionPoseFactorRollingShutter(
@@ -114,13 +113,11 @@ class SmartProjectionPoseFactorRollingShutter
     cameraRig_.push_back(camera);
   }
 
-
   /** Virtual destructor */
   ~SmartProjectionPoseFactorRollingShutter() override = default;
 
   /**
-   * add a new measurement, with 2 pose keys, interpolation factor, camera
+   * add a new measurement, with 2 pose keys, interpolation factor, and cameraId
-   * (intrinsic and extrinsic) calibration, and observed pixel.
    * @param measured 2-dimensional location of the projection of a single
    *  landmark in a single view (the measurement), interpolated from the 2 poses
    * @param world_P_body_key1 key corresponding to the first body poses (time <=
@@ -129,8 +126,7 @@ class SmartProjectionPoseFactorRollingShutter
    *  >= time pixel is acquired)
    * @param alpha interpolation factor in [0,1], such that if alpha = 0 the
    *  interpolated pose is the same as world_P_body_key1
-   * @param K (fixed) camera intrinsic calibration
+   * @param cameraId ID of the camera taking the measurement (default 0)
-   * @param body_P_sensor (fixed) camera extrinsic calibration
    */
   void add(const Point2& measured, const Key& world_P_body_key1,
            const Key& world_P_body_key2, const double& alpha,
@@ -168,8 +164,7 @@ class SmartProjectionPoseFactorRollingShutter
    *  for the i0-th measurement can be interpolated
    * @param alphas vector of interpolation params (in [0,1]), one for each
    * measurement (in the same order)
-   * @param Ks vector of (fixed) intrinsic calibration objects
+   * @param cameraIds IDs of the cameras taking each measurement (same order as the measurements)
-   * @param body_P_sensors vector of (fixed) extrinsic calibration objects
    */
   void add(const Point2Vector& measurements,
            const std::vector<std::pair<Key, Key>>& world_P_body_key_pairs,
@@ -223,7 +218,7 @@ class SmartProjectionPoseFactorRollingShutter
       const std::string& s = "",
       const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
     std::cout << s << "SmartProjectionPoseFactorRollingShutter: \n ";
-    for (size_t i = 0; i < K_all_.size(); i++) {
+    for (size_t i = 0; i < cameraIds_.size(); i++) {
       std::cout << "-- Measurement nr " << i << std::endl;
       std::cout << " pose1 key: "
                 << keyFormatter(world_P_body_key_pairs_[i].first) << std::endl;
@@ -409,14 +404,8 @@ class SmartProjectionPoseFactorRollingShutter
    * @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
+    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 =

gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp

@@ -496,6 +496,91 @@ TEST(SmartProjectionPoseFactorRollingShutter, optimization_3poses_multiCam) {
   EXPECT(assert_equal(pose_above, result.at<Pose3>(x3), 1e-6));
 }
 
+/* *************************************************************************/
+TEST(SmartProjectionPoseFactorRollingShutter, optimization_3poses_multiCam2) {
+  using namespace vanillaPoseRS;
+
+  Point2Vector measurements_lmk1, measurements_lmk2, measurements_lmk3;
+
+  // create arbitrary body_T_sensor (transforms from sensor to body)
+  Pose3 body_T_sensor1 = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2),
+                              Point3(1, 1, 1));
+  Pose3 body_T_sensor2 = Pose3(Rot3::Ypr(-M_PI / 5, 0., -M_PI / 2),
+                              Point3(0, 0, 1));
+  Pose3 body_T_sensor3 = Pose3::identity();
+
+  Camera camera1(interp_pose1*body_T_sensor1, sharedK);
+  Camera camera2(interp_pose2*body_T_sensor2, sharedK);
+  Camera camera3(interp_pose3*body_T_sensor3, sharedK);
+
+  // Project three landmarks into three cameras
+  projectToMultipleCameras(camera1, camera2, camera3, landmark1, measurements_lmk1);
+  projectToMultipleCameras(camera1, camera2, camera3, landmark2, measurements_lmk2);
+  projectToMultipleCameras(camera1, camera2, camera3, 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);
+
+  Cameras cameraRig;
+  cameraRig.push_back(Camera(body_T_sensor1, sharedK));
+  cameraRig.push_back(Camera(body_T_sensor2, sharedK));
+  cameraRig.push_back(Camera(body_T_sensor3, sharedK));
+
+  SmartFactorRS::shared_ptr smartFactor1(new SmartFactorRS(model, cameraRig));
+  smartFactor1->add(measurements_lmk1, key_pairs, interp_factors, {0,1,2});
+
+  SmartFactorRS::shared_ptr smartFactor2(new SmartFactorRS(model, cameraRig));
+  smartFactor2->add(measurements_lmk2, key_pairs, interp_factors, {0,1,2});
+
+  SmartFactorRS::shared_ptr smartFactor3(new SmartFactorRS(model, cameraRig));
+  smartFactor3->add(measurements_lmk3, key_pairs, interp_factors, {0,1,2});
+
+  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); // pose above is the pose of the camera
+  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-4));
+}
+
 /* *************************************************************************/
 TEST(SmartProjectionPoseFactorRollingShutter, hessian_simple_2poses) {
   // here we replicate a test in SmartProjectionPoseFactor by setting