diff --git a/gtsam/base/Lie.h b/gtsam/base/Lie.h index fe730c934..34422edd7 100644 --- a/gtsam/base/Lie.h +++ b/gtsam/base/Lie.h @@ -17,6 +17,7 @@ * @author Frank Dellaert * @author Mike Bosse * @author Duy Nguyen Ta + * @author Yotam Stern */ @@ -319,11 +320,27 @@ 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), with optional jacobians. */ template -T interpolate(const T& X, const T& Y, double t) { - assert(t >= 0 && t <= 1); +T interpolate(const T& X, const T& Y, double t, + typename MakeOptionalJacobian::type Hx = boost::none, + typename MakeOptionalJacobian::type Hy = boost::none) { + + if (Hx || Hy) { + typename traits::TangentVector log_Xinv_Y; + typename MakeJacobian::type between_H_x, log_H, exp_H, compose_H_x; + + T Xinv_Y = traits::Between(X, Y, between_H_x); // between_H_y = identity + log_Xinv_Y = traits::Logmap(Xinv_Y, log_H); + Xinv_Y = traits::Expmap(t * log_Xinv_Y, exp_H); + Xinv_Y = traits::Compose(X, Xinv_Y, 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 Xinv_Y; + } return traits::Compose(X, traits::Expmap(t * traits::Logmap(traits::Between(X, Y)))); } diff --git a/gtsam/geometry/CameraSet.h b/gtsam/geometry/CameraSet.h index 7d2f818fa..3fc77bb2e 100644 --- a/gtsam/geometry/CameraSet.h +++ b/gtsam/geometry/CameraSet.h @@ -148,7 +148,7 @@ public: * g = F' * (b - E * P * E' * b) * Fixed size version */ - template // N = 2 or 3, ND is the camera dimension + template // N = 2 or 3 (point dimension), ND is the camera dimension static SymmetricBlockMatrix SchurComplement( const std::vector< Eigen::Matrix, Eigen::aligned_allocator< Eigen::Matrix > >& Fs, const Matrix& E, const Eigen::Matrix& P, const Vector& b) { @@ -193,6 +193,106 @@ public: 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. + */ + template // N = 2 or 3 (point dimension), ND is the Jacobian block dimension, NDD is the Hessian block dimension + static SymmetricBlockMatrix SchurComplementAndRearrangeBlocks( + const std::vector, + Eigen::aligned_allocator > >& Fs, + const Matrix& E, const Eigen::Matrix& 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(Fs,E,P,b); + + // now pack into an Hessian factor + std::vector dims(nrUniqueKeys + 1); // this also includes the b term + std::fill(dims.begin(), dims.end() - 1, NDD); + dims.back() = 1; + SymmetricBlockMatrix augmentedHessianUniqueKeys; + + // here we have to deal with the fact that some blocks may share the same keys + 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 nonuniqueDims(nrNonuniqueKeys + 1); // this also includes the b term + 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 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()); + } + } + } + } + // update bottom right element of the matrix + augmentedHessianUniqueKeys.updateDiagonalBlock( + nrUniqueKeys, augmentedHessian.diagonalBlock(nrNonuniqueKeys)); + } + return augmentedHessianUniqueKeys; + } + + /** + * non-templated version of function above + */ + static SymmetricBlockMatrix SchurComplementAndRearrangeBlocks_3_12_6( + const std::vector, + Eigen::aligned_allocator > >& Fs, + const Matrix& E, const Eigen::Matrix& P, const Vector& b, + const KeyVector jacobianKeys, const KeyVector hessianKeys) { + return SchurComplementAndRearrangeBlocks<3,12,6>(Fs, E, P, b, + jacobianKeys, + hessianKeys); + } + /** * Do Schur complement, given Jacobian as Fs,E,P, return SymmetricBlockMatrix * G = F' * F - F' * E * P * E' * F @@ -206,7 +306,7 @@ public: } /// Computes Point Covariance P, with lambda parameter - template // N = 2 or 3 + template // N = 2 or 3 (point dimension) static void ComputePointCovariance(Eigen::Matrix& P, const Matrix& E, double lambda, bool diagonalDamping = false) { @@ -258,7 +358,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 // N = 2 or 3 + template // N = 2 or 3 (point dimension) static void UpdateSchurComplement(const FBlocks& Fs, const Matrix& E, const Eigen::Matrix& P, const Vector& b, const KeyVector& allKeys, const KeyVector& keys, diff --git a/gtsam/geometry/tests/testCameraSet.cpp b/gtsam/geometry/tests/testCameraSet.cpp index 761ef3a8c..e583c0e80 100644 --- a/gtsam/geometry/tests/testCameraSet.cpp +++ b/gtsam/geometry/tests/testCameraSet.cpp @@ -17,6 +17,7 @@ */ #include +#include #include #include #include @@ -125,6 +126,90 @@ TEST(CameraSet, Pinhole) { EXPECT(assert_equal(actualE, E)); } +/* ************************************************************************* */ +TEST(CameraSet, SchurComplementAndRearrangeBlocks) { + typedef PinholePose Camera; + typedef CameraSet Set; + + // this is the (block) Jacobian with respect to the nonuniqueKeys + std::vector, + Eigen::aligned_allocator > > 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 TEST(CameraSet, Stereo) { diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index 11b8791d4..7c1fa81e6 100644 --- a/gtsam/geometry/tests/testPose3.cpp +++ b/gtsam/geometry/tests/testPose3.cpp @@ -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(testing_interpolate, X, Y, t); + EXPECT(assert_equal(expectedJacobianX,actualJacobianX,1e-6)); + + Matrix expectedJacobianY = numericalDerivative32(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(testing_interpolate, X, Y, t); + EXPECT(assert_equal(expectedJacobianX,actualJacobianX,1e-6)); + + Matrix expectedJacobianY = numericalDerivative32(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(testing_interpolate, X, Y, t); + EXPECT(assert_equal(expectedJacobianX,actualJacobianX,1e-6)); + + Matrix expectedJacobianY = numericalDerivative32(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(testing_interpolate, X, Y, t); + EXPECT(assert_equal(expectedJacobianX,actualJacobianX,1e-6)); + + Matrix expectedJacobianY = numericalDerivative32(testing_interpolate, X, Y, t); + EXPECT(assert_equal(expectedJacobianY,actualJacobianY,1e-6)); + } +} + /* ************************************************************************* */ TEST(Pose3, Create) { Matrix63 actualH1, actualH2; diff --git a/gtsam/slam/SmartFactorBase.h b/gtsam/slam/SmartFactorBase.h index 6d493b533..e7584a4da 100644 --- a/gtsam/slam/SmartFactorBase.h +++ b/gtsam/slam/SmartFactorBase.h @@ -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_) diff --git a/gtsam/slam/SmartProjectionFactor.h b/gtsam/slam/SmartProjectionFactor.h index 475a9e829..f67ca0740 100644 --- a/gtsam/slam/SmartProjectionFactor.h +++ b/gtsam/slam/SmartProjectionFactor.h @@ -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; diff --git a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp index c7f220c10..997c33846 100644 --- a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp @@ -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) { diff --git a/gtsam_unstable/slam/ProjectionFactorRollingShutter.cpp b/gtsam_unstable/slam/ProjectionFactorRollingShutter.cpp new file mode 100644 index 000000000..5fc1c05eb --- /dev/null +++ b/gtsam_unstable/slam/ProjectionFactorRollingShutter.cpp @@ -0,0 +1,73 @@ +/* ---------------------------------------------------------------------------- + + * 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 + +namespace gtsam { + +Vector ProjectionFactorRollingShutter::evaluateError( + const Pose3& pose_a, const Pose3& pose_b, const Point3& point, + boost::optional H1, boost::optional H2, + boost::optional H3) const { + + try { + Pose3 pose = interpolate(pose_a, pose_b, alpha_, H1, H2); + gtsam::Matrix Hprj; + if (body_P_sensor_) { + if (H1 || H2 || H3) { + gtsam::Matrix HbodySensor; + PinholeCamera 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 camera(pose.compose(*body_P_sensor_), *K_); + return camera.project(point) - measured_; + } + } else { + PinholeCamera 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 diff --git a/gtsam_unstable/slam/ProjectionFactorRollingShutter.h b/gtsam_unstable/slam/ProjectionFactorRollingShutter.h new file mode 100644 index 000000000..5827a538c --- /dev/null +++ b/gtsam_unstable/slam/ProjectionFactorRollingShutter.h @@ -0,0 +1,220 @@ +/* ---------------------------------------------------------------------------- + + * 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 +#include +#include +#include +#include + +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 { + 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 K_; ///< shared pointer to calibration object + boost::optional 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 Base; + + /// shorthand for this class + typedef ProjectionFactorRollingShutter This; + + /// shorthand for a smart pointer to a factor + typedef boost::shared_ptr 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& K, + boost::optional 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& K, + bool throwCheirality, bool verboseCheirality, + boost::optional 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 + virtual gtsam::NonlinearFactor::shared_ptr clone() const { + 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 { + std::cout << s << "ProjectionFactorRollingShutter, z = "; + traits::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 + virtual bool equals(const NonlinearFactor& p, double tol = 1e-9) const { + const This *e = dynamic_cast(&p); + return e && Base::equals(p, tol) && (alpha_ == e->alpha()) + && traits::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 H1 = + boost::none, + boost::optional H2 = boost::none, + boost::optional H3 = boost::none) const; + + /** return the measurement */ + const Point2& measured() const { + return measured_; + } + + /** return the calibration object */ + inline const boost::shared_ptr 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 + 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 : public Testable< + ProjectionFactorRollingShutter> { +}; + +} //namespace gtsam diff --git a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h new file mode 100644 index 000000000..dbe734a2c --- /dev/null +++ b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h @@ -0,0 +1,438 @@ +/* ---------------------------------------------------------------------------- + + * 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 + +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 SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor< +PinholePose > { + + protected: + /// shared pointer to calibration object (one for each observation) + std::vector > 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> world_P_body_key_pairs_; + + /// interpolation factor (one for each observation) to interpolate between pair of consecutive poses + std::vector alphas_; + + /// Pose of the camera in the body frame + std::vector body_P_sensors_; + + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + /// shorthand for base class type + typedef SmartProjectionFactor > Base; + + /// shorthand for this class + typedef SmartProjectionPoseFactorRollingShutter This; + + /// shorthand for a smart pointer to a factor + typedef boost::shared_ptr 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 MatrixZD; // F blocks (derivatives wrt block of 2 poses) + typedef std::vector > 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& 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>& world_P_body_key_pairs, + const std::vector& alphas, + const std::vector>& Ks, + const std::vector 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>& world_P_body_key_pairs, + const std::vector& alphas, + const boost::shared_ptr& 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 + inline std::vector> calibration() const { + return K_all_; + } + + /// return (for each observation) the keys of the pair of poses from which we interpolate + const std::vector> world_P_body_key_pairs() const { + return world_P_body_key_pairs_; + } + + /// return the interpolation factors alphas + const std::vector alphas() const { + return alphas_; + } + + /// return the extrinsic camera calibration body_P_sensors + const std::vector 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* e = + dynamic_cast*>(&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 dProject_dPoseCam; + Eigen::Matrix dInterpPose_dPoseBody1, + dInterpPose_dPoseBody2, dPoseCam_dInterpPose; + Eigen::Matrix Ei; + + for (size_t i = 0; i < numViews; i++) { // for each camera/measurement + const Pose3& w_P_body1 = values.at(world_P_body_key_pairs_[i].first); + const Pose3& w_P_body2 = values.at(world_P_body_key_pairs_[i].second); + double interpolationFactor = alphas_[i]; + // get interpolated pose: + const Pose3& w_P_body = interpolate(w_P_body1, w_P_body2,interpolationFactor, dInterpPose_dPoseBody1, dInterpPose_dPoseBody2); + const Pose3& body_P_cam = body_P_sensors_[i]; + const Pose3& w_P_cam = w_P_body.compose(body_P_cam, dPoseCam_dInterpPose); + PinholeCamera 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 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(row) = -reprojectionError_i; + E.block(row, 0) = Ei; + } + } + } + + /// linearize and return a Hessianfactor that is an approximation of error(p) + boost::shared_ptr > 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 + > (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::SchurComplementAndRearrangeBlocks_3_12_6( + Fs, E, P, b, nonuniqueKeys, this->keys_); + + return boost::make_shared < RegularHessianFactor + > (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(world_P_body_key_pairs_[i].first); + const Pose3& w_P_body2 = values.at(world_P_body_key_pairs_[i].second); + double interpolationFactor = alphas_[i]; + const Pose3& w_P_body = interpolate(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 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 linearize(const Values& values) const + override { + return this->linearizeDamped(values); + } + + private: + /// Serialization function + friend class boost::serialization::access; + template + 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 +struct traits > : +public Testable > { +}; + +} // namespace gtsam diff --git a/gtsam_unstable/slam/SmartStereoProjectionFactorPP.h b/gtsam_unstable/slam/SmartStereoProjectionFactorPP.h index 40d90d614..25be48b0f 100644 --- a/gtsam_unstable/slam/SmartStereoProjectionFactorPP.h +++ b/gtsam_unstable/slam/SmartStereoProjectionFactorPP.h @@ -61,10 +61,10 @@ class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor { /// shorthand for a smart pointer to a factor typedef boost::shared_ptr 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 MatrixZD; // F blocks (derivatives wrt camera) + typedef Eigen::Matrix MatrixZD; // F blocks (derivatives wrt camera) typedef std::vector > 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 J; // 3 x 12 + Eigen::Matrix J; // 3 x 12 J.block(0, 0) = dProject_dPoseCam_i * dPoseCam_dPoseBody_i; // (3x6) * (6x6) J.block(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 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 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 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 > (keys_, augmentedHessianUniqueKeys); } diff --git a/gtsam_unstable/slam/tests/testProjectionFactorRollingShutter.cpp b/gtsam_unstable/slam/tests/testProjectionFactorRollingShutter.cpp new file mode 100644 index 000000000..943e350d4 --- /dev/null +++ b/gtsam_unstable/slam/tests/testProjectionFactorRollingShutter.cpp @@ -0,0 +1,375 @@ +/* ---------------------------------------------------------------------------- + + * 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 +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +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::X; +using symbol_shorthand::L; +using symbol_shorthand::T; + +// 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(pose1, pose2, t); + PinholeCamera 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(pose1, pose2, t); + Pose3 body_P_sensor3(Rot3::RzRyRx(-0.1, -0.1, 0.0), Point3(0,0.2,0.1)); + PinholeCamera 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(pose1, pose2, t); + PinholeCamera 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( + std::function( + 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( + std::function( + 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( + std::function( + 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(pose1, pose2, t); + Pose3 body_P_sensor3(Rot3::RzRyRx(-0.1, -0.1, 0.0), Point3(0,0.2,0.1)); + PinholeCamera 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( + std::function( + 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( + std::function( + 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( + std::function( + 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(pose1, pose2, t); + PinholeCamera 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( + std::function( + 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( + std::function( + 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( + std::function( + 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); } +/* ************************************************************************* */ + diff --git a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp new file mode 100644 index 000000000..75aaf4ac1 --- /dev/null +++ b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp @@ -0,0 +1,842 @@ +/* ---------------------------------------------------------------------------- + + * 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 testSmartProjectionPoseFactorRollingShutter.cpp + * @brief Unit tests for SmartProjectionPoseFactorRollingShutter Class + * @author Luca Carlone + * @date July 2021 + */ + +#include "gtsam/slam/tests/smartFactorScenarios.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#define DISABLE_TIMING + +using namespace gtsam; +using namespace boost::assign; +using namespace std::placeholders; + +static const double rankTol = 1.0; +// Create a noise model for the pixel error +static const double sigma = 0.1; +static SharedIsotropic model(noiseModel::Isotropic::Sigma(2, sigma)); + +// Convenience for named keys +using symbol_shorthand::X; +using symbol_shorthand::L; + +// tests data +static Symbol x1('X', 1); +static Symbol x2('X', 2); +static Symbol x3('X', 3); +static Symbol x4('X', 4); +static Symbol l0('L', 0); +static Pose3 body_P_sensor = Pose3(Rot3::Ypr(-0.1, 0.2, -0.2), + Point3(0.1, 0.0, 0.0)); + +static Point2 measurement1(323.0, 240.0); +static Point2 measurement2(200.0, 220.0); +static Point2 measurement3(320.0, 10.0); +static double interp_factor = 0.5; +static double interp_factor1 = 0.3; +static double interp_factor2 = 0.4; +static double interp_factor3 = 0.5; + +/* ************************************************************************* */ +// default Cal3_S2 poses with rolling shutter effect +namespace vanillaPoseRS { +typedef PinholePose Camera; +static Cal3_S2::shared_ptr sharedK(new Cal3_S2(fov, w, h)); +Pose3 interp_pose1 = interpolate(level_pose,pose_right,interp_factor1); +Pose3 interp_pose2 = interpolate(pose_right,pose_above,interp_factor2); +Pose3 interp_pose3 = interpolate(pose_above,level_pose,interp_factor3); +Camera cam1(interp_pose1, sharedK); +Camera cam2(interp_pose2, sharedK); +Camera cam3(interp_pose3, sharedK); +} + +LevenbergMarquardtParams lmParams; +typedef SmartProjectionPoseFactorRollingShutter SmartFactorRS; + +/* ************************************************************************* */ +TEST( SmartProjectionPoseFactorRollingShutter, Constructor) { + SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model)); +} + +/* ************************************************************************* */ +TEST( SmartProjectionPoseFactorRollingShutter, Constructor2) { + SmartProjectionParams params; + params.setRankTolerance(rankTol); + SmartFactorRS factor1(model, params); +} + +/* ************************************************************************* */ +TEST( SmartProjectionPoseFactorRollingShutter, add) { + using namespace vanillaPose; + SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model)); + factor1->add(measurement1, x1, x2, interp_factor, sharedK, body_P_sensor); +} + +/* ************************************************************************* */ +TEST( SmartProjectionPoseFactorRollingShutter, Equals ) { + using namespace vanillaPose; + + // create fake measurements + Point2Vector measurements; + measurements.push_back(measurement1); + measurements.push_back(measurement2); + measurements.push_back(measurement3); + + std::vector> key_pairs; + key_pairs.push_back(std::make_pair(x1,x2)); + key_pairs.push_back(std::make_pair(x2,x3)); + key_pairs.push_back(std::make_pair(x3,x4)); + + std::vector> intrinsicCalibrations; + intrinsicCalibrations.push_back(sharedK); + intrinsicCalibrations.push_back(sharedK); + intrinsicCalibrations.push_back(sharedK); + + std::vector extrinsicCalibrations; + extrinsicCalibrations.push_back(body_P_sensor); + extrinsicCalibrations.push_back(body_P_sensor); + extrinsicCalibrations.push_back(body_P_sensor); + + std::vector interp_factors; + interp_factors.push_back(interp_factor1); + interp_factors.push_back(interp_factor2); + interp_factors.push_back(interp_factor3); + + // create by adding a batch of measurements with a bunch of calibrations + SmartFactorRS::shared_ptr factor2(new SmartFactorRS(model)); + factor2->add(measurements, key_pairs, interp_factors, intrinsicCalibrations, extrinsicCalibrations); + + // create by adding a batch of measurements with a single calibrations + SmartFactorRS::shared_ptr factor3(new SmartFactorRS(model)); + factor3->add(measurements, key_pairs, interp_factors, sharedK, body_P_sensor); + + { // create equal factors and show equal returns true + SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model)); + factor1->add(measurement1, x1, x2, interp_factor1, sharedK, body_P_sensor); + factor1->add(measurement2, x2, x3, interp_factor2, sharedK, body_P_sensor); + factor1->add(measurement3, x3, x4, interp_factor3, sharedK, body_P_sensor); + + EXPECT(assert_equal(*factor1, *factor2)); + EXPECT(assert_equal(*factor1, *factor3)); + } + { // create slightly different factors (different keys) and show equal returns false + SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model)); + factor1->add(measurement1, x1, x2, interp_factor1, sharedK, body_P_sensor); + factor1->add(measurement2, x2, x2, interp_factor2, sharedK, body_P_sensor); // different! + factor1->add(measurement3, x3, x4, interp_factor3, sharedK, body_P_sensor); + + EXPECT(!assert_equal(*factor1, *factor2)); + EXPECT(!assert_equal(*factor1, *factor3)); + } + { // create slightly different factors (different extrinsics) and show equal returns false + SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model)); + factor1->add(measurement1, x1, x2, interp_factor1, sharedK, body_P_sensor); + factor1->add(measurement2, x2, x3, interp_factor2, sharedK, body_P_sensor*body_P_sensor); // different! + factor1->add(measurement3, x3, x4, interp_factor3, sharedK, body_P_sensor); + + EXPECT(!assert_equal(*factor1, *factor2)); + EXPECT(!assert_equal(*factor1, *factor3)); + } + { // create slightly different factors (different interp factors) and show equal returns false + SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model)); + factor1->add(measurement1, x1, x2, interp_factor1, sharedK, body_P_sensor); + factor1->add(measurement2, x2, x3, interp_factor1, sharedK, body_P_sensor); // different! + factor1->add(measurement3, x3, x4, interp_factor3, sharedK, body_P_sensor); + + EXPECT(!assert_equal(*factor1, *factor2)); + EXPECT(!assert_equal(*factor1, *factor3)); + } +} + +static const int DimBlock = 12; ///< size of the variable stacking 2 poses from which the observation pose is interpolated +static const int ZDim = 2; ///< Measurement dimension (Point2) +typedef Eigen::Matrix MatrixZD; // F blocks (derivatives wrt camera) +typedef std::vector > FBlocks; // vector of F blocks + +/* *************************************************************************/ +TEST( SmartProjectionPoseFactorRollingShutter, noiselessErrorAndJacobians ) { + using namespace vanillaPoseRS; + + // Project two landmarks into two cameras + Point2 level_uv = cam1.project(landmark1); + Point2 level_uv_right = cam2.project(landmark1); + Pose3 body_P_sensorId = Pose3::identity(); + + SmartFactorRS factor(model); + factor.add(level_uv, x1, x2, interp_factor1, sharedK, body_P_sensorId); + factor.add(level_uv_right, x2, x3, interp_factor2, sharedK, body_P_sensorId); + + Values values; // it's a pose factor, hence these are poses + values.insert(x1, level_pose); + values.insert(x2, pose_right); + values.insert(x3, pose_above); + + double actualError = factor.error(values); + double expectedError = 0.0; + EXPECT_DOUBLES_EQUAL(expectedError, actualError, 1e-7); + + // Check triangulation + factor.triangulateSafe(factor.cameras(values)); + TriangulationResult point = factor.point(); + EXPECT(point.valid()); // check triangulated point is valid + EXPECT(assert_equal(landmark1, *point)); // check triangulation result matches expected 3D landmark + + // Check Jacobians + // -- actual Jacobians + FBlocks actualFs; + Matrix actualE; + Vector actualb; + factor.computeJacobiansWithTriangulatedPoint(actualFs, actualE, actualb, values); + EXPECT(actualE.rows() == 4); EXPECT(actualE.cols() == 3); + EXPECT(actualb.rows() == 4); EXPECT(actualb.cols() == 1); + EXPECT(actualFs.size() == 2); + + // -- expected Jacobians from ProjectionFactorsRollingShutter + ProjectionFactorRollingShutter factor1(level_uv, interp_factor1, model, x1, x2, l0, sharedK, body_P_sensorId); + Matrix expectedF11, expectedF12, expectedE1; + Vector expectedb1 = factor1.evaluateError(level_pose, pose_right, landmark1, expectedF11, expectedF12, expectedE1); + EXPECT(assert_equal( expectedF11, Matrix(actualFs[0].block(0,0,2,6)), 1e-5)); + EXPECT(assert_equal( expectedF12, Matrix(actualFs[0].block(0,6,2,6)), 1e-5)); + EXPECT(assert_equal( expectedE1, Matrix(actualE.block(0,0,2,3)), 1e-5)); + // by definition computeJacobiansWithTriangulatedPoint returns minus reprojectionError + EXPECT(assert_equal( expectedb1, -Vector(actualb.segment<2>(0)), 1e-5)); + + ProjectionFactorRollingShutter factor2(level_uv_right, interp_factor2, model, x2, x3, l0, sharedK, body_P_sensorId); + Matrix expectedF21, expectedF22, expectedE2; + Vector expectedb2 = factor2.evaluateError(pose_right, pose_above, landmark1, expectedF21, expectedF22, expectedE2); + EXPECT(assert_equal( expectedF21, Matrix(actualFs[1].block(0,0,2,6)), 1e-5)); + EXPECT(assert_equal( expectedF22, Matrix(actualFs[1].block(0,6,2,6)), 1e-5)); + EXPECT(assert_equal( expectedE2, Matrix(actualE.block(2,0,2,3)), 1e-5)); + // by definition computeJacobiansWithTriangulatedPoint returns minus reprojectionError + EXPECT(assert_equal( expectedb2, -Vector(actualb.segment<2>(2)), 1e-5)); +} + +/* *************************************************************************/ +TEST( SmartProjectionPoseFactorRollingShutter, noisyErrorAndJacobians ) { + // also includes non-identical extrinsic calibration + using namespace vanillaPoseRS; + + // Project two landmarks into two cameras + Point2 pixelError(0.5, 1.0); + Point2 level_uv = cam1.project(landmark1) + pixelError; + Point2 level_uv_right = cam2.project(landmark1); + Pose3 body_P_sensorNonId = body_P_sensor; + + SmartFactorRS factor(model); + factor.add(level_uv, x1, x2, interp_factor1, sharedK, body_P_sensorNonId); + factor.add(level_uv_right, x2, x3, interp_factor2, sharedK, body_P_sensorNonId); + + Values values; // it's a pose factor, hence these are poses + values.insert(x1, level_pose); + values.insert(x2, pose_right); + values.insert(x3, pose_above); + + // Perform triangulation + factor.triangulateSafe(factor.cameras(values)); + TriangulationResult point = factor.point(); + EXPECT(point.valid()); // check triangulated point is valid + Point3 landmarkNoisy = *point; + + // Check Jacobians + // -- actual Jacobians + FBlocks actualFs; + Matrix actualE; + Vector actualb; + factor.computeJacobiansWithTriangulatedPoint(actualFs, actualE, actualb, values); + EXPECT(actualE.rows() == 4); EXPECT(actualE.cols() == 3); + EXPECT(actualb.rows() == 4); EXPECT(actualb.cols() == 1); + EXPECT(actualFs.size() == 2); + + // -- expected Jacobians from ProjectionFactorsRollingShutter + ProjectionFactorRollingShutter factor1(level_uv, interp_factor1, model, x1, x2, l0, sharedK, body_P_sensorNonId); + Matrix expectedF11, expectedF12, expectedE1; + Vector expectedb1 = factor1.evaluateError(level_pose, pose_right, landmarkNoisy, expectedF11, expectedF12, expectedE1); + EXPECT(assert_equal( expectedF11, Matrix(actualFs[0].block(0,0,2,6)), 1e-5)); + EXPECT(assert_equal( expectedF12, Matrix(actualFs[0].block(0,6,2,6)), 1e-5)); + EXPECT(assert_equal( expectedE1, Matrix(actualE.block(0,0,2,3)), 1e-5)); + // by definition computeJacobiansWithTriangulatedPoint returns minus reprojectionError + EXPECT(assert_equal( expectedb1, -Vector(actualb.segment<2>(0)), 1e-5)); + + ProjectionFactorRollingShutter factor2(level_uv_right, interp_factor2, model, x2, x3, l0, sharedK, body_P_sensorNonId); + Matrix expectedF21, expectedF22, expectedE2; + Vector expectedb2 = factor2.evaluateError(pose_right, pose_above, landmarkNoisy, expectedF21, expectedF22, expectedE2); + EXPECT(assert_equal( expectedF21, Matrix(actualFs[1].block(0,0,2,6)), 1e-5)); + EXPECT(assert_equal( expectedF22, Matrix(actualFs[1].block(0,6,2,6)), 1e-5)); + EXPECT(assert_equal( expectedE2, Matrix(actualE.block(2,0,2,3)), 1e-5)); + // by definition computeJacobiansWithTriangulatedPoint returns minus reprojectionError + EXPECT(assert_equal( expectedb2, -Vector(actualb.segment<2>(2)), 1e-5)); + + // Check errors + double actualError = factor.error(values); // from smart factor + NonlinearFactorGraph nfg; + nfg.add(factor1); + nfg.add(factor2); + values.insert(l0, landmarkNoisy); + double expectedError = nfg.error(values); + EXPECT_DOUBLES_EQUAL(expectedError, actualError, 1e-7); +} + +/* *************************************************************************/ +TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses ) { + + using namespace vanillaPoseRS; + Point2Vector measurements_lmk1, measurements_lmk2, measurements_lmk3; + + // Project three landmarks into three cameras + projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_lmk1); + projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_lmk2); + projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_lmk3); + + // create inputs + std::vector> key_pairs; + key_pairs.push_back(std::make_pair(x1,x2)); + key_pairs.push_back(std::make_pair(x2,x3)); + key_pairs.push_back(std::make_pair(x3,x1)); + + std::vector interp_factors; + interp_factors.push_back(interp_factor1); + interp_factors.push_back(interp_factor2); + interp_factors.push_back(interp_factor3); + + SmartFactorRS::shared_ptr smartFactor1(new SmartFactorRS(model)); + smartFactor1->add(measurements_lmk1, key_pairs, interp_factors, sharedK); + + SmartFactorRS::shared_ptr smartFactor2(new SmartFactorRS(model)); + smartFactor2->add(measurements_lmk2, key_pairs, interp_factors, sharedK); + + SmartFactorRS::shared_ptr smartFactor3(new SmartFactorRS(model)); + smartFactor3->add(measurements_lmk3, key_pairs, interp_factors, sharedK); + + const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); + + NonlinearFactorGraph graph; + graph.push_back(smartFactor1); + graph.push_back(smartFactor2); + graph.push_back(smartFactor3); + graph.addPrior(x1, level_pose, noisePrior); + graph.addPrior(x2, pose_right, noisePrior); + + Values groundTruth; + groundTruth.insert(x1, level_pose); + groundTruth.insert(x2, pose_right); + groundTruth.insert(x3, pose_above); + DOUBLES_EQUAL(0, graph.error(groundTruth), 1e-9); + + // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below + Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), + Point3(0.1, 0.1, 0.1)); // smaller noise + Values values; + values.insert(x1, level_pose); + values.insert(x2, pose_right); + // initialize third pose with some noise, we expect it to move back to original pose_above + values.insert(x3, pose_above * noise_pose); + EXPECT( // check that the pose is actually noisy + assert_equal( + Pose3( + Rot3(0, -0.0314107591, 0.99950656, -0.99950656, -0.0313952598, + -0.000986635786, 0.0314107591, -0.999013364, -0.0313952598), + Point3(0.1, -0.1, 1.9)), values.at(x3))); + + Values result; + LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); + result = optimizer.optimize(); + EXPECT(assert_equal(pose_above, result.at(x3), 1e-6)); +} + +/* *************************************************************************/ +TEST( SmartProjectionPoseFactorRollingShutter, hessian_simple_2poses ) { + // here we replicate a test in SmartProjectionPoseFactor by setting interpolation + // factors to 0 and 1 (such that the rollingShutter measurements falls back to standard pixel measurements) + // Note: this is a quite extreme test since in typical camera you would not have more than + // 1 measurement per landmark at each interpolated pose + using namespace vanillaPose; + + // Default cameras for simple derivatives + static Cal3_S2::shared_ptr sharedKSimple(new Cal3_S2(100, 100, 0, 0, 0)); + + Rot3 R = Rot3::identity(); + Pose3 pose1 = Pose3(R, Point3(0, 0, 0)); + Pose3 pose2 = Pose3(R, Point3(1, 0, 0)); + Camera cam1(pose1, sharedKSimple), cam2(pose2, sharedKSimple); + Pose3 body_P_sensorId = Pose3::identity(); + + // one landmarks 1m in front of camera + Point3 landmark1(0, 0, 10); + + Point2Vector measurements_lmk1; + + // Project 2 landmarks into 2 cameras + measurements_lmk1.push_back(cam1.project(landmark1)); + measurements_lmk1.push_back(cam2.project(landmark1)); + + SmartFactorRS::shared_ptr smartFactor1(new SmartFactorRS(model)); + double interp_factor = 0; // equivalent to measurement taken at pose 1 + smartFactor1->add(measurements_lmk1[0], x1, x2, interp_factor, sharedKSimple, + body_P_sensorId); + interp_factor = 1; // equivalent to measurement taken at pose 2 + smartFactor1->add(measurements_lmk1[1], x1, x2, interp_factor, sharedKSimple, + body_P_sensorId); + + SmartFactor::Cameras cameras; + cameras.push_back(cam1); + cameras.push_back(cam2); + + // Make sure triangulation works + EXPECT(smartFactor1->triangulateSafe(cameras)); + EXPECT(!smartFactor1->isDegenerate()); + EXPECT(!smartFactor1->isPointBehindCamera()); + boost::optional p = smartFactor1->point(); + EXPECT(p); + EXPECT(assert_equal(landmark1, *p)); + + VectorValues zeroDelta; + Vector6 delta; + delta.setZero(); + zeroDelta.insert(x1, delta); + zeroDelta.insert(x2, delta); + + VectorValues perturbedDelta; + delta.setOnes(); + perturbedDelta.insert(x1, delta); + perturbedDelta.insert(x2, delta); + double expectedError = 2500; + + // After eliminating the point, A1 and A2 contain 2-rank information on cameras: + Matrix16 A1, A2; + A1 << -10, 0, 0, 0, 1, 0; + A2 << 10, 0, 1, 0, -1, 0; + A1 *= 10. / sigma; + A2 *= 10. / sigma; + Matrix expectedInformation; // filled below + + // createHessianFactor + Matrix66 G11 = 0.5 * A1.transpose() * A1; + Matrix66 G12 = 0.5 * A1.transpose() * A2; + Matrix66 G22 = 0.5 * A2.transpose() * A2; + + Vector6 g1; + g1.setZero(); + Vector6 g2; + g2.setZero(); + + double f = 0; + + RegularHessianFactor<6> expected(x1, x2, G11, G12, g1, G22, g2, f); + expectedInformation = expected.information(); + + Values values; + values.insert(x1, pose1); + values.insert(x2, pose2); + boost::shared_ptr < RegularHessianFactor<6> > actual = smartFactor1 + ->createHessianFactor(values); + EXPECT(assert_equal(expectedInformation, actual->information(), 1e-6)); + EXPECT(assert_equal(expected, *actual, 1e-6)); + EXPECT_DOUBLES_EQUAL(0, actual->error(zeroDelta), 1e-6); + EXPECT_DOUBLES_EQUAL(expectedError, actual->error(perturbedDelta), 1e-6); +} + +/* *************************************************************************/ +TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_EPI ) { + using namespace vanillaPoseRS; + Point2Vector measurements_lmk1, measurements_lmk2, measurements_lmk3; + + // Project three landmarks into three cameras + projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_lmk1); + projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_lmk2); + projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_lmk3); + + // create inputs + std::vector> key_pairs; + key_pairs.push_back(std::make_pair(x1, x2)); + key_pairs.push_back(std::make_pair(x2, x3)); + key_pairs.push_back(std::make_pair(x3, x1)); + + std::vector interp_factors; + interp_factors.push_back(interp_factor1); + interp_factors.push_back(interp_factor2); + interp_factors.push_back(interp_factor3); + + double excludeLandmarksFutherThanDist = 1e10; //very large + SmartProjectionParams params; + params.setRankTolerance(1.0); + params.setLinearizationMode(gtsam::HESSIAN); + params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY); + params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist); + params.setEnableEPI(true); + + SmartFactorRS smartFactor1(model,params); + smartFactor1.add(measurements_lmk1, key_pairs, interp_factors, sharedK); + + SmartFactorRS smartFactor2(model,params); + smartFactor2.add(measurements_lmk2, key_pairs, interp_factors, sharedK); + + SmartFactorRS smartFactor3(model,params); + smartFactor3.add(measurements_lmk3, key_pairs, interp_factors, sharedK); + + const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); + + NonlinearFactorGraph graph; + graph.push_back(smartFactor1); + graph.push_back(smartFactor2); + graph.push_back(smartFactor3); + graph.addPrior(x1, level_pose, noisePrior); + graph.addPrior(x2, pose_right, noisePrior); + + Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), + Point3(0.1, 0.1, 0.1)); // smaller noise + Values values; + values.insert(x1, level_pose); + values.insert(x2, pose_right); + // initialize third pose with some noise, we expect it to move back to original pose_above + values.insert(x3, pose_above * noise_pose); + + // Optimization should correct 3rd pose + Values result; + LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); + result = optimizer.optimize(); + EXPECT(assert_equal(pose_above, result.at(x3), 1e-6)); +} + +/* *************************************************************************/ +TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_landmarkDistance ) { + using namespace vanillaPoseRS; + Point2Vector measurements_lmk1, measurements_lmk2, measurements_lmk3; + + // Project three landmarks into three cameras + projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_lmk1); + projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_lmk2); + projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_lmk3); + + // create inputs + std::vector> key_pairs; + key_pairs.push_back(std::make_pair(x1, x2)); + key_pairs.push_back(std::make_pair(x2, x3)); + key_pairs.push_back(std::make_pair(x3, x1)); + + std::vector interp_factors; + interp_factors.push_back(interp_factor1); + interp_factors.push_back(interp_factor2); + interp_factors.push_back(interp_factor3); + + double excludeLandmarksFutherThanDist = 2; + SmartProjectionParams params; + params.setRankTolerance(1.0); + params.setLinearizationMode(gtsam::HESSIAN); + params.setDegeneracyMode(gtsam::IGNORE_DEGENERACY); + params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist); + params.setEnableEPI(false); + + SmartFactorRS smartFactor1(model,params); + smartFactor1.add(measurements_lmk1, key_pairs, interp_factors, sharedK); + + SmartFactorRS smartFactor2(model,params); + smartFactor2.add(measurements_lmk2, key_pairs, interp_factors, sharedK); + + SmartFactorRS smartFactor3(model,params); + smartFactor3.add(measurements_lmk3, key_pairs, interp_factors, sharedK); + + const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); + + NonlinearFactorGraph graph; + graph.push_back(smartFactor1); + graph.push_back(smartFactor2); + graph.push_back(smartFactor3); + graph.addPrior(x1, level_pose, noisePrior); + graph.addPrior(x2, pose_right, noisePrior); + + Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), + Point3(0.1, 0.1, 0.1)); // smaller noise + Values values; + values.insert(x1, level_pose); + values.insert(x2, pose_right); + // initialize third pose with some noise, we expect it to move back to original pose_above + values.insert(x3, pose_above * noise_pose); + + // All factors are disabled (due to the distance threshold) and pose should remain where it is + Values result; + LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); + result = optimizer.optimize(); + EXPECT(assert_equal(values.at(x3), result.at(x3))); +} + +/* *************************************************************************/ +TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_dynamicOutlierRejection ) { + using namespace vanillaPoseRS; + // add fourth landmark + Point3 landmark4(5, -0.5, 1); + + Point2Vector measurements_lmk1, measurements_lmk2, measurements_lmk3, + measurements_lmk4; + // Project 4 landmarks into cameras + projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_lmk1); + projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_lmk2); + projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_lmk3); + projectToMultipleCameras(cam1, cam2, cam3, landmark4, measurements_lmk4); + measurements_lmk4.at(0) = measurements_lmk4.at(0) + Point2(10, 10); // add outlier + + // create inputs + std::vector> key_pairs; + key_pairs.push_back(std::make_pair(x1, x2)); + key_pairs.push_back(std::make_pair(x2, x3)); + key_pairs.push_back(std::make_pair(x3, x1)); + + std::vector interp_factors; + interp_factors.push_back(interp_factor1); + interp_factors.push_back(interp_factor2); + interp_factors.push_back(interp_factor3); + + double excludeLandmarksFutherThanDist = 1e10; + double dynamicOutlierRejectionThreshold = 3; // max 3 pixel of average reprojection error + + SmartProjectionParams params; + params.setRankTolerance(1.0); + params.setLinearizationMode(gtsam::HESSIAN); + params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY); + params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist); + params.setDynamicOutlierRejectionThreshold(dynamicOutlierRejectionThreshold); + params.setEnableEPI(false); + + SmartFactorRS::shared_ptr smartFactor1(new SmartFactorRS(model, params)); + smartFactor1->add(measurements_lmk1, key_pairs, interp_factors, sharedK); + + SmartFactorRS::shared_ptr smartFactor2(new SmartFactorRS(model, params)); + smartFactor2->add(measurements_lmk2, key_pairs, interp_factors, sharedK); + + SmartFactorRS::shared_ptr smartFactor3(new SmartFactorRS(model, params)); + smartFactor3->add(measurements_lmk3, key_pairs, interp_factors, sharedK); + + SmartFactorRS::shared_ptr smartFactor4(new SmartFactorRS(model, params)); + smartFactor4->add(measurements_lmk4, key_pairs, interp_factors, sharedK); + + const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); + + NonlinearFactorGraph graph; + graph.push_back(smartFactor1); + graph.push_back(smartFactor2); + graph.push_back(smartFactor3); + graph.push_back(smartFactor4); + graph.addPrior(x1, level_pose, noisePrior); + graph.addPrior(x2, pose_right, noisePrior); + + Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), + Point3(0.01, 0.01, 0.01)); // smaller noise, otherwise outlier rejection will kick in + Values values; + values.insert(x1, level_pose); + values.insert(x2, pose_right); + // initialize third pose with some noise, we expect it to move back to original pose_above + values.insert(x3, pose_above * noise_pose); + + // Optimization should correct 3rd pose + Values result; + LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); + result = optimizer.optimize(); + EXPECT(assert_equal(pose_above, result.at(x3), 1e-6)); +} + +/* *************************************************************************/ +TEST( SmartProjectionPoseFactorRollingShutter, hessianComparedToProjFactorsRollingShutter) { + + using namespace vanillaPoseRS; + Point2Vector measurements_lmk1; + + // Project three landmarks into three cameras + projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_lmk1); + + // create inputs + std::vector> key_pairs; + key_pairs.push_back(std::make_pair(x1, x2)); + key_pairs.push_back(std::make_pair(x2, x3)); + key_pairs.push_back(std::make_pair(x3, x1)); + + std::vector interp_factors; + interp_factors.push_back(interp_factor1); + interp_factors.push_back(interp_factor2); + interp_factors.push_back(interp_factor3); + + SmartFactorRS::shared_ptr smartFactor1(new SmartFactorRS(model)); + smartFactor1->add(measurements_lmk1, key_pairs, interp_factors, sharedK); + + Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), + Point3(0.1, 0.1, 0.1)); // smaller noise + Values values; + values.insert(x1, level_pose); + values.insert(x2, pose_right); + // initialize third pose with some noise to get a nontrivial linearization point + values.insert(x3, pose_above * noise_pose); + EXPECT( // check that the pose is actually noisy + assert_equal( Pose3( Rot3(0, -0.0314107591, 0.99950656, -0.99950656, -0.0313952598, -0.000986635786, 0.0314107591, -0.999013364, -0.0313952598), Point3(0.1, -0.1, 1.9)), values.at(x3))); + + // linearization point for the poses + Pose3 pose1 = level_pose; + Pose3 pose2 = pose_right; + Pose3 pose3 = pose_above * noise_pose; + + // ==== check Hessian of smartFactor1 ===== + // -- compute actual Hessian + boost::shared_ptr linearfactor1 = smartFactor1->linearize( + values); + Matrix actualHessian = linearfactor1->information(); + + // -- compute expected Hessian from manual Schur complement from Jacobians + // linearization point for the 3D point + smartFactor1->triangulateSafe(smartFactor1->cameras(values)); + TriangulationResult point = smartFactor1->point(); + EXPECT(point.valid()); // check triangulated point is valid + + // Use the factor to calculate the Jacobians + Matrix F = Matrix::Zero(2 * 3, 6 * 3); + Matrix E = Matrix::Zero(2 * 3, 3); + Vector b = Vector::Zero(6); + + // create projection factors rolling shutter + ProjectionFactorRollingShutter factor11(measurements_lmk1[0], interp_factor1, + model, x1, x2, l0, sharedK); + Matrix H1Actual, H2Actual, H3Actual; + // note: b is minus the reprojection error, cf the smart factor jacobian computation + b.segment<2>(0) = -factor11.evaluateError(pose1, pose2, *point, H1Actual, H2Actual, H3Actual); + F.block<2, 6>(0, 0) = H1Actual; + F.block<2, 6>(0, 6) = H2Actual; + E.block<2, 3>(0, 0) = H3Actual; + + ProjectionFactorRollingShutter factor12(measurements_lmk1[1], interp_factor2, + model, x2, x3, l0, sharedK); + b.segment<2>(2) = -factor12.evaluateError(pose2, pose3, *point, H1Actual, H2Actual, H3Actual); + F.block<2, 6>(2, 6) = H1Actual; + F.block<2, 6>(2, 12) = H2Actual; + E.block<2, 3>(2, 0) = H3Actual; + + ProjectionFactorRollingShutter factor13(measurements_lmk1[2], interp_factor3, + model, x3, x1, l0, sharedK); + b.segment<2>(4) = -factor13.evaluateError(pose3, pose1, *point, H1Actual, H2Actual, H3Actual); + F.block<2, 6>(4, 12) = H1Actual; + F.block<2, 6>(4, 0) = H2Actual; + E.block<2, 3>(4, 0) = H3Actual; + + // whiten + F = (1/sigma) * F; + E = (1/sigma) * E; + b = (1/sigma) * b; + //* G = F' * F - F' * E * P * E' * F + Matrix P = (E.transpose() * E).inverse(); + Matrix expectedHessian = F.transpose() * F + - (F.transpose() * E * P * E.transpose() * F); + EXPECT(assert_equal(expectedHessian, actualHessian, 1e-6)); + + // ==== check Information vector of smartFactor1 ===== + GaussianFactorGraph gfg; + gfg.add(linearfactor1); + Matrix actualHessian_v2 = gfg.hessian().first; + EXPECT(assert_equal(actualHessian_v2, actualHessian, 1e-6)); // sanity check on hessian + + // -- compute actual information vector + Vector actualInfoVector = gfg.hessian().second; + + // -- compute expected information vector from manual Schur complement from Jacobians + //* g = F' * (b - E * P * E' * b) + Vector expectedInfoVector = F.transpose() * (b - E * P * E.transpose() * b); + EXPECT(assert_equal(expectedInfoVector, actualInfoVector, 1e-6)); + + // ==== check error of smartFactor1 (again) ===== + NonlinearFactorGraph nfg_projFactorsRS; + nfg_projFactorsRS.add(factor11); + nfg_projFactorsRS.add(factor12); + nfg_projFactorsRS.add(factor13); + values.insert(l0, *point); + + double actualError = smartFactor1->error(values); + double expectedError = nfg_projFactorsRS.error(values); + EXPECT_DOUBLES_EQUAL(expectedError, actualError, 1e-7); +} + +#ifndef DISABLE_TIMING +#include +// -Total: 0 CPU (0 times, 0 wall, 0.04 children, min: 0 max: 0) +//| -SF RS LINEARIZE: 0.02 CPU (1000 times, 0.017244 wall, 0.02 children, min: 0 max: 0) +//| -RS LINEARIZE: 0.02 CPU (1000 times, 0.009035 wall, 0.02 children, min: 0 max: 0) +/* *************************************************************************/ +TEST( SmartProjectionPoseFactorRollingShutter, timing ) { + + using namespace vanillaPose; + + // Default cameras for simple derivatives + static Cal3_S2::shared_ptr sharedKSimple(new Cal3_S2(100, 100, 0, 0, 0)); + + Rot3 R = Rot3::identity(); + Pose3 pose1 = Pose3(R, Point3(0, 0, 0)); + Pose3 pose2 = Pose3(R, Point3(1, 0, 0)); + Camera cam1(pose1, sharedKSimple), cam2(pose2, sharedKSimple); + Pose3 body_P_sensorId = Pose3::identity(); + + // one landmarks 1m in front of camera + Point3 landmark1(0, 0, 10); + + Point2Vector measurements_lmk1; + + // Project 2 landmarks into 2 cameras + measurements_lmk1.push_back(cam1.project(landmark1)); + measurements_lmk1.push_back(cam2.project(landmark1)); + + size_t nrTests = 1000; + + for(size_t i = 0; iadd(measurements_lmk1[0], x1, x2, interp_factor, sharedKSimple, + body_P_sensorId); + interp_factor = 1; // equivalent to measurement taken at pose 2 + smartFactorRS->add(measurements_lmk1[1], x1, x2, interp_factor, sharedKSimple, + body_P_sensorId); + + Values values; + values.insert(x1, pose1); + values.insert(x2, pose2); + gttic_(SF_RS_LINEARIZE); + smartFactorRS->linearize(values); + gttoc_(SF_RS_LINEARIZE); + } + + for(size_t i = 0; iadd(measurements_lmk1[0], x1); + smartFactor->add(measurements_lmk1[1], x2); + + Values values; + values.insert(x1, pose1); + values.insert(x2, pose2); + gttic_(RS_LINEARIZE); + smartFactor->linearize(values); + gttoc_(RS_LINEARIZE); + } + tictoc_print_(); +} +#endif + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ +