Removed use of GTSAM_MAGIC_KEY for constructing Symbols from strings
parent
565185da02
commit
1d0aaacbd6
|
@ -20,9 +20,6 @@
|
||||||
#include <boost/foreach.hpp>
|
#include <boost/foreach.hpp>
|
||||||
using namespace boost;
|
using namespace boost;
|
||||||
|
|
||||||
// Magically casts strings like "x3" to a Symbol('x',3) key, see Symbol.h
|
|
||||||
#define GTSAM_MAGIC_KEY
|
|
||||||
|
|
||||||
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
|
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
|
||||||
#include <gtsam/nonlinear/NonlinearOptimizer.h>
|
#include <gtsam/nonlinear/NonlinearOptimizer.h>
|
||||||
#include <gtsam/slam/visualSLAM.h>
|
#include <gtsam/slam/visualSLAM.h>
|
||||||
|
|
|
@ -19,9 +19,6 @@
|
||||||
#include <boost/shared_ptr.hpp>
|
#include <boost/shared_ptr.hpp>
|
||||||
using namespace boost;
|
using namespace boost;
|
||||||
|
|
||||||
// Magically casts strings like "x3" to a Symbol('x',3) key, see Symbol.h
|
|
||||||
#define GTSAM_MAGIC_KEY
|
|
||||||
|
|
||||||
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
|
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
|
||||||
#include <gtsam/nonlinear/NonlinearOptimizer.h>
|
#include <gtsam/nonlinear/NonlinearOptimizer.h>
|
||||||
#include <gtsam/slam/visualSLAM.h>
|
#include <gtsam/slam/visualSLAM.h>
|
||||||
|
|
|
@ -25,9 +25,6 @@ using namespace boost::assign;
|
||||||
|
|
||||||
#include <CppUnitLite/TestHarness.h>
|
#include <CppUnitLite/TestHarness.h>
|
||||||
|
|
||||||
// Magically casts strings like "x3" to a Symbol('x',3) key, see Symbol.h
|
|
||||||
#define GTSAM_MAGIC_KEY
|
|
||||||
|
|
||||||
#include <gtsam/inference/SymbolicFactorGraph.h>
|
#include <gtsam/inference/SymbolicFactorGraph.h>
|
||||||
|
|
||||||
using namespace std;
|
using namespace std;
|
||||||
|
|
|
@ -21,9 +21,6 @@ using namespace boost::assign;
|
||||||
|
|
||||||
#include <CppUnitLite/TestHarness.h>
|
#include <CppUnitLite/TestHarness.h>
|
||||||
|
|
||||||
// Magically casts strings like "x3" to a Symbol('x',3) key, see Symbol.h
|
|
||||||
#define GTSAM_MAGIC_KEY
|
|
||||||
|
|
||||||
#include <gtsam/inference/BayesNet.h>
|
#include <gtsam/inference/BayesNet.h>
|
||||||
#include <gtsam/inference/IndexConditional.h>
|
#include <gtsam/inference/IndexConditional.h>
|
||||||
#include <gtsam/inference/SymbolicFactorGraph.h>
|
#include <gtsam/inference/SymbolicFactorGraph.h>
|
||||||
|
|
|
@ -23,14 +23,9 @@
|
||||||
#include <boost/mpl/char.hpp>
|
#include <boost/mpl/char.hpp>
|
||||||
#include <boost/format.hpp>
|
#include <boost/format.hpp>
|
||||||
#include <boost/serialization/nvp.hpp>
|
#include <boost/serialization/nvp.hpp>
|
||||||
#ifdef GTSAM_MAGIC_KEY
|
|
||||||
#include <boost/lexical_cast.hpp>
|
|
||||||
#endif
|
|
||||||
|
|
||||||
#include <gtsam/nonlinear/Key.h>
|
#include <gtsam/nonlinear/Key.h>
|
||||||
|
|
||||||
#define ALPHA '\224'
|
|
||||||
|
|
||||||
namespace gtsam {
|
namespace gtsam {
|
||||||
|
|
||||||
/**
|
/**
|
||||||
|
@ -60,35 +55,6 @@ public:
|
||||||
c_(c), j_(j) {
|
c_(c), j_(j) {
|
||||||
}
|
}
|
||||||
|
|
||||||
/** "Magic" key casting constructor from string */
|
|
||||||
#ifdef GTSAM_MAGIC_KEY
|
|
||||||
Symbol(const std::string& str) {
|
|
||||||
if(str.length() < 1)
|
|
||||||
throw std::invalid_argument("Cannot parse string key '" + str + "'");
|
|
||||||
else {
|
|
||||||
const char *c_str = str.c_str();
|
|
||||||
c_ = c_str[0];
|
|
||||||
if(str.length() > 1)
|
|
||||||
j_ = boost::lexical_cast<size_t>(c_str+1);
|
|
||||||
else
|
|
||||||
j_ = 0;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
Symbol(const char *c_str) {
|
|
||||||
std::string str(c_str);
|
|
||||||
if(str.length() < 1)
|
|
||||||
throw std::invalid_argument("Cannot parse string key '" + str + "'");
|
|
||||||
else {
|
|
||||||
c_ = c_str[0];
|
|
||||||
if(str.length() > 1)
|
|
||||||
j_ = boost::lexical_cast<size_t>(c_str+1);
|
|
||||||
else
|
|
||||||
j_ = 0;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
#endif
|
|
||||||
|
|
||||||
/** Constructor that decodes an integer Key */
|
/** Constructor that decodes an integer Key */
|
||||||
Symbol(Key key) {
|
Symbol(Key key) {
|
||||||
const size_t keyBytes = sizeof(Key);
|
const size_t keyBytes = sizeof(Key);
|
||||||
|
|
|
@ -20,8 +20,6 @@
|
||||||
#include <boost/assign/std/list.hpp> // for operator +=
|
#include <boost/assign/std/list.hpp> // for operator +=
|
||||||
using namespace boost::assign;
|
using namespace boost::assign;
|
||||||
|
|
||||||
#define GTSAM_MAGIC_KEY
|
|
||||||
|
|
||||||
#include <gtsam/base/Testable.h>
|
#include <gtsam/base/Testable.h>
|
||||||
#include <gtsam/base/TestableAssertions.h>
|
#include <gtsam/base/TestableAssertions.h>
|
||||||
#include <gtsam/base/LieVector.h>
|
#include <gtsam/base/LieVector.h>
|
||||||
|
@ -33,7 +31,7 @@ using namespace gtsam;
|
||||||
using namespace std;
|
using namespace std;
|
||||||
static double inf = std::numeric_limits<double>::infinity();
|
static double inf = std::numeric_limits<double>::infinity();
|
||||||
|
|
||||||
Key key1(Symbol("v1")), key2(Symbol("v2")), key3(Symbol("v3")), key4(Symbol("v4"));
|
Key key1(Symbol('v',1)), key2(Symbol('v',2)), key3(Symbol('v',3)), key4(Symbol('v',4));
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
TEST( Values, equals1 )
|
TEST( Values, equals1 )
|
||||||
|
|
|
@ -24,9 +24,6 @@
|
||||||
|
|
||||||
using namespace std;
|
using namespace std;
|
||||||
|
|
||||||
// Magically casts strings like Symbol("x3") to a Symbol('x',3) key, see Symbol.h
|
|
||||||
#define GTSAM_MAGIC_KEY
|
|
||||||
|
|
||||||
#include <gtsam/base/Matrix.h>
|
#include <gtsam/base/Matrix.h>
|
||||||
#include <gtsam/nonlinear/NonlinearFactor.h>
|
#include <gtsam/nonlinear/NonlinearFactor.h>
|
||||||
#include <gtsam/slam/smallExample.h>
|
#include <gtsam/slam/smallExample.h>
|
||||||
|
@ -52,6 +49,9 @@ namespace example {
|
||||||
static const Index _l1_=0, _x1_=1, _x2_=2;
|
static const Index _l1_=0, _x1_=1, _x2_=2;
|
||||||
static const Index _x_=0, _y_=1, _z_=2;
|
static const Index _x_=0, _y_=1, _z_=2;
|
||||||
|
|
||||||
|
Key kx(size_t i) { return Symbol('x',i); }
|
||||||
|
Key kl(size_t i) { return Symbol('l',i); }
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
boost::shared_ptr<const Graph> sharedNonlinearFactorGraph() {
|
boost::shared_ptr<const Graph> sharedNonlinearFactorGraph() {
|
||||||
// Create
|
// Create
|
||||||
|
@ -121,18 +121,18 @@ namespace example {
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
VectorValues createCorrectDelta(const Ordering& ordering) {
|
VectorValues createCorrectDelta(const Ordering& ordering) {
|
||||||
VectorValues c(vector<size_t>(3,2));
|
VectorValues c(vector<size_t>(3,2));
|
||||||
c[ordering[Symbol(Symbol("l1"))]] = Vector_(2, -0.1, 0.1);
|
c[ordering[kl(1)]] = Vector_(2, -0.1, 0.1);
|
||||||
c[ordering[Symbol("x1")]] = Vector_(2, -0.1, -0.1);
|
c[ordering[kx(1)]] = Vector_(2, -0.1, -0.1);
|
||||||
c[ordering[Symbol("x2")]] = Vector_(2, 0.1, -0.2);
|
c[ordering[kx(2)]] = Vector_(2, 0.1, -0.2);
|
||||||
return c;
|
return c;
|
||||||
}
|
}
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
VectorValues createZeroDelta(const Ordering& ordering) {
|
VectorValues createZeroDelta(const Ordering& ordering) {
|
||||||
VectorValues c(vector<size_t>(3,2));
|
VectorValues c(vector<size_t>(3,2));
|
||||||
c[ordering[Symbol(Symbol("l1"))]] = zero(2);
|
c[ordering[kl(1)]] = zero(2);
|
||||||
c[ordering[Symbol("x1")]] = zero(2);
|
c[ordering[kx(1)]] = zero(2);
|
||||||
c[ordering[Symbol("x2")]] = zero(2);
|
c[ordering[kx(2)]] = zero(2);
|
||||||
return c;
|
return c;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -144,16 +144,16 @@ namespace example {
|
||||||
SharedDiagonal unit2 = noiseModel::Unit::Create(2);
|
SharedDiagonal unit2 = noiseModel::Unit::Create(2);
|
||||||
|
|
||||||
// linearized prior on x1: c[_x1_]+x1=0 i.e. x1=-c[_x1_]
|
// linearized prior on x1: c[_x1_]+x1=0 i.e. x1=-c[_x1_]
|
||||||
fg.add(ordering[Symbol("x1")], 10*eye(2), -1.0*ones(2), unit2);
|
fg.add(ordering[kx(1)], 10*eye(2), -1.0*ones(2), unit2);
|
||||||
|
|
||||||
// odometry between x1 and x2: x2-x1=[0.2;-0.1]
|
// odometry between x1 and x2: x2-x1=[0.2;-0.1]
|
||||||
fg.add(ordering[Symbol("x1")], -10*eye(2),ordering[Symbol("x2")], 10*eye(2), Vector_(2, 2.0, -1.0), unit2);
|
fg.add(ordering[kx(1)], -10*eye(2),ordering[kx(2)], 10*eye(2), Vector_(2, 2.0, -1.0), unit2);
|
||||||
|
|
||||||
// measurement between x1 and l1: l1-x1=[0.0;0.2]
|
// measurement between x1 and l1: l1-x1=[0.0;0.2]
|
||||||
fg.add(ordering[Symbol("x1")], -5*eye(2), ordering[Symbol("l1")], 5*eye(2), Vector_(2, 0.0, 1.0), unit2);
|
fg.add(ordering[kx(1)], -5*eye(2), ordering[kl(1)], 5*eye(2), Vector_(2, 0.0, 1.0), unit2);
|
||||||
|
|
||||||
// measurement between x2 and l1: l1-x2=[-0.2;0.3]
|
// measurement between x2 and l1: l1-x2=[-0.2;0.3]
|
||||||
fg.add(ordering[Symbol("x2")], -5*eye(2), ordering[Symbol("l1")], 5*eye(2), Vector_(2, -1.0, 1.5), unit2);
|
fg.add(ordering[kx(2)], -5*eye(2), ordering[kl(1)], 5*eye(2), Vector_(2, -1.0, 1.5), unit2);
|
||||||
|
|
||||||
return *fg.dynamicCastFactors<FactorGraph<JacobianFactor> >();
|
return *fg.dynamicCastFactors<FactorGraph<JacobianFactor> >();
|
||||||
}
|
}
|
||||||
|
|
|
@ -12,9 +12,6 @@
|
||||||
#include <CppUnitLite/TestHarness.h>
|
#include <CppUnitLite/TestHarness.h>
|
||||||
using namespace boost;
|
using namespace boost;
|
||||||
|
|
||||||
// Magically casts strings like "x3" to a Symbol('x',3) key, see Symbol.h
|
|
||||||
#define GTSAM_MAGIC_KEY
|
|
||||||
|
|
||||||
#include <gtsam/base/Testable.h>
|
#include <gtsam/base/Testable.h>
|
||||||
#include <gtsam/geometry/Cal3_S2.h>
|
#include <gtsam/geometry/Cal3_S2.h>
|
||||||
#include <gtsam/geometry/PinholeCamera.h>
|
#include <gtsam/geometry/PinholeCamera.h>
|
||||||
|
@ -74,7 +71,7 @@ TEST( GeneralSFMFactor, equals )
|
||||||
{
|
{
|
||||||
// Create two identical factors and make sure they're equal
|
// Create two identical factors and make sure they're equal
|
||||||
Vector z = Vector_(2,323.,240.);
|
Vector z = Vector_(2,323.,240.);
|
||||||
const Symbol cameraFrameNumber="x1", landmarkNumber="l1";
|
const Symbol cameraFrameNumber('x',1), landmarkNumber('l',1);
|
||||||
const SharedNoiseModel sigma(noiseModel::Unit::Create(1));
|
const SharedNoiseModel sigma(noiseModel::Unit::Create(1));
|
||||||
boost::shared_ptr<Projection>
|
boost::shared_ptr<Projection>
|
||||||
factor1(new Projection(z, sigma, cameraFrameNumber, landmarkNumber));
|
factor1(new Projection(z, sigma, cameraFrameNumber, landmarkNumber));
|
||||||
|
@ -89,14 +86,14 @@ TEST( GeneralSFMFactor, equals )
|
||||||
TEST( GeneralSFMFactor, error ) {
|
TEST( GeneralSFMFactor, error ) {
|
||||||
Point2 z(3.,0.);
|
Point2 z(3.,0.);
|
||||||
const SharedNoiseModel sigma(noiseModel::Unit::Create(1));
|
const SharedNoiseModel sigma(noiseModel::Unit::Create(1));
|
||||||
boost::shared_ptr<Projection> factor(new Projection(z, sigma, Symbol("x1"), Symbol("l1")));
|
boost::shared_ptr<Projection> factor(new Projection(z, sigma, Symbol('x',1), Symbol('l',1)));
|
||||||
// For the following configuration, the factor predicts 320,240
|
// For the following configuration, the factor predicts 320,240
|
||||||
Values values;
|
Values values;
|
||||||
Rot3 R;
|
Rot3 R;
|
||||||
Point3 t1(0,0,-6);
|
Point3 t1(0,0,-6);
|
||||||
Pose3 x1(R,t1);
|
Pose3 x1(R,t1);
|
||||||
values.insert(Symbol("x1"), GeneralCamera(x1));
|
values.insert(Symbol('x',1), GeneralCamera(x1));
|
||||||
Point3 l1; values.insert(Symbol("l1"), l1);
|
Point3 l1; values.insert(Symbol('l',1), l1);
|
||||||
EXPECT(assert_equal(Vector_(2, -3.0, 0.0), factor->unwhitenedError(values)));
|
EXPECT(assert_equal(Vector_(2, -3.0, 0.0), factor->unwhitenedError(values)));
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -12,9 +12,6 @@
|
||||||
#include <CppUnitLite/TestHarness.h>
|
#include <CppUnitLite/TestHarness.h>
|
||||||
using namespace boost;
|
using namespace boost;
|
||||||
|
|
||||||
// Magically casts strings like "x3" to a Symbol('x',3) key, see Symbol.h
|
|
||||||
#define GTSAM_MAGIC_KEY
|
|
||||||
|
|
||||||
#include <gtsam/base/Testable.h>
|
#include <gtsam/base/Testable.h>
|
||||||
#include <gtsam/geometry/Cal3Bundler.h>
|
#include <gtsam/geometry/Cal3Bundler.h>
|
||||||
#include <gtsam/geometry/PinholeCamera.h>
|
#include <gtsam/geometry/PinholeCamera.h>
|
||||||
|
@ -75,7 +72,7 @@ TEST( GeneralSFMFactor, equals )
|
||||||
{
|
{
|
||||||
// Create two identical factors and make sure they're equal
|
// Create two identical factors and make sure they're equal
|
||||||
Vector z = Vector_(2,323.,240.);
|
Vector z = Vector_(2,323.,240.);
|
||||||
const Symbol cameraFrameNumber="x1", landmarkNumber="l1";
|
const Symbol cameraFrameNumber('x',1), landmarkNumber('l',1);
|
||||||
const SharedNoiseModel sigma(noiseModel::Unit::Create(1));
|
const SharedNoiseModel sigma(noiseModel::Unit::Create(1));
|
||||||
boost::shared_ptr<Projection>
|
boost::shared_ptr<Projection>
|
||||||
factor1(new Projection(z, sigma, cameraFrameNumber, landmarkNumber));
|
factor1(new Projection(z, sigma, cameraFrameNumber, landmarkNumber));
|
||||||
|
@ -91,14 +88,14 @@ TEST( GeneralSFMFactor, error ) {
|
||||||
Point2 z(3.,0.);
|
Point2 z(3.,0.);
|
||||||
const SharedNoiseModel sigma(noiseModel::Unit::Create(1));
|
const SharedNoiseModel sigma(noiseModel::Unit::Create(1));
|
||||||
boost::shared_ptr<Projection>
|
boost::shared_ptr<Projection>
|
||||||
factor(new Projection(z, sigma, Symbol("x1"), Symbol("l1")));
|
factor(new Projection(z, sigma, Symbol('x',1), Symbol('l',1)));
|
||||||
// For the following configuration, the factor predicts 320,240
|
// For the following configuration, the factor predicts 320,240
|
||||||
Values values;
|
Values values;
|
||||||
Rot3 R;
|
Rot3 R;
|
||||||
Point3 t1(0,0,-6);
|
Point3 t1(0,0,-6);
|
||||||
Pose3 x1(R,t1);
|
Pose3 x1(R,t1);
|
||||||
values.insert(Symbol("x1"), GeneralCamera(x1));
|
values.insert(Symbol('x',1), GeneralCamera(x1));
|
||||||
Point3 l1; values.insert(Symbol("l1"), l1);
|
Point3 l1; values.insert(Symbol('l',1), l1);
|
||||||
EXPECT(assert_equal(Vector_(2, -3.0, 0.0), factor->unwhitenedError(values)));
|
EXPECT(assert_equal(Vector_(2, -3.0, 0.0), factor->unwhitenedError(values)));
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -14,9 +14,6 @@
|
||||||
* @author Frank Dellaert, Viorela Ila
|
* @author Frank Dellaert, Viorela Ila
|
||||||
**/
|
**/
|
||||||
|
|
||||||
// Magically casts strings like "x3" to a Symbol('x',3) key, see Symbol.h
|
|
||||||
#define GTSAM_MAGIC_KEY
|
|
||||||
|
|
||||||
#include <gtsam/slam/pose2SLAM.h>
|
#include <gtsam/slam/pose2SLAM.h>
|
||||||
#include <gtsam/nonlinear/NonlinearOptimizer.h>
|
#include <gtsam/nonlinear/NonlinearOptimizer.h>
|
||||||
#include <gtsam/inference/FactorGraph.h>
|
#include <gtsam/inference/FactorGraph.h>
|
||||||
|
@ -46,7 +43,7 @@ static noiseModel::Gaussian::shared_ptr covariance(
|
||||||
)));
|
)));
|
||||||
//static noiseModel::Gaussian::shared_ptr I3(noiseModel::Unit::Create(3));
|
//static noiseModel::Gaussian::shared_ptr I3(noiseModel::Unit::Create(3));
|
||||||
|
|
||||||
const Key kx0 = Symbol("x0"), kx1 = Symbol("x1"), kx2 = Symbol("x2"), kx3 = Symbol("x3"), kx4 = Symbol("x4"), kx5 = Symbol("x5"), kl1 = Symbol("l1");
|
const Key kx0 = Symbol('x',0), kx1 = Symbol('x',1), kx2 = Symbol('x',2), kx3 = Symbol('x',3), kx4 = Symbol('x',4), kx5 = Symbol('x',5), kl1 = Symbol('l',1);
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
// Test constraint, small displacement
|
// Test constraint, small displacement
|
||||||
|
|
|
@ -28,9 +28,6 @@ using namespace boost::assign;
|
||||||
// TODO: DANGEROUS, create shared pointers
|
// TODO: DANGEROUS, create shared pointers
|
||||||
#define GTSAM_MAGIC_GAUSSIAN 6
|
#define GTSAM_MAGIC_GAUSSIAN 6
|
||||||
|
|
||||||
// Magically casts strings like "x3" to a Symbol('x',3) key, see Symbol.h
|
|
||||||
#define GTSAM_MAGIC_KEY
|
|
||||||
|
|
||||||
#include <gtsam/slam/pose3SLAM.h>
|
#include <gtsam/slam/pose3SLAM.h>
|
||||||
#include <gtsam/slam/PartialPriorFactor.h>
|
#include <gtsam/slam/PartialPriorFactor.h>
|
||||||
|
|
||||||
|
@ -43,7 +40,7 @@ static Matrix covariance = eye(6);
|
||||||
|
|
||||||
const double tol=1e-5;
|
const double tol=1e-5;
|
||||||
|
|
||||||
const Key kx0 = Symbol("x0"), kx1 = Symbol("x1"), kx2 = Symbol("x2"), kx3 = Symbol("x3"), kx4 = Symbol("x4"), kx5 = Symbol("x5");
|
const Key kx0 = Symbol('x',0), kx1 = Symbol('x',1), kx2 = Symbol('x',2), kx3 = Symbol('x',3), kx4 = Symbol('x',4), kx5 = Symbol('x',5);
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
// test optimization with 6 poses arranged in a hexagon and a loop closure
|
// test optimization with 6 poses arranged in a hexagon and a loop closure
|
||||||
|
|
|
@ -18,9 +18,6 @@
|
||||||
|
|
||||||
#include <CppUnitLite/TestHarness.h>
|
#include <CppUnitLite/TestHarness.h>
|
||||||
|
|
||||||
// Magically casts strings like "x3" to a Symbol('x',3) key, see Symbol.h
|
|
||||||
#define GTSAM_MAGIC_KEY
|
|
||||||
|
|
||||||
#include <gtsam/slam/visualSLAM.h>
|
#include <gtsam/slam/visualSLAM.h>
|
||||||
|
|
||||||
using namespace std;
|
using namespace std;
|
||||||
|
@ -40,7 +37,7 @@ static Cal3_S2 K(fov,w,h);
|
||||||
static SharedNoiseModel sigma(noiseModel::Unit::Create(1));
|
static SharedNoiseModel sigma(noiseModel::Unit::Create(1));
|
||||||
static shared_ptrK sK(new Cal3_S2(K));
|
static shared_ptrK sK(new Cal3_S2(K));
|
||||||
|
|
||||||
const Key kx1 = Symbol("x1"), kl1 = Symbol("l1");
|
const Key kx1 = Symbol('x',1), kl1 = Symbol('l',1);
|
||||||
|
|
||||||
// make cameras
|
// make cameras
|
||||||
|
|
||||||
|
|
|
@ -16,8 +16,6 @@
|
||||||
* @date Feb 7, 2012
|
* @date Feb 7, 2012
|
||||||
*/
|
*/
|
||||||
|
|
||||||
#define GTSAM_MAGIC_KEY
|
|
||||||
|
|
||||||
#include <gtsam/linear/GaussianISAM.h>
|
#include <gtsam/linear/GaussianISAM.h>
|
||||||
#include <gtsam/linear/GaussianMultifrontalSolver.h>
|
#include <gtsam/linear/GaussianMultifrontalSolver.h>
|
||||||
#include <gtsam/slam/smallExample.h>
|
#include <gtsam/slam/smallExample.h>
|
||||||
|
@ -51,7 +49,7 @@ BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal");
|
||||||
TEST (Serialization, smallExample_linear) {
|
TEST (Serialization, smallExample_linear) {
|
||||||
using namespace example;
|
using namespace example;
|
||||||
|
|
||||||
Ordering ordering; ordering += Symbol("x1"),Symbol("x2"),Symbol("l1");
|
Ordering ordering; ordering += Symbol('x',1),Symbol('x',2),Symbol('l',1);
|
||||||
GaussianFactorGraph fg = createGaussianFactorGraph(ordering);
|
GaussianFactorGraph fg = createGaussianFactorGraph(ordering);
|
||||||
EXPECT(equalsObj(ordering));
|
EXPECT(equalsObj(ordering));
|
||||||
EXPECT(equalsXML(ordering));
|
EXPECT(equalsXML(ordering));
|
||||||
|
|
|
@ -22,9 +22,6 @@
|
||||||
#include <boost/shared_ptr.hpp>
|
#include <boost/shared_ptr.hpp>
|
||||||
using namespace boost;
|
using namespace boost;
|
||||||
|
|
||||||
// Magically casts strings like "x3" to a Symbol('x',3) key, see Symbol.h
|
|
||||||
#define GTSAM_MAGIC_KEY
|
|
||||||
|
|
||||||
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
|
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
|
||||||
#include <gtsam/nonlinear/NonlinearOptimizer.h>
|
#include <gtsam/nonlinear/NonlinearOptimizer.h>
|
||||||
#include <gtsam/slam/visualSLAM.h>
|
#include <gtsam/slam/visualSLAM.h>
|
||||||
|
|
|
@ -17,8 +17,6 @@
|
||||||
|
|
||||||
#include <CppUnitLite/TestHarness.h>
|
#include <CppUnitLite/TestHarness.h>
|
||||||
|
|
||||||
#define GTSAM_MAGIC_KEY
|
|
||||||
|
|
||||||
#include <gtsam/slam/simulated2DConstraints.h>
|
#include <gtsam/slam/simulated2DConstraints.h>
|
||||||
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
|
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
|
||||||
#include <gtsam/nonlinear/NonlinearOptimizer.h>
|
#include <gtsam/nonlinear/NonlinearOptimizer.h>
|
||||||
|
@ -153,7 +151,7 @@ TEST( testBoundingConstraint, unary_simple_optimization1) {
|
||||||
Point2 start_pt(0.0, 1.0);
|
Point2 start_pt(0.0, 1.0);
|
||||||
|
|
||||||
shared_graph graph(new Graph());
|
shared_graph graph(new Graph());
|
||||||
Symbol x1("x1");
|
Symbol x1('x',1);
|
||||||
graph->add(iq2D::PoseXInequality(x1, 1.0, true));
|
graph->add(iq2D::PoseXInequality(x1, 1.0, true));
|
||||||
graph->add(iq2D::PoseYInequality(x1, 2.0, true));
|
graph->add(iq2D::PoseYInequality(x1, 2.0, true));
|
||||||
graph->add(simulated2D::Prior(start_pt, soft_model2, x1));
|
graph->add(simulated2D::Prior(start_pt, soft_model2, x1));
|
||||||
|
@ -175,7 +173,7 @@ TEST( testBoundingConstraint, unary_simple_optimization2) {
|
||||||
Point2 start_pt(2.0, 3.0);
|
Point2 start_pt(2.0, 3.0);
|
||||||
|
|
||||||
shared_graph graph(new Graph());
|
shared_graph graph(new Graph());
|
||||||
Symbol x1("x1");
|
Symbol x1('x',1);
|
||||||
graph->add(iq2D::PoseXInequality(x1, 1.0, false));
|
graph->add(iq2D::PoseXInequality(x1, 1.0, false));
|
||||||
graph->add(iq2D::PoseYInequality(x1, 2.0, false));
|
graph->add(iq2D::PoseYInequality(x1, 2.0, false));
|
||||||
graph->add(simulated2D::Prior(start_pt, soft_model2, x1));
|
graph->add(simulated2D::Prior(start_pt, soft_model2, x1));
|
||||||
|
@ -191,7 +189,7 @@ TEST( testBoundingConstraint, unary_simple_optimization2) {
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
TEST( testBoundingConstraint, MaxDistance_basics) {
|
TEST( testBoundingConstraint, MaxDistance_basics) {
|
||||||
Symbol key1("x1"), key2("x2");
|
Symbol key1('x',1), key2('x',2);
|
||||||
Point2 pt1, pt2(1.0, 0.0), pt3(2.0, 0.0), pt4(3.0, 0.0);
|
Point2 pt1, pt2(1.0, 0.0), pt3(2.0, 0.0), pt4(3.0, 0.0);
|
||||||
iq2D::PoseMaxDistConstraint rangeBound(key1, key2, 2.0, mu);
|
iq2D::PoseMaxDistConstraint rangeBound(key1, key2, 2.0, mu);
|
||||||
EXPECT_DOUBLES_EQUAL(2.0, rangeBound.threshold(), tol);
|
EXPECT_DOUBLES_EQUAL(2.0, rangeBound.threshold(), tol);
|
||||||
|
@ -233,7 +231,7 @@ TEST( testBoundingConstraint, MaxDistance_basics) {
|
||||||
TEST( testBoundingConstraint, MaxDistance_simple_optimization) {
|
TEST( testBoundingConstraint, MaxDistance_simple_optimization) {
|
||||||
|
|
||||||
Point2 pt1, pt2_init(5.0, 0.0), pt2_goal(2.0, 0.0);
|
Point2 pt1, pt2_init(5.0, 0.0), pt2_goal(2.0, 0.0);
|
||||||
Symbol x1("x1"), x2("x2");
|
Symbol x1('x',1), x2('x',2);
|
||||||
|
|
||||||
Graph graph;
|
Graph graph;
|
||||||
graph.add(simulated2D::equality_constraints::UnaryEqualityConstraint(pt1, x1));
|
graph.add(simulated2D::equality_constraints::UnaryEqualityConstraint(pt1, x1));
|
||||||
|
@ -256,7 +254,7 @@ TEST( testBoundingConstraint, MaxDistance_simple_optimization) {
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
TEST( testBoundingConstraint, avoid_demo) {
|
TEST( testBoundingConstraint, avoid_demo) {
|
||||||
|
|
||||||
Symbol x1("x1"), x2("x2"), x3("x3"), l1("l1");
|
Symbol x1('x',1), x2('x',2), x3('x',3), l1('l',1);
|
||||||
double radius = 1.0;
|
double radius = 1.0;
|
||||||
Point2 x1_pt, x2_init(2.0, 0.5), x2_goal(2.0, 1.0), x3_pt(4.0, 0.0), l1_pt(2.0, 0.0);
|
Point2 x1_pt, x2_init(2.0, 0.5), x2_goal(2.0, 1.0), x3_pt(4.0, 0.0), l1_pt(2.0, 0.0);
|
||||||
Point2 odo(2.0, 0.0);
|
Point2 odo(2.0, 0.0);
|
||||||
|
|
|
@ -16,8 +16,6 @@
|
||||||
|
|
||||||
#include <CppUnitLite/TestHarness.h>
|
#include <CppUnitLite/TestHarness.h>
|
||||||
|
|
||||||
#define GTSAM_MAGIC_KEY
|
|
||||||
|
|
||||||
#include <gtsam/slam/PriorFactor.h>
|
#include <gtsam/slam/PriorFactor.h>
|
||||||
#include <gtsam/slam/BetweenFactor.h>
|
#include <gtsam/slam/BetweenFactor.h>
|
||||||
#include <gtsam/nonlinear/ExtendedKalmanFilter-inl.h>
|
#include <gtsam/nonlinear/ExtendedKalmanFilter-inl.h>
|
||||||
|
@ -37,7 +35,7 @@ TEST( ExtendedKalmanFilter, linear ) {
|
||||||
ExtendedKalmanFilter<Point2> ekf(x_initial, P_initial);
|
ExtendedKalmanFilter<Point2> ekf(x_initial, P_initial);
|
||||||
|
|
||||||
// Create the TestKeys for our example
|
// Create the TestKeys for our example
|
||||||
Symbol x0("x0"), x1("x1"), x2("x2"), x3("x3");
|
Symbol x0('x',0), x1('x',1), x2('x',2), x3('x',3);
|
||||||
|
|
||||||
// Create the controls and measurement properties for our example
|
// Create the controls and measurement properties for our example
|
||||||
double dt = 1.0;
|
double dt = 1.0;
|
||||||
|
|
|
@ -25,9 +25,6 @@
|
||||||
#include <boost/assign/std/list.hpp> // for operator +=
|
#include <boost/assign/std/list.hpp> // for operator +=
|
||||||
using namespace boost::assign;
|
using namespace boost::assign;
|
||||||
|
|
||||||
// Magically casts strings like "x3" to a Symbol('x',3) key, see Symbol.h
|
|
||||||
#define GTSAM_MAGIC_KEY
|
|
||||||
|
|
||||||
#include <gtsam/base/Testable.h>
|
#include <gtsam/base/Testable.h>
|
||||||
#include <gtsam/inference/BayesNet.h>
|
#include <gtsam/inference/BayesNet.h>
|
||||||
#include <gtsam/linear/GaussianBayesNet.h>
|
#include <gtsam/linear/GaussianBayesNet.h>
|
||||||
|
|
|
@ -26,9 +26,6 @@ using namespace boost::assign;
|
||||||
|
|
||||||
#include <CppUnitLite/TestHarness.h>
|
#include <CppUnitLite/TestHarness.h>
|
||||||
|
|
||||||
// Magically casts strings like kx3 to a Symbol('x',3) key, see Symbol.h
|
|
||||||
#define GTSAM_MAGIC_KEY
|
|
||||||
|
|
||||||
#include <gtsam/base/Matrix.h>
|
#include <gtsam/base/Matrix.h>
|
||||||
#include <gtsam/base/Testable.h>
|
#include <gtsam/base/Testable.h>
|
||||||
#include <gtsam/linear/GaussianConditional.h>
|
#include <gtsam/linear/GaussianConditional.h>
|
||||||
|
@ -44,7 +41,7 @@ static SharedDiagonal
|
||||||
sigma0_1 = sharedSigma(2,0.1), sigma_02 = sharedSigma(2,0.2),
|
sigma0_1 = sharedSigma(2,0.1), sigma_02 = sharedSigma(2,0.2),
|
||||||
constraintModel = noiseModel::Constrained::All(2);
|
constraintModel = noiseModel::Constrained::All(2);
|
||||||
|
|
||||||
const Key kx1 = Symbol("x1"), kx2 = Symbol("x2"), kl1 = Symbol("l1");
|
const Key kx1 = Symbol('x',1), kx2 = Symbol('x',2), kl1 = Symbol('l',1);
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
TEST( GaussianFactor, linearFactor )
|
TEST( GaussianFactor, linearFactor )
|
||||||
|
|
|
@ -28,9 +28,6 @@ using namespace boost::assign;
|
||||||
|
|
||||||
#include <CppUnitLite/TestHarness.h>
|
#include <CppUnitLite/TestHarness.h>
|
||||||
|
|
||||||
// Magically casts strings like kx(3) to a Symbol('x',3) key, see Symbol.h
|
|
||||||
#define GTSAM_MAGIC_KEY
|
|
||||||
|
|
||||||
#include <gtsam/base/Matrix.h>
|
#include <gtsam/base/Matrix.h>
|
||||||
#include <gtsam/base/Testable.h>
|
#include <gtsam/base/Testable.h>
|
||||||
#include <gtsam/base/numericalDerivative.h>
|
#include <gtsam/base/numericalDerivative.h>
|
||||||
|
|
|
@ -21,9 +21,6 @@ using namespace boost::assign;
|
||||||
|
|
||||||
#include <CppUnitLite/TestHarness.h>
|
#include <CppUnitLite/TestHarness.h>
|
||||||
|
|
||||||
// Magically casts strings like kx(3) to a Symbol('x',3) key, see Symbol.h
|
|
||||||
#define GTSAM_MAGIC_KEY
|
|
||||||
|
|
||||||
#include <gtsam/geometry/Rot2.h>
|
#include <gtsam/geometry/Rot2.h>
|
||||||
#include <gtsam/nonlinear/Ordering.h>
|
#include <gtsam/nonlinear/Ordering.h>
|
||||||
#include <gtsam/linear/GaussianBayesNet.h>
|
#include <gtsam/linear/GaussianBayesNet.h>
|
||||||
|
|
|
@ -11,8 +11,6 @@ using namespace boost::assign;
|
||||||
|
|
||||||
#include <CppUnitLite/TestHarness.h>
|
#include <CppUnitLite/TestHarness.h>
|
||||||
|
|
||||||
#define GTSAM_MAGIC_KEY
|
|
||||||
|
|
||||||
#include <gtsam/base/debug.h>
|
#include <gtsam/base/debug.h>
|
||||||
#include <gtsam/base/TestableAssertions.h>
|
#include <gtsam/base/TestableAssertions.h>
|
||||||
#include <gtsam/nonlinear/Ordering.h>
|
#include <gtsam/nonlinear/Ordering.h>
|
||||||
|
|
|
@ -25,9 +25,6 @@
|
||||||
#include <boost/assign/std/vector.hpp>
|
#include <boost/assign/std/vector.hpp>
|
||||||
using namespace boost::assign;
|
using namespace boost::assign;
|
||||||
|
|
||||||
// Magically casts strings like kx(3) to a Symbol('x',3) key, see Symbol.h
|
|
||||||
#define GTSAM_MAGIC_KEY
|
|
||||||
|
|
||||||
#include <gtsam/base/debug.h>
|
#include <gtsam/base/debug.h>
|
||||||
#include <gtsam/base/cholesky.h>
|
#include <gtsam/base/cholesky.h>
|
||||||
#include <gtsam/inference/BayesTree-inl.h>
|
#include <gtsam/inference/BayesTree-inl.h>
|
||||||
|
|
|
@ -23,8 +23,6 @@ using namespace boost::assign;
|
||||||
|
|
||||||
#include <CppUnitLite/TestHarness.h>
|
#include <CppUnitLite/TestHarness.h>
|
||||||
|
|
||||||
#define GTSAM_MAGIC_KEY
|
|
||||||
|
|
||||||
// TODO: DANGEROUS, create shared pointers
|
// TODO: DANGEROUS, create shared pointers
|
||||||
#define GTSAM_MAGIC_GAUSSIAN 3
|
#define GTSAM_MAGIC_GAUSSIAN 3
|
||||||
|
|
||||||
|
|
|
@ -17,9 +17,6 @@
|
||||||
|
|
||||||
#include <CppUnitLite/TestHarness.h>
|
#include <CppUnitLite/TestHarness.h>
|
||||||
|
|
||||||
// Magically casts strings like "x3" to a Symbol('x',3) key, see Symbol.h
|
|
||||||
#define GTSAM_MAGIC_KEY
|
|
||||||
|
|
||||||
#include <gtsam/linear/GaussianSequentialSolver.h>
|
#include <gtsam/linear/GaussianSequentialSolver.h>
|
||||||
#include <gtsam/slam/smallExample.h>
|
#include <gtsam/slam/smallExample.h>
|
||||||
#include <gtsam/slam/planarSLAM.h>
|
#include <gtsam/slam/planarSLAM.h>
|
||||||
|
|
|
@ -25,9 +25,6 @@
|
||||||
// TODO: DANGEROUS, create shared pointers
|
// TODO: DANGEROUS, create shared pointers
|
||||||
#define GTSAM_MAGIC_GAUSSIAN 2
|
#define GTSAM_MAGIC_GAUSSIAN 2
|
||||||
|
|
||||||
// Magically casts strings like PoseKey(3) to a Symbol('x',3) key, see Symbol.h
|
|
||||||
#define GTSAM_MAGIC_KEY
|
|
||||||
|
|
||||||
#include <gtsam/base/Testable.h>
|
#include <gtsam/base/Testable.h>
|
||||||
#include <gtsam/base/Matrix.h>
|
#include <gtsam/base/Matrix.h>
|
||||||
#include <gtsam/base/LieVector.h>
|
#include <gtsam/base/LieVector.h>
|
||||||
|
|
|
@ -27,9 +27,6 @@ using namespace boost::assign;
|
||||||
|
|
||||||
#include <CppUnitLite/TestHarness.h>
|
#include <CppUnitLite/TestHarness.h>
|
||||||
|
|
||||||
// Magically casts strings like "x3" to a Symbol('x',3) key, see Symbol.h
|
|
||||||
#define GTSAM_MAGIC_KEY
|
|
||||||
|
|
||||||
#include <gtsam/base/Testable.h>
|
#include <gtsam/base/Testable.h>
|
||||||
#include <gtsam/base/Matrix.h>
|
#include <gtsam/base/Matrix.h>
|
||||||
#include <gtsam/slam/smallExample.h>
|
#include <gtsam/slam/smallExample.h>
|
||||||
|
|
|
@ -26,9 +26,6 @@ using namespace boost::assign;
|
||||||
#include <boost/shared_ptr.hpp>
|
#include <boost/shared_ptr.hpp>
|
||||||
using namespace boost;
|
using namespace boost;
|
||||||
|
|
||||||
// Magically casts strings like kx(3) to a Symbol('x',3) key, see Symbol.h
|
|
||||||
#define GTSAM_MAGIC_KEY
|
|
||||||
|
|
||||||
#include <gtsam/base/Matrix.h>
|
#include <gtsam/base/Matrix.h>
|
||||||
#include <gtsam/slam/smallExample.h>
|
#include <gtsam/slam/smallExample.h>
|
||||||
#include <gtsam/slam/pose2SLAM.h>
|
#include <gtsam/slam/pose2SLAM.h>
|
||||||
|
|
|
@ -21,9 +21,6 @@ using namespace boost::assign;
|
||||||
|
|
||||||
#include <CppUnitLite/TestHarness.h>
|
#include <CppUnitLite/TestHarness.h>
|
||||||
|
|
||||||
// Magically casts strings like kx(3) to a Symbol('x',3) key, see Symbol.h
|
|
||||||
#define GTSAM_MAGIC_KEY
|
|
||||||
|
|
||||||
#include <gtsam/base/Testable.h>
|
#include <gtsam/base/Testable.h>
|
||||||
#include <gtsam/slam/smallExample.h>
|
#include <gtsam/slam/smallExample.h>
|
||||||
#include <gtsam/inference/SymbolicFactorGraph.h>
|
#include <gtsam/inference/SymbolicFactorGraph.h>
|
||||||
|
|
|
@ -20,9 +20,6 @@ using namespace boost::assign;
|
||||||
|
|
||||||
#include <CppUnitLite/TestHarness.h>
|
#include <CppUnitLite/TestHarness.h>
|
||||||
|
|
||||||
// Magically casts strings like kx(3) to a Symbol('x',3) key, see Symbol.h
|
|
||||||
#define GTSAM_MAGIC_KEY
|
|
||||||
|
|
||||||
#include <gtsam/slam/smallExample.h>
|
#include <gtsam/slam/smallExample.h>
|
||||||
#include <gtsam/inference/SymbolicFactorGraph.h>
|
#include <gtsam/inference/SymbolicFactorGraph.h>
|
||||||
#include <gtsam/inference/BayesNet-inl.h>
|
#include <gtsam/inference/BayesNet-inl.h>
|
||||||
|
|
|
@ -20,9 +20,6 @@
|
||||||
|
|
||||||
#include <gtsam/base/TestableAssertions.h>
|
#include <gtsam/base/TestableAssertions.h>
|
||||||
|
|
||||||
// Magically casts strings like "x3" to a Symbol('x',3) key, see Symbol.h
|
|
||||||
#define GTSAM_MAGIC_KEY
|
|
||||||
|
|
||||||
#include <gtsam/geometry/Pose2.h>
|
#include <gtsam/geometry/Pose2.h>
|
||||||
#include <gtsam/geometry/Point2.h>
|
#include <gtsam/geometry/Point2.h>
|
||||||
#include <gtsam/geometry/Pose3.h>
|
#include <gtsam/geometry/Pose3.h>
|
||||||
|
|
|
@ -15,9 +15,6 @@
|
||||||
* @author Frank Dellaert
|
* @author Frank Dellaert
|
||||||
*/
|
*/
|
||||||
|
|
||||||
// Magically casts strings like "x3" to a Symbol('x',3) key, see Symbol.h
|
|
||||||
#define GTSAM_MAGIC_KEY
|
|
||||||
|
|
||||||
#include <time.h>
|
#include <time.h>
|
||||||
#include <boost/foreach.hpp>
|
#include <boost/foreach.hpp>
|
||||||
#include <boost/assign/std/list.hpp> // for operator += in Ordering
|
#include <boost/assign/std/list.hpp> // for operator += in Ordering
|
||||||
|
|
|
@ -40,8 +40,6 @@ using namespace std;
|
||||||
using namespace boost; // not usual, but for conciseness of generated code
|
using namespace boost; // not usual, but for conciseness of generated code
|
||||||
|
|
||||||
// start GTSAM Specifics /////////////////////////////////////////////////
|
// start GTSAM Specifics /////////////////////////////////////////////////
|
||||||
// to make keys be constructed from strings:
|
|
||||||
#define GTSAM_MAGIC_KEY
|
|
||||||
// to enable Matrix and Vector constructor for SharedGaussian:
|
// to enable Matrix and Vector constructor for SharedGaussian:
|
||||||
#define GTSAM_MAGIC_GAUSSIAN
|
#define GTSAM_MAGIC_GAUSSIAN
|
||||||
// end GTSAM Specifics /////////////////////////////////////////////////
|
// end GTSAM Specifics /////////////////////////////////////////////////
|
||||||
|
|
Loading…
Reference in New Issue