249 lines
8.5 KiB
C++
249 lines
8.5 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 testReferenceFrameFactor.cpp
|
|
* @author Alex Cunningham
|
|
*/
|
|
|
|
#include <iostream>
|
|
|
|
#include <boost/bind.hpp>
|
|
|
|
#include <CppUnitLite/TestHarness.h>
|
|
|
|
#include <gtsam/base/Testable.h>
|
|
#include <gtsam/base/numericalDerivative.h>
|
|
#include <gtsam/geometry/Pose2.h>
|
|
#include <gtsam/nonlinear/Symbol.h>
|
|
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
|
|
#include <gtsam/nonlinear/NonlinearEquality.h>
|
|
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
|
|
|
|
#include <gtsam_unstable/slam/ReferenceFrameFactor.h>
|
|
|
|
using namespace std;
|
|
using namespace boost;
|
|
using namespace gtsam;
|
|
|
|
typedef gtsam::ReferenceFrameFactor<gtsam::Point2, gtsam::Pose2> PointReferenceFrameFactor;
|
|
typedef gtsam::ReferenceFrameFactor<gtsam::Pose2, gtsam::Pose2> PoseReferenceFrameFactor;
|
|
|
|
Key lA1 = symbol_shorthand::L(1), lA2 = symbol_shorthand::L(2), lB1 = symbol_shorthand::L(11), lB2 = symbol_shorthand::L(12);
|
|
Key tA1 = symbol_shorthand::T(1), tB1 = symbol_shorthand::T(2);
|
|
|
|
/* ************************************************************************* */
|
|
TEST( ReferenceFrameFactor, equals ) {
|
|
PointReferenceFrameFactor
|
|
c1(lB1, tA1, lA1),
|
|
c2(lB1, tA1, lA1),
|
|
c3(lB1, tA1, lA2);
|
|
|
|
EXPECT(assert_equal(c1, c1));
|
|
EXPECT(assert_equal(c1, c2));
|
|
EXPECT(!c1.equals(c3));
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
LieVector evaluateError_(const PointReferenceFrameFactor& c,
|
|
const Point2& global, const Pose2& trans, const Point2& local) {
|
|
return LieVector(c.evaluateError(global, trans, local));
|
|
}
|
|
TEST( ReferenceFrameFactor, jacobians ) {
|
|
|
|
// from examples below
|
|
Point2 local(2.0, 3.0), global(-1.0, 2.0);
|
|
Pose2 trans(1.5, 2.5, 0.3);
|
|
|
|
PointReferenceFrameFactor tc(lA1, tA1, lB1);
|
|
Matrix actualDT, actualDL, actualDF;
|
|
tc.evaluateError(global, trans, local, actualDF, actualDT, actualDL);
|
|
|
|
Matrix numericalDT, numericalDL, numericalDF;
|
|
numericalDF = numericalDerivative31<LieVector,Point2,Pose2,Point2>(
|
|
boost::bind(evaluateError_, tc, _1, _2, _3),
|
|
global, trans, local, 1e-5);
|
|
numericalDT = numericalDerivative32<LieVector,Point2,Pose2,Point2>(
|
|
boost::bind(evaluateError_, tc, _1, _2, _3),
|
|
global, trans, local, 1e-5);
|
|
numericalDL = numericalDerivative33<LieVector,Point2,Pose2,Point2>(
|
|
boost::bind(evaluateError_, tc, _1, _2, _3),
|
|
global, trans, local, 1e-5);
|
|
|
|
EXPECT(assert_equal(numericalDF, actualDF));
|
|
EXPECT(assert_equal(numericalDL, actualDL));
|
|
EXPECT(assert_equal(numericalDT, actualDT));
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
TEST( ReferenceFrameFactor, jacobians_zero ) {
|
|
|
|
// get values that are ideal
|
|
Pose2 trans(2.0, 3.0, 0.0);
|
|
Point2 global(5.0, 6.0);
|
|
Point2 local = trans.transform_from(global);
|
|
|
|
PointReferenceFrameFactor tc(lA1, tA1, lB1);
|
|
Vector actCost = tc.evaluateError(global, trans, local),
|
|
expCost = zero(2);
|
|
EXPECT(assert_equal(expCost, actCost, 1e-5));
|
|
|
|
Matrix actualDT, actualDL, actualDF;
|
|
tc.evaluateError(global, trans, local, actualDF, actualDT, actualDL);
|
|
|
|
Matrix numericalDT, numericalDL, numericalDF;
|
|
numericalDF = numericalDerivative31<LieVector,Point2,Pose2,Point2>(
|
|
boost::bind(evaluateError_, tc, _1, _2, _3),
|
|
global, trans, local, 1e-5);
|
|
numericalDT = numericalDerivative32<LieVector,Point2,Pose2,Point2>(
|
|
boost::bind(evaluateError_, tc, _1, _2, _3),
|
|
global, trans, local, 1e-5);
|
|
numericalDL = numericalDerivative33<LieVector,Point2,Pose2,Point2>(
|
|
boost::bind(evaluateError_, tc, _1, _2, _3),
|
|
global, trans, local, 1e-5);
|
|
|
|
EXPECT(assert_equal(numericalDF, actualDF));
|
|
EXPECT(assert_equal(numericalDL, actualDL));
|
|
EXPECT(assert_equal(numericalDT, actualDT));
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
TEST_UNSAFE( ReferenceFrameFactor, converge_trans ) {
|
|
|
|
// initial points
|
|
Point2 local1(2.0, 2.0), local2(4.0, 5.0),
|
|
global1(-1.0, 5.0), global2(2.0, 3.0);
|
|
Pose2 transIdeal(7.0, 3.0, M_PI/2);
|
|
|
|
// verify direction
|
|
EXPECT(assert_equal(local1, transIdeal.transform_from(global1)));
|
|
EXPECT(assert_equal(local2, transIdeal.transform_from(global2)));
|
|
|
|
// choose transform
|
|
// Pose2 trans = transIdeal; // ideal - works
|
|
// Pose2 trans = transIdeal * Pose2(0.1, 1.0, 0.00); // translation - works
|
|
// Pose2 trans = transIdeal * Pose2(10.1, 1.0, 0.00); // large translation - works
|
|
// Pose2 trans = transIdeal * Pose2(0.0, 0.0, 0.1); // small rotation - works
|
|
Pose2 trans = transIdeal * Pose2(-200.0, 100.0, 1.3); // combined - works
|
|
// Pose2 trans = transIdeal * Pose2(-200.0, 100.0, 2.0); // beyond pi/2 - fails
|
|
|
|
NonlinearFactorGraph graph;
|
|
graph.add(PointReferenceFrameFactor(lB1, tA1, lA1));
|
|
graph.add(PointReferenceFrameFactor(lB2, tA1, lA2));
|
|
|
|
// hard constraints on points
|
|
double error_gain = 1000.0;
|
|
graph.add(NonlinearEquality<gtsam::Point2>(lA1, local1, error_gain));
|
|
graph.add(NonlinearEquality<gtsam::Point2>(lA2, local2, error_gain));
|
|
graph.add(NonlinearEquality<gtsam::Point2>(lB1, global1, error_gain));
|
|
graph.add(NonlinearEquality<gtsam::Point2>(lB2, global2, error_gain));
|
|
|
|
// create initial estimate
|
|
Values init;
|
|
init.insert(lA1, local1);
|
|
init.insert(lA2, local2);
|
|
init.insert(lB1, global1);
|
|
init.insert(lB2, global2);
|
|
init.insert(tA1, trans);
|
|
|
|
// optimize
|
|
LevenbergMarquardtOptimizer solver(graph, init);
|
|
Values actual = solver.optimize();
|
|
|
|
Values expected;
|
|
expected.insert(lA1, local1);
|
|
expected.insert(lA2, local2);
|
|
expected.insert(lB1, global1);
|
|
expected.insert(lB2, global2);
|
|
expected.insert(tA1, transIdeal);
|
|
|
|
EXPECT(assert_equal(expected, actual, 1e-4));
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
TEST( ReferenceFrameFactor, converge_local ) {
|
|
|
|
// initial points
|
|
Point2 global(-1.0, 2.0);
|
|
// Pose2 trans(1.5, 2.5, 0.3); // original
|
|
// Pose2 trans(1.5, 2.5, 1.0); // larger rotation
|
|
Pose2 trans(1.5, 2.5, 3.1); // significant rotation
|
|
|
|
Point2 idealLocal = trans.transform_from(global);
|
|
|
|
// perturb the initial estimate
|
|
// Point2 local = idealLocal; // Ideal case - works
|
|
// Point2 local = idealLocal + Point2(1.0, 0.0); // works
|
|
Point2 local = idealLocal + Point2(-10.0, 10.0); // works
|
|
|
|
NonlinearFactorGraph graph;
|
|
double error_gain = 1000.0;
|
|
graph.add(PointReferenceFrameFactor(lB1, tA1, lA1));
|
|
graph.add(NonlinearEquality<gtsam::Point2>(lB1, global, error_gain));
|
|
graph.add(NonlinearEquality<gtsam::Pose2>(tA1, trans, error_gain));
|
|
|
|
// create initial estimate
|
|
Values init;
|
|
init.insert(lA1, local);
|
|
init.insert(lB1, global);
|
|
init.insert(tA1, trans);
|
|
|
|
// optimize
|
|
LevenbergMarquardtOptimizer solver(graph, init);
|
|
Values actual = solver.optimize();
|
|
|
|
CHECK(actual.exists(lA1));
|
|
EXPECT(assert_equal(idealLocal, actual.at<Point2>(lA1), 1e-5));
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
TEST( ReferenceFrameFactor, converge_global ) {
|
|
|
|
// initial points
|
|
Point2 local(2.0, 3.0);
|
|
// Pose2 trans(1.5, 2.5, 0.3); // original
|
|
// Pose2 trans(1.5, 2.5, 1.0); // larger rotation
|
|
Pose2 trans(1.5, 2.5, 3.1); // significant rotation
|
|
|
|
Point2 idealForeign = trans.inverse().transform_from(local);
|
|
|
|
// perturb the initial estimate
|
|
// Point2 global = idealForeign; // Ideal - works
|
|
// Point2 global = idealForeign + Point2(1.0, 0.0); // simple - works
|
|
Point2 global = idealForeign + Point2(10.0, -10.0); // larger - works
|
|
|
|
NonlinearFactorGraph graph;
|
|
double error_gain = 1000.0;
|
|
graph.add(PointReferenceFrameFactor(lB1, tA1, lA1));
|
|
graph.add(NonlinearEquality<gtsam::Point2>(lA1, local, error_gain));
|
|
graph.add(NonlinearEquality<gtsam::Pose2>(tA1, trans, error_gain));
|
|
|
|
// create initial estimate
|
|
Values init;
|
|
init.insert(lA1, local);
|
|
init.insert(lB1, global);
|
|
init.insert(tA1, trans);
|
|
|
|
// optimize
|
|
LevenbergMarquardtOptimizer solver(graph, init);
|
|
Values actual = solver.optimize();
|
|
|
|
// verify
|
|
CHECK(actual.exists(lB1));
|
|
EXPECT(assert_equal(idealForeign, actual.at<Point2>(lB1), 1e-5));
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
|
|
/* ************************************************************************* */
|
|
|
|
|