VariableSlots.h

Go to the documentation of this file.

/* ----------------------------------------------------------------------------
 
 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
 
 * See LICENSE for the license information
 
 * -------------------------------------------------------------------------- */
 
#pragma once
 
#include <gtsam/global_includes.h>
#include <gtsam/base/FastMap.h>
#include <gtsam/base/FastVector.h>
#include <gtsam/base/timing.h>
#include <gtsam/base/Testable.h>
 
 
#include <iostream>
#include <string>
 
namespace gtsam {
 
class VariableSlots : public FastMap<Key, FastVector<size_t> > {
 
public:
 
  typedef FastMap<Key, FastVector<size_t> > Base;
  GTSAM_EXPORT static const size_t Empty;
 
 
  template<class FG>
  VariableSlots(const FG& factorGraph);
 
 
 
  GTSAM_EXPORT void print(const std::string& str = "VariableSlots: ") const;
 
  GTSAM_EXPORT bool equals(const VariableSlots& rhs, double tol = 0.0) const;
 
};
 
template<> struct traits<VariableSlots> : public Testable<VariableSlots> {};
 
/* ************************************************************************* */
template<class FG>
VariableSlots::VariableSlots(const FG& factorGraph)
{
  gttic(VariableSlots_constructor);
  static const bool debug = false;
 
  // Compute a mapping (called variableSlots) *from* each involved
  // variable that will be in the new joint factor *to* the slot in each
  // removed factor in which that variable appears.  For each variable,
  // this is stored as a vector of slot numbers, stored in order of the
  // removed factors.  The slot number is the max integer value if the
  // factor does not involve that variable.
  size_t jointFactorPos = 0;
  for(const typename FG::sharedFactor& factor: factorGraph) {
    if (!factor) {
      continue;
    }
    size_t factorVarSlot = 0;
    for(const Key involvedVariable: *factor) {
      // Set the slot in this factor for this variable.  If the
      // variable was not already discovered, create an array for it
      // that we'll fill with the slot indices for each factor that
      // we're combining.  Initially we put the max integer value in
      // the array entry for each factor that will indicate the factor
      // does not involve the variable.
      auto [thisVarSlots, inserted] = this->insert({involvedVariable, FastVector<size_t>()});
      if(inserted)
        thisVarSlots->second.resize(factorGraph.nrFactors(), Empty);
      thisVarSlots->second[jointFactorPos] = factorVarSlot;
      if(debug) std::cout << "  var " << involvedVariable << " rowblock " << jointFactorPos << " comes from factor's slot " << factorVarSlot << std::endl;
      ++ factorVarSlot;
    }
    ++ jointFactorPos;
  }
}
 
}