done with tests

gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp

@@ -424,6 +424,78 @@ TEST(SmartProjectionPoseFactorRollingShutter, optimization_3poses) {
   EXPECT(assert_equal(pose_above, result.at<Pose3>(x3), 1e-6));
 }
 
+/* *************************************************************************/
+TEST(SmartProjectionPoseFactorRollingShutter, optimization_3poses_multiCam) {
+  using namespace vanillaPoseRS;
+  Point2Vector measurements_lmk1, measurements_lmk2, measurements_lmk3;
+
+  // Project three landmarks into three cameras
+  projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_lmk1);
+  projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_lmk2);
+  projectToMultipleCameras(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);
+
+  Cameras cameraRig;
+  cameraRig.push_back(Camera(body_P_sensor, sharedK));
+  cameraRig.push_back(Camera(Pose3::identity(), sharedK));
+
+  SmartFactorRS::shared_ptr smartFactor1(new SmartFactorRS(model, cameraRig));
+  smartFactor1->add(measurements_lmk1, key_pairs, interp_factors, {1,1,1});
+
+  SmartFactorRS::shared_ptr smartFactor2(new SmartFactorRS(model, cameraRig));
+  smartFactor2->add(measurements_lmk2, key_pairs, interp_factors, {1,1,1});
+
+  SmartFactorRS::shared_ptr smartFactor3(new SmartFactorRS(model, cameraRig));
+  smartFactor3->add(measurements_lmk3, key_pairs, interp_factors, {1,1,1});
+
+  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-6));
+}
+
 /* *************************************************************************/
 TEST(SmartProjectionPoseFactorRollingShutter, hessian_simple_2poses) {
   // here we replicate a test in SmartProjectionPoseFactor by setting