diff --git a/gtsam/sfm/tests/testTransferFactor.cpp b/gtsam/sfm/tests/testTransferFactor.cpp index b8eed3b3a..b1655e0a9 100644 --- a/gtsam/sfm/tests/testTransferFactor.cpp +++ b/gtsam/sfm/tests/testTransferFactor.cpp @@ -49,8 +49,12 @@ std::array 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(pose, cal); }; +std::array, 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 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 points3D; std::vector> 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 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 cameraPoses = generateCameraPoses(); - - // Create calibration - const Cal3_S2 commonK(focalLength, focalLength, 0.0, principalPoint.x(), - principalPoint.y()); - - // Create cameras - std::array, 3> cameras; - for (size_t i = 0; i < 3; ++i) { - cameras[i] = PinholeCamera(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)