30 void NonlinearISAM::saveGraph(
const string&
s,
const KeyFormatter& keyFormatter)
const {
31 isam_.saveGraph(
s, keyFormatter);
37 if(newFactors.
size() > 0) {
40 if(reorderInterval_ > 0 && ++reorderCounter_ >= reorderInterval_) {
41 reorder_relinearize();
45 factors_.push_back(newFactors);
49 linPoint_.insert(initialValues);
51 std::shared_ptr<GaussianFactorGraph> linearizedNewFactors = newFactors.
linearize(linPoint_);
54 isam_.update(*linearizedNewFactors, eliminationFunction_);
59 void NonlinearISAM::reorder_relinearize() {
63 if(factors_.size() > 0) {
65 const Values newLinPoint = estimate();
72 isam_.update(*factors_.linearize(newLinPoint), eliminationFunction_);
75 linPoint_ = newLinPoint;
80 Values NonlinearISAM::estimate()
const {
82 return linPoint_.
retract(isam_.optimize());
89 return isam_.marginalCovariance(
key);
94 cout <<
s <<
"ReorderInterval: " << reorderInterval_ <<
" Current Count: " << reorderCounter_ << endl;
95 isam_.print(
"GaussianISAM:\n");
96 linPoint_.print(
"Linearization Point:\n", keyFormatter);
97 factors_.print(
"Nonlinear Graph:\n", keyFormatter);
101 void NonlinearISAM::printStats()
const {
102 isam_.getCliqueData().getStats().print();