55 using namespace gtsam;
58 int main(
int argc,
char** argv) {
84 newTimestamps[priorKey] = 0.0;
96 newTimestamps[currentKey] =
time;
102 newValues.
insert(currentKey, currentPose);
105 Pose2 odometryMeasurement1 =
Pose2(0.61, -0.08, 0.02);
109 Pose2 odometryMeasurement2 =
Pose2(0.47, 0.03, 0.01);
117 smootherBatch.
update(newFactors, newValues, newTimestamps);
118 smootherISAM2.
update(newFactors, newValues, newTimestamps);
119 for(
size_t i = 1;
i < 2; ++
i) {
124 cout << setprecision(5) <<
"Timestamp = " <<
time << endl;
130 newTimestamps.clear();
137 cout <<
"After 3.0 seconds, " << endl;
138 cout <<
" Batch Smoother Keys: " << endl;
139 for(
const FixedLagSmoother::KeyTimestampMap::value_type& key_timestamp: smootherBatch.
timestamps()) {
140 cout << setprecision(5) <<
" Key: " << key_timestamp.first <<
" Time: " << key_timestamp.second << endl;
142 cout <<
" iSAM2 Smoother Keys: " << endl;
143 for(
const FixedLagSmoother::KeyTimestampMap::value_type& key_timestamp: smootherISAM2.
timestamps()) {
144 cout << setprecision(5) <<
" Key: " << key_timestamp.first <<
" Time: " << key_timestamp.second << endl;
152 auto &factorGraph = smootherISAM2.
getFactors();
155 std::shared_ptr<GaussianFactorGraph> linearGraph = factorGraph.
linearize(
result);
158 Matrix jacobian = linearGraph->jacobian().first;
159 cout <<
" Jacobian: " << jacobian << endl;