12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Merge pull request #827 from borglab/feature/rollingShutterSmartFactors 

Feature/rolling shutter smart factors
release/4.3a0
Frank Dellaert 2021-08-28 18:42:31 -04:00 committed by GitHub
parent ff7ddf48bd 9798bfa815
commit d7f048b6c4
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
13 changed files with 2636 additions and 121 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

25
gtsam/base/Lie.h
View File

@ -17,6 +17,7 @@
* @author Frank Dellaert
* @author Mike Bosse
* @author Duy Nguyen Ta
* @author Yotam Stern
*/
@ -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))));
}
/**

170
gtsam/geometry/CameraSet.h
View File

@ -147,51 +147,149 @@ public:
* 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) {
*/
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();
// 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));
// 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
// 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;
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)
// 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));
// (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];
// 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) = (Dx2) * ( (2x2) * (2xD) )
augmentedHessian.setOffDiagonalBlock(i, j, -FiT
* (Ei_P * E.block(ZDim * j, 0, ZDim, N).transpose() * Fj));
augmentedHessian.diagonalBlock(m)(0, 0) += b.squaredNorm();
return augmentedHessian;
}
/**
* 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;
}
// 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
@ -206,7 +304,7 @@ public:
}
/// 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) {
@ -258,7 +356,7 @@ public:
* 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,

84
gtsam/geometry/tests/testCameraSet.cpp
View File

@ -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>
@ -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) {

62
gtsam/geometry/tests/testPose3.cpp
View File

@ -1046,6 +1046,68 @@ TEST(Pose3, interpolate) {
EXPECT(assert_equal(expected2, T2.interpolateRt(T3, t)));
}
/* ************************************************************************* */
Pose3 testing_interpolate(const Pose3& t1, const Pose3& t2, double gamma) { return interpolate(t1,t2,gamma); }
TEST(Pose3, interpolateJacobians) {
{
Pose3 X = Pose3::identity();
Pose3 Y(Rot3::Rz(M_PI_2), Point3(1, 0, 0));
double t = 0.5;
Pose3 expectedPoseInterp(Rot3::Rz(M_PI_4), Point3(0.5, -0.207107, 0)); // note: different from test above: this is full Pose3 interpolation
Matrix actualJacobianX, actualJacobianY;
EXPECT(assert_equal(expectedPoseInterp, interpolate(X, Y, t, actualJacobianX, actualJacobianY), 1e-5));
Matrix expectedJacobianX = numericalDerivative31<Pose3,Pose3,Pose3,double>(testing_interpolate, X, Y, t);
EXPECT(assert_equal(expectedJacobianX,actualJacobianX,1e-6));
Matrix expectedJacobianY = numericalDerivative32<Pose3,Pose3,Pose3,double>(testing_interpolate, X, Y, t);
EXPECT(assert_equal(expectedJacobianY,actualJacobianY,1e-6));
}
{
Pose3 X = Pose3::identity();
Pose3 Y(Rot3::identity(), Point3(1, 0, 0));
double t = 0.3;
Pose3 expectedPoseInterp(Rot3::identity(), Point3(0.3, 0, 0));
Matrix actualJacobianX, actualJacobianY;
EXPECT(assert_equal(expectedPoseInterp, interpolate(X, Y, t, actualJacobianX, actualJacobianY), 1e-5));
Matrix expectedJacobianX = numericalDerivative31<Pose3,Pose3,Pose3,double>(testing_interpolate, X, Y, t);
EXPECT(assert_equal(expectedJacobianX,actualJacobianX,1e-6));
Matrix expectedJacobianY = numericalDerivative32<Pose3,Pose3,Pose3,double>(testing_interpolate, X, Y, t);
EXPECT(assert_equal(expectedJacobianY,actualJacobianY,1e-6));
}
{
Pose3 X = Pose3::identity();
Pose3 Y(Rot3::Rz(M_PI_2), Point3(0, 0, 0));
double t = 0.5;
Pose3 expectedPoseInterp(Rot3::Rz(M_PI_4), Point3(0, 0, 0));
Matrix actualJacobianX, actualJacobianY;
EXPECT(assert_equal(expectedPoseInterp, interpolate(X, Y, t, actualJacobianX, actualJacobianY), 1e-5));
Matrix expectedJacobianX = numericalDerivative31<Pose3,Pose3,Pose3,double>(testing_interpolate, X, Y, t);
EXPECT(assert_equal(expectedJacobianX,actualJacobianX,1e-6));
Matrix expectedJacobianY = numericalDerivative32<Pose3,Pose3,Pose3,double>(testing_interpolate, X, Y, t);
EXPECT(assert_equal(expectedJacobianY,actualJacobianY,1e-6));
}
{
Pose3 X(Rot3::Ypr(0.1,0.2,0.3), Point3(10, 5, -2));
Pose3 Y(Rot3::Ypr(1.1,-2.2,-0.3), Point3(-5, 1, 1));
double t = 0.3;
Pose3 expectedPoseInterp(Rot3::Rz(M_PI_4), Point3(0, 0, 0));
Matrix actualJacobianX, actualJacobianY;
interpolate(X, Y, t, actualJacobianX, actualJacobianY);
Matrix expectedJacobianX = numericalDerivative31<Pose3,Pose3,Pose3,double>(testing_interpolate, X, Y, t);
EXPECT(assert_equal(expectedJacobianX,actualJacobianX,1e-6));
Matrix expectedJacobianY = numericalDerivative32<Pose3,Pose3,Pose3,double>(testing_interpolate, X, Y, t);
EXPECT(assert_equal(expectedJacobianY,actualJacobianY,1e-6));
}
}
/* ************************************************************************* */
TEST(Pose3, Create) {
Matrix63 actualH1, actualH2;

2
gtsam/slam/SmartFactorBase.h
View File

@ -178,7 +178,7 @@ protected:
DefaultKeyFormatter) const override {
std::cout << s << "SmartFactorBase, z = \n";
for (size_t k = 0; k < measured_.size(); ++k) {
std::cout << "measurement, p = " << measured_[k] << "\t";
std::cout << "measurement " << k<<", px = \n" << measured_[k] << "\n";
noiseModel_->print("noise model = ");
}
if(body_P_sensor_)

2
gtsam/slam/SmartProjectionFactor.h
View File

@ -101,7 +101,7 @@ public:
void print(const std::string& s = "", const KeyFormatter& keyFormatter =
DefaultKeyFormatter) const override {
std::cout << s << "SmartProjectionFactor\n";
std::cout << "linearizationMode:\n" << params_.linearizationMode
std::cout << "linearizationMode: " << params_.linearizationMode
<< std::endl;
std::cout << "triangulationParameters:\n" << params_.triangulation
<< std::endl;

2
gtsam/slam/tests/testSmartProjectionPoseFactor.cpp
View File

@ -50,7 +50,7 @@ static Point2 measurement1(323.0, 240.0);
LevenbergMarquardtParams lmParams;
// Make more verbose like so (in tests):
// params.verbosityLM = LevenbergMarquardtParams::SUMMARY;
// lmParams.verbosityLM = LevenbergMarquardtParams::SUMMARY;
/* ************************************************************************* */
TEST( SmartProjectionPoseFactor, Constructor) {

64
gtsam_unstable/slam/ProjectionFactorRollingShutter.cpp Normal file
View File

@ -0,0 +1,64 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file ProjectionFactorRollingShutter.cpp
* @brief Basic projection factor for rolling shutter cameras
* @author Yotam Stern
*/
#include <gtsam_unstable/slam/ProjectionFactorRollingShutter.h>
namespace gtsam {
Vector ProjectionFactorRollingShutter::evaluateError(
const Pose3& pose_a, const Pose3& pose_b, const Point3& point,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2,
boost::optional<Matrix&> H3) const {
try {
Pose3 pose = interpolate<Pose3>(pose_a, pose_b, alpha_, H1, H2);
gtsam::Matrix Hprj;
if (body_P_sensor_) {
if (H1 || H2 || H3) {
gtsam::Matrix HbodySensor;
PinholeCamera<Cal3_S2> camera(
pose.compose(*body_P_sensor_, HbodySensor), *K_);
Point2 reprojectionError(camera.project(point, Hprj, H3, boost::none) -
measured_);
if (H1) *H1 = Hprj * HbodySensor * (*H1);
if (H2) *H2 = Hprj * HbodySensor * (*H2);
return reprojectionError;
} else {
PinholeCamera<Cal3_S2> camera(pose.compose(*body_P_sensor_), *K_);
return camera.project(point) - measured_;
}
} else {
PinholeCamera<Cal3_S2> camera(pose, *K_);
Point2 reprojectionError(camera.project(point, Hprj, H3, boost::none) -
measured_);
if (H1) *H1 = Hprj * (*H1);
if (H2) *H2 = Hprj * (*H2);
return reprojectionError;
}
} catch (CheiralityException& e) {
if (H1) *H1 = Matrix::Zero(2, 6);
if (H2) *H2 = Matrix::Zero(2, 6);
if (H3) *H3 = Matrix::Zero(2, 3);
if (verboseCheirality_)
std::cout << e.what() << ": Landmark "
<< DefaultKeyFormatter(this->key2()) << " moved behind camera "
<< DefaultKeyFormatter(this->key1()) << std::endl;
if (throwCheirality_) throw CheiralityException(this->key2());
}
return Vector2::Constant(2.0 * K_->fx());
}
} // namespace gtsam

217
gtsam_unstable/slam/ProjectionFactorRollingShutter.h Normal file
View File

@ -0,0 +1,217 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file ProjectionFactorRollingShutter.h
* @brief Basic projection factor for rolling shutter cameras
* @author Yotam Stern
*/
#pragma once
#include <gtsam/geometry/Cal3_S2.h>
#include <gtsam/geometry/CalibratedCamera.h>
#include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <boost/optional.hpp>
namespace gtsam {
/**
* Non-linear factor for 2D projection measurement obtained using a rolling
* shutter camera. The calibration is known here. This version takes rolling
* shutter information into account as follows: consider two consecutive poses A
* and B, and a Point2 measurement taken starting at time A using a rolling
* shutter camera. Pose A has timestamp t_A, and Pose B has timestamp t_B. The
* Point2 measurement has timestamp t_p (with t_A <= t_p <= t_B) corresponding
* to the time of exposure of the row of the image the pixel belongs to. Let us
* define the alpha = (t_p - t_A) / (t_B - t_A), we will use the pose
* interpolated between A and B by the alpha to project the corresponding
* landmark to Point2.
* @addtogroup SLAM
*/
class ProjectionFactorRollingShutter
: public NoiseModelFactor3<Pose3, Pose3, Point3> {
protected:
// Keep a copy of measurement and calibration for I/O
Point2 measured_; ///< 2D measurement
double alpha_; ///< interpolation parameter in [0,1] corresponding to the
///< point2 measurement
boost::shared_ptr<Cal3_S2> K_; ///< shared pointer to calibration object
boost::optional<Pose3>
body_P_sensor_; ///< The pose of the sensor in the body frame
// verbosity handling for Cheirality Exceptions
bool throwCheirality_; ///< If true, rethrows Cheirality exceptions (default:
///< false)
bool verboseCheirality_; ///< If true, prints text for Cheirality exceptions
///< (default: false)
public:
/// shorthand for base class type
typedef NoiseModelFactor3<Pose3, Pose3, Point3> Base;
/// shorthand for this class
typedef ProjectionFactorRollingShutter This;
/// shorthand for a smart pointer to a factor
typedef boost::shared_ptr<This> shared_ptr;
/// Default constructor
ProjectionFactorRollingShutter()
: measured_(0, 0),
alpha_(0),
throwCheirality_(false),
verboseCheirality_(false) {}
/**
* Constructor
* @param measured is the 2-dimensional pixel location of point in the image
* (the measurement)
* @param alpha in [0,1] is the rolling shutter parameter for the measurement
* @param model is the noise model
* @param poseKey_a is the key of the first camera
* @param poseKey_b is the key of the second camera
* @param pointKey is the key of the landmark
* @param K shared pointer to the constant calibration
* @param body_P_sensor is the transform from body to sensor frame (default
* identity)
*/
ProjectionFactorRollingShutter(
const Point2& measured, double alpha, const SharedNoiseModel& model,
Key poseKey_a, Key poseKey_b, Key pointKey,
const boost::shared_ptr<Cal3_S2>& K,
boost::optional<Pose3> body_P_sensor = boost::none)
: Base(model, poseKey_a, poseKey_b, pointKey),
measured_(measured),
alpha_(alpha),
K_(K),
body_P_sensor_(body_P_sensor),
throwCheirality_(false),
verboseCheirality_(false) {}
/**
* Constructor with exception-handling flags
* @param measured is the 2-dimensional pixel location of point in the image
* (the measurement)
* @param alpha in [0,1] is the rolling shutter parameter for the measurement
* @param model is the noise model
* @param poseKey_a is the key of the first camera
* @param poseKey_b is the key of the second camera
* @param pointKey is the key of the landmark
* @param K shared pointer to the constant calibration
* @param throwCheirality determines whether Cheirality exceptions are
* rethrown
* @param verboseCheirality determines whether exceptions are printed for
* Cheirality
* @param body_P_sensor is the transform from body to sensor frame (default
* identity)
*/
ProjectionFactorRollingShutter(
const Point2& measured, double alpha, const SharedNoiseModel& model,
Key poseKey_a, Key poseKey_b, Key pointKey,
const boost::shared_ptr<Cal3_S2>& K, bool throwCheirality,
bool verboseCheirality,
boost::optional<Pose3> body_P_sensor = boost::none)
: Base(model, poseKey_a, poseKey_b, pointKey),
measured_(measured),
alpha_(alpha),
K_(K),
body_P_sensor_(body_P_sensor),
throwCheirality_(throwCheirality),
verboseCheirality_(verboseCheirality) {}
/** Virtual destructor */
virtual ~ProjectionFactorRollingShutter() {}
/// @return a deep copy of this factor
gtsam::NonlinearFactor::shared_ptr clone() const override {
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this)));
}
/**
* print
* @param s optional string naming the factor
* @param keyFormatter optional formatter useful for printing Symbols
*/
void print(
const std::string& s = "",
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
std::cout << s << "ProjectionFactorRollingShutter, z = ";
traits<Point2>::Print(measured_);
std::cout << " rolling shutter interpolation param = " << alpha_;
if (this->body_P_sensor_)
this->body_P_sensor_->print(" sensor pose in body frame: ");
Base::print("", keyFormatter);
}
/// equals
bool equals(const NonlinearFactor& p, double tol = 1e-9) const override {
const This* e = dynamic_cast<const This*>(&p);
return e && Base::equals(p, tol) && (alpha_ == e->alpha()) &&
traits<Point2>::Equals(this->measured_, e->measured_, tol) &&
this->K_->equals(*e->K_, tol) &&
(this->throwCheirality_ == e->throwCheirality_) &&
(this->verboseCheirality_ == e->verboseCheirality_) &&
((!body_P_sensor_ && !e->body_P_sensor_) ||
(body_P_sensor_ && e->body_P_sensor_ &&
body_P_sensor_->equals(*e->body_P_sensor_)));
}
/// Evaluate error h(x)-z and optionally derivatives
Vector evaluateError(
const Pose3& pose_a, const Pose3& pose_b, const Point3& point,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none,
boost::optional<Matrix&> H3 = boost::none) const override;
/** return the measurement */
const Point2& measured() const { return measured_; }
/** return the calibration object */
inline const boost::shared_ptr<Cal3_S2> calibration() const { return K_; }
/** returns the rolling shutter interp param*/
inline double alpha() const { return alpha_; }
/** return verbosity */
inline bool verboseCheirality() const { return verboseCheirality_; }
/** return flag for throwing cheirality exceptions */
inline bool throwCheirality() const { return throwCheirality_; }
private:
/// Serialization function
friend class boost::serialization::access;
template <class ARCHIVE>
void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
ar& BOOST_SERIALIZATION_NVP(measured_);
ar& BOOST_SERIALIZATION_NVP(alpha_);
ar& BOOST_SERIALIZATION_NVP(K_);
ar& BOOST_SERIALIZATION_NVP(body_P_sensor_);
ar& BOOST_SERIALIZATION_NVP(throwCheirality_);
ar& BOOST_SERIALIZATION_NVP(verboseCheirality_);
}
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
/// traits
template <>
struct traits<ProjectionFactorRollingShutter>
: public Testable<ProjectionFactorRollingShutter> {};
} // namespace gtsam

