133 lines
4.9 KiB
C++
133 lines
4.9 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 testPlanarSLAMExample_lago.cpp
|
|
* @brief Unit tests for planar SLAM example using the initialization technique
|
|
* LAGO (Linear Approximation for Graph Optimization) proposed in:
|
|
*
|
|
* L. Carlone, R. Aragues, J. Castellanos, and B. Bona, A fast and accurate
|
|
* approximation for planar pose graph optimization, IJRR, 2014.
|
|
*
|
|
* L. Carlone, R. Aragues, J.A. Castellanos, and B. Bona, A linear approximation
|
|
* for graph-based simultaneous localization and mapping, RSS, 2011.
|
|
*
|
|
* @author Luca Carlone
|
|
* @author Frank Dellaert
|
|
* @date May 14, 2014
|
|
*/
|
|
|
|
// As this is a planar SLAM example, we will use Pose2 variables (x, y, theta) to represent
|
|
// the robot positions and Point2 variables (x, y) to represent the landmark coordinates.
|
|
#include <gtsam/geometry/Pose2.h>
|
|
|
|
// Each variable in the system (poses and landmarks) must be identified with a unique key.
|
|
// We can either use simple integer keys (1, 2, 3, ...) or symbols (X1, X2, L1).
|
|
// Here we will use Symbols
|
|
#include <gtsam/inference/Symbol.h>
|
|
|
|
// In GTSAM, measurement functions are represented as 'factors'. Several common factors
|
|
// have been provided with the library for solving robotics/SLAM/Bundle Adjustment problems.
|
|
// Here we will use a RangeBearing factor for the range-bearing measurements to identified
|
|
// landmarks, and Between factors for the relative motion described by odometry measurements.
|
|
// Also, we will initialize the robot at the origin using a Prior factor.
|
|
#include <gtsam/slam/PriorFactor.h>
|
|
#include <gtsam/slam/BetweenFactor.h>
|
|
|
|
// When the factors are created, we will add them to a Factor Graph. As the factors we are using
|
|
// are nonlinear factors, we will need a Nonlinear Factor Graph.
|
|
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
|
|
|
|
#include <gtsam/base/TestableAssertions.h>
|
|
#include <CppUnitLite/TestHarness.h>
|
|
#include <boost/math/constants/constants.hpp>
|
|
#include <cmath>
|
|
|
|
using namespace std;
|
|
using namespace gtsam;
|
|
using namespace boost::assign;
|
|
|
|
Symbol x0('x', 0), x1('x', 1), x2('x', 2), x3('x',3);
|
|
static SharedNoiseModel model(noiseModel::Isotropic::Sigma(3, 0.1));
|
|
static const double PI = boost::math::constants::pi<double>();
|
|
|
|
/* ************************************************************************* */
|
|
Values initializeLago(const NonlinearFactorGraph& graph) {
|
|
// Order measurements: ordered spanning path first, loop closure later
|
|
|
|
// Extract angles in so2 from relative rotations in SO2
|
|
|
|
// Correct orientations along loops
|
|
|
|
// Create a linear factor graph (LFG) of scalars
|
|
|
|
// Solve the LFG
|
|
|
|
// Store solution of the LFG in values
|
|
Values estimateLago;
|
|
return estimateLago;
|
|
}
|
|
|
|
|
|
/* *************************************************************************** */
|
|
TEST( Lago, smallGraph_GTmeasurements ) {
|
|
// We consider a small graph:
|
|
// symbolic FG
|
|
// x2 0 1
|
|
// / | \ 1 2
|
|
// / | \ 2 3
|
|
// x3 | x4 2 1
|
|
// \ | / 1 3
|
|
// \ | /
|
|
// x0
|
|
|
|
Pose2 pose0 = Pose2( 0.000000, 0.000000, 0.000000);
|
|
Pose2 pose1 = Pose2( 1.000000, 1.000000, 1.570796);
|
|
Pose2 pose2 = Pose2( 0.000000, 2.000000, 3.141593);
|
|
Pose2 pose3 = Pose2(-1.000000, 1.000000, 4.712389);
|
|
|
|
NonlinearFactorGraph graph;
|
|
|
|
BetweenFactor<Pose2> factor01(x0, x1, pose0.between(pose1), model);
|
|
graph.add(factor01);
|
|
|
|
BetweenFactor<Pose2> factor12(x1, x2, pose1.between(pose2), model);
|
|
graph.add(factor12);
|
|
|
|
BetweenFactor<Pose2> factor23(x2, x3, pose2.between(pose3), model);
|
|
graph.add(factor23);
|
|
|
|
BetweenFactor<Pose2> factor20(x2, x0, pose2.between(pose0), model);
|
|
graph.add(factor20);
|
|
|
|
BetweenFactor<Pose2> factor03(x0, x3, pose0.between(pose3), model);
|
|
graph.add(factor03);
|
|
|
|
// graph.print("graph");
|
|
|
|
Values initialGuessLago = initializeLago(graph);
|
|
|
|
Vector expectedOrientations = (Vector(4) << 0.0, 0.5*PI, PI, 1.5*PI);
|
|
Vector actualOrientations(4);
|
|
actualOrientations(0) = (initialGuessLago.at<Pose2>(x0)).theta();
|
|
actualOrientations(1) = (initialGuessLago.at<Pose2>(x1)).theta();
|
|
actualOrientations(2) = (initialGuessLago.at<Pose2>(x2)).theta();
|
|
actualOrientations(3) = (initialGuessLago.at<Pose2>(x3)).theta();
|
|
|
|
EXPECT(assert_equal(expectedOrientations, actualOrientations, 1e-6));
|
|
//DOUBLES_EQUAL(expected, actual, 1e-6);
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
|
|
/* ************************************************************************* */
|
|
|