From 15802e58f98b1b7474d621fd19a80f36e711da79 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 8 Jan 2023 21:09:31 -0800 Subject: [PATCH] Address review comments --- gtsam/base/tests/testVerticalBlockMatrix.cpp | 3 +- gtsam/geometry/CameraSet.h | 281 ++++++++++-------- gtsam/inference/tests/testOrdering.cpp | 4 +- .../linear/tests/testGaussianFactorGraph.cpp | 2 +- .../tests/testSymbolicEliminationTree.cpp | 3 +- 5 files changed, 154 insertions(+), 139 deletions(-) diff --git a/gtsam/base/tests/testVerticalBlockMatrix.cpp b/gtsam/base/tests/testVerticalBlockMatrix.cpp index e6630427a..758d776c8 100644 --- a/gtsam/base/tests/testVerticalBlockMatrix.cpp +++ b/gtsam/base/tests/testVerticalBlockMatrix.cpp @@ -24,8 +24,7 @@ using namespace gtsam; -const std::list L{3, 2, 1}; -const std::vector dimensions(L.begin(), L.end()); +const std::vector dimensions{3, 2, 1}; //***************************************************************************** TEST(VerticalBlockMatrix, Constructor1) { diff --git a/gtsam/geometry/CameraSet.h b/gtsam/geometry/CameraSet.h index 0fbb50f02..a814fea87 100644 --- a/gtsam/geometry/CameraSet.h +++ b/gtsam/geometry/CameraSet.h @@ -18,12 +18,13 @@ #pragma once -#include -#include // for Cheirality exception -#include -#include #include +#include +#include +#include // for Cheirality exception +#include #include + #include namespace gtsam { @@ -31,70 +32,67 @@ namespace gtsam { /** * @brief A set of cameras, all with their own calibration */ -template -class CameraSet : public std::vector > { +template +class CameraSet : public std::vector> { + protected: + using Base = std::vector>; -protected: - using Base = std::vector>; + /** + * 2D measurement and noise model for each of the m views + * The order is kept the same as the keys that we use to create the factor. + */ + typedef typename CAMERA::Measurement Z; + typedef typename CAMERA::MeasurementVector ZVector; - /** - * 2D measurement and noise model for each of the m views - * The order is kept the same as the keys that we use to create the factor. - */ - typedef typename CAMERA::Measurement Z; - typedef typename CAMERA::MeasurementVector ZVector; + static const int D = traits::dimension; ///< Camera dimension + static const int ZDim = traits::dimension; ///< Measurement dimension - static const int D = traits::dimension; ///< Camera dimension - static const int ZDim = traits::dimension; ///< Measurement dimension + /// Make a vector of re-projection errors + static Vector ErrorVector(const ZVector& predicted, const ZVector& measured) { + // Check size + size_t m = predicted.size(); + if (measured.size() != m) + throw std::runtime_error("CameraSet::errors: size mismatch"); - /// Make a vector of re-projection errors - static Vector ErrorVector(const ZVector& predicted, const ZVector& measured) { - // Check size - size_t m = predicted.size(); - if (measured.size() != m) - throw std::runtime_error("CameraSet::errors: size mismatch"); - - // Project and fill error vector - Vector b(ZDim * m); - for (size_t i = 0, row = 0; i < m; i++, row += ZDim) { - Vector bi = traits::Local(measured[i], predicted[i]); - if (ZDim == 3 && std::isnan(bi(1))) { // if it is a stereo point and the - // right pixel is missing (nan) - bi(1) = 0; - } - b.segment(row) = bi; - } - return b; + // Project and fill error vector + Vector b(ZDim * m); + for (size_t i = 0, row = 0; i < m; i++, row += ZDim) { + Vector bi = traits::Local(measured[i], predicted[i]); + if (ZDim == 3 && std::isnan(bi(1))) { // if it is a stereo point and the + // right pixel is missing (nan) + bi(1) = 0; + } + b.segment(row) = bi; + } + return b; } -public: - using Base::Base; // Inherit the vector constructors + public: + using Base::Base; // Inherit the vector constructors - /// Destructor - virtual ~CameraSet() = default; + /// Destructor + virtual ~CameraSet() = default; - /// Definitions for blocks of F - using MatrixZD = Eigen::Matrix; - using FBlocks = std::vector>; + /// Definitions for blocks of F + using MatrixZD = Eigen::Matrix; + using FBlocks = std::vector>; - /** - * print - * @param s optional string naming the factor - * @param keyFormatter optional formatter useful for printing Symbols - */ - virtual void print(const std::string& s = "") const { - std::cout << s << "CameraSet, cameras = \n"; - for (size_t k = 0; k < this->size(); ++k) this->at(k).print(s); + /** + * print + * @param s optional string naming the factor + * @param keyFormatter optional formatter useful for printing Symbols + */ + virtual void print(const std::string& s = "") const { + std::cout << s << "CameraSet, cameras = \n"; + for (size_t k = 0; k < this->size(); ++k) this->at(k).print(s); } /// equals bool equals(const CameraSet& p, double tol = 1e-9) const { - if (this->size() != p.size()) - return false; + if (this->size() != p.size()) return false; bool camerasAreEqual = true; for (size_t i = 0; i < this->size(); i++) { - if (this->at(i).equals(p.at(i), tol) == false) - camerasAreEqual = false; + if (this->at(i).equals(p.at(i), tol) == false) camerasAreEqual = false; break; } return camerasAreEqual; @@ -106,11 +104,10 @@ public: * matrix this function returns the diagonal blocks. * throws CheiralityException */ - template - ZVector project2(const POINT& point, // - boost::optional Fs = boost::none, // - boost::optional E = boost::none) const { - + template + ZVector project2(const POINT& point, // + boost::optional Fs = boost::none, // + boost::optional E = boost::none) const { static const int N = FixedDimension::value; // Allocate result @@ -135,19 +132,19 @@ public: } /// Calculate vector [project2(point)-z] of re-projection errors - template + template Vector reprojectionError(const POINT& point, const ZVector& measured, - boost::optional Fs = boost::none, // - boost::optional E = boost::none) const { + boost::optional Fs = boost::none, // + boost::optional E = boost::none) const { return ErrorVector(project2(point, Fs, E), measured); } /** - * 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) - * Fixed size version - */ + * 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) + * Fixed size version + */ template // N = 2 or 3 (point dimension), ND is the camera dimension static SymmetricBlockMatrix SchurComplement( @@ -158,38 +155,47 @@ public: // 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) + // 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::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 + 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 = // + 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) + 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)); + 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 + 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)); + augmentedHessian.setOffDiagonalBlock( + i, j, + -FiT * (Ei_P * E.block(ZDim * j, 0, ZDim, N).transpose() * Fj)); } - } // end of for over cameras + } // end of for over cameras augmentedHessian.diagonalBlock(m)(0, 0) += b.squaredNorm(); return augmentedHessian; @@ -297,20 +303,21 @@ public: * g = F' * (b - E * P * E' * b) * Fixed size version */ - template // N = 2 or 3 - static SymmetricBlockMatrix SchurComplement(const FBlocks& Fs, - const Matrix& E, const Eigen::Matrix& P, const Vector& b) { - return SchurComplement(Fs, E, P, b); + template // N = 2 or 3 + static SymmetricBlockMatrix SchurComplement( + const FBlocks& Fs, const Matrix& E, const Eigen::Matrix& P, + const Vector& b) { + return SchurComplement(Fs, E, P, b); } /// Computes Point Covariance P, with lambda parameter - template // N = 2 or 3 (point dimension) + template // N = 2 or 3 (point dimension) static void ComputePointCovariance(Eigen::Matrix& P, - const Matrix& E, double lambda, bool diagonalDamping = false) { - + const Matrix& E, double lambda, + bool diagonalDamping = false) { Matrix EtE = E.transpose() * E; - if (diagonalDamping) { // diagonal of the hessian + if (diagonalDamping) { // diagonal of the hessian EtE.diagonal() += lambda * EtE.diagonal(); } else { DenseIndex n = E.cols(); @@ -322,7 +329,7 @@ public: /// Computes Point Covariance P, with lambda parameter, dynamic version static Matrix PointCov(const Matrix& E, const double lambda = 0.0, - bool diagonalDamping = false) { + bool diagonalDamping = false) { if (E.cols() == 2) { Matrix2 P2; ComputePointCovariance<2>(P2, E, lambda, diagonalDamping); @@ -339,8 +346,9 @@ public: * Dynamic version */ static SymmetricBlockMatrix SchurComplement(const FBlocks& Fblocks, - const Matrix& E, const Vector& b, const double lambda = 0.0, - bool diagonalDamping = false) { + const Matrix& E, const Vector& b, + const double lambda = 0.0, + bool diagonalDamping = false) { if (E.cols() == 2) { Matrix2 P; ComputePointCovariance<2>(P, E, lambda, diagonalDamping); @@ -353,17 +361,17 @@ 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. + * 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 (point dimension) - static void UpdateSchurComplement(const FBlocks& Fs, const Matrix& E, - const Eigen::Matrix& P, const Vector& b, - const KeyVector& allKeys, const KeyVector& keys, - /*output ->*/SymmetricBlockMatrix& augmentedHessian) { - - assert(keys.size()==Fs.size()); - assert(keys.size()<=allKeys.size()); + 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, + /*output ->*/ SymmetricBlockMatrix& augmentedHessian) { + assert(keys.size() == Fs.size()); + assert(keys.size() <= allKeys.size()); FastMap KeySlotMap; for (size_t slot = 0; slot < allKeys.size(); slot++) @@ -374,39 +382,49 @@ public: // g = F' * (b - E * P * E' * b) // a single point is observed in m cameras - size_t m = Fs.size(); // cameras observing current point - size_t M = (augmentedHessian.rows() - 1) / D; // all cameras in the group - assert(allKeys.size()==M); + size_t m = Fs.size(); // cameras observing current point + size_t M = (augmentedHessian.rows() - 1) / D; // all cameras in the group + assert(allKeys.size() == M); // Blockwise Schur complement - for (size_t i = 0; i < m; i++) { // for each camera in the current factor + for (size_t i = 0; i < m; i++) { // for each camera in the current factor const MatrixZD& Fi = Fs[i]; const auto FiT = Fi.transpose(); - const Eigen::Matrix Ei_P = E.template block( - ZDim * i, 0) * P; + const Eigen::Matrix Ei_P = + E.template block(ZDim * i, 0) * P; // D = (DxZDim) * (ZDim) // allKeys are the list of all camera keys in the group, e.g, (1,3,4,5,7) - // we should map those to a slot in the local (grouped) hessian (0,1,2,3,4) - // Key cameraKey_i = this->keys_[i]; + // we should map those to a slot in the local (grouped) hessian + // (0,1,2,3,4) Key cameraKey_i = this->keys_[i]; DenseIndex aug_i = KeySlotMap.at(keys[i]); // information vector - store previous vector // vectorBlock = augmentedHessian(aug_i, aug_m).knownOffDiagonal(); // add contribution of current factor - augmentedHessian.updateOffDiagonalBlock(aug_i, M, - FiT * b.segment(ZDim * i) // F' * b - - FiT * (Ei_P * (E.transpose() * b))); // D = (DxZDim) * (ZDimx3) * (N*ZDimm) * (ZDimm x 1) + augmentedHessian.updateOffDiagonalBlock( + aug_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) ) // add contribution of current factor - // TODO(gareth): Eigen doesn't let us pass the expression. Call eval() for now... - augmentedHessian.updateDiagonalBlock(aug_i, - ((FiT * (Fi - Ei_P * E.template block(ZDim * i, 0).transpose() * Fi))).eval()); + // TODO(gareth): Eigen doesn't let us pass the expression. Call eval() for + // now... + augmentedHessian.updateDiagonalBlock( + aug_i, + ((FiT * + (Fi - + Ei_P * E.template block(ZDim * i, 0).transpose() * Fi))) + .eval()); // upper triangular part of the hessian - for (size_t j = i + 1; j < m; j++) { // for each camera + for (size_t j = i + 1; j < m; j++) { // for each camera const MatrixZD& Fj = Fs[j]; DenseIndex aug_j = KeySlotMap.at(keys[j]); @@ -415,39 +433,38 @@ public: // off diagonal block - store previous block // matrixBlock = augmentedHessian(aug_i, aug_j).knownOffDiagonal(); // add contribution of current factor - augmentedHessian.updateOffDiagonalBlock(aug_i, aug_j, - -FiT * (Ei_P * E.template block(ZDim * j, 0).transpose() * Fj)); + augmentedHessian.updateOffDiagonalBlock( + aug_i, aug_j, + -FiT * (Ei_P * E.template block(ZDim * j, 0).transpose() * + Fj)); } - } // end of for over cameras + } // end of for over cameras augmentedHessian.diagonalBlock(M)(0, 0) += b.squaredNorm(); } -private: - + private: /// Serialization function friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int /*version*/) { - ar & (*this); + template + void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + ar&(*this); } -public: + public: GTSAM_MAKE_ALIGNED_OPERATOR_NEW }; -template +template const int CameraSet::D; -template +template const int CameraSet::ZDim; -template -struct traits > : public Testable > { -}; +template +struct traits> : public Testable> {}; -template -struct traits > : public Testable > { -}; +template +struct traits> : public Testable> {}; -} // \ namespace gtsam +} // namespace gtsam diff --git a/gtsam/inference/tests/testOrdering.cpp b/gtsam/inference/tests/testOrdering.cpp index 0ec120fe1..761c330b4 100644 --- a/gtsam/inference/tests/testOrdering.cpp +++ b/gtsam/inference/tests/testOrdering.cpp @@ -44,7 +44,7 @@ TEST(Ordering, constrained_ordering) { // unconstrained version { Ordering actual = Ordering::Colamd(symbolicGraph); - Ordering expected = Ordering({0, 1, 2, 3, 4, 5}); + Ordering expected{0, 1, 2, 3, 4, 5}; EXPECT(assert_equal(expected, actual)); } @@ -102,7 +102,7 @@ TEST(Ordering, grouped_constrained_ordering) { constraints[5] = 2; Ordering actual = Ordering::ColamdConstrained(symbolicGraph, constraints); - Ordering expected = {0, 1, 3, 2, 4, 5}; + Ordering expected{0, 1, 3, 2, 4, 5}; EXPECT(assert_equal(expected, actual)); } diff --git a/gtsam/linear/tests/testGaussianFactorGraph.cpp b/gtsam/linear/tests/testGaussianFactorGraph.cpp index ca9b31a1b..ad25f3e59 100644 --- a/gtsam/linear/tests/testGaussianFactorGraph.cpp +++ b/gtsam/linear/tests/testGaussianFactorGraph.cpp @@ -222,7 +222,7 @@ TEST(GaussianFactorGraph, gradient) { VectorValues solution = fg.optimize(); VectorValues actual2 = fg.gradient(solution); EXPECT(assert_equal(VectorValues::Zero(solution), actual2)); - } +} /* ************************************************************************* */ TEST(GaussianFactorGraph, transposeMultiplication) { diff --git a/gtsam/symbolic/tests/testSymbolicEliminationTree.cpp b/gtsam/symbolic/tests/testSymbolicEliminationTree.cpp index 4998cb9f1..ce5279d7b 100644 --- a/gtsam/symbolic/tests/testSymbolicEliminationTree.cpp +++ b/gtsam/symbolic/tests/testSymbolicEliminationTree.cpp @@ -96,8 +96,7 @@ TEST(EliminationTree, Create) { EliminationTreeTester::buildHardcodedTree(simpleTestGraph1); // Build from factor graph - Ordering order; - order += 0, 1, 2, 3, 4; + const Ordering order{0, 1, 2, 3, 4}; SymbolicEliminationTree actual(simpleTestGraph1, order); CHECK(assert_equal(expected, actual));