30 void HybridNonlinearISAM::saveGraph(
const string&
s,
32 isam_.saveGraph(
s, keyFormatter);
37 const Values& initialValues,
38 const std::optional<size_t>& maxNrLeaves,
39 const std::optional<Ordering>&
ordering) {
40 if (newFactors.
size() > 0) {
42 if (reorderInterval_ > 0 && ++reorderCounter_ >= reorderInterval_) {
47 factors_.push_back(newFactors);
51 linPoint_.insert(initialValues);
53 std::shared_ptr<HybridGaussianFactorGraph> linearizedNewFactors =
57 isam_.update(*linearizedNewFactors, maxNrLeaves,
ordering,
58 eliminationFunction_);
63 void HybridNonlinearISAM::reorderRelinearize() {
64 if (factors_.size() > 0) {
66 const Values newLinPoint = estimate();
68 auto discreteProbs = *(isam_.roots().at(0)->conditional()->asDiscrete());
74 for (
auto&& factor : factors_) {
75 if (
auto nf = std::dynamic_pointer_cast<HybridNonlinearFactor>(factor)) {
76 pruned_factors.
push_back(nf->prune(discreteProbs));
81 factors_ = pruned_factors;
86 isam_.update(*factors_.linearize(newLinPoint), {}, {},
87 eliminationFunction_);
90 linPoint_ = newLinPoint;
95 Values HybridNonlinearISAM::estimate() {
97 if (isam_.size() > 0) {
99 assignment_ =
values.discrete();
100 return linPoint_.retract(
values.continuous());
114 cout <<
s <<
"ReorderInterval: " << reorderInterval_
115 <<
" Current Count: " << reorderCounter_ << endl;
116 std::cout <<
"HybridGaussianISAM:" << std::endl;
117 isam_.print(
"", keyFormatter);
118 linPoint_.print(
"Linearization Point:\n", keyFormatter);
119 std::cout <<
"Nonlinear Graph:" << std::endl;
120 factors_.print(
"", keyFormatter);
124 void HybridNonlinearISAM::printStats()
const {
125 isam_.getCliqueData().getStats().print();