Skip to content

Commit

Permalink
Merge pull request #827 from borglab/feature/rollingShutterSmartFactors
Browse files Browse the repository at this point in the history 
Feature/rolling shutter smart factors
  • Loading branch information
@dellaert
dellaert authored Aug 28, 2021
2 parents ff7ddf4 + 9798bfa commit d7f048b
Show file tree
Hide file tree
Showing 13 changed files with 2,641 additions and 126 deletions.
25 changes: 21 additions & 4 deletions gtsam/base/Lie.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -17,6 +17,7 @@
* @author Frank Dellaert
* @author Mike Bosse
* @author Duy Nguyen Ta
* @author Yotam Stern
*/


Expand Down Expand Up @@ -319,12 +320,28 @@ T expm(const Vector& x, int K=7) {
}

/**
* Linear interpolation between X and Y by coefficient t in [0, 1].
* Linear interpolation between X and Y by coefficient t. Typically t \in [0,1],
* but can also be used to extrapolate before pose X or after pose Y.
*/
template <typename T>
T interpolate(const T& X, const T& Y, double t) {
assert(t >= 0 && t <= 1);
return traits<T>::Compose(X, traits<T>::Expmap(t * traits<T>::Logmap(traits<T>::Between(X, Y))));
T interpolate(const T& X, const T& Y, double t,
typename MakeOptionalJacobian<T, T>::type Hx = boost::none,
typename MakeOptionalJacobian<T, T>::type Hy = boost::none) {
if (Hx || Hy) {
typename MakeJacobian<T, T>::type between_H_x, log_H, exp_H, compose_H_x;
const T between =
traits<T>::Between(X, Y, between_H_x); // between_H_y = identity
typename traits<T>::TangentVector delta = traits<T>::Logmap(between, log_H);
const T Delta = traits<T>::Expmap(t * delta, exp_H);
const T result = traits<T>::Compose(
X, Delta, compose_H_x); // compose_H_xinv_y = identity

if (Hx) *Hx = compose_H_x + t * exp_H * log_H * between_H_x;
if (Hy) *Hy = t * exp_H * log_H;
return result;
}
return traits<T>::Compose(
X, traits<T>::Expmap(t * traits<T>::Logmap(traits<T>::Between(X, Y))));
}

/**
Expand Down
180 changes: 139 additions & 41 deletions gtsam/geometry/CameraSet.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -147,51 +147,149 @@ class CameraSet : public std::vector<CAMERA, Eigen::aligned_allocator<CAMERA> >
* G = F' * F - F' * E * P * E' * F
* g = F' * (b - E * P * E' * b)
* Fixed size version
*/
template<int N, int ND> // N = 2 or 3, ND is the camera dimension
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) {

// a single point is observed in m cameras
size_t m = Fs.size();

// Create a SymmetricBlockMatrix (augmented hessian, with extra row/column with info vector)
size_t M1 = ND * m + 1;
std::vector<DenseIndex> dims(m + 1); // this also includes the b term
std::fill(dims.begin(), dims.end() - 1, ND);
dims.back() = 1;
SymmetricBlockMatrix augmentedHessian(dims, Matrix::Zero(M1, M1));

// Blockwise Schur complement
for (size_t i = 0; i < m; i++) { // for each camera

const Eigen::Matrix<double, ZDim, ND>& Fi = Fs[i];
const auto FiT = Fi.transpose();
const Eigen::Matrix<double, ZDim, N> Ei_P = //
E.block(ZDim * i, 0, ZDim, N) * P;

// D = (Dx2) * ZDim
augmentedHessian.setOffDiagonalBlock(i, m, FiT * b.segment<ZDim>(ZDim * i) // F' * b
- FiT * (Ei_P * (E.transpose() * b))); // D = (DxZDim) * (ZDimx3) * (N*ZDimm) * (ZDimm x 1)
*/
template <int N,
int ND> // N = 2 or 3 (point dimension), ND is the camera dimension
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) {
// a single point is observed in m cameras
size_t m = Fs.size();

// Create a SymmetricBlockMatrix (augmented hessian, with extra row/column with info vector)
size_t M1 = ND * m + 1;
std::vector<DenseIndex> dims(m + 1); // this also includes the b term
std::fill(dims.begin(), dims.end() - 1, ND);
dims.back() = 1;
SymmetricBlockMatrix augmentedHessian(dims, Matrix::Zero(M1, M1));

// Blockwise Schur complement
for (size_t i = 0; i < m; i++) { // for each camera

const Eigen::Matrix<double, ZDim, ND>& Fi = Fs[i];
const auto FiT = Fi.transpose();
const Eigen::Matrix<double, ZDim, N> Ei_P = //
E.block(ZDim * i, 0, ZDim, N) * P;

// D = (Dx2) * ZDim
augmentedHessian.setOffDiagonalBlock(i, m, FiT * b.segment<ZDim>(ZDim * i) // F' * b
- FiT * (Ei_P * (E.transpose() * b))); // D = (DxZDim) * (ZDimx3) * (N*ZDimm) * (ZDimm x 1)

// (DxD) = (DxZDim) * ( (ZDimxD) - (ZDimx3) * (3xZDim) * (ZDimxD) )
augmentedHessian.setDiagonalBlock(i, FiT
* (Fi - Ei_P * E.block(ZDim * i, 0, ZDim, N).transpose() * Fi));

// upper triangular part of the hessian
for (size_t j = i + 1; j < m; j++) { // for each camera
const Eigen::Matrix<double, ZDim, ND>& Fj = Fs[j];

// (DxD) = (Dx2) * ( (2x2) * (2xD) )
augmentedHessian.setOffDiagonalBlock(i, j, -FiT
* (Ei_P * E.block(ZDim * j, 0, ZDim, N).transpose() * Fj));
}
} // end of for over cameras

// (DxD) = (DxZDim) * ( (ZDimxD) - (ZDimx3) * (3xZDim) * (ZDimxD) )
augmentedHessian.setDiagonalBlock(i, FiT
* (Fi - Ei_P * E.block(ZDim * i, 0, ZDim, N).transpose() * Fi));
augmentedHessian.diagonalBlock(m)(0, 0) += b.squaredNorm();
return augmentedHessian;
}

// upper triangular part of the hessian
for (size_t j = i + 1; j < m; j++) { // for each camera
const Eigen::Matrix<double, ZDim, ND>& Fj = Fs[j];
/**
* Do Schur complement, given Jacobian as Fs,E,P, return SymmetricBlockMatrix
* G = F' * F - F' * E * P * E' * F
* g = F' * (b - E * P * E' * b)
* In this version, we allow for the case where the keys in the Jacobian are
* organized differently from the keys in the output SymmetricBlockMatrix In
* particular: each diagonal block of the Jacobian F captures 2 poses (useful
* for rolling shutter and extrinsic calibration) such that F keeps the block
* structure that makes the Schur complement trick fast.
*
* N = 2 or 3 (point dimension), ND is the Jacobian block dimension, NDD is
* the Hessian block dimension
*/
template <int N, int ND, int NDD>
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) {
size_t nrNonuniqueKeys = jacobianKeys.size();
size_t nrUniqueKeys = hessianKeys.size();

// Marginalize point: note - we reuse the standard SchurComplement function.
SymmetricBlockMatrix augmentedHessian = SchurComplement<N, ND>(Fs, E, P, b);

// Pack into an Hessian factor, allow space for b term.
std::vector<DenseIndex> dims(nrUniqueKeys + 1);
std::fill(dims.begin(), dims.end() - 1, NDD);
dims.back() = 1;
SymmetricBlockMatrix augmentedHessianUniqueKeys;

// Deal with the fact that some blocks may share the same keys.
if (nrUniqueKeys == nrNonuniqueKeys) {
// Case when there is 1 calibration key per camera:
augmentedHessianUniqueKeys = SymmetricBlockMatrix(
dims, Matrix(augmentedHessian.selfadjointView()));
} else {
// When multiple cameras share a calibration we have to rearrange
// the results of the Schur complement matrix.
std::vector<DenseIndex> nonuniqueDims(nrNonuniqueKeys + 1); // includes b
std::fill(nonuniqueDims.begin(), nonuniqueDims.end() - 1, NDD);
nonuniqueDims.back() = 1;
augmentedHessian = SymmetricBlockMatrix(
nonuniqueDims, Matrix(augmentedHessian.selfadjointView()));

// Get map from key to location in the new augmented Hessian matrix (the
// one including only unique keys).
std::map<Key, size_t> keyToSlotMap;
for (size_t k = 0; k < nrUniqueKeys; k++) {
keyToSlotMap[hessianKeys[k]] = k;
}

// (DxD) = (Dx2) * ( (2x2) * (2xD) )
augmentedHessian.setOffDiagonalBlock(i, j, -FiT
* (Ei_P * E.block(ZDim * j, 0, ZDim, N).transpose() * Fj));
// Initialize matrix to zero.
augmentedHessianUniqueKeys = SymmetricBlockMatrix(
dims, Matrix::Zero(NDD * nrUniqueKeys + 1, NDD * nrUniqueKeys + 1));

// Add contributions for each key: note this loops over the hessian with
// nonUnique keys (augmentedHessian) and populates an Hessian that only
// includes the unique keys (that is what we want to return).
for (size_t i = 0; i < nrNonuniqueKeys; i++) { // rows
Key key_i = jacobianKeys.at(i);

// Update information vector.
augmentedHessianUniqueKeys.updateOffDiagonalBlock(
keyToSlotMap[key_i], nrUniqueKeys,
augmentedHessian.aboveDiagonalBlock(i, nrNonuniqueKeys));

// Update blocks.
for (size_t j = i; j < nrNonuniqueKeys; j++) { // cols
Key key_j = jacobianKeys.at(j);
if (i == j) {
augmentedHessianUniqueKeys.updateDiagonalBlock(
keyToSlotMap[key_i], augmentedHessian.diagonalBlock(i));
} else { // (i < j)
if (keyToSlotMap[key_i] != keyToSlotMap[key_j]) {
augmentedHessianUniqueKeys.updateOffDiagonalBlock(
keyToSlotMap[key_i], keyToSlotMap[key_j],
augmentedHessian.aboveDiagonalBlock(i, j));
} else {
augmentedHessianUniqueKeys.updateDiagonalBlock(
keyToSlotMap[key_i],
augmentedHessian.aboveDiagonalBlock(i, j) +
augmentedHessian.aboveDiagonalBlock(i, j).transpose());
}
}
}
} // end of for over cameras
}

augmentedHessian.diagonalBlock(m)(0, 0) += b.squaredNorm();
return augmentedHessian;
// Update bottom right element of the matrix.
augmentedHessianUniqueKeys.updateDiagonalBlock(
nrUniqueKeys, augmentedHessian.diagonalBlock(nrNonuniqueKeys));
}
return augmentedHessianUniqueKeys;
}

