diff --git a/cpp/Makefile.am b/cpp/Makefile.am index 550020c7d..8490a3e0b 100644 --- a/cpp/Makefile.am +++ b/cpp/Makefile.am @@ -247,7 +247,7 @@ testVSLAMGraph_LDADD = libgtsam.la testVSLAMConfig_SOURCES = testVSLAMConfig.cpp testVSLAMConfig_LDADD = libgtsam.la -headers += Point2Prior.h Simulated2DOdometry.h Simulated2DMeasurement.h smallExample.h +headers += smallExample.h headers += $(sources:.cpp=.h) # create both dynamic and static libraries diff --git a/cpp/Point2Prior.h b/cpp/Point2Prior.h deleted file mode 100644 index 72bc3cbcc..000000000 --- a/cpp/Point2Prior.h +++ /dev/null @@ -1,24 +0,0 @@ -// Frank Dellaert -// Simulated2D Prior - -#include "NonlinearFactor.h" -#include "simulated2D.h" - -namespace simulated2D { - - struct Point2Prior: public gtsam::NonlinearFactor1 { - - Vector z_; - - Point2Prior(const Vector& z, double sigma, const std::string& key) : - gtsam::NonlinearFactor1(sigma, key), z_(z) { - } - - Vector evaluateError(const Vector& x, boost::optional H = boost::none) const { - if (H) *H = Dprior(x); - return prior(x) - z_; - } - - }; - -} // namespace simulated2D diff --git a/cpp/Simulated2DMeasurement.h b/cpp/Simulated2DMeasurement.h deleted file mode 100644 index 920eeb251..000000000 --- a/cpp/Simulated2DMeasurement.h +++ /dev/null @@ -1,29 +0,0 @@ -// Christian Potthast -// Simulated2D Odometry - -#include "NonlinearFactor.h" -#include "simulated2D.h" - -namespace simulated2D { - - struct Simulated2DMeasurement: public gtsam::NonlinearFactor2 { - - Vector z_; - - Simulated2DMeasurement(const Vector& z, double sigma, const std::string& j1, - const std::string& j2) : - z_(z), gtsam::NonlinearFactor2(sigma, j1, j2) { - } - - Vector evaluateError(const Vector& x1, const Vector& x2, boost::optional< - Matrix&> H1 = boost::none, boost::optional H2 = boost::none) const { - if (H1) *H1 = Dmea1(x1, x2); - if (H2) *H2 = Dmea2(x1, x2); - return mea(x1, x2) - z_; - } - - }; - -} // namespace simulated2D diff --git a/cpp/Simulated2DOdometry.h b/cpp/Simulated2DOdometry.h deleted file mode 100644 index 1d966d06c..000000000 --- a/cpp/Simulated2DOdometry.h +++ /dev/null @@ -1,28 +0,0 @@ -// Christian Potthast -// Simulated2D Odometry - -#include "NonlinearFactor.h" -#include "simulated2D.h" - -namespace simulated2D { - - struct Simulated2DOdometry: public gtsam::NonlinearFactor2 { - Vector z_; - - Simulated2DOdometry(const Vector& z, double sigma, const std::string& j1, - const std::string& j2) : - z_(z), gtsam::NonlinearFactor2(sigma, j1, j2) { - } - - Vector evaluateError(const Vector& x1, const Vector& x2, boost::optional< - Matrix&> H1 = boost::none, boost::optional H2 = boost::none) const { - if (H1) *H1 = Dodo1(x1, x2); - if (H2) *H2 = Dodo2(x1, x2); - return odo(x1, x2) - z_; - } - - }; - -} // namespace gtsam diff --git a/cpp/simulated2D.cpp b/cpp/simulated2D.cpp index 6b67c7196..67bfec89a 100644 --- a/cpp/simulated2D.cpp +++ b/cpp/simulated2D.cpp @@ -8,60 +8,30 @@ namespace simulated2D { + static Matrix I = gtsam::eye(2); -/** prior on a single pose */ + /* ************************************************************************* */ + Vector prior(const Vector& x, boost::optional H) { + if (H) *H = I; + return x; + } -/* ************************************************************************* */ -Vector prior (const Vector& x) {return x;} + /* ************************************************************************* */ + Vector odo(const Vector& x1, const Vector& x2, boost::optional H1, + boost::optional H2) { + if (H1) *H1 = -I; + if (H2) *H2 = I; + return x2 - x1; + } -/* ************************************************************************* */ -Matrix Dprior(const Vector& x) { - Matrix H(2,2); - H(0,0)=1; H(0,1)=0; - H(1,0)=0; H(1,1)=1; - return H; -} -/* ************************************************************************* */ - -/** odometry between two poses */ - -/* ************************************************************************* */ -Vector odo(const Vector& x1, const Vector& x2) {return x2 - x1;} -Matrix Dodo1(const Vector& x1, const Vector& x2) { - Matrix H(2,2); - H(0,0)=-1; H(0,1)= 0; - H(1,0)= 0; H(1,1)=-1; - return H; -} - -/* ************************************************************************* */ -Matrix Dodo2(const Vector& x1, const Vector& x2) { - Matrix H(2,2); - H(0,0)= 1; H(0,1)= 0; - H(1,0)= 0; H(1,1)= 1; - return H; -} + /* ************************************************************************* */ + Vector mea(const Vector& x, const Vector& l, boost::optional H1, + boost::optional H2) { + if (H1) *H1 = -I; + if (H2) *H2 = I; + return l - x; + } /* ************************************************************************* */ -/** measurement between landmark and pose */ - -/* ************************************************************************* */ -Vector mea(const Vector& x, const Vector& l) {return l - x;} -Matrix Dmea1(const Vector& x, const Vector& l) { - Matrix H(2,2); - H(0,0)=-1; H(0,1)= 0; - H(1,0)= 0; H(1,1)=-1; - return H; -} - -/* ************************************************************************* */ -Matrix Dmea2(const Vector& x, const Vector& l) { - Matrix H(2,2); - H(0,0)= 1; H(0,1)= 0; - H(1,0)= 0; H(1,1)= 1; - return H; -} - -/* ************************************************************************* */ } // namespace simulated2D diff --git a/cpp/simulated2D.h b/cpp/simulated2D.h index c9a078a3d..f2c1f778c 100644 --- a/cpp/simulated2D.h +++ b/cpp/simulated2D.h @@ -16,36 +16,88 @@ namespace simulated2D { typedef gtsam::VectorConfig VectorConfig; + typedef std::string PoseKey; + typedef std::string PointKey; - struct PoseKey: public std::string { - PoseKey(const std::string&s) : - std::string(s) { + /** + * Prior on a single pose, and optional derivative version + */ + inline Vector prior(const Vector& x) {return x;} + Vector prior(const Vector& x, boost::optional H = boost::none); + + /** + * odometry between two poses, and optional derivative version + */ + inline Vector odo(const Vector& x1, const Vector& x2) {return x2-x1;} + Vector odo(const Vector& x1, const Vector& x2, boost::optional H1 = + boost::none, boost::optional H2 = boost::none); + + /** + * measurement between landmark and pose, and optional derivative version + */ + inline Vector mea(const Vector& x, const Vector& l) {return l-x;} + Vector mea(const Vector& x, const Vector& l, boost::optional H1 = + boost::none, boost::optional H2 = boost::none); + + /** + * Unary factor encoding a soft prior on a vector + */ + struct Prior: public gtsam::NonlinearFactor1 { + + Vector z_; + + Prior(const Vector& z, double sigma, const std::string& key) : + gtsam::NonlinearFactor1(sigma, key), + z_(z) { } - }; - struct PointKey: public std::string { - PointKey(const std::string&s) : - std::string(s) { + + Vector evaluateError(const Vector& x, boost::optional H = + boost::none) const { + return prior(x, H) - z_; } + }; /** - * Prior on a single pose + * Binary factor simulating "odometry" between two Vectors */ - Vector prior(const Vector& x); - Matrix Dprior(const Vector& x); + struct Odometry: public gtsam::NonlinearFactor2 { + Vector z_; + + Odometry(const Vector& z, double sigma, const std::string& j1, + const std::string& j2) : + z_(z), gtsam::NonlinearFactor2(sigma, j1, j2) { + } + + Vector evaluateError(const Vector& x1, const Vector& x2, boost::optional< + Matrix&> H1 = boost::none, boost::optional H2 = boost::none) const { + return odo(x1, x2, H1, H2) - z_; + } + + }; /** - * odometry between two poses + * Binary factor simulating "measurement" between two Vectors */ - Vector odo(const Vector& x1, const Vector& x2); - Matrix Dodo1(const Vector& x1, const Vector& x2); - Matrix Dodo2(const Vector& x1, const Vector& x2); + struct Measurement: public gtsam::NonlinearFactor2 { - /** - * measurement between landmark and pose - */ - Vector mea(const Vector& x, const Vector& l); - Matrix Dmea1(const Vector& x, const Vector& l); - Matrix Dmea2(const Vector& x, const Vector& l); + Vector z_; + + Measurement(const Vector& z, double sigma, const std::string& j1, + const std::string& j2) : + z_(z), gtsam::NonlinearFactor2(sigma, j1, j2) { + } + + Vector evaluateError(const Vector& x1, const Vector& x2, boost::optional< + Matrix&> H1 = boost::none, boost::optional H2 = boost::none) const { + return mea(x1, x2, H1, H2) - z_; + } + + }; } // namespace simulated2D diff --git a/cpp/smallExample.cpp b/cpp/smallExample.cpp index 056eb185c..6aed23bf6 100644 --- a/cpp/smallExample.cpp +++ b/cpp/smallExample.cpp @@ -16,9 +16,6 @@ using namespace std; #include "Matrix.h" #include "NonlinearFactor.h" #include "smallExample.h" -#include "Point2Prior.h" -#include "Simulated2DOdometry.h" -#include "Simulated2DMeasurement.h" #include "simulated2D.h" // template definitions @@ -38,7 +35,7 @@ namespace gtsam { // prior on x1 double sigma1 = 0.1; Vector mu = zero(2); - shared f1(new simulated2D::Point2Prior(mu, sigma1, "x1")); + shared f1(new simulated2D::Prior(mu, sigma1, "x1")); nlfg->push_back(f1); // odometry between x1 and x2 @@ -46,7 +43,7 @@ namespace gtsam { Vector z2(2); z2(0) = 1.5; z2(1) = 0; - shared f2(new simulated2D::Simulated2DOdometry(z2, sigma2, "x1", "x2")); + shared f2(new simulated2D::Odometry(z2, sigma2, "x1", "x2")); nlfg->push_back(f2); // measurement between x1 and l1 @@ -54,7 +51,7 @@ namespace gtsam { Vector z3(2); z3(0) = 0.; z3(1) = -1.; - shared f3(new simulated2D::Simulated2DMeasurement(z3, sigma3, "x1", "l1")); + shared f3(new simulated2D::Measurement(z3, sigma3, "x1", "l1")); nlfg->push_back(f3); // measurement between x2 and l1 @@ -62,7 +59,7 @@ namespace gtsam { Vector z4(2); z4(0) = -1.5; z4(1) = -1.; - shared f4(new simulated2D::Simulated2DMeasurement(z4, sigma4, "x2", "l1")); + shared f4(new simulated2D::Measurement(z4, sigma4, "x2", "l1")); nlfg->push_back(f4); return nlfg; @@ -234,7 +231,7 @@ namespace gtsam { // prior on x1 Vector x1 = Vector_(2, 1.0, 0.0); string key1 = symbol('x', 1); - shared prior(new simulated2D::Point2Prior(x1, sigma1, key1)); + shared prior(new simulated2D::Prior(x1, sigma1, key1)); nlfg.push_back(prior); poses.insert(key1, x1); @@ -242,13 +239,13 @@ namespace gtsam { // odometry between x_t and x_{t-1} Vector odo = Vector_(2, 1.0, 0.0); string key = symbol('x', t); - shared odometry(new simulated2D::Simulated2DOdometry(odo, sigma2, symbol('x', t - 1), + shared odometry(new simulated2D::Odometry(odo, sigma2, symbol('x', t - 1), key)); nlfg.push_back(odometry); // measurement on x_t is like perfect GPS Vector xt = Vector_(2, (double) t, 0.0); - shared measurement(new simulated2D::Point2Prior(xt, sigma1, key)); + shared measurement(new simulated2D::Prior(xt, sigma1, key)); nlfg.push_back(measurement); // initial estimate @@ -529,7 +526,7 @@ namespace gtsam { // Create almost hard constraint on x11, sigma=0 will work for PCG not for normal double sigma0 = 1e-3; - shared constraint(new simulated2D::Point2Prior(Vector_(2, 1.0, 1.0), sigma0, "x11")); + shared constraint(new simulated2D::Prior(Vector_(2, 1.0, 1.0), sigma0, "x11")); nlfg.push_back(constraint); double sigma = 0.01; @@ -538,7 +535,7 @@ namespace gtsam { Vector z1 = Vector_(2, 1.0, 0.0); // move right for (size_t x = 1; x < N; x++) for (size_t y = 1; y <= N; y++) { - shared f(new simulated2D::Simulated2DOdometry(z1, sigma, key(x, y), key(x + 1, y))); + shared f(new simulated2D::Odometry(z1, sigma, key(x, y), key(x + 1, y))); nlfg.push_back(f); } @@ -546,7 +543,7 @@ namespace gtsam { Vector z2 = Vector_(2, 0.0, 1.0); // move up for (size_t x = 1; x <= N; x++) for (size_t y = 1; y < N; y++) { - shared f(new simulated2D::Simulated2DOdometry(z2, sigma, key(x, y), key(x, y + 1))); + shared f(new simulated2D::Odometry(z2, sigma, key(x, y), key(x, y + 1))); nlfg.push_back(f); } diff --git a/cpp/testNonlinearFactor.cpp b/cpp/testNonlinearFactor.cpp index 5bd0b097d..93ccbac4f 100644 --- a/cpp/testNonlinearFactor.cpp +++ b/cpp/testNonlinearFactor.cpp @@ -13,7 +13,7 @@ #include "Matrix.h" #include "smallExample.h" -#include "Simulated2DMeasurement.h" +#include "Simulated2D.h" #include "GaussianFactor.h" using namespace std; @@ -28,11 +28,11 @@ TEST( NonlinearFactor, equals ) // create two nonlinear2 factors Vector z3 = Vector_(2,0.,-1.); - simulated2D::Simulated2DMeasurement f0(z3, sigma, "x1", "l1"); + simulated2D::Measurement f0(z3, sigma, "x1", "l1"); // measurement between x2 and l1 Vector z4 = Vector_(2,-1.5, -1.); - simulated2D::Simulated2DMeasurement f1(z4, sigma, "x2", "l1"); + simulated2D::Measurement f1(z4, sigma, "x2", "l1"); CHECK(assert_equal(f0,f0)); CHECK(f0.equals(f0)); diff --git a/cpp/testSQP.cpp b/cpp/testSQP.cpp index ff45bd85d..dcd64c00d 100644 --- a/cpp/testSQP.cpp +++ b/cpp/testSQP.cpp @@ -17,7 +17,6 @@ #include #include #include -#include #include #include #include @@ -279,12 +278,12 @@ TEST (SQP, two_pose_truth ) { // measurement from x1 to l1 Vector z1 = Vector_(2, 0.0, 5.0); double sigma1 = 0.1; - shared f1(new simulated2D::Simulated2DMeasurement(z1, sigma1, "x1", "l1")); + shared f1(new simulated2D::Measurement(z1, sigma1, "x1", "l1")); // measurement from x2 to l1 Vector z2 = Vector_(2, -4.0, 0.0); double sigma2 = 0.1; - shared f2(new simulated2D::Simulated2DMeasurement(z2, sigma2, "x2", "l1")); + shared f2(new simulated2D::Measurement(z2, sigma2, "x2", "l1")); // construct the graph shared_ptr graph(new NLGraph()); @@ -376,12 +375,12 @@ TEST (SQP, two_pose ) { // measurement from x1 to l1 Vector z1 = Vector_(2, 0.0, 5.0); double sigma1 = 0.1; - shared f1(new simulated2D::Simulated2DMeasurement(z1, sigma1, "x1", "l1")); + shared f1(new simulated2D::Measurement(z1, sigma1, "x1", "l1")); // measurement from x2 to l2 Vector z2 = Vector_(2, -4.0, 0.0); double sigma2 = 0.1; - shared f2(new simulated2D::Simulated2DMeasurement(z2, sigma2, "x2", "l2")); + shared f2(new simulated2D::Measurement(z2, sigma2, "x2", "l2")); // equality constraint between l1 and l2 list keys2; keys2 += "l1", "l2"; diff --git a/cpp/testSQPOptimizer.cpp b/cpp/testSQPOptimizer.cpp index f19fc58af..078894300 100644 --- a/cpp/testSQPOptimizer.cpp +++ b/cpp/testSQPOptimizer.cpp @@ -8,8 +8,6 @@ #include // for operator += #include // for insert #include -#include -#include #include #include "NonlinearFactorGraph.h" #include "NonlinearConstraint.h" @@ -102,12 +100,12 @@ NLGraph linearMapWarpGraph() { // measurement from x1 to l1 Vector z1 = Vector_(2, 0.0, 5.0); double sigma1 = 0.1; - shared f1(new simulated2D::Simulated2DMeasurement(z1, sigma1, "x1", "l1")); + shared f1(new simulated2D::Measurement(z1, sigma1, "x1", "l1")); // measurement from x2 to l2 Vector z2 = Vector_(2, -4.0, 0.0); double sigma2 = 0.1; - shared f2(new simulated2D::Simulated2DMeasurement(z2, sigma2, "x2", "l2")); + shared f2(new simulated2D::Measurement(z2, sigma2, "x2", "l2")); // equality constraint between l1 and l2 list keys; keys += "l1", "l2"; @@ -262,12 +260,12 @@ pair obstacleAvoidGraph() { // measurement from x1 to x2 Vector x1x2 = Vector_(2, 5.0, 0.0); double sigma1 = 0.1; - shared f1(new simulated2D::Simulated2DOdometry(x1x2, sigma1, "x1", "x2")); + shared f1(new simulated2D::Odometry(x1x2, sigma1, "x1", "x2")); // measurement from x2 to x3 Vector x2x3 = Vector_(2, 5.0, 0.0); double sigma2 = 0.1; - shared f2(new simulated2D::Simulated2DOdometry(x2x3, sigma2, "x2", "x3")); + shared f2(new simulated2D::Odometry(x2x3, sigma2, "x2", "x3")); // create a binary inequality constraint that forces the middle point away from // the obstacle @@ -394,12 +392,12 @@ pair obstacleAvoidGraphGeneral() { // measurement from x1 to x2 Vector x1x2 = Vector_(2, 5.0, 0.0); double sigma1 = 0.1; - shared f1(new simulated2D::Simulated2DOdometry(x1x2, sigma1, "x1", "x2")); + shared f1(new simulated2D::Odometry(x1x2, sigma1, "x1", "x2")); // measurement from x2 to x3 Vector x2x3 = Vector_(2, 5.0, 0.0); double sigma2 = 0.1; - shared f2(new simulated2D::Simulated2DOdometry(x2x3, sigma2, "x2", "x3")); + shared f2(new simulated2D::Odometry(x2x3, sigma2, "x2", "x3")); double radius = 1.0; diff --git a/cpp/testSimulated2D.cpp b/cpp/testSimulated2D.cpp index 54f1cc60c..69b8b91f4 100644 --- a/cpp/testSimulated2D.cpp +++ b/cpp/testSimulated2D.cpp @@ -20,28 +20,22 @@ TEST( simulated2D, Dprior ) { Vector x(2);x(0)=1;x(1)=-9; Matrix numerical = numericalDerivative11(prior,x); - Matrix computed = Dprior(x); + Matrix computed; + prior(x,computed); CHECK(assert_equal(numerical,computed,1e-9)); } /* ************************************************************************* */ - TEST( simulated2D, DOdo1 ) + TEST( simulated2D, DOdo ) { Vector x1(2);x1(0)= 1;x1(1)=-9; Vector x2(2);x2(0)=-5;x2(1)= 6; - Matrix numerical = numericalDerivative21(odo,x1,x2); - Matrix computed = Dodo1(x1,x2); - CHECK(assert_equal(numerical,computed,1e-9)); -} - -/* ************************************************************************* */ - TEST( simulated2D, DOdo2 ) -{ - Vector x1(2);x1(0)= 1;x1(1)=-9; - Vector x2(2);x2(0)=-5;x2(1)= 6; - Matrix numerical = numericalDerivative22(odo,x1,x2); - Matrix computed = Dodo2(x1,x2); - CHECK(assert_equal(numerical,computed,1e-9)); + Matrix H1,H2; + odo(x1,x2,H1,H2); + Matrix A1 = numericalDerivative21(odo,x1,x2); + CHECK(assert_equal(A1,H1,1e-9)); + Matrix A2 = numericalDerivative22(odo,x1,x2); + CHECK(assert_equal(A2,H2,1e-9)); } /* ************************************************************************* */ diff --git a/cpp/testVectorConfig.cpp b/cpp/testVectorConfig.cpp index 2d4073cb6..a80b298e1 100644 --- a/cpp/testVectorConfig.cpp +++ b/cpp/testVectorConfig.cpp @@ -47,11 +47,15 @@ TEST( VectorConfig, equals2 ) } /* ************************************************************************* */ + +#include +double inf = std::numeric_limits::infinity(); + TEST( VectorConfig, equals_nan ) { VectorConfig cfg1, cfg2; Vector v1 = Vector_(3, 5.0, 6.0, 7.0); - Vector v2 = Vector_(3, 0.0/0.0, 0.0/0.0, 0.0/0.0); + Vector v2 = Vector_(3, inf, inf, inf); cfg1.insert("x", v1); cfg2.insert("x", v2); CHECK(!cfg1.equals(cfg2));