testVisualISAM2.cpp

Go to the documentation of this file.

/* ----------------------------------------------------------------------------
 
 * 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
 
 * -------------------------------------------------------------------------- */
 
#include <CppUnitLite/TestHarness.h>
 
#include <examples/SFMdata.h>
#include <gtsam/geometry/Point2.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/nonlinear/ISAM2.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/slam/ProjectionFactor.h>
 
#include <vector>
 
using namespace std;
using namespace gtsam;
 
/* ************************************************************************* */
TEST(testVisualISAM2, all)
{
    Cal3_S2::shared_ptr K(new Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0));
 
    auto measurementNoise = noiseModel::Isotropic::Sigma(2, 1.0);
 
    // Create ground truth data
    vector<Point3> points = createPoints();
    vector<Pose3> poses = createPoses();
 
    // Set the parameters
    ISAM2Params parameters;
    parameters.relinearizeThreshold = 0.01;
    parameters.relinearizeSkip = 1;
    ISAM2 isam(parameters);
 
    // Create a Factor Graph and Values to hold the new data
    NonlinearFactorGraph graph;
    Values initialEstimate;
 
    // Loop over the poses, adding the observations to iSAM incrementally
    for (size_t i = 0; i < poses.size(); ++i)
    {
        // Add factors for each landmark observation
        for (size_t j = 0; j < points.size(); ++j)
        {
            PinholeCamera<Cal3_S2> camera(poses[i], *K);
            Point2 measurement = camera.project(points[j]);
            graph.emplace_shared<GenericProjectionFactor<Pose3, Point3, Cal3_S2>>(
                measurement, measurementNoise, Symbol('x', i), Symbol('l', j), K);
        }
 
        // Add an initial guess for the current pose
        // Intentionally initialize the variables off from the ground truth
        static Pose3 kDeltaPose(Rot3::Rodrigues(-0.1, 0.2, 0.25),
                                Point3(0.05, -0.10, 0.20));
        initialEstimate.insert(Symbol('x', i), poses[i] * kDeltaPose);
 
        // Treat first iteration as special case
        if (i == 0)
        {
            // Add a prior on pose x0, 30cm std on x,y,z and 0.1 rad on roll,pitch,yaw
            static auto kPosePrior = noiseModel::Diagonal::Sigmas(
                (Vector(6) << Vector3::Constant(0.1), Vector3::Constant(0.3))
                    .finished());
            graph.addPrior(Symbol('x', 0), poses[0], kPosePrior);
 
            // Add a prior on landmark l0
            static auto kPointPrior = noiseModel::Isotropic::Sigma(3, 0.1);
            graph.addPrior(Symbol('l', 0), points[0], kPointPrior);
 
            // Add initial guesses to all observed landmarks
            // Intentionally initialize the variables off from the ground truth
            static Point3 kDeltaPoint(-0.25, 0.20, 0.15);
            for (size_t j = 0; j < points.size(); ++j)
                initialEstimate.insert<Point3>(Symbol('l', j), points[j] + kDeltaPoint);
        }
        else
        {
            // Update iSAM with the new factors
            isam.update(graph, initialEstimate);
 
            // Do an extra update to converge withing these 8 iterations
            isam.update();
 
            // Optimize
            Values currentEstimate = isam.calculateEstimate();
 
            // reset for next iteration
            graph.resize(0);
            initialEstimate.clear();
        }
    } // for loop
 
    auto result = isam.calculateEstimate();
    EXPECT_LONGS_EQUAL(16, result.size());
    for (size_t j = 0; j < points.size(); ++j)
    {
        Symbol key('l', j);
        EXPECT(assert_equal(points[j], result.at<Point3>(key), 0.01));
    }
}
 
/* ************************************************************************* */
int main()
{
    TestResult tr;
    return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */