35template <
class CAMERA>
36class CameraSet :
public std::vector<CAMERA, Eigen::aligned_allocator<CAMERA>> {
38 using Base = std::vector<CAMERA, typename Eigen::aligned_allocator<CAMERA>>;
44 typedef typename CAMERA::Measurement
Z;
45 typedef typename CAMERA::MeasurementVector ZVector;
51 static Vector
ErrorVector(
const ZVector& predicted,
const ZVector& measured) {
53 size_t m = predicted.size();
54 if (measured.size() != m)
55 throw std::runtime_error(
"CameraSet::errors: size mismatch");
59 for (
size_t i = 0,
row = 0; i < m; i++,
row +=
ZDim) {
61 if (
ZDim == 3 && std::isnan(bi(1))) {
77 using MatrixZD = Eigen::Matrix<double, ZDim, D>;
78 using FBlocks = std::vector<MatrixZD, Eigen::aligned_allocator<MatrixZD>>;
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);
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;
98 return camerasAreEqual;
107 template <
class POINT>
109 boost::optional<FBlocks&> Fs = boost::none,
110 boost::optional<Matrix&> E = boost::none)
const {
114 size_t m = this->size();
119 if (E) E->resize(
ZDim * m, N);
120 if (Fs) Fs->resize(m);
123 for (
size_t i = 0; i < m; i++) {
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;
135 template <
class POINT>
137 boost::optional<FBlocks&> Fs = boost::none,
138 boost::optional<Matrix&> E = boost::none)
const {
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) {
156 size_t m = Fs.size();
160 size_t M1 = ND * m + 1;
161 std::vector<DenseIndex> dims(m + 1);
162 std::fill(dims.begin(), dims.end() - 1, ND);
167 for (
size_t i = 0; i < m; i++) {
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 =
187 FiT * (Fi - Ei_P * E.block(
ZDim * i, 0,
ZDim, N).transpose() * Fi));
190 for (
size_t j = i + 1; j < m; j++) {
191 const Eigen::Matrix<double, ZDim, ND>& Fj = Fs[j];
196 -FiT * (Ei_P * E.block(
ZDim * j, 0,
ZDim, N).transpose() * Fj));
201 return augmentedHessian;
217 template <
int N,
int ND,
int NDD>
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,
224 size_t nrNonuniqueKeys = jacobianKeys.size();
225 size_t nrUniqueKeys = hessianKeys.size();
231 std::vector<DenseIndex> dims(nrUniqueKeys + 1);
232 std::fill(dims.begin(), dims.end() - 1, NDD);
237 if (nrUniqueKeys == nrNonuniqueKeys) {
244 std::vector<DenseIndex> nonuniqueDims(nrNonuniqueKeys + 1);
245 std::fill(nonuniqueDims.begin(), nonuniqueDims.end() - 1, NDD);
246 nonuniqueDims.back() = 1;
252 std::map<Key, size_t> keyToSlotMap;
253 for (
size_t k = 0; k < nrUniqueKeys; k++) {
254 keyToSlotMap[hessianKeys[k]] = k;
259 dims, Matrix::Zero(NDD * nrUniqueKeys + 1, NDD * nrUniqueKeys + 1));
264 for (
size_t i = 0; i < nrNonuniqueKeys; i++) {
265 Key key_i = jacobianKeys.at(i);
269 keyToSlotMap[key_i], nrUniqueKeys,
273 for (
size_t j = i; j < nrNonuniqueKeys; j++) {
274 Key key_j = jacobianKeys.at(j);
279 if (keyToSlotMap[key_i] != keyToSlotMap[key_j]) {
281 keyToSlotMap[key_i], keyToSlotMap[key_j],
295 nrUniqueKeys, augmentedHessian.
diagonalBlock(nrNonuniqueKeys));
297 return augmentedHessianUniqueKeys;
308 const FBlocks& Fs,
const Matrix& E,
const Eigen::Matrix<double, N, N>& P,
310 return SchurComplement<N, D>(Fs, E, P, b);
316 const Matrix& E,
double lambda,
317 bool diagonalDamping =
false) {
318 Matrix EtE = E.transpose() * E;
320 if (diagonalDamping) {
321 EtE.diagonal() += lambda * EtE.diagonal();
324 EtE += lambda * Eigen::MatrixXd::Identity(n, n);
331 static Matrix
PointCov(
const Matrix& E,
const double lambda = 0.0,
332 bool diagonalDamping =
false) {
335 ComputePointCovariance<2>(P2, E, lambda, diagonalDamping);
339 ComputePointCovariance<3>(P3, E, lambda, diagonalDamping);
349 const Matrix& E,
const Vector& b,
350 const double lambda = 0.0,
351 bool diagonalDamping =
false) {
354 ComputePointCovariance<2>(P, E, lambda, diagonalDamping);
355 return SchurComplement<2>(Fblocks, E, P, b);
358 ComputePointCovariance<3>(P, E, lambda, diagonalDamping);
359 return SchurComplement<3>(Fblocks, E, P, b);
370 const FBlocks& Fs,
const Matrix& E,
const Eigen::Matrix<double, N, N>& P,
373 assert(keys.size() == Fs.size());
374 assert(keys.size() <= allKeys.size());
377 for (
size_t slot = 0; slot < allKeys.size(); slot++)
378 KeySlotMap.insert(std::make_pair(allKeys[slot], slot));
385 size_t m = Fs.size();
386 size_t M = (augmentedHessian.
rows() - 1) /
D;
387 assert(allKeys.size() == M);
390 for (
size_t i = 0; i < m; 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;
423 Ei_P * E.template block<ZDim, N>(
ZDim * i, 0).transpose() * Fi)))
427 for (
size_t j = i + 1; j < m; j++) {
438 -FiT * (Ei_P * E.template block<ZDim, N>(
ZDim * j, 0).transpose() *
449 template <
class ARCHIVE>
450 void serialize(ARCHIVE& ar,
const unsigned int ) {
458template <
class CAMERA>
461template <
class CAMERA>
464template <
class CAMERA>
467template <
class CAMERA>
Concept check for values that can be used in unit tests.
A thin wrapper around std::map that uses boost's fast_pool_allocator.
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
This marks a GTSAM object to require alignment.
Definition types.h:308
Access to matrices via blocks of pre-defined sizes.
Calibrated camera for which only pose is unknown.
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