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

Merge pull request #2003 from borglab/feature/move_constructor 

move constructors
release/4.3a0
Frank Dellaert 2025-01-30 00:36:09 -05:00 committed by GitHub
parent bb0c70b482 003730f844
commit 588a20fc87
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
9 changed files with 168 additions and 82 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
gtsam/base/SymmetricBlockMatrix.h
View File

@ -256,11 +256,16 @@ namespace gtsam {
full().triangularView<Eigen::Upper>() = xpr.template triangularView<Eigen::Upper>();
}
/// Set the entire active matrix zero.
/// Set the entire *active* matrix zero.
void setZero() {
full().triangularView<Eigen::Upper>().setZero();
}
/// Set entire matrix zero.
void setAllZero() {
matrix_.setZero();
}
/// Negate the entire active matrix.
void negate();

43
gtsam/base/VerticalBlockMatrix.h
View File

@ -57,13 +57,46 @@ namespace gtsam {
DenseIndex blockStart_; ///< Changes apparent matrix view, see main class comment.
public:
/** Construct an empty VerticalBlockMatrix */
VerticalBlockMatrix() :
rowStart_(0), rowEnd_(0), blockStart_(0)
{
VerticalBlockMatrix() : rowStart_(0), rowEnd_(0), blockStart_(0) {
variableColOffsets_.push_back(0);
assertInvariants();
}
// Destructor
~VerticalBlockMatrix() = default;
// Copy constructor (default)
VerticalBlockMatrix(const VerticalBlockMatrix& other) = default;
// Copy assignment operator (default)
VerticalBlockMatrix& operator=(const VerticalBlockMatrix& other) = default;
// Move constructor
VerticalBlockMatrix(VerticalBlockMatrix&& other) noexcept
: matrix_(std::move(other.matrix_)),
variableColOffsets_(std::move(other.variableColOffsets_)),
rowStart_(other.rowStart_),
rowEnd_(other.rowEnd_),
blockStart_(other.blockStart_) {
other.rowStart_ = 0;
other.rowEnd_ = 0;
other.blockStart_ = 0;
}
// Move assignment operator
VerticalBlockMatrix& operator=(VerticalBlockMatrix&& other) noexcept {
if (this != &other) {
matrix_ = std::move(other.matrix_);
variableColOffsets_ = std::move(other.variableColOffsets_);
rowStart_ = other.rowStart_;
rowEnd_ = other.rowEnd_;
blockStart_ = other.blockStart_;
other.rowStart_ = 0;
other.rowEnd_ = 0;
other.blockStart_ = 0;
}
return *this;
}
/** Construct from a container of the sizes of each vertical block. */

6
gtsam/linear/GaussianConditional-inl.h
View File

@ -33,4 +33,10 @@ namespace gtsam {
const KEYS& keys, size_t nrFrontals, const VerticalBlockMatrix& augmentedMatrix, const SharedDiagonal& sigmas) :
BaseFactor(keys, augmentedMatrix, sigmas), BaseConditional(nrFrontals) {}
/* ************************************************************************* */
template<typename KEYS>
GaussianConditional::GaussianConditional(
const KEYS& keys, size_t nrFrontals, VerticalBlockMatrix&& augmentedMatrix, const SharedDiagonal& sigmas) :
BaseFactor(keys, std::move(augmentedMatrix), sigmas), BaseConditional(nrFrontals) {}
} // gtsam

37
gtsam/linear/GaussianConditional.h
View File

@ -75,14 +75,35 @@ namespace gtsam {
size_t nrFrontals, const Vector& d,
const SharedDiagonal& sigmas = SharedDiagonal());
/** Constructor with arbitrary number keys, and where the augmented matrix is given all together
* instead of in block terms. Note that only the active view of the provided augmented matrix
* is used, and that the matrix data is copied into a newly-allocated matrix in the constructed
* factor. */
template<typename KEYS>
GaussianConditional(
const KEYS& keys, size_t nrFrontals, const VerticalBlockMatrix& augmentedMatrix,
const SharedDiagonal& sigmas = SharedDiagonal());
/**
* @brief Constructor with an arbitrary number of keys, where the augmented matrix
* is given all together instead of in block terms.
*
* @tparam KEYS Type of the keys container.
* @param keys Container of keys.
* @param nrFrontals Number of frontal variables.
* @param augmentedMatrix The augmented matrix containing the coefficients.
* @param sigmas Optional noise model (default is an empty SharedDiagonal).
*/
template <typename KEYS>
GaussianConditional(const KEYS& keys, size_t nrFrontals,
const VerticalBlockMatrix& augmentedMatrix,
const SharedDiagonal& sigmas = SharedDiagonal());
/**
* @brief Constructor with an arbitrary number of keys, where the augmented matrix
* is given all together instead of in block terms, using move semantics for efficiency.
*
* @tparam KEYS Type of the keys container.
* @param keys Container of keys.
* @param nrFrontals Number of frontal variables.
* @param augmentedMatrix The augmented matrix containing the coefficients (moved).
* @param sigmas Optional noise model (default is an empty SharedDiagonal).
*/
template <typename KEYS>
GaussianConditional(const KEYS& keys, size_t nrFrontals,
VerticalBlockMatrix&& augmentedMatrix,
const SharedDiagonal& sigmas = SharedDiagonal());
/// Construct from mean `mu` and standard deviation `sigma`.
static GaussianConditional FromMeanAndStddev(Key key, const Vector& mu,

36
gtsam/linear/HessianFactor.cpp
View File

@ -245,7 +245,7 @@ HessianFactor::HessianFactor(const GaussianFactorGraph& factors,
// Form A' * A
gttic(update);
info_.setZero();
info_.setAllZero();
for(const auto& factor: factors)
if (factor)
factor->updateHessian(keys_, &info_);
@ -348,28 +348,26 @@ double HessianFactor::error(const VectorValues& c) const {
/* ************************************************************************* */
void HessianFactor::updateHessian(const KeyVector& infoKeys,
SymmetricBlockMatrix* info) const {
gttic(updateHessian_HessianFactor);
assert(info);
// Apply updates to the upper triangle
DenseIndex nrVariablesInThisFactor = size(), nrBlocksInInfo = info->nBlocks() - 1;
gttic(updateHessian_HessianFactor);
const DenseIndex nrVariablesInThisFactor = size();
vector<DenseIndex> slots(nrVariablesInThisFactor + 1);
for (DenseIndex j = 0; j < nrVariablesInThisFactor; ++j)
slots[j] = Slot(infoKeys, keys_[j]);
slots[nrVariablesInThisFactor] = info->nBlocks() - 1;
// Apply updates to the upper triangle
// Loop over this factor's blocks with indices (i,j)
// For every block (i,j), we determine the block (I,J) in info.
for (DenseIndex j = 0; j <= nrVariablesInThisFactor; ++j) {
const bool rhs = (j == nrVariablesInThisFactor);
const DenseIndex J = rhs ? nrBlocksInInfo : Slot(infoKeys, keys_[j]);
slots[j] = J;
for (DenseIndex i = 0; i <= j; ++i) {
const DenseIndex I = slots[i]; // because i<=j, slots[i] is valid.
if (i == j) {
assert(I == J);
info->updateDiagonalBlock(I, info_.diagonalBlock(i));
} else {
assert(i < j);
assert(I != J);
info->updateOffDiagonalBlock(I, J, info_.aboveDiagonalBlock(i, j));
}
const DenseIndex J = slots[j];
info->updateDiagonalBlock(J, info_.diagonalBlock(j));
for (DenseIndex i = 0; i < j; ++i) {
const DenseIndex I = slots[i];
assert(i < j);
assert(I != J);
info->updateOffDiagonalBlock(I, J, info_.aboveDiagonalBlock(i, j));
}
}
}
@ -470,7 +468,7 @@ std::shared_ptr<GaussianConditional> HessianFactor::eliminateCholesky(const Orde
// TODO(frank): pre-allocate GaussianConditional and write into it
const VerticalBlockMatrix Ab = info_.split(nFrontals);
conditional = std::make_shared<GaussianConditional>(keys_, nFrontals, Ab);
conditional = std::make_shared<GaussianConditional>(keys_, nFrontals, std::move(Ab));
// Erase the eliminated keys in this factor
keys_.erase(begin(), begin() + nFrontals);

42
gtsam/linear/JacobianFactor-inl.h
View File

@ -23,36 +23,28 @@
namespace gtsam {
/* ************************************************************************* */
template<typename TERMS>
JacobianFactor::JacobianFactor(const TERMS&terms, const Vector &b, const SharedDiagonal& model)
{
template <typename TERMS>
JacobianFactor::JacobianFactor(const TERMS& terms, const Vector& b,
const SharedDiagonal& model) {
fillTerms(terms, b, model);
}
/* ************************************************************************* */
template<typename KEYS>
JacobianFactor::JacobianFactor(
const KEYS& keys, const VerticalBlockMatrix& augmentedMatrix, const SharedDiagonal& model) :
Base(keys), Ab_(augmentedMatrix)
{
// Check noise model dimension
if(model && (DenseIndex)model->dim() != augmentedMatrix.rows())
throw InvalidNoiseModel(augmentedMatrix.rows(), model->dim());
template <typename KEYS>
JacobianFactor::JacobianFactor(const KEYS& keys,
const VerticalBlockMatrix& augmentedMatrix,
const SharedDiagonal& model)
: Base(keys), Ab_(augmentedMatrix), model_(model) {
checkAb(model, augmentedMatrix);
}
// Check number of variables
if((DenseIndex)Base::keys_.size() != augmentedMatrix.nBlocks() - 1)
throw std::invalid_argument(
"Error in JacobianFactor constructor input. Number of provided keys plus\n"
"one for the RHS vector must equal the number of provided matrix blocks.");
// Check RHS dimension
if(augmentedMatrix(augmentedMatrix.nBlocks() - 1).cols() != 1)
throw std::invalid_argument(
"Error in JacobianFactor constructor input. The last provided matrix block\n"
"must be the RHS vector, but the last provided block had more than one column.");
// Take noise model
model_ = model;
/* ************************************************************************* */
template <typename KEYS>
JacobianFactor::JacobianFactor(const KEYS& keys,
VerticalBlockMatrix&& augmentedMatrix,
const SharedDiagonal& model)
: Base(keys), Ab_(std::move(augmentedMatrix)), model_(model) {
checkAb(model, Ab_);
}
/* ************************************************************************* */

33
gtsam/linear/JacobianFactor.cpp
View File

@ -112,6 +112,28 @@ JacobianFactor::JacobianFactor(const HessianFactor& factor)
}
}
/* ************************************************************************* */
void JacobianFactor::checkAb(const SharedDiagonal& model,
const VerticalBlockMatrix& augmentedMatrix) const {
// Check noise model dimension
if (model && (DenseIndex)model->dim() != augmentedMatrix.rows())
throw InvalidNoiseModel(augmentedMatrix.rows(), model->dim());
// Check number of variables
if ((DenseIndex)Base::keys_.size() != augmentedMatrix.nBlocks() - 1)
throw std::invalid_argument(
"Error in JacobianFactor constructor input. Number of provided keys "
"plus one for the RHS vector must equal the number of provided "
"matrix blocks.");
// Check RHS dimension
if (augmentedMatrix(augmentedMatrix.nBlocks() - 1).cols() != 1)
throw std::invalid_argument(
"Error in JacobianFactor constructor input. The last provided "
"matrix block must be the RHS vector, but the last provided block "
"had more than one column.");
}
/* ************************************************************************* */
// Helper functions for combine constructor
namespace {
@ -580,16 +602,19 @@ void JacobianFactor::updateHessian(const KeyVector& infoKeys,
// Ab_ is the augmented Jacobian matrix A, and we perform I += A'*A below
DenseIndex n = Ab_.nBlocks() - 1, N = info->nBlocks() - 1;
// Pre-calculate slots
vector<DenseIndex> slots(n + 1);
for (DenseIndex j = 0; j < n; ++j) slots[j] = Slot(infoKeys, keys_[j]);
slots[n] = N;
// Apply updates to the upper triangle
// Loop over blocks of A, including RHS with j==n
vector<DenseIndex> slots(n+1);
for (DenseIndex j = 0; j <= n; ++j) {
Eigen::Block<const Matrix> Ab_j = Ab_(j);
const DenseIndex J = (j == n) ? N : Slot(infoKeys, keys_[j]);
slots[j] = J;
const DenseIndex J = slots[j];
// Fill off-diagonal blocks with Ai'*Aj
for (DenseIndex i = 0; i < j; ++i) {
const DenseIndex I = slots[i]; // because i<j, slots[i] is valid.
const DenseIndex I = slots[i];
info->updateOffDiagonalBlock(I, J, Ab_(i).transpose() * Ab_j);
}
// Fill diagonal block with Aj'*Aj

24
gtsam/linear/JacobianFactor.h
View File

@ -145,13 +145,17 @@ namespace gtsam {
template<typename TERMS>
JacobianFactor(const TERMS& terms, const Vector& b, const SharedDiagonal& model = SharedDiagonal());
/** Constructor with arbitrary number keys, and where the augmented matrix is given all together
* instead of in block terms. Note that only the active view of the provided augmented matrix
* is used, and that the matrix data is copied into a newly-allocated matrix in the constructed
* factor. */
template<typename KEYS>
JacobianFactor(
const KEYS& keys, const VerticalBlockMatrix& augmentedMatrix, const SharedDiagonal& sigmas = SharedDiagonal());
/** Constructor with arbitrary number keys, and where the augmented matrix
* is given all together instead of in block terms.
*/
template <typename KEYS>
JacobianFactor(const KEYS& keys, const VerticalBlockMatrix& augmentedMatrix,
const SharedDiagonal& sigmas = SharedDiagonal());
/** Construct with an rvalue VerticalBlockMatrix, to allow std::move. */
template <typename KEYS>
JacobianFactor(const KEYS& keys, VerticalBlockMatrix&& augmentedMatrix,
const SharedDiagonal& model);
/**
* Build a dense joint factor from all the factors in a factor graph. If a VariableSlots
@ -398,7 +402,11 @@ namespace gtsam {
template<typename TERMS>
void fillTerms(const TERMS& terms, const Vector& b, const SharedDiagonal& noiseModel);
private:
/// Common code between VerticalBlockMatrix constructors
void checkAb(const SharedDiagonal& model,
const VerticalBlockMatrix& augmentedMatrix) const;
private:
/**
* Helper function for public constructors:

22
timing/timeBatch.cpp
View File

@ -29,18 +29,16 @@ int main(int argc, char *argv[]) {
cout << "Loading data..." << endl;
string datasetFile = findExampleDataFile("w10000");
std::pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> data =
load2D(datasetFile);
NonlinearFactorGraph graph = *data.first;
Values initial = *data.second;
auto [graph, initial] = load2D(datasetFile);
graph->addPrior(0, initial->at<Pose2>(0), noiseModel::Unit::Create(3));
cout << "Optimizing..." << endl;
gttic_(Create_optimizer);
LevenbergMarquardtOptimizer optimizer(graph, initial);
LevenbergMarquardtOptimizer optimizer(*graph, *initial);
gttoc_(Create_optimizer);
tictoc_print_();
tictoc_reset_();
double lastError = optimizer.error();
do {
gttic_(Iterate_optimizer);
@ -53,19 +51,19 @@ int main(int argc, char *argv[]) {
} while(!checkConvergence(optimizer.params().relativeErrorTol,
optimizer.params().absoluteErrorTol, optimizer.params().errorTol,
lastError, optimizer.error(), optimizer.params().verbosity));
tictoc_reset_();
// Compute marginals
Marginals marginals(graph, optimizer.values());
int i=0;
for(Key key: initial.keys()) {
gttic_(ConstructMarginals);
Marginals marginals(*graph, optimizer.values());
gttoc_(ConstructMarginals);
for(Key key: initial->keys()) {
gttic_(marginalInformation);
Matrix info = marginals.marginalInformation(key);
gttoc_(marginalInformation);
tictoc_finishedIteration_();
if(i % 1000 == 0)
tictoc_print_();
++i;
}
tictoc_print_();
} catch(std::exception& e) {
cout << e.what() << endl;