gtsam 4.2.0
gtsam
Loading...
Searching...
No Matches
CameraSet.h
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#pragma once
20
21#include <gtsam/base/FastMap.h>
23#include <gtsam/base/Testable.h>
24#include <gtsam/geometry/CalibratedCamera.h> // for Cheirality exception
26#include <gtsam/inference/Key.h>
27
28#include <vector>
29
30namespace gtsam {
31
35template <class CAMERA>
36class CameraSet : public std::vector<CAMERA, Eigen::aligned_allocator<CAMERA>> {
37 protected:
38 using Base = std::vector<CAMERA, typename Eigen::aligned_allocator<CAMERA>>;
39
44 typedef typename CAMERA::Measurement Z;
45 typedef typename CAMERA::MeasurementVector ZVector;
46
47 static const int D = traits<CAMERA>::dimension;
48 static const int ZDim = traits<Z>::dimension;
49
51 static Vector ErrorVector(const ZVector& predicted, const ZVector& measured) {
52 // Check size
53 size_t m = predicted.size();
54 if (measured.size() != m)
55 throw std::runtime_error("CameraSet::errors: size mismatch");
56
57 // Project and fill error vector
58 Vector b(ZDim * m);
59 for (size_t i = 0, row = 0; i < m; i++, row += ZDim) {
60 Vector bi = traits<Z>::Local(measured[i], predicted[i]);
61 if (ZDim == 3 && std::isnan(bi(1))) { // if it is a stereo point and the
62 // right pixel is missing (nan)
63 bi(1) = 0;
64 }
65 b.segment<ZDim>(row) = bi;
66 }
67 return b;
68 }
69
70 public:
71 using Base::Base; // Inherit the vector constructors
72
74 virtual ~CameraSet() = default;
75
77 using MatrixZD = Eigen::Matrix<double, ZDim, D>;
78 using FBlocks = std::vector<MatrixZD, Eigen::aligned_allocator<MatrixZD>>;
79
85 virtual void print(const std::string& s = "") const {
86 std::cout << s << "CameraSet, cameras = \n";
87 for (size_t k = 0; k < this->size(); ++k) this->at(k).print(s);
88 }
89
91 bool equals(const CameraSet& p, double tol = 1e-9) const {
92 if (this->size() != p.size()) return false;
93 bool camerasAreEqual = true;
94 for (size_t i = 0; i < this->size(); i++) {
95 if (this->at(i).equals(p.at(i), tol) == false) camerasAreEqual = false;
96 break;
97 }
98 return camerasAreEqual;
99 }
100
107 template <class POINT>
108 ZVector project2(const POINT& point, //
109 boost::optional<FBlocks&> Fs = boost::none, //
110 boost::optional<Matrix&> E = boost::none) const {
111 static const int N = FixedDimension<POINT>::value;
112
113 // Allocate result
114 size_t m = this->size();
115 ZVector z;
116 z.reserve(m);
117
118 // Allocate derivatives
119 if (E) E->resize(ZDim * m, N);
120 if (Fs) Fs->resize(m);
121
122 // Project and fill derivatives
123 for (size_t i = 0; i < m; i++) {
124 MatrixZD Fi;
125 Eigen::Matrix<double, ZDim, N> Ei;
126 z.emplace_back(this->at(i).project2(point, Fs ? &Fi : 0, E ? &Ei : 0));
127 if (Fs) (*Fs)[i] = Fi;
128 if (E) E->block<ZDim, N>(ZDim * i, 0) = Ei;
129 }
130
131 return z;
132 }
133
135 template <class POINT>
136 Vector reprojectionError(const POINT& point, const ZVector& measured,
137 boost::optional<FBlocks&> Fs = boost::none, //
138 boost::optional<Matrix&> E = boost::none) const {
139 return ErrorVector(project2(point, Fs, E), measured);
140 }
141
148 template <int N,
149 int ND> // N = 2 or 3 (point dimension), ND is the camera dimension
151 const std::vector<
152 Eigen::Matrix<double, ZDim, ND>,
153 Eigen::aligned_allocator<Eigen::Matrix<double, ZDim, ND>>>& Fs,
154 const Matrix& E, const Eigen::Matrix<double, N, N>& P, const Vector& b) {
155 // a single point is observed in m cameras
156 size_t m = Fs.size();
157
158 // Create a SymmetricBlockMatrix (augmented hessian, with extra row/column
159 // with info vector)
160 size_t M1 = ND * m + 1;
161 std::vector<DenseIndex> dims(m + 1); // this also includes the b term
162 std::fill(dims.begin(), dims.end() - 1, ND);
163 dims.back() = 1;
164 SymmetricBlockMatrix augmentedHessian(dims, Matrix::Zero(M1, M1));
165
166 // Blockwise Schur complement
167 for (size_t i = 0; i < m; i++) { // for each camera
168
169 const Eigen::Matrix<double, ZDim, ND>& Fi = Fs[i];
170 const auto FiT = Fi.transpose();
171 const Eigen::Matrix<double, ZDim, N> Ei_P = //
172 E.block(ZDim * i, 0, ZDim, N) * P;
173
174 // D = (Dx2) * ZDim
175 augmentedHessian.setOffDiagonalBlock(
176 i, m,
177 FiT * b.segment<ZDim>(ZDim * i) // F' * b
178 -
179 FiT *
180 (Ei_P *
181 (E.transpose() *
182 b))); // D = (DxZDim) * (ZDimx3) * (N*ZDimm) * (ZDimm x 1)
183
184 // (DxD) = (DxZDim) * ( (ZDimxD) - (ZDimx3) * (3xZDim) * (ZDimxD) )
185 augmentedHessian.setDiagonalBlock(
186 i,
187 FiT * (Fi - Ei_P * E.block(ZDim * i, 0, ZDim, N).transpose() * Fi));
188
189 // upper triangular part of the hessian
190 for (size_t j = i + 1; j < m; j++) { // for each camera
191 const Eigen::Matrix<double, ZDim, ND>& Fj = Fs[j];
192
193 // (DxD) = (Dx2) * ( (2x2) * (2xD) )
194 augmentedHessian.setOffDiagonalBlock(
195 i, j,
196 -FiT * (Ei_P * E.block(ZDim * j, 0, ZDim, N).transpose() * Fj));
197 }
198 } // end of for over cameras
199
200 augmentedHessian.diagonalBlock(m)(0, 0) += b.squaredNorm();
201 return augmentedHessian;
202 }
203
217 template <int N, int ND, int NDD>
219 const std::vector<
220 Eigen::Matrix<double, ZDim, ND>,
221 Eigen::aligned_allocator<Eigen::Matrix<double, ZDim, ND>>>& Fs,
222 const Matrix& E, const Eigen::Matrix<double, N, N>& P, const Vector& b,
223 const KeyVector& jacobianKeys, const KeyVector& hessianKeys) {
224 size_t nrNonuniqueKeys = jacobianKeys.size();
225 size_t nrUniqueKeys = hessianKeys.size();
226
227 // Marginalize point: note - we reuse the standard SchurComplement function.
228 SymmetricBlockMatrix augmentedHessian = SchurComplement<N, ND>(Fs, E, P, b);
229
230 // Pack into an Hessian factor, allow space for b term.
231 std::vector<DenseIndex> dims(nrUniqueKeys + 1);
232 std::fill(dims.begin(), dims.end() - 1, NDD);
233 dims.back() = 1;
234 SymmetricBlockMatrix augmentedHessianUniqueKeys;
235
236 // Deal with the fact that some blocks may share the same keys.
237 if (nrUniqueKeys == nrNonuniqueKeys) {
238 // Case when there is 1 calibration key per camera:
239 augmentedHessianUniqueKeys = SymmetricBlockMatrix(
240 dims, Matrix(augmentedHessian.selfadjointView()));
241 } else {
242 // When multiple cameras share a calibration we have to rearrange
243 // the results of the Schur complement matrix.
244 std::vector<DenseIndex> nonuniqueDims(nrNonuniqueKeys + 1); // includes b
245 std::fill(nonuniqueDims.begin(), nonuniqueDims.end() - 1, NDD);
246 nonuniqueDims.back() = 1;
247 augmentedHessian = SymmetricBlockMatrix(
248 nonuniqueDims, Matrix(augmentedHessian.selfadjointView()));
249
250 // Get map from key to location in the new augmented Hessian matrix (the
251 // one including only unique keys).
252 std::map<Key, size_t> keyToSlotMap;
253 for (size_t k = 0; k < nrUniqueKeys; k++) {
254 keyToSlotMap[hessianKeys[k]] = k;
255 }
256
257 // Initialize matrix to zero.
258 augmentedHessianUniqueKeys = SymmetricBlockMatrix(
259 dims, Matrix::Zero(NDD * nrUniqueKeys + 1, NDD * nrUniqueKeys + 1));
260
261 // Add contributions for each key: note this loops over the hessian with
262 // nonUnique keys (augmentedHessian) and populates an Hessian that only
263 // includes the unique keys (that is what we want to return).
264 for (size_t i = 0; i < nrNonuniqueKeys; i++) { // rows
265 Key key_i = jacobianKeys.at(i);
266
267 // Update information vector.
268 augmentedHessianUniqueKeys.updateOffDiagonalBlock(
269 keyToSlotMap[key_i], nrUniqueKeys,
270 augmentedHessian.aboveDiagonalBlock(i, nrNonuniqueKeys));
271
272 // Update blocks.
273 for (size_t j = i; j < nrNonuniqueKeys; j++) { // cols
274 Key key_j = jacobianKeys.at(j);
275 if (i == j) {
276 augmentedHessianUniqueKeys.updateDiagonalBlock(
277 keyToSlotMap[key_i], augmentedHessian.diagonalBlock(i));
278 } else { // (i < j)
279 if (keyToSlotMap[key_i] != keyToSlotMap[key_j]) {
280 augmentedHessianUniqueKeys.updateOffDiagonalBlock(
281 keyToSlotMap[key_i], keyToSlotMap[key_j],
282 augmentedHessian.aboveDiagonalBlock(i, j));
283 } else {
284 augmentedHessianUniqueKeys.updateDiagonalBlock(
285 keyToSlotMap[key_i],
286 augmentedHessian.aboveDiagonalBlock(i, j) +
287 augmentedHessian.aboveDiagonalBlock(i, j).transpose());
288 }
289 }
290 }
291 }
292
293 // Update bottom right element of the matrix.
294 augmentedHessianUniqueKeys.updateDiagonalBlock(
295 nrUniqueKeys, augmentedHessian.diagonalBlock(nrNonuniqueKeys));
296 }
297 return augmentedHessianUniqueKeys;
298 }
299
306 template <int N> // N = 2 or 3
308 const FBlocks& Fs, const Matrix& E, const Eigen::Matrix<double, N, N>& P,
309 const Vector& b) {
310 return SchurComplement<N, D>(Fs, E, P, b);
311 }
312
314 template <int N> // N = 2 or 3 (point dimension)
315 static void ComputePointCovariance(Eigen::Matrix<double, N, N>& P,
316 const Matrix& E, double lambda,
317 bool diagonalDamping = false) {
318 Matrix EtE = E.transpose() * E;
319
320 if (diagonalDamping) { // diagonal of the hessian
321 EtE.diagonal() += lambda * EtE.diagonal();
322 } else {
323 DenseIndex n = E.cols();
324 EtE += lambda * Eigen::MatrixXd::Identity(n, n);
325 }
326
327 P = (EtE).inverse();
328 }
329
331 static Matrix PointCov(const Matrix& E, const double lambda = 0.0,
332 bool diagonalDamping = false) {
333 if (E.cols() == 2) {
334 Matrix2 P2;
335 ComputePointCovariance<2>(P2, E, lambda, diagonalDamping);
336 return P2;
337 } else {
338 Matrix3 P3;
339 ComputePointCovariance<3>(P3, E, lambda, diagonalDamping);
340 return P3;
341 }
342 }
343
348 static SymmetricBlockMatrix SchurComplement(const FBlocks& Fblocks,
349 const Matrix& E, const Vector& b,
350 const double lambda = 0.0,
351 bool diagonalDamping = false) {
352 if (E.cols() == 2) {
353 Matrix2 P;
354 ComputePointCovariance<2>(P, E, lambda, diagonalDamping);
355 return SchurComplement<2>(Fblocks, E, P, b);
356 } else {
357 Matrix3 P;
358 ComputePointCovariance<3>(P, E, lambda, diagonalDamping);
359 return SchurComplement<3>(Fblocks, E, P, b);
360 }
361 }
362
368 template <int N> // N = 2 or 3 (point dimension)
370 const FBlocks& Fs, const Matrix& E, const Eigen::Matrix<double, N, N>& P,
371 const Vector& b, const KeyVector& allKeys, const KeyVector& keys,
372 /*output ->*/ SymmetricBlockMatrix& augmentedHessian) {
373 assert(keys.size() == Fs.size());
374 assert(keys.size() <= allKeys.size());
375
376 FastMap<Key, size_t> KeySlotMap;
377 for (size_t slot = 0; slot < allKeys.size(); slot++)
378 KeySlotMap.insert(std::make_pair(allKeys[slot], slot));
379
380 // Schur complement trick
381 // G = F' * F - F' * E * P * E' * F
382 // g = F' * (b - E * P * E' * b)
383
384 // a single point is observed in m cameras
385 size_t m = Fs.size(); // cameras observing current point
386 size_t M = (augmentedHessian.rows() - 1) / D; // all cameras in the group
387 assert(allKeys.size() == M);
388
389 // Blockwise Schur complement
390 for (size_t i = 0; i < m; i++) { // for each camera in the current factor
391
392 const MatrixZD& Fi = Fs[i];
393 const auto FiT = Fi.transpose();
394 const Eigen::Matrix<double, 2, N> Ei_P =
395 E.template block<ZDim, N>(ZDim * i, 0) * P;
396
397 // D = (DxZDim) * (ZDim)
398 // allKeys are the list of all camera keys in the group, e.g, (1,3,4,5,7)
399 // we should map those to a slot in the local (grouped) hessian
400 // (0,1,2,3,4) Key cameraKey_i = this->keys_[i];
401 DenseIndex aug_i = KeySlotMap.at(keys[i]);
402
403 // information vector - store previous vector
404 // vectorBlock = augmentedHessian(aug_i, aug_m).knownOffDiagonal();
405 // add contribution of current factor
406 augmentedHessian.updateOffDiagonalBlock(
407 aug_i, M,
408 FiT * b.segment<ZDim>(ZDim * i) // F' * b
409 -
410 FiT *
411 (Ei_P *
412 (E.transpose() *
413 b))); // D = (DxZDim) * (ZDimx3) * (N*ZDimm) * (ZDimm x 1)
414
415 // (DxD) += (DxZDim) * ( (ZDimxD) - (ZDimx3) * (3xZDim) * (ZDimxD) )
416 // add contribution of current factor
417 // TODO(gareth): Eigen doesn't let us pass the expression. Call eval() for
418 // now...
419 augmentedHessian.updateDiagonalBlock(
420 aug_i,
421 ((FiT *
422 (Fi -
423 Ei_P * E.template block<ZDim, N>(ZDim * i, 0).transpose() * Fi)))
424 .eval());
425
426 // upper triangular part of the hessian
427 for (size_t j = i + 1; j < m; j++) { // for each camera
428 const MatrixZD& Fj = Fs[j];
429
430 DenseIndex aug_j = KeySlotMap.at(keys[j]);
431
432 // (DxD) = (DxZDim) * ( (ZDimxZDim) * (ZDimxD) )
433 // off diagonal block - store previous block
434 // matrixBlock = augmentedHessian(aug_i, aug_j).knownOffDiagonal();
435 // add contribution of current factor
436 augmentedHessian.updateOffDiagonalBlock(
437 aug_i, aug_j,
438 -FiT * (Ei_P * E.template block<ZDim, N>(ZDim * j, 0).transpose() *
439 Fj));
440 }
441 } // end of for over cameras
442
443 augmentedHessian.diagonalBlock(M)(0, 0) += b.squaredNorm();
444 }
445
446 private:
449 template <class ARCHIVE>
450 void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
451 ar&(*this);
452 }
453
454 public:
456};
457
458template <class CAMERA>
459const int CameraSet<CAMERA>::D;
460
461template <class CAMERA>
463
464template <class CAMERA>
465struct traits<CameraSet<CAMERA>> : public Testable<CameraSet<CAMERA>> {};
466
467template <class CAMERA>
468struct traits<const CameraSet<CAMERA>> : public Testable<CameraSet<CAMERA>> {};
469
470} // namespace gtsam
A thin wrapper around std::map that uses boost's fast_pool_allocator.
Access to matrices via blocks of pre-defined sizes.
Concept check for values that can be used in unit tests.
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
This marks a GTSAM object to require alignment.
Definition types.h:308
Calibrated camera for which only pose is unknown.
3D Point
Global functions in a separate testing namespace.
Definition chartTesting.h:28
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition Key.h:86
ptrdiff_t DenseIndex
The index type for Eigen objects.
Definition types.h:106
const MATRIX::ConstRowXpr row(const MATRIX &A, size_t j)
Extracts a row view from a matrix that avoids a copy.
Definition Matrix.h:222
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
FastMap is a thin wrapper around std::map that uses the boost fast_pool_allocator instead of the defa...
Definition FastMap.h:38
Give fixed size dimension of a type, fails at compile time if dynamic.
Definition Manifold.h:164
This class stores a dense matrix and allows it to be accessed as a collection of blocks.
Definition SymmetricBlockMatrix.h:52
void setDiagonalBlock(DenseIndex I, const XprType &xpr)
Set a diagonal block. Only the upper triangular portion of xpr is evaluated.
Definition SymmetricBlockMatrix.h:195
void setOffDiagonalBlock(DenseIndex I, DenseIndex J, const XprType &xpr)
Set an off-diagonal block. Only the upper triangular portion of xpr is evaluated.
Definition SymmetricBlockMatrix.h:201
constBlock aboveDiagonalBlock(DenseIndex I, DenseIndex J) const
Get block above the diagonal (I, J).
Definition SymmetricBlockMatrix.h:150
DenseIndex rows() const
Row size.
Definition SymmetricBlockMatrix.h:114
void updateOffDiagonalBlock(DenseIndex I, DenseIndex J, const XprType &xpr)
Update an off diagonal block.
Definition SymmetricBlockMatrix.h:228
Eigen::SelfAdjointView< Block, Eigen::Upper > diagonalBlock(DenseIndex J)
Return the J'th diagonal block as a self adjoint view.
Definition SymmetricBlockMatrix.h:135
void updateDiagonalBlock(DenseIndex I, const XprType &xpr)
Increment the diagonal block by the values in xpr. Only reads the upper triangular part of xpr.
Definition SymmetricBlockMatrix.h:212
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:156
A helper that implements the traits interface for GTSAM types.
Definition Testable.h:151
A set of cameras, all with their own calibration.
Definition CameraSet.h:36
Vector reprojectionError(const POINT &point, const ZVector &measured, boost::optional< FBlocks & > Fs=boost::none, boost::optional< Matrix & > E=boost::none) const
Calculate vector [project2(point)-z] of re-projection errors.
Definition CameraSet.h:136
virtual void print(const std::string &s="") const
print
Definition CameraSet.h:85
static void ComputePointCovariance(Eigen::Matrix< double, N, N > &P, const Matrix &E, double lambda, bool diagonalDamping=false)
Computes Point Covariance P, with lambda parameter.
Definition CameraSet.h:315
static SymmetricBlockMatrix SchurComplement(const FBlocks &Fblocks, const Matrix &E, const Vector &b, const double lambda=0.0, bool diagonalDamping=false)
Do Schur complement, given Jacobian as Fs,E,P, return SymmetricBlockMatrix Dynamic version.
Definition CameraSet.h:348
CAMERA::Measurement Z
2D measurement and noise model for each of the m views The order is kept the same as the keys that we...
Definition CameraSet.h:44
virtual ~CameraSet()=default
Destructor.
bool equals(const CameraSet &p, double tol=1e-9) const
equals
Definition CameraSet.h:91
static Matrix PointCov(const Matrix &E, const double lambda=0.0, bool diagonalDamping=false)
Computes Point Covariance P, with lambda parameter, dynamic version.
Definition CameraSet.h:331
static Vector ErrorVector(const ZVector &predicted, const ZVector &measured)
Make a vector of re-projection errors.
Definition CameraSet.h:51
static void UpdateSchurComplement(const FBlocks &Fs, const Matrix &E, const Eigen::Matrix< double, N, N > &P, const Vector &b, const KeyVector &allKeys, const KeyVector &keys, SymmetricBlockMatrix &augmentedHessian)
Applies Schur complement (exploiting block structure) to get a smart factor on cameras,...
Definition CameraSet.h:369
static SymmetricBlockMatrix SchurComplementAndRearrangeBlocks(const std::vector< Eigen::Matrix< double, ZDim, ND >, Eigen::aligned_allocator< Eigen::Matrix< double, ZDim, ND > > > &Fs, const Matrix &E, const Eigen::Matrix< double, N, N > &P, const Vector &b, const KeyVector &jacobianKeys, const KeyVector &hessianKeys)
Do Schur complement, given Jacobian as Fs,E,P, return SymmetricBlockMatrix G = F' * F - F' * E * P * ...
Definition CameraSet.h:218
static SymmetricBlockMatrix SchurComplement(const std::vector< Eigen::Matrix< double, ZDim, ND >, Eigen::aligned_allocator< Eigen::Matrix< double, ZDim, ND > > > &Fs, const Matrix &E, const Eigen::Matrix< double, N, N > &P, const Vector &b)
Do Schur complement, given Jacobian as Fs,E,P, return SymmetricBlockMatrix G = F' * F - F' * E * P * ...
Definition CameraSet.h:150
static const int ZDim
Measurement dimension.
Definition CameraSet.h:48
static SymmetricBlockMatrix SchurComplement(const FBlocks &Fs, const Matrix &E, const Eigen::Matrix< double, N, N > &P, const Vector &b)
Do Schur complement, given Jacobian as Fs,E,P, return SymmetricBlockMatrix G = F' * F - F' * E * P * ...
Definition CameraSet.h:307
static const int D
Camera dimension.
Definition CameraSet.h:47
friend class boost::serialization::access
Serialization function.
Definition CameraSet.h:448
Eigen::Matrix< double, ZDim, D > MatrixZD
Definitions for blocks of F.
Definition CameraSet.h:77
ZVector project2(const POINT &point, boost::optional< FBlocks & > Fs=boost::none, boost::optional< Matrix & > E=boost::none) const
Project a point (possibly Unit3 at infinity), with derivatives Note that F is a sparse block-diagonal...
Definition CameraSet.h:108