testTransferFactor.cpp
Go to the documentation of this file.
1 /*
2  * @file testTransferFactor.cpp
3  * @brief Test TransferFactor classes
4  * @author Frank Dellaert
5  * @date October 2024
6  */
7 
13 
14 #include <memory>
15 
16 using namespace gtsam;
18 
19 //*************************************************************************
21 std::array<Pose3, 3> generateCameraPoses() {
22  std::array<Pose3, 3> cameraPoses;
23  const double radius = 1.0;
24  for (int i = 0; i < 3; ++i) {
25  double angle = i * 2.0 * M_PI / 3.0;
26  double c = cos(angle), s = sin(angle);
27  Rot3 aRb({-s, c, 0}, {0, 0, -1}, {-c, -s, 0});
28  cameraPoses[i] = {aRb, Point3(radius * c, radius * s, 0)};
29  }
30  return cameraPoses;
31 }
32 
33 //*************************************************************************
34 // Function to generate a TripleF from camera poses
36  const std::array<Pose3, 3>& cameraPoses) {
37  std::array<SimpleFundamentalMatrix, 3> F;
38  for (size_t i = 0; i < 3; ++i) {
39  size_t j = (i + 1) % 3;
40  const Pose3 iPj = cameraPoses[i].between(cameraPoses[j]);
42  F[i] = {E, 1000.0, 1000.0, Point2(640 / 2, 480 / 2),
43  Point2(640 / 2, 480 / 2)};
44  }
45  return {F[0], F[1], F[2]}; // Return a TripleF instance
46 }
47 
48 //*************************************************************************
49 namespace fixture {
50 // Generate cameras on a circle
51 std::array<Pose3, 3> cameraPoses = generateCameraPoses();
53 double focalLength = 1000;
54 Point2 principalPoint(640 / 2, 480 / 2);
55 const Cal3_S2 cal(focalLength, focalLength, 0.0, //
57 // Create cameras
58 auto f = [](const Pose3& pose) { return PinholeCamera<Cal3_S2>(pose, cal); };
59 std::array<PinholeCamera<Cal3_S2>, 3> cameras = {
60  f(cameraPoses[0]), f(cameraPoses[1]), f(cameraPoses[2])};
61 } // namespace fixture
62 
63 //*************************************************************************
64 // Test for getMatrices
65 TEST(TransferFactor, GetMatrices) {
66  using namespace fixture;
67  TransferFactor<SimpleFundamentalMatrix> factor{{2, 0}, {1, 2}, {}};
68 
69  // Check that getMatrices is correct
70  auto [Fki, Fkj] = factor.getMatrices(triplet.Fca, triplet.Fbc);
71  EXPECT(assert_equal<Matrix3>(triplet.Fca.matrix(), Fki));
72  EXPECT(assert_equal<Matrix3>(triplet.Fbc.matrix().transpose(), Fkj));
73 }
74 
75 //*************************************************************************
76 // Test for TransferFactor
77 TEST(TransferFactor, Jacobians) {
78  using namespace fixture;
79 
80  // Now project a point into the three cameras
81  const Point3 P(0.1, 0.2, 0.3);
82  std::array<Point2, 3> p;
83  for (size_t i = 0; i < 3; ++i) {
84  p[i] = cameras[i].project(P);
85  }
86 
87  // Create a TransferFactor
88  EdgeKey key01(0, 1), key12(1, 2), key20(2, 0);
90  factor0{key01, key20, {{p[1], p[2], p[0]}}},
91  factor1{key12, key01, {{p[2], p[0], p[1]}}},
92  factor2{key20, key12, {{p[0], p[1], p[2]}}};
93 
94  // Create Values with edge keys
95  Values values;
96  values.insert(key01, triplet.Fab);
97  values.insert(key12, triplet.Fbc);
98  values.insert(key20, triplet.Fca);
99 
100  // Check error and Jacobians
101  for (auto&& f : {factor0, factor1, factor2}) {
102  Vector error = f.unwhitenedError(values);
103  EXPECT(assert_equal<Vector>(Z_2x1, error));
105  }
106 }
107 
108 //*************************************************************************
109 // Test for TransferFactor with multiple tuples
110 TEST(TransferFactor, MultipleTuples) {
111  using namespace fixture;
112 
113  // Now project multiple points into the three cameras
114  const size_t numPoints = 5; // Number of points to test
115  std::vector<std::array<Point2, 3>> projectedPoints;
116 
117  // Generate random 3D points and project them
118  for (size_t n = 0; n < numPoints; ++n) {
119  Point3 P(0.1 * n, 0.2 * n, 0.3 + 0.1 * n);
120  std::array<Point2, 3> p;
121  for (size_t i = 0; i < 3; ++i) {
122  p[i] = cameras[i].project(P);
123  }
124  projectedPoints.push_back(p);
125  }
126 
127  // Create a vector of tuples for the TransferFactor
128  EdgeKey key01(0, 1), key12(1, 2), key20(2, 0);
129  std::vector<std::tuple<Point2, Point2, Point2>> tuples;
130 
131  for (size_t n = 0; n < numPoints; ++n) {
132  const auto& p = projectedPoints[n];
133  tuples.emplace_back(p[1], p[2], p[0]);
134  }
135 
136  // Create TransferFactors using the new constructor
137  TransferFactor<SimpleFundamentalMatrix> factor{key01, key20, tuples};
138 
139  // Create Values with edge keys
140  Values values;
141  values.insert(key01, triplet.Fab);
142  values.insert(key12, triplet.Fbc);
143  values.insert(key20, triplet.Fca);
144 
145  // Check error and Jacobians for multiple tuples
146  Vector error = factor.unwhitenedError(values);
147  // The error vector should be of size 2 * numPoints
148  EXPECT_LONGS_EQUAL(2 * numPoints, error.size());
149  // Since the points are perfectly matched, the error should be zero
150  EXPECT(assert_equal<Vector>(Vector::Zero(2 * numPoints), error, 1e-9));
151 
152  // Check the Jacobians
153  EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-5, 1e-7);
154 }
155 
156 //*************************************************************************
157 // Test for EssentialTransferFactor
159  using namespace fixture;
160 
161  // Create EssentialMatrices between cameras
162  EssentialMatrix E01 =
164  EssentialMatrix E02 =
166 
167  // Project a point into the three cameras
168  const Point3 P(0.1, 0.2, 0.3);
169  std::array<Point2, 3> p;
170  for (size_t i = 0; i < 3; ++i) {
171  p[i] = cameras[i].project(P);
172  }
173 
174  // Create EdgeKeys
175  EdgeKey key01(0, 1);
176  EdgeKey key02(0, 2);
177 
178  // Create an EssentialTransferFactor
179  auto sharedCal = std::make_shared<Cal3_S2>(cal);
180  EssentialTransferFactor<Cal3_S2> factor(key01, key02, {{p[1], p[2], p[0]}},
181  sharedCal);
182 
183  // Create Values and insert variables
184  Values values;
185  values.insert(key01, E01); // Essential matrix between views 0 and 1
186  values.insert(key02, E02); // Essential matrix between views 0 and 2
187 
188  // Check error
189  Matrix H1, H2, H3, H4, H5;
190  Vector error = factor.evaluateError(E01, E02, //
191  &H1, &H2);
192  EXPECT(H1.rows() == 2 && H1.cols() == 5)
193  EXPECT(H2.rows() == 2 && H2.cols() == 5)
194  EXPECT(assert_equal(Vector::Zero(2), error, 1e-9))
195 
196  // Check Jacobians
197  EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-5, 1e-7);
198 }
199 
200 //*************************************************************************
201 // Test for EssentialTransferFactorK
203  using namespace fixture;
204 
205  // Create EssentialMatrices between cameras
206  EssentialMatrix E01 =
208  EssentialMatrix E02 =
210 
211  // Project a point into the three cameras
212  const Point3 P(0.1, 0.2, 0.3);
213  std::array<Point2, 3> p;
214  for (size_t i = 0; i < 3; ++i) {
215  p[i] = cameras[i].project(P);
216  }
217 
218  // Create EdgeKeys
219  EdgeKey key01(0, 1);
220  EdgeKey key02(0, 2);
221 
222  // Create an EssentialTransferFactorK
223  EssentialTransferFactorK<Cal3_S2> factor(key01, key02, {{p[1], p[2], p[0]}});
224 
225  // Create Values and insert variables
226  Values values;
227  values.insert(key01, E01); // Essential matrix between views 0 and 1
228  values.insert(key02, E02); // Essential matrix between views 0 and 2
229  values.insert(K(1), cal); // Calibration for view A (view 1)
230  values.insert(K(2), cal); // Calibration for view B (view 2)
231  values.insert(K(0), cal); // Calibration for view C (view 0)
232 
233  // Check error
234  Matrix H1, H2, H3, H4, H5;
235  Vector error = factor.evaluateError(E01, E02, //
236  cal, cal, cal, //
237  &H1, &H2, &H3, &H4, &H5);
238  EXPECT(H1.rows() == 2 && H1.cols() == 5)
239  EXPECT(H2.rows() == 2 && H2.cols() == 5)
240  EXPECT(H3.rows() == 2 && H3.cols() == 5)
241  EXPECT(H4.rows() == 2 && H4.cols() == 5)
242  EXPECT(H5.rows() == 2 && H5.cols() == 5)
243  EXPECT(assert_equal(Vector::Zero(2), error, 1e-9))
244 
245  // Check Jacobians
246  EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-5, 1e-7);
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:52
EXPECT_CORRECT_FACTOR_JACOBIANS
#define EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, numerical_derivative_step, tolerance)
Check the Jacobians produced by a factor against finite differences.
Definition: factorTesting.h:114
pose
static const Pose3 pose(Rot3(Vector3(1, -1, -1).asDiagonal()), Point3(0, 0, 0.5))
gtsam::between
Expression< T > between(const Expression< T > &t1, const Expression< T > &t2)
Definition: nonlinear/expressions.h:17
aRb
Rot3 aRb
Definition: testFundamentalMatrix.cpp:146
fixture::cameras
std::array< PinholeCamera< Cal3_S2 >, 3 > cameras
Definition: testTransferFactor.cpp:59
simple_graph::factor2
auto factor2
Definition: testJacobianFactor.cpp:203
s
RealScalar s
Definition: level1_cplx_impl.h:126
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
EXPECT_LONGS_EQUAL
#define EXPECT_LONGS_EQUAL(expected, actual)
Definition: Test.h:154
ceres::sin
Jet< T, N > sin(const Jet< T, N > &f)
Definition: jet.h:439
EXPECT
#define EXPECT(condition)
Definition: Test.h:150
Cal3_S2.h
The most common 5DOF 3D->2D calibration.
TestHarness.h
generateCameraPoses
std::array< Pose3, 3 > generateCameraPoses()
Generate three cameras on a circle, looking inwards.
Definition: testTransferFactor.cpp:21
c
Scalar Scalar * c
Definition: benchVecAdd.cpp:17
fixture
Definition: testTransferFactor.cpp:49
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
generateTripleF
TripleF< SimpleFundamentalMatrix > generateTripleF(const std::array< Pose3, 3 > &cameraPoses)
Definition: testTransferFactor.cpp:35
different_sigmas::values
HybridValues values
Definition: testHybridBayesNet.cpp:245
gtsam::EssentialTransferFactorK
Transfers points between views using essential matrices, optimizes for calibrations of the views,...
Definition: TransferFactor.h:224
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:38
fixture::cal
const Cal3_S2 cal(focalLength, focalLength, 0.0, principalPoint.x(), principalPoint.y())
ceres::cos
Jet< T, N > cos(const Jet< T, N > &f)
Definition: jet.h:426
gtsam::EssentialTransferFactor
Transfers points between views using essential matrices with a shared calibration.
Definition: TransferFactor.h:146
focalLength
double focalLength
Definition: testFundamentalMatrix.cpp:118
n
int n
Definition: BiCGSTAB_simple.cpp:1
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
gtsam::PinholeCamera
Definition: PinholeCamera.h:33
gtsam::Pose3::translation
const Point3 & translation(OptionalJacobian< 3, 6 > Hself={}) const
get translation
Definition: Pose3.cpp:308
factorTesting.h
Evaluate derivatives of a nonlinear factor numerically.
principalPoint
Point2 principalPoint(640/2, 480/2)
gtsam::HybridValues::insert
void insert(Key j, const Vector &value)
Definition: HybridValues.cpp:85
gtsam::Point2
Vector2 Point2
Definition: Point2.h:32
sharedCal
static const std::shared_ptr< Cal3_S2 > sharedCal
Definition: testTriangulationFactor.cpp:33
TransferFactor.h
gtsam::symbol_shorthand::F
Key F(std::uint64_t j)
Definition: inference/Symbol.h:153
gtsam::TransferFactor
Definition: TransferFactor.h:87
TestResult
Definition: TestResult.h:26
gtsam::symbol_shorthand::K
Key K(std::uint64_t j)
Definition: inference/Symbol.h:158
E
DiscreteKey E(5, 2)
fixture::f
auto f
Definition: testTransferFactor.cpp:58
fixture::cameraPoses
std::array< Pose3, 3 > cameraPoses
Definition: testTransferFactor.cpp:51
gtsam::Pose3::rotation
const Rot3 & rotation(OptionalJacobian< 3, 6 > Hself={}) const
get rotation
Definition: Pose3.cpp:315
gtsam::EdgeKey
Definition: EdgeKey.h:25
gtsam
traits
Definition: SFMdata.h:40
gtsam::TEST
TEST(SmartFactorBase, Pinhole)
Definition: testSmartFactorBase.cpp:38
error
static double error
Definition: testRot3.cpp:37
K
#define K
Definition: igam.h:8
gtsam::Values
Definition: Values.h:65
p
float * p
Definition: Tutorial_Map_using.cpp:9
gtsam::Z_2x1
static const Eigen::MatrixBase< Vector2 >::ConstantReturnType Z_2x1
Definition: Vector.h:45
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::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
main
int main()
Definition: testTransferFactor.cpp:250
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
M_PI
#define M_PI
Definition: mconf.h:117
gtsam::TransferEdges::getMatrices
std::pair< Matrix3, Matrix3 > getMatrices(const F &F1, const F &F2) const
Create Matrix3 objects based on EdgeKey configurations.
Definition: TransferFactor.h:66
i
int i
Definition: BiCGSTAB_step_by_step.cpp:9
simple_graph::factor1
auto factor1
Definition: testJacobianFactor.cpp:196
gtsam::EssentialMatrix::FromPose3
static GTSAM_EXPORT EssentialMatrix FromPose3(const Pose3 &_1P2_, OptionalJacobian< 5, 6 > H={})
Named constructor converting a Pose3 with scale to EssentialMatrix (no scale)
Definition: EssentialMatrix.cpp:27


gtsam
Author(s):
autogenerated on Sat Nov 16 2024 04:08:47