12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Use fixture

release/4.3a0
Frank Dellaert 2024-10-25 21:05:09 -07:00
parent 5b94956a59
commit 049191fe1d
1 changed files with 18 additions and 31 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

41
gtsam/sfm/tests/testTransferFactor.cpp
View File

@ -49,8 +49,12 @@ std::array<Pose3, 3> cameraPoses = generateCameraPoses();
auto triplet = generateTripleF(cameraPoses); auto triplet = generateTripleF(cameraPoses);
double focalLength = 1000; double focalLength = 1000;
Point2 principalPoint(640 / 2, 480 / 2); Point2 principalPoint(640 / 2, 480 / 2);
const Cal3_S2 K(focalLength, focalLength, 0.0, // const Cal3_S2 cal(focalLength, focalLength, 0.0, //
principalPoint.x(), principalPoint.y()); 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 } // namespace fixture
//************************************************************************* //*************************************************************************
@ -72,12 +76,9 @@ TEST(TransferFactor, Jacobians) {
// Now project a point into the three cameras // Now project a point into the three cameras
const Point3 P(0.1, 0.2, 0.3); const Point3 P(0.1, 0.2, 0.3);
std::array<Point2, 3> p; std::array<Point2, 3> p;
for (size_t i = 0; i < 3; ++i) { for (size_t i = 0; i < 3; ++i) {
// Project the point into each camera p[i] = cameras[i].project(P);
PinholeCameraCal3_S2 camera(cameraPoses[i], K);
p[i] = camera.project(P);
} }
// Create a TransferFactor // Create a TransferFactor
@ -108,19 +109,14 @@ TEST(TransferFactor, MultipleTuples) {
// Now project multiple points into the three cameras // Now project multiple points into the three cameras
const size_t numPoints = 5; // Number of points to test const size_t numPoints = 5; // Number of points to test
std::vector<Point3> points3D;
std::vector<std::array<Point2, 3>> projectedPoints; std::vector<std::array<Point2, 3>> projectedPoints;
// Generate random 3D points and project them // Generate random 3D points and project them
for (size_t n = 0; n < numPoints; ++n) { for (size_t n = 0; n < numPoints; ++n) {
Point3 P(0.1 * n, 0.2 * n, 0.3 + 0.1 * n); Point3 P(0.1 * n, 0.2 * n, 0.3 + 0.1 * n);
points3D.push_back(P);
std::array<Point2, 3> p; std::array<Point2, 3> p;
for (size_t i = 0; i < 3; ++i) { for (size_t i = 0; i < 3; ++i) {
// Project the point into each camera p[i] = cameras[i].project(P);
PinholeCameraCal3_S2 camera(cameraPoses[i], K);
p[i] = camera.project(P);
} }
projectedPoints.push_back(p); projectedPoints.push_back(p);
} }
@ -157,18 +153,7 @@ TEST(TransferFactor, MultipleTuples) {
//************************************************************************* //*************************************************************************
// Test for EssentialTransferFactor // Test for EssentialTransferFactor
TEST(EssentialTransferFactor, Jacobians) { TEST(EssentialTransferFactor, Jacobians) {
// Generate cameras on a circle using namespace fixture;
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);
}
// Create EssentialMatrices between cameras // Create EssentialMatrices between cameras
EssentialMatrix E01 = EssentialMatrix E01 =
@ -194,13 +179,15 @@ TEST(EssentialTransferFactor, Jacobians) {
Values values; Values values;
values.insert(key01, E01); // Essential matrix between views 0 and 1 values.insert(key01, E01); // Essential matrix between views 0 and 1
values.insert(key02, E02); // Essential matrix between views 0 and 2 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(1), cal); // Calibration for view A (view 1)
values.insert(K(2), commonK); // Calibration for view B (view 2) values.insert(K(2), cal); // Calibration for view B (view 2)
values.insert(K(0), commonK); // Calibration for view C (view 0) values.insert(K(0), cal); // Calibration for view C (view 0)
// Check error // Check error
Matrix H1, H2, H3, H4, H5; Matrix H1, H2, H3, H4, H5;
Vector error = factor.evaluateError(E01, E02, commonK, commonK, commonK, // Vector error =
factor.evaluateError(E01, E02, //
fixture::cal, fixture::cal, fixture::cal, //
&H1, &H2, &H3, &H4, &H5); &H1, &H2, &H3, &H4, &H5);
EXPECT(H1.rows() == 2 && H1.cols() == 5) EXPECT(H1.rows() == 2 && H1.cols() == 5)
EXPECT(H2.rows() == 2 && H2.cols() == 5) EXPECT(H2.rows() == 2 && H2.cols() == 5)