12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

simulated2D now reduced to one .h and .cpp, in its own namespace, better naming, and new-style functions to serve as example

release/4.3a0
Frank Dellaert 2010-01-14 02:50:06 +00:00
parent fca2de8f95
commit 6b3e8cf49c
12 changed files with 130 additions and 197 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
cpp/Makefile.am
View File

@ -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

24
cpp/Point2Prior.h
View File

@ -1,24 +0,0 @@
// Frank Dellaert
// Simulated2D Prior
#include "NonlinearFactor.h"
#include "simulated2D.h"
namespace simulated2D {
struct Point2Prior: public gtsam::NonlinearFactor1<VectorConfig, std::string, Vector> {
Vector z_;
Point2Prior(const Vector& z, double sigma, const std::string& key) :
gtsam::NonlinearFactor1<VectorConfig, std::string, Vector>(sigma, key), z_(z) {
}
Vector evaluateError(const Vector& x, boost::optional<Matrix&> H = boost::none) const {
if (H) *H = Dprior(x);
return prior(x) - z_;
}
};
} // namespace simulated2D

29
cpp/Simulated2DMeasurement.h
View File

@ -1,29 +0,0 @@
// Christian Potthast
// Simulated2D Odometry
#include "NonlinearFactor.h"
#include "simulated2D.h"
namespace simulated2D {
struct Simulated2DMeasurement: public gtsam::NonlinearFactor2<VectorConfig,
PoseKey, Vector, PointKey, Vector> {
Vector z_;
Simulated2DMeasurement(const Vector& z, double sigma, const std::string& j1,
const std::string& j2) :
z_(z), gtsam::NonlinearFactor2<VectorConfig, PoseKey, Vector, PointKey,
Vector>(sigma, j1, j2) {
}
Vector evaluateError(const Vector& x1, const Vector& x2, boost::optional<
Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 = boost::none) const {
if (H1) *H1 = Dmea1(x1, x2);
if (H2) *H2 = Dmea2(x1, x2);
return mea(x1, x2) - z_;
}
};
} // namespace simulated2D

28
cpp/Simulated2DOdometry.h
View File

@ -1,28 +0,0 @@
// Christian Potthast
// Simulated2D Odometry
#include "NonlinearFactor.h"
#include "simulated2D.h"
namespace simulated2D {
struct Simulated2DOdometry: public gtsam::NonlinearFactor2<VectorConfig,
PoseKey, Vector, PointKey, Vector> {
Vector z_;
Simulated2DOdometry(const Vector& z, double sigma, const std::string& j1,
const std::string& j2) :
z_(z), gtsam::NonlinearFactor2<VectorConfig, PoseKey, Vector, PointKey,
Vector>(sigma, j1, j2) {
}
Vector evaluateError(const Vector& x1, const Vector& x2, boost::optional<
Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 = boost::none) const {
if (H1) *H1 = Dodo1(x1, x2);
if (H2) *H2 = Dodo2(x1, x2);
return odo(x1, x2) - z_;
}
};
} // namespace gtsam

64
cpp/simulated2D.cpp
View File

@ -8,60 +8,30 @@
namespace simulated2D {
/** prior on a single pose */
static Matrix I = gtsam::eye(2);
/* ************************************************************************* */
Vector prior (const Vector& x) {return x;}
/* ************************************************************************* */
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;
Vector prior(const Vector& x, boost::optional<Matrix&> H) {
if (H) *H = I;
return x;
}
/* ************************************************************************* */
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 odo(const Vector& x1, const Vector& x2, boost::optional<Matrix&> H1,
boost::optional<Matrix&> H2) {
if (H1) *H1 = -I;
if (H2) *H2 = I;
return x2 - x1;
}
/* ************************************************************************* */
Vector mea(const Vector& x, const Vector& l, boost::optional<Matrix&> H1,
boost::optional<Matrix&> 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

92
cpp/simulated2D.h
View File

@ -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<Matrix&> 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<Matrix&> H1 =
boost::none, boost::optional<Matrix&> 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<Matrix&> H1 =
boost::none, boost::optional<Matrix&> H2 = boost::none);
/**
* Unary factor encoding a soft prior on a vector
*/
struct Prior: public gtsam::NonlinearFactor1<VectorConfig, std::string,
Vector> {
Vector z_;
Prior(const Vector& z, double sigma, const std::string& key) :
gtsam::NonlinearFactor1<VectorConfig, std::string, Vector>(sigma, key),
z_(z) {
}
};
struct PointKey: public std::string {
PointKey(const std::string&s) :
std::string(s) {
Vector evaluateError(const Vector& x, boost::optional<Matrix&> 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<VectorConfig, PoseKey,
Vector, PointKey, Vector> {
Vector z_;
Odometry(const Vector& z, double sigma, const std::string& j1,
const std::string& j2) :
z_(z), gtsam::NonlinearFactor2<VectorConfig, PoseKey, Vector, PointKey,
Vector>(sigma, j1, j2) {
}
Vector evaluateError(const Vector& x1, const Vector& x2, boost::optional<
Matrix&> H1 = boost::none, boost::optional<Matrix&> 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<VectorConfig, PoseKey,
Vector, PointKey, Vector> {
/**
* 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<VectorConfig, PoseKey, Vector, PointKey,
Vector>(sigma, j1, j2) {
}
Vector evaluateError(const Vector& x1, const Vector& x2, boost::optional<
Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 = boost::none) const {
return mea(x1, x2, H1, H2) - z_;
}
};
} // namespace simulated2D

23
cpp/smallExample.cpp
View File

@ -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);
}

6
cpp/testNonlinearFactor.cpp
View File

@ -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));

9
cpp/testSQP.cpp
View File

@ -17,7 +17,6 @@
#include <NonlinearFactorGraph.h>
#include <NonlinearOptimizer.h>
#include <SQPOptimizer.h>
#include <Simulated2DMeasurement.h>
#include <simulated2D.h>
#include <Ordering.h>
#include <VSLAMConfig.h>
@ -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<NLGraph> 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<string> keys2; keys2 += "l1", "l2";

14
cpp/testSQPOptimizer.cpp
View File

@ -8,8 +8,6 @@
#include <boost/assign/std/list.hpp> // for operator +=
#include <boost/assign/std/map.hpp> // for insert
#include <boost/bind.hpp>
#include <Simulated2DMeasurement.h>
#include <Simulated2DOdometry.h>
#include <simulated2D.h>
#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<string> keys; keys += "l1", "l2";
@ -262,12 +260,12 @@ pair<NLGraph, VectorConfig> 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<NLGraph, VectorConfig> 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;

24
cpp/testSimulated2D.cpp
View File

@ -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));
}
/* ************************************************************************* */

6
cpp/testVectorConfig.cpp
View File

@ -47,11 +47,15 @@ TEST( VectorConfig, equals2 )
}
/* ************************************************************************* */
#include <limits>
double inf = std::numeric_limits<double>::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));