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

make check const method

release/4.3a0
Frank Dellaert 2025-01-25 15:31:40 -05:00
parent c60d257e80
commit d3cd876cf9
3 changed files with 32 additions and 35 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -23,9 +23,9 @@
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);
}
@ -34,8 +34,8 @@ namespace gtsam {
JacobianFactor::JacobianFactor(const KEYS& keys,
const VerticalBlockMatrix& augmentedMatrix,
const SharedDiagonal& model)
: Base(keys), Ab_(augmentedMatrix) {
checkAndAssignModel(model, augmentedMatrix);
: Base(keys), Ab_(augmentedMatrix), model_(model) {
checkAb(model, augmentedMatrix);
}
/* ************************************************************************* */
@ -43,33 +43,8 @@ namespace gtsam {
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())
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.");
// Take noise model
model_ = model;
: Base(keys), Ab_(std::move(augmentedMatrix)), model_(model) {
checkAb(model, Ab_);
}
/* ************************************************************************* */

22
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 {

4
gtsam/linear/JacobianFactor.h
View File

@ -403,8 +403,8 @@ namespace gtsam {
void fillTerms(const TERMS& terms, const Vector& b, const SharedDiagonal& noiseModel);
/// Common code between VerticalBlockMatrix constructors
void checkAndAssignModel(const SharedDiagonal& model,
const VerticalBlockMatrix& augmentedMatrix);
void checkAb(const SharedDiagonal& model,
const VerticalBlockMatrix& augmentedMatrix) const;
private: