Added iSAM2 test to check issue #412
parent
63d7d7c54b
commit
ab0fb0d4a5
|
@ -0,0 +1,125 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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 testVisualISAM2.cpp
|
||||
* @brief Test convergence of visualSLAM example.
|
||||
* @author Duy-Nguyen Ta
|
||||
* @author Frank Dellaert
|
||||
*/
|
||||
|
||||
#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/PriorFactor.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);
|
||||
|
||||
vector<Point3> points = createPoints();
|
||||
|
||||
// Create the set of ground-truth poses
|
||||
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)
|
||||
{
|
||||
SimpleCamera 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.3), Vector3::Constant(0.1))
|
||||
.finished());
|
||||
graph.emplace_shared<PriorFactor<Pose3>>(Symbol('x', 0), poses[0],
|
||||
kPosePrior);
|
||||
|
||||
// Add a prior on landmark l0
|
||||
static auto kPointPrior = noiseModel::Isotropic::Sigma(3, 0.1);
|
||||
graph.emplace_shared<PriorFactor<Point3>>(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);
|
||||
|
||||
// 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);
|
||||
}
|
||||
/* ************************************************************************* */
|
Loading…
Reference in New Issue