Added clone and rekey facilities to nonlinear factor graph
Alex Cunningham
parent
32871bfceb
commit
47830cd56f
|
|
@ -161,7 +161,23 @@ public:
|
|||
virtual shared_ptr clone() const =0;
|
||||
|
||||
/**
|
||||
* Clones a factor and replaces its keys
|
||||
* Creates a shared_ptr clone of the factor with different keys using
|
||||
* a map from old->new keys
|
||||
*/
|
||||
shared_ptr rekey(const std::map<Key,Key>& rekey_mapping) const {
|
||||
shared_ptr new_factor = clone();
|
||||
for (size_t i=0; i<new_factor->size(); ++i) {
|
||||
Key& cur_key = new_factor->keys()[i];
|
||||
std::map<Key,Key>::const_iterator mapping = rekey_mapping.find(cur_key);
|
||||
if (mapping != rekey_mapping.end())
|
||||
cur_key = mapping->second;
|
||||
}
|
||||
return new_factor;
|
||||
}
|
||||
|
||||
/**
|
||||
* Clones a factor and fully replaces its keys
|
||||
* @param new_keys is the full replacement set of keys
|
||||
*/
|
||||
shared_ptr rekey(const std::vector<Key>& new_keys) const {
|
||||
assert(new_keys.size() == this->keys().size());
|
||||
|
|
|
|||
|
|
@ -27,112 +27,136 @@ using namespace std;
|
|||
|
||||
namespace gtsam {
|
||||
|
||||
/* ************************************************************************* */
|
||||
double NonlinearFactorGraph::probPrime(const Values& c) const {
|
||||
return exp(-0.5 * error(c));
|
||||
/* ************************************************************************* */
|
||||
double NonlinearFactorGraph::probPrime(const Values& c) const {
|
||||
return exp(-0.5 * error(c));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void NonlinearFactorGraph::print(const std::string& str, const KeyFormatter& keyFormatter) const {
|
||||
cout << str << "size: " << size() << endl;
|
||||
for (size_t i = 0; i < factors_.size(); i++) {
|
||||
stringstream ss;
|
||||
ss << "factor " << i << ": ";
|
||||
if (factors_[i] != NULL) factors_[i]->print(ss.str(), keyFormatter);
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void NonlinearFactorGraph::print(const std::string& str, const KeyFormatter& keyFormatter) const {
|
||||
cout << str << "size: " << size() << endl;
|
||||
for (size_t i = 0; i < factors_.size(); i++) {
|
||||
stringstream ss;
|
||||
ss << "factor " << i << ": ";
|
||||
if (factors_[i] != NULL) factors_[i]->print(ss.str(), keyFormatter);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
double NonlinearFactorGraph::error(const Values& c) const {
|
||||
double total_error = 0.;
|
||||
// iterate over all the factors_ to accumulate the log probabilities
|
||||
BOOST_FOREACH(const sharedFactor& factor, this->factors_) {
|
||||
if(factor)
|
||||
total_error += factor->error(c);
|
||||
}
|
||||
return total_error;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
double NonlinearFactorGraph::error(const Values& c) const {
|
||||
double total_error = 0.;
|
||||
// iterate over all the factors_ to accumulate the log probabilities
|
||||
BOOST_FOREACH(const sharedFactor& factor, this->factors_) {
|
||||
if(factor)
|
||||
total_error += factor->error(c);
|
||||
}
|
||||
return total_error;
|
||||
/* ************************************************************************* */
|
||||
std::set<Key> NonlinearFactorGraph::keys() const {
|
||||
std::set<Key> keys;
|
||||
BOOST_FOREACH(const sharedFactor& factor, this->factors_) {
|
||||
if(factor)
|
||||
keys.insert(factor->begin(), factor->end());
|
||||
}
|
||||
return keys;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
std::set<Key> NonlinearFactorGraph::keys() const {
|
||||
std::set<Key> keys;
|
||||
BOOST_FOREACH(const sharedFactor& factor, this->factors_) {
|
||||
if(factor)
|
||||
keys.insert(factor->begin(), factor->end());
|
||||
}
|
||||
return keys;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Ordering::shared_ptr NonlinearFactorGraph::orderingCOLAMD(
|
||||
const Values& config) const {
|
||||
|
||||
// Create symbolic graph and initial (iterator) ordering
|
||||
SymbolicFactorGraph::shared_ptr symbolic;
|
||||
Ordering::shared_ptr ordering;
|
||||
boost::tie(symbolic, ordering) = this->symbolic(config);
|
||||
|
||||
// Compute the VariableIndex (column-wise index)
|
||||
VariableIndex variableIndex(*symbolic, ordering->size());
|
||||
if (config.size() != variableIndex.size()) throw std::runtime_error(
|
||||
"orderingCOLAMD: some variables in the graph are not constrained!");
|
||||
|
||||
// Compute a fill-reducing ordering with COLAMD
|
||||
Permutation::shared_ptr colamdPerm(inference::PermutationCOLAMD(
|
||||
variableIndex));
|
||||
|
||||
// Permute the Ordering with the COLAMD ordering
|
||||
ordering->permuteWithInverse(*colamdPerm->inverse());
|
||||
|
||||
// Return the Ordering and VariableIndex to be re-used during linearization
|
||||
// and elimination
|
||||
return ordering;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
SymbolicFactorGraph::shared_ptr NonlinearFactorGraph::symbolic(const Ordering& ordering) const {
|
||||
// Generate the symbolic factor graph
|
||||
SymbolicFactorGraph::shared_ptr symbolicfg(new SymbolicFactorGraph);
|
||||
symbolicfg->reserve(this->size());
|
||||
|
||||
BOOST_FOREACH(const sharedFactor& factor, this->factors_) {
|
||||
if(factor)
|
||||
symbolicfg->push_back(factor->symbolic(ordering));
|
||||
else
|
||||
symbolicfg->push_back(SymbolicFactorGraph::sharedFactor());
|
||||
}
|
||||
|
||||
return symbolicfg;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
pair<SymbolicFactorGraph::shared_ptr, Ordering::shared_ptr> NonlinearFactorGraph::symbolic(
|
||||
/* ************************************************************************* */
|
||||
Ordering::shared_ptr NonlinearFactorGraph::orderingCOLAMD(
|
||||
const Values& config) const {
|
||||
// Generate an initial key ordering in iterator order
|
||||
Ordering::shared_ptr ordering(config.orderingArbitrary());
|
||||
return make_pair(symbolic(*ordering), ordering);
|
||||
|
||||
// Create symbolic graph and initial (iterator) ordering
|
||||
SymbolicFactorGraph::shared_ptr symbolic;
|
||||
Ordering::shared_ptr ordering;
|
||||
boost::tie(symbolic, ordering) = this->symbolic(config);
|
||||
|
||||
// Compute the VariableIndex (column-wise index)
|
||||
VariableIndex variableIndex(*symbolic, ordering->size());
|
||||
if (config.size() != variableIndex.size()) throw std::runtime_error(
|
||||
"orderingCOLAMD: some variables in the graph are not constrained!");
|
||||
|
||||
// Compute a fill-reducing ordering with COLAMD
|
||||
Permutation::shared_ptr colamdPerm(inference::PermutationCOLAMD(
|
||||
variableIndex));
|
||||
|
||||
// Permute the Ordering with the COLAMD ordering
|
||||
ordering->permuteWithInverse(*colamdPerm->inverse());
|
||||
|
||||
// Return the Ordering and VariableIndex to be re-used during linearization
|
||||
// and elimination
|
||||
return ordering;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
SymbolicFactorGraph::shared_ptr NonlinearFactorGraph::symbolic(const Ordering& ordering) const {
|
||||
// Generate the symbolic factor graph
|
||||
SymbolicFactorGraph::shared_ptr symbolicfg(new SymbolicFactorGraph);
|
||||
symbolicfg->reserve(this->size());
|
||||
|
||||
BOOST_FOREACH(const sharedFactor& factor, this->factors_) {
|
||||
if(factor)
|
||||
symbolicfg->push_back(factor->symbolic(ordering));
|
||||
else
|
||||
symbolicfg->push_back(SymbolicFactorGraph::sharedFactor());
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
GaussianFactorGraph::shared_ptr NonlinearFactorGraph::linearize(
|
||||
const Values& config, const Ordering& ordering) const {
|
||||
return symbolicfg;
|
||||
}
|
||||
|
||||
// create an empty linear FG
|
||||
GaussianFactorGraph::shared_ptr linearFG(new GaussianFactorGraph);
|
||||
linearFG->reserve(this->size());
|
||||
/* ************************************************************************* */
|
||||
pair<SymbolicFactorGraph::shared_ptr, Ordering::shared_ptr> NonlinearFactorGraph::symbolic(
|
||||
const Values& config) const {
|
||||
// Generate an initial key ordering in iterator order
|
||||
Ordering::shared_ptr ordering(config.orderingArbitrary());
|
||||
return make_pair(symbolic(*ordering), ordering);
|
||||
}
|
||||
|
||||
// linearize all factors
|
||||
BOOST_FOREACH(const sharedFactor& factor, this->factors_) {
|
||||
if(factor) {
|
||||
GaussianFactor::shared_ptr lf = factor->linearize(config, ordering);
|
||||
if (lf) linearFG->push_back(lf);
|
||||
} else
|
||||
linearFG->push_back(GaussianFactor::shared_ptr());
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
GaussianFactorGraph::shared_ptr NonlinearFactorGraph::linearize(
|
||||
const Values& config, const Ordering& ordering) const {
|
||||
|
||||
return linearFG;
|
||||
// create an empty linear FG
|
||||
GaussianFactorGraph::shared_ptr linearFG(new GaussianFactorGraph);
|
||||
linearFG->reserve(this->size());
|
||||
|
||||
// linearize all factors
|
||||
BOOST_FOREACH(const sharedFactor& factor, this->factors_) {
|
||||
if(factor) {
|
||||
GaussianFactor::shared_ptr lf = factor->linearize(config, ordering);
|
||||
if (lf) linearFG->push_back(lf);
|
||||
} else
|
||||
linearFG->push_back(GaussianFactor::shared_ptr());
|
||||
}
|
||||
|
||||
return linearFG;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
NonlinearFactorGraph NonlinearFactorGraph::clone() const {
|
||||
NonlinearFactorGraph result;
|
||||
BOOST_FOREACH(const sharedFactor& f, *this) {
|
||||
if (f)
|
||||
result.push_back(f->clone());
|
||||
else
|
||||
result.push_back(sharedFactor()); // Passes on null factors so indices remain valid
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
NonlinearFactorGraph NonlinearFactorGraph::rekey(const std::map<Key,Key>& rekey_mapping) const {
|
||||
NonlinearFactorGraph result;
|
||||
BOOST_FOREACH(const sharedFactor& f, *this) {
|
||||
if (f)
|
||||
result.push_back(f->rekey(rekey_mapping));
|
||||
else
|
||||
result.push_back(sharedFactor());
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
||||
} // namespace gtsam
|
||||
|
|
|
|||
|
|
@ -87,6 +87,22 @@ namespace gtsam {
|
|||
boost::shared_ptr<GaussianFactorGraph >
|
||||
linearize(const Values& config, const Ordering& ordering) const;
|
||||
|
||||
/**
|
||||
* Clone() performs a deep-copy of the graph, including all of the factors
|
||||
*/
|
||||
NonlinearFactorGraph clone() const;
|
||||
|
||||
/**
|
||||
* Rekey() performs a deep-copy of all of the factors, and changes
|
||||
* keys according to a mapping.
|
||||
*
|
||||
* Keys not specified in the mapping will remain unchanged.
|
||||
*
|
||||
* @param rekey_mapping is a map of old->new keys
|
||||
* @result a cloned graph with updated keys
|
||||
*/
|
||||
NonlinearFactorGraph rekey(const std::map<Key,Key>& rekey_mapping) const;
|
||||
|
||||
private:
|
||||
|
||||
/** Serialization function */
|
||||
|
|
|
|||
|
|
@ -107,6 +107,43 @@ TEST( Graph, linearize )
|
|||
CHECK(assert_equal(expected,*linearized)); // Needs correct linearizations
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( Graph, clone )
|
||||
{
|
||||
Graph fg = createNonlinearFactorGraph();
|
||||
Graph actClone = fg.clone();
|
||||
EXPECT(assert_equal(fg, actClone));
|
||||
for (size_t i=0; i<fg.size(); ++i)
|
||||
EXPECT(fg[i] != actClone[i]);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( Graph, rekey )
|
||||
{
|
||||
Graph init = createNonlinearFactorGraph();
|
||||
map<Key,Key> rekey_mapping;
|
||||
rekey_mapping.insert(make_pair(kl(1), kl(4)));
|
||||
Graph actRekey = init.rekey(rekey_mapping);
|
||||
|
||||
// ensure deep clone
|
||||
LONGS_EQUAL(init.size(), actRekey.size());
|
||||
for (size_t i=0; i<init.size(); ++i)
|
||||
EXPECT(init[i] != actRekey[i]);
|
||||
|
||||
Graph expRekey;
|
||||
// original measurements
|
||||
expRekey.push_back(init[0]);
|
||||
expRekey.push_back(init[1]);
|
||||
|
||||
// updated measurements
|
||||
Point2 z3(0, -1), z4(-1.5, -1.);
|
||||
SharedDiagonal sigma0_2 = noiseModel::Isotropic::Sigma(2,0.2);
|
||||
expRekey.add(simulated2D::Measurement(z3, sigma0_2, kx(1), kl(4)));
|
||||
expRekey.add(simulated2D::Measurement(z4, sigma0_2, kx(2), kl(4)));
|
||||
|
||||
EXPECT(assert_equal(expRekey, actRekey));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() {
|
||||
TestResult tr;
|
||||
|
|
|
|||
Loading…
Reference in New Issue