diff --git a/gtsam/base/Lie.h b/gtsam/base/Lie.h index fe730c934..ac7c2a9a5 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,12 +320,28 @@ T expm(const Vector& x, int K=7) { } /** - * Linear interpolation between X and Y by coefficient t in [0, 1]. + * Linear interpolation between X and Y by coefficient t. Typically t \in [0,1], + * but can also be used to extrapolate before pose X or after pose Y. */ template -T interpolate(const T& X, const T& Y, double t) { - assert(t >= 0 && t <= 1); - return traits::Compose(X, traits::Expmap(t * traits::Logmap(traits::Between(X, Y)))); +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 MakeJacobian::type between_H_x, log_H, exp_H, compose_H_x; + const T between = + traits::Between(X, Y, between_H_x); // between_H_y = identity + typename traits::TangentVector delta = traits::Logmap(between, log_H); + const T Delta = traits::Expmap(t * delta, exp_H); + const T result = traits::Compose( + X, Delta, compose_H_x); // compose_H_xinv_y = identity + + if (Hx) *Hx = compose_H_x + t * exp_H * log_H * between_H_x; + if (Hy) *Hy = t * exp_H * log_H; + return result; + } + return traits::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..143d4bc3c 100644 --- a/gtsam/geometry/CameraSet.h +++ b/gtsam/geometry/CameraSet.h @@ -147,51 +147,149 @@ public: * G = F' * F - F' * E * P * E' * F * g = F' * (b - E * P * E' * b) * Fixed size version - */ - template // N = 2 or 3, 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) { + */ + template // N = 2 or 3 (point dimension), ND is the camera dimension + static SymmetricBlockMatrix SchurComplement( + const std::vector< + Eigen::Matrix, + Eigen::aligned_allocator>>& Fs, + const Matrix& E, const Eigen::Matrix& P, const Vector& b) { + // a single point is observed in m cameras + size_t m = Fs.size(); - // a single point is observed in m cameras - size_t m = Fs.size(); + // Create a SymmetricBlockMatrix (augmented hessian, with extra row/column with info vector) + size_t M1 = ND * m + 1; + std::vector dims(m + 1); // this also includes the b term + std::fill(dims.begin(), dims.end() - 1, ND); + dims.back() = 1; + SymmetricBlockMatrix augmentedHessian(dims, Matrix::Zero(M1, M1)); - // Create a SymmetricBlockMatrix (augmented hessian, with extra row/column with info vector) - size_t M1 = ND * m + 1; - std::vector dims(m + 1); // this also includes the b term - std::fill(dims.begin(), dims.end() - 1, ND); - dims.back() = 1; - SymmetricBlockMatrix augmentedHessian(dims, Matrix::Zero(M1, M1)); + // Blockwise Schur complement + for (size_t i = 0; i < m; i++) { // for each camera - // Blockwise Schur complement - for (size_t i = 0; i < m; i++) { // for each camera + const Eigen::Matrix& Fi = Fs[i]; + const auto FiT = Fi.transpose(); + const Eigen::Matrix Ei_P = // + E.block(ZDim * i, 0, ZDim, N) * P; - const Eigen::Matrix& Fi = Fs[i]; - const auto FiT = Fi.transpose(); - const Eigen::Matrix Ei_P = // - E.block(ZDim * i, 0, ZDim, N) * P; + // D = (Dx2) * ZDim + augmentedHessian.setOffDiagonalBlock(i, m, FiT * b.segment(ZDim * i) // F' * b + - FiT * (Ei_P * (E.transpose() * b))); // D = (DxZDim) * (ZDimx3) * (N*ZDimm) * (ZDimm x 1) - // D = (Dx2) * ZDim - augmentedHessian.setOffDiagonalBlock(i, m, FiT * b.segment(ZDim * i) // F' * b - - FiT * (Ei_P * (E.transpose() * b))); // D = (DxZDim) * (ZDimx3) * (N*ZDimm) * (ZDimm x 1) + // (DxD) = (DxZDim) * ( (ZDimxD) - (ZDimx3) * (3xZDim) * (ZDimxD) ) + augmentedHessian.setDiagonalBlock(i, FiT + * (Fi - Ei_P * E.block(ZDim * i, 0, ZDim, N).transpose() * Fi)); - // (DxD) = (DxZDim) * ( (ZDimxD) - (ZDimx3) * (3xZDim) * (ZDimxD) ) - augmentedHessian.setDiagonalBlock(i, FiT - * (Fi - Ei_P * E.block(ZDim * i, 0, ZDim, N).transpose() * Fi)); + // upper triangular part of the hessian + for (size_t j = i + 1; j < m; j++) { // for each camera + const Eigen::Matrix& Fj = Fs[j]; - // upper triangular part of the hessian - for (size_t j = i + 1; j < m; j++) { // for each camera - const Eigen::Matrix& Fj = Fs[j]; + // (DxD) = (Dx2) * ( (2x2) * (2xD) ) + augmentedHessian.setOffDiagonalBlock(i, j, -FiT + * (Ei_P * E.block(ZDim * j, 0, ZDim, N).transpose() * Fj)); + } + } // end of for over cameras - // (DxD) = (Dx2) * ( (2x2) * (2xD) ) - augmentedHessian.setOffDiagonalBlock(i, j, -FiT - * (Ei_P * E.block(ZDim * j, 0, ZDim, N).transpose() * Fj)); + augmentedHessian.diagonalBlock(m)(0, 0) += b.squaredNorm(); + return augmentedHessian; + } + + /** + * Do Schur complement, given Jacobian as Fs,E,P, return SymmetricBlockMatrix + * G = F' * F - F' * E * P * E' * F + * g = F' * (b - E * P * E' * b) + * In this version, we allow for the case where the keys in the Jacobian are + * organized differently from the keys in the output SymmetricBlockMatrix In + * particular: each diagonal block of the Jacobian F captures 2 poses (useful + * for rolling shutter and extrinsic calibration) such that F keeps the block + * structure that makes the Schur complement trick fast. + * + * N = 2 or 3 (point dimension), ND is the Jacobian block dimension, NDD is + * the Hessian block dimension + */ + template + static SymmetricBlockMatrix SchurComplementAndRearrangeBlocks( + const std::vector< + Eigen::Matrix, + 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); + + // Pack into an Hessian factor, allow space for b term. + std::vector dims(nrUniqueKeys + 1); + std::fill(dims.begin(), dims.end() - 1, NDD); + dims.back() = 1; + SymmetricBlockMatrix augmentedHessianUniqueKeys; + + // Deal with the fact that some blocks may share the same keys. + if (nrUniqueKeys == nrNonuniqueKeys) { + // Case when there is 1 calibration key per camera: + augmentedHessianUniqueKeys = SymmetricBlockMatrix( + dims, Matrix(augmentedHessian.selfadjointView())); + } else { + // When multiple cameras share a calibration we have to rearrange + // the results of the Schur complement matrix. + std::vector nonuniqueDims(nrNonuniqueKeys + 1); // includes b + std::fill(nonuniqueDims.begin(), nonuniqueDims.end() - 1, NDD); + nonuniqueDims.back() = 1; + augmentedHessian = SymmetricBlockMatrix( + nonuniqueDims, Matrix(augmentedHessian.selfadjointView())); + + // Get map from key to location in the new augmented Hessian matrix (the + // one including only unique keys). + std::map keyToSlotMap; + for (size_t k = 0; k < nrUniqueKeys; k++) { + keyToSlotMap[hessianKeys[k]] = k; + } + + // Initialize matrix to zero. + augmentedHessianUniqueKeys = SymmetricBlockMatrix( + dims, Matrix::Zero(NDD * nrUniqueKeys + 1, NDD * nrUniqueKeys + 1)); + + // Add contributions for each key: note this loops over the hessian with + // nonUnique keys (augmentedHessian) and populates an Hessian that only + // includes the unique keys (that is what we want to return). + for (size_t i = 0; i < nrNonuniqueKeys; i++) { // rows + Key key_i = jacobianKeys.at(i); + + // Update information vector. + augmentedHessianUniqueKeys.updateOffDiagonalBlock( + keyToSlotMap[key_i], nrUniqueKeys, + augmentedHessian.aboveDiagonalBlock(i, nrNonuniqueKeys)); + + // Update blocks. + for (size_t j = i; j < nrNonuniqueKeys; j++) { // cols + Key key_j = jacobianKeys.at(j); + if (i == j) { + augmentedHessianUniqueKeys.updateDiagonalBlock( + keyToSlotMap[key_i], augmentedHessian.diagonalBlock(i)); + } else { // (i < j) + if (keyToSlotMap[key_i] != keyToSlotMap[key_j]) { + augmentedHessianUniqueKeys.updateOffDiagonalBlock( + keyToSlotMap[key_i], keyToSlotMap[key_j], + augmentedHessian.aboveDiagonalBlock(i, j)); + } else { + augmentedHessianUniqueKeys.updateDiagonalBlock( + keyToSlotMap[key_i], + augmentedHessian.aboveDiagonalBlock(i, j) + + augmentedHessian.aboveDiagonalBlock(i, j).transpose()); + } + } } - } // end of for over cameras + } - augmentedHessian.diagonalBlock(m)(0, 0) += b.squaredNorm(); - return augmentedHessian; + // Update bottom right element of the matrix. + augmentedHessianUniqueKeys.updateDiagonalBlock( + nrUniqueKeys, augmentedHessian.diagonalBlock(nrNonuniqueKeys)); } + return augmentedHessianUniqueKeys; + } /** * Do Schur complement, given Jacobian as Fs,E,P, return SymmetricBlockMatrix @@ -206,7 +304,7 @@ public: } /// Computes Point Covariance P, with lambda parameter - template // 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 +356,7 @@ public: * Applies Schur complement (exploiting block structure) to get a smart factor on cameras, * and adds the contribution of the smart factor to a pre-allocated augmented Hessian. */ - template // 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..144f28314 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,89 @@ 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 0b0308c5c..380283141 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..c92a13daf --- /dev/null +++ b/gtsam_unstable/slam/ProjectionFactorRollingShutter.cpp @@ -0,0 +1,64 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file ProjectionFactorRollingShutter.cpp + * @brief Basic projection factor for rolling shutter cameras + * @author Yotam Stern + */ + +#include + +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..c92653c13 --- /dev/null +++ b/gtsam_unstable/slam/ProjectionFactorRollingShutter.h @@ -0,0 +1,217 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file ProjectionFactorRollingShutter.h + * @brief Basic projection factor for rolling shutter cameras + * @author Yotam Stern + */ + +#pragma once + +#include +#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 + gtsam::NonlinearFactor::shared_ptr clone() const override { + return boost::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new This(*this))); + } + + /** + * print + * @param s optional string naming the factor + * @param keyFormatter optional formatter useful for printing Symbols + */ + void print( + const std::string& s = "", + const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override { + std::cout << s << "ProjectionFactorRollingShutter, z = "; + traits::Print(measured_); + std::cout << " rolling shutter interpolation param = " << alpha_; + if (this->body_P_sensor_) + this->body_P_sensor_->print(" sensor pose in body frame: "); + Base::print("", keyFormatter); + } + + /// equals + bool equals(const NonlinearFactor& p, double tol = 1e-9) const override { + const This* e = dynamic_cast(&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 override; + + /** 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 {}; + +} // namespace gtsam diff --git a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h new file mode 100644 index 000000000..7660ff236 --- /dev/null +++ b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h @@ -0,0 +1,485 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file SmartProjectionPoseFactorRollingShutter.h + * @brief Smart projection factor on poses modeling rolling shutter effect with + * given readout time + * @author Luca Carlone + */ + +#pragma once + +#include +#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 { + public: + typedef typename CAMERA::CalibrationType CALIBRATION; + + 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 + const 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 + auto w_P_body1 = values.at(world_P_body_key_pairs_[i].first); + auto w_P_body2 = values.at(world_P_body_key_pairs_[i].second); + double interpolationFactor = alphas_[i]; + // get interpolated pose: + auto w_P_body = + interpolate(w_P_body1, w_P_body2, interpolationFactor, + dInterpPose_dPoseBody1, dInterpPose_dPoseBody2); + auto body_P_cam = body_P_sensors_[i]; + auto w_P_cam = w_P_body.compose(body_P_cam, dPoseCam_dInterpPose); + PinholeCamera 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 Gs(nrUniqueKeys * (nrUniqueKeys + 1) / 2); + std::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>(this->keys_, + Gs, gs, 0.0); + } else { + throw std::runtime_error( + "SmartProjectionPoseFactorRollingShutter: " + "only supported degeneracy mode is ZERO_ON_DEGENERACY"); + } + } + // compute Jacobian given triangulated 3D Point + FBlocks Fs; + Matrix E; + Vector b; + this->computeJacobiansWithTriangulatedPoint(Fs, E, b, values); + + // Whiten using noise model + this->noiseModel_->WhitenSystem(E, b); + for (size_t i = 0; i < Fs.size(); i++) + Fs[i] = this->noiseModel_->Whiten(Fs[i]); + + Matrix3 P = Base::Cameras::PointCov(E, lambda, diagonalDamping); + + // Collect all the key pairs: these are the keys that correspond to the + // blocks in Fs (on which we apply the Schur Complement) + KeyVector nonuniqueKeys; + for (size_t i = 0; i < world_P_body_key_pairs_.size(); i++) { + nonuniqueKeys.push_back(world_P_body_key_pairs_.at(i).first); + nonuniqueKeys.push_back(world_P_body_key_pairs_.at(i).second); + } + + // Build augmented Hessian (with last row/column being the information + // vector) Note: we need to get the augumented hessian wrt the unique keys + // in key_ + SymmetricBlockMatrix augmentedHessianUniqueKeys = + Base::Cameras::template SchurComplementAndRearrangeBlocks<3, 12, 6>( + Fs, E, P, b, nonuniqueKeys, this->keys_); + + return boost::make_shared>( + 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..161c9aa55 --- /dev/null +++ b/gtsam_unstable/slam/tests/testProjectionFactorRollingShutter.cpp @@ -0,0 +1,407 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file ProjectionFactorRollingShutterRollingShutter.cpp + * @brief Unit tests for ProjectionFactorRollingShutter Class + * @author Luca Carlone + * @date July 2021 + */ + +#include +#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::L; +using symbol_shorthand::T; +using symbol_shorthand::X; + +// Convenience to define common variables across many tests +static Key poseKey1(X(1)); +static Key poseKey2(X(2)); +static Key pointKey(L(1)); +static double interp_params = 0.5; +static Point2 measurement(323.0, 240.0); +static Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), + Point3(0.25, -0.10, 1.0)); + +/* ************************************************************************* */ +TEST(ProjectionFactorRollingShutter, Constructor) { + ProjectionFactorRollingShutter factor(measurement, interp_params, model, + poseKey1, poseKey2, pointKey, K); +} + +/* ************************************************************************* */ +TEST(ProjectionFactorRollingShutter, ConstructorWithTransform) { + ProjectionFactorRollingShutter factor(measurement, interp_params, model, + poseKey1, poseKey2, pointKey, K, + body_P_sensor); +} + +/* ************************************************************************* */ +TEST(ProjectionFactorRollingShutter, Equals) { + { // factors are equal + ProjectionFactorRollingShutter factor1(measurement, interp_params, model, + poseKey1, poseKey2, pointKey, K); + ProjectionFactorRollingShutter factor2(measurement, interp_params, model, + poseKey1, poseKey2, pointKey, K); + CHECK(assert_equal(factor1, factor2)); + } + { // factors are NOT equal (keys are different) + ProjectionFactorRollingShutter factor1(measurement, interp_params, model, + poseKey1, poseKey2, pointKey, K); + ProjectionFactorRollingShutter factor2(measurement, interp_params, model, + poseKey1, poseKey1, pointKey, K); + CHECK(!assert_equal(factor1, factor2)); // not equal + } + { // factors are NOT equal (different interpolation) + ProjectionFactorRollingShutter factor1(measurement, 0.1, model, poseKey1, + poseKey1, pointKey, K); + ProjectionFactorRollingShutter factor2(measurement, 0.5, model, poseKey1, + poseKey2, pointKey, K); + CHECK(!assert_equal(factor1, factor2)); // not equal + } +} + +/* ************************************************************************* */ +TEST(ProjectionFactorRollingShutter, EqualsWithTransform) { + { // factors are equal + ProjectionFactorRollingShutter factor1(measurement, interp_params, model, + poseKey1, poseKey2, pointKey, K, + body_P_sensor); + ProjectionFactorRollingShutter factor2(measurement, interp_params, model, + poseKey1, poseKey2, pointKey, K, + body_P_sensor); + CHECK(assert_equal(factor1, factor2)); + } + { // factors are NOT equal + ProjectionFactorRollingShutter factor1(measurement, interp_params, model, + poseKey1, poseKey2, pointKey, K, + body_P_sensor); + Pose3 body_P_sensor2( + Rot3::RzRyRx(0.0, 0.0, 0.0), + Point3(0.25, -0.10, 1.0)); // rotation different from body_P_sensor + ProjectionFactorRollingShutter factor2(measurement, interp_params, model, + poseKey1, poseKey2, pointKey, K, + body_P_sensor2); + CHECK(!assert_equal(factor1, factor2)); + } +} + +/* ************************************************************************* */ +TEST(ProjectionFactorRollingShutter, Error) { + { + // Create the factor with a measurement that is 3 pixels off in x + // Camera pose corresponds to the first camera + double t = 0.0; + ProjectionFactorRollingShutter factor(measurement, t, model, poseKey1, + poseKey2, pointKey, K); + + // Set the linearization point + Pose3 pose1(Rot3(), Point3(0, 0, -6)); + Pose3 pose2(Rot3(), Point3(0, 0, -4)); + Point3 point(0.0, 0.0, 0.0); + + // Use the factor to calculate the error + Vector actualError(factor.evaluateError(pose1, pose2, point)); + + // The expected error is (-3.0, 0.0) pixels / UnitCovariance + Vector expectedError = Vector2(-3.0, 0.0); + + // Verify we get the expected error + CHECK(assert_equal(expectedError, actualError, 1e-9)); + } + { + // Create the factor with a measurement that is 3 pixels off in x + // Camera pose is actually interpolated now + double t = 0.5; + ProjectionFactorRollingShutter factor(measurement, t, model, poseKey1, + poseKey2, pointKey, K); + + // Set the linearization point + Pose3 pose1(Rot3(), Point3(0, 0, -8)); + Pose3 pose2(Rot3(), Point3(0, 0, -4)); + Point3 point(0.0, 0.0, 0.0); + + // Use the factor to calculate the error + Vector actualError(factor.evaluateError(pose1, pose2, point)); + + // The expected error is (-3.0, 0.0) pixels / UnitCovariance + Vector expectedError = Vector2(-3.0, 0.0); + + // Verify we get the expected error + CHECK(assert_equal(expectedError, actualError, 1e-9)); + } + { + // Create measurement by projecting 3D landmark + double t = 0.3; + Pose3 pose1(Rot3::RzRyRx(0.1, 0.0, 0.1), Point3(0, 0, 0)); + Pose3 pose2(Rot3::RzRyRx(-0.1, -0.1, 0.0), Point3(0, 0, 1)); + Pose3 poseInterp = interpolate(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..0b94d2c3f --- /dev/null +++ b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp @@ -0,0 +1,1145 @@ +/* ---------------------------------------------------------------------------- + + * 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 +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include "gtsam/slam/tests/smartFactorScenarios.h" +#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::L; +using symbol_shorthand::X; + +// 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); +} // namespace vanillaPoseRS + +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(factor1->equals(*factor2)); + EXPECT(factor1->equals(*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(!factor1->equals(*factor2)); + EXPECT(!factor1->equals(*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(!factor1->equals(*factor2)); + EXPECT(!factor1->equals(*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(!factor1->equals(*factor2)); + EXPECT(!factor1->equals(*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> 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); +} + +/* *************************************************************************/ +TEST(SmartProjectionPoseFactorRollingShutter, + hessianComparedToProjFactorsRollingShutter_measurementsFromSamePose) { + // in this test we make sure the fact works even if we have multiple pixel + // measurements of the same landmark at a single pose, a setup that occurs in + // multi-camera systems + + using namespace vanillaPoseRS; + Point2Vector measurements_lmk1; + + // Project three landmarks into three cameras + projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_lmk1); + + // create redundant measurements: + Camera::MeasurementVector measurements_lmk1_redundant = measurements_lmk1; + measurements_lmk1_redundant.push_back( + measurements_lmk1.at(0)); // we readd the first measurement + + // 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)); + key_pairs.push_back(std::make_pair(x1, x2)); + + std::vector interp_factors; + interp_factors.push_back(interp_factor1); + interp_factors.push_back(interp_factor2); + interp_factors.push_back(interp_factor3); + interp_factors.push_back(interp_factor1); + + SmartFactorRS::shared_ptr smartFactor1(new SmartFactorRS(model)); + smartFactor1->add(measurements_lmk1_redundant, 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 standard ProjectionFactorRollingShutter factor to calculate the + // Jacobians + Matrix F = Matrix::Zero(2 * 4, 6 * 3); + Matrix E = Matrix::Zero(2 * 4, 3); + Vector b = Vector::Zero(8); + + // create projection factors rolling shutter + ProjectionFactorRollingShutter factor11(measurements_lmk1_redundant[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_redundant[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_redundant[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; + + ProjectionFactorRollingShutter factor14(measurements_lmk1_redundant[3], + interp_factor1, model, x1, x2, l0, + sharedK); + b.segment<2>(6) = -factor11.evaluateError(pose1, pose2, *point, H1Actual, + H2Actual, H3Actual); + F.block<2, 6>(6, 0) = H1Actual; + F.block<2, 6>(6, 6) = H2Actual; + E.block<2, 3>(6, 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); + nfg_projFactorsRS.add(factor14); + values.insert(l0, *point); + + double actualError = smartFactor1->error(values); + double expectedError = nfg_projFactorsRS.error(values); + EXPECT_DOUBLES_EQUAL(expectedError, actualError, 1e-7); +} + +/* *************************************************************************/ +TEST(SmartProjectionPoseFactorRollingShutter, + optimization_3poses_measurementsFromSamePose) { + 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); + + // For first factor, we create redundant measurement (taken by the same keys + // as factor 1, to make sure the redundancy in the keys does not create + // problems) + Camera::MeasurementVector& measurements_lmk1_redundant = measurements_lmk1; + measurements_lmk1_redundant.push_back( + measurements_lmk1.at(0)); // we readd the first measurement + std::vector> key_pairs_redundant = key_pairs; + key_pairs_redundant.push_back( + key_pairs.at(0)); // we readd the first pair of keys + std::vector interp_factors_redundant = interp_factors; + interp_factors_redundant.push_back( + interp_factors.at(0)); // we readd the first interp factor + + SmartFactorRS::shared_ptr smartFactor1(new SmartFactorRS(model)); + smartFactor1->add(measurements_lmk1_redundant, key_pairs_redundant, + interp_factors_redundant, 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-5)); +} + +#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; i < nrTests; i++) { + SmartFactorRS::shared_ptr smartFactorRS(new SmartFactorRS(model)); + double interp_factor = 0; // equivalent to measurement taken at pose 1 + smartFactorRS->add(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; i < nrTests; i++) { + SmartFactor::shared_ptr smartFactor(new SmartFactor(model, sharedKSimple)); + smartFactor->add(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); +} +/* ************************************************************************* */