gtsam  4.1.0
gtsam
FactorGraph-inst.h
1 /* ----------------------------------------------------------------------------
2 
3  * GTSAM Copyright 2010, Georgia Tech Research Corporation,
4  * Atlanta, Georgia 30332-0415
5  * All Rights Reserved
6  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7 
8  * See LICENSE for the license information
9 
10  * -------------------------------------------------------------------------- */
11 
22 #pragma once
23 
25 
26 #include <boost/bind.hpp>
27 
28 #include <stdio.h>
29 #include <algorithm>
30 #include <iostream> // for cout :-(
31 #include <sstream>
32 #include <string>
33 
34 namespace gtsam {
35 
36 /* ************************************************************************* */
37 template <class FACTOR>
38 void FactorGraph<FACTOR>::print(const std::string& s,
39  const KeyFormatter& formatter) const {
40  std::cout << s << std::endl;
41  std::cout << "size: " << size() << std::endl;
42  for (size_t i = 0; i < factors_.size(); i++) {
43  std::stringstream ss;
44  ss << "factor " << i << ": ";
45  if (factors_[i]) factors_[i]->print(ss.str(), formatter);
46  }
47 }
48 
49 /* ************************************************************************* */
50 template <class FACTOR>
51 bool FactorGraph<FACTOR>::equals(const This& fg, double tol) const {
52  // check whether the two factor graphs have the same number of factors.
53  if (factors_.size() != fg.size()) return false;
54 
55  // check whether the factors are the same, in same order.
56  for (size_t i = 0; i < factors_.size(); i++) {
57  sharedFactor f1 = factors_[i], f2 = fg.factors_[i];
58  if (f1 == nullptr && f2 == nullptr) continue;
59  if (f1 == nullptr || f2 == nullptr) return false;
60  if (!f1->equals(*f2, tol)) return false;
61  }
62  return true;
63 }
64 
65 /* ************************************************************************* */
66 template <class FACTOR>
68  size_t size_ = 0;
69  for (const sharedFactor& factor : factors_)
70  if (factor) size_++;
71  return size_;
72 }
73 
74 /* ************************************************************************* */
75 template <class FACTOR>
77  KeySet keys;
78  for (const sharedFactor& factor : this->factors_) {
79  if (factor) keys.insert(factor->begin(), factor->end());
80  }
81  return keys;
82 }
83 
84 /* ************************************************************************* */
85 template <class FACTOR>
87  KeyVector keys;
88  keys.reserve(2 * size()); // guess at size
89  for (const sharedFactor& factor : factors_)
90  if (factor) keys.insert(keys.end(), factor->begin(), factor->end());
91  std::sort(keys.begin(), keys.end());
92  auto last = std::unique(keys.begin(), keys.end());
93  keys.erase(last, keys.end());
94  return keys;
95 }
96 
97 /* ************************************************************************* */
98 template <class FACTOR>
99 template <typename CONTAINER, typename>
101  bool useEmptySlots) {
102  const size_t num_factors = factors.size();
103  FactorIndices newFactorIndices(num_factors);
104  if (useEmptySlots) {
105  size_t i = 0;
106  for (size_t j = 0; j < num_factors; ++j) {
107  // Loop to find the next available factor slot
108  do {
109  if (i >= size())
110  // Make room for remaining factors, happens only once.
111  resize(size() + num_factors - j);
112  else if (at(i))
113  ++i; // Move on to the next slot or past end.
114  else
115  break; // We found an empty slot, break to fill it.
116  } while (true);
117 
118  // Use the current slot, updating graph and newFactorSlots.
119  at(i) = factors[j];
120  newFactorIndices[j] = i;
121  }
122  } else {
123  // We're not looking for unused slots, so just add the factors at the end.
124  for (size_t i = 0; i < num_factors; ++i) newFactorIndices[i] = i + size();
125  push_back(factors);
126  }
127  return newFactorIndices;
128 }
129 
130 } // namespace gtsam
gtsam::FactorGraph::factors_
FastVector< sharedFactor > factors_
concept check, makes sure FACTOR defines print and equals
Definition: FactorGraph.h:128
gtsam::FactorGraph::size
size_t size() const
return the number of factors (including any null factors set by remove() ).
Definition: FactorGraph.h:302
gtsam::FactorGraph::equals
bool equals(const This &fg, double tol=1e-9) const
Check equality.
Definition: FactorGraph-inst.h:51
FactorGraph.h
Factor Graph Base Class.
gtsam
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
gtsam::FactorIndices
FastVector< FactorIndex > FactorIndices
Define collection types:
Definition: Factor.h:32
gtsam::FactorGraph
A factor graph is a bipartite graph with factor nodes connected to variable nodes.
Definition: FactorGraph.h:94
gtsam::FactorGraph::sharedFactor
boost::shared_ptr< FACTOR > sharedFactor
Shared pointer to a factor.
Definition: FactorGraph.h:98
gtsam::FastSet< Key >
gtsam::FactorGraph::add_factors
FactorIndices add_factors(const CONTAINER &factors, bool useEmptySlots=false)
Add new factors to a factor graph and returns a list of new factor indices, optionally finding and re...
Definition: FactorGraph-inst.h:100
gtsam::FactorGraph::print
void print(const std::string &s="FactorGraph", const KeyFormatter &formatter=DefaultKeyFormatter) const
print out graph
Definition: FactorGraph-inst.h:38
gtsam::KeyFormatter
boost::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition: Key.h:35
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition: Key.h:86
gtsam::FactorGraph::keys
KeySet keys() const
Potentially slow function to return all keys involved, sorted, as a set.
Definition: FactorGraph-inst.h:76
gtsam::FactorGraph::keyVector
KeyVector keyVector() const
Potentially slow function to return all keys involved, sorted, as a vector.
Definition: FactorGraph-inst.h:86
gtsam::FactorGraph::nrFactors
size_t nrFactors() const
return the number of non-null factors
Definition: FactorGraph-inst.h:67