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>
JacobianFactor::JacobianFactor(
const KEYS& keys, const VerticalBlockMatrix& augmentedMatrix, const SharedDiagonal& model) :
Base(keys), Ab_(augmentedMatrix)
{
template <typename KEYS>
JacobianFactor::JacobianFactor(const KEYS& keys,
const VerticalBlockMatrix& 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
if(model && (DenseIndex)model->dim() != augmentedMatrix.rows())
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)
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.");
"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)
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.");
"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.");
// Take noise model
model_ = model;

22
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,6 +402,10 @@ namespace gtsam {
template<typename TERMS>
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:
/**