gtsam  4.1.0
gtsam
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/geometry/Point3.h>
22 #include <gtsam/geometry/CalibratedCamera.h> // for Cheirality exception
23 #include <gtsam/base/Testable.h>
25 #include <gtsam/base/FastMap.h>
26 #include <gtsam/inference/Key.h>
27 #include <vector>
28 
29 namespace gtsam {
30 
34 template<class CAMERA>
35 class CameraSet : public std::vector<CAMERA, Eigen::aligned_allocator<CAMERA> > {
36 
37 protected:
38 
43  typedef typename CAMERA::Measurement Z;
44  typedef typename CAMERA::MeasurementVector ZVector;
45 
46  static const int D = traits<CAMERA>::dimension;
47  static const int ZDim = traits<Z>::dimension;
48 
50  static Vector ErrorVector(const ZVector& predicted,
51  const ZVector& measured) {
52 
53  // Check size
54  size_t m = predicted.size();
55  if (measured.size() != m)
56  throw std::runtime_error("CameraSet::errors: size mismatch");
57 
58  // Project and fill error vector
59  Vector b(ZDim * m);
60  for (size_t i = 0, row = 0; i < m; i++, row += ZDim) {
61  Vector bi = traits<Z>::Local(measured[i], predicted[i]);
62  if(ZDim==3 && std::isnan(bi(1))){ // if it is a stereo point and the 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 
73  typedef Eigen::Matrix<double, ZDim, D> MatrixZD;
74  typedef std::vector<MatrixZD, Eigen::aligned_allocator<MatrixZD> > FBlocks;
75 
81  virtual void print(const std::string& s = "") const {
82  std::cout << s << "CameraSet, cameras = \n";
83  for (size_t k = 0; k < this->size(); ++k)
84  this->at(k).print(s);
85  }
86 
88  bool equals(const CameraSet& p, double tol = 1e-9) const {
89  if (this->size() != p.size())
90  return false;
91  bool camerasAreEqual = true;
92  for (size_t i = 0; i < this->size(); i++) {
93  if (this->at(i).equals(p.at(i), tol) == false)
94  camerasAreEqual = false;
95  break;
96  }
97  return camerasAreEqual;
98  }
99 
106  template<class POINT>
107  ZVector project2(const POINT& point, //
108  boost::optional<FBlocks&> Fs = boost::none, //
109  boost::optional<Matrix&> E = boost::none) const {
110 
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> // N = 2 or 3
149  static SymmetricBlockMatrix SchurComplement(const FBlocks& Fs,
150  const Matrix& E, const Eigen::Matrix<double, N, N>& P, const Vector& b) {
151 
152  // a single point is observed in m cameras
153  size_t m = Fs.size();
154 
155  // Create a SymmetricBlockMatrix
156  size_t M1 = D * m + 1;
157  std::vector<DenseIndex> dims(m + 1); // this also includes the b term
158  std::fill(dims.begin(), dims.end() - 1, D);
159  dims.back() = 1;
160  SymmetricBlockMatrix augmentedHessian(dims, Matrix::Zero(M1, M1));
161 
162  // Blockwise Schur complement
163  for (size_t i = 0; i < m; i++) { // for each camera
164 
165  const MatrixZD& Fi = Fs[i];
166  const auto FiT = Fi.transpose();
167  const Eigen::Matrix<double, ZDim, N> Ei_P = //
168  E.block(ZDim * i, 0, ZDim, N) * P;
169 
170  // D = (Dx2) * ZDim
171  augmentedHessian.setOffDiagonalBlock(i, m, FiT * b.segment<ZDim>(ZDim * i) // F' * b
172  - FiT * (Ei_P * (E.transpose() * b))); // D = (DxZDim) * (ZDimx3) * (N*ZDimm) * (ZDimm x 1)
173 
174  // (DxD) = (DxZDim) * ( (ZDimxD) - (ZDimx3) * (3xZDim) * (ZDimxD) )
175  augmentedHessian.setDiagonalBlock(i, FiT
176  * (Fi - Ei_P * E.block(ZDim * i, 0, ZDim, N).transpose() * Fi));
177 
178  // upper triangular part of the hessian
179  for (size_t j = i + 1; j < m; j++) { // for each camera
180  const MatrixZD& Fj = Fs[j];
181 
182  // (DxD) = (Dx2) * ( (2x2) * (2xD) )
183  augmentedHessian.setOffDiagonalBlock(i, j, -FiT
184  * (Ei_P * E.block(ZDim * j, 0, ZDim, N).transpose() * Fj));
185  }
186  } // end of for over cameras
187 
188  augmentedHessian.diagonalBlock(m)(0, 0) += b.squaredNorm();
189  return augmentedHessian;
190  }
191 
193  template<int N> // N = 2 or 3
194  static void ComputePointCovariance(Eigen::Matrix<double, N, N>& P,
195  const Matrix& E, double lambda, bool diagonalDamping = false) {
196 
197  Matrix EtE = E.transpose() * E;
198 
199  if (diagonalDamping) { // diagonal of the hessian
200  EtE.diagonal() += lambda * EtE.diagonal();
201  } else {
202  DenseIndex n = E.cols();
203  EtE += lambda * Eigen::MatrixXd::Identity(n, n);
204  }
205 
206  P = (EtE).inverse();
207  }
208 
210  static Matrix PointCov(const Matrix& E, const double lambda = 0.0,
211  bool diagonalDamping = false) {
212  if (E.cols() == 2) {
213  Matrix2 P2;
214  ComputePointCovariance<2>(P2, E, lambda, diagonalDamping);
215  return P2;
216  } else {
217  Matrix3 P3;
218  ComputePointCovariance<3>(P3, E, lambda, diagonalDamping);
219  return P3;
220  }
221  }
222 
227  static SymmetricBlockMatrix SchurComplement(const FBlocks& Fblocks,
228  const Matrix& E, const Vector& b, const double lambda = 0.0,
229  bool diagonalDamping = false) {
230  if (E.cols() == 2) {
231  Matrix2 P;
232  ComputePointCovariance<2>(P, E, lambda, diagonalDamping);
233  return SchurComplement<2>(Fblocks, E, P, b);
234  } else {
235  Matrix3 P;
236  ComputePointCovariance<3>(P, E, lambda, diagonalDamping);
237  return SchurComplement<3>(Fblocks, E, P, b);
238  }
239  }
240 
245  template<int N> // N = 2 or 3
246  static void UpdateSchurComplement(const FBlocks& Fs, const Matrix& E,
247  const Eigen::Matrix<double, N, N>& P, const Vector& b,
248  const KeyVector& allKeys, const KeyVector& keys,
249  /*output ->*/SymmetricBlockMatrix& augmentedHessian) {
250 
251  assert(keys.size()==Fs.size());
252  assert(keys.size()<=allKeys.size());
253 
254  FastMap<Key, size_t> KeySlotMap;
255  for (size_t slot = 0; slot < allKeys.size(); slot++)
256  KeySlotMap.insert(std::make_pair(allKeys[slot], slot));
257 
258  // Schur complement trick
259  // G = F' * F - F' * E * P * E' * F
260  // g = F' * (b - E * P * E' * b)
261 
262  // a single point is observed in m cameras
263  size_t m = Fs.size(); // cameras observing current point
264  size_t M = (augmentedHessian.rows() - 1) / D; // all cameras in the group
265  assert(allKeys.size()==M);
266 
267  // Blockwise Schur complement
268  for (size_t i = 0; i < m; i++) { // for each camera in the current factor
269 
270  const MatrixZD& Fi = Fs[i];
271  const auto FiT = Fi.transpose();
272  const Eigen::Matrix<double, 2, N> Ei_P = E.template block<ZDim, N>(
273  ZDim * i, 0) * P;
274 
275  // D = (DxZDim) * (ZDim)
276  // allKeys are the list of all camera keys in the group, e.g, (1,3,4,5,7)
277  // we should map those to a slot in the local (grouped) hessian (0,1,2,3,4)
278  // Key cameraKey_i = this->keys_[i];
279  DenseIndex aug_i = KeySlotMap.at(keys[i]);
280 
281  // information vector - store previous vector
282  // vectorBlock = augmentedHessian(aug_i, aug_m).knownOffDiagonal();
283  // add contribution of current factor
284  augmentedHessian.updateOffDiagonalBlock(aug_i, M,
285  FiT * b.segment<ZDim>(ZDim * i) // F' * b
286  - FiT * (Ei_P * (E.transpose() * b))); // D = (DxZDim) * (ZDimx3) * (N*ZDimm) * (ZDimm x 1)
287 
288  // (DxD) += (DxZDim) * ( (ZDimxD) - (ZDimx3) * (3xZDim) * (ZDimxD) )
289  // add contribution of current factor
290  // TODO(gareth): Eigen doesn't let us pass the expression. Call eval() for now...
291  augmentedHessian.updateDiagonalBlock(aug_i,
292  ((FiT * (Fi - Ei_P * E.template block<ZDim, N>(ZDim * i, 0).transpose() * Fi))).eval());
293 
294  // upper triangular part of the hessian
295  for (size_t j = i + 1; j < m; j++) { // for each camera
296  const MatrixZD& Fj = Fs[j];
297 
298  DenseIndex aug_j = KeySlotMap.at(keys[j]);
299 
300  // (DxD) = (DxZDim) * ( (ZDimxZDim) * (ZDimxD) )
301  // off diagonal block - store previous block
302  // matrixBlock = augmentedHessian(aug_i, aug_j).knownOffDiagonal();
303  // add contribution of current factor
304  augmentedHessian.updateOffDiagonalBlock(aug_i, aug_j,
305  -FiT * (Ei_P * E.template block<ZDim, N>(ZDim * j, 0).transpose() * Fj));
306  }
307  } // end of for over cameras
308 
309  augmentedHessian.diagonalBlock(M)(0, 0) += b.squaredNorm();
310  }
311 
312 private:
313 
316  template<class ARCHIVE>
317  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
318  ar & (*this);
319  }
320 
321 public:
323 };
324 
325 template<class CAMERA>
326 const int CameraSet<CAMERA>::D;
327 
328 template<class CAMERA>
329 const int CameraSet<CAMERA>::ZDim;
330 
331 template<class CAMERA>
332 struct traits<CameraSet<CAMERA> > : public Testable<CameraSet<CAMERA> > {
333 };
334 
335 template<class CAMERA>
336 struct traits<const CameraSet<CAMERA> > : public Testable<CameraSet<CAMERA> > {
337 };
338 
339 } // \ namespace gtsam
gtsam::SymmetricBlockMatrix::updateOffDiagonalBlock
void updateOffDiagonalBlock(DenseIndex I, DenseIndex J, const XprType &xpr)
Update an off diagonal block.
Definition: SymmetricBlockMatrix.h:233
Testable.h
Concept check for values that can be used in unit tests.
gtsam::CameraSet::ZDim
static const int ZDim
Measurement dimension.
Definition: CameraSet.h:47
gtsam::SymmetricBlockMatrix::setDiagonalBlock
void setDiagonalBlock(DenseIndex I, const XprType &xpr)
Set a diagonal block. Only the upper triangular portion of xpr is evaluated.
Definition: SymmetricBlockMatrix.h:200
gtsam::CameraSet::equals
bool equals(const CameraSet &p, double tol=1e-9) const
equals
Definition: CameraSet.h:88
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition: concepts.h:30
gtsam::CameraSet::Z
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:43
gtsam::CameraSet::project2
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:107
gtsam::CameraSet::PointCov
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:210
gtsam::CameraSet::print
virtual void print(const std::string &s="") const
print
Definition: CameraSet.h:81
FastMap.h
A thin wrapper around std::map that uses boost's fast_pool_allocator.
gtsam::CameraSet
A set of cameras, all with their own calibration.
Definition: CameraSet.h:35
gtsam::CameraSet::D
static const int D
Camera dimension.
Definition: CameraSet.h:46
gtsam::FixedDimension
Give fixed size dimension of a type, fails at compile time if dynamic.
Definition: Manifold.h:164
gtsam
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
gtsam::CameraSet::MatrixZD
Eigen::Matrix< double, ZDim, D > MatrixZD
Definitions for blocks of F.
Definition: CameraSet.h:73
gtsam::CameraSet::access
friend class boost::serialization::access
Serialization function.
Definition: CameraSet.h:315
gtsam::Testable
A helper that implements the traits interface for GTSAM types.
Definition: Testable.h:150
gtsam::CameraSet::reprojectionError
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
gtsam::row
const MATRIX::ConstRowXpr row(const MATRIX &A, size_t j)
Extracts a row view from a matrix that avoids a copy.
Definition: Matrix.h:225
Point3.h
3D Point
gtsam::SymmetricBlockMatrix::updateDiagonalBlock
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:217
Key.h
gtsam::DenseIndex
ptrdiff_t DenseIndex
The index type for Eigen objects.
Definition: types.h:67
gtsam::CameraSet::ComputePointCovariance
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:194
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
This marks a GTSAM object to require alignment.
Definition: types.h:269
gtsam::SymmetricBlockMatrix::rows
DenseIndex rows() const
Row size.
Definition: SymmetricBlockMatrix.h:119
gtsam::CameraSet::UpdateSchurComplement
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:246
gtsam::CameraSet::SchurComplement
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:227
gtsam::SymmetricBlockMatrix::diagonalBlock
Eigen::SelfAdjointView< Block, Eigen::Upper > diagonalBlock(DenseIndex J)
Return the J'th diagonal block as a self adjoint view.
Definition: SymmetricBlockMatrix.h:140
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition: Key.h:86
gtsam::FastMap< Key, size_t >
gtsam::SymmetricBlockMatrix
Definition: SymmetricBlockMatrix.h:52
gtsam::CameraSet::ErrorVector
static Vector ErrorVector(const ZVector &predicted, const ZVector &measured)
Make a vector of re-projection errors.
Definition: CameraSet.h:50
SymmetricBlockMatrix.h
Access to matrices via blocks of pre-defined sizes.
gtsam::CameraSet::SchurComplement
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:149
gtsam::SymmetricBlockMatrix::setOffDiagonalBlock
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:206
CalibratedCamera.h
Calibrated camera for which only pose is unknown.