35 using namespace gtsam;
59 SETDEBUG(
"IncrementalFixedLagSmoother update",
true);
66 typedef IncrementalFixedLagSmoother::KeyTimestampMap Timestamps;
82 Timestamps newTimestamps;
86 newTimestamps[key0] = 0.0;
89 fullinit.
insert(newValues);
92 smoother.
update(newFactors, newValues, newTimestamps);
107 Timestamps newTimestamps;
111 newTimestamps[
key2] = double(
i);
114 fullinit.
insert(newValues);
117 smoother.
update(newFactors, newValues, newTimestamps);
133 Timestamps newTimestamps;
138 newTimestamps[
key2] = double(
i);
141 fullinit.
insert(newValues);
144 smoother.
update(newFactors, newValues, newTimestamps);
159 Timestamps newTimestamps;
163 newTimestamps[
key2] = double(
i);
166 fullinit.
insert(newValues);
169 smoother.
update(newFactors, newValues, newTimestamps);
184 Timestamps newTimestamps;
190 newTimestamps[
key2] = double(
i);
193 fullinit.
insert(newValues);
196 smoother.
update(newFactors, newValues, newTimestamps);
205 Timestamps emptyNewTimestamps;
207 size_t factorIndex = 25;
209 factorToRemove.push_back(factorIndex);
214 smoother.
update(emptyNewFactors, emptyNewValues, emptyNewTimestamps,factorToRemove);
224 for(
size_t i=0;
i< smootherFactorsBeforeRemove.
size();
i++){
226 if(smootherFactorsBeforeRemove[
i] &&
i != factorIndex){