Use fixture

borglab · Oct 26, 2024 · c953ae7 · c953ae7
1 parent 5893b17
commit c953ae7
Showing 1 changed file with 18 additions and 31 deletions.
diff --git a/gtsam/sfm/tests/testTransferFactor.cpp b/gtsam/sfm/tests/testTransferFactor.cpp
@@ -49,8 +49,12 @@ std::array<Pose3, 3> cameraPoses = generateCameraPoses();
 auto triplet = generateTripleF(cameraPoses);
 double focalLength = 1000;
 Point2 principalPoint(640 / 2, 480 / 2);
-const Cal3_S2 K(focalLength, focalLength, 0.0,  //
-                principalPoint.x(), principalPoint.y());
+const Cal3_S2 cal(focalLength, focalLength, 0.0,  //
+                  principalPoint.x(), principalPoint.y());
+// Create cameras
+auto f = [](const Pose3& pose) { return PinholeCamera<Cal3_S2>(pose, cal); };
+std::array<PinholeCamera<Cal3_S2>, 3> cameras = {
+    f(cameraPoses[0]), f(cameraPoses[1]), f(cameraPoses[2])};
 }  // namespace fixture
 
 //*************************************************************************
@@ -72,12 +76,9 @@ TEST(TransferFactor, Jacobians) {
 
   // Now project a point into the three cameras
   const Point3 P(0.1, 0.2, 0.3);
-
   std::array<Point2, 3> p;
   for (size_t i = 0; i < 3; ++i) {
-    // Project the point into each camera
-    PinholeCameraCal3_S2 camera(cameraPoses[i], K);
-    p[i] = camera.project(P);
+    p[i] = cameras[i].project(P);
   }
 
   // Create a TransferFactor
@@ -108,19 +109,14 @@ TEST(TransferFactor, MultipleTuples) {
 
   // Now project multiple points into the three cameras
   const size_t numPoints = 5;  // Number of points to test
-  std::vector<Point3> points3D;
   std::vector<std::array<Point2, 3>> projectedPoints;
 
   // Generate random 3D points and project them
   for (size_t n = 0; n < numPoints; ++n) {
     Point3 P(0.1 * n, 0.2 * n, 0.3 + 0.1 * n);
-    points3D.push_back(P);
-
     std::array<Point2, 3> p;
     for (size_t i = 0; i < 3; ++i) {
-      // Project the point into each camera
-      PinholeCameraCal3_S2 camera(cameraPoses[i], K);
-      p[i] = camera.project(P);
+      p[i] = cameras[i].project(P);
     }
     projectedPoints.push_back(p);
   }
@@ -157,18 +153,7 @@ TEST(TransferFactor, MultipleTuples) {
 //*************************************************************************
 // Test for EssentialTransferFactor
 TEST(EssentialTransferFactor, Jacobians) {
-  // Generate cameras on a circle
-  std::array<Pose3, 3> cameraPoses = generateCameraPoses();
-
-  // Create calibration
-  const Cal3_S2 commonK(focalLength, focalLength, 0.0, principalPoint.x(),
-                        principalPoint.y());
-
-  // Create cameras
-  std::array<PinholeCamera<Cal3_S2>, 3> cameras;
-  for (size_t i = 0; i < 3; ++i) {
-    cameras[i] = PinholeCamera<Cal3_S2>(cameraPoses[i], commonK);
-  }
+  using namespace fixture;
 
   // Create EssentialMatrices between cameras
   EssentialMatrix E01 =
@@ -192,16 +177,18 @@ TEST(EssentialTransferFactor, Jacobians) {
 
   // Create Values and insert variables
   Values values;
-  values.insert(key01, E01);     // Essential matrix between views 0 and 1
-  values.insert(key02, E02);     // Essential matrix between views 0 and 2
-  values.insert(K(1), commonK);  // Calibration for view A (view 1)
-  values.insert(K(2), commonK);  // Calibration for view B (view 2)
-  values.insert(K(0), commonK);  // Calibration for view C (view 0)
+  values.insert(key01, E01);  // Essential matrix between views 0 and 1
+  values.insert(key02, E02);  // Essential matrix between views 0 and 2
+  values.insert(K(1), cal);   // Calibration for view A (view 1)
+  values.insert(K(2), cal);   // Calibration for view B (view 2)
+  values.insert(K(0), cal);   // Calibration for view C (view 0)
 
   // Check error
   Matrix H1, H2, H3, H4, H5;
-  Vector error = factor.evaluateError(E01, E02, commonK, commonK, commonK,  //
-                                      &H1, &H2, &H3, &H4, &H5);
+  Vector error =
+      factor.evaluateError(E01, E02,                                  //
+                           fixture::cal, fixture::cal, fixture::cal,  //
+                           &H1, &H2, &H3, &H4, &H5);
   EXPECT(H1.rows() == 2 && H1.cols() == 5)
   EXPECT(H2.rows() == 2 && H2.cols() == 5)
   EXPECT(H3.rows() == 2 && H3.cols() == 5)