29 void HybridNonlinearISAM::saveGraph(
const string&
s,
31 isam_.saveGraph(
s, keyFormatter);
36 const Values& initialValues,
37 const std::optional<size_t>& maxNrLeaves,
38 const std::optional<Ordering>&
ordering) {
39 if (newFactors.
size() > 0) {
41 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();
73 isam_.update(*factors_.linearize(newLinPoint), {}, {},
74 eliminationFunction_);
77 linPoint_ = newLinPoint;
82 Values HybridNonlinearISAM::estimate() {
84 if (isam_.size() > 0) {
86 assignment_ =
values.discrete();
87 return linPoint_.retract(
values.continuous());
101 cout <<
s <<
"ReorderInterval: " << reorderInterval_
102 <<
" Current Count: " << reorderCounter_ << endl;
103 std::cout <<
"HybridGaussianISAM:" << std::endl;
104 isam_.print(
"", keyFormatter);
105 linPoint_.print(
"Linearization Point:\n", keyFormatter);
106 std::cout <<
"Nonlinear Graph:" << std::endl;
107 factors_.print(
"", keyFormatter);
111 void HybridNonlinearISAM::printStats()
const {
112 isam_.getCliqueData().getStats().print();