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;
  }
}

}