110 lines
3.7 KiB
C++
110 lines
3.7 KiB
C++
/* ----------------------------------------------------------------------------
|
|
|
|
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
|
* Atlanta, Georgia 30332-0415
|
|
* All Rights Reserved
|
|
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
|
|
|
* See LICENSE for the license information
|
|
|
|
* -------------------------------------------------------------------------- */
|
|
|
|
/**
|
|
* @file VisualISAMExample.cpp
|
|
* @brief An ISAM example for synthesis sequence, single camera
|
|
* @author Duy-Nguyen Ta
|
|
*/
|
|
|
|
#include <gtsam/nonlinear/Symbol.h>
|
|
#include <gtsam/nonlinear/NonlinearISAM.h>
|
|
#include <gtsam/slam/visualSLAM.h>
|
|
#include <gtsam/slam/BetweenFactor.h>
|
|
#include "VisualSLAMData.h"
|
|
|
|
using namespace std;
|
|
using namespace gtsam;
|
|
|
|
// Convenience for named keys
|
|
using symbol_shorthand::X;
|
|
using symbol_shorthand::L;
|
|
|
|
/* ************************************************************************* */
|
|
int main(int argc, char* argv[]) {
|
|
|
|
VisualSLAMExampleData data = VisualSLAMExampleData::generate();
|
|
|
|
/* 1. Create a NonlinearISAM which will be relinearized and reordered after every "relinearizeInterval" updates */
|
|
int relinearizeInterval = 3;
|
|
NonlinearISAM isam(relinearizeInterval);
|
|
|
|
/* 2. At each frame (poseId) with new camera pose and set of associated measurements,
|
|
* create a graph of new factors and update ISAM */
|
|
|
|
// Store the current best estimate from ISAM
|
|
Values currentEstimate;
|
|
|
|
// First two frames:
|
|
// Add factors and initial values for the first two poses and landmarks then update ISAM.
|
|
// Note: measurements from the first pose only are not enough to update ISAM:
|
|
// the system is underconstrained.
|
|
{
|
|
visualSLAM::Graph newFactors;
|
|
|
|
// First pose with prior factor
|
|
newFactors.addPosePrior(X(0), data.poses[0], data.noiseX);
|
|
|
|
// Second pose with odometry measurement
|
|
newFactors.addRelativePose(X(0), X(1), data.odometry, data.noiseX);
|
|
|
|
// Visual measurements at both poses
|
|
for (size_t i=0; i<2; ++i) {
|
|
for (size_t j=0; j<data.z[i].size(); ++j) {
|
|
newFactors.addMeasurement(data.z[i][j], data.noiseZ, X(i), L(j), data.sK);
|
|
}
|
|
}
|
|
|
|
// Initial values for the first two poses, simulated with Gaussian noise
|
|
Values initials;
|
|
initials.insert(X(0), data.poses[0]);
|
|
initials.insert(X(1), data.poses[0]*data.odometry);
|
|
|
|
// Initial values for the landmarks
|
|
for (size_t j=0; j<data.points.size(); ++j)
|
|
initials.insert(L(j), data.points[j]);
|
|
|
|
// Update ISAM the first time and obtain the current estimate
|
|
isam.update(newFactors, initials);
|
|
currentEstimate = isam.estimate();
|
|
cout << "Frame 0 and 1: " << endl;
|
|
currentEstimate.print("Current estimate: ");
|
|
}
|
|
|
|
// Subsequent frames: Add new odometry and measurement factors and initial values,
|
|
// then update ISAM at each frame
|
|
for (size_t i=2; i<data.poses.size(); ++i) {
|
|
visualSLAM::Graph newFactors;
|
|
// Factor for odometry measurements, simulated by adding Gaussian noise to the ground-truth.
|
|
Pose3 odoMeasurement = data.odometry;
|
|
newFactors.addRelativePose(X(i-1), X(i), data.odometry, data.noiseX);
|
|
// Factors for visual measurements
|
|
for (size_t j=0; j<data.z[i].size(); ++j) {
|
|
newFactors.addMeasurement(data.z[i][j], data.noiseZ, X(i), L(j), data.sK);
|
|
}
|
|
|
|
// Initial estimates for the new node Xi, simulated by Gaussian noises
|
|
Values initials;
|
|
initials.insert(X(i), currentEstimate.at<Pose3>(X(i-1))*data.odometry);
|
|
|
|
// update ISAM
|
|
isam.update(newFactors, initials);
|
|
currentEstimate = isam.estimate();
|
|
cout << "****************************************************" << endl;
|
|
cout << "Frame " << i << ": " << endl;
|
|
currentEstimate.print("Current estimate: ");
|
|
}
|
|
|
|
return 0;
|
|
}
|
|
/* ************************************************************************* */
|
|
|