/**
* Do Schur complement, given Jacobian as Fs,E,P, return SymmetricBlockMatrix
Expand All @@ -206,7 +304,7 @@ class CameraSet : public std::vector<CAMERA, Eigen::aligned_allocator<CAMERA> >
}

/// Computes Point Covariance P, with lambda parameter
template<int N> // N = 2 or 3
template<int N> // N = 2 or 3 (point dimension)
static void ComputePointCovariance(Eigen::Matrix<double, N, N>& P,
const Matrix& E, double lambda, bool diagonalDamping = false) {

Expand Down Expand Up @@ -258,7 +356,7 @@ class CameraSet : public std::vector<CAMERA, Eigen::aligned_allocator<CAMERA> >
* Applies Schur complement (exploiting block structure) to get a smart factor on cameras,
* and adds the contribution of the smart factor to a pre-allocated augmented Hessian.
*/
template<int N> // N = 2 or 3
template<int N> // N = 2 or 3 (point dimension)
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,
Expand Down
84 changes: 84 additions & 0 deletions gtsam/geometry/tests/testCameraSet.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -17,6 +17,7 @@
*/

#include <gtsam/geometry/CameraSet.h>
#include <gtsam/geometry/Cal3_S2.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/base/numericalDerivative.h>
#include <CppUnitLite/TestHarness.h>
Expand Down Expand Up @@ -125,6 +126,89 @@ TEST(CameraSet, Pinhole) {
EXPECT(assert_equal(actualE, E));
}

