testFundamentalMatrix.cpp
Go to the documentation of this file.
1 /*
2  * @file testFundamentalMatrix.cpp
3  * @brief Test FundamentalMatrix classes
4  * @author Frank Dellaert
5  * @date October 23, 2024
6  */
7 
9 #include <gtsam/base/Matrix.h>
10 #include <gtsam/base/Testable.h>
12 #include <gtsam/geometry/Rot3.h>
14 #include <gtsam/geometry/Unit3.h>
15 
16 #include <cmath>
17 
18 using namespace std::placeholders;
19 using namespace std;
20 using namespace gtsam;
21 
24 
25 //*************************************************************************
26 // Create two rotations and corresponding fundamental matrix F
29 double trueS = 0.5;
31 
32 //*************************************************************************
33 TEST(FundamentalMatrix, LocalCoordinates) {
34  Vector expected = Z_7x1; // Assuming 7 dimensions for U, V, and s
36  EXPECT(assert_equal(expected, actual, 1e-8));
37 }
38 
39 //*************************************************************************
41  FundamentalMatrix actual = trueF.retract(Z_7x1);
42  EXPECT(assert_equal(trueF, actual));
43 }
44 
45 //*************************************************************************
46 // Test conversion from F matrices, including non-rotations
47 TEST(FundamentalMatrix, Conversion) {
48  Matrix3 U = trueU.matrix();
49  Matrix3 V = trueV.matrix();
50  std::vector<FundamentalMatrix> Fs = {
53 
54  for (const auto& trueF : Fs) {
55  const Matrix3 F = trueF.matrix();
56  FundamentalMatrix actual(F);
57  // We check the matrices as the underlying representation is not unique
58  CHECK(assert_equal(F, actual.matrix()));
59  }
60 }
61 
62 //*************************************************************************
63 TEST(FundamentalMatrix, RoundTrip) {
64  Vector7 d;
65  d << 0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7;
67  Vector actual = trueF.localCoordinates(hx);
68  EXPECT(assert_equal(d, actual, 1e-8));
69 }
70 
71 //*************************************************************************
72 // Create the simplest SimpleFundamentalMatrix, a stereo pair
73 EssentialMatrix defaultE(Rot3(), Unit3(1, 0, 0));
74 Point2 zero(0.0, 0.0);
76 
77 //*************************************************************************
78 TEST(SimpleStereo, Conversion) {
79  FundamentalMatrix convertedF(stereoF.matrix());
80  EXPECT(assert_equal(stereoF.matrix(), convertedF.matrix(), 1e-8));
81 }
82 
83 //*************************************************************************
84 TEST(SimpleStereo, LocalCoordinates) {
85  Vector expected = Z_7x1;
87  EXPECT(assert_equal(expected, actual, 1e-8));
88 }
89 
90 //*************************************************************************
91 TEST(SimpleStereo, Retract) {
92  SimpleFundamentalMatrix actual = stereoF.retract(Z_9x1);
93  EXPECT(assert_equal(stereoF, actual));
94 }
95 
96 //*************************************************************************
97 TEST(SimpleStereo, RoundTrip) {
98  Vector7 d;
99  d << 0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7;
101  Vector actual = stereoF.localCoordinates(hx);
102  EXPECT(assert_equal(d, actual, 1e-8));
103 }
104 
105 //*************************************************************************
106 TEST(SimpleStereo, EpipolarLine) {
107  // Create a point in b
108  Point3 p_b(0, 2, 1);
109  // Convert the point to a horizontal line in a
110  Vector3 l_a = stereoF.matrix() * p_b;
111  // Check if the line is horizontal at height 2
112  EXPECT(assert_equal(Vector3(0, -1, 2), l_a));
113 }
114 
115 //*************************************************************************
116 // Create a stereo pair, but in pixels not normalized coordinates.
117 // We're still using zero principal points here.
118 double focalLength = 1000;
120  zero);
121 
122 //*************************************************************************
123 TEST(PixelStereo, Conversion) {
124  auto expected = pixelStereo.matrix();
125 
126  FundamentalMatrix convertedF(pixelStereo.matrix());
127 
128  // Check equality of F-matrices up to a scale
129  auto actual = convertedF.matrix();
130  actual *= expected(1, 2) / actual(1, 2);
131  EXPECT(assert_equal(expected, actual, 1e-5));
132 }
133 
134 //*************************************************************************
135 TEST(PixelStereo, PointInBToHorizontalLineInA) {
136  // Create a point in b
137  Point3 p_b = Point3(0, 300, 1);
138  // Convert the point to a horizontal line in a
139  Vector3 l_a = pixelStereo.matrix() * p_b;
140  // Check if the line is horizontal at height 2
141  EXPECT(assert_equal(Vector3(0, -0.001, 0.3), l_a));
142 }
143 
144 //*************************************************************************
145 // Create a stereo pair with the right camera rotated 90 degrees
146 Rot3 aRb = Rot3::Rz(M_PI_2); // Rotate 90 degrees around the Z-axis
147 EssentialMatrix rotatedE(aRb, Unit3(1, 0, 0));
149  zero, zero);
150 
151 //*************************************************************************
152 TEST(RotatedPixelStereo, Conversion) {
154 
156 
157  // Check equality of F-matrices up to a scale
158  auto actual = convertedF.matrix();
159  actual *= expected(1, 2) / actual(1, 2);
160  EXPECT(assert_equal(expected, actual, 1e-4));
161 }
162 
163 //*************************************************************************
164 TEST(RotatedPixelStereo, PointInBToHorizontalLineInA) {
165  // Create a point in b
166  Point3 p_b = Point3(300, 0, 1);
167  // Convert the point to a horizontal line in a
168  Vector3 l_a = rotatedPixelStereo.matrix() * p_b;
169  // Check if the line is horizontal at height 2
170  EXPECT(assert_equal(Vector3(0, -0.001, 0.3), l_a));
171 }
172 
173 //*************************************************************************
174 // Now check that principal points also survive conversion
175 Point2 principalPoint(640 / 2, 480 / 2);
179 
180 //*************************************************************************
183 
185 
186  // Check equality of F-matrices up to a scale
187  auto actual = convertedF.matrix();
188  actual *= expected(1, 2) / actual(1, 2);
189  EXPECT(assert_equal(expected, actual, 1e-4));
190 }
191 
192 //*************************************************************************
194 std::array<Pose3, 3> generateCameraPoses() {
195  std::array<Pose3, 3> cameraPoses;
196  const double radius = 1.0;
197  for (int i = 0; i < 3; ++i) {
198  double angle = i * 2.0 * M_PI / 3.0;
199  double c = cos(angle), s = sin(angle);
200  Rot3 aRb({-s, c, 0}, {0, 0, -1}, {-c, -s, 0});
201  cameraPoses[i] = {aRb, Point3(radius * c, radius * s, 0)};
202  }
203  return cameraPoses;
204 }
205 
206 //*************************************************************************
209  const std::array<Pose3, 3>& cameraPoses) {
210  std::array<SimpleFundamentalMatrix, 3> F;
211  for (size_t i = 0; i < 3; ++i) {
212  size_t j = (i + 1) % 3;
213  const Pose3 iPj = cameraPoses[i].between(cameraPoses[j]);
214  EssentialMatrix E(iPj.rotation(), Unit3(iPj.translation()));
216  }
217  return {F[0], F[1], F[2]}; // Return a TripleF instance
218 }
219 
220 //*************************************************************************
221 TEST(TripleF, Transfer) {
222  // Generate cameras on a circle, as well as fundamental matrices
225 
226  // Check that they are all equal
227  EXPECT(triplet.Fab.equals(triplet.Fbc, 1e-9));
228  EXPECT(triplet.Fbc.equals(triplet.Fca, 1e-9));
229  EXPECT(triplet.Fca.equals(triplet.Fab, 1e-9));
230 
231  // Now project a point into the three cameras
232  const Point3 P(0.1, 0.2, 0.3);
233  const Cal3_S2 K(focalLength, focalLength, 0.0, //
234  principalPoint.x(), principalPoint.y());
235 
236  std::array<Point2, 3> p;
237  for (size_t i = 0; i < 3; ++i) {
238  // Project the point into each camera
240  p[i] = camera.project(P);
241  }
242 
243  // Check that transfer works
244  EXPECT(assert_equal<Point2>(p[0], triplet.transferToA(p[1], p[2]), 1e-9));
245  EXPECT(assert_equal<Point2>(p[1], triplet.transferToB(p[0], p[2]), 1e-9));
246  EXPECT(assert_equal<Point2>(p[2], triplet.transferToC(p[0], p[1]), 1e-9));
247 }
248 
249 //*************************************************************************
250 int main() {
251  TestResult tr;
252  return TestRegistry::runAllTests(tr);
253 }
254 //*************************************************************************
TestRegistry::runAllTests
static int runAllTests(TestResult &result)
Definition: TestRegistry.cpp:27
fixture::triplet
auto triplet
Definition: testTransferFactor.cpp:48
stereoWithPrincipalPoints
SimpleFundamentalMatrix stereoWithPrincipalPoints(rotatedE, focalLength, focalLength, principalPoint, principalPoint)
aRb
Rot3 aRb
Definition: testFundamentalMatrix.cpp:146
s
RealScalar s
Definition: level1_cplx_impl.h:126
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
d
static const double d[K][N]
Definition: igam.h:11
gtsam::SimpleFundamentalMatrix::matrix
Matrix3 matrix() const
Definition: FundamentalMatrix.cpp:115
GTSAM_CONCEPT_TESTABLE_INST
#define GTSAM_CONCEPT_TESTABLE_INST(T)
Definition: Testable.h:176
Testable.h
Concept check for values that can be used in unit tests.
ceres::sin
Jet< T, N > sin(const Jet< T, N > &f)
Definition: jet.h:439
EXPECT
#define EXPECT(condition)
Definition: Test.h:150
TestHarness.h
c
Scalar Scalar * c
Definition: benchVecAdd.cpp:17
Matrix.h
typedef and functions to augment Eigen's MatrixXd
gtsam::SimpleFundamentalMatrix::localCoordinates
Vector localCoordinates(const SimpleFundamentalMatrix &F) const
Return local coordinates with respect to another SimpleFundamentalMatrix.
Definition: FundamentalMatrix.cpp:133
TEST
TEST(FundamentalMatrix, LocalCoordinates)
Definition: testFundamentalMatrix.cpp:33
pixelStereo
SimpleFundamentalMatrix pixelStereo(defaultE, focalLength, focalLength, zero, zero)
rotatedE
EssentialMatrix rotatedE(aRb, Unit3(1, 0, 0))
trueF
FundamentalMatrix trueF(trueU.matrix(), trueS, trueV.matrix())
gtsam::Vector3
Eigen::Vector3d Vector3
Definition: Vector.h:43
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:38
ceres::cos
Jet< T, N > cos(const Jet< T, N > &f)
Definition: jet.h:426
Unit3.h
Rot3.h
3D rotation represented as a rotation matrix or quaternion
focalLength
double focalLength
Definition: testFundamentalMatrix.cpp:118
generateTripleF
TripleF< SimpleFundamentalMatrix > generateTripleF(const std::array< Pose3, 3 > &cameraPoses)
Function to generate a TripleF from camera poses.
Definition: testFundamentalMatrix.cpp:208
generateCameraPoses
std::array< Pose3, 3 > generateCameraPoses()
Generate three cameras on a circle, looking in.
Definition: testFundamentalMatrix.cpp:194
j
std::ptrdiff_t j
Definition: tut_arithmetic_redux_minmax.cpp:2
gtsam::EssentialMatrix
Definition: EssentialMatrix.h:26
gtsam::Rot3
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition: Rot3.h:58
gtsam::Pose3
Definition: Pose3.h:37
M_PI_4
#define M_PI_4
Definition: mconf.h:119
gtsam::PinholeCamera
Definition: PinholeCamera.h:33
gtsam::FundamentalMatrix::matrix
Matrix3 matrix() const
Return the fundamental matrix representation.
Definition: FundamentalMatrix.cpp:72
main
int main()
Definition: testFundamentalMatrix.cpp:250
stereoF
SimpleFundamentalMatrix stereoF(defaultE, 1.0, 1.0, zero, zero)
cholesky::expected
Matrix expected
Definition: testMatrix.cpp:971
gtsam::Pose3::translation
const Point3 & translation(OptionalJacobian< 3, 6 > Hself={}) const
get translation
Definition: Pose3.cpp:308
principalPoint
Point2 principalPoint(640/2, 480/2)
rotatedPixelStereo
SimpleFundamentalMatrix rotatedPixelStereo(rotatedE, focalLength, focalLength, zero, zero)
gtsam::Point2
Vector2 Point2
Definition: Point2.h:32
gtsam::symbol_shorthand::F
Key F(std::uint64_t j)
Definition: inference/Symbol.h:153
gtsam::SimpleFundamentalMatrix::retract
SimpleFundamentalMatrix retract(const Vector &delta) const
Retract the given vector to get a new SimpleFundamentalMatrix.
Definition: FundamentalMatrix.cpp:142
TestResult
Definition: TestResult.h:26
matrix
Map< Matrix< T, Dynamic, Dynamic, ColMajor >, 0, OuterStride<> > matrix(T *data, int rows, int cols, int stride)
Definition: gtsam/3rdparty/Eigen/blas/common.h:110
M_PI_2
#define M_PI_2
Definition: mconf.h:118
E
DiscreteKey E(5, 2)
trueV
Rot3 trueV
Definition: testFundamentalMatrix.cpp:28
zero
Point2 zero(0.0, 0.0)
fixture::cameraPoses
std::array< Pose3, 3 > cameraPoses
Definition: testTransferFactor.cpp:47
gtsam::Pose3::rotation
const Rot3 & rotation(OptionalJacobian< 3, 6 > Hself={}) const
get rotation
Definition: Pose3.cpp:315
gtsam
traits
Definition: SFMdata.h:40
K
#define K
Definition: igam.h:8
gtsam::SimpleFundamentalMatrix
Class for representing a simple fundamental matrix.
Definition: FundamentalMatrix.h:128
CHECK
#define CHECK(condition)
Definition: Test.h:108
std
Definition: BFloat16.h:88
p
float * p
Definition: Tutorial_Map_using.cpp:9
FundamentalMatrix.h
gtsam::Rot3::matrix
Matrix3 matrix() const
Definition: Rot3M.cpp:218
gtsam::assert_equal
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:40
P
static double P[]
Definition: ellpe.c:68
GTSAM_CONCEPT_MANIFOLD_INST
#define GTSAM_CONCEPT_MANIFOLD_INST(T)
‍**
Definition: Manifold.h:177
gtsam::FundamentalMatrix::localCoordinates
Vector localCoordinates(const FundamentalMatrix &F) const
Return local coordinates with respect to another FundamentalMatrix.
Definition: FundamentalMatrix.cpp:87
gtsam::Cal3_S2
The most common 5DOF 3D->2D calibration.
Definition: Cal3_S2.h:34
gtsam::TripleF
Definition: FundamentalMatrix.h:199
gtsam::Point3
Vector3 Point3
Definition: Point3.h:38
U
@ U
Definition: testDecisionTree.cpp:349
V
MatrixXcd V
Definition: EigenSolver_EigenSolver_MatrixType.cpp:15
trueS
double trueS
Definition: testFundamentalMatrix.cpp:29
SimpleCamera.h
A simple camera class with a Cal3_S2 calibration.
gtsam::Unit3
Represents a 3D point on a unit sphere.
Definition: Unit3.h:42
camera
static const CalibratedCamera camera(kDefaultPose)
M_PI
#define M_PI
Definition: mconf.h:117
defaultE
EssentialMatrix defaultE(Rot3(), Unit3(1, 0, 0))
gtsam::FundamentalMatrix
Represents a fundamental matrix in computer vision, which encodes the epipolar geometry between two v...
Definition: FundamentalMatrix.h:28
i
int i
Definition: BiCGSTAB_step_by_step.cpp:9
trueU
Rot3 trueU
Definition: testFundamentalMatrix.cpp:27
gtsam::CalibratedCamera::project
Point2 project(const Point3 &point, OptionalJacobian< 2, 6 > Dcamera={}, OptionalJacobian< 2, 3 > Dpoint={}) const
Definition: CalibratedCamera.cpp:188
gtsam::FundamentalMatrix::retract
FundamentalMatrix retract(const Vector &delta) const
Retract the given vector to get a new FundamentalMatrix.
Definition: FundamentalMatrix.cpp:95


gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:39:57