42 return e !=
nullptr && FixedLagSmoother::equals(*
e,
tol)
43 && factors_.equals(
e->factors_,
tol) && theta_.equals(
e->theta_,
tol);
49 "BatchFixedLagSmoother::marginalCovariance not implemented");
58 gttic(augment_system);
60 theta_.insert(newTheta);
62 for (
const auto key : newTheta.
keys()) {
63 ordering_.push_back(
key);
69 insertFactors(newFactors);
70 gttoc(augment_system);
73 for(
const size_t i : factorsToRemove){
79 updateKeyTimestampMap(timestamps);
82 double current_timestamp = getCurrentTimestamp();
85 KeyVector marginalizableKeys = findKeysBefore(
86 current_timestamp - smootherLag_);
90 reorder(marginalizableKeys);
96 if (factors_.size() > 0) {
103 if (marginalizableKeys.size() > 0) {
104 marginalize(marginalizableKeys);
112 void BatchFixedLagSmoother::insertFactors(
114 for(
const auto& factor: newFactors) {
117 if (availableSlots_.size() > 0) {
118 index = availableSlots_.front();
119 availableSlots_.pop();
120 factors_.replace(index, factor);
122 index = factors_.size();
123 factors_.push_back(factor);
127 factorIndex_[
key].insert(index);
133 void BatchFixedLagSmoother::removeFactors(
134 const set<size_t>& deleteFactors) {
135 for(
size_t slot: deleteFactors) {
136 if (factors_.at(slot)) {
138 for(
Key key: *(factors_.at(slot))) {
139 factorIndex_[
key].erase(slot);
142 factors_.remove(slot);
144 availableSlots_.push(slot);
147 cout <<
"Attempting to remove a factor from slot " << slot
148 <<
", but it is already nullptr." << endl;
161 factorIndex_.erase(
key);
164 if (linearValues_.exists(
key)) {
165 linearValues_.erase(
key);
169 eraseKeyTimestampMap(
keys);
173 ordering_.erase(find(ordering_.begin(), ordering_.end(),
key));
179 void BatchFixedLagSmoother::reorder(
const KeyVector& marginalizeKeys) {
181 ordering_ = Ordering::ColamdConstrainedFirst(factors_, marginalizeKeys);
189 result.nonlinearVariables = theta_.size() - linearValues_.size();
190 result.linearVariables = linearValues_.size();
193 double lambda = parameters_.lambdaInitial;
194 double lambdaFactor = parameters_.lambdaFactor;
195 double lambdaUpperBound = parameters_.lambdaUpperBound;
196 double lambdaLowerBound = 1.0e-10;
197 size_t maxIterations = parameters_.maxIterations;
198 double relativeErrorTol = parameters_.relativeErrorTol;
199 double absoluteErrorTol = parameters_.absoluteErrorTol;
200 double errorTol = parameters_.errorTol;
204 result.error = factors_.error(evalpoint);
207 if (
result.error <= errorTol) {
212 double previousError;
215 previousError =
result.error;
218 gttic(optimizer_iteration);
229 dampedFactorGraph.
reserve(linearFactorGraph.
size() + delta_.size());
233 for(
const auto& key_value: delta_) {
234 size_t dim = key_value.second.size();
235 Matrix A = Matrix::Identity(dim, dim);
244 result.intermediateSteps++;
248 newDelta = dampedFactorGraph.
optimize(ordering_,
249 parameters_.getEliminationFunction());
251 evalpoint = theta_.
retract(newDelta);
255 gttic(compute_error);
256 double error = factors_.error(evalpoint);
257 gttoc(compute_error);
268 if (enforceConsistency_ && (linearValues_.size() > 0)) {
269 theta_.
update(linearValues_);
270 for(
const auto key: linearValues_.keys()) {
276 if (
lambda < lambdaLowerBound) {
277 lambda = lambdaLowerBound;
283 if (
lambda >= lambdaUpperBound) {
285 if(parameters_.verbosity >= NonlinearOptimizerParams::TERMINATION
286 || parameters_.verbosityLM == LevenbergMarquardtParams::SUMMARY) {
288 <<
"Warning: Levenberg-Marquardt giving up because cannot decrease error with maximum lambda"
299 gttoc(optimizer_iteration);
302 }
while (
result.iterations < maxIterations
304 previousError,
result.error, NonlinearOptimizerParams::SILENT));
310 void BatchFixedLagSmoother::marginalize(
const KeyVector& marginalizeKeys) {
318 set<size_t> removedFactorSlots;
320 for(
Key key: marginalizeKeys) {
321 const auto& slots = variableIndex[
key];
322 removedFactorSlots.insert(slots.begin(), slots.end());
327 for(
size_t slot: removedFactorSlots) {
328 if (factors_.at(slot)) {
329 removedFactors.
push_back(factors_.at(slot));
335 removedFactors, theta_, marginalizeKeys, parameters_.getEliminationFunction());
338 removeFactors(removedFactorSlots);
341 eraseKeys(marginalizeKeys);
344 insertFactors(marginalFactors);
349 const string& label) {
359 const string& label) {
368 void BatchFixedLagSmoother::PrintSymbolicFactor(
372 for(
Key key: factor->keys()) {
378 cout <<
" )" << endl;
382 void BatchFixedLagSmoother::PrintSymbolicFactor(
385 for(
Key key: factor->keys()) {
388 cout <<
" )" << endl;
392 void BatchFixedLagSmoother::PrintSymbolicGraph(
394 cout << label << endl;
395 for(
const auto& factor:
graph) {
396 PrintSymbolicFactor(factor);
402 const string& label) {
403 cout << label << endl;
404 for(
const auto& factor:
graph) {
405 PrintSymbolicFactor(factor);
412 const GaussianFactorGraph::Eliminate& eliminateFunction) {
413 if (
keys.size() == 0) {
418 return *
graph.eliminatePartialMultifrontal(
keys, eliminateFunction).second;
425 const GaussianFactorGraph::Eliminate& eliminateFunction) {
426 if (
keys.size() == 0) {
431 const auto linearFactorGraph =
graph.linearize(theta);
433 const auto marginalLinearFactors =
434 CalculateMarginalFactors(*linearFactorGraph,
keys, eliminateFunction);
437 return LinearContainerFactor::ConvertLinearGraph(marginalLinearFactors, theta);