testCameraSet.cpp
Go to the documentation of this file.
1 /* ----------------------------------------------------------------------------
2 
3  * GTSAM Copyright 2010, Georgia Tech Research Corporation,
4  * Atlanta, Georgia 30332-0415
5  * All Rights Reserved
6  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7 
8  * See LICENSE for the license information
9 
10  * -------------------------------------------------------------------------- */
11 
19 #include <gtsam/geometry/CameraSet.h>
20 #include <gtsam/geometry/Cal3_S2.h>
21 #include <gtsam/geometry/Pose3.h>
22 #include <gtsam/base/numericalDerivative.h>
23 #include <CppUnitLite/TestHarness.h>
24 
25 using namespace std;
26 using namespace gtsam;
27 
28 /* ************************************************************************* */
29 // Cal3Bundler test
30 #include <gtsam/geometry/PinholeCamera.h>
31 #include <gtsam/geometry/Cal3Bundler.h>
32 TEST(CameraSet, Pinhole) {
33  typedef PinholeCamera<Cal3Bundler> Camera;
34  typedef CameraSet<Camera> Set;
35  typedef Point2Vector ZZ;
36  Set set;
37  Camera camera;
38  set.push_back(camera);
39  set.push_back(camera);
40  Point3 p(0, 0, 1);
41  EXPECT(assert_equal(set, set));
42  Set set2 = set;
43  set2.push_back(camera);
44  EXPECT(!set.equals(set2));
45 
46  // Check measurements
47  Point2 expected(0,0);
48  ZZ z = set.project2(p);
49  EXPECT(assert_equal(expected, z[0]));
50  EXPECT(assert_equal(expected, z[1]));
51 
52  // Calculate expected derivatives using Pinhole
53  Matrix actualE;
54  Matrix29 F1;
55  {
56  Matrix23 E1;
57  camera.project2(p, F1, E1);
58  actualE.resize(4,3);
59  actualE << E1, E1;
60  }
61 
62  // Check computed derivatives
63  Set::FBlocks Fs;
64  Matrix E;
65  set.project2(p, Fs, E);
66  LONGS_EQUAL(2, Fs.size());
67  EXPECT(assert_equal(F1, Fs[0]));
68  EXPECT(assert_equal(F1, Fs[1]));
69  EXPECT(assert_equal(actualE, E));
70 
71  // Check errors
72  ZZ measured;
73  measured.push_back(Point2(1, 2));
74  measured.push_back(Point2(3, 4));
75  Vector4 expectedV;
76 
77  // reprojectionError
78  expectedV << -1, -2, -3, -4;
79  Vector actualV = set.reprojectionError(p, measured);
80  EXPECT(assert_equal(expectedV, actualV));
81 
82  // Check Schur complement
83  Matrix F(4, 18);
84  F << F1, Matrix29::Zero(), Matrix29::Zero(), F1;
85  Matrix Ft = F.transpose();
86  Matrix34 Et = E.transpose();
87  Matrix3 P = Et * E;
88  Matrix schur(19, 19);
89  Vector4 b = actualV;
90  Vector v = Ft * (b - E * P * Et * b);
91  schur << Ft * F - Ft * E * P * Et * F, v, v.transpose(), 30;
92  SymmetricBlockMatrix actualReduced = Set::SchurComplement<3>(Fs, E, P, b);
93  EXPECT(assert_equal(schur, actualReduced.selfadjointView()));
94 
95  // Check Schur complement update, same order, should just double
96  KeyVector allKeys {1, 2}, keys {1, 2};
97  Set::UpdateSchurComplement<3>(Fs, E, P, b, allKeys, keys, actualReduced);
98  EXPECT(assert_equal((Matrix )(2.0 * schur), actualReduced.selfadjointView()));
99 
100  // Check Schur complement update, keys reversed
101  KeyVector keys2 {2, 1};
102  Set::UpdateSchurComplement<3>(Fs, E, P, b, allKeys, keys2, actualReduced);
103  Vector4 reverse_b;
104  reverse_b << b.tail<2>(), b.head<2>();
105  Vector reverse_v = Ft * (reverse_b - E * P * Et * reverse_b);
106  Matrix A(19, 19);
107  A << Ft * F - Ft * E * P * Et * F, reverse_v, reverse_v.transpose(), 30;
108  EXPECT(assert_equal((Matrix )(2.0 * schur + A), actualReduced.selfadjointView()));
109 
110  // reprojectionErrorAtInfinity
111  Unit3 pointAtInfinity(0, 0, 1000);
112  EXPECT(
113  assert_equal(pointAtInfinity,
114  camera.backprojectPointAtInfinity(Point2(0,0))));
115  actualV = set.reprojectionError(pointAtInfinity, measured, Fs, E);
116  EXPECT(assert_equal(expectedV, actualV));
117  LONGS_EQUAL(2, Fs.size());
118  {
119  Matrix22 E1;
120  camera.project2(pointAtInfinity, F1, E1);
121  actualE.resize(4,2);
122  actualE << E1, E1;
123  }
124  EXPECT(assert_equal(F1, Fs[0]));
125  EXPECT(assert_equal(F1, Fs[1]));
126  EXPECT(assert_equal(actualE, E));
127 }
128 
129 /* ************************************************************************* */
130 TEST(CameraSet, SchurComplementAndRearrangeBlocks) {
131  typedef PinholePose<Cal3Bundler> Camera;
132  typedef CameraSet<Camera> Set;
133 
134  // this is the (block) Jacobian with respect to the nonuniqueKeys
135  std::vector<Eigen::Matrix<double, 2, 12>,
136  Eigen::aligned_allocator<Eigen::Matrix<double, 2, 12> > > Fs;
137  Fs.push_back(1 * Matrix::Ones(2, 12)); // corresponding to key pair (0,1)
138  Fs.push_back(2 * Matrix::Ones(2, 12)); // corresponding to key pair (1,2)
139  Fs.push_back(3 * Matrix::Ones(2, 12)); // corresponding to key pair (2,0)
140  Matrix E = 4 * Matrix::Identity(6, 3) + Matrix::Ones(6, 3);
141  E(0, 0) = 3;
142  E(1, 1) = 2;
143  E(2, 2) = 5;
144  Matrix Et = E.transpose();
145  Matrix P = (Et * E).inverse();
146  Vector b = 5 * Vector::Ones(6);
147 
148  { // SchurComplement
149  // Actual
150  SymmetricBlockMatrix augmentedHessianBM = Set::SchurComplement<3, 12>(Fs, E,
151  P, b);
152  Matrix actualAugmentedHessian = augmentedHessianBM.selfadjointView();
153 
154  // Expected
155  Matrix F = Matrix::Zero(6, 3 * 12);
156  F.block<2, 12>(0, 0) = 1 * Matrix::Ones(2, 12);
157  F.block<2, 12>(2, 12) = 2 * Matrix::Ones(2, 12);
158  F.block<2, 12>(4, 24) = 3 * Matrix::Ones(2, 12);
159 
160  Matrix Ft = F.transpose();
161  Matrix H = Ft * F - Ft * E * P * Et * F;
162  Vector v = Ft * (b - E * P * Et * b);
163  Matrix expectedAugmentedHessian = Matrix::Zero(3 * 12 + 1, 3 * 12 + 1);
164  expectedAugmentedHessian.block<36, 36>(0, 0) = H;
165  expectedAugmentedHessian.block<36, 1>(0, 36) = v;
166  expectedAugmentedHessian.block<1, 36>(36, 0) = v.transpose();
167  expectedAugmentedHessian(36, 36) = b.squaredNorm();
168 
169  EXPECT(assert_equal(expectedAugmentedHessian, actualAugmentedHessian));
170  }
171 
172  { // SchurComplementAndRearrangeBlocks
173  KeyVector nonuniqueKeys;
174  nonuniqueKeys.push_back(0);
175  nonuniqueKeys.push_back(1);
176  nonuniqueKeys.push_back(1);
177  nonuniqueKeys.push_back(2);
178  nonuniqueKeys.push_back(2);
179  nonuniqueKeys.push_back(0);
180 
181  KeyVector uniqueKeys;
182  uniqueKeys.push_back(0);
183  uniqueKeys.push_back(1);
184  uniqueKeys.push_back(2);
185 
186  // Actual
187  SymmetricBlockMatrix augmentedHessianBM =
188  Set::SchurComplementAndRearrangeBlocks<3, 12, 6>(
189  Fs, E, P, b, nonuniqueKeys, uniqueKeys);
190  Matrix actualAugmentedHessian = augmentedHessianBM.selfadjointView();
191 
192  // Expected
193  // we first need to build the Jacobian F according to unique keys
194  Matrix F = Matrix::Zero(6, 18);
195  F.block<2, 6>(0, 0) = Fs[0].block<2, 6>(0, 0);
196  F.block<2, 6>(0, 6) = Fs[0].block<2, 6>(0, 6);
197  F.block<2, 6>(2, 6) = Fs[1].block<2, 6>(0, 0);
198  F.block<2, 6>(2, 12) = Fs[1].block<2, 6>(0, 6);
199  F.block<2, 6>(4, 12) = Fs[2].block<2, 6>(0, 0);
200  F.block<2, 6>(4, 0) = Fs[2].block<2, 6>(0, 6);
201 
202  Matrix Ft = F.transpose();
203  Vector v = Ft * (b - E * P * Et * b);
204  Matrix H = Ft * F - Ft * E * P * Et * F;
205  Matrix expectedAugmentedHessian(19, 19);
206  expectedAugmentedHessian << H, v, v.transpose(), b.squaredNorm();
207 
208  EXPECT(assert_equal(expectedAugmentedHessian, actualAugmentedHessian));
209  }
210 }
211 
212 /* ************************************************************************* */
213 #include <gtsam/geometry/StereoCamera.h>
214 TEST(CameraSet, Stereo) {
215  CameraSet<StereoCamera> set;
216  typedef StereoCamera::MeasurementVector ZZ;
217  StereoCamera camera;
218  set.push_back(camera);
219  set.push_back(camera);
220  Point3 p(0, 0, 1);
221  EXPECT_LONGS_EQUAL(6, traits<StereoCamera>::dimension);
222 
223  // Check measurements
224  StereoPoint2 expected(0, -1, 0);
225  ZZ z = set.project2(p);
226  EXPECT(assert_equal(expected, z[0]));
227  EXPECT(assert_equal(expected, z[1]));
228 
229  // Calculate expected derivatives using Pinhole
230  Matrix63 actualE;
231  Matrix F1;
232  {
233  Matrix33 E1;
234  camera.project2(p, F1, E1);
235  actualE << E1, E1;
236  }
237 
238  // Check computed derivatives
239  CameraSet<StereoCamera>::FBlocks Fs;
240  Matrix E;
241  set.project2(p, Fs, E);
242  LONGS_EQUAL(2, Fs.size());
243  EXPECT(assert_equal(F1, Fs[0]));
244  EXPECT(assert_equal(F1, Fs[1]));
245  EXPECT(assert_equal(actualE, E));
246 }
247 
248 /* ************************************************************************* */
249 int main() {
250  TestResult tr;
251  return TestRegistry::runAllTests(tr);
252 }
253 /* ************************************************************************* */
254 
measured
Point2 measured(-17, 30)
schur
ComplexSchur< MatrixXcf > schur(4)
gtsam::symbol_shorthand::F
Key F(std::uint64_t j)
Definition: inference/Symbol.h:153
b
Scalar * b
Definition: benchVecAdd.cpp:17
TestRegistry::runAllTests
static int runAllTests(TestResult &result)
Definition: TestRegistry.cpp:27
cholesky::expected
Matrix expected
Definition: testMatrix.cpp:971
CameraSet.h
Base class to create smart factors on poses or cameras.
gtsam::Point2
Vector2 Point2
Definition: Point2.h:32
gtsam::assert_equal
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:40
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
std
Definition: BFloat16.h:88
numericalDerivative.h
Some functions to compute numerical derivatives.
A
Matrix< SCALARA, Dynamic, Dynamic, opt_A > A
Definition: bench_gemm.cpp:48
E1
E1
Definition: test_numpy_dtypes.cpp:102
H
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy y set format x g set format y g set format x2 g set format y2 g set format z g set angles radians set nogrid set key title set key left top Right noreverse box linetype linewidth samplen spacing width set nolabel set noarrow set nologscale set logscale x set set pointsize set encoding default set nopolar set noparametric set set set set surface set nocontour set clabel set mapping cartesian set nohidden3d set cntrparam order set cntrparam linear set cntrparam levels auto set cntrparam points set size set set xzeroaxis lt lw set x2zeroaxis lt lw set yzeroaxis lt lw set y2zeroaxis lt lw set tics in set ticslevel set tics set mxtics default set mytics default set mx2tics default set my2tics default set xtics border mirror norotate autofreq set ytics border mirror norotate autofreq set ztics border nomirror norotate autofreq set nox2tics set noy2tics set timestamp bottom norotate set rrange [*:*] noreverse nowriteback set trange [*:*] noreverse nowriteback set urange [*:*] noreverse nowriteback set vrange [*:*] noreverse nowriteback set xlabel matrix size set x2label set timefmt d m y n H
Definition: gnuplot_common_settings.hh:12
F1
const Matrix26 F1
Definition: testRegularImplicitSchurFactor.cpp:37
gtsam::CameraSet
A set of cameras, all with their own calibration.
Definition: CameraSet.h:36
inverse
EIGEN_DEVICE_FUNC const InverseReturnType inverse() const
Definition: ArrayCwiseUnaryOps.h:411
gtsam::StereoCamera::MeasurementVector
StereoPoint2Vector MeasurementVector
Definition: StereoCamera.h:56
gtsam::Unit3
Represents a 3D point on a unit sphere.
Definition: Unit3.h:42
PinholeCamera.h
Base class for all pinhole cameras.
gtsam::SymmetricBlockMatrix::selfadjointView
Eigen::SelfAdjointView< constBlock, Eigen::Upper > selfadjointView(DenseIndex I, DenseIndex J) const
Return the square sub-matrix that contains blocks(i:j, i:j).
Definition: SymmetricBlockMatrix.h:158
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:38
Eigen::aligned_allocator
STL compatible allocator to use with types requiring a non standrad alignment.
Definition: Memory.h:878
EXPECT
#define EXPECT(condition)
Definition: Test.h:150
v
Array< int, Dynamic, 1 > v
Definition: Array_initializer_list_vector_cxx11.cpp:1
gtsam::CameraSet::FBlocks
std::vector< MatrixZD, Eigen::aligned_allocator< MatrixZD > > FBlocks
Definition: CameraSet.h:78
LONGS_EQUAL
#define LONGS_EQUAL(expected, actual)
Definition: Test.h:134
gtsam
traits
Definition: chartTesting.h:28
FBlocks
std::vector< MatrixZD, Eigen::aligned_allocator< MatrixZD > > FBlocks
Definition: testSmartProjectionPoseFactorRollingShutter.cpp:222
E
DiscreteKey E(5, 2)
StereoCamera.h
A Stereo Camera based on two Simple Cameras.
EXPECT_LONGS_EQUAL
#define EXPECT_LONGS_EQUAL(expected, actual)
Definition: Test.h:154
pointAtInfinity
Unit3 pointAtInfinity(0, 0, -1000)
gtsam::Point2Vector
std::vector< Point2, Eigen::aligned_allocator< Point2 > > Point2Vector
Definition: Point2.h:49
main
int main()
Definition: testCameraSet.cpp:249
p
float * p
Definition: Tutorial_Map_using.cpp:9
pybind_wrapper_test_script.z
z
Definition: pybind_wrapper_test_script.py:61
TEST
TEST(CameraSet, Pinhole)
Definition: testCameraSet.cpp:32
Camera
PinholePose< Cal3_S2 > Camera
Definition: SFMExample_SmartFactor.cpp:38
gtsam::Point3
Vector3 Point3
Definition: Point3.h:38
keys
const KeyVector keys
Definition: testRegularImplicitSchurFactor.cpp:40
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition: Key.h:86
camera
static const CalibratedCamera camera(kDefaultPose)
Pose3.h
3D Pose
Cal3Bundler.h
Calibration used by Bundler.
Cal3_S2.h
The most common 5DOF 3D->2D calibration.
gtsam::StereoCamera::project2
StereoPoint2 project2(const Point3 &point, OptionalJacobian< 3, 6 > H1={}, OptionalJacobian< 3, 3 > H2={}) const
Definition: StereoCamera.cpp:37

gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:37:59