Merge pull request #2003 from borglab/feature/move_constructor

move constructors

gtsam/base/SymmetricBlockMatrix.h

@@ -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();
 

gtsam/base/VerticalBlockMatrix.h

@@ -57,13 +57,46 @@ namespace gtsam {
     DenseIndex blockStart_; ///< Changes apparent matrix view, see main class comment.
 
   public:
-
     /** Construct an empty VerticalBlockMatrix */
-    VerticalBlockMatrix() :
+    VerticalBlockMatrix() : rowStart_(0), rowEnd_(0), blockStart_(0) {
-      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. */

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<typename KEYS>
+  GaussianConditional::GaussianConditional(
+    const KEYS& keys, size_t nrFrontals, VerticalBlockMatrix&& augmentedMatrix, const SharedDiagonal& sigmas) :
+  BaseFactor(keys, std::move(augmentedMatrix), sigmas), BaseConditional(nrFrontals) {}
+
 } // gtsam

gtsam/linear/GaussianConditional.h

@@ -75,13 +75,34 @@ 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
+     * @brief Constructor with an arbitrary number of keys, where the augmented matrix
-     *  is used, and that the matrix data is copied into a newly-allocated matrix in the constructed
+     * is given all together instead of in block terms.
-     *  factor. */
+     *
-    template<typename KEYS>
+     * @tparam KEYS Type of the keys container.
-    GaussianConditional(
+     * @param keys Container of keys.
-      const KEYS& keys, size_t nrFrontals, const VerticalBlockMatrix& augmentedMatrix,
+     * @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`.

gtsam/linear/HessianFactor.cpp

@@ -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,30 +348,28 @@ 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
+  gttic(updateHessian_HessianFactor);
-  DenseIndex nrVariablesInThisFactor = size(), nrBlocksInInfo = info->nBlocks() - 1;
+  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 = slots[j];
-    const DenseIndex J = rhs ? nrBlocksInInfo : Slot(infoKeys, keys_[j]);
+    info->updateDiagonalBlock(J, info_.diagonalBlock(j));
-    slots[j] = J;
+    for (DenseIndex i = 0; i < j; ++i) {
-    for (DenseIndex i = 0; i <= j; ++i) {
+      const DenseIndex I = slots[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));
     }
   }
-  }
 }
 
 /* ************************************************************************* */
@@ -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);

gtsam/linear/JacobianFactor-inl.h

@@ -23,36 +23,28 @@
 namespace gtsam {
 
   /* ************************************************************************* */
-  template<typename TERMS>
+  template <typename TERMS>
-  JacobianFactor::JacobianFactor(const TERMS&terms, const Vector &b, const SharedDiagonal& model)
+  JacobianFactor::JacobianFactor(const TERMS& terms, const Vector& b,
-  {
+                                const SharedDiagonal& model) {
     fillTerms(terms, b, model);
   }
 
   /* ************************************************************************* */
-  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), model_(model) {
-    // Check noise model dimension
+    checkAb(model, augmentedMatrix);
-    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)
+  template <typename KEYS>
-      throw std::invalid_argument(
+  JacobianFactor::JacobianFactor(const KEYS& keys,
-      "Error in JacobianFactor constructor input.  Number of provided keys plus\n"
+                                 VerticalBlockMatrix&& augmentedMatrix,
-      "one for the RHS vector must equal the number of provided matrix blocks.");
+                                 const SharedDiagonal& model)
-
+      : Base(keys), Ab_(std::move(augmentedMatrix)), model_(model) {
-    // Check RHS dimension
+    checkAb(model, Ab_);
-    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;
   }
 
   /* ************************************************************************* */

gtsam/linear/JacobianFactor.cpp

@@ -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]);
+      const DenseIndex J = slots[j];
-      slots[j] = 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

gtsam/linear/JacobianFactor.h

@@ -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
+    /** 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
@@ -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 checkAb(const SharedDiagonal& model,
+                 const VerticalBlockMatrix& augmentedMatrix) const;
+
    private:
 
     /**

timing/timeBatch.cpp

@@ -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 =
+    auto [graph, initial] = load2D(datasetFile);
-      load2D(datasetFile);
+    graph->addPrior(0, initial->at<Pose2>(0), noiseModel::Unit::Create(3));
-
-    NonlinearFactorGraph graph = *data.first;
-    Values initial = *data.second;
 
     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());
+    gttic_(ConstructMarginals);
-    int i=0;
+    Marginals marginals(*graph, optimizer.values());
-    for(Key key: initial.keys()) {
+    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;