100 lines
3.2 KiB
C++
100 lines
3.2 KiB
C++
/**
|
|
* @file LinearApproxFactor.h
|
|
* @brief A dummy factor that allows a linear factor to act as a nonlinear factor
|
|
* @author Alex Cunningham
|
|
*/
|
|
|
|
#pragma once
|
|
|
|
#include <iostream>
|
|
#include <boost/foreach.hpp>
|
|
|
|
#include <gtsam/slam/LinearApproxFactor.h>
|
|
|
|
namespace gtsam {
|
|
|
|
/* ************************************************************************* */
|
|
template <class Values, class Key>
|
|
LinearApproxFactor<Values,Key>::LinearApproxFactor(
|
|
GaussianFactor::shared_ptr lin_factor, const Ordering& ordering, const Values& lin_points)
|
|
: Base(noiseModel::Unit::Create(lin_factor->get_model()->dim())),
|
|
b_(lin_factor->getb()), model_(lin_factor->get_model()), lin_points_(lin_points)
|
|
{
|
|
BOOST_FOREACH(const Ordering::Map::value_type& p, ordering) {
|
|
Symbol key = p.first;
|
|
varid_t var = p.second;
|
|
|
|
// check if actually in factor
|
|
Factor::const_iterator it = lin_factor->find(var);
|
|
if (it != lin_factor->end()) {
|
|
// store matrix
|
|
Matrix A = lin_factor->getA(it);
|
|
matrices_.insert(make_pair(key, A));
|
|
|
|
// store keys
|
|
nonlinearKeys_.push_back(Key(key.index()));
|
|
this->keys_.push_back(key);
|
|
}
|
|
}
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
template <class Values, class Key>
|
|
Vector LinearApproxFactor<Values,Key>::unwhitenedError(const Values& c) const {
|
|
// extract the points in the new config
|
|
Vector ret = - b_;
|
|
|
|
BOOST_FOREACH(const Key& key, nonlinearKeys_) {
|
|
X newPt = c[key], linPt = lin_points_[key];
|
|
Vector d = linPt.logmap(newPt);
|
|
const Matrix& A = matrices_.at(Symbol(key));
|
|
ret += prod(A, d);
|
|
}
|
|
|
|
return ret;
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
template <class Values, class Key>
|
|
boost::shared_ptr<GaussianFactor>
|
|
LinearApproxFactor<Values,Key>::linearize(const Values& c, const Ordering& ordering) const {
|
|
|
|
// sort by varid - only known at linearization time
|
|
typedef std::map<varid_t, Matrix> VarMatrixMap;
|
|
VarMatrixMap sorting_terms;
|
|
BOOST_FOREACH(const SymbolMatrixMap::value_type& p, matrices_)
|
|
sorting_terms.insert(std::make_pair(ordering[p.first], p.second));
|
|
|
|
// move into terms
|
|
std::vector<std::pair<varid_t, Matrix> > terms;
|
|
BOOST_FOREACH(const VarMatrixMap::value_type& p, sorting_terms)
|
|
terms.push_back(p);
|
|
|
|
return boost::shared_ptr<GaussianFactor>(new GaussianFactor(terms, b_, model_));
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
template <class Values, class Key>
|
|
Factor::shared_ptr
|
|
LinearApproxFactor<Values,Key>::symbolic(const Ordering& ordering) const {
|
|
std::vector<varid_t> key_ids(this->keys_.size());
|
|
size_t i=0;
|
|
BOOST_FOREACH(const Symbol& key, this->keys_)
|
|
key_ids[i++] = ordering[key];
|
|
std::sort(key_ids.begin(), key_ids.end());
|
|
return boost::shared_ptr<Factor>(new Factor(key_ids.begin(), key_ids.end()));
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
template <class Values, class Key>
|
|
void LinearApproxFactor<Values,Key>::print(const std::string& s) const {
|
|
LinearApproxFactor<Values,Key>::Base::print(s);
|
|
BOOST_FOREACH(const SymbolMatrixMap::value_type& p, matrices_) {
|
|
gtsam::print(p.second, (std::string) p.first);
|
|
}
|
|
gtsam::print(b_, std::string("b"));
|
|
model_->print("model");
|
|
}
|
|
|
|
} // \namespace gtsam
|