485
gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h Normal file
View File

@ -0,0 +1,485 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file SmartProjectionPoseFactorRollingShutter.h
* @brief Smart projection factor on poses modeling rolling shutter effect with
* given readout time
* @author Luca Carlone
*/
#pragma once
#include <gtsam/geometry/CameraSet.h>
#include <gtsam/slam/SmartProjectionFactor.h>
namespace gtsam {
/**
*
* @addtogroup SLAM
*
* If you are using the factor, please cite:
* L. Carlone, Z. Kira, C. Beall, V. Indelman, F. Dellaert,
* Eliminating conditionally independent sets in factor graphs:
* a unifying perspective based on smart factors,
* Int. Conf. on Robotics and Automation (ICRA), 2014.
*/
/**
* This factor optimizes two consecutive poses of the body assuming a rolling
* shutter model of the camera with given readout time. The factor requires that
* values contain (for each pixel observation) two consecutive camera poses from
* which the pixel observation pose can be interpolated.
* @addtogroup SLAM
*/
template <class CAMERA>
class SmartProjectionPoseFactorRollingShutter
: public SmartProjectionFactor<CAMERA> {
public:
typedef typename CAMERA::CalibrationType CALIBRATION;
protected:
/// shared pointer to calibration object (one for each observation)
std::vector<boost::shared_ptr<CALIBRATION>> K_all_;
/// The keys of the pose of the body (with respect to an external world
/// frame): two consecutive poses for each observation
std::vector<std::pair<Key, Key>> world_P_body_key_pairs_;
/// interpolation factor (one for each observation) to interpolate between
/// pair of consecutive poses
std::vector<double> alphas_;
/// Pose of the camera in the body frame
std::vector<Pose3> body_P_sensors_;
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/// shorthand for base class type
typedef SmartProjectionFactor<PinholePose<CALIBRATION>> Base;
/// shorthand for this class
typedef SmartProjectionPoseFactorRollingShutter This;
/// shorthand for a smart pointer to a factor
typedef boost::shared_ptr<This> shared_ptr;
static const int DimBlock =
12; ///< size of the variable stacking 2 poses from which the observation
///< pose is interpolated
static const int DimPose = 6; ///< Pose3 dimension
static const int ZDim = 2; ///< Measurement dimension (Point2)
typedef Eigen::Matrix<double, ZDim, DimBlock>
MatrixZD; // F blocks (derivatives wrt block of 2 poses)
typedef std::vector<MatrixZD, Eigen::aligned_allocator<MatrixZD>>
FBlocks; // vector of F blocks
/**
* Constructor
* @param Isotropic measurement noise
* @param params internal parameters of the smart factors
*/
SmartProjectionPoseFactorRollingShutter(
const SharedNoiseModel& sharedNoiseModel,
const SmartProjectionParams& params = SmartProjectionParams())
: Base(sharedNoiseModel, params) {}
/** Virtual destructor */
~SmartProjectionPoseFactorRollingShutter() override = default;
/**
* add a new measurement, with 2 pose keys, interpolation factor, camera
* (intrinsic and extrinsic) calibration, and observed pixel.
* @param measured 2-dimensional location of the projection of a single
* landmark in a single view (the measurement), interpolated from the 2 poses
* @param world_P_body_key1 key corresponding to the first body poses (time <=
* time pixel is acquired)
* @param world_P_body_key2 key corresponding to the second body poses (time
* >= time pixel is acquired)
* @param alpha interpolation factor in [0,1], such that if alpha = 0 the
* interpolated pose is the same as world_P_body_key1
* @param K (fixed) camera intrinsic calibration
* @param body_P_sensor (fixed) camera extrinsic calibration
*/
void add(const Point2& measured, const Key& world_P_body_key1,
const Key& world_P_body_key2, const double& alpha,
const boost::shared_ptr<CALIBRATION>& K,
const Pose3& body_P_sensor = Pose3::identity()) {
// store measurements in base class
this->measured_.push_back(measured);
// store the pair of keys for each measurement, in the same order
world_P_body_key_pairs_.push_back(
std::make_pair(world_P_body_key1, world_P_body_key2));
// also store keys in the keys_ vector: these keys are assumed to be
// unique, so we avoid duplicates here
if (std::find(this->keys_.begin(), this->keys_.end(), world_P_body_key1) ==
this->keys_.end())
this->keys_.push_back(world_P_body_key1); // add only unique keys
if (std::find(this->keys_.begin(), this->keys_.end(), world_P_body_key2) ==
this->keys_.end())
this->keys_.push_back(world_P_body_key2); // add only unique keys
// store interpolation factor
alphas_.push_back(alpha);
// store fixed intrinsic calibration
K_all_.push_back(K);
// store fixed extrinsics of the camera
body_P_sensors_.push_back(body_P_sensor);
}
/**
* Variant of the previous "add" function in which we include multiple
* measurements
* @param measurements vector of the 2m dimensional location of the projection
* of a single landmark in the m views (the measurements)
* @param world_P_body_key_pairs vector where the i-th element contains a pair
* of keys corresponding to the pair of poses from which the observation pose
* for the i0-th measurement can be interpolated
* @param alphas vector of interpolation params (in [0,1]), one for each
* measurement (in the same order)
* @param Ks vector of (fixed) intrinsic calibration objects
* @param body_P_sensors vector of (fixed) extrinsic calibration objects
*/
void add(const Point2Vector& measurements,
const std::vector<std::pair<Key, Key>>& world_P_body_key_pairs,
const std::vector<double>& alphas,
const std::vector<boost::shared_ptr<CALIBRATION>>& Ks,
const std::vector<Pose3>& body_P_sensors) {
assert(world_P_body_key_pairs.size() == measurements.size());
assert(world_P_body_key_pairs.size() == alphas.size());
assert(world_P_body_key_pairs.size() == Ks.size());
for (size_t i = 0; i < measurements.size(); i++) {
add(measurements[i], world_P_body_key_pairs[i].first,
world_P_body_key_pairs[i].second, alphas[i], Ks[i],
body_P_sensors[i]);
}
}
/**
* Variant of the previous "add" function in which we include multiple
* measurements with the same (intrinsic and extrinsic) calibration
* @param measurements vector of the 2m dimensional location of the projection
* of a single landmark in the m views (the measurements)
* @param world_P_body_key_pairs vector where the i-th element contains a pair
* of keys corresponding to the pair of poses from which the observation pose
* for the i0-th measurement can be interpolated
* @param alphas vector of interpolation params (in [0,1]), one for each
* measurement (in the same order)
* @param K (fixed) camera intrinsic calibration (same for all measurements)
* @param body_P_sensor (fixed) camera extrinsic calibration (same for all
* measurements)
*/
void add(const Point2Vector& measurements,
const std::vector<std::pair<Key, Key>>& world_P_body_key_pairs,
const std::vector<double>& alphas,
const boost::shared_ptr<CALIBRATION>& K,
const Pose3& body_P_sensor = Pose3::identity()) {
assert(world_P_body_key_pairs.size() == measurements.size());
assert(world_P_body_key_pairs.size() == alphas.size());
for (size_t i = 0; i < measurements.size(); i++) {
add(measurements[i], world_P_body_key_pairs[i].first,
world_P_body_key_pairs[i].second, alphas[i], K, body_P_sensor);
}
}
/// return the calibration object
const std::vector<boost::shared_ptr<CALIBRATION>>& calibration() const {
return K_all_;
}
/// return (for each observation) the keys of the pair of poses from which we
/// interpolate
const std::vector<std::pair<Key, Key>>& world_P_body_key_pairs() const {
return world_P_body_key_pairs_;
}
/// return the interpolation factors alphas
const std::vector<double>& alphas() const { return alphas_; }
/// return the extrinsic camera calibration body_P_sensors
const std::vector<Pose3>& body_P_sensors() const { return body_P_sensors_; }
/**
* print
* @param s optional string naming the factor
* @param keyFormatter optional formatter useful for printing Symbols
*/
void print(
const std::string& s = "",
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
std::cout << s << "SmartProjectionPoseFactorRollingShutter: \n ";
for (size_t i = 0; i < K_all_.size(); i++) {
std::cout << "-- Measurement nr " << i << std::endl;
std::cout << " pose1 key: "
<< keyFormatter(world_P_body_key_pairs_[i].first) << std::endl;
std::cout << " pose2 key: "
<< keyFormatter(world_P_body_key_pairs_[i].second) << std::endl;
std::cout << " alpha: " << alphas_[i] << std::endl;
body_P_sensors_[i].print("extrinsic calibration:\n");
K_all_[i]->print("intrinsic calibration = ");
}
Base::print("", keyFormatter);
}
/// equals
bool equals(const NonlinearFactor& p, double tol = 1e-9) const override {
const SmartProjectionPoseFactorRollingShutter<CAMERA>* e =
dynamic_cast<const SmartProjectionPoseFactorRollingShutter<CAMERA>*>(
&p);
double keyPairsEqual = true;
if (this->world_P_body_key_pairs_.size() ==
e->world_P_body_key_pairs().size()) {
for (size_t k = 0; k < this->world_P_body_key_pairs_.size(); k++) {
const Key key1own = world_P_body_key_pairs_[k].first;
const Key key1e = e->world_P_body_key_pairs()[k].first;
const Key key2own = world_P_body_key_pairs_[k].second;
const Key key2e = e->world_P_body_key_pairs()[k].second;
if (!(key1own == key1e) || !(key2own == key2e)) {
keyPairsEqual = false;
break;
}
}
} else {
keyPairsEqual = false;
}
double extrinsicCalibrationEqual = true;
if (this->body_P_sensors_.size() == e->body_P_sensors().size()) {
for (size_t i = 0; i < this->body_P_sensors_.size(); i++) {
if (!body_P_sensors_[i].equals(e->body_P_sensors()[i])) {
extrinsicCalibrationEqual = false;
break;
}
}
} else {
extrinsicCalibrationEqual = false;
}
return e && Base::equals(p, tol) && K_all_ == e->calibration() &&
alphas_ == e->alphas() && keyPairsEqual && extrinsicCalibrationEqual;
}
/**
* Compute jacobian F, E and error vector at a given linearization point
* @param values Values structure which must contain camera poses
* corresponding to keys involved in this factor
* @return Return arguments are the camera jacobians Fs (including the
* jacobian with respect to both body poses we interpolate from), the point
* Jacobian E, and the error vector b. Note that the jacobians are computed
* for a given point.
*/
void computeJacobiansWithTriangulatedPoint(FBlocks& Fs, Matrix& E, Vector& b,
const Values& values) const {
if (!this->result_) {
throw("computeJacobiansWithTriangulatedPoint");
} else { // valid result: compute jacobians
size_t numViews = this->measured_.size();
E = Matrix::Zero(2 * numViews,
3); // a Point2 for each view (point jacobian)
b = Vector::Zero(2 * numViews); // a Point2 for each view
// intermediate Jacobians
Eigen::Matrix<double, ZDim, DimPose> dProject_dPoseCam;
Eigen::Matrix<double, DimPose, DimPose> dInterpPose_dPoseBody1,
dInterpPose_dPoseBody2, dPoseCam_dInterpPose;
Eigen::Matrix<double, ZDim, 3> Ei;
for (size_t i = 0; i < numViews; i++) { // for each camera/measurement
auto w_P_body1 = values.at<Pose3>(world_P_body_key_pairs_[i].first);
auto w_P_body2 = values.at<Pose3>(world_P_body_key_pairs_[i].second);
double interpolationFactor = alphas_[i];
// get interpolated pose:
auto w_P_body =
interpolate<Pose3>(w_P_body1, w_P_body2, interpolationFactor,
dInterpPose_dPoseBody1, dInterpPose_dPoseBody2);
auto body_P_cam = body_P_sensors_[i];
auto w_P_cam = w_P_body.compose(body_P_cam, dPoseCam_dInterpPose);
PinholeCamera<CALIBRATION> camera(w_P_cam, *K_all_[i]);
// get jacobians and error vector for current measurement
Point2 reprojectionError_i =
Point2(camera.project(*this->result_, dProject_dPoseCam, Ei) -
this->measured_.at(i));
Eigen::Matrix<double, ZDim, DimBlock> J; // 2 x 12
J.block(0, 0, ZDim, 6) =
dProject_dPoseCam * dPoseCam_dInterpPose *
dInterpPose_dPoseBody1; // (2x6) * (6x6) * (6x6)
J.block(0, 6, ZDim, 6) =
dProject_dPoseCam * dPoseCam_dInterpPose *
dInterpPose_dPoseBody2; // (2x6) * (6x6) * (6x6)
// fit into the output structures
Fs.push_back(J);
size_t row = 2 * i;
b.segment<ZDim>(row) = -reprojectionError_i;
E.block<ZDim, 3>(row, 0) = Ei;
}
}
}
/// linearize and return a Hessianfactor that is an approximation of error(p)
boost::shared_ptr<RegularHessianFactor<DimPose>> createHessianFactor(
const Values& values, const double lambda = 0.0,
bool diagonalDamping = false) const {
// we may have multiple observation sharing the same keys (due to the
// rolling shutter interpolation), hence the number of unique keys may be
// smaller than 2 * nrMeasurements
size_t nrUniqueKeys =
this->keys_
.size(); // note: by construction, keys_ only contains unique keys
// Create structures for Hessian Factors
KeyVector js;
std::vector<Matrix> Gs(nrUniqueKeys * (nrUniqueKeys + 1) / 2);
std::vector<Vector> gs(nrUniqueKeys);
if (this->measured_.size() !=
this->cameras(values).size()) // 1 observation per interpolated camera
throw std::runtime_error(
"SmartProjectionPoseFactorRollingShutter: "
"measured_.size() inconsistent with input");
// triangulate 3D point at given linearization point
this->triangulateSafe(this->cameras(values));
if (!this->result_) { // failed: return "empty/zero" Hessian
if (this->params_.degeneracyMode == ZERO_ON_DEGENERACY) {
for (Matrix& m : Gs) m = Matrix::Zero(DimPose, DimPose);
for (Vector& v : gs) v = Vector::Zero(DimPose);
return boost::make_shared<RegularHessianFactor<DimPose>>(this->keys_,
Gs, gs, 0.0);
} else {
throw std::runtime_error(
"SmartProjectionPoseFactorRollingShutter: "
"only supported degeneracy mode is ZERO_ON_DEGENERACY");
}
}
// compute Jacobian given triangulated 3D Point
FBlocks Fs;
Matrix E;
Vector b;
this->computeJacobiansWithTriangulatedPoint(Fs, E, b, values);
// Whiten using noise model
this->noiseModel_->WhitenSystem(E, b);
for (size_t i = 0; i < Fs.size(); i++)
Fs[i] = this->noiseModel_->Whiten(Fs[i]);
Matrix3 P = Base::Cameras::PointCov(E, lambda, diagonalDamping);
// Collect all the key pairs: these are the keys that correspond to the
// blocks in Fs (on which we apply the Schur Complement)
KeyVector nonuniqueKeys;
for (size_t i = 0; i < world_P_body_key_pairs_.size(); i++) {
nonuniqueKeys.push_back(world_P_body_key_pairs_.at(i).first);
nonuniqueKeys.push_back(world_P_body_key_pairs_.at(i).second);
}
// Build augmented Hessian (with last row/column being the information
// vector) Note: we need to get the augumented hessian wrt the unique keys
// in key_
SymmetricBlockMatrix augmentedHessianUniqueKeys =
Base::Cameras::template SchurComplementAndRearrangeBlocks<3, 12, 6>(
Fs, E, P, b, nonuniqueKeys, this->keys_);
return boost::make_shared<RegularHessianFactor<DimPose>>(
this->keys_, augmentedHessianUniqueKeys);
}
/**
* error calculates the error of the factor.
*/
double error(const Values& values) const override {
if (this->active(values)) {
return this->totalReprojectionError(this->cameras(values));
} else { // else of active flag
return 0.0;
}
}
/**
* Collect all cameras involved in this factor
* @param values Values structure which must contain camera poses
* corresponding to keys involved in this factor
* @return Cameras
*/
typename Base::Cameras cameras(const Values& values) const override {
size_t numViews = this->measured_.size();
assert(numViews == K_all_.size());
assert(numViews == alphas_.size());
assert(numViews == body_P_sensors_.size());
assert(numViews == world_P_body_key_pairs_.size());
typename Base::Cameras cameras;
for (size_t i = 0; i < numViews; i++) { // for each measurement
const Pose3& w_P_body1 =
values.at<Pose3>(world_P_body_key_pairs_[i].first);
const Pose3& w_P_body2 =
values.at<Pose3>(world_P_body_key_pairs_[i].second);
double interpolationFactor = alphas_[i];
const Pose3& w_P_body =
interpolate<Pose3>(w_P_body1, w_P_body2, interpolationFactor);
const Pose3& body_P_cam = body_P_sensors_[i];
const Pose3& w_P_cam = w_P_body.compose(body_P_cam);
cameras.emplace_back(w_P_cam, K_all_[i]);
}
return cameras;
}
/**
* Linearize to Gaussian Factor (possibly adding a damping factor Lambda for
* LM)
* @param values Values structure which must contain camera poses and
* extrinsic pose for this factor
* @return a Gaussian factor
*/
boost::shared_ptr<GaussianFactor> linearizeDamped(
const Values& values, const double lambda = 0.0) const {
// depending on flag set on construction we may linearize to different
// linear factors
switch (this->params_.linearizationMode) {
case HESSIAN:
return this->createHessianFactor(values, lambda);
default:
throw std::runtime_error(
"SmartProjectionPoseFactorRollingShutter: unknown linearization "
"mode");
}
}
/// linearize
boost::shared_ptr<GaussianFactor> linearize(
const Values& values) const override {
return this->linearizeDamped(values);
}
private:
/// Serialization function
friend class boost::serialization::access;
template <class ARCHIVE>
void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
ar& BOOST_SERIALIZATION_NVP(K_all_);
}
};
// end of class declaration
/// traits
template <class CAMERA>
struct traits<SmartProjectionPoseFactorRollingShutter<CAMERA>>
: public Testable<SmartProjectionPoseFactorRollingShutter<CAMERA>> {};
} // namespace gtsam

