Moved TransformConstraint into gtsam, cleaned up includes
Alex Cunningham
parent
4f9a60d41c
commit
1c72d92365
|
|
@ -45,6 +45,7 @@ public:
|
|||
*/
|
||||
NonlinearConstraint(size_t dim, double mu = 1000.0):
|
||||
Base(noiseModel::Constrained::All(dim)), mu_(fabs(mu)), dim_(dim) {}
|
||||
virtual ~NonlinearConstraint() {}
|
||||
|
||||
/** returns the gain mu */
|
||||
double mu() const { return mu_; }
|
||||
|
|
@ -116,6 +117,7 @@ public:
|
|||
: Base(dim, mu), key_(key) {
|
||||
this->keys_.push_back(key);
|
||||
}
|
||||
virtual ~NonlinearConstraint1() {}
|
||||
|
||||
/* print */
|
||||
void print(const std::string& s = "") const {
|
||||
|
|
@ -173,6 +175,7 @@ protected:
|
|||
public:
|
||||
NonlinearEqualityConstraint1(const Key& key, size_t dim, double mu = 1000.0)
|
||||
: Base(key, dim, mu) {}
|
||||
virtual ~NonlinearEqualityConstraint1() {}
|
||||
|
||||
/** Always active, so fixed value for active() */
|
||||
virtual bool active(const Config& c) const { return true; }
|
||||
|
|
@ -206,6 +209,7 @@ public:
|
|||
this->keys_.push_back(key1);
|
||||
this->keys_.push_back(key2);
|
||||
}
|
||||
virtual ~NonlinearConstraint2() {}
|
||||
|
||||
/* print */
|
||||
void print(const std::string& s = "") const {
|
||||
|
|
@ -267,11 +271,119 @@ protected:
|
|||
public:
|
||||
NonlinearEqualityConstraint2(const Key1& key1, const Key2& key2, size_t dim, double mu = 1000.0)
|
||||
: Base(key1, key2, dim, mu) {}
|
||||
virtual ~NonlinearEqualityConstraint2() {}
|
||||
|
||||
|
||||
/** Always active, so fixed value for active() */
|
||||
virtual bool active(const Config& c) const { return true; }
|
||||
};
|
||||
|
||||
/**
|
||||
* A ternary constraint
|
||||
*/
|
||||
template <class Config, class Key1, class X1, class Key2, class X2, class Key3, class X3>
|
||||
class NonlinearConstraint3 : public NonlinearConstraint<Config> {
|
||||
|
||||
protected:
|
||||
typedef NonlinearConstraint3<Config,Key1,X1,Key2,X2,Key3,X3> This;
|
||||
typedef NonlinearConstraint<Config> Base;
|
||||
|
||||
/** keys for the constrained variables */
|
||||
Key1 key1_;
|
||||
Key2 key2_;
|
||||
Key3 key3_;
|
||||
|
||||
public:
|
||||
|
||||
/**
|
||||
* Basic constructor
|
||||
* @param key1 is the identifier for the first variable constrained
|
||||
* @param key2 is the identifier for the second variable constrained
|
||||
* @param key3 is the identifier for the second variable constrained
|
||||
* @param dim is the size of the constraint (p)
|
||||
* @param mu is the gain for the factor
|
||||
*/
|
||||
NonlinearConstraint3(const Key1& key1, const Key2& key2, const Key3& key3,
|
||||
size_t dim, double mu = 1000.0) :
|
||||
Base(dim, mu), key1_(key1), key2_(key2), key3_(key3) {
|
||||
this->keys_.push_back(key1);
|
||||
this->keys_.push_back(key2);
|
||||
this->keys_.push_back(key3);
|
||||
}
|
||||
virtual ~NonlinearConstraint3() {}
|
||||
|
||||
/* print */
|
||||
void print(const std::string& s = "") const {
|
||||
std::cout << "NonlinearConstraint3 " << s << std::endl;
|
||||
std::cout << "key1: " << (std::string) key1_ << std::endl;
|
||||
std::cout << "key2: " << (std::string) key2_ << std::endl;
|
||||
std::cout << "key3: " << (std::string) key3_ << std::endl;
|
||||
std::cout << "mu: " << this->mu_ << std::endl;
|
||||
}
|
||||
|
||||
/** Check if two factors are equal. Note type is Factor and needs cast. */
|
||||
virtual bool equals(const Factor<Config>& f, double tol = 1e-9) const {
|
||||
const This* p = dynamic_cast<const This*> (&f);
|
||||
if (p == NULL) return false;
|
||||
return Base::equals(*p, tol) && (key1_ == p->key1_) && (key2_ == p->key2_) && (key3_ == p->key3_);
|
||||
}
|
||||
|
||||
/** error function g(x), switched depending on whether the constraint is active */
|
||||
inline Vector unwhitenedError(const Config& x) const {
|
||||
if (!active(x)) {
|
||||
return zero(this->dim());
|
||||
}
|
||||
const Key1& j1 = key1_;
|
||||
const Key2& j2 = key2_;
|
||||
const Key3& j3 = key3_;
|
||||
const X1& xj1 = x[j1];
|
||||
const X2& xj2 = x[j2];
|
||||
const X3& xj3 = x[j3];
|
||||
return evaluateError(xj1, xj2, xj3);
|
||||
}
|
||||
|
||||
/** Linearize from config */
|
||||
boost::shared_ptr<GaussianFactor> linearize(const Config& c) const {
|
||||
if (!active(c)) {
|
||||
boost::shared_ptr<GaussianFactor> factor;
|
||||
return factor;
|
||||
}
|
||||
const Key1& j1 = key1_; const Key2& j2 = key2_; const Key3& j3 = key3_;
|
||||
const X1& x1 = c[j1]; const X2& x2 = c[j2]; const X3& x3 = c[j3];
|
||||
Matrix grad1, grad2, grad3;
|
||||
Vector g = -1.0 * evaluateError(x1, x2, x3, grad1, grad2, grad3);
|
||||
SharedDiagonal model = noiseModel::Constrained::All(this->dim());
|
||||
return GaussianFactor::shared_ptr(new GaussianFactor(j1, grad1, j2, grad2, j3, grad3, g, model));
|
||||
}
|
||||
|
||||
/** g(x) with optional derivative3 - does not depend on active */
|
||||
virtual Vector evaluateError(const X1& x1, const X2& x2, const X3& x3,
|
||||
boost::optional<Matrix&> H1 = boost::none,
|
||||
boost::optional<Matrix&> H2 = boost::none,
|
||||
boost::optional<Matrix&> H3 = boost::none) const = 0;
|
||||
};
|
||||
|
||||
/**
|
||||
* Ternary Equality constraint - simply forces the value of active() to true
|
||||
*/
|
||||
template <class Config, class Key1, class X1, class Key2, class X2, class Key3, class X3>
|
||||
class NonlinearEqualityConstraint3 : public NonlinearConstraint3<Config, Key1, X1, Key2, X2, Key3, X3> {
|
||||
|
||||
protected:
|
||||
typedef NonlinearEqualityConstraint3<Config,Key1,X1,Key2,X2,Key3,X3> This;
|
||||
typedef NonlinearConstraint3<Config,Key1,X1,Key2,X2,Key3,X3> Base;
|
||||
|
||||
public:
|
||||
NonlinearEqualityConstraint3(const Key1& key1, const Key2& key2, const Key3& key3,
|
||||
size_t dim, double mu = 1000.0)
|
||||
: Base(key1, key2, key3, dim, mu) {}
|
||||
virtual ~NonlinearEqualityConstraint3() {}
|
||||
|
||||
/** Always active, so fixed value for active() */
|
||||
virtual bool active(const Config& c) const { return true; }
|
||||
};
|
||||
|
||||
|
||||
/**
|
||||
* Simple unary equality constraint - fixes a value for a variable
|
||||
*/
|
||||
|
|
@ -288,6 +400,7 @@ public:
|
|||
|
||||
NonlinearEquality1(const X& value, const Key& key1, double mu = 1000.0)
|
||||
: Base(key1, X::dim(), mu), value_(value) {}
|
||||
virtual ~NonlinearEquality1() {}
|
||||
|
||||
/** g(x) with optional derivative */
|
||||
Vector evaluateError(const X& x1, boost::optional<Matrix&> H1 = boost::none) const {
|
||||
|
|
@ -313,6 +426,7 @@ public:
|
|||
|
||||
NonlinearEquality2(const Key& key1, const Key& key2, double mu = 1000.0)
|
||||
: Base(key1, key2, X::dim(), mu) {}
|
||||
virtual ~NonlinearEquality2() {}
|
||||
|
||||
/** g(x) with optional derivative2 */
|
||||
Vector evaluateError(const X& x1, const X& x2,
|
||||
|
|
|
|||
|
|
@ -388,7 +388,7 @@ namespace gtsam {
|
|||
|
||||
/** Print */
|
||||
void print(const std::string& s = "") const {
|
||||
std::cout << "NonlinearFactor2 " << s << std::endl;
|
||||
std::cout << "NonlinearFactor3 " << s << std::endl;
|
||||
std::cout << "key1: " << (std::string) key1_ << std::endl;
|
||||
std::cout << "key2: " << (std::string) key2_ << std::endl;
|
||||
std::cout << "key3: " << (std::string) key3_ << std::endl;
|
||||
|
|
@ -419,7 +419,7 @@ namespace gtsam {
|
|||
boost::shared_ptr<GaussianFactor> linearize(const Config& c) const {
|
||||
const X1& x1 = c[key1_];
|
||||
const X2& x2 = c[key2_];
|
||||
const X2& x3 = c[key3_];
|
||||
const X3& x3 = c[key3_];
|
||||
Matrix A1, A2, A3;
|
||||
Vector b = -evaluateError(x1, x2, x3, A1, A2, A3);
|
||||
// TODO pass unwhitened + noise model to Gaussian factor
|
||||
|
|
|
|||
|
|
@ -6,7 +6,7 @@
|
|||
|
||||
#pragma once
|
||||
|
||||
#include <NonlinearConstraint.h>
|
||||
#include "NonlinearConstraint.h"
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
|
|
|
|||
|
|
@ -6,8 +6,8 @@
|
|||
|
||||
#pragma once
|
||||
|
||||
#include <Lie.h>
|
||||
#include <NonlinearConstraint.h>
|
||||
#include "Lie.h"
|
||||
#include "NonlinearConstraint.h"
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
|
|
|
|||
|
|
@ -28,7 +28,7 @@ check_PROGRAMS += tests/testSimulated3D
|
|||
headers += BetweenFactor.h PriorFactor.h
|
||||
|
||||
# General constraints
|
||||
headers += BetweenConstraint.h BoundingConstraint.h
|
||||
headers += BetweenConstraint.h BoundingConstraint.h TransformConstraint.h
|
||||
|
||||
# 2D Pose SLAM
|
||||
sources += pose2SLAM.cpp Pose2SLAMOptimizer.cpp dataset.cpp
|
||||
|
|
|
|||
|
|
@ -0,0 +1,66 @@
|
|||
/*
|
||||
* @file TransformConstraint.h
|
||||
* @brief A constraint for combining graphs by common landmarks and a transform node
|
||||
* @author Alex Cunningham
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "NonlinearConstraint.h"
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/**
|
||||
* A constraint between two landmarks in separate maps
|
||||
* Templated on:
|
||||
* Config : The overall config
|
||||
* PKey : Key of landmark being constrained
|
||||
* Point : Type of landmark
|
||||
* TKey : Key of transform used
|
||||
* Transform : Transform variable class
|
||||
*
|
||||
* The Point and Transform concepts must be Lie types, and the transform
|
||||
* relationship "Point = transform_from(Transform, Point)" must exist.
|
||||
*
|
||||
* This base class should be specialized to implement the cost function for
|
||||
* specific classes of landmarks
|
||||
*/
|
||||
template<class Config, class PKey, class Point, class TKey, class Transform>
|
||||
class TransformConstraint : public NonlinearEqualityConstraint3<Config, PKey, Point, TKey, Transform, PKey, Point> {
|
||||
|
||||
public:
|
||||
typedef NonlinearEqualityConstraint3<Config, PKey, Point, TKey, Transform, PKey, Point> Base;
|
||||
typedef TransformConstraint<Config, PKey, Point, TKey, Transform> This;
|
||||
|
||||
/**
|
||||
* General constructor with all of the keys to variables in the map
|
||||
* Extracts everything necessary from the key types
|
||||
*/
|
||||
TransformConstraint(const PKey& foreignKey, const TKey& transKey, const PKey& localKey, double mu = 1000.0)
|
||||
: Base(foreignKey, transKey, localKey, Point().dim(), mu) {}
|
||||
|
||||
virtual ~TransformConstraint(){}
|
||||
|
||||
/** Combined cost and derivative function using boost::optional */
|
||||
virtual Vector evaluateError(const Point& foreign, const Transform& trans, const Point& local,
|
||||
boost::optional<Matrix&> Dforeign = boost::none,
|
||||
boost::optional<Matrix&> Dtrans = boost::none,
|
||||
boost::optional<Matrix&> Dlocal = boost::none) const {
|
||||
if (Dforeign) {
|
||||
Matrix Af;
|
||||
transform_from(trans, foreign, boost::none, Af);
|
||||
*Dforeign = Af;
|
||||
}
|
||||
if (Dtrans) {
|
||||
Matrix At;
|
||||
transform_from(trans, foreign, At, boost::none);
|
||||
*Dtrans = At;
|
||||
}
|
||||
if (Dlocal) {
|
||||
Point dummy;
|
||||
*Dlocal = -1* eye(dummy.dim());
|
||||
}
|
||||
return gtsam::logmap(gtsam::between<Point>(local, transform_from(trans, foreign)));
|
||||
}
|
||||
};
|
||||
} // \namespace gtsam
|
||||
|
|
@ -10,10 +10,10 @@
|
|||
|
||||
#include <numericalDerivative.h> // TODO: remove
|
||||
|
||||
#include <NonlinearConstraint.h>
|
||||
#include <BetweenConstraint.h>
|
||||
#include <BoundingConstraint.h>
|
||||
#include <simulated2D.h>
|
||||
#include "NonlinearConstraint.h"
|
||||
#include "BetweenConstraint.h"
|
||||
#include "BoundingConstraint.h"
|
||||
#include "simulated2D.h"
|
||||
|
||||
// \namespace
|
||||
|
||||
|
|
|
|||
|
|
@ -11,6 +11,7 @@ check_PROGRAMS += testNonlinearEquality testNonlinearFactor testNonlinearFactorG
|
|||
check_PROGRAMS += testNonlinearOptimizer testSubgraphPreconditioner
|
||||
check_PROGRAMS += testSymbolicBayesNet testSymbolicFactorGraph testTupleConfig
|
||||
check_PROGRAMS += testNonlinearEqualityConstraint testBoundingConstraint
|
||||
check_PROGRAMS += testTransformConstraint
|
||||
|
||||
if USE_LDL
|
||||
check_PROGRAMS += testConstraintOptimizer
|
||||
|
|
|
|||
|
|
@ -0,0 +1,263 @@
|
|||
/*
|
||||
* @file testTransformConstraint.cpp
|
||||
* @author Alex Cunningham
|
||||
*/
|
||||
|
||||
#include <iostream>
|
||||
|
||||
#include <boost/bind.hpp>
|
||||
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
|
||||
#include <Ordering.h>
|
||||
#include <Key.h>
|
||||
#include <numericalDerivative.h>
|
||||
|
||||
#include <Pose2.h>
|
||||
#include <Point2.h>
|
||||
|
||||
#include <TransformConstraint.h>
|
||||
#include <NonlinearEquality.h>
|
||||
|
||||
// implementations
|
||||
#include <LieConfig-inl.h>
|
||||
#include <TupleConfig-inl.h>
|
||||
#include <NonlinearFactorGraph-inl.h>
|
||||
#include <NonlinearOptimizer-inl.h>
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
using namespace boost;
|
||||
|
||||
typedef TypedSymbol<Pose2, 'x'> PoseKey;
|
||||
typedef TypedSymbol<Point2, 'l'> PointKey;
|
||||
typedef TypedSymbol<Pose2, 'T'> TransformKey;
|
||||
|
||||
typedef LieConfig<PoseKey, Pose2> PoseConfig;
|
||||
typedef LieConfig<PointKey, Point2> PointConfig;
|
||||
typedef LieConfig<TransformKey, Pose2> TransformConfig;
|
||||
|
||||
typedef TupleConfig3< PoseConfig, PointConfig, TransformConfig > DDFConfig;
|
||||
typedef NonlinearFactorGraph<DDFConfig> DDFGraph;
|
||||
typedef NonlinearOptimizer<DDFGraph, DDFConfig> Optimizer;
|
||||
|
||||
typedef NonlinearEquality<DDFConfig, PoseKey, Pose2> PoseConstraint;
|
||||
typedef NonlinearEquality<DDFConfig, PointKey, Point2> PointConstraint;
|
||||
typedef NonlinearEquality<DDFConfig, TransformKey, Pose2> TransformPriorConstraint;
|
||||
|
||||
typedef TransformConstraint<DDFConfig, PointKey, Point2, TransformKey, Pose2> PointTransformConstraint;
|
||||
|
||||
PointKey lA1(1), lA2(2), lB1(3);
|
||||
TransformKey t1(1);
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( TransformConstraint, equals ) {
|
||||
PointTransformConstraint c1(lB1, t1, lA1),
|
||||
c2(lB1, t1, lA1),
|
||||
c3(lB1, t1, lA2);
|
||||
|
||||
CHECK(assert_equal(c1, c1));
|
||||
CHECK(assert_equal(c1, c2));
|
||||
CHECK(!c1.equals(c3));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( TransformConstraint, jacobians ) {
|
||||
|
||||
// from examples below
|
||||
Point2 local(2.0, 3.0), global(-1.0, 2.0);
|
||||
Pose2 trans(1.5, 2.5, 0.3);
|
||||
|
||||
PointTransformConstraint tc(lA1, t1, lB1);
|
||||
Matrix actualDT, actualDL, actualDF;
|
||||
tc.evaluateError(global, trans, local, actualDF, actualDT, actualDL);
|
||||
|
||||
Matrix numericalDT, numericalDL, numericalDF;
|
||||
numericalDF = numericalDerivative31<Vector, Point2, Pose2, Point2>(
|
||||
boost::bind(&PointTransformConstraint::evaluateError, ref(tc), _1, _2, _3, boost::none, boost::none, boost::none),
|
||||
global, trans, local, 1e-5);
|
||||
numericalDT = numericalDerivative32<Vector, Point2, Pose2, Point2>(
|
||||
boost::bind(&PointTransformConstraint::evaluateError, ref(tc), _1, _2, _3, boost::none, boost::none, boost::none),
|
||||
global, trans, local, 1e-5);
|
||||
numericalDL = numericalDerivative33<Vector, Point2, Pose2, Point2>(
|
||||
boost::bind(&PointTransformConstraint::evaluateError, ref(tc), _1, _2, _3, boost::none, boost::none, boost::none),
|
||||
global, trans, local, 1e-5);
|
||||
|
||||
CHECK(assert_equal(numericalDF, actualDF));
|
||||
CHECK(assert_equal(numericalDL, actualDL));
|
||||
CHECK(assert_equal(numericalDT, actualDT));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( TransformConstraint, jacobians_zero ) {
|
||||
|
||||
// get values that are ideal
|
||||
Pose2 trans(2.0, 3.0, 0.0);
|
||||
Point2 global(5.0, 6.0);
|
||||
Point2 local = transform_from(trans, global);
|
||||
|
||||
PointTransformConstraint tc(lA1, t1, lB1);
|
||||
Vector actCost = tc.evaluateError(global, trans, local),
|
||||
expCost = zero(2);
|
||||
CHECK(assert_equal(expCost, actCost, 1e-5));
|
||||
|
||||
Matrix actualDT, actualDL, actualDF;
|
||||
tc.evaluateError(global, trans, local, actualDF, actualDT, actualDL);
|
||||
|
||||
Matrix numericalDT, numericalDL, numericalDF;
|
||||
numericalDF = numericalDerivative31<Vector, Point2, Pose2, Point2>(
|
||||
boost::bind(&PointTransformConstraint::evaluateError, ref(tc), _1, _2, _3, boost::none, boost::none, boost::none),
|
||||
global, trans, local, 1e-5);
|
||||
numericalDT = numericalDerivative32<Vector, Point2, Pose2, Point2>(
|
||||
boost::bind(&PointTransformConstraint::evaluateError, ref(tc), _1, _2, _3, boost::none, boost::none, boost::none),
|
||||
global, trans, local, 1e-5);
|
||||
numericalDL = numericalDerivative33<Vector, Point2, Pose2, Point2>(
|
||||
boost::bind(&PointTransformConstraint::evaluateError, ref(tc), _1, _2, _3, boost::none, boost::none, boost::none),
|
||||
global, trans, local, 1e-5);
|
||||
|
||||
CHECK(assert_equal(numericalDF, actualDF));
|
||||
CHECK(assert_equal(numericalDL, actualDL));
|
||||
CHECK(assert_equal(numericalDT, actualDT));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( TransformConstraint, 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
|
||||
CHECK(assert_equal(local1, transform_from(transIdeal, global1)));
|
||||
CHECK(assert_equal(local2, transform_from(transIdeal, 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
|
||||
|
||||
// keys
|
||||
PointKey localK1(1), localK2(2),
|
||||
globalK1(3), globalK2(4);
|
||||
TransformKey transK(1);
|
||||
|
||||
DDFGraph graph;
|
||||
|
||||
graph.add(PointTransformConstraint(globalK1, transK, localK1));
|
||||
graph.add(PointTransformConstraint(globalK2, transK, localK2));
|
||||
|
||||
// hard constraints on points
|
||||
double error_gain = 1000.0;
|
||||
graph.add(PointConstraint(localK1, local1, error_gain));
|
||||
graph.add(PointConstraint(localK2, local2, error_gain));
|
||||
graph.add(PointConstraint(globalK1, global1, error_gain));
|
||||
graph.add(PointConstraint(globalK2, global2, error_gain));
|
||||
|
||||
// create initial estimate
|
||||
DDFConfig init;
|
||||
init.insert(localK1, local1);
|
||||
init.insert(localK2, local2);
|
||||
init.insert(globalK1, global1);
|
||||
init.insert(globalK2, global2);
|
||||
init.insert(transK, trans);
|
||||
|
||||
// optimize
|
||||
Optimizer::shared_config actual = Optimizer::optimizeLM(graph, init);
|
||||
|
||||
DDFConfig expected;
|
||||
expected.insert(localK1, local1);
|
||||
expected.insert(localK2, local2);
|
||||
expected.insert(globalK1, global1);
|
||||
expected.insert(globalK2, global2);
|
||||
expected.insert(transK, transIdeal);
|
||||
|
||||
CHECK(assert_equal(expected, *actual, 1e-4));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( TransformConstraint, 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 = transform_from(trans, 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
|
||||
|
||||
|
||||
// keys
|
||||
PointKey localK(1), globalK(2);
|
||||
TransformKey transK(1);
|
||||
|
||||
DDFGraph graph;
|
||||
double error_gain = 1000.0;
|
||||
graph.add(PointTransformConstraint(globalK, transK, localK));
|
||||
graph.add(PointConstraint(globalK, global, error_gain));
|
||||
graph.add(TransformPriorConstraint(transK, trans, error_gain));
|
||||
|
||||
// create initial estimate
|
||||
DDFConfig init;
|
||||
init.insert(localK, local);
|
||||
init.insert(globalK, global);
|
||||
init.insert(transK, trans);
|
||||
|
||||
// optimize
|
||||
Optimizer::shared_config actual = Optimizer::optimizeLM(graph, init);
|
||||
|
||||
CHECK(assert_equal(idealLocal, actual->at(localK), 1e-5));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( TransformConstraint, 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 = transform_from(inverse(trans), 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
|
||||
|
||||
// keys
|
||||
PointKey localK(1), globalK(2);
|
||||
TransformKey transK(1);
|
||||
|
||||
DDFGraph graph;
|
||||
double error_gain = 1000.0;
|
||||
graph.add(PointTransformConstraint(globalK, transK, localK));
|
||||
graph.add(PointConstraint(localK, local, error_gain));
|
||||
graph.add(TransformPriorConstraint(transK, trans, error_gain));
|
||||
|
||||
// create initial estimate
|
||||
DDFConfig init;
|
||||
init.insert(localK, local);
|
||||
init.insert(globalK, global);
|
||||
init.insert(transK, trans);
|
||||
|
||||
// optimize
|
||||
Optimizer::shared_config actual = Optimizer::optimizeLM(graph, init);
|
||||
|
||||
// verify
|
||||
CHECK(assert_equal(idealForeign, actual->at(globalK), 1e-5));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
|
||||
/* ************************************************************************* */
|
||||
|
||||
|
||||
Loading…
Reference in New Issue