/* ************************************************************************* */
TEST(CameraSet, SchurComplementAndRearrangeBlocks) {
typedef PinholePose<Cal3Bundler> Camera;
typedef CameraSet<Camera> Set;

// this is the (block) Jacobian with respect to the nonuniqueKeys
std::vector<Eigen::Matrix<double, 2, 12>,
Eigen::aligned_allocator<Eigen::Matrix<double, 2, 12> > > Fs;
Fs.push_back(1 * Matrix::Ones(2, 12)); // corresponding to key pair (0,1)
Fs.push_back(2 * Matrix::Ones(2, 12)); // corresponding to key pair (1,2)
Fs.push_back(3 * Matrix::Ones(2, 12)); // corresponding to key pair (2,0)
Matrix E = 4 * Matrix::Identity(6, 3) + Matrix::Ones(6, 3);
E(0, 0) = 3;
E(1, 1) = 2;
E(2, 2) = 5;
Matrix Et = E.transpose();
Matrix P = (Et * E).inverse();
Vector b = 5 * Vector::Ones(6);

{ // SchurComplement
// Actual
SymmetricBlockMatrix augmentedHessianBM = Set::SchurComplement<3, 12>(Fs, E,
P, b);
Matrix actualAugmentedHessian = augmentedHessianBM.selfadjointView();

// Expected
Matrix F = Matrix::Zero(6, 3 * 12);
F.block<2, 12>(0, 0) = 1 * Matrix::Ones(2, 12);
F.block<2, 12>(2, 12) = 2 * Matrix::Ones(2, 12);
F.block<2, 12>(4, 24) = 3 * Matrix::Ones(2, 12);

Matrix Ft = F.transpose();
Matrix H = Ft * F - Ft * E * P * Et * F;
Vector v = Ft * (b - E * P * Et * b);
Matrix expectedAugmentedHessian = Matrix::Zero(3 * 12 + 1, 3 * 12 + 1);
expectedAugmentedHessian.block<36, 36>(0, 0) = H;
expectedAugmentedHessian.block<36, 1>(0, 36) = v;
expectedAugmentedHessian.block<1, 36>(36, 0) = v.transpose();
expectedAugmentedHessian(36, 36) = b.squaredNorm();

EXPECT(assert_equal(expectedAugmentedHessian, actualAugmentedHessian));
}

{ // SchurComplementAndRearrangeBlocks
KeyVector nonuniqueKeys;
nonuniqueKeys.push_back(0);
nonuniqueKeys.push_back(1);
nonuniqueKeys.push_back(1);
nonuniqueKeys.push_back(2);
nonuniqueKeys.push_back(2);
nonuniqueKeys.push_back(0);

KeyVector uniqueKeys;
uniqueKeys.push_back(0);
uniqueKeys.push_back(1);
uniqueKeys.push_back(2);

// Actual
SymmetricBlockMatrix augmentedHessianBM =
Set::SchurComplementAndRearrangeBlocks<3, 12, 6>(
Fs, E, P, b, nonuniqueKeys, uniqueKeys);
Matrix actualAugmentedHessian = augmentedHessianBM.selfadjointView();

// Expected
// we first need to build the Jacobian F according to unique keys
Matrix F = Matrix::Zero(6, 18);
F.block<2, 6>(0, 0) = Fs[0].block<2, 6>(0, 0);
F.block<2, 6>(0, 6) = Fs[0].block<2, 6>(0, 6);
F.block<2, 6>(2, 6) = Fs[1].block<2, 6>(0, 0);
F.block<2, 6>(2, 12) = Fs[1].block<2, 6>(0, 6);
F.block<2, 6>(4, 12) = Fs[2].block<2, 6>(0, 0);
F.block<2, 6>(4, 0) = Fs[2].block<2, 6>(0, 6);

Matrix Ft = F.transpose();
Vector v = Ft * (b - E * P * Et * b);
Matrix H = Ft * F - Ft * E * P * Et * F;
Matrix expectedAugmentedHessian(19, 19);
expectedAugmentedHessian << H, v, v.transpose(), b.squaredNorm();

EXPECT(assert_equal(expectedAugmentedHessian, actualAugmentedHessian));
}
}

/* ************************************************************************* */
#include <gtsam/geometry/StereoCamera.h>
TEST(CameraSet, Stereo) {
Expand Down
Loading

0 comments on commit d7f048b

Please sign in to comment.