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

rvalue constructor

release/4.3a0
Frank Dellaert 2025-01-25 15:10:30 -05:00
parent 3302ad46c8
commit be5d30c57a
2 changed files with 44 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

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

22
gtsam/linear/JacobianFactor.h
View File

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