gtsam/examples/vSLAMexample/vSFMexample.cpp

/**
 * @file    vSLAMexample.cpp
 * @brief   An vSLAM example for synthesis sequence
 * single camera
 * @author  Duy-Nguyen
 */

#include <boost/shared_ptr.hpp>
using namespace boost;

#define GTSAM_MAGIC_KEY

#include <gtsam/nonlinear/NonlinearFactorGraph-inl.h>
#include <gtsam/nonlinear/NonlinearOptimizer-inl.h>
#include <gtsam/inference/graph-inl.h>
#include <gtsam/slam/visualSLAM.h>
#include <gtsam/slam/PriorFactor.h>

#include "vSLAMutils.h"
#include "Feature2D.h"

using namespace std;
using namespace gtsam;
using namespace gtsam::visualSLAM;
using namespace boost;

/* ************************************************************************* */
#define CALIB_FILE          "calib.txt"
#define LANDMARKS_FILE      "landmarks.txt"
#define POSES_FILE          "poses.txt"
#define MEASUREMENTS_FILE    "measurements.txt"

// Base data folder
string g_dataFolder;

// Store groundtruth values, read from files
shared_ptrK g_calib;
map<int, Point3> g_landmarks;        // map: <landmark_id, landmark_position>
map<int, Pose3> g_poses;            // map: <camera_id, pose>
std::vector<Feature2D> g_measurements;    // Feature2D: {camera_id, landmark_id, 2d feature_position}

// Noise models
SharedGaussian measurementSigma(noiseModel::Isotropic::Sigma(2, 5.0f));

/* ************************************************************************* */
/**
  * Read all data: calibration file, landmarks, poses, and all features measurements
  * Data is stored in global variables.
  */
void readAllData()
{
    g_calib = readCalibData(g_dataFolder +"/" + CALIB_FILE);

    // Read groundtruth landmarks' positions. These will be used later as intial estimates for landmark nodes.
    g_landmarks = readLandMarks(g_dataFolder +"/"+ LANDMARKS_FILE);

    // Read groundtruth camera poses. These will be used later as intial estimates for pose nodes.
    g_poses = readPoses(g_dataFolder, POSES_FILE);

    // Read all 2d measurements. Those will become factors linking their associating pose and the corresponding landmark.
    g_measurements = readAllMeasurements(g_dataFolder, MEASUREMENTS_FILE);
}

/* ************************************************************************* */
/**
  * Setup vSLAM graph
  * by adding and linking 2D features (measurements) detected in each captured image
  * with their corresponding landmarks.
  */
Graph setupGraph(std::vector<Feature2D>& measurements, SharedGaussian measurementSigma, shared_ptrK calib)
{
    Graph g;    

    cout << "Built graph: " << endl;
    for (size_t i= 0; i<measurements.size(); i++)
    {
        measurements[i].print();

        g.addMeasurement(measurements[i].m_p,
                         measurementSigma,
                         measurements[i].m_idCamera,
                         measurements[i].m_idLandmark,
                         calib);
    }

    return g;
}

/* ************************************************************************* */
/**
  * Create a structure of initial estimates for all nodes (landmarks and poses) in the graph.
  * The returned Values structure contains all initial values for all nodes.
  */
Values initialize(std::map<int, Point3> landmarks, std::map<int, Pose3> poses)
{
    Values initValues;

    // Initialize landmarks 3D positions.
    for (map<int, Point3>::iterator lmit = landmarks.begin(); lmit != landmarks.end(); lmit++)
        initValues.insert( lmit->first, lmit->second );

    // Initialize camera poses.
    for (map<int, Pose3>::iterator poseit = poses.begin(); poseit != poses.end(); poseit++)
        initValues.insert( poseit->first, poseit->second);

    return initValues;
}

/* ************************************************************************* */
int main(int argc, char* argv[])
{
    if (argc <2)
    {
        cout << "Usage: vSFMexample <DataFolder>" << endl << endl;
        cout << "\tPlease specify <DataFolder>, which contains calibration file, initial landmarks, initial poses, and feature data." << endl;
        cout << "\tSample folder is in $gtsam_source_folder$/examples/vSLAMexample/Data" << endl << endl;
        cout << "Example usage: vSFMexample '$gtsam_source_folder$/examples/vSLAMexample/Data'" << endl;
        exit(0);
    }

    g_dataFolder = string(argv[1]) + "/";
    readAllData();

    // Create a graph using the 2D measurements (features) and the calibration data
    boost::shared_ptr<Graph> graph(new Graph(setupGraph(g_measurements, measurementSigma, g_calib)));

    // Create an initial Values structure using groundtruth values as the initial estimates
    boost::shared_ptr<Values> initialEstimates(new Values(initialize(g_landmarks, g_poses)));
    cout << "*******************************************************" << endl;
    initialEstimates->print("INITIAL ESTIMATES: ");

    // Add prior factor for all poses in the graph
    map<int, Pose3>::iterator poseit = g_poses.begin();
    for (; poseit != g_poses.end(); poseit++)
        graph->addPosePrior(poseit->first, poseit->second, noiseModel::Unit::Create(1));

    // Optimize the graph
    cout << "*******************************************************" << endl;
    Optimizer::Parameters::verbosityLevel verborsity = Optimizer::Parameters::DAMPED;
    Optimizer::shared_values result = Optimizer::optimizeGN( graph, initialEstimates, verborsity );

    // Print final results
    cout << "*******************************************************" << endl;
    result->print("FINAL RESULTS: ");

}
/* ************************************************************************* */