43 double _wildfireThreshold =
47 : wildfireThreshold(_wildfireThreshold) {}
51 cout <<
str <<
"type: ISAM2GaussNewtonParams\n";
52 cout <<
str <<
"wildfireThreshold: " << wildfireThreshold <<
"\n";
58 this->wildfireThreshold = wildfireThreshold;
81 double _initialDelta = 1.0,
82 double _wildfireThreshold =
86 SEARCH_EACH_ITERATION,
89 : initialDelta(_initialDelta),
90 wildfireThreshold(_wildfireThreshold),
91 adaptationMode(_adaptationMode),
96 cout <<
str <<
"type: ISAM2DoglegParams\n";
97 cout <<
str <<
"initialDelta: " << initialDelta <<
"\n";
98 cout <<
str <<
"wildfireThreshold: " << wildfireThreshold <<
"\n";
100 <<
"adaptationMode: " << adaptationModeTranslator(adaptationMode)
108 return adaptationModeTranslator(adaptationMode);
112 this->initialDelta = initialDelta;
115 this->wildfireThreshold = wildfireThreshold;
118 this->adaptationMode = adaptationModeTranslator(adaptationMode);
122 std::string adaptationModeTranslator(
126 const std::string& adaptationMode)
const;
137 typedef std::variant<ISAM2GaussNewtonParams, ISAM2DoglegParams>
139 typedef std::variant<double, FastMap<char, Vector> >
233 int _relinearizeSkip = 10,
bool _enableRelinearization =
true,
234 bool _evaluateNonlinearError =
false,
236 bool _cacheLinearizedFactors =
true,
239 bool _enableDetailedResults =
false)
240 : optimizationParams(_optimizationParams),
241 relinearizeThreshold(_relinearizeThreshold),
242 relinearizeSkip(_relinearizeSkip),
243 enableRelinearization(_enableRelinearization),
244 evaluateNonlinearError(_evaluateNonlinearError),
245 factorization(_factorization),
246 cacheLinearizedFactors(_cacheLinearizedFactors),
247 keyFormatter(_keyFormatter),
248 enableDetailedResults(_enableDetailedResults),
249 enablePartialRelinearizationCheck(false),
250 findUnusedFactorSlots(false) {}
257 static const std::string kStr(
"optimizationParams: ");
258 if (std::holds_alternative<ISAM2GaussNewtonParams>(optimizationParams)) {
259 std::get<ISAM2GaussNewtonParams>(optimizationParams).print();
260 }
else if (std::holds_alternative<ISAM2DoglegParams>(optimizationParams)) {
261 std::get<ISAM2DoglegParams>(optimizationParams).print(kStr);
263 cout << kStr <<
"{unknown type}\n";
266 cout <<
"relinearizeThreshold: ";
267 if (std::holds_alternative<double>(relinearizeThreshold)) {
268 cout << std::get<double>(relinearizeThreshold) <<
"\n";
270 cout <<
"{mapped}\n";
272 std::get<ISAM2ThresholdMap>(relinearizeThreshold)) {
273 cout <<
" '" <<
value.first
274 <<
"' -> [" <<
value.second.transpose() <<
" ]\n";
278 cout <<
"relinearizeSkip: " << relinearizeSkip <<
"\n";
279 cout <<
"enableRelinearization: " << enableRelinearization
281 cout <<
"evaluateNonlinearError: " << evaluateNonlinearError
283 cout <<
"factorization: "
284 << factorizationTranslator(factorization) <<
"\n";
285 cout <<
"cacheLinearizedFactors: " << cacheLinearizedFactors
287 cout <<
"enableDetailedResults: " << enableDetailedResults
289 cout <<
"enablePartialRelinearizationCheck: "
290 << enablePartialRelinearizationCheck <<
"\n";
291 cout <<
"findUnusedFactorSlots: " << findUnusedFactorSlots
300 return this->optimizationParams;
303 return relinearizeThreshold;
306 return factorizationTranslator(factorization);
311 this->optimizationParams = optimizationParams;
314 this->relinearizeThreshold = relinearizeThreshold;
317 this->factorization = factorizationTranslator(factorization);
320 this->keyFormatter = keyFormatter;
324 return factorization == CHOLESKY
334 static Factorization factorizationTranslator(
const std::string&
str);
335 static std::string factorizationTranslator(
const Factorization&
value);