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

Address review comments

release/4.3a0
Frank Dellaert 2023-01-08 21:09:31 -08:00
parent 73754f271a
commit 15802e58f9
5 changed files with 154 additions and 139 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
gtsam/base/tests/testVerticalBlockMatrix.cpp
View File

@ -24,8 +24,7 @@
using namespace gtsam; using namespace gtsam;
const std::list<size_t> L{3, 2, 1}; const std::vector<size_t> dimensions{3, 2, 1};
const std::vector<size_t> dimensions(L.begin(), L.end());
//***************************************************************************** //*****************************************************************************
TEST(VerticalBlockMatrix, Constructor1) { TEST(VerticalBlockMatrix, Constructor1) {

107
gtsam/geometry/CameraSet.h
View File

@ -18,12 +18,13 @@
#pragma once #pragma once
#include <gtsam/geometry/Point3.h>
#include <gtsam/geometry/CalibratedCamera.h> // for Cheirality exception
#include <gtsam/base/Testable.h>
#include <gtsam/base/SymmetricBlockMatrix.h>
#include <gtsam/base/FastMap.h> #include <gtsam/base/FastMap.h>
#include <gtsam/base/SymmetricBlockMatrix.h>
#include <gtsam/base/Testable.h>
#include <gtsam/geometry/CalibratedCamera.h> // for Cheirality exception
#include <gtsam/geometry/Point3.h>
#include <gtsam/inference/Key.h> #include <gtsam/inference/Key.h>
#include <vector> #include <vector>
namespace gtsam { namespace gtsam {
@ -33,7 +34,6 @@ namespace gtsam {
*/ */
template <class CAMERA> template <class CAMERA>
class CameraSet : public std::vector<CAMERA, Eigen::aligned_allocator<CAMERA>> { class CameraSet : public std::vector<CAMERA, Eigen::aligned_allocator<CAMERA>> {
protected: protected:
using Base = std::vector<CAMERA, typename Eigen::aligned_allocator<CAMERA>>; using Base = std::vector<CAMERA, typename Eigen::aligned_allocator<CAMERA>>;
@ -89,12 +89,10 @@ public:
/// equals /// equals
bool equals(const CameraSet& p, double tol = 1e-9) const { bool equals(const CameraSet& p, double tol = 1e-9) const {
if (this->size() != p.size()) if (this->size() != p.size()) return false;
return false;
bool camerasAreEqual = true; bool camerasAreEqual = true;
for (size_t i = 0; i < this->size(); i++) { for (size_t i = 0; i < this->size(); i++) {
if (this->at(i).equals(p.at(i), tol) == false) if (this->at(i).equals(p.at(i), tol) == false) camerasAreEqual = false;
camerasAreEqual = false;
break; break;
} }
return camerasAreEqual; return camerasAreEqual;
@ -110,7 +108,6 @@ public:
ZVector project2(const POINT& point, // ZVector project2(const POINT& point, //
boost::optional<FBlocks&> Fs = boost::none, // boost::optional<FBlocks&> Fs = boost::none, //
boost::optional<Matrix&> E = boost::none) const { boost::optional<Matrix&> E = boost::none) const {
static const int N = FixedDimension<POINT>::value; static const int N = FixedDimension<POINT>::value;
// Allocate result // Allocate result
@ -158,7 +155,8 @@ public:
// a single point is observed in m cameras // a single point is observed in m cameras
size_t m = Fs.size(); 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; size_t M1 = ND * m + 1;
std::vector<DenseIndex> dims(m + 1); // this also includes the b term std::vector<DenseIndex> dims(m + 1); // this also includes the b term
std::fill(dims.begin(), dims.end() - 1, ND); std::fill(dims.begin(), dims.end() - 1, ND);
@ -174,20 +172,28 @@ public:
E.block(ZDim * i, 0, ZDim, N) * P; E.block(ZDim * i, 0, ZDim, N) * P;
// D = (Dx2) * ZDim // D = (Dx2) * ZDim
augmentedHessian.setOffDiagonalBlock(i, m, FiT * b.segment<ZDim>(ZDim * i) // F' * b augmentedHessian.setOffDiagonalBlock(
- FiT * (Ei_P * (E.transpose() * b))); // D = (DxZDim) * (ZDimx3) * (N*ZDimm) * (ZDimm x 1) i, m,
FiT * b.segment<ZDim>(ZDim * i) // F' * b
-
FiT *
(Ei_P *
(E.transpose() *
b))); // D = (DxZDim) * (ZDimx3) * (N*ZDimm) * (ZDimm x 1)
// (DxD) = (DxZDim) * ( (ZDimxD) - (ZDimx3) * (3xZDim) * (ZDimxD) ) // (DxD) = (DxZDim) * ( (ZDimxD) - (ZDimx3) * (3xZDim) * (ZDimxD) )
augmentedHessian.setDiagonalBlock(i, FiT augmentedHessian.setDiagonalBlock(
* (Fi - Ei_P * E.block(ZDim * i, 0, ZDim, N).transpose() * Fi)); i,
FiT * (Fi - Ei_P * E.block(ZDim * i, 0, ZDim, N).transpose() * Fi));
// upper triangular part of the hessian // 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<double, ZDim, ND>& Fj = Fs[j]; const Eigen::Matrix<double, ZDim, ND>& Fj = Fs[j];
// (DxD) = (Dx2) * ( (2x2) * (2xD) ) // (DxD) = (Dx2) * ( (2x2) * (2xD) )
augmentedHessian.setOffDiagonalBlock(i, j, -FiT augmentedHessian.setOffDiagonalBlock(
* (Ei_P * E.block(ZDim * j, 0, ZDim, N).transpose() * Fj)); i, j,
-FiT * (Ei_P * E.block(ZDim * j, 0, ZDim, N).transpose() * Fj));
} }
} // end of for over cameras } // end of for over cameras
@ -298,16 +304,17 @@ public:
* Fixed size version * Fixed size version
*/ */
template <int N> // N = 2 or 3 template <int N> // N = 2 or 3
static SymmetricBlockMatrix SchurComplement(const FBlocks& Fs, static SymmetricBlockMatrix SchurComplement(
const Matrix& E, const Eigen::Matrix<double, N, N>& P, const Vector& b) { const FBlocks& Fs, const Matrix& E, const Eigen::Matrix<double, N, N>& P,
const Vector& b) {
return SchurComplement<N, D>(Fs, E, P, b); return SchurComplement<N, D>(Fs, E, P, b);
} }
/// Computes Point Covariance P, with lambda parameter /// Computes Point Covariance P, with lambda parameter
template <int N> // N = 2 or 3 (point dimension) template <int N> // N = 2 or 3 (point dimension)
static void ComputePointCovariance(Eigen::Matrix<double, N, N>& P, static void ComputePointCovariance(Eigen::Matrix<double, N, N>& P,
const Matrix& E, double lambda, bool diagonalDamping = false) { const Matrix& E, double lambda,
bool diagonalDamping = false) {
Matrix EtE = E.transpose() * E; Matrix EtE = E.transpose() * E;
if (diagonalDamping) { // diagonal of the hessian if (diagonalDamping) { // diagonal of the hessian
@ -339,7 +346,8 @@ public:
* Dynamic version * Dynamic version
*/ */
static SymmetricBlockMatrix SchurComplement(const FBlocks& Fblocks, static SymmetricBlockMatrix SchurComplement(const FBlocks& Fblocks,
const Matrix& E, const Vector& b, const double lambda = 0.0, const Matrix& E, const Vector& b,
const double lambda = 0.0,
bool diagonalDamping = false) { bool diagonalDamping = false) {
if (E.cols() == 2) { if (E.cols() == 2) {
Matrix2 P; Matrix2 P;
@ -353,15 +361,15 @@ public:
} }
/** /**
* Applies Schur complement (exploiting block structure) to get a smart factor on cameras, * Applies Schur complement (exploiting block structure) to get a smart factor
* and adds the contribution of the smart factor to a pre-allocated augmented Hessian. * on cameras, and adds the contribution of the smart factor to a
* pre-allocated augmented Hessian.
*/ */
template <int N> // N = 2 or 3 (point dimension) template <int N> // N = 2 or 3 (point dimension)
static void UpdateSchurComplement(const FBlocks& Fs, const Matrix& E, static void UpdateSchurComplement(
const Eigen::Matrix<double, N, N>& P, const Vector& b, const FBlocks& Fs, const Matrix& E, const Eigen::Matrix<double, N, N>& P,
const KeyVector& allKeys, const KeyVector& keys, const Vector& b, const KeyVector& allKeys, const KeyVector& keys,
/*output ->*/ SymmetricBlockMatrix& augmentedHessian) { /*output ->*/ SymmetricBlockMatrix& augmentedHessian) {
assert(keys.size() == Fs.size()); assert(keys.size() == Fs.size());
assert(keys.size() <= allKeys.size()); assert(keys.size() <= allKeys.size());
@ -383,27 +391,37 @@ public:
const MatrixZD& Fi = Fs[i]; const MatrixZD& Fi = Fs[i];
const auto FiT = Fi.transpose(); const auto FiT = Fi.transpose();
const Eigen::Matrix<double, 2, N> Ei_P = E.template block<ZDim, N>( const Eigen::Matrix<double, 2, N> Ei_P =
ZDim * i, 0) * P; E.template block<ZDim, N>(ZDim * i, 0) * P;
// D = (DxZDim) * (ZDim) // D = (DxZDim) * (ZDim)
// allKeys are the list of all camera keys in the group, e.g, (1,3,4,5,7) // 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) // we should map those to a slot in the local (grouped) hessian
// Key cameraKey_i = this->keys_[i]; // (0,1,2,3,4) Key cameraKey_i = this->keys_[i];
DenseIndex aug_i = KeySlotMap.at(keys[i]); DenseIndex aug_i = KeySlotMap.at(keys[i]);
// information vector - store previous vector // information vector - store previous vector
// vectorBlock = augmentedHessian(aug_i, aug_m).knownOffDiagonal(); // vectorBlock = augmentedHessian(aug_i, aug_m).knownOffDiagonal();
// add contribution of current factor // add contribution of current factor
augmentedHessian.updateOffDiagonalBlock(aug_i, M, augmentedHessian.updateOffDiagonalBlock(
aug_i, M,
FiT * b.segment<ZDim>(ZDim * i) // F' * b FiT * b.segment<ZDim>(ZDim * i) // F' * b
- FiT * (Ei_P * (E.transpose() * b))); // D = (DxZDim) * (ZDimx3) * (N*ZDimm) * (ZDimm x 1) -
FiT *
(Ei_P *
(E.transpose() *
b))); // D = (DxZDim) * (ZDimx3) * (N*ZDimm) * (ZDimm x 1)
// (DxD) += (DxZDim) * ( (ZDimxD) - (ZDimx3) * (3xZDim) * (ZDimxD) ) // (DxD) += (DxZDim) * ( (ZDimxD) - (ZDimx3) * (3xZDim) * (ZDimxD) )
// add contribution of current factor // add contribution of current factor
// TODO(gareth): Eigen doesn't let us pass the expression. Call eval() for now... // TODO(gareth): Eigen doesn't let us pass the expression. Call eval() for
augmentedHessian.updateDiagonalBlock(aug_i, // now...
((FiT * (Fi - Ei_P * E.template block<ZDim, N>(ZDim * i, 0).transpose() * Fi))).eval()); augmentedHessian.updateDiagonalBlock(
aug_i,
((FiT *
(Fi -
Ei_P * E.template block<ZDim, N>(ZDim * i, 0).transpose() * Fi)))
.eval());
// upper triangular part of the hessian // 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
@ -415,8 +433,10 @@ public:
// off diagonal block - store previous block // off diagonal block - store previous block
// matrixBlock = augmentedHessian(aug_i, aug_j).knownOffDiagonal(); // matrixBlock = augmentedHessian(aug_i, aug_j).knownOffDiagonal();
// add contribution of current factor // add contribution of current factor
augmentedHessian.updateOffDiagonalBlock(aug_i, aug_j, augmentedHessian.updateOffDiagonalBlock(
-FiT * (Ei_P * E.template block<ZDim, N>(ZDim * j, 0).transpose() * Fj)); aug_i, aug_j,
-FiT * (Ei_P * E.template block<ZDim, N>(ZDim * j, 0).transpose() *
Fj));
} }
} // end of for over cameras } // end of for over cameras
@ -424,7 +444,6 @@ public:
} }
private: private:
/// Serialization function /// Serialization function
friend class boost::serialization::access; friend class boost::serialization::access;
template <class ARCHIVE> template <class ARCHIVE>
@ -443,11 +462,9 @@ template<class CAMERA>
const int CameraSet<CAMERA>::ZDim; const int CameraSet<CAMERA>::ZDim;
template <class CAMERA> template <class CAMERA>
struct traits<CameraSet<CAMERA> > : public Testable<CameraSet<CAMERA> > { struct traits<CameraSet<CAMERA>> : public Testable<CameraSet<CAMERA>> {};
};
template <class CAMERA> template <class CAMERA>
struct traits<const CameraSet<CAMERA> > : public Testable<CameraSet<CAMERA> > { struct traits<const CameraSet<CAMERA>> : public Testable<CameraSet<CAMERA>> {};
};
} // \ namespace gtsam } // namespace gtsam

4
gtsam/inference/tests/testOrdering.cpp
View File

@ -44,7 +44,7 @@ TEST(Ordering, constrained_ordering) {
// unconstrained version // unconstrained version
{ {
Ordering actual = Ordering::Colamd(symbolicGraph); 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)); EXPECT(assert_equal(expected, actual));
} }
@ -102,7 +102,7 @@ TEST(Ordering, grouped_constrained_ordering) {
constraints[5] = 2; constraints[5] = 2;
Ordering actual = Ordering::ColamdConstrained(symbolicGraph, constraints); 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)); EXPECT(assert_equal(expected, actual));
} }

3
gtsam/symbolic/tests/testSymbolicEliminationTree.cpp
View File

@ -96,8 +96,7 @@ TEST(EliminationTree, Create) {
EliminationTreeTester::buildHardcodedTree(simpleTestGraph1); EliminationTreeTester::buildHardcodedTree(simpleTestGraph1);
// Build from factor graph // Build from factor graph
Ordering order; const Ordering order{0, 1, 2, 3, 4};
order += 0, 1, 2, 3, 4;
SymbolicEliminationTree actual(simpleTestGraph1, order); SymbolicEliminationTree actual(simpleTestGraph1, order);
CHECK(assert_equal(expected, actual)); CHECK(assert_equal(expected, actual));