115 lines
4.3 KiB
C++
115 lines
4.3 KiB
C++
/* ----------------------------------------------------------------------------
|
|
|
|
* 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
|
|
|
|
* -------------------------------------------------------------------------- */
|
|
|
|
/**
|
|
* @file VariableSlots.h
|
|
* @brief VariableSlots describes the structure of a combined factor in terms of where each block comes from in the source factors.
|
|
* @author Richard Roberts
|
|
* @created Oct 4, 2010
|
|
*/
|
|
|
|
#pragma once
|
|
|
|
#include <gtsam/base/types.h>
|
|
#include <gtsam/base/Testable.h>
|
|
#include <gtsam/base/FastMap.h>
|
|
|
|
#include <iostream>
|
|
|
|
#include <boost/foreach.hpp>
|
|
#include <boost/tuple/tuple.hpp>
|
|
|
|
#include <map>
|
|
#include <vector>
|
|
#include <string>
|
|
|
|
namespace gtsam {
|
|
|
|
/**
|
|
* A combined factor is assembled as one block of rows for each component
|
|
* factor. In each row-block (factor), some of the column-blocks (variables)
|
|
* may be empty since factors involving different sets of variables are
|
|
* interleaved.
|
|
*
|
|
* VariableSlots describes the 2D block structure of the combined factor. It
|
|
* is essentially a map<Index, vector<size_t> >. The Index is the real
|
|
* variable index of the combined factor slot. The vector<size_t> tells, for
|
|
* each row-block (factor), which column-block (variable slot) from the
|
|
* component factor appears in this block of the combined factor.
|
|
*
|
|
* As an example, if the combined factor contains variables 1, 3, and 5, then
|
|
* "variableSlots[3][2] == 0" indicates that column-block 1 (corresponding to
|
|
* variable index 3), row-block 2 (also meaning component factor 2), comes from
|
|
* column-block 0 of component factor 2.
|
|
*
|
|
* Note that in the case of GaussianFactor, the rows of the combined factor are
|
|
* further sorted so that the column-block position of the first structural
|
|
* non-zero increases monotonically through the rows. This additional process
|
|
* is not performed by this class.
|
|
*/
|
|
|
|
class VariableSlots : public FastMap<Index, std::vector<Index> >, public Testable<VariableSlots> {
|
|
|
|
public:
|
|
|
|
typedef FastMap<Index, std::vector<Index> > Base;
|
|
|
|
/**
|
|
* Constructor from a set of factors to be combined. Sorts the variables
|
|
* and keeps track of which variable from each factor ends up in each slot
|
|
* of the combined factor, as described in the class comment.
|
|
*/
|
|
template<class FG>
|
|
VariableSlots(const FG& factorGraph);
|
|
|
|
/** print */
|
|
void print(const std::string& str = "VariableSlots: ") const;
|
|
|
|
/** equals */
|
|
bool equals(const VariableSlots& rhs, double tol = 0.0) const;
|
|
};
|
|
|
|
/* ************************************************************************* */
|
|
template<class FG>
|
|
VariableSlots::VariableSlots(const FG& factorGraph) {
|
|
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;
|
|
BOOST_FOREACH(const typename FG::sharedFactor& factor, factorGraph) {
|
|
assert(factor);
|
|
Index factorVarSlot = 0;
|
|
BOOST_FOREACH(const Index 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.
|
|
iterator thisVarSlots; bool inserted;
|
|
boost::tie(thisVarSlots, inserted) = this->insert(make_pair(involvedVariable, std::vector<Index>()));
|
|
if(inserted)
|
|
thisVarSlots->second.resize(factorGraph.size(), std::numeric_limits<Index>::max());
|
|
thisVarSlots->second[jointFactorPos] = factorVarSlot;
|
|
if(debug) std::cout << " var " << involvedVariable << " rowblock " << jointFactorPos << " comes from factor's slot " << factorVarSlot << std::endl;
|
|
++ factorVarSlot;
|
|
}
|
|
++ jointFactorPos;
|
|
}
|
|
}
|
|
|
|
}
|