92
gtsam_unstable/slam/SmartStereoProjectionFactorPP.h
View File

@ -61,10 +61,10 @@ class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor {
/// shorthand for a smart pointer to a factor
typedef boost::shared_ptr<This> shared_ptr;
static const int Dim = 12; ///< Camera dimension: 6 for body pose, 6 for extrinsic pose
static const int DimBlock = 12; ///< Camera dimension: 6 for body pose, 6 for extrinsic pose
static const int DimPose = 6; ///< Pose3 dimension
static const int ZDim = 3; ///< Measurement dimension (for a StereoPoint2 measurement)
typedef Eigen::Matrix<double, ZDim, Dim> MatrixZD; // F blocks (derivatives wrt camera)
typedef Eigen::Matrix<double, ZDim, DimBlock> MatrixZD; // F blocks (derivatives wrt camera)
typedef std::vector<MatrixZD, Eigen::aligned_allocator<MatrixZD> > FBlocks; // vector of F blocks
/**
@ -180,7 +180,7 @@ class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor {
// get jacobians and error vector for current measurement
StereoPoint2 reprojectionError_i = StereoPoint2(
camera.project(*result_, dProject_dPoseCam_i, Ei) - measured_.at(i));
Eigen::Matrix<double, ZDim, Dim> J; // 3 x 12
Eigen::Matrix<double, ZDim, DimBlock> J; // 3 x 12
J.block<ZDim, 6>(0, 0) = dProject_dPoseCam_i * dPoseCam_dPoseBody_i; // (3x6) * (6x6)
J.block<ZDim, 6>(0, 6) = dProject_dPoseCam_i * dPoseCam_dPoseExt_i; // (3x6) * (6x6)
// if the right pixel is invalid, fix jacobians
@ -209,8 +209,6 @@ class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor {
// of keys may be smaller than 2 * nrMeasurements (which is the upper bound where we
// have a body key and an extrinsic calibration key for each measurement)
size_t nrUniqueKeys = keys_.size();
size_t nrNonuniqueKeys = world_P_body_keys_.size()
+ body_P_cam_keys_.size();
// Create structures for Hessian Factors
KeyVector js;
@ -246,81 +244,19 @@ class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor {
// build augmented Hessian (with last row/column being the information vector)
Matrix3 P;
Cameras::ComputePointCovariance<3>(P, E, lambda, diagonalDamping);
Cameras::ComputePointCovariance <3> (P, E, lambda, diagonalDamping);
// marginalize point: note - we reuse the standard SchurComplement function
SymmetricBlockMatrix augmentedHessian =
Cameras::SchurComplement<3, Dim>(Fs, E, P, b);
// now pack into an Hessian factor
std::vector<DenseIndex> dims(nrUniqueKeys + 1); // this also includes the b term
std::fill(dims.begin(), dims.end() - 1, 6);
dims.back() = 1;
SymmetricBlockMatrix augmentedHessianUniqueKeys;
// here we have to deal with the fact that some cameras may share the same extrinsic key
if (nrUniqueKeys == nrNonuniqueKeys) { // if there is 1 calibration key per camera
augmentedHessianUniqueKeys = SymmetricBlockMatrix(
dims, Matrix(augmentedHessian.selfadjointView()));
} else { // if multiple cameras share a calibration we have to rearrange
// the results of the Schur complement matrix
std::vector<DenseIndex> nonuniqueDims(nrNonuniqueKeys + 1); // this also includes the b term
std::fill(nonuniqueDims.begin(), nonuniqueDims.end() - 1, 6);
nonuniqueDims.back() = 1;
augmentedHessian = SymmetricBlockMatrix(
nonuniqueDims, Matrix(augmentedHessian.selfadjointView()));
// these are the keys that correspond to the blocks in augmentedHessian (output of SchurComplement)
KeyVector nonuniqueKeys;
for (size_t i = 0; i < world_P_body_keys_.size(); i++) {
nonuniqueKeys.push_back(world_P_body_keys_.at(i));
nonuniqueKeys.push_back(body_P_cam_keys_.at(i));
}
// 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[keys_[k]] = k;
}
// initialize matrix to zero
augmentedHessianUniqueKeys = SymmetricBlockMatrix(
dims, Matrix::Zero(6 * nrUniqueKeys + 1, 6 * 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 = nonuniqueKeys.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 = nonuniqueKeys.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());
}
}
}
}
// update bottom right element of the matrix
augmentedHessianUniqueKeys.updateDiagonalBlock(
nrUniqueKeys, augmentedHessian.diagonalBlock(nrNonuniqueKeys));
// these are the keys that correspond to the blocks in augmentedHessian (output of SchurComplement)
KeyVector nonuniqueKeys;
for (size_t i = 0; i < world_P_body_keys_.size(); i++) {
nonuniqueKeys.push_back(world_P_body_keys_.at(i));
nonuniqueKeys.push_back(body_P_cam_keys_.at(i));
}
// but we need to get the augumented hessian wrt the unique keys in key_
SymmetricBlockMatrix augmentedHessianUniqueKeys =
Cameras::SchurComplementAndRearrangeBlocks<3,DimBlock,DimPose>(Fs,E,P,b,
nonuniqueKeys, keys_);
return boost::make_shared < RegularHessianFactor<DimPose>
> (keys_, augmentedHessianUniqueKeys);
}

407
gtsam_unstable/slam/tests/testProjectionFactorRollingShutter.cpp Normal file
View File

@ -0,0 +1,407 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file ProjectionFactorRollingShutterRollingShutter.cpp
* @brief Unit tests for ProjectionFactorRollingShutter Class
* @author Luca Carlone
* @date July 2021
*/
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/TestableAssertions.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/geometry/Cal3DS2.h>
#include <gtsam/geometry/Cal3_S2.h>
#include <gtsam/geometry/Point2.h>
#include <gtsam/geometry/Point3.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam_unstable/slam/ProjectionFactorRollingShutter.h>
using namespace std::placeholders;
using namespace std;
using namespace gtsam;
// make a realistic calibration matrix
static double fov = 60; // degrees
static size_t w = 640, h = 480;
static Cal3_S2::shared_ptr K(new Cal3_S2(fov, w, h));
// Create a noise model for the pixel error
static SharedNoiseModel model(noiseModel::Unit::Create(2));
// Convenience for named keys
using symbol_shorthand::L;
using symbol_shorthand::T;
using symbol_shorthand::X;
// Convenience to define common variables across many tests
static Key poseKey1(X(1));
static Key poseKey2(X(2));
static Key pointKey(L(1));
static double interp_params = 0.5;
static Point2 measurement(323.0, 240.0);
static Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2),
Point3(0.25, -0.10, 1.0));
/* ************************************************************************* */
TEST(ProjectionFactorRollingShutter, Constructor) {
ProjectionFactorRollingShutter factor(measurement, interp_params, model,
poseKey1, poseKey2, pointKey, K);
}
/* ************************************************************************* */
TEST(ProjectionFactorRollingShutter, ConstructorWithTransform) {
ProjectionFactorRollingShutter factor(measurement, interp_params, model,
poseKey1, poseKey2, pointKey, K,
body_P_sensor);
}
/* ************************************************************************* */
TEST(ProjectionFactorRollingShutter, Equals) {
{ // factors are equal
ProjectionFactorRollingShutter factor1(measurement, interp_params, model,
poseKey1, poseKey2, pointKey, K);
ProjectionFactorRollingShutter factor2(measurement, interp_params, model,
poseKey1, poseKey2, pointKey, K);
CHECK(assert_equal(factor1, factor2));
}
{ // factors are NOT equal (keys are different)
ProjectionFactorRollingShutter factor1(measurement, interp_params, model,
poseKey1, poseKey2, pointKey, K);
ProjectionFactorRollingShutter factor2(measurement, interp_params, model,
poseKey1, poseKey1, pointKey, K);
CHECK(!assert_equal(factor1, factor2)); // not equal
}
{ // factors are NOT equal (different interpolation)
ProjectionFactorRollingShutter factor1(measurement, 0.1, model, poseKey1,
poseKey1, pointKey, K);
ProjectionFactorRollingShutter factor2(measurement, 0.5, model, poseKey1,
poseKey2, pointKey, K);
CHECK(!assert_equal(factor1, factor2)); // not equal
}
}
/* ************************************************************************* */
TEST(ProjectionFactorRollingShutter, EqualsWithTransform) {
{ // factors are equal
ProjectionFactorRollingShutter factor1(measurement, interp_params, model,
poseKey1, poseKey2, pointKey, K,
body_P_sensor);
ProjectionFactorRollingShutter factor2(measurement, interp_params, model,
poseKey1, poseKey2, pointKey, K,
body_P_sensor);
CHECK(assert_equal(factor1, factor2));
}
{ // factors are NOT equal
ProjectionFactorRollingShutter factor1(measurement, interp_params, model,
poseKey1, poseKey2, pointKey, K,
body_P_sensor);
Pose3 body_P_sensor2(
Rot3::RzRyRx(0.0, 0.0, 0.0),
Point3(0.25, -0.10, 1.0)); // rotation different from body_P_sensor
ProjectionFactorRollingShutter factor2(measurement, interp_params, model,
poseKey1, poseKey2, pointKey, K,
body_P_sensor2);
CHECK(!assert_equal(factor1, factor2));
}
}
/* ************************************************************************* */
TEST(ProjectionFactorRollingShutter, Error) {
{
// Create the factor with a measurement that is 3 pixels off in x
// Camera pose corresponds to the first camera
double t = 0.0;
ProjectionFactorRollingShutter factor(measurement, t, model, poseKey1,
poseKey2, pointKey, K);
// Set the linearization point
Pose3 pose1(Rot3(), Point3(0, 0, -6));
Pose3 pose2(Rot3(), Point3(0, 0, -4));
Point3 point(0.0, 0.0, 0.0);
// Use the factor to calculate the error
Vector actualError(factor.evaluateError(pose1, pose2, point));
// The expected error is (-3.0, 0.0) pixels / UnitCovariance
Vector expectedError = Vector2(-3.0, 0.0);
// Verify we get the expected error
CHECK(assert_equal(expectedError, actualError, 1e-9));
}
{
// Create the factor with a measurement that is 3 pixels off in x
// Camera pose is actually interpolated now
double t = 0.5;
ProjectionFactorRollingShutter factor(measurement, t, model, poseKey1,
poseKey2, pointKey, K);
// Set the linearization point
Pose3 pose1(Rot3(), Point3(0, 0, -8));
Pose3 pose2(Rot3(), Point3(0, 0, -4));
Point3 point(0.0, 0.0, 0.0);
// Use the factor to calculate the error
Vector actualError(factor.evaluateError(pose1, pose2, point));
// The expected error is (-3.0, 0.0) pixels / UnitCovariance
Vector expectedError = Vector2(-3.0, 0.0);
// Verify we get the expected error
CHECK(assert_equal(expectedError, actualError, 1e-9));
}
{
// Create measurement by projecting 3D landmark
double t = 0.3;
Pose3 pose1(Rot3::RzRyRx(0.1, 0.0, 0.1), Point3(0, 0, 0));
Pose3 pose2(Rot3::RzRyRx(-0.1, -0.1, 0.0), Point3(0, 0, 1));
Pose3 poseInterp = interpolate<Pose3>(pose1, pose2, t);
PinholeCamera<Cal3_S2> camera(poseInterp, *K);
Point3 point(0.0, 0.0, 5.0); // 5 meters in front of the camera
Point2 measured = camera.project(point);
// create factor
ProjectionFactorRollingShutter factor(measured, t, model, poseKey1,
poseKey2, pointKey, K);
// Use the factor to calculate the error
Vector actualError(factor.evaluateError(pose1, pose2, point));
// The expected error is zero
Vector expectedError = Vector2(0.0, 0.0);
// Verify we get the expected error
CHECK(assert_equal(expectedError, actualError, 1e-9));
}
}
/* ************************************************************************* */
TEST(ProjectionFactorRollingShutter, ErrorWithTransform) {
// Create measurement by projecting 3D landmark
double t = 0.3;
Pose3 pose1(Rot3::RzRyRx(0.1, 0.0, 0.1), Point3(0, 0, 0));
Pose3 pose2(Rot3::RzRyRx(-0.1, -0.1, 0.0), Point3(0, 0, 1));
Pose3 poseInterp = interpolate<Pose3>(pose1, pose2, t);
Pose3 body_P_sensor3(Rot3::RzRyRx(-0.1, -0.1, 0.0), Point3(0, 0.2, 0.1));
PinholeCamera<Cal3_S2> camera(poseInterp * body_P_sensor3, *K);
Point3 point(0.0, 0.0, 5.0); // 5 meters in front of the camera
Point2 measured = camera.project(point);
// create factor
ProjectionFactorRollingShutter factor(measured, t, model, poseKey1, poseKey2,
pointKey, K, body_P_sensor3);
// Use the factor to calculate the error
Vector actualError(factor.evaluateError(pose1, pose2, point));
// The expected error is zero
Vector expectedError = Vector2(0.0, 0.0);
// Verify we get the expected error
CHECK(assert_equal(expectedError, actualError, 1e-9));
}
/* ************************************************************************* */
TEST(ProjectionFactorRollingShutter, Jacobian) {
// Create measurement by projecting 3D landmark
double t = 0.3;
Pose3 pose1(Rot3::RzRyRx(0.1, 0.0, 0.1), Point3(0, 0, 0));
Pose3 pose2(Rot3::RzRyRx(-0.1, -0.1, 0.0), Point3(0, 0, 1));
Pose3 poseInterp = interpolate<Pose3>(pose1, pose2, t);
PinholeCamera<Cal3_S2> camera(poseInterp, *K);
Point3 point(0.0, 0.0, 5.0); // 5 meters in front of the camera
Point2 measured = camera.project(point);
// create factor
ProjectionFactorRollingShutter factor(measured, t, model, poseKey1, poseKey2,
pointKey, K);
// Use the factor to calculate the Jacobians
Matrix H1Actual, H2Actual, H3Actual;
factor.evaluateError(pose1, pose2, point, H1Actual, H2Actual, H3Actual);
// Expected Jacobians via numerical derivatives
Matrix H1Expected = numericalDerivative31<Vector, Pose3, Pose3, Point3>(
std::function<Vector(const Pose3&, const Pose3&, const Point3&)>(
std::bind(&ProjectionFactorRollingShutter::evaluateError, &factor,
std::placeholders::_1, std::placeholders::_2,
std::placeholders::_3, boost::none, boost::none,
boost::none)),
pose1, pose2, point);
Matrix H2Expected = numericalDerivative32<Vector, Pose3, Pose3, Point3>(
std::function<Vector(const Pose3&, const Pose3&, const Point3&)>(
std::bind(&ProjectionFactorRollingShutter::evaluateError, &factor,
std::placeholders::_1, std::placeholders::_2,
std::placeholders::_3, boost::none, boost::none,
boost::none)),
pose1, pose2, point);
Matrix H3Expected = numericalDerivative33<Vector, Pose3, Pose3, Point3>(
std::function<Vector(const Pose3&, const Pose3&, const Point3&)>(
std::bind(&ProjectionFactorRollingShutter::evaluateError, &factor,
std::placeholders::_1, std::placeholders::_2,
std::placeholders::_3, boost::none, boost::none,
boost::none)),
pose1, pose2, point);
CHECK(assert_equal(H1Expected, H1Actual, 1e-5));
CHECK(assert_equal(H2Expected, H2Actual, 1e-5));
CHECK(assert_equal(H3Expected, H3Actual, 1e-5));
}
/* ************************************************************************* */
TEST(ProjectionFactorRollingShutter, JacobianWithTransform) {
// Create measurement by projecting 3D landmark
double t = 0.6;
Pose3 pose1(Rot3::RzRyRx(0.1, 0.0, 0.1), Point3(0, 0, 0));
Pose3 pose2(Rot3::RzRyRx(-0.1, -0.1, 0.0), Point3(0, 0, 1));
Pose3 poseInterp = interpolate<Pose3>(pose1, pose2, t);
Pose3 body_P_sensor3(Rot3::RzRyRx(-0.1, -0.1, 0.0), Point3(0, 0.2, 0.1));
PinholeCamera<Cal3_S2> camera(poseInterp * body_P_sensor3, *K);
Point3 point(0.0, 0.0, 5.0); // 5 meters in front of the camera
Point2 measured = camera.project(point);
// create factor
ProjectionFactorRollingShutter factor(measured, t, model, poseKey1, poseKey2,
pointKey, K, body_P_sensor3);
// Use the factor to calculate the Jacobians
Matrix H1Actual, H2Actual, H3Actual;
factor.evaluateError(pose1, pose2, point, H1Actual, H2Actual, H3Actual);
// Expected Jacobians via numerical derivatives
Matrix H1Expected = numericalDerivative31<Vector, Pose3, Pose3, Point3>(
std::function<Vector(const Pose3&, const Pose3&, const Point3&)>(
std::bind(&ProjectionFactorRollingShutter::evaluateError, &factor,
std::placeholders::_1, std::placeholders::_2,
std::placeholders::_3, boost::none, boost::none,
boost::none)),
pose1, pose2, point);
Matrix H2Expected = numericalDerivative32<Vector, Pose3, Pose3, Point3>(
std::function<Vector(const Pose3&, const Pose3&, const Point3&)>(
std::bind(&ProjectionFactorRollingShutter::evaluateError, &factor,
std::placeholders::_1, std::placeholders::_2,
std::placeholders::_3, boost::none, boost::none,
boost::none)),
pose1, pose2, point);
Matrix H3Expected = numericalDerivative33<Vector, Pose3, Pose3, Point3>(
std::function<Vector(const Pose3&, const Pose3&, const Point3&)>(
std::bind(&ProjectionFactorRollingShutter::evaluateError, &factor,
std::placeholders::_1, std::placeholders::_2,
std::placeholders::_3, boost::none, boost::none,
boost::none)),
pose1, pose2, point);
CHECK(assert_equal(H1Expected, H1Actual, 1e-5));
CHECK(assert_equal(H2Expected, H2Actual, 1e-5));
CHECK(assert_equal(H3Expected, H3Actual, 1e-5));
}
/* ************************************************************************* */
TEST(ProjectionFactorRollingShutter, cheirality) {
// Create measurement by projecting 3D landmark behind camera
double t = 0.3;
Pose3 pose1(Rot3::RzRyRx(0.1, 0.0, 0.1), Point3(0, 0, 0));
Pose3 pose2(Rot3::RzRyRx(-0.1, -0.1, 0.0), Point3(0, 0, 1));
Pose3 poseInterp = interpolate<Pose3>(pose1, pose2, t);
PinholeCamera<Cal3_S2> camera(poseInterp, *K);
Point3 point(0.0, 0.0, -5.0); // 5 meters behind the camera
#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
Point2 measured = Point2(0.0, 0.0); // project would throw an exception
{ // check that exception is thrown if we set throwCheirality = true
bool throwCheirality = true;
bool verboseCheirality = true;
ProjectionFactorRollingShutter factor(measured, t, model, poseKey1,
poseKey2, pointKey, K,
throwCheirality, verboseCheirality);
CHECK_EXCEPTION(factor.evaluateError(pose1, pose2, point),
CheiralityException);
}
{ // check that exception is NOT thrown if we set throwCheirality = false,
// and outputs are correct
bool throwCheirality = false; // default
bool verboseCheirality = false; // default
ProjectionFactorRollingShutter factor(measured, t, model, poseKey1,
poseKey2, pointKey, K,
throwCheirality, verboseCheirality);
// Use the factor to calculate the error
Matrix H1Actual, H2Actual, H3Actual;
Vector actualError(factor.evaluateError(pose1, pose2, point, H1Actual,
H2Actual, H3Actual));
// The expected error is zero
Vector expectedError = Vector2::Constant(
2.0 * K->fx()); // this is what we return when point is behind camera
// Verify we get the expected error
CHECK(assert_equal(expectedError, actualError, 1e-9));
CHECK(assert_equal(Matrix::Zero(2, 6), H1Actual, 1e-5));
CHECK(assert_equal(Matrix::Zero(2, 6), H2Actual, 1e-5));
CHECK(assert_equal(Matrix::Zero(2, 3), H3Actual, 1e-5));
}
#else
{
// everything is well defined, hence this matches the test "Jacobian" above:
Point2 measured = camera.project(point);
// create factor
ProjectionFactorRollingShutter factor(measured, t, model, poseKey1,
poseKey2, pointKey, K);
// Use the factor to calculate the Jacobians
Matrix H1Actual, H2Actual, H3Actual;
factor.evaluateError(pose1, pose2, point, H1Actual, H2Actual, H3Actual);
// Expected Jacobians via numerical derivatives
Matrix H1Expected = numericalDerivative31<Vector, Pose3, Pose3, Point3>(
std::function<Vector(const Pose3&, const Pose3&, const Point3&)>(
std::bind(&ProjectionFactorRollingShutter::evaluateError, &factor,
std::placeholders::_1, std::placeholders::_2,
std::placeholders::_3, boost::none, boost::none,
boost::none)),
pose1, pose2, point);
Matrix H2Expected = numericalDerivative32<Vector, Pose3, Pose3, Point3>(
std::function<Vector(const Pose3&, const Pose3&, const Point3&)>(
std::bind(&ProjectionFactorRollingShutter::evaluateError, &factor,
std::placeholders::_1, std::placeholders::_2,
std::placeholders::_3, boost::none, boost::none,
boost::none)),
pose1, pose2, point);
Matrix H3Expected = numericalDerivative33<Vector, Pose3, Pose3, Point3>(
std::function<Vector(const Pose3&, const Pose3&, const Point3&)>(
std::bind(&ProjectionFactorRollingShutter::evaluateError, &factor,
std::placeholders::_1, std::placeholders::_2,
std::placeholders::_3, boost::none, boost::none,
boost::none)),
pose1, pose2, point);
CHECK(assert_equal(H1Expected, H1Actual, 1e-5));
CHECK(assert_equal(H2Expected, H2Actual, 1e-5));
CHECK(assert_equal(H3Expected, H3Actual, 1e-5));
}
#endif
}
/* ************************************************************************* */
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */

1145
gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp Normal file
View File

File diff suppressed because it is too large Load Diff