diff --git a/gtsam/linear/GaussianConditional-inl.h b/gtsam/linear/GaussianConditional-inl.h index fe5b1e0d7..2756b690d 100644 --- a/gtsam/linear/GaussianConditional-inl.h +++ b/gtsam/linear/GaussianConditional-inl.h @@ -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 + GaussianConditional::GaussianConditional( + const KEYS& keys, size_t nrFrontals, VerticalBlockMatrix&& augmentedMatrix, const SharedDiagonal& sigmas) : + BaseFactor(keys, std::move(augmentedMatrix), sigmas), BaseConditional(nrFrontals) {} + } // gtsam diff --git a/gtsam/linear/GaussianConditional.h b/gtsam/linear/GaussianConditional.h index 14b1ce87f..d71119d6a 100644 --- a/gtsam/linear/GaussianConditional.h +++ b/gtsam/linear/GaussianConditional.h @@ -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 - 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 + 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 + 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, diff --git a/gtsam/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp index 1172dc281..701e79f41 100644 --- a/gtsam/linear/HessianFactor.cpp +++ b/gtsam/linear/HessianFactor.cpp @@ -470,7 +470,7 @@ std::shared_ptr HessianFactor::eliminateCholesky(const Orde // TODO(frank): pre-allocate GaussianConditional and write into it const VerticalBlockMatrix Ab = info_.split(nFrontals); - conditional = std::make_shared(keys_, nFrontals, Ab); + conditional = std::make_shared(keys_, nFrontals, std::move(Ab)); // Erase the eliminated keys in this factor keys_.erase(begin(), begin() + nFrontals);