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

Using symbol_shorthand instead of redundant kx, kl functions

release/4.3a0
Frank Dellaert 2012-06-02 19:28:21 +00:00
parent a2512475c9
commit 8440939f27
24 changed files with 704 additions and 664 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
examples/vSLAMexample/vISAMexample.cpp
View File

@ -32,6 +32,10 @@ using boost::shared_ptr;
using namespace std; using namespace std;
using namespace gtsam; using namespace gtsam;
// Convenience for named keys
using symbol_shorthand::X;
using symbol_shorthand::L;
/* ************************************************************************* */ /* ************************************************************************* */
#define CALIB_FILE "calib.txt" #define CALIB_FILE "calib.txt"
#define LANDMARKS_FILE "landmarks.txt" #define LANDMARKS_FILE "landmarks.txt"
@ -83,22 +87,22 @@ void createNewFactors(shared_ptr<visualSLAM::Graph>& newFactors, boost::shared_p
newFactors->addMeasurement( newFactors->addMeasurement(
measurements[i].m_p, measurements[i].m_p,
measurementSigma, measurementSigma,
Symbol('x',pose_id), X(pose_id),
Symbol('l',measurements[i].m_idLandmark), L(measurements[i].m_idLandmark),
calib); calib);
} }
// ... we need priors on the new pose and all new landmarks // ... we need priors on the new pose and all new landmarks
newFactors->addPosePrior(pose_id, pose, poseSigma); newFactors->addPosePrior(pose_id, pose, poseSigma);
for (size_t i = 0; i < measurements.size(); i++) { for (size_t i = 0; i < measurements.size(); i++) {
newFactors->addPointPrior(Symbol('x',measurements[i].m_idLandmark), g_landmarks[measurements[i].m_idLandmark]); newFactors->addPointPrior(X(measurements[i].m_idLandmark), g_landmarks[measurements[i].m_idLandmark]);
} }
// Create initial values for all nodes in the newFactors // Create initial values for all nodes in the newFactors
initialValues = shared_ptr<Values> (new Values()); initialValues = shared_ptr<Values> (new Values());
initialValues->insert(Symbol('x',pose_id), pose); initialValues->insert(X(pose_id), pose);
for (size_t i = 0; i < measurements.size(); i++) { for (size_t i = 0; i < measurements.size(); i++) {
initialValues->insert(Symbol('l',measurements[i].m_idLandmark), g_landmarks[measurements[i].m_idLandmark]); initialValues->insert(L(measurements[i].m_idLandmark), g_landmarks[measurements[i].m_idLandmark]);
} }
} }

14
examples/vSLAMexample/vSFMexample.cpp
View File

@ -30,6 +30,10 @@ using boost::shared_ptr;
using namespace std; using namespace std;
using namespace gtsam; using namespace gtsam;
// Convenience for named keys
using symbol_shorthand::X;
using symbol_shorthand::L;
/* ************************************************************************* */ /* ************************************************************************* */
#define CALIB_FILE "calib.txt" #define CALIB_FILE "calib.txt"
#define LANDMARKS_FILE "landmarks.txt" #define LANDMARKS_FILE "landmarks.txt"
@ -84,8 +88,8 @@ visualSLAM::Graph setupGraph(std::vector<Feature2D>& measurements, SharedNoiseMo
g.addMeasurement( g.addMeasurement(
measurements[i].m_p, measurements[i].m_p,
measurementSigma, measurementSigma,
Symbol('x',measurements[i].m_idCamera), X(measurements[i].m_idCamera),
Symbol('l',measurements[i].m_idLandmark), L(measurements[i].m_idLandmark),
calib); calib);
} }
@ -103,11 +107,11 @@ Values initialize(std::map<int, Point3> landmarks, std::map<int, Pose3> poses) {
// Initialize landmarks 3D positions. // Initialize landmarks 3D positions.
for (map<int, Point3>::iterator lmit = landmarks.begin(); lmit != landmarks.end(); lmit++) for (map<int, Point3>::iterator lmit = landmarks.begin(); lmit != landmarks.end(); lmit++)
initValues.insert(Symbol('l',lmit->first), lmit->second); initValues.insert(L(lmit->first), lmit->second);
// Initialize camera poses. // Initialize camera poses.
for (map<int, Pose3>::iterator poseit = poses.begin(); poseit != poses.end(); poseit++) for (map<int, Pose3>::iterator poseit = poses.begin(); poseit != poses.end(); poseit++)
initValues.insert(Symbol('x',poseit->first), poseit->second); initValues.insert(X(poseit->first), poseit->second);
return initValues; return initValues;
} }
@ -137,7 +141,7 @@ int main(int argc, char* argv[]) {
// Add prior factor for all poses in the graph // Add prior factor for all poses in the graph
map<int, Pose3>::iterator poseit = g_poses.begin(); map<int, Pose3>::iterator poseit = g_poses.begin();
for (; poseit != g_poses.end(); poseit++) for (; poseit != g_poses.end(); poseit++)
graph.addPosePrior(Symbol('x',poseit->first), poseit->second, noiseModel::Unit::Create(1)); graph.addPosePrior(X(poseit->first), poseit->second, noiseModel::Unit::Create(1));
// Optimize the graph // Optimize the graph
cout << "*******************************************************" << endl; cout << "*******************************************************" << endl;

8
gtsam/nonlinear/tests/testValues.cpp
View File

@ -32,6 +32,10 @@ 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();
// Convenience for named keys
using symbol_shorthand::X;
using symbol_shorthand::L;
const Symbol key1('v',1), key2('v',2), key3('v',3), key4('v',4); const Symbol key1('v',1), key2('v',2), key3('v',3), key4('v',4);
/* ************************************************************************* */ /* ************************************************************************* */
@ -333,9 +337,9 @@ TEST(Values, Symbol_filter) {
Pose3 pose3(Pose2(0.3, 0.7, 0.9)); Pose3 pose3(Pose2(0.3, 0.7, 0.9));
Values values; Values values;
values.insert(Symbol('x',0), pose0); values.insert(X(0), pose0);
values.insert(Symbol('y',1), pose1); values.insert(Symbol('y',1), pose1);
values.insert(Symbol('x',2), pose2); values.insert(X(2), pose2);
values.insert(Symbol('y',3), pose3); values.insert(Symbol('y',3), pose3);
int i = 0; int i = 0;

58
gtsam/slam/smallExample.cpp
View File

@ -47,8 +47,8 @@ namespace example {
static const Index _x_=0, _y_=1, _z_=2; static const Index _x_=0, _y_=1, _z_=2;
// Convenience for named keys // Convenience for named keys
Key kx(size_t i) { return Symbol('x',i); } using symbol_shorthand::X;
Key kl(size_t i) { return Symbol('l',i); } using symbol_shorthand::L;
/* ************************************************************************* */ /* ************************************************************************* */
boost::shared_ptr<const Graph> sharedNonlinearFactorGraph() { boost::shared_ptr<const Graph> sharedNonlinearFactorGraph() {
@ -58,22 +58,22 @@ namespace example {
// prior on x1 // prior on x1
Point2 mu; Point2 mu;
shared f1(new simulated2D::Prior(mu, sigma0_1, kx(1))); shared f1(new simulated2D::Prior(mu, sigma0_1, X(1)));
nlfg->push_back(f1); nlfg->push_back(f1);
// odometry between x1 and x2 // odometry between x1 and x2
Point2 z2(1.5, 0); Point2 z2(1.5, 0);
shared f2(new simulated2D::Odometry(z2, sigma0_1, kx(1), kx(2))); shared f2(new simulated2D::Odometry(z2, sigma0_1, X(1), X(2)));
nlfg->push_back(f2); nlfg->push_back(f2);
// measurement between x1 and l1 // measurement between x1 and l1
Point2 z3(0, -1); Point2 z3(0, -1);
shared f3(new simulated2D::Measurement(z3, sigma0_2, kx(1), kl(1))); shared f3(new simulated2D::Measurement(z3, sigma0_2, X(1), L(1)));
nlfg->push_back(f3); nlfg->push_back(f3);
// measurement between x2 and l1 // measurement between x2 and l1
Point2 z4(-1.5, -1.); Point2 z4(-1.5, -1.);
shared f4(new simulated2D::Measurement(z4, sigma0_2, kx(2), kl(1))); shared f4(new simulated2D::Measurement(z4, sigma0_2, X(2), L(1)));
nlfg->push_back(f4); nlfg->push_back(f4);
return nlfg; return nlfg;
@ -87,9 +87,9 @@ namespace example {
/* ************************************************************************* */ /* ************************************************************************* */
Values createValues() { Values createValues() {
Values c; Values c;
c.insert(kx(1), Point2(0.0, 0.0)); c.insert(X(1), Point2(0.0, 0.0));
c.insert(kx(2), Point2(1.5, 0.0)); c.insert(X(2), Point2(1.5, 0.0));
c.insert(kl(1), Point2(0.0, -1.0)); c.insert(L(1), Point2(0.0, -1.0));
return c; return c;
} }
@ -105,9 +105,9 @@ namespace example {
/* ************************************************************************* */ /* ************************************************************************* */
boost::shared_ptr<const Values> sharedNoisyValues() { boost::shared_ptr<const Values> sharedNoisyValues() {
boost::shared_ptr<Values> c(new Values); boost::shared_ptr<Values> c(new Values);
c->insert(kx(1), Point2(0.1, 0.1)); c->insert(X(1), Point2(0.1, 0.1));
c->insert(kx(2), Point2(1.4, 0.2)); c->insert(X(2), Point2(1.4, 0.2));
c->insert(kl(1), Point2(0.1, -1.1)); c->insert(L(1), Point2(0.1, -1.1));
return c; return c;
} }
@ -119,18 +119,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[kl(1)]] = Vector_(2, -0.1, 0.1); c[ordering[L(1)]] = Vector_(2, -0.1, 0.1);
c[ordering[kx(1)]] = Vector_(2, -0.1, -0.1); c[ordering[X(1)]] = Vector_(2, -0.1, -0.1);
c[ordering[kx(2)]] = Vector_(2, 0.1, -0.2); c[ordering[X(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[kl(1)]] = zero(2); c[ordering[L(1)]] = zero(2);
c[ordering[kx(1)]] = zero(2); c[ordering[X(1)]] = zero(2);
c[ordering[kx(2)]] = zero(2); c[ordering[X(2)]] = zero(2);
return c; return c;
} }
@ -142,16 +142,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[kx(1)], 10*eye(2), -1.0*ones(2), unit2); fg.add(ordering[X(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[kx(1)], -10*eye(2),ordering[kx(2)], 10*eye(2), Vector_(2, 2.0, -1.0), unit2); fg.add(ordering[X(1)], -10*eye(2),ordering[X(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[kx(1)], -5*eye(2), ordering[kl(1)], 5*eye(2), Vector_(2, 0.0, 1.0), unit2); fg.add(ordering[X(1)], -5*eye(2), ordering[L(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[kx(2)], -5*eye(2), ordering[kl(1)], 5*eye(2), Vector_(2, -1.0, 1.5), unit2); fg.add(ordering[X(2)], -5*eye(2), ordering[L(1)], 5*eye(2), Vector_(2, -1.0, 1.5), unit2);
return *fg.dynamicCastFactors<FactorGraph<JacobianFactor> >(); return *fg.dynamicCastFactors<FactorGraph<JacobianFactor> >();
} }
@ -221,7 +221,7 @@ namespace example {
Vector z = Vector_(2, 1.0, 0.0); Vector z = Vector_(2, 1.0, 0.0);
double sigma = 0.1; double sigma = 0.1;
boost::shared_ptr<smallOptimize::UnaryFactor> factor( boost::shared_ptr<smallOptimize::UnaryFactor> factor(
new smallOptimize::UnaryFactor(z, noiseModel::Isotropic::Sigma(2,sigma), kx(1))); new smallOptimize::UnaryFactor(z, noiseModel::Isotropic::Sigma(2,sigma), X(1)));
fg->push_back(factor); fg->push_back(factor);
return fg; return fg;
} }
@ -239,23 +239,23 @@ namespace example {
// prior on x1 // prior on x1
Point2 x1(1.0, 0.0); Point2 x1(1.0, 0.0);
shared prior(new simulated2D::Prior(x1, sigma1_0, kx(1))); shared prior(new simulated2D::Prior(x1, sigma1_0, X(1)));
nlfg.push_back(prior); nlfg.push_back(prior);
poses.insert(kx(1), x1); poses.insert(X(1), x1);
for (int t = 2; t <= T; t++) { for (int t = 2; t <= T; t++) {
// odometry between x_t and x_{t-1} // odometry between x_t and x_{t-1}
Point2 odo(1.0, 0.0); Point2 odo(1.0, 0.0);
shared odometry(new simulated2D::Odometry(odo, sigma1_0, kx(t - 1), kx(t))); shared odometry(new simulated2D::Odometry(odo, sigma1_0, X(t - 1), X(t)));
nlfg.push_back(odometry); nlfg.push_back(odometry);
// measurement on x_t is like perfect GPS // measurement on x_t is like perfect GPS
Point2 xt(t, 0); Point2 xt(t, 0);
shared measurement(new simulated2D::Prior(xt, sigma1_0, kx(t))); shared measurement(new simulated2D::Prior(xt, sigma1_0, X(t)));
nlfg.push_back(measurement); nlfg.push_back(measurement);
// initial estimate // initial estimate
poses.insert(kx(t), xt); poses.insert(X(t), xt);
} }
return make_pair(nlfg, poses); return make_pair(nlfg, poses);
@ -416,7 +416,7 @@ namespace example {
/* ************************************************************************* */ /* ************************************************************************* */
// Create key for simulated planar graph // Create key for simulated planar graph
Symbol key(int x, int y) { Symbol key(int x, int y) {
return kx(1000*x+y); return X(1000*x+y);
} }
/* ************************************************************************* */ /* ************************************************************************* */

236
gtsam/slam/tests/testGeneralSFMFactor.cpp
View File

@ -38,6 +38,10 @@ using namespace boost;
using namespace std; using namespace std;
using namespace gtsam; using namespace gtsam;
// Convenience for named keys
using symbol_shorthand::X;
using symbol_shorthand::L;
typedef PinholeCamera<Cal3_S2> GeneralCamera; typedef PinholeCamera<Cal3_S2> GeneralCamera;
typedef GeneralSFMFactor<GeneralCamera, Point3> Projection; typedef GeneralSFMFactor<GeneralCamera, Point3> Projection;
typedef NonlinearEquality<GeneralCamera> CameraConstraint; typedef NonlinearEquality<GeneralCamera> CameraConstraint;
@ -46,16 +50,16 @@ typedef NonlinearEquality<Point3> Point3Constraint;
class Graph: public NonlinearFactorGraph { class Graph: public NonlinearFactorGraph {
public: public:
void addMeasurement(int i, int j, const Point2& z, const SharedNoiseModel& model) { void addMeasurement(int i, int j, const Point2& z, const SharedNoiseModel& model) {
push_back(boost::make_shared<Projection>(z, model, Symbol('x',i), Symbol('l',j))); push_back(boost::make_shared<Projection>(z, model, X(i), L(j)));
} }
void addCameraConstraint(int j, const GeneralCamera& p) { void addCameraConstraint(int j, const GeneralCamera& p) {
boost::shared_ptr<CameraConstraint> factor(new CameraConstraint(Symbol('x',j), p)); boost::shared_ptr<CameraConstraint> factor(new CameraConstraint(X(j), p));
push_back(factor); push_back(factor);
} }
void addPoint3Constraint(int j, const Point3& p) { void addPoint3Constraint(int j, const Point3& p) {
boost::shared_ptr<Point3Constraint> factor(new Point3Constraint(Symbol('l',j), p)); boost::shared_ptr<Point3Constraint> factor(new Point3Constraint(L(j), p));
push_back(factor); push_back(factor);
} }
@ -98,14 +102,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('x',1), Symbol('l',1))); boost::shared_ptr<Projection> factor(new Projection(z, sigma, X(1), 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('x',1), GeneralCamera(x1)); values.insert(X(1), GeneralCamera(x1));
Point3 l1; values.insert(Symbol('l',1), l1); Point3 l1; values.insert(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)));
} }
@ -114,20 +118,20 @@ static const double baseline = 5.0 ;
/* ************************************************************************* */ /* ************************************************************************* */
vector<Point3> genPoint3() { vector<Point3> genPoint3() {
const double z = 5; const double z = 5;
vector<Point3> L ; vector<Point3> landmarks ;
L.push_back(Point3 (-1.0,-1.0, z)); landmarks.push_back(Point3 (-1.0,-1.0, z));
L.push_back(Point3 (-1.0, 1.0, z)); landmarks.push_back(Point3 (-1.0, 1.0, z));
L.push_back(Point3 ( 1.0, 1.0, z)); landmarks.push_back(Point3 ( 1.0, 1.0, z));
L.push_back(Point3 ( 1.0,-1.0, z)); landmarks.push_back(Point3 ( 1.0,-1.0, z));
L.push_back(Point3 (-1.5,-1.5, 1.5*z)); landmarks.push_back(Point3 (-1.5,-1.5, 1.5*z));
L.push_back(Point3 (-1.5, 1.5, 1.5*z)); landmarks.push_back(Point3 (-1.5, 1.5, 1.5*z));
L.push_back(Point3 ( 1.5, 1.5, 1.5*z)); landmarks.push_back(Point3 ( 1.5, 1.5, 1.5*z));
L.push_back(Point3 ( 1.5,-1.5, 1.5*z)); landmarks.push_back(Point3 ( 1.5,-1.5, 1.5*z));
L.push_back(Point3 (-2.0,-2.0, 2*z)); landmarks.push_back(Point3 (-2.0,-2.0, 2*z));
L.push_back(Point3 (-2.0, 2.0, 2*z)); landmarks.push_back(Point3 (-2.0, 2.0, 2*z));
L.push_back(Point3 ( 2.0, 2.0, 2*z)); landmarks.push_back(Point3 ( 2.0, 2.0, 2*z));
L.push_back(Point3 ( 2.0,-2.0, 2*z)); landmarks.push_back(Point3 ( 2.0,-2.0, 2*z));
return L ; return landmarks ;
} }
vector<GeneralCamera> genCameraDefaultCalibration() { vector<GeneralCamera> genCameraDefaultCalibration() {
@ -145,10 +149,10 @@ vector<GeneralCamera> genCameraVariableCalibration() {
return X ; return X ;
} }
shared_ptr<Ordering> getOrdering(const vector<GeneralCamera>& X, const vector<Point3>& L) { shared_ptr<Ordering> getOrdering(const vector<GeneralCamera>& cameras, const vector<Point3>& landmarks) {
shared_ptr<Ordering> ordering(new Ordering); shared_ptr<Ordering> ordering(new Ordering);
for ( size_t i = 0 ; i < L.size() ; ++i ) ordering->push_back(Symbol('l', i)) ; for ( size_t i = 0 ; i < landmarks.size() ; ++i ) ordering->push_back(L(i)) ;
for ( size_t i = 0 ; i < X.size() ; ++i ) ordering->push_back(Symbol('x', i)) ; for ( size_t i = 0 ; i < cameras.size() ; ++i ) ordering->push_back(X(i)) ;
return ordering ; return ordering ;
} }
@ -156,37 +160,37 @@ shared_ptr<Ordering> getOrdering(const vector<GeneralCamera>& X, const vector<Po
/* ************************************************************************* */ /* ************************************************************************* */
TEST( GeneralSFMFactor, optimize_defaultK ) { TEST( GeneralSFMFactor, optimize_defaultK ) {
vector<Point3> L = genPoint3(); vector<Point3> landmarks = genPoint3();
vector<GeneralCamera> X = genCameraDefaultCalibration(); vector<GeneralCamera> cameras = genCameraDefaultCalibration();
// add measurement with noise // add measurement with noise
Graph graph; Graph graph;
for ( size_t j = 0 ; j < X.size() ; ++j) { for ( size_t j = 0 ; j < cameras.size() ; ++j) {
for ( size_t i = 0 ; i < L.size() ; ++i) { for ( size_t i = 0 ; i < landmarks.size() ; ++i) {
Point2 pt = X[j].project(L[i]) ; Point2 pt = cameras[j].project(landmarks[i]) ;
graph.addMeasurement(j, i, pt, sigma1); graph.addMeasurement(j, i, pt, sigma1);
} }
} }
const size_t nMeasurements = X.size()*L.size() ; const size_t nMeasurements = cameras.size()*landmarks.size() ;
// add initial // add initial
const double noise = baseline*0.1; const double noise = baseline*0.1;
Values values; Values values;
for ( size_t i = 0 ; i < X.size() ; ++i ) for ( size_t i = 0 ; i < cameras.size() ; ++i )
values.insert(Symbol('x',i), X[i]) ; values.insert(X(i), cameras[i]) ;
for ( size_t i = 0 ; i < L.size() ; ++i ) { for ( size_t i = 0 ; i < landmarks.size() ; ++i ) {
Point3 pt(L[i].x()+noise*getGaussian(), Point3 pt(landmarks[i].x()+noise*getGaussian(),
L[i].y()+noise*getGaussian(), landmarks[i].y()+noise*getGaussian(),
L[i].z()+noise*getGaussian()); landmarks[i].z()+noise*getGaussian());
values.insert(Symbol('l',i), pt) ; values.insert(L(i), pt) ;
} }
graph.addCameraConstraint(0, X[0]); graph.addCameraConstraint(0, cameras[0]);
// Create an ordering of the variables // Create an ordering of the variables
Ordering ordering = *getOrdering(X,L); Ordering ordering = *getOrdering(cameras,landmarks);
LevenbergMarquardtOptimizer optimizer(graph, values, ordering); LevenbergMarquardtOptimizer optimizer(graph, values, ordering);
Values final = optimizer.optimize(); Values final = optimizer.optimize();
EXPECT(optimizer.error() < 0.5 * 1e-5 * nMeasurements); EXPECT(optimizer.error() < 0.5 * 1e-5 * nMeasurements);
@ -194,42 +198,42 @@ TEST( GeneralSFMFactor, optimize_defaultK ) {
/* ************************************************************************* */ /* ************************************************************************* */
TEST( GeneralSFMFactor, optimize_varK_SingleMeasurementError ) { TEST( GeneralSFMFactor, optimize_varK_SingleMeasurementError ) {
vector<Point3> L = genPoint3(); vector<Point3> landmarks = genPoint3();
vector<GeneralCamera> X = genCameraVariableCalibration(); vector<GeneralCamera> cameras = genCameraVariableCalibration();
// add measurement with noise // add measurement with noise
Graph graph; Graph graph;
for ( size_t j = 0 ; j < X.size() ; ++j) { for ( size_t j = 0 ; j < cameras.size() ; ++j) {
for ( size_t i = 0 ; i < L.size() ; ++i) { for ( size_t i = 0 ; i < landmarks.size() ; ++i) {
Point2 pt = X[j].project(L[i]) ; Point2 pt = cameras[j].project(landmarks[i]) ;
graph.addMeasurement(j, i, pt, sigma1); graph.addMeasurement(j, i, pt, sigma1);
} }
} }
const size_t nMeasurements = X.size()*L.size() ; const size_t nMeasurements = cameras.size()*landmarks.size() ;
// add initial // add initial
const double noise = baseline*0.1; const double noise = baseline*0.1;
Values values; Values values;
for ( size_t i = 0 ; i < X.size() ; ++i ) for ( size_t i = 0 ; i < cameras.size() ; ++i )
values.insert(Symbol('x',i), X[i]) ; values.insert(X(i), cameras[i]) ;
// add noise only to the first landmark // add noise only to the first landmark
for ( size_t i = 0 ; i < L.size() ; ++i ) { for ( size_t i = 0 ; i < landmarks.size() ; ++i ) {
if ( i == 0 ) { if ( i == 0 ) {
Point3 pt(L[i].x()+noise*getGaussian(), Point3 pt(landmarks[i].x()+noise*getGaussian(),
L[i].y()+noise*getGaussian(), landmarks[i].y()+noise*getGaussian(),
L[i].z()+noise*getGaussian()); landmarks[i].z()+noise*getGaussian());
values.insert(Symbol('l',i), pt) ; values.insert(L(i), pt) ;
} }
else { else {
values.insert(Symbol('l',i), L[i]) ; values.insert(L(i), landmarks[i]) ;
} }
} }
graph.addCameraConstraint(0, X[0]); graph.addCameraConstraint(0, cameras[0]);
const double reproj_error = 1e-5; const double reproj_error = 1e-5;
Ordering ordering = *getOrdering(X,L); Ordering ordering = *getOrdering(cameras,landmarks);
LevenbergMarquardtOptimizer optimizer(graph, values, ordering); LevenbergMarquardtOptimizer optimizer(graph, values, ordering);
Values final = optimizer.optimize(); Values final = optimizer.optimize();
EXPECT(optimizer.error() < 0.5 * reproj_error * nMeasurements); EXPECT(optimizer.error() < 0.5 * reproj_error * nMeasurements);
@ -238,39 +242,39 @@ TEST( GeneralSFMFactor, optimize_varK_SingleMeasurementError ) {
/* ************************************************************************* */ /* ************************************************************************* */
TEST( GeneralSFMFactor, optimize_varK_FixCameras ) { TEST( GeneralSFMFactor, optimize_varK_FixCameras ) {
vector<Point3> L = genPoint3(); vector<Point3> landmarks = genPoint3();
vector<GeneralCamera> X = genCameraVariableCalibration(); vector<GeneralCamera> cameras = genCameraVariableCalibration();
// add measurement with noise // add measurement with noise
const double noise = baseline*0.1; const double noise = baseline*0.1;
Graph graph; Graph graph;
for ( size_t j = 0 ; j < X.size() ; ++j) { for ( size_t j = 0 ; j < cameras.size() ; ++j) {
for ( size_t i = 0 ; i < L.size() ; ++i) { for ( size_t i = 0 ; i < landmarks.size() ; ++i) {
Point2 pt = X[j].project(L[i]) ; Point2 pt = cameras[j].project(landmarks[i]) ;
graph.addMeasurement(j, i, pt, sigma1); graph.addMeasurement(j, i, pt, sigma1);
} }
} }
const size_t nMeasurements = L.size()*X.size(); const size_t nMeasurements = landmarks.size()*cameras.size();
Values values; Values values;
for ( size_t i = 0 ; i < X.size() ; ++i ) for ( size_t i = 0 ; i < cameras.size() ; ++i )
values.insert(Symbol('x',i), X[i]) ; values.insert(X(i), cameras[i]) ;
for ( size_t i = 0 ; i < L.size() ; ++i ) { for ( size_t i = 0 ; i < landmarks.size() ; ++i ) {
Point3 pt(L[i].x()+noise*getGaussian(), Point3 pt(landmarks[i].x()+noise*getGaussian(),
L[i].y()+noise*getGaussian(), landmarks[i].y()+noise*getGaussian(),
L[i].z()+noise*getGaussian()); landmarks[i].z()+noise*getGaussian());
//Point3 pt(L[i].x(), L[i].y(), L[i].z()); //Point3 pt(landmarks[i].x(), landmarks[i].y(), landmarks[i].z());
values.insert(Symbol('l',i), pt) ; values.insert(L(i), pt) ;
} }
for ( size_t i = 0 ; i < X.size() ; ++i ) for ( size_t i = 0 ; i < cameras.size() ; ++i )
graph.addCameraConstraint(i, X[i]); graph.addCameraConstraint(i, cameras[i]);
const double reproj_error = 1e-5 ; const double reproj_error = 1e-5 ;
Ordering ordering = *getOrdering(X,L); Ordering ordering = *getOrdering(cameras,landmarks);
LevenbergMarquardtOptimizer optimizer(graph, values, ordering); LevenbergMarquardtOptimizer optimizer(graph, values, ordering);
Values final = optimizer.optimize(); Values final = optimizer.optimize();
EXPECT(optimizer.error() < 0.5 * reproj_error * nMeasurements); EXPECT(optimizer.error() < 0.5 * reproj_error * nMeasurements);
@ -279,29 +283,29 @@ TEST( GeneralSFMFactor, optimize_varK_FixCameras ) {
/* ************************************************************************* */ /* ************************************************************************* */
TEST( GeneralSFMFactor, optimize_varK_FixLandmarks ) { TEST( GeneralSFMFactor, optimize_varK_FixLandmarks ) {
vector<Point3> L = genPoint3(); vector<Point3> landmarks = genPoint3();
vector<GeneralCamera> X = genCameraVariableCalibration(); vector<GeneralCamera> cameras = genCameraVariableCalibration();
// add measurement with noise // add measurement with noise
Graph graph; Graph graph;
for ( size_t j = 0 ; j < X.size() ; ++j) { for ( size_t j = 0 ; j < cameras.size() ; ++j) {
for ( size_t i = 0 ; i < L.size() ; ++i) { for ( size_t i = 0 ; i < landmarks.size() ; ++i) {
Point2 pt = X[j].project(L[i]) ; Point2 pt = cameras[j].project(landmarks[i]) ;
graph.addMeasurement(j, i, pt, sigma1); graph.addMeasurement(j, i, pt, sigma1);
} }
} }
const size_t nMeasurements = L.size()*X.size(); const size_t nMeasurements = landmarks.size()*cameras.size();
Values values; Values values;
for ( size_t i = 0 ; i < X.size() ; ++i ) { for ( size_t i = 0 ; i < cameras.size() ; ++i ) {
const double const double
rot_noise = 1e-5, rot_noise = 1e-5,
trans_noise = 1e-3, trans_noise = 1e-3,
focal_noise = 1, focal_noise = 1,
skew_noise = 1e-5; skew_noise = 1e-5;
if ( i == 0 ) { if ( i == 0 ) {
values.insert(Symbol('x',i), X[i]) ; values.insert(X(i), cameras[i]) ;
} }
else { else {
@ -312,22 +316,22 @@ TEST( GeneralSFMFactor, optimize_varK_FixLandmarks ) {
skew_noise, // s skew_noise, // s
trans_noise, trans_noise // ux, uy trans_noise, trans_noise // ux, uy
) ; ) ;
values.insert(Symbol('x',i), X[i].retract(delta)) ; values.insert(X(i), cameras[i].retract(delta)) ;
} }
} }
for ( size_t i = 0 ; i < L.size() ; ++i ) { for ( size_t i = 0 ; i < landmarks.size() ; ++i ) {
values.insert(Symbol('l',i), L[i]) ; values.insert(L(i), landmarks[i]) ;
} }
// fix X0 and all landmarks, allow only the X[1] to move // fix X0 and all landmarks, allow only the cameras[1] to move
graph.addCameraConstraint(0, X[0]); graph.addCameraConstraint(0, cameras[0]);
for ( size_t i = 0 ; i < L.size() ; ++i ) for ( size_t i = 0 ; i < landmarks.size() ; ++i )
graph.addPoint3Constraint(i, L[i]); graph.addPoint3Constraint(i, landmarks[i]);
const double reproj_error = 1e-5 ; const double reproj_error = 1e-5 ;
Ordering ordering = *getOrdering(X,L); Ordering ordering = *getOrdering(cameras,landmarks);
LevenbergMarquardtOptimizer optimizer(graph, values, ordering); LevenbergMarquardtOptimizer optimizer(graph, values, ordering);
Values final = optimizer.optimize(); Values final = optimizer.optimize();
EXPECT(optimizer.error() < 0.5 * reproj_error * nMeasurements); EXPECT(optimizer.error() < 0.5 * reproj_error * nMeasurements);
@ -335,43 +339,43 @@ TEST( GeneralSFMFactor, optimize_varK_FixLandmarks ) {
/* ************************************************************************* */ /* ************************************************************************* */
TEST( GeneralSFMFactor, optimize_varK_BA ) { TEST( GeneralSFMFactor, optimize_varK_BA ) {
vector<Point3> L = genPoint3(); vector<Point3> landmarks = genPoint3();
vector<GeneralCamera> X = genCameraVariableCalibration(); vector<GeneralCamera> cameras = genCameraVariableCalibration();
// add measurement with noise // add measurement with noise
Graph graph; Graph graph;
for ( size_t j = 0 ; j < X.size() ; ++j) { for ( size_t j = 0 ; j < cameras.size() ; ++j) {
for ( size_t i = 0 ; i < L.size() ; ++i) { for ( size_t i = 0 ; i < landmarks.size() ; ++i) {
Point2 pt = X[j].project(L[i]) ; Point2 pt = cameras[j].project(landmarks[i]) ;
graph.addMeasurement(j, i, pt, sigma1); graph.addMeasurement(j, i, pt, sigma1);
} }
} }
const size_t nMeasurements = X.size()*L.size() ; const size_t nMeasurements = cameras.size()*landmarks.size() ;
// add initial // add initial
const double noise = baseline*0.1; const double noise = baseline*0.1;
Values values; Values values;
for ( size_t i = 0 ; i < X.size() ; ++i ) for ( size_t i = 0 ; i < cameras.size() ; ++i )
values.insert(Symbol('x',i), X[i]) ; values.insert(X(i), cameras[i]) ;
// add noise only to the first landmark // add noise only to the first landmark
for ( size_t i = 0 ; i < L.size() ; ++i ) { for ( size_t i = 0 ; i < landmarks.size() ; ++i ) {
Point3 pt(L[i].x()+noise*getGaussian(), Point3 pt(landmarks[i].x()+noise*getGaussian(),
L[i].y()+noise*getGaussian(), landmarks[i].y()+noise*getGaussian(),
L[i].z()+noise*getGaussian()); landmarks[i].z()+noise*getGaussian());
values.insert(Symbol('l',i), pt) ; values.insert(L(i), pt) ;
} }
// Constrain position of system with the first camera constrained to the origin // Constrain position of system with the first camera constrained to the origin
graph.addCameraConstraint(0, X[0]); graph.addCameraConstraint(0, cameras[0]);
// Constrain the scale of the problem with a soft range factor of 1m between the cameras // Constrain the scale of the problem with a soft range factor of 1m between the cameras
graph.add(RangeFactor<GeneralCamera,GeneralCamera>(Symbol('x',0), Symbol('x',1), 2.0, sharedSigma(1, 10.0))); graph.add(RangeFactor<GeneralCamera,GeneralCamera>(X(0), X(1), 2.0, sharedSigma(1, 10.0)));
const double reproj_error = 1e-5 ; const double reproj_error = 1e-5 ;
Ordering ordering = *getOrdering(X,L); Ordering ordering = *getOrdering(cameras,landmarks);
LevenbergMarquardtOptimizer optimizer(graph, values, ordering); LevenbergMarquardtOptimizer optimizer(graph, values, ordering);
Values final = optimizer.optimize(); Values final = optimizer.optimize();
EXPECT(optimizer.error() < 0.5 * reproj_error * nMeasurements); EXPECT(optimizer.error() < 0.5 * reproj_error * nMeasurements);
@ -382,17 +386,17 @@ TEST(GeneralSFMFactor, GeneralCameraPoseRange) {
// Tests range factor between a GeneralCamera and a Pose3 // Tests range factor between a GeneralCamera and a Pose3
Graph graph; Graph graph;
graph.addCameraConstraint(0, GeneralCamera()); graph.addCameraConstraint(0, GeneralCamera());
graph.add(RangeFactor<GeneralCamera, Pose3>(Symbol('x',0), Symbol('x',1), 2.0, sharedSigma(1, 1.0))); graph.add(RangeFactor<GeneralCamera, Pose3>(X(0), X(1), 2.0, sharedSigma(1, 1.0)));
graph.add(PriorFactor<Pose3>(Symbol('x',1), Pose3(Rot3(), Point3(1.0, 0.0, 0.0)), sharedSigma(6, 1.0))); graph.add(PriorFactor<Pose3>(X(1), Pose3(Rot3(), Point3(1.0, 0.0, 0.0)), sharedSigma(6, 1.0)));
Values init; Values init;
init.insert(Symbol('x',0), GeneralCamera()); init.insert(X(0), GeneralCamera());
init.insert(Symbol('x',1), Pose3(Rot3(), Point3(1.0,1.0,1.0))); init.insert(X(1), Pose3(Rot3(), Point3(1.0,1.0,1.0)));
// The optimal value between the 2m range factor and 1m prior is 1.5m // The optimal value between the 2m range factor and 1m prior is 1.5m
Values expected; Values expected;
expected.insert(Symbol('x',0), GeneralCamera()); expected.insert(X(0), GeneralCamera());
expected.insert(Symbol('x',1), Pose3(Rot3(), Point3(1.5,0.0,0.0))); expected.insert(X(1), Pose3(Rot3(), Point3(1.5,0.0,0.0)));
LevenbergMarquardtParams params; LevenbergMarquardtParams params;
params.absoluteErrorTol = 1e-9; params.absoluteErrorTol = 1e-9;
@ -406,17 +410,17 @@ TEST(GeneralSFMFactor, GeneralCameraPoseRange) {
TEST(GeneralSFMFactor, CalibratedCameraPoseRange) { TEST(GeneralSFMFactor, CalibratedCameraPoseRange) {
// Tests range factor between a CalibratedCamera and a Pose3 // Tests range factor between a CalibratedCamera and a Pose3
NonlinearFactorGraph graph; NonlinearFactorGraph graph;
graph.add(PriorFactor<CalibratedCamera>(Symbol('x',0), CalibratedCamera(), sharedSigma(6, 1.0))); graph.add(PriorFactor<CalibratedCamera>(X(0), CalibratedCamera(), sharedSigma(6, 1.0)));
graph.add(RangeFactor<CalibratedCamera, Pose3>(Symbol('x',0), Symbol('x',1), 2.0, sharedSigma(1, 1.0))); graph.add(RangeFactor<CalibratedCamera, Pose3>(X(0), X(1), 2.0, sharedSigma(1, 1.0)));
graph.add(PriorFactor<Pose3>(Symbol('x',1), Pose3(Rot3(), Point3(1.0, 0.0, 0.0)), sharedSigma(6, 1.0))); graph.add(PriorFactor<Pose3>(X(1), Pose3(Rot3(), Point3(1.0, 0.0, 0.0)), sharedSigma(6, 1.0)));
Values init; Values init;
init.insert(Symbol('x',0), CalibratedCamera()); init.insert(X(0), CalibratedCamera());
init.insert(Symbol('x',1), Pose3(Rot3(), Point3(1.0,1.0,1.0))); init.insert(X(1), Pose3(Rot3(), Point3(1.0,1.0,1.0)));
Values expected; Values expected;
expected.insert(Symbol('x',0), CalibratedCamera(Pose3(Rot3(), Point3(-0.333333333333, 0, 0)))); expected.insert(X(0), CalibratedCamera(Pose3(Rot3(), Point3(-0.333333333333, 0, 0))));
expected.insert(Symbol('x',1), Pose3(Rot3(), Point3(1.333333333333, 0, 0))); expected.insert(X(1), Pose3(Rot3(), Point3(1.333333333333, 0, 0)));
LevenbergMarquardtParams params; LevenbergMarquardtParams params;
params.absoluteErrorTol = 1e-9; params.absoluteErrorTol = 1e-9;

226
gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp
View File

@ -38,6 +38,10 @@
using namespace std; using namespace std;
using namespace gtsam; using namespace gtsam;
// Convenience for named keys
using symbol_shorthand::X;
using symbol_shorthand::L;
typedef PinholeCamera<Cal3Bundler> GeneralCamera; typedef PinholeCamera<Cal3Bundler> GeneralCamera;
typedef GeneralSFMFactor<GeneralCamera, Point3> Projection; typedef GeneralSFMFactor<GeneralCamera, Point3> Projection;
typedef NonlinearEquality<GeneralCamera> CameraConstraint; typedef NonlinearEquality<GeneralCamera> CameraConstraint;
@ -47,16 +51,16 @@ typedef NonlinearEquality<Point3> Point3Constraint;
class Graph: public NonlinearFactorGraph { class Graph: public NonlinearFactorGraph {
public: public:
void addMeasurement(const int& i, const int& j, const Point2& z, const SharedNoiseModel& model) { void addMeasurement(const int& i, const int& j, const Point2& z, const SharedNoiseModel& model) {
push_back(boost::make_shared<Projection>(z, model, Symbol('x',i), Symbol('l',j))); push_back(boost::make_shared<Projection>(z, model, X(i), L(j)));
} }
void addCameraConstraint(int j, const GeneralCamera& p) { void addCameraConstraint(int j, const GeneralCamera& p) {
boost::shared_ptr<CameraConstraint> factor(new CameraConstraint(Symbol('x', j), p)); boost::shared_ptr<CameraConstraint> factor(new CameraConstraint(X(j), p));
push_back(factor); push_back(factor);
} }
void addPoint3Constraint(int j, const Point3& p) { void addPoint3Constraint(int j, const Point3& p) {
boost::shared_ptr<Point3Constraint> factor(new Point3Constraint(Symbol('l', j), p)); boost::shared_ptr<Point3Constraint> factor(new Point3Constraint(L(j), p));
push_back(factor); push_back(factor);
} }
@ -100,14 +104,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('x',1), Symbol('l',1))); factor(new Projection(z, sigma, X(1), 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('x',1), GeneralCamera(x1)); values.insert(X(1), GeneralCamera(x1));
Point3 l1; values.insert(Symbol('l',1), l1); Point3 l1; values.insert(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)));
} }
@ -117,78 +121,78 @@ static const double baseline = 5.0 ;
/* ************************************************************************* */ /* ************************************************************************* */
vector<Point3> genPoint3() { vector<Point3> genPoint3() {
const double z = 5; const double z = 5;
vector<Point3> L ; vector<Point3> landmarks ;
L.push_back(Point3 (-1.0,-1.0, z)); landmarks.push_back(Point3 (-1.0,-1.0, z));
L.push_back(Point3 (-1.0, 1.0, z)); landmarks.push_back(Point3 (-1.0, 1.0, z));
L.push_back(Point3 ( 1.0, 1.0, z)); landmarks.push_back(Point3 ( 1.0, 1.0, z));
L.push_back(Point3 ( 1.0,-1.0, z)); landmarks.push_back(Point3 ( 1.0,-1.0, z));
L.push_back(Point3 (-1.5,-1.5, 1.5*z)); landmarks.push_back(Point3 (-1.5,-1.5, 1.5*z));
L.push_back(Point3 (-1.5, 1.5, 1.5*z)); landmarks.push_back(Point3 (-1.5, 1.5, 1.5*z));
L.push_back(Point3 ( 1.5, 1.5, 1.5*z)); landmarks.push_back(Point3 ( 1.5, 1.5, 1.5*z));
L.push_back(Point3 ( 1.5,-1.5, 1.5*z)); landmarks.push_back(Point3 ( 1.5,-1.5, 1.5*z));
L.push_back(Point3 (-2.0,-2.0, 2*z)); landmarks.push_back(Point3 (-2.0,-2.0, 2*z));
L.push_back(Point3 (-2.0, 2.0, 2*z)); landmarks.push_back(Point3 (-2.0, 2.0, 2*z));
L.push_back(Point3 ( 2.0, 2.0, 2*z)); landmarks.push_back(Point3 ( 2.0, 2.0, 2*z));
L.push_back(Point3 ( 2.0,-2.0, 2*z)); landmarks.push_back(Point3 ( 2.0,-2.0, 2*z));
return L ; return landmarks ;
} }
vector<GeneralCamera> genCameraDefaultCalibration() { vector<GeneralCamera> genCameraDefaultCalibration() {
vector<GeneralCamera> X ; vector<GeneralCamera> cameras ;
X.push_back(GeneralCamera(Pose3(eye(3),Point3(-baseline/2.0, 0.0, 0.0)))); cameras.push_back(GeneralCamera(Pose3(eye(3),Point3(-baseline/2.0, 0.0, 0.0))));
X.push_back(GeneralCamera(Pose3(eye(3),Point3( baseline/2.0, 0.0, 0.0)))); cameras.push_back(GeneralCamera(Pose3(eye(3),Point3( baseline/2.0, 0.0, 0.0))));
return X ; return cameras ;
} }
vector<GeneralCamera> genCameraVariableCalibration() { vector<GeneralCamera> genCameraVariableCalibration() {
const Cal3Bundler K(500,1e-3,1e-3); const Cal3Bundler K(500,1e-3,1e-3);
vector<GeneralCamera> X ; vector<GeneralCamera> cameras ;
X.push_back(GeneralCamera(Pose3(eye(3),Point3(-baseline/2.0, 0.0, 0.0)), K)); cameras.push_back(GeneralCamera(Pose3(eye(3),Point3(-baseline/2.0, 0.0, 0.0)), K));
X.push_back(GeneralCamera(Pose3(eye(3),Point3( baseline/2.0, 0.0, 0.0)), K)); cameras.push_back(GeneralCamera(Pose3(eye(3),Point3( baseline/2.0, 0.0, 0.0)), K));
return X ; return cameras ;
} }
boost::shared_ptr<Ordering> getOrdering(const std::vector<GeneralCamera>& X, const std::vector<Point3>& L) { boost::shared_ptr<Ordering> getOrdering(const std::vector<GeneralCamera>& cameras, const std::vector<Point3>& landmarks) {
boost::shared_ptr<Ordering> ordering(new Ordering); boost::shared_ptr<Ordering> ordering(new Ordering);
for ( size_t i = 0 ; i < L.size() ; ++i ) ordering->push_back(Symbol('l', i)) ; for ( size_t i = 0 ; i < landmarks.size() ; ++i ) ordering->push_back(L(i)) ;
for ( size_t i = 0 ; i < X.size() ; ++i ) ordering->push_back(Symbol('x', i)) ; for ( size_t i = 0 ; i < cameras.size() ; ++i ) ordering->push_back(X(i)) ;
return ordering ; return ordering ;
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( GeneralSFMFactor, optimize_defaultK ) { TEST( GeneralSFMFactor, optimize_defaultK ) {
vector<Point3> L = genPoint3(); vector<Point3> landmarks = genPoint3();
vector<GeneralCamera> X = genCameraDefaultCalibration(); vector<GeneralCamera> cameras = genCameraDefaultCalibration();
// add measurement with noise // add measurement with noise
Graph graph; Graph graph;
for ( size_t j = 0 ; j < X.size() ; ++j) { for ( size_t j = 0 ; j < cameras.size() ; ++j) {
for ( size_t i = 0 ; i < L.size() ; ++i) { for ( size_t i = 0 ; i < landmarks.size() ; ++i) {
Point2 pt = X[j].project(L[i]) ; Point2 pt = cameras[j].project(landmarks[i]) ;
graph.addMeasurement(j, i, pt, sigma1); graph.addMeasurement(j, i, pt, sigma1);
} }
} }
const size_t nMeasurements = X.size()*L.size() ; const size_t nMeasurements = cameras.size()*landmarks.size() ;
// add initial // add initial
const double noise = baseline*0.1; const double noise = baseline*0.1;
Values values; Values values;
for ( size_t i = 0 ; i < X.size() ; ++i ) for ( size_t i = 0 ; i < cameras.size() ; ++i )
values.insert(Symbol('x',i), X[i]) ; values.insert(X(i), cameras[i]) ;
for ( size_t i = 0 ; i < L.size() ; ++i ) { for ( size_t i = 0 ; i < landmarks.size() ; ++i ) {
Point3 pt(L[i].x()+noise*getGaussian(), Point3 pt(landmarks[i].x()+noise*getGaussian(),
L[i].y()+noise*getGaussian(), landmarks[i].y()+noise*getGaussian(),
L[i].z()+noise*getGaussian()); landmarks[i].z()+noise*getGaussian());
values.insert(Symbol('l',i), pt) ; values.insert(L(i), pt) ;
} }
graph.addCameraConstraint(0, X[0]); graph.addCameraConstraint(0, cameras[0]);
// Create an ordering of the variables // Create an ordering of the variables
Ordering ordering = *getOrdering(X,L); Ordering ordering = *getOrdering(cameras,landmarks);
LevenbergMarquardtOptimizer optimizer(graph, values, ordering); LevenbergMarquardtOptimizer optimizer(graph, values, ordering);
Values final = optimizer.optimize(); Values final = optimizer.optimize();
EXPECT(optimizer.error() < 0.5 * 1e-5 * nMeasurements); EXPECT(optimizer.error() < 0.5 * 1e-5 * nMeasurements);
@ -196,42 +200,42 @@ TEST( GeneralSFMFactor, optimize_defaultK ) {
/* ************************************************************************* */ /* ************************************************************************* */
TEST( GeneralSFMFactor, optimize_varK_SingleMeasurementError ) { TEST( GeneralSFMFactor, optimize_varK_SingleMeasurementError ) {
vector<Point3> L = genPoint3(); vector<Point3> landmarks = genPoint3();
vector<GeneralCamera> X = genCameraVariableCalibration(); vector<GeneralCamera> cameras = genCameraVariableCalibration();
// add measurement with noise // add measurement with noise
Graph graph; Graph graph;
for ( size_t j = 0 ; j < X.size() ; ++j) { for ( size_t j = 0 ; j < cameras.size() ; ++j) {
for ( size_t i = 0 ; i < L.size() ; ++i) { for ( size_t i = 0 ; i < landmarks.size() ; ++i) {
Point2 pt = X[j].project(L[i]) ; Point2 pt = cameras[j].project(landmarks[i]) ;
graph.addMeasurement(j, i, pt, sigma1); graph.addMeasurement(j, i, pt, sigma1);
} }
} }
const size_t nMeasurements = X.size()*L.size() ; const size_t nMeasurements = cameras.size()*landmarks.size() ;
// add initial // add initial
const double noise = baseline*0.1; const double noise = baseline*0.1;
Values values; Values values;
for ( size_t i = 0 ; i < X.size() ; ++i ) for ( size_t i = 0 ; i < cameras.size() ; ++i )
values.insert(Symbol('x',i), X[i]) ; values.insert(X(i), cameras[i]) ;
// add noise only to the first landmark // add noise only to the first landmark
for ( size_t i = 0 ; i < L.size() ; ++i ) { for ( size_t i = 0 ; i < landmarks.size() ; ++i ) {
if ( i == 0 ) { if ( i == 0 ) {
Point3 pt(L[i].x()+noise*getGaussian(), Point3 pt(landmarks[i].x()+noise*getGaussian(),
L[i].y()+noise*getGaussian(), landmarks[i].y()+noise*getGaussian(),
L[i].z()+noise*getGaussian()); landmarks[i].z()+noise*getGaussian());
values.insert(Symbol('l',i), pt) ; values.insert(L(i), pt) ;
} }
else { else {
values.insert(Symbol('l',i), L[i]) ; values.insert(L(i), landmarks[i]) ;
} }
} }
graph.addCameraConstraint(0, X[0]); graph.addCameraConstraint(0, cameras[0]);
const double reproj_error = 1e-5; const double reproj_error = 1e-5;
Ordering ordering = *getOrdering(X,L); Ordering ordering = *getOrdering(cameras,landmarks);
LevenbergMarquardtOptimizer optimizer(graph, values, ordering); LevenbergMarquardtOptimizer optimizer(graph, values, ordering);
Values final = optimizer.optimize(); Values final = optimizer.optimize();
EXPECT(optimizer.error() < 0.5 * reproj_error * nMeasurements); EXPECT(optimizer.error() < 0.5 * reproj_error * nMeasurements);
@ -240,39 +244,39 @@ TEST( GeneralSFMFactor, optimize_varK_SingleMeasurementError ) {
/* ************************************************************************* */ /* ************************************************************************* */
TEST( GeneralSFMFactor, optimize_varK_FixCameras ) { TEST( GeneralSFMFactor, optimize_varK_FixCameras ) {
vector<Point3> L = genPoint3(); vector<Point3> landmarks = genPoint3();
vector<GeneralCamera> X = genCameraVariableCalibration(); vector<GeneralCamera> cameras = genCameraVariableCalibration();
// add measurement with noise // add measurement with noise
const double noise = baseline*0.1; const double noise = baseline*0.1;
Graph graph; Graph graph;
for ( size_t j = 0 ; j < X.size() ; ++j) { for ( size_t j = 0 ; j < cameras.size() ; ++j) {
for ( size_t i = 0 ; i < L.size() ; ++i) { for ( size_t i = 0 ; i < landmarks.size() ; ++i) {
Point2 pt = X[j].project(L[i]) ; Point2 pt = cameras[j].project(landmarks[i]) ;
graph.addMeasurement(j, i, pt, sigma1); graph.addMeasurement(j, i, pt, sigma1);
} }
} }
const size_t nMeasurements = L.size()*X.size(); const size_t nMeasurements = landmarks.size()*cameras.size();
Values values; Values values;
for ( size_t i = 0 ; i < X.size() ; ++i ) for ( size_t i = 0 ; i < cameras.size() ; ++i )
values.insert(Symbol('x',i), X[i]) ; values.insert(X(i), cameras[i]) ;
for ( size_t i = 0 ; i < L.size() ; ++i ) { for ( size_t i = 0 ; i < landmarks.size() ; ++i ) {
Point3 pt(L[i].x()+noise*getGaussian(), Point3 pt(landmarks[i].x()+noise*getGaussian(),
L[i].y()+noise*getGaussian(), landmarks[i].y()+noise*getGaussian(),
L[i].z()+noise*getGaussian()); landmarks[i].z()+noise*getGaussian());
//Point3 pt(L[i].x(), L[i].y(), L[i].z()); //Point3 pt(landmarks[i].x(), landmarks[i].y(), landmarks[i].z());
values.insert(Symbol('l',i), pt) ; values.insert(L(i), pt) ;
} }
for ( size_t i = 0 ; i < X.size() ; ++i ) for ( size_t i = 0 ; i < cameras.size() ; ++i )
graph.addCameraConstraint(i, X[i]); graph.addCameraConstraint(i, cameras[i]);
const double reproj_error = 1e-5 ; const double reproj_error = 1e-5 ;
Ordering ordering = *getOrdering(X,L); Ordering ordering = *getOrdering(cameras,landmarks);
LevenbergMarquardtOptimizer optimizer(graph, values, ordering); LevenbergMarquardtOptimizer optimizer(graph, values, ordering);
Values final = optimizer.optimize(); Values final = optimizer.optimize();
EXPECT(optimizer.error() < 0.5 * reproj_error * nMeasurements); EXPECT(optimizer.error() < 0.5 * reproj_error * nMeasurements);
@ -281,27 +285,27 @@ TEST( GeneralSFMFactor, optimize_varK_FixCameras ) {
/* ************************************************************************* */ /* ************************************************************************* */
TEST( GeneralSFMFactor, optimize_varK_FixLandmarks ) { TEST( GeneralSFMFactor, optimize_varK_FixLandmarks ) {
vector<Point3> L = genPoint3(); vector<Point3> landmarks = genPoint3();
vector<GeneralCamera> X = genCameraVariableCalibration(); vector<GeneralCamera> cameras = genCameraVariableCalibration();
// add measurement with noise // add measurement with noise
Graph graph; Graph graph;
for ( size_t j = 0 ; j < X.size() ; ++j) { for ( size_t j = 0 ; j < cameras.size() ; ++j) {
for ( size_t i = 0 ; i < L.size() ; ++i) { for ( size_t i = 0 ; i < landmarks.size() ; ++i) {
Point2 pt = X[j].project(L[i]) ; Point2 pt = cameras[j].project(landmarks[i]) ;
graph.addMeasurement(j, i, pt, sigma1); graph.addMeasurement(j, i, pt, sigma1);
} }
} }
const size_t nMeasurements = L.size()*X.size(); const size_t nMeasurements = landmarks.size()*cameras.size();
Values values; Values values;
for ( size_t i = 0 ; i < X.size() ; ++i ) { for ( size_t i = 0 ; i < cameras.size() ; ++i ) {
const double const double
rot_noise = 1e-5, trans_noise = 1e-3, rot_noise = 1e-5, trans_noise = 1e-3,
focal_noise = 1, distort_noise = 1e-3; focal_noise = 1, distort_noise = 1e-3;
if ( i == 0 ) { if ( i == 0 ) {
values.insert(Symbol('x',i), X[i]) ; values.insert(X(i), cameras[i]) ;
} }
else { else {
@ -310,22 +314,22 @@ TEST( GeneralSFMFactor, optimize_varK_FixLandmarks ) {
trans_noise, trans_noise, trans_noise, // translation trans_noise, trans_noise, trans_noise, // translation
focal_noise, distort_noise, distort_noise // f, k1, k2 focal_noise, distort_noise, distort_noise // f, k1, k2
) ; ) ;
values.insert(Symbol('x',i), X[i].retract(delta)) ; values.insert(X(i), cameras[i].retract(delta)) ;
} }
} }
for ( size_t i = 0 ; i < L.size() ; ++i ) { for ( size_t i = 0 ; i < landmarks.size() ; ++i ) {
values.insert(Symbol('l',i), L[i]) ; values.insert(L(i), landmarks[i]) ;
} }
// fix X0 and all landmarks, allow only the X[1] to move // fix X0 and all landmarks, allow only the cameras[1] to move
graph.addCameraConstraint(0, X[0]); graph.addCameraConstraint(0, cameras[0]);
for ( size_t i = 0 ; i < L.size() ; ++i ) for ( size_t i = 0 ; i < landmarks.size() ; ++i )
graph.addPoint3Constraint(i, L[i]); graph.addPoint3Constraint(i, landmarks[i]);
const double reproj_error = 1e-5 ; const double reproj_error = 1e-5 ;
Ordering ordering = *getOrdering(X,L); Ordering ordering = *getOrdering(cameras,landmarks);
LevenbergMarquardtOptimizer optimizer(graph, values, ordering); LevenbergMarquardtOptimizer optimizer(graph, values, ordering);
Values final = optimizer.optimize(); Values final = optimizer.optimize();
EXPECT(optimizer.error() < 0.5 * reproj_error * nMeasurements); EXPECT(optimizer.error() < 0.5 * reproj_error * nMeasurements);
@ -333,43 +337,43 @@ TEST( GeneralSFMFactor, optimize_varK_FixLandmarks ) {
/* ************************************************************************* */ /* ************************************************************************* */
TEST( GeneralSFMFactor, optimize_varK_BA ) { TEST( GeneralSFMFactor, optimize_varK_BA ) {
vector<Point3> L = genPoint3(); vector<Point3> landmarks = genPoint3();
vector<GeneralCamera> X = genCameraVariableCalibration(); vector<GeneralCamera> cameras = genCameraVariableCalibration();
// add measurement with noise // add measurement with noise
Graph graph; Graph graph;
for ( size_t j = 0 ; j < X.size() ; ++j) { for ( size_t j = 0 ; j < cameras.size() ; ++j) {
for ( size_t i = 0 ; i < L.size() ; ++i) { for ( size_t i = 0 ; i < landmarks.size() ; ++i) {
Point2 pt = X[j].project(L[i]) ; Point2 pt = cameras[j].project(landmarks[i]) ;
graph.addMeasurement(j, i, pt, sigma1); graph.addMeasurement(j, i, pt, sigma1);
} }
} }
const size_t nMeasurements = X.size()*L.size() ; const size_t nMeasurements = cameras.size()*landmarks.size() ;
// add initial // add initial
const double noise = baseline*0.1; const double noise = baseline*0.1;
Values values; Values values;
for ( size_t i = 0 ; i < X.size() ; ++i ) for ( size_t i = 0 ; i < cameras.size() ; ++i )
values.insert(Symbol('x',i), X[i]) ; values.insert(X(i), cameras[i]) ;
// add noise only to the first landmark // add noise only to the first landmark
for ( size_t i = 0 ; i < L.size() ; ++i ) { for ( size_t i = 0 ; i < landmarks.size() ; ++i ) {
Point3 pt(L[i].x()+noise*getGaussian(), Point3 pt(landmarks[i].x()+noise*getGaussian(),
L[i].y()+noise*getGaussian(), landmarks[i].y()+noise*getGaussian(),
L[i].z()+noise*getGaussian()); landmarks[i].z()+noise*getGaussian());
values.insert(Symbol('l',i), pt) ; values.insert(L(i), pt) ;
} }
// Constrain position of system with the first camera constrained to the origin // Constrain position of system with the first camera constrained to the origin
graph.addCameraConstraint(0, X[0]); graph.addCameraConstraint(X(0), cameras[0]);
// Constrain the scale of the problem with a soft range factor of 1m between the cameras // Constrain the scale of the problem with a soft range factor of 1m between the cameras
graph.add(RangeFactor<GeneralCamera,GeneralCamera>(Symbol('x',0), Symbol('x',1), 2.0, sharedSigma(1, 10.0))); graph.add(RangeFactor<GeneralCamera,GeneralCamera>(X(0), X(1), 2.0, sharedSigma(1, 10.0)));
const double reproj_error = 1e-5 ; const double reproj_error = 1e-5 ;
Ordering ordering = *getOrdering(X,L); Ordering ordering = *getOrdering(cameras,landmarks);
LevenbergMarquardtOptimizer optimizer(graph, values, ordering); LevenbergMarquardtOptimizer optimizer(graph, values, ordering);
Values final = optimizer.optimize(); Values final = optimizer.optimize();
EXPECT(optimizer.error() < 0.5 * reproj_error * nMeasurements); EXPECT(optimizer.error() < 0.5 * reproj_error * nMeasurements);

26
gtsam/slam/tests/testProjectionFactor.cpp
View File

@ -37,8 +37,8 @@ static SharedNoiseModel sigma(noiseModel::Unit::Create(1));
static shared_ptrK sK(new Cal3_S2(K)); static shared_ptrK sK(new Cal3_S2(K));
// Convenience for named keys // Convenience for named keys
Key kx(size_t i) { return Symbol('x',i); } using symbol_shorthand::X;
Key kl(size_t i) { return Symbol('l',i); } using symbol_shorthand::L;
/* ************************************************************************* */ /* ************************************************************************* */
TEST( ProjectionFactor, error ) TEST( ProjectionFactor, error )
@ -47,12 +47,12 @@ TEST( ProjectionFactor, error )
Point2 z(323.,240.); Point2 z(323.,240.);
int i=1, j=1; int i=1, j=1;
boost::shared_ptr<visualSLAM::ProjectionFactor> boost::shared_ptr<visualSLAM::ProjectionFactor>
factor(new visualSLAM::ProjectionFactor(z, sigma, kx(i), kl(j), sK)); factor(new visualSLAM::ProjectionFactor(z, sigma, X(i), L(j), sK));
// For the following values structure, the factor predicts 320,240 // For the following values structure, the factor predicts 320,240
Values config; Values config;
Rot3 R;Point3 t1(0,0,-6); Pose3 x1(R,t1); config.insert(kx(1), x1); Rot3 R;Point3 t1(0,0,-6); Pose3 x1(R,t1); config.insert(X(1), x1);
Point3 l1; config.insert(kl(1), l1); Point3 l1; config.insert(L(1), l1);
// Point should project to Point2(320.,240.) // Point should project to Point2(320.,240.)
CHECK(assert_equal(Vector_(2, -3.0, 0.0), factor->unwhitenedError(config))); CHECK(assert_equal(Vector_(2, -3.0, 0.0), factor->unwhitenedError(config)));
@ -60,12 +60,12 @@ TEST( ProjectionFactor, error )
DOUBLES_EQUAL(4.5,factor->error(config),1e-9); DOUBLES_EQUAL(4.5,factor->error(config),1e-9);
// Check linearize // Check linearize
Ordering ordering; ordering += kx(1),kl(1); Ordering ordering; ordering += X(1),L(1);
Matrix Ax1 = Matrix_(2, 6, 0., -554.256, 0., -92.376, 0., 0., 554.256, 0., 0., 0., -92.376, 0.); Matrix Ax1 = Matrix_(2, 6, 0., -554.256, 0., -92.376, 0., 0., 554.256, 0., 0., 0., -92.376, 0.);
Matrix Al1 = Matrix_(2, 3, 92.376, 0., 0., 0., 92.376, 0.); Matrix Al1 = Matrix_(2, 3, 92.376, 0., 0., 0., 92.376, 0.);
Vector b = Vector_(2,3.,0.); Vector b = Vector_(2,3.,0.);
SharedDiagonal probModel1 = noiseModel::Unit::Create(2); SharedDiagonal probModel1 = noiseModel::Unit::Create(2);
JacobianFactor expected(ordering[kx(1)], Ax1, ordering[kl(1)], Al1, b, probModel1); JacobianFactor expected(ordering[X(1)], Ax1, ordering[L(1)], Al1, b, probModel1);
JacobianFactor::shared_ptr actual = JacobianFactor::shared_ptr actual =
boost::dynamic_pointer_cast<JacobianFactor>(factor->linearize(config, ordering)); boost::dynamic_pointer_cast<JacobianFactor>(factor->linearize(config, ordering));
CHECK(assert_equal(expected,*actual,1e-3)); CHECK(assert_equal(expected,*actual,1e-3));
@ -80,11 +80,11 @@ TEST( ProjectionFactor, error )
// expmap on a config // expmap on a config
Values expected_config; Values expected_config;
Point3 t2(1,1,-5); Pose3 x2(R,t2); expected_config.insert(kx(1), x2); Point3 t2(1,1,-5); Pose3 x2(R,t2); expected_config.insert(X(1), x2);
Point3 l2(1,2,3); expected_config.insert(kl(1), l2); Point3 l2(1,2,3); expected_config.insert(L(1), l2);
VectorValues delta(expected_config.dims(ordering)); VectorValues delta(expected_config.dims(ordering));
delta[ordering[kx(1)]] = Vector_(6, 0.,0.,0., 1.,1.,1.); delta[ordering[X(1)]] = Vector_(6, 0.,0.,0., 1.,1.,1.);
delta[ordering[kl(1)]] = Vector_(3, 1.,2.,3.); delta[ordering[L(1)]] = Vector_(3, 1.,2.,3.);
Values actual_config = config.retract(delta, ordering); Values actual_config = config.retract(delta, ordering);
CHECK(assert_equal(expected_config,actual_config,1e-9)); CHECK(assert_equal(expected_config,actual_config,1e-9));
} }
@ -96,10 +96,10 @@ TEST( ProjectionFactor, equals )
Vector z = Vector_(2,323.,240.); Vector z = Vector_(2,323.,240.);
int i=1, j=1; int i=1, j=1;
boost::shared_ptr<visualSLAM::ProjectionFactor> boost::shared_ptr<visualSLAM::ProjectionFactor>
factor1(new visualSLAM::ProjectionFactor(z, sigma, kx(i), kl(j), sK)); factor1(new visualSLAM::ProjectionFactor(z, sigma, X(i), L(j), sK));
boost::shared_ptr<visualSLAM::ProjectionFactor> boost::shared_ptr<visualSLAM::ProjectionFactor>
factor2(new visualSLAM::ProjectionFactor(z, sigma, kx(i), kl(j), sK)); factor2(new visualSLAM::ProjectionFactor(z, sigma, X(i), L(j), sK));
CHECK(assert_equal(*factor1, *factor2)); CHECK(assert_equal(*factor1, *factor2));
} }

6
gtsam/slam/tests/testSerializationSLAM.cpp
View File

@ -29,6 +29,10 @@ using namespace std;
using namespace gtsam; using namespace gtsam;
using namespace gtsam::serializationTestHelpers; using namespace gtsam::serializationTestHelpers;
// Convenience for named keys
using symbol_shorthand::X;
using symbol_shorthand::L;
/* Create GUIDs for factors */ /* Create GUIDs for factors */
/* ************************************************************************* */ /* ************************************************************************* */
BOOST_CLASS_EXPORT_GUID(gtsam::JacobianFactor, "gtsam::JacobianFactor"); BOOST_CLASS_EXPORT_GUID(gtsam::JacobianFactor, "gtsam::JacobianFactor");
@ -49,7 +53,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('x',1),Symbol('x',2),Symbol('l',1); Ordering ordering; ordering += X(1),X(2),L(1);
GaussianFactorGraph fg = createGaussianFactorGraph(ordering); GaussianFactorGraph fg = createGaussianFactorGraph(ordering);
EXPECT(equalsObj(ordering)); EXPECT(equalsObj(ordering));
EXPECT(equalsXML(ordering)); EXPECT(equalsXML(ordering));

10
gtsam/slam/tests/testSimulated2DOriented.cpp
View File

@ -29,8 +29,8 @@ using namespace gtsam;
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
// Convenience for named keys // Convenience for named keys
Key kx(size_t i) { return Symbol('x',i); } using symbol_shorthand::X;
Key kl(size_t i) { return Symbol('l',i); } using symbol_shorthand::L;
/* ************************************************************************* */ /* ************************************************************************* */
TEST( simulated2DOriented, Dprior ) TEST( simulated2DOriented, Dprior )
@ -59,11 +59,11 @@ TEST( simulated2DOriented, constructor )
{ {
Pose2 measurement(0.2, 0.3, 0.1); Pose2 measurement(0.2, 0.3, 0.1);
SharedDiagonal model(Vector_(3, 1., 1., 1.)); SharedDiagonal model(Vector_(3, 1., 1., 1.));
simulated2DOriented::Odometry factor(measurement, model, kx(1), kx(2)); simulated2DOriented::Odometry factor(measurement, model, X(1), X(2));
simulated2DOriented::Values config; simulated2DOriented::Values config;
config.insert(kx(1), Pose2(1., 0., 0.2)); config.insert(X(1), Pose2(1., 0., 0.2));
config.insert(kx(2), Pose2(2., 0., 0.1)); config.insert(X(2), Pose2(2., 0., 0.1));
boost::shared_ptr<GaussianFactor> lf = factor.linearize(config, *config.orderingArbitrary()); boost::shared_ptr<GaussianFactor> lf = factor.linearize(config, *config.orderingArbitrary());
} }

12
gtsam/slam/tests/testStereoFactor.cpp
View File

@ -43,8 +43,8 @@ Point3 p(0, 0, 5);
static SharedNoiseModel sigma(noiseModel::Unit::Create(1)); static SharedNoiseModel sigma(noiseModel::Unit::Create(1));
// Convenience for named keys // Convenience for named keys
Key kx(size_t i) { return Symbol('x',i); } using symbol_shorthand::X;
Key kl(size_t i) { return Symbol('l',i); } using symbol_shorthand::L;
/* ************************************************************************* */ /* ************************************************************************* */
TEST( StereoFactor, singlePoint) TEST( StereoFactor, singlePoint)
@ -53,18 +53,18 @@ TEST( StereoFactor, singlePoint)
boost::shared_ptr<Cal3_S2Stereo> K(new Cal3_S2Stereo(625, 625, 0, 320, 240, 0.5)); boost::shared_ptr<Cal3_S2Stereo> K(new Cal3_S2Stereo(625, 625, 0, 320, 240, 0.5));
NonlinearFactorGraph graph; NonlinearFactorGraph graph;
graph.add(visualSLAM::PoseConstraint(kx(1),camera1)); graph.add(visualSLAM::PoseConstraint(X(1),camera1));
StereoPoint2 z14(320,320.0-50, 240); StereoPoint2 z14(320,320.0-50, 240);
// arguments: measurement, sigma, cam#, measurement #, K, baseline (m) // arguments: measurement, sigma, cam#, measurement #, K, baseline (m)
graph.add(visualSLAM::StereoFactor(z14,sigma, kx(1), kl(1), K)); graph.add(visualSLAM::StereoFactor(z14,sigma, X(1), L(1), K));
// Create a configuration corresponding to the ground truth // Create a configuration corresponding to the ground truth
Values values; Values values;
values.insert(kx(1), camera1); // add camera at z=6.25m looking towards origin values.insert(X(1), camera1); // add camera at z=6.25m looking towards origin
Point3 l1(0, 0, 0); Point3 l1(0, 0, 0);
values.insert(kl(1), l1); // add point at origin; values.insert(L(1), l1); // add point at origin;
GaussNewtonOptimizer optimizer(graph, values); GaussNewtonOptimizer optimizer(graph, values);

92
gtsam/slam/tests/testVisualSLAM.cpp
View File

@ -34,8 +34,8 @@ using namespace gtsam;
static SharedNoiseModel sigma(noiseModel::Unit::Create(1)); static SharedNoiseModel sigma(noiseModel::Unit::Create(1));
// Convenience for named keys // Convenience for named keys
Key kx(size_t i) { return Symbol('x',i); } using symbol_shorthand::X;
Key kl(size_t i) { return Symbol('l',i); } using symbol_shorthand::L;
static Point3 landmark1(-1.0,-1.0, 0.0); static Point3 landmark1(-1.0,-1.0, 0.0);
static Point3 landmark2(-1.0, 1.0, 0.0); static Point3 landmark2(-1.0, 1.0, 0.0);
@ -69,14 +69,14 @@ visualSLAM::Graph testGraph() {
shared_ptrK sK(new Cal3_S2(625, 625, 0, 0, 0)); shared_ptrK sK(new Cal3_S2(625, 625, 0, 0, 0));
visualSLAM::Graph g; visualSLAM::Graph g;
g.addMeasurement(z11, sigma, kx(1), kl(1), sK); g.addMeasurement(z11, sigma, X(1), L(1), sK);
g.addMeasurement(z12, sigma, kx(1), kl(2), sK); g.addMeasurement(z12, sigma, X(1), L(2), sK);
g.addMeasurement(z13, sigma, kx(1), kl(3), sK); g.addMeasurement(z13, sigma, X(1), L(3), sK);
g.addMeasurement(z14, sigma, kx(1), kl(4), sK); g.addMeasurement(z14, sigma, X(1), L(4), sK);
g.addMeasurement(z21, sigma, kx(2), kl(1), sK); g.addMeasurement(z21, sigma, X(2), L(1), sK);
g.addMeasurement(z22, sigma, kx(2), kl(2), sK); g.addMeasurement(z22, sigma, X(2), L(2), sK);
g.addMeasurement(z23, sigma, kx(2), kl(3), sK); g.addMeasurement(z23, sigma, X(2), L(3), sK);
g.addMeasurement(z24, sigma, kx(2), kl(4), sK); g.addMeasurement(z24, sigma, X(2), L(4), sK);
return g; return g;
} }
@ -86,22 +86,22 @@ TEST( Graph, optimizeLM)
// build a graph // build a graph
visualSLAM::Graph graph(testGraph()); visualSLAM::Graph graph(testGraph());
// add 3 landmark constraints // add 3 landmark constraints
graph.addPointConstraint(kl(1), landmark1); graph.addPointConstraint(L(1), landmark1);
graph.addPointConstraint(kl(2), landmark2); graph.addPointConstraint(L(2), landmark2);
graph.addPointConstraint(kl(3), landmark3); graph.addPointConstraint(L(3), landmark3);
// Create an initial values structure corresponding to the ground truth // Create an initial values structure corresponding to the ground truth
Values initialEstimate; Values initialEstimate;
initialEstimate.insert(kx(1), camera1); initialEstimate.insert(X(1), camera1);
initialEstimate.insert(kx(2), camera2); initialEstimate.insert(X(2), camera2);
initialEstimate.insert(kl(1), landmark1); initialEstimate.insert(L(1), landmark1);
initialEstimate.insert(kl(2), landmark2); initialEstimate.insert(L(2), landmark2);
initialEstimate.insert(kl(3), landmark3); initialEstimate.insert(L(3), landmark3);
initialEstimate.insert(kl(4), landmark4); initialEstimate.insert(L(4), landmark4);
// Create an ordering of the variables // Create an ordering of the variables
Ordering ordering; Ordering ordering;
ordering += kl(1),kl(2),kl(3),kl(4),kx(1),kx(2); ordering += L(1),L(2),L(3),L(4),X(1),X(2);
// Create an optimizer and check its error // Create an optimizer and check its error
// We expect the initial to be zero because config is the ground truth // We expect the initial to be zero because config is the ground truth
@ -124,21 +124,21 @@ TEST( Graph, optimizeLM2)
// build a graph // build a graph
visualSLAM::Graph graph(testGraph()); visualSLAM::Graph graph(testGraph());
// add 2 camera constraints // add 2 camera constraints
graph.addPoseConstraint(kx(1), camera1); graph.addPoseConstraint(X(1), camera1);
graph.addPoseConstraint(kx(2), camera2); graph.addPoseConstraint(X(2), camera2);
// Create an initial values structure corresponding to the ground truth // Create an initial values structure corresponding to the ground truth
Values initialEstimate; Values initialEstimate;
initialEstimate.insert(kx(1), camera1); initialEstimate.insert(X(1), camera1);
initialEstimate.insert(kx(2), camera2); initialEstimate.insert(X(2), camera2);
initialEstimate.insert(kl(1), landmark1); initialEstimate.insert(L(1), landmark1);
initialEstimate.insert(kl(2), landmark2); initialEstimate.insert(L(2), landmark2);
initialEstimate.insert(kl(3), landmark3); initialEstimate.insert(L(3), landmark3);
initialEstimate.insert(kl(4), landmark4); initialEstimate.insert(L(4), landmark4);
// Create an ordering of the variables // Create an ordering of the variables
Ordering ordering; Ordering ordering;
ordering += kl(1),kl(2),kl(3),kl(4),kx(1),kx(2); ordering += L(1),L(2),L(3),L(4),X(1),X(2);
// Create an optimizer and check its error // Create an optimizer and check its error
// We expect the initial to be zero because config is the ground truth // We expect the initial to be zero because config is the ground truth
@ -161,17 +161,17 @@ TEST( Graph, CHECK_ORDERING)
// build a graph // build a graph
visualSLAM::Graph graph = testGraph(); visualSLAM::Graph graph = testGraph();
// add 2 camera constraints // add 2 camera constraints
graph.addPoseConstraint(kx(1), camera1); graph.addPoseConstraint(X(1), camera1);
graph.addPoseConstraint(kx(2), camera2); graph.addPoseConstraint(X(2), camera2);
// Create an initial values structure corresponding to the ground truth // Create an initial values structure corresponding to the ground truth
Values initialEstimate; Values initialEstimate;
initialEstimate.insert(kx(1), camera1); initialEstimate.insert(X(1), camera1);
initialEstimate.insert(kx(2), camera2); initialEstimate.insert(X(2), camera2);
initialEstimate.insert(kl(1), landmark1); initialEstimate.insert(L(1), landmark1);
initialEstimate.insert(kl(2), landmark2); initialEstimate.insert(L(2), landmark2);
initialEstimate.insert(kl(3), landmark3); initialEstimate.insert(L(3), landmark3);
initialEstimate.insert(kl(4), landmark4); initialEstimate.insert(L(4), landmark4);
// Create an optimizer and check its error // Create an optimizer and check its error
// We expect the initial to be zero because config is the ground truth // We expect the initial to be zero because config is the ground truth
@ -192,21 +192,21 @@ TEST( Values, update_with_large_delta) {
// this test ensures that if the update for delta is larger than // this test ensures that if the update for delta is larger than
// the size of the config, it only updates existing variables // the size of the config, it only updates existing variables
Values init; Values init;
init.insert(kx(1), Pose3()); init.insert(X(1), Pose3());
init.insert(kl(1), Point3(1.0, 2.0, 3.0)); init.insert(L(1), Point3(1.0, 2.0, 3.0));
Values expected; Values expected;
expected.insert(kx(1), Pose3(Rot3(), Point3(0.1, 0.1, 0.1))); expected.insert(X(1), Pose3(Rot3(), Point3(0.1, 0.1, 0.1)));
expected.insert(kl(1), Point3(1.1, 2.1, 3.1)); expected.insert(L(1), Point3(1.1, 2.1, 3.1));
Ordering largeOrdering; Ordering largeOrdering;
Values largeValues = init; Values largeValues = init;
largeValues.insert(kx(2), Pose3()); largeValues.insert(X(2), Pose3());
largeOrdering += kx(1),kl(1),kx(2); largeOrdering += X(1),L(1),X(2);
VectorValues delta(largeValues.dims(largeOrdering)); VectorValues delta(largeValues.dims(largeOrdering));
delta[largeOrdering[kx(1)]] = Vector_(6, 0.0, 0.0, 0.0, 0.1, 0.1, 0.1); delta[largeOrdering[X(1)]] = Vector_(6, 0.0, 0.0, 0.0, 0.1, 0.1, 0.1);
delta[largeOrdering[kl(1)]] = Vector_(3, 0.1, 0.1, 0.1); delta[largeOrdering[L(1)]] = Vector_(3, 0.1, 0.1, 0.1);
delta[largeOrdering[kx(2)]] = Vector_(6, 0.0, 0.0, 0.0, 100.1, 4.1, 9.1); delta[largeOrdering[X(2)]] = Vector_(6, 0.0, 0.0, 0.0, 100.1, 4.1, 9.1);
Values actual = init.retract(delta, largeOrdering); Values actual = init.retract(delta, largeOrdering);
CHECK(assert_equal(expected,actual)); CHECK(assert_equal(expected,actual));

16
gtsam_unstable/slam/tests/testSimulated3D.cpp
View File

@ -15,24 +15,28 @@
* @author Alex Cunningham * @author Alex Cunningham
**/ **/
#include <iostream>
#include <CppUnitLite/TestHarness.h>
#include <gtsam_unstable/slam/simulated3D.h> #include <gtsam_unstable/slam/simulated3D.h>
#include <gtsam/nonlinear/Symbol.h> #include <gtsam/nonlinear/Symbol.h>
#include <gtsam/geometry/Pose3.h> #include <gtsam/geometry/Pose3.h>
#include <gtsam/base/Testable.h> #include <gtsam/base/Testable.h>
#include <gtsam/base/numericalDerivative.h> #include <gtsam/base/numericalDerivative.h>
#include <CppUnitLite/TestHarness.h>
#include <iostream>
using namespace gtsam; using namespace gtsam;
using namespace simulated3D;
// Convenience for named keys
using symbol_shorthand::X;
using symbol_shorthand::L;
/* ************************************************************************* */ /* ************************************************************************* */
TEST( simulated3D, Values ) TEST( simulated3D, Values )
{ {
Values actual; Values actual;
actual.insert(Symbol('l',1),Point3(1,1,1)); actual.insert(L(1),Point3(1,1,1));
actual.insert(Symbol('x',2),Point3(2,2,2)); actual.insert(X(2),Point3(2,2,2));
EXPECT(assert_equal(actual,actual,1e-9)); EXPECT(assert_equal(actual,actual,1e-9));
} }

8
tests/testDoglegOptimizer.cpp
View File

@ -35,6 +35,10 @@ using namespace std;
using namespace gtsam; using namespace gtsam;
using boost::shared_ptr; using boost::shared_ptr;
// Convenience for named keys
using symbol_shorthand::X;
using symbol_shorthand::L;
/* ************************************************************************* */ /* ************************************************************************* */
double computeError(const GaussianBayesNet& gbn, const LieVector& values) { double computeError(const GaussianBayesNet& gbn, const LieVector& values) {
@ -389,11 +393,11 @@ TEST(DoglegOptimizer, Iterate) {
// config far from minimum // config far from minimum
Point2 x0(3,0); Point2 x0(3,0);
boost::shared_ptr<Values> config(new Values); boost::shared_ptr<Values> config(new Values);
config->insert(Symbol('x',1), x0); config->insert(X(1), x0);
// ordering // ordering
shared_ptr<Ordering> ord(new Ordering()); shared_ptr<Ordering> ord(new Ordering());
ord->push_back(Symbol('x',1)); ord->push_back(X(1));
double Delta = 1.0; double Delta = 1.0;
for(size_t it=0; it<10; ++it) { for(size_t it=0; it<10; ++it) {

8
tests/testExtendedKalmanFilter.cpp
View File

@ -25,6 +25,10 @@
using namespace gtsam; using namespace gtsam;
// Convenience for named keys
using symbol_shorthand::X;
using symbol_shorthand::L;
/* ************************************************************************* */ /* ************************************************************************* */
TEST( ExtendedKalmanFilter, linear ) { TEST( ExtendedKalmanFilter, linear ) {
@ -414,11 +418,11 @@ TEST( ExtendedKalmanFilter, nonlinear ) {
Point2 x_predict, x_update; Point2 x_predict, x_update;
for(unsigned int i = 0; i < 10; ++i){ for(unsigned int i = 0; i < 10; ++i){
// Create motion factor // Create motion factor
NonlinearMotionModel motionFactor(Symbol('x',i), Symbol('x',i+1)); NonlinearMotionModel motionFactor(X(i), X(i+1));
x_predict = ekf.predict(motionFactor); x_predict = ekf.predict(motionFactor);
// Create a measurement factor // Create a measurement factor
NonlinearMeasurementModel measurementFactor(Symbol('x',i+1), Vector_(1, z[i])); NonlinearMeasurementModel measurementFactor(X(i+1), Vector_(1, z[i]));
x_update = ekf.update(measurementFactor); x_update = ekf.update(measurementFactor);
EXPECT(assert_equal(expected_predict[i],x_predict, 1e-6)); EXPECT(assert_equal(expected_predict[i],x_predict, 1e-6));

6
tests/testGaussianFactor.cpp
View File

@ -36,11 +36,15 @@ using namespace boost::assign;
using namespace std; using namespace std;
using namespace gtsam; using namespace gtsam;
// Convenience for named keys
using symbol_shorthand::X;
using symbol_shorthand::L;
static SharedDiagonal 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('x',1), kx2 = Symbol('x',2), kl1 = Symbol('l',1); const Key kx1 = X(1), kx2 = X(2), kl1 = L(1);
/* ************************************************************************* */ /* ************************************************************************* */
TEST( GaussianFactor, linearFactor ) TEST( GaussianFactor, linearFactor )

236
tests/testGaussianFactorGraphB.cpp
View File

@ -42,13 +42,13 @@ using namespace example;
double tol=1e-5; double tol=1e-5;
Key kx(size_t i) { return Symbol('x',i); } using symbol_shorthand::X;
Key kl(size_t i) { return Symbol('l',i); } using symbol_shorthand::L;
/* ************************************************************************* */ /* ************************************************************************* */
TEST( GaussianFactorGraph, equals ) { TEST( GaussianFactorGraph, equals ) {
Ordering ordering; ordering += kx(1),kx(2),kl(1); Ordering ordering; ordering += X(1),X(2),L(1);
GaussianFactorGraph fg = createGaussianFactorGraph(ordering); GaussianFactorGraph fg = createGaussianFactorGraph(ordering);
GaussianFactorGraph fg2 = createGaussianFactorGraph(ordering); GaussianFactorGraph fg2 = createGaussianFactorGraph(ordering);
EXPECT(fg.equals(fg2)); EXPECT(fg.equals(fg2));
@ -56,7 +56,7 @@ TEST( GaussianFactorGraph, equals ) {
/* ************************************************************************* */ /* ************************************************************************* */
//TEST( GaussianFactorGraph, error ) { //TEST( GaussianFactorGraph, error ) {
// Ordering ordering; ordering += kx(1),kx(2),kl(1); // Ordering ordering; ordering += X(1),X(2),L(1);
// FactorGraph<JacobianFactor> fg = createGaussianFactorGraph(ordering); // FactorGraph<JacobianFactor> fg = createGaussianFactorGraph(ordering);
// VectorValues cfg = createZeroDelta(ordering); // VectorValues cfg = createZeroDelta(ordering);
// //
@ -74,10 +74,10 @@ TEST( GaussianFactorGraph, equals ) {
//{ //{
// GaussianFactorGraph fg = createGaussianFactorGraph(); // GaussianFactorGraph fg = createGaussianFactorGraph();
// //
// set<Symbol> separator = fg.find_separator(kx(2)); // set<Symbol> separator = fg.find_separator(X(2));
// set<Symbol> expected; // set<Symbol> expected;
// expected.insert(kx(1)); // expected.insert(X(1));
// expected.insert(kl(1)); // expected.insert(L(1));
// //
// EXPECT(separator.size()==expected.size()); // EXPECT(separator.size()==expected.size());
// set<Symbol>::iterator it1 = separator.begin(), it2 = expected.begin(); // set<Symbol>::iterator it1 = separator.begin(), it2 = expected.begin();
@ -92,7 +92,7 @@ TEST( GaussianFactorGraph, equals ) {
// GaussianFactorGraph fg = createGaussianFactorGraph(); // GaussianFactorGraph fg = createGaussianFactorGraph();
// //
// // combine all factors // // combine all factors
// GaussianFactor::shared_ptr actual = removeAndCombineFactors(fg,kx(1)); // GaussianFactor::shared_ptr actual = removeAndCombineFactors(fg,X(1));
// //
// // the expected linear factor // // the expected linear factor
// Matrix Al1 = Matrix_(6,2, // Matrix Al1 = Matrix_(6,2,
@ -132,11 +132,11 @@ TEST( GaussianFactorGraph, equals ) {
// b(5) = 1; // b(5) = 1;
// //
// vector<pair<Symbol, Matrix> > meas; // vector<pair<Symbol, Matrix> > meas;
// meas.push_back(make_pair(kl(1), Al1)); // meas.push_back(make_pair(L(1), Al1));
// meas.push_back(make_pair(kx(1), Ax1)); // meas.push_back(make_pair(X(1), Ax1));
// meas.push_back(make_pair(kx(2), Ax2)); // meas.push_back(make_pair(X(2), Ax2));
// GaussianFactor expected(meas, b, ones(6)); // GaussianFactor expected(meas, b, ones(6));
// //GaussianFactor expected(kl(1), Al1, kx(1), Ax1, kx(2), Ax2, b); // //GaussianFactor expected(L(1), Al1, X(1), Ax1, X(2), Ax2, b);
// //
// // check if the two factors are the same // // check if the two factors are the same
// EXPECT(assert_equal(expected,*actual)); // EXPECT(assert_equal(expected,*actual));
@ -149,7 +149,7 @@ TEST( GaussianFactorGraph, equals ) {
// GaussianFactorGraph fg = createGaussianFactorGraph(); // GaussianFactorGraph fg = createGaussianFactorGraph();
// //
// // combine all factors // // combine all factors
// GaussianFactor::shared_ptr actual = removeAndCombineFactors(fg,kx(2)); // GaussianFactor::shared_ptr actual = removeAndCombineFactors(fg,X(2));
// //
// // the expected linear factor // // the expected linear factor
// Matrix Al1 = Matrix_(4,2, // Matrix Al1 = Matrix_(4,2,
@ -184,9 +184,9 @@ TEST( GaussianFactorGraph, equals ) {
// b(3) = 1.5; // b(3) = 1.5;
// //
// vector<pair<Symbol, Matrix> > meas; // vector<pair<Symbol, Matrix> > meas;
// meas.push_back(make_pair(kl(1), Al1)); // meas.push_back(make_pair(L(1), Al1));
// meas.push_back(make_pair(kx(1), Ax1)); // meas.push_back(make_pair(X(1), Ax1));
// meas.push_back(make_pair(kx(2), Ax2)); // meas.push_back(make_pair(X(2), Ax2));
// GaussianFactor expected(meas, b, ones(4)); // GaussianFactor expected(meas, b, ones(4));
// //
// // check if the two factors are the same // // check if the two factors are the same
@ -196,7 +196,7 @@ TEST( GaussianFactorGraph, equals ) {
/* ************************************************************************* */ /* ************************************************************************* */
TEST( GaussianFactorGraph, eliminateOne_x1 ) TEST( GaussianFactorGraph, eliminateOne_x1 )
{ {
Ordering ordering; ordering += kx(1),kl(1),kx(2); Ordering ordering; ordering += X(1),L(1),X(2);
GaussianFactorGraph fg = createGaussianFactorGraph(ordering); GaussianFactorGraph fg = createGaussianFactorGraph(ordering);
GaussianFactorGraph::FactorizationResult result = inference::eliminateOne(fg, 0, EliminateQR); GaussianFactorGraph::FactorizationResult result = inference::eliminateOne(fg, 0, EliminateQR);
@ -204,7 +204,7 @@ TEST( GaussianFactorGraph, eliminateOne_x1 )
// create expected Conditional Gaussian // create expected Conditional Gaussian
Matrix I = 15*eye(2), R11 = I, S12 = -0.111111*I, S13 = -0.444444*I; Matrix I = 15*eye(2), R11 = I, S12 = -0.111111*I, S13 = -0.444444*I;
Vector d = Vector_(2, -0.133333, -0.0222222), sigma = ones(2); Vector d = Vector_(2, -0.133333, -0.0222222), sigma = ones(2);
GaussianConditional expected(ordering[kx(1)],15*d,R11,ordering[kl(1)],S12,ordering[kx(2)],S13,sigma); GaussianConditional expected(ordering[X(1)],15*d,R11,ordering[L(1)],S12,ordering[X(2)],S13,sigma);
EXPECT(assert_equal(expected,*result.first,tol)); EXPECT(assert_equal(expected,*result.first,tol));
} }
@ -212,7 +212,7 @@ TEST( GaussianFactorGraph, eliminateOne_x1 )
/* ************************************************************************* */ /* ************************************************************************* */
TEST( GaussianFactorGraph, eliminateOne_x2 ) TEST( GaussianFactorGraph, eliminateOne_x2 )
{ {
Ordering ordering; ordering += kx(2),kl(1),kx(1); Ordering ordering; ordering += X(2),L(1),X(1);
GaussianFactorGraph fg = createGaussianFactorGraph(ordering); GaussianFactorGraph fg = createGaussianFactorGraph(ordering);
GaussianConditional::shared_ptr actual = inference::eliminateOne(fg, 0, EliminateQR).first; GaussianConditional::shared_ptr actual = inference::eliminateOne(fg, 0, EliminateQR).first;
@ -220,7 +220,7 @@ TEST( GaussianFactorGraph, eliminateOne_x2 )
double sig = 0.0894427; double sig = 0.0894427;
Matrix I = eye(2)/sig, R11 = I, S12 = -0.2*I, S13 = -0.8*I; Matrix I = eye(2)/sig, R11 = I, S12 = -0.2*I, S13 = -0.8*I;
Vector d = Vector_(2, 0.2, -0.14)/sig, sigma = ones(2); Vector d = Vector_(2, 0.2, -0.14)/sig, sigma = ones(2);
GaussianConditional expected(ordering[kx(2)],d,R11,ordering[kl(1)],S12,ordering[kx(1)],S13,sigma); GaussianConditional expected(ordering[X(2)],d,R11,ordering[L(1)],S12,ordering[X(1)],S13,sigma);
EXPECT(assert_equal(expected,*actual,tol)); EXPECT(assert_equal(expected,*actual,tol));
} }
@ -228,7 +228,7 @@ TEST( GaussianFactorGraph, eliminateOne_x2 )
/* ************************************************************************* */ /* ************************************************************************* */
TEST( GaussianFactorGraph, eliminateOne_l1 ) TEST( GaussianFactorGraph, eliminateOne_l1 )
{ {
Ordering ordering; ordering += kl(1),kx(1),kx(2); Ordering ordering; ordering += L(1),X(1),X(2);
GaussianFactorGraph fg = createGaussianFactorGraph(ordering); GaussianFactorGraph fg = createGaussianFactorGraph(ordering);
GaussianConditional::shared_ptr actual = inference::eliminateOne(fg, 0, EliminateQR).first; GaussianConditional::shared_ptr actual = inference::eliminateOne(fg, 0, EliminateQR).first;
@ -236,7 +236,7 @@ TEST( GaussianFactorGraph, eliminateOne_l1 )
double sig = sqrt(2)/10.; double sig = sqrt(2)/10.;
Matrix I = eye(2)/sig, R11 = I, S12 = -0.5*I, S13 = -0.5*I; Matrix I = eye(2)/sig, R11 = I, S12 = -0.5*I, S13 = -0.5*I;
Vector d = Vector_(2, -0.1, 0.25)/sig, sigma = ones(2); Vector d = Vector_(2, -0.1, 0.25)/sig, sigma = ones(2);
GaussianConditional expected(ordering[kl(1)],d,R11,ordering[kx(1)],S12,ordering[kx(2)],S13,sigma); GaussianConditional expected(ordering[L(1)],d,R11,ordering[X(1)],S12,ordering[X(2)],S13,sigma);
EXPECT(assert_equal(expected,*actual,tol)); EXPECT(assert_equal(expected,*actual,tol));
} }
@ -244,16 +244,16 @@ TEST( GaussianFactorGraph, eliminateOne_l1 )
/* ************************************************************************* */ /* ************************************************************************* */
TEST( GaussianFactorGraph, eliminateOne_x1_fast ) TEST( GaussianFactorGraph, eliminateOne_x1_fast )
{ {
Ordering ordering; ordering += kx(1),kl(1),kx(2); Ordering ordering; ordering += X(1),L(1),X(2);
GaussianFactorGraph fg = createGaussianFactorGraph(ordering); GaussianFactorGraph fg = createGaussianFactorGraph(ordering);
GaussianFactorGraph::FactorizationResult result = inference::eliminateOne(fg, ordering[kx(1)], EliminateQR); GaussianFactorGraph::FactorizationResult result = inference::eliminateOne(fg, ordering[X(1)], EliminateQR);
GaussianConditional::shared_ptr conditional = result.first; GaussianConditional::shared_ptr conditional = result.first;
GaussianFactorGraph remaining = result.second; GaussianFactorGraph remaining = result.second;
// create expected Conditional Gaussian // create expected Conditional Gaussian
Matrix I = 15*eye(2), R11 = I, S12 = -0.111111*I, S13 = -0.444444*I; Matrix I = 15*eye(2), R11 = I, S12 = -0.111111*I, S13 = -0.444444*I;
Vector d = Vector_(2, -0.133333, -0.0222222), sigma = ones(2); Vector d = Vector_(2, -0.133333, -0.0222222), sigma = ones(2);
GaussianConditional expected(ordering[kx(1)],15*d,R11,ordering[kl(1)],S12,ordering[kx(2)],S13,sigma); GaussianConditional expected(ordering[X(1)],15*d,R11,ordering[L(1)],S12,ordering[X(2)],S13,sigma);
// Create expected remaining new factor // Create expected remaining new factor
JacobianFactor expectedFactor(1, Matrix_(4,2, JacobianFactor expectedFactor(1, Matrix_(4,2,
@ -275,15 +275,15 @@ TEST( GaussianFactorGraph, eliminateOne_x1_fast )
/* ************************************************************************* */ /* ************************************************************************* */
TEST( GaussianFactorGraph, eliminateOne_x2_fast ) TEST( GaussianFactorGraph, eliminateOne_x2_fast )
{ {
Ordering ordering; ordering += kx(1),kl(1),kx(2); Ordering ordering; ordering += X(1),L(1),X(2);
GaussianFactorGraph fg = createGaussianFactorGraph(ordering); GaussianFactorGraph fg = createGaussianFactorGraph(ordering);
GaussianConditional::shared_ptr actual = inference::eliminateOne(fg, ordering[kx(2)], EliminateQR).first; GaussianConditional::shared_ptr actual = inference::eliminateOne(fg, ordering[X(2)], EliminateQR).first;
// create expected Conditional Gaussian // create expected Conditional Gaussian
double sig = 0.0894427; double sig = 0.0894427;
Matrix I = eye(2)/sig, R11 = I, S12 = -0.2*I, S13 = -0.8*I; Matrix I = eye(2)/sig, R11 = I, S12 = -0.2*I, S13 = -0.8*I;
Vector d = Vector_(2, 0.2, -0.14)/sig, sigma = ones(2); Vector d = Vector_(2, 0.2, -0.14)/sig, sigma = ones(2);
GaussianConditional expected(ordering[kx(2)],d,R11,ordering[kx(1)],S13,ordering[kl(1)],S12,sigma); GaussianConditional expected(ordering[X(2)],d,R11,ordering[X(1)],S13,ordering[L(1)],S12,sigma);
EXPECT(assert_equal(expected,*actual,tol)); EXPECT(assert_equal(expected,*actual,tol));
} }
@ -291,15 +291,15 @@ TEST( GaussianFactorGraph, eliminateOne_x2_fast )
/* ************************************************************************* */ /* ************************************************************************* */
TEST( GaussianFactorGraph, eliminateOne_l1_fast ) TEST( GaussianFactorGraph, eliminateOne_l1_fast )
{ {
Ordering ordering; ordering += kx(1),kl(1),kx(2); Ordering ordering; ordering += X(1),L(1),X(2);
GaussianFactorGraph fg = createGaussianFactorGraph(ordering); GaussianFactorGraph fg = createGaussianFactorGraph(ordering);
GaussianConditional::shared_ptr actual = inference::eliminateOne(fg, ordering[kl(1)], EliminateQR).first; GaussianConditional::shared_ptr actual = inference::eliminateOne(fg, ordering[L(1)], EliminateQR).first;
// create expected Conditional Gaussian // create expected Conditional Gaussian
double sig = sqrt(2)/10.; double sig = sqrt(2)/10.;
Matrix I = eye(2)/sig, R11 = I, S12 = -0.5*I, S13 = -0.5*I; Matrix I = eye(2)/sig, R11 = I, S12 = -0.5*I, S13 = -0.5*I;
Vector d = Vector_(2, -0.1, 0.25)/sig, sigma = ones(2); Vector d = Vector_(2, -0.1, 0.25)/sig, sigma = ones(2);
GaussianConditional expected(ordering[kl(1)],d,R11,ordering[kx(1)],S12,ordering[kx(2)],S13,sigma); GaussianConditional expected(ordering[L(1)],d,R11,ordering[X(1)],S12,ordering[X(2)],S13,sigma);
EXPECT(assert_equal(expected,*actual,tol)); EXPECT(assert_equal(expected,*actual,tol));
} }
@ -311,18 +311,18 @@ TEST( GaussianFactorGraph, eliminateAll )
Matrix I = eye(2); Matrix I = eye(2);
Ordering ordering; Ordering ordering;
ordering += kx(2),kl(1),kx(1); ordering += X(2),L(1),X(1);
Vector d1 = Vector_(2, -0.1,-0.1); Vector d1 = Vector_(2, -0.1,-0.1);
GaussianBayesNet expected = simpleGaussian(ordering[kx(1)],d1,0.1); GaussianBayesNet expected = simpleGaussian(ordering[X(1)],d1,0.1);
double sig1 = 0.149071; double sig1 = 0.149071;
Vector d2 = Vector_(2, 0.0, 0.2)/sig1, sigma2 = ones(2); Vector d2 = Vector_(2, 0.0, 0.2)/sig1, sigma2 = ones(2);
push_front(expected,ordering[kl(1)],d2, I/sig1,ordering[kx(1)], (-1)*I/sig1,sigma2); push_front(expected,ordering[L(1)],d2, I/sig1,ordering[X(1)], (-1)*I/sig1,sigma2);
double sig2 = 0.0894427; double sig2 = 0.0894427;
Vector d3 = Vector_(2, 0.2, -0.14)/sig2, sigma3 = ones(2); Vector d3 = Vector_(2, 0.2, -0.14)/sig2, sigma3 = ones(2);
push_front(expected,ordering[kx(2)],d3, I/sig2,ordering[kl(1)], (-0.2)*I/sig2, ordering[kx(1)], (-0.8)*I/sig2, sigma3); push_front(expected,ordering[X(2)],d3, I/sig2,ordering[L(1)], (-0.2)*I/sig2, ordering[X(1)], (-0.8)*I/sig2, sigma3);
// Check one ordering // Check one ordering
GaussianFactorGraph fg1 = createGaussianFactorGraph(ordering); GaussianFactorGraph fg1 = createGaussianFactorGraph(ordering);
@ -340,20 +340,20 @@ TEST( GaussianFactorGraph, eliminateAll )
// Matrix I = eye(2); // Matrix I = eye(2);
// //
// Vector d1 = Vector_(2, -0.1,-0.1); // Vector d1 = Vector_(2, -0.1,-0.1);
// GaussianBayesNet expected = simpleGaussian(kx(1),d1,0.1); // GaussianBayesNet expected = simpleGaussian(X(1),d1,0.1);
// //
// double sig1 = 0.149071; // double sig1 = 0.149071;
// Vector d2 = Vector_(2, 0.0, 0.2)/sig1, sigma2 = ones(2); // Vector d2 = Vector_(2, 0.0, 0.2)/sig1, sigma2 = ones(2);
// push_front(expected,kl(1),d2, I/sig1,kx(1), (-1)*I/sig1,sigma2); // push_front(expected,L(1),d2, I/sig1,X(1), (-1)*I/sig1,sigma2);
// //
// double sig2 = 0.0894427; // double sig2 = 0.0894427;
// Vector d3 = Vector_(2, 0.2, -0.14)/sig2, sigma3 = ones(2); // Vector d3 = Vector_(2, 0.2, -0.14)/sig2, sigma3 = ones(2);
// push_front(expected,kx(2),d3, I/sig2,kl(1), (-0.2)*I/sig2, kx(1), (-0.8)*I/sig2, sigma3); // push_front(expected,X(2),d3, I/sig2,L(1), (-0.2)*I/sig2, X(1), (-0.8)*I/sig2, sigma3);
// //
// // Check one ordering // // Check one ordering
// GaussianFactorGraph fg1 = createGaussianFactorGraph(); // GaussianFactorGraph fg1 = createGaussianFactorGraph();
// Ordering ordering; // Ordering ordering;
// ordering += kx(2),kl(1),kx(1); // ordering += X(2),L(1),X(1);
// GaussianBayesNet actual = fg1.eliminate(ordering, false); // GaussianBayesNet actual = fg1.eliminate(ordering, false);
// EXPECT(assert_equal(expected,actual,tol)); // EXPECT(assert_equal(expected,actual,tol));
//} //}
@ -361,16 +361,16 @@ TEST( GaussianFactorGraph, eliminateAll )
///* ************************************************************************* */ ///* ************************************************************************* */
//TEST( GaussianFactorGraph, add_priors ) //TEST( GaussianFactorGraph, add_priors )
//{ //{
// Ordering ordering; ordering += kl(1),kx(1),kx(2); // Ordering ordering; ordering += L(1),X(1),X(2);
// GaussianFactorGraph fg = createGaussianFactorGraph(ordering); // GaussianFactorGraph fg = createGaussianFactorGraph(ordering);
// GaussianFactorGraph actual = fg.add_priors(3, vector<size_t>(3,2)); // GaussianFactorGraph actual = fg.add_priors(3, vector<size_t>(3,2));
// GaussianFactorGraph expected = createGaussianFactorGraph(ordering); // GaussianFactorGraph expected = createGaussianFactorGraph(ordering);
// Matrix A = eye(2); // Matrix A = eye(2);
// Vector b = zero(2); // Vector b = zero(2);
// SharedDiagonal sigma = sharedSigma(2,3.0); // SharedDiagonal sigma = sharedSigma(2,3.0);
// expected.push_back(GaussianFactor::shared_ptr(new JacobianFactor(ordering[kl(1)],A,b,sigma))); // expected.push_back(GaussianFactor::shared_ptr(new JacobianFactor(ordering[L(1)],A,b,sigma)));
// expected.push_back(GaussianFactor::shared_ptr(new JacobianFactor(ordering[kx(1)],A,b,sigma))); // expected.push_back(GaussianFactor::shared_ptr(new JacobianFactor(ordering[X(1)],A,b,sigma)));
// expected.push_back(GaussianFactor::shared_ptr(new JacobianFactor(ordering[kx(2)],A,b,sigma))); // expected.push_back(GaussianFactor::shared_ptr(new JacobianFactor(ordering[X(2)],A,b,sigma)));
// EXPECT(assert_equal(expected,actual)); // EXPECT(assert_equal(expected,actual));
//} //}
@ -378,7 +378,7 @@ TEST( GaussianFactorGraph, eliminateAll )
TEST( GaussianFactorGraph, copying ) TEST( GaussianFactorGraph, copying )
{ {
// Create a graph // Create a graph
Ordering ordering; ordering += kx(2),kl(1),kx(1); Ordering ordering; ordering += X(2),L(1),X(1);
GaussianFactorGraph actual = createGaussianFactorGraph(ordering); GaussianFactorGraph actual = createGaussianFactorGraph(ordering);
// Copy the graph ! // Copy the graph !
@ -399,7 +399,7 @@ TEST( GaussianFactorGraph, copying )
//{ //{
// // render with a given ordering // // render with a given ordering
// Ordering ord; // Ordering ord;
// ord += kx(2),kl(1),kx(1); // ord += X(2),L(1),X(1);
// //
// // Create a graph // // Create a graph
// GaussianFactorGraph fg = createGaussianFactorGraph(ordering); // GaussianFactorGraph fg = createGaussianFactorGraph(ordering);
@ -438,7 +438,7 @@ TEST( GaussianFactorGraph, copying )
TEST( GaussianFactorGraph, CONSTRUCTOR_GaussianBayesNet ) TEST( GaussianFactorGraph, CONSTRUCTOR_GaussianBayesNet )
{ {
Ordering ord; Ordering ord;
ord += kx(2),kl(1),kx(1); ord += X(2),L(1),X(1);
GaussianFactorGraph fg = createGaussianFactorGraph(ord); GaussianFactorGraph fg = createGaussianFactorGraph(ord);
// render with a given ordering // render with a given ordering
@ -458,20 +458,20 @@ TEST( GaussianFactorGraph, CONSTRUCTOR_GaussianBayesNet )
/* ************************************************************************* */ /* ************************************************************************* */
TEST( GaussianFactorGraph, getOrdering) TEST( GaussianFactorGraph, getOrdering)
{ {
Ordering original; original += kl(1),kx(1),kx(2); Ordering original; original += L(1),X(1),X(2);
FactorGraph<IndexFactor> symbolic(createGaussianFactorGraph(original)); FactorGraph<IndexFactor> symbolic(createGaussianFactorGraph(original));
Permutation perm(*inference::PermutationCOLAMD(VariableIndex(symbolic))); Permutation perm(*inference::PermutationCOLAMD(VariableIndex(symbolic)));
Ordering actual = original; actual.permuteWithInverse((*perm.inverse())); Ordering actual = original; actual.permuteWithInverse((*perm.inverse()));
Ordering expected; expected += kl(1),kx(2),kx(1); Ordering expected; expected += L(1),X(2),X(1);
EXPECT(assert_equal(expected,actual)); EXPECT(assert_equal(expected,actual));
} }
// SL-FIX TEST( GaussianFactorGraph, getOrdering2) // SL-FIX TEST( GaussianFactorGraph, getOrdering2)
//{ //{
// Ordering expected; // Ordering expected;
// expected += kl(1),kx(1); // expected += L(1),X(1);
// GaussianFactorGraph fg = createGaussianFactorGraph(); // GaussianFactorGraph fg = createGaussianFactorGraph();
// set<Symbol> interested; interested += kl(1),kx(1); // set<Symbol> interested; interested += L(1),X(1);
// Ordering actual = fg.getOrdering(interested); // Ordering actual = fg.getOrdering(interested);
// EXPECT(assert_equal(expected,actual)); // EXPECT(assert_equal(expected,actual));
//} //}
@ -480,7 +480,7 @@ TEST( GaussianFactorGraph, getOrdering)
TEST( GaussianFactorGraph, optimize_Cholesky ) TEST( GaussianFactorGraph, optimize_Cholesky )
{ {
// create an ordering // create an ordering
Ordering ord; ord += kx(2),kl(1),kx(1); Ordering ord; ord += X(2),L(1),X(1);
// create a graph // create a graph
GaussianFactorGraph fg = createGaussianFactorGraph(ord); GaussianFactorGraph fg = createGaussianFactorGraph(ord);
@ -498,7 +498,7 @@ TEST( GaussianFactorGraph, optimize_Cholesky )
TEST( GaussianFactorGraph, optimize_QR ) TEST( GaussianFactorGraph, optimize_QR )
{ {
// create an ordering // create an ordering
Ordering ord; ord += kx(2),kl(1),kx(1); Ordering ord; ord += X(2),L(1),X(1);
// create a graph // create a graph
GaussianFactorGraph fg = createGaussianFactorGraph(ord); GaussianFactorGraph fg = createGaussianFactorGraph(ord);
@ -516,7 +516,7 @@ TEST( GaussianFactorGraph, optimize_QR )
// SL-FIX TEST( GaussianFactorGraph, optimizeMultiFrontlas ) // SL-FIX TEST( GaussianFactorGraph, optimizeMultiFrontlas )
//{ //{
// // create an ordering // // create an ordering
// Ordering ord; ord += kx(2),kl(1),kx(1); // Ordering ord; ord += X(2),L(1),X(1);
// //
// // create a graph // // create a graph
// GaussianFactorGraph fg = createGaussianFactorGraph(ord); // GaussianFactorGraph fg = createGaussianFactorGraph(ord);
@ -534,7 +534,7 @@ TEST( GaussianFactorGraph, optimize_QR )
TEST( GaussianFactorGraph, combine) TEST( GaussianFactorGraph, combine)
{ {
// create an ordering // create an ordering
Ordering ord; ord += kx(2),kl(1),kx(1); Ordering ord; ord += X(2),L(1),X(1);
// create a test graph // create a test graph
GaussianFactorGraph fg1 = createGaussianFactorGraph(ord); GaussianFactorGraph fg1 = createGaussianFactorGraph(ord);
@ -556,7 +556,7 @@ TEST( GaussianFactorGraph, combine)
TEST( GaussianFactorGraph, combine2) TEST( GaussianFactorGraph, combine2)
{ {
// create an ordering // create an ordering
Ordering ord; ord += kx(2),kl(1),kx(1); Ordering ord; ord += X(2),L(1),X(1);
// create a test graph // create a test graph
GaussianFactorGraph fg1 = createGaussianFactorGraph(ord); GaussianFactorGraph fg1 = createGaussianFactorGraph(ord);
@ -589,13 +589,13 @@ void print(vector<int> v) {
// GaussianFactorGraph fg = createGaussianFactorGraph(); // GaussianFactorGraph fg = createGaussianFactorGraph();
// //
// // ask for all factor indices connected to x1 // // ask for all factor indices connected to x1
// list<size_t> x1_factors = fg.factors(kx(1)); // list<size_t> x1_factors = fg.factors(X(1));
// size_t x1_indices[] = { 0, 1, 2 }; // size_t x1_indices[] = { 0, 1, 2 };
// list<size_t> x1_expected(x1_indices, x1_indices + 3); // list<size_t> x1_expected(x1_indices, x1_indices + 3);
// EXPECT(x1_factors==x1_expected); // EXPECT(x1_factors==x1_expected);
// //
// // ask for all factor indices connected to x2 // // ask for all factor indices connected to x2
// list<size_t> x2_factors = fg.factors(kx(2)); // list<size_t> x2_factors = fg.factors(X(2));
// size_t x2_indices[] = { 1, 3 }; // size_t x2_indices[] = { 1, 3 };
// list<size_t> x2_expected(x2_indices, x2_indices + 2); // list<size_t> x2_expected(x2_indices, x2_indices + 2);
// EXPECT(x2_factors==x2_expected); // EXPECT(x2_factors==x2_expected);
@ -613,7 +613,7 @@ void print(vector<int> v) {
// GaussianFactor::shared_ptr f2 = fg[2]; // GaussianFactor::shared_ptr f2 = fg[2];
// //
// // call the function // // call the function
// vector<GaussianFactor::shared_ptr> factors = fg.findAndRemoveFactors(kx(1)); // vector<GaussianFactor::shared_ptr> factors = fg.findAndRemoveFactors(X(1));
// //
// // Check the factors // // Check the factors
// EXPECT(f0==factors[0]); // EXPECT(f0==factors[0]);
@ -638,7 +638,7 @@ TEST(GaussianFactorGraph, createSmoother)
//{ //{
// GaussianFactorGraph fg = createGaussianFactorGraph(); // GaussianFactorGraph fg = createGaussianFactorGraph();
// Dimensions expected; // Dimensions expected;
// insert(expected)(kl(1), 2)(kx(1), 2)(kx(2), 2); // insert(expected)(L(1), 2)(X(1), 2)(X(2), 2);
// Dimensions actual = fg.dimensions(); // Dimensions actual = fg.dimensions();
// EXPECT(expected==actual); // EXPECT(expected==actual);
//} //}
@ -648,7 +648,7 @@ TEST(GaussianFactorGraph, createSmoother)
//{ //{
// GaussianFactorGraph fg = createGaussianFactorGraph(); // GaussianFactorGraph fg = createGaussianFactorGraph();
// Ordering expected; // Ordering expected;
// expected += kl(1),kx(1),kx(2); // expected += L(1),X(1),X(2);
// EXPECT(assert_equal(expected,fg.keys())); // EXPECT(assert_equal(expected,fg.keys()));
//} //}
@ -656,16 +656,16 @@ TEST(GaussianFactorGraph, createSmoother)
// SL-NEEDED? TEST( GaussianFactorGraph, involves ) // SL-NEEDED? TEST( GaussianFactorGraph, involves )
//{ //{
// GaussianFactorGraph fg = createGaussianFactorGraph(); // GaussianFactorGraph fg = createGaussianFactorGraph();
// EXPECT(fg.involves(kl(1))); // EXPECT(fg.involves(L(1)));
// EXPECT(fg.involves(kx(1))); // EXPECT(fg.involves(X(1)));
// EXPECT(fg.involves(kx(2))); // EXPECT(fg.involves(X(2)));
// EXPECT(!fg.involves(kx(3))); // EXPECT(!fg.involves(X(3)));
//} //}
/* ************************************************************************* */ /* ************************************************************************* */
double error(const VectorValues& x) { double error(const VectorValues& x) {
// create an ordering // create an ordering
Ordering ord; ord += kx(2),kl(1),kx(1); Ordering ord; ord += X(2),L(1),X(1);
GaussianFactorGraph fg = createGaussianFactorGraph(ord); GaussianFactorGraph fg = createGaussianFactorGraph(ord);
return fg.error(x); return fg.error(x);
@ -679,11 +679,11 @@ double error(const VectorValues& x) {
// // Construct expected gradient // // Construct expected gradient
// VectorValues expected; // VectorValues expected;
// //
// // 2*f(x) = 100*(x1+c[kx(1)])^2 + 100*(x2-x1-[0.2;-0.1])^2 + 25*(l1-x1-[0.0;0.2])^2 + 25*(l1-x2-[-0.2;0.3])^2 // // 2*f(x) = 100*(x1+c[X(1)])^2 + 100*(x2-x1-[0.2;-0.1])^2 + 25*(l1-x1-[0.0;0.2])^2 + 25*(l1-x2-[-0.2;0.3])^2
// // worked out: df/dx1 = 100*[0.1;0.1] + 100*[0.2;-0.1]) + 25*[0.0;0.2] = [10+20;10-10+5] = [30;5] // // worked out: df/dx1 = 100*[0.1;0.1] + 100*[0.2;-0.1]) + 25*[0.0;0.2] = [10+20;10-10+5] = [30;5]
// expected.insert(kl(1),Vector_(2, 5.0,-12.5)); // expected.insert(L(1),Vector_(2, 5.0,-12.5));
// expected.insert(kx(1),Vector_(2, 30.0, 5.0)); // expected.insert(X(1),Vector_(2, 30.0, 5.0));
// expected.insert(kx(2),Vector_(2,-25.0, 17.5)); // expected.insert(X(2),Vector_(2,-25.0, 17.5));
// //
// // Check the gradient at delta=0 // // Check the gradient at delta=0
// VectorValues zero = createZeroDelta(); // VectorValues zero = createZeroDelta();
@ -696,7 +696,7 @@ double error(const VectorValues& x) {
// //
// // Check the gradient at the solution (should be zero) // // Check the gradient at the solution (should be zero)
// Ordering ord; // Ordering ord;
// ord += kx(2),kl(1),kx(1); // ord += X(2),L(1),X(1);
// GaussianFactorGraph fg2 = createGaussianFactorGraph(); // GaussianFactorGraph fg2 = createGaussianFactorGraph();
// VectorValues solution = fg2.optimize(ord); // destructive // VectorValues solution = fg2.optimize(ord); // destructive
// VectorValues actual2 = fg.gradient(solution); // VectorValues actual2 = fg.gradient(solution);
@ -707,7 +707,7 @@ double error(const VectorValues& x) {
TEST( GaussianFactorGraph, multiplication ) TEST( GaussianFactorGraph, multiplication )
{ {
// create an ordering // create an ordering
Ordering ord; ord += kx(2),kl(1),kx(1); Ordering ord; ord += X(2),L(1),X(1);
FactorGraph<JacobianFactor> A = createGaussianFactorGraph(ord); FactorGraph<JacobianFactor> A = createGaussianFactorGraph(ord);
VectorValues x = createCorrectDelta(ord); VectorValues x = createCorrectDelta(ord);
@ -724,7 +724,7 @@ TEST( GaussianFactorGraph, multiplication )
// SL-NEEDED? TEST( GaussianFactorGraph, transposeMultiplication ) // SL-NEEDED? TEST( GaussianFactorGraph, transposeMultiplication )
//{ //{
// // create an ordering // // create an ordering
// Ordering ord; ord += kx(2),kl(1),kx(1); // Ordering ord; ord += X(2),L(1),X(1);
// //
// GaussianFactorGraph A = createGaussianFactorGraph(ord); // GaussianFactorGraph A = createGaussianFactorGraph(ord);
// Errors e; // Errors e;
@ -734,9 +734,9 @@ TEST( GaussianFactorGraph, multiplication )
// e += Vector_(2,-7.5,-5.0); // e += Vector_(2,-7.5,-5.0);
// //
// VectorValues expected = createZeroDelta(ord), actual = A ^ e; // VectorValues expected = createZeroDelta(ord), actual = A ^ e;
// expected[ord[kl(1)]] = Vector_(2, -37.5,-50.0); // expected[ord[L(1)]] = Vector_(2, -37.5,-50.0);
// expected[ord[kx(1)]] = Vector_(2,-150.0, 25.0); // expected[ord[X(1)]] = Vector_(2,-150.0, 25.0);
// expected[ord[kx(2)]] = Vector_(2, 187.5, 25.0); // expected[ord[X(2)]] = Vector_(2, 187.5, 25.0);
// EXPECT(assert_equal(expected,actual)); // EXPECT(assert_equal(expected,actual));
//} //}
@ -744,7 +744,7 @@ TEST( GaussianFactorGraph, multiplication )
// SL-NEEDED? TEST( GaussianFactorGraph, rhs ) // SL-NEEDED? TEST( GaussianFactorGraph, rhs )
//{ //{
// // create an ordering // // create an ordering
// Ordering ord; ord += kx(2),kl(1),kx(1); // Ordering ord; ord += X(2),L(1),X(1);
// //
// GaussianFactorGraph Ab = createGaussianFactorGraph(ord); // GaussianFactorGraph Ab = createGaussianFactorGraph(ord);
// Errors expected = createZeroDelta(ord), actual = Ab.rhs(); // Errors expected = createZeroDelta(ord), actual = Ab.rhs();
@ -760,21 +760,21 @@ TEST( GaussianFactorGraph, multiplication )
TEST( GaussianFactorGraph, elimination ) TEST( GaussianFactorGraph, elimination )
{ {
Ordering ord; Ordering ord;
ord += kx(1), kx(2); ord += X(1), X(2);
// Create Gaussian Factor Graph // Create Gaussian Factor Graph
GaussianFactorGraph fg; GaussianFactorGraph fg;
Matrix Ap = eye(1), An = eye(1) * -1; Matrix Ap = eye(1), An = eye(1) * -1;
Vector b = Vector_(1, 0.0); Vector b = Vector_(1, 0.0);
SharedDiagonal sigma = sharedSigma(1,2.0); SharedDiagonal sigma = sharedSigma(1,2.0);
fg.add(ord[kx(1)], An, ord[kx(2)], Ap, b, sigma); fg.add(ord[X(1)], An, ord[X(2)], Ap, b, sigma);
fg.add(ord[kx(1)], Ap, b, sigma); fg.add(ord[X(1)], Ap, b, sigma);
fg.add(ord[kx(2)], Ap, b, sigma); fg.add(ord[X(2)], Ap, b, sigma);
// Eliminate // Eliminate
GaussianBayesNet bayesNet = *GaussianSequentialSolver(fg).eliminate(); GaussianBayesNet bayesNet = *GaussianSequentialSolver(fg).eliminate();
// Check sigma // Check sigma
EXPECT_DOUBLES_EQUAL(1.0,bayesNet[ord[kx(2)]]->get_sigmas()(0),1e-5); EXPECT_DOUBLES_EQUAL(1.0,bayesNet[ord[X(2)]]->get_sigmas()(0),1e-5);
// Check matrix // Check matrix
Matrix R;Vector d; Matrix R;Vector d;
@ -877,18 +877,18 @@ SharedDiagonal model = sharedSigma(2,1);
// GaussianFactorGraph g; // GaussianFactorGraph g;
// Matrix I = eye(2); // Matrix I = eye(2);
// Vector b = Vector_(0, 0, 0); // Vector b = Vector_(0, 0, 0);
// g.add(kx(1), I, kx(2), I, b, model); // g.add(X(1), I, X(2), I, b, model);
// g.add(kx(1), I, kx(3), I, b, model); // g.add(X(1), I, X(3), I, b, model);
// g.add(kx(1), I, kx(4), I, b, model); // g.add(X(1), I, X(4), I, b, model);
// g.add(kx(2), I, kx(3), I, b, model); // g.add(X(2), I, X(3), I, b, model);
// g.add(kx(2), I, kx(4), I, b, model); // g.add(X(2), I, X(4), I, b, model);
// g.add(kx(3), I, kx(4), I, b, model); // g.add(X(3), I, X(4), I, b, model);
// //
// map<string, string> tree = g.findMinimumSpanningTree<string, GaussianFactor>(); // map<string, string> tree = g.findMinimumSpanningTree<string, GaussianFactor>();
// EXPECT(tree[kx(1)].compare(kx(1))==0); // EXPECT(tree[X(1)].compare(X(1))==0);
// EXPECT(tree[kx(2)].compare(kx(1))==0); // EXPECT(tree[X(2)].compare(X(1))==0);
// EXPECT(tree[kx(3)].compare(kx(1))==0); // EXPECT(tree[X(3)].compare(X(1))==0);
// EXPECT(tree[kx(4)].compare(kx(1))==0); // EXPECT(tree[X(4)].compare(X(1))==0);
//} //}
///* ************************************************************************* */ ///* ************************************************************************* */
@ -897,17 +897,17 @@ SharedDiagonal model = sharedSigma(2,1);
// GaussianFactorGraph g; // GaussianFactorGraph g;
// Matrix I = eye(2); // Matrix I = eye(2);
// Vector b = Vector_(0, 0, 0); // Vector b = Vector_(0, 0, 0);
// g.add(kx(1), I, kx(2), I, b, model); // g.add(X(1), I, X(2), I, b, model);
// g.add(kx(1), I, kx(3), I, b, model); // g.add(X(1), I, X(3), I, b, model);
// g.add(kx(1), I, kx(4), I, b, model); // g.add(X(1), I, X(4), I, b, model);
// g.add(kx(2), I, kx(3), I, b, model); // g.add(X(2), I, X(3), I, b, model);
// g.add(kx(2), I, kx(4), I, b, model); // g.add(X(2), I, X(4), I, b, model);
// //
// PredecessorMap<string> tree; // PredecessorMap<string> tree;
// tree[kx(1)] = kx(1); // tree[X(1)] = X(1);
// tree[kx(2)] = kx(1); // tree[X(2)] = X(1);
// tree[kx(3)] = kx(1); // tree[X(3)] = X(1);
// tree[kx(4)] = kx(1); // tree[X(4)] = X(1);
// //
// GaussianFactorGraph Ab1, Ab2; // GaussianFactorGraph Ab1, Ab2;
// g.split<string, GaussianFactor>(tree, Ab1, Ab2); // g.split<string, GaussianFactor>(tree, Ab1, Ab2);
@ -918,17 +918,17 @@ SharedDiagonal model = sharedSigma(2,1);
/* ************************************************************************* */ /* ************************************************************************* */
TEST(GaussianFactorGraph, replace) TEST(GaussianFactorGraph, replace)
{ {
Ordering ord; ord += kx(1),kx(2),kx(3),kx(4),kx(5),kx(6); Ordering ord; ord += X(1),X(2),X(3),X(4),X(5),X(6);
SharedDiagonal noise(sharedSigma(3, 1.0)); SharedDiagonal noise(sharedSigma(3, 1.0));
GaussianFactorGraph::sharedFactor f1(new JacobianFactor( GaussianFactorGraph::sharedFactor f1(new JacobianFactor(
ord[kx(1)], eye(3,3), ord[kx(2)], eye(3,3), zero(3), noise)); ord[X(1)], eye(3,3), ord[X(2)], eye(3,3), zero(3), noise));
GaussianFactorGraph::sharedFactor f2(new JacobianFactor( GaussianFactorGraph::sharedFactor f2(new JacobianFactor(
ord[kx(2)], eye(3,3), ord[kx(3)], eye(3,3), zero(3), noise)); ord[X(2)], eye(3,3), ord[X(3)], eye(3,3), zero(3), noise));
GaussianFactorGraph::sharedFactor f3(new JacobianFactor( GaussianFactorGraph::sharedFactor f3(new JacobianFactor(
ord[kx(3)], eye(3,3), ord[kx(4)], eye(3,3), zero(3), noise)); ord[X(3)], eye(3,3), ord[X(4)], eye(3,3), zero(3), noise));
GaussianFactorGraph::sharedFactor f4(new JacobianFactor( GaussianFactorGraph::sharedFactor f4(new JacobianFactor(
ord[kx(5)], eye(3,3), ord[kx(6)], eye(3,3), zero(3), noise)); ord[X(5)], eye(3,3), ord[X(6)], eye(3,3), zero(3), noise));
GaussianFactorGraph actual; GaussianFactorGraph actual;
actual.push_back(f1); actual.push_back(f1);
@ -964,7 +964,7 @@ TEST(GaussianFactorGraph, replace)
// //
// // combine in a factor // // combine in a factor
// Matrix Ab; SharedDiagonal noise; // Matrix Ab; SharedDiagonal noise;
// Ordering order; order += kx(2), kl(1), kx(1); // Ordering order; order += X(2), L(1), X(1);
// boost::tie(Ab, noise) = combineFactorsAndCreateMatrix(lfg, order, dimensions); // boost::tie(Ab, noise) = combineFactorsAndCreateMatrix(lfg, order, dimensions);
// //
// // the expected augmented matrix // // the expected augmented matrix
@ -992,26 +992,26 @@ TEST(GaussianFactorGraph, replace)
// typedef GaussianFactorGraph::sharedFactor Factor; // typedef GaussianFactorGraph::sharedFactor Factor;
// SharedDiagonal model(Vector_(1, 0.5)); // SharedDiagonal model(Vector_(1, 0.5));
// GaussianFactorGraph fg; // GaussianFactorGraph fg;
// Factor factor1(new JacobianFactor(kx(1), Matrix_(1,1,1.), kx(2), Matrix_(1,1,1.), Vector_(1,1.), model)); // Factor factor1(new JacobianFactor(X(1), Matrix_(1,1,1.), X(2), Matrix_(1,1,1.), Vector_(1,1.), model));
// Factor factor2(new JacobianFactor(kx(2), Matrix_(1,1,1.), kx(3), Matrix_(1,1,1.), Vector_(1,1.), model)); // Factor factor2(new JacobianFactor(X(2), Matrix_(1,1,1.), X(3), Matrix_(1,1,1.), Vector_(1,1.), model));
// Factor factor3(new JacobianFactor(kx(3), Matrix_(1,1,1.), kx(3), Matrix_(1,1,1.), Vector_(1,1.), model)); // Factor factor3(new JacobianFactor(X(3), Matrix_(1,1,1.), X(3), Matrix_(1,1,1.), Vector_(1,1.), model));
// fg.push_back(factor1); // fg.push_back(factor1);
// fg.push_back(factor2); // fg.push_back(factor2);
// fg.push_back(factor3); // fg.push_back(factor3);
// //
// Ordering frontals; frontals += kx(2), kx(1); // Ordering frontals; frontals += X(2), X(1);
// GaussianBayesNet bn = fg.eliminateFrontals(frontals); // GaussianBayesNet bn = fg.eliminateFrontals(frontals);
// //
// GaussianBayesNet bn_expected; // GaussianBayesNet bn_expected;
// GaussianBayesNet::sharedConditional conditional1(new GaussianConditional(kx(2), Vector_(1, 2.), Matrix_(1, 1, 2.), // GaussianBayesNet::sharedConditional conditional1(new GaussianConditional(X(2), Vector_(1, 2.), Matrix_(1, 1, 2.),
// kx(1), Matrix_(1, 1, 1.), kx(3), Matrix_(1, 1, 1.), Vector_(1, 1.))); // X(1), Matrix_(1, 1, 1.), X(3), Matrix_(1, 1, 1.), Vector_(1, 1.)));
// GaussianBayesNet::sharedConditional conditional2(new GaussianConditional(kx(1), Vector_(1, 0.), Matrix_(1, 1, -1.), // GaussianBayesNet::sharedConditional conditional2(new GaussianConditional(X(1), Vector_(1, 0.), Matrix_(1, 1, -1.),
// kx(3), Matrix_(1, 1, 1.), Vector_(1, 1.))); // X(3), Matrix_(1, 1, 1.), Vector_(1, 1.)));
// bn_expected.push_back(conditional1); // bn_expected.push_back(conditional1);
// bn_expected.push_back(conditional2); // bn_expected.push_back(conditional2);
// EXPECT(assert_equal(bn_expected, bn)); // EXPECT(assert_equal(bn_expected, bn));
// //
// GaussianFactorGraph::sharedFactor factor_expected(new JacobianFactor(kx(3), Matrix_(1, 1, 2.), Vector_(1, 2.), SharedDiagonal(Vector_(1, 1.)))); // GaussianFactorGraph::sharedFactor factor_expected(new JacobianFactor(X(3), Matrix_(1, 1, 2.), Vector_(1, 2.), SharedDiagonal(Vector_(1, 1.))));
// GaussianFactorGraph fg_expected; // GaussianFactorGraph fg_expected;
// fg_expected.push_back(factor_expected); // fg_expected.push_back(factor_expected);
// EXPECT(assert_equal(fg_expected, fg)); // EXPECT(assert_equal(fg_expected, fg));
@ -1027,8 +1027,8 @@ TEST(GaussianFactorGraph, createSmoother2)
LONGS_EQUAL(5,fg2.size()); LONGS_EQUAL(5,fg2.size());
// eliminate // eliminate
vector<Index> x3var; x3var.push_back(ordering[kx(3)]); vector<Index> x3var; x3var.push_back(ordering[X(3)]);
vector<Index> x1var; x1var.push_back(ordering[kx(1)]); vector<Index> x1var; x1var.push_back(ordering[X(1)]);
GaussianBayesNet p_x3 = *GaussianSequentialSolver( GaussianBayesNet p_x3 = *GaussianSequentialSolver(
*GaussianSequentialSolver(fg2).jointFactorGraph(x3var)).eliminate(); *GaussianSequentialSolver(fg2).jointFactorGraph(x3var)).eliminate();
GaussianBayesNet p_x1 = *GaussianSequentialSolver( GaussianBayesNet p_x1 = *GaussianSequentialSolver(
@ -1045,7 +1045,7 @@ TEST(GaussianFactorGraph, hasConstraints)
FactorGraph<GaussianFactor> fgc2 = createSimpleConstraintGraph() ; FactorGraph<GaussianFactor> fgc2 = createSimpleConstraintGraph() ;
EXPECT(hasConstraints(fgc2)); EXPECT(hasConstraints(fgc2));
Ordering ordering; ordering += kx(1), kx(2), kl(1); Ordering ordering; ordering += X(1), X(2), L(1);
FactorGraph<GaussianFactor> fg = createGaussianFactorGraph(ordering); FactorGraph<GaussianFactor> fg = createGaussianFactorGraph(ordering);
EXPECT(!hasConstraints(fg)); EXPECT(!hasConstraints(fg));
} }

94
tests/testGaussianISAM.cpp
View File

@ -35,8 +35,8 @@ using namespace std;
using namespace gtsam; using namespace gtsam;
using namespace example; using namespace example;
Key kx(size_t i) { return Symbol('x',i); } using symbol_shorthand::X;
Key kl(size_t i) { return Symbol('l',i); } using symbol_shorthand::L;
/* ************************************************************************* */ /* ************************************************************************* */
// Some numbers that should be consistent among all smoother tests // Some numbers that should be consistent among all smoother tests
@ -50,7 +50,7 @@ const double tol = 1e-4;
TEST_UNSAFE( ISAM, iSAM_smoother ) TEST_UNSAFE( ISAM, iSAM_smoother )
{ {
Ordering ordering; Ordering ordering;
for (int t = 1; t <= 7; t++) ordering += kx(t); for (int t = 1; t <= 7; t++) ordering += X(t);
// Create smoother with 7 nodes // Create smoother with 7 nodes
GaussianFactorGraph smoother = createSmoother(7, ordering).first; GaussianFactorGraph smoother = createSmoother(7, ordering).first;
@ -83,7 +83,7 @@ TEST_UNSAFE( ISAM, iSAM_smoother )
// GaussianFactorGraph smoother = createSmoother(7); // GaussianFactorGraph smoother = createSmoother(7);
// //
// // Create initial tree from first 4 timestamps in reverse order ! // // Create initial tree from first 4 timestamps in reverse order !
// Ordering ord; ord += kx(4),kx(3),kx(2),kx(1); // Ordering ord; ord += X(4),X(3),X(2),X(1);
// GaussianFactorGraph factors1; // GaussianFactorGraph factors1;
// for (int i=0;i<7;i++) factors1.push_back(smoother[i]); // for (int i=0;i<7;i++) factors1.push_back(smoother[i]);
// GaussianISAM actual(*inference::Eliminate(factors1)); // GaussianISAM actual(*inference::Eliminate(factors1));
@ -130,7 +130,7 @@ TEST_UNSAFE( BayesTree, linear_smoother_shortcuts )
EXPECT(assert_equal(empty,actual1,tol)); EXPECT(assert_equal(empty,actual1,tol));
// Check the conditional P(C2|Root) // Check the conditional P(C2|Root)
GaussianISAM::sharedClique C2 = isamTree[ordering[kx(5)]]; GaussianISAM::sharedClique C2 = isamTree[ordering[X(5)]];
GaussianBayesNet actual2 = GaussianISAM::shortcut(C2,R); GaussianBayesNet actual2 = GaussianISAM::shortcut(C2,R);
EXPECT(assert_equal(empty,actual2,tol)); EXPECT(assert_equal(empty,actual2,tol));
@ -138,8 +138,8 @@ TEST_UNSAFE( BayesTree, linear_smoother_shortcuts )
double sigma3 = 0.61808; double sigma3 = 0.61808;
Matrix A56 = Matrix_(2,2,-0.382022,0.,0.,-0.382022); Matrix A56 = Matrix_(2,2,-0.382022,0.,0.,-0.382022);
GaussianBayesNet expected3; GaussianBayesNet expected3;
push_front(expected3,ordering[kx(5)], zero(2), eye(2)/sigma3, ordering[kx(6)], A56/sigma3, ones(2)); push_front(expected3,ordering[X(5)], zero(2), eye(2)/sigma3, ordering[X(6)], A56/sigma3, ones(2));
GaussianISAM::sharedClique C3 = isamTree[ordering[kx(4)]]; GaussianISAM::sharedClique C3 = isamTree[ordering[X(4)]];
GaussianBayesNet actual3 = GaussianISAM::shortcut(C3,R); GaussianBayesNet actual3 = GaussianISAM::shortcut(C3,R);
EXPECT(assert_equal(expected3,actual3,tol)); EXPECT(assert_equal(expected3,actual3,tol));
@ -147,8 +147,8 @@ TEST_UNSAFE( BayesTree, linear_smoother_shortcuts )
double sigma4 = 0.661968; double sigma4 = 0.661968;
Matrix A46 = Matrix_(2,2,-0.146067,0.,0.,-0.146067); Matrix A46 = Matrix_(2,2,-0.146067,0.,0.,-0.146067);
GaussianBayesNet expected4; GaussianBayesNet expected4;
push_front(expected4, ordering[kx(4)], zero(2), eye(2)/sigma4, ordering[kx(6)], A46/sigma4, ones(2)); push_front(expected4, ordering[X(4)], zero(2), eye(2)/sigma4, ordering[X(6)], A46/sigma4, ones(2));
GaussianISAM::sharedClique C4 = isamTree[ordering[kx(3)]]; GaussianISAM::sharedClique C4 = isamTree[ordering[X(3)]];
GaussianBayesNet actual4 = GaussianISAM::shortcut(C4,R); GaussianBayesNet actual4 = GaussianISAM::shortcut(C4,R);
EXPECT(assert_equal(expected4,actual4,tol)); EXPECT(assert_equal(expected4,actual4,tol));
} }
@ -176,7 +176,7 @@ TEST_UNSAFE( BayesTree, balanced_smoother_marginals )
{ {
// Create smoother with 7 nodes // Create smoother with 7 nodes
Ordering ordering; Ordering ordering;
ordering += kx(1),kx(3),kx(5),kx(7),kx(2),kx(6),kx(4); ordering += X(1),X(3),X(5),X(7),X(2),X(6),X(4);
GaussianFactorGraph smoother = createSmoother(7, ordering).first; GaussianFactorGraph smoother = createSmoother(7, ordering).first;
// Create the Bayes tree // Create the Bayes tree
@ -193,48 +193,48 @@ TEST_UNSAFE( BayesTree, balanced_smoother_marginals )
double tol=1e-5; double tol=1e-5;
// Check marginal on x1 // Check marginal on x1
GaussianBayesNet expected1 = simpleGaussian(ordering[kx(1)], zero(2), sigmax1); GaussianBayesNet expected1 = simpleGaussian(ordering[X(1)], zero(2), sigmax1);
GaussianBayesNet actual1 = *bayesTree.marginalBayesNet(ordering[kx(1)]); GaussianBayesNet actual1 = *bayesTree.marginalBayesNet(ordering[X(1)]);
Matrix expectedCovarianceX1 = eye(2,2) * (sigmax1 * sigmax1); Matrix expectedCovarianceX1 = eye(2,2) * (sigmax1 * sigmax1);
Matrix actualCovarianceX1; Matrix actualCovarianceX1;
actualCovarianceX1 = bayesTree.marginalCovariance(ordering[kx(1)]); actualCovarianceX1 = bayesTree.marginalCovariance(ordering[X(1)]);
EXPECT(assert_equal(expectedCovarianceX1, actualCovarianceX1, tol)); EXPECT(assert_equal(expectedCovarianceX1, actualCovarianceX1, tol));
EXPECT(assert_equal(expected1,actual1,tol)); EXPECT(assert_equal(expected1,actual1,tol));
// Check marginal on x2 // Check marginal on x2
double sigx2 = 0.68712938; // FIXME: this should be corrected analytically double sigx2 = 0.68712938; // FIXME: this should be corrected analytically
GaussianBayesNet expected2 = simpleGaussian(ordering[kx(2)], zero(2), sigx2); GaussianBayesNet expected2 = simpleGaussian(ordering[X(2)], zero(2), sigx2);
GaussianBayesNet actual2 = *bayesTree.marginalBayesNet(ordering[kx(2)]); GaussianBayesNet actual2 = *bayesTree.marginalBayesNet(ordering[X(2)]);
Matrix expectedCovarianceX2 = eye(2,2) * (sigx2 * sigx2); Matrix expectedCovarianceX2 = eye(2,2) * (sigx2 * sigx2);
Matrix actualCovarianceX2; Matrix actualCovarianceX2;
actualCovarianceX2 = bayesTree.marginalCovariance(ordering[kx(2)]); actualCovarianceX2 = bayesTree.marginalCovariance(ordering[X(2)]);
EXPECT(assert_equal(expectedCovarianceX2, actualCovarianceX2, tol)); EXPECT(assert_equal(expectedCovarianceX2, actualCovarianceX2, tol));
EXPECT(assert_equal(expected2,actual2,tol)); EXPECT(assert_equal(expected2,actual2,tol));
// Check marginal on x3 // Check marginal on x3
GaussianBayesNet expected3 = simpleGaussian(ordering[kx(3)], zero(2), sigmax3); GaussianBayesNet expected3 = simpleGaussian(ordering[X(3)], zero(2), sigmax3);
GaussianBayesNet actual3 = *bayesTree.marginalBayesNet(ordering[kx(3)]); GaussianBayesNet actual3 = *bayesTree.marginalBayesNet(ordering[X(3)]);
Matrix expectedCovarianceX3 = eye(2,2) * (sigmax3 * sigmax3); Matrix expectedCovarianceX3 = eye(2,2) * (sigmax3 * sigmax3);
Matrix actualCovarianceX3; Matrix actualCovarianceX3;
actualCovarianceX3 = bayesTree.marginalCovariance(ordering[kx(3)]); actualCovarianceX3 = bayesTree.marginalCovariance(ordering[X(3)]);
EXPECT(assert_equal(expectedCovarianceX3, actualCovarianceX3, tol)); EXPECT(assert_equal(expectedCovarianceX3, actualCovarianceX3, tol));
EXPECT(assert_equal(expected3,actual3,tol)); EXPECT(assert_equal(expected3,actual3,tol));
// Check marginal on x4 // Check marginal on x4
GaussianBayesNet expected4 = simpleGaussian(ordering[kx(4)], zero(2), sigmax4); GaussianBayesNet expected4 = simpleGaussian(ordering[X(4)], zero(2), sigmax4);
GaussianBayesNet actual4 = *bayesTree.marginalBayesNet(ordering[kx(4)]); GaussianBayesNet actual4 = *bayesTree.marginalBayesNet(ordering[X(4)]);
Matrix expectedCovarianceX4 = eye(2,2) * (sigmax4 * sigmax4); Matrix expectedCovarianceX4 = eye(2,2) * (sigmax4 * sigmax4);
Matrix actualCovarianceX4; Matrix actualCovarianceX4;
actualCovarianceX4 = bayesTree.marginalCovariance(ordering[kx(4)]); actualCovarianceX4 = bayesTree.marginalCovariance(ordering[X(4)]);
EXPECT(assert_equal(expectedCovarianceX4, actualCovarianceX4, tol)); EXPECT(assert_equal(expectedCovarianceX4, actualCovarianceX4, tol));
EXPECT(assert_equal(expected4,actual4,tol)); EXPECT(assert_equal(expected4,actual4,tol));
// Check marginal on x7 (should be equal to x1) // Check marginal on x7 (should be equal to x1)
GaussianBayesNet expected7 = simpleGaussian(ordering[kx(7)], zero(2), sigmax7); GaussianBayesNet expected7 = simpleGaussian(ordering[X(7)], zero(2), sigmax7);
GaussianBayesNet actual7 = *bayesTree.marginalBayesNet(ordering[kx(7)]); GaussianBayesNet actual7 = *bayesTree.marginalBayesNet(ordering[X(7)]);
Matrix expectedCovarianceX7 = eye(2,2) * (sigmax7 * sigmax7); Matrix expectedCovarianceX7 = eye(2,2) * (sigmax7 * sigmax7);
Matrix actualCovarianceX7; Matrix actualCovarianceX7;
actualCovarianceX7 = bayesTree.marginalCovariance(ordering[kx(7)]); actualCovarianceX7 = bayesTree.marginalCovariance(ordering[X(7)]);
EXPECT(assert_equal(expectedCovarianceX7, actualCovarianceX7, tol)); EXPECT(assert_equal(expectedCovarianceX7, actualCovarianceX7, tol));
EXPECT(assert_equal(expected7,actual7,tol)); EXPECT(assert_equal(expected7,actual7,tol));
} }
@ -244,7 +244,7 @@ TEST_UNSAFE( BayesTree, balanced_smoother_shortcuts )
{ {
// Create smoother with 7 nodes // Create smoother with 7 nodes
Ordering ordering; Ordering ordering;
ordering += kx(1),kx(3),kx(5),kx(7),kx(2),kx(6),kx(4); ordering += X(1),X(3),X(5),X(7),X(2),X(6),X(4);
GaussianFactorGraph smoother = createSmoother(7, ordering).first; GaussianFactorGraph smoother = createSmoother(7, ordering).first;
// Create the Bayes tree // Create the Bayes tree
@ -258,19 +258,19 @@ TEST_UNSAFE( BayesTree, balanced_smoother_shortcuts )
EXPECT(assert_equal(empty,actual1,tol)); EXPECT(assert_equal(empty,actual1,tol));
// Check the conditional P(C2|Root) // Check the conditional P(C2|Root)
GaussianISAM::sharedClique C2 = isamTree[ordering[kx(3)]]; GaussianISAM::sharedClique C2 = isamTree[ordering[X(3)]];
GaussianBayesNet actual2 = GaussianISAM::shortcut(C2,R); GaussianBayesNet actual2 = GaussianISAM::shortcut(C2,R);
EXPECT(assert_equal(empty,actual2,tol)); EXPECT(assert_equal(empty,actual2,tol));
// Check the conditional P(C3|Root), which should be equal to P(x2|x4) // Check the conditional P(C3|Root), which should be equal to P(x2|x4)
/** TODO: Note for multifrontal conditional: /** TODO: Note for multifrontal conditional:
* p_x2_x4 is now an element conditional of the multifrontal conditional bayesTree[ordering[kx(2)]]->conditional() * p_x2_x4 is now an element conditional of the multifrontal conditional bayesTree[ordering[X(2)]]->conditional()
* We don't know yet how to take it out. * We don't know yet how to take it out.
*/ */
// GaussianConditional::shared_ptr p_x2_x4 = bayesTree[ordering[kx(2)]]->conditional(); // GaussianConditional::shared_ptr p_x2_x4 = bayesTree[ordering[X(2)]]->conditional();
// p_x2_x4->print("Conditional p_x2_x4: "); // p_x2_x4->print("Conditional p_x2_x4: ");
// GaussianBayesNet expected3(p_x2_x4); // GaussianBayesNet expected3(p_x2_x4);
// GaussianISAM::sharedClique C3 = isamTree[ordering[kx(1)]]; // GaussianISAM::sharedClique C3 = isamTree[ordering[X(1)]];
// GaussianBayesNet actual3 = GaussianISAM::shortcut(C3,R); // GaussianBayesNet actual3 = GaussianISAM::shortcut(C3,R);
// EXPECT(assert_equal(expected3,actual3,tol)); // EXPECT(assert_equal(expected3,actual3,tol));
} }
@ -280,7 +280,7 @@ TEST_UNSAFE( BayesTree, balanced_smoother_shortcuts )
//{ //{
// // Create smoother with 7 nodes // // Create smoother with 7 nodes
// Ordering ordering; // Ordering ordering;
// ordering += kx(1),kx(3),kx(5),kx(7),kx(2),kx(6),kx(4); // ordering += X(1),X(3),X(5),X(7),X(2),X(6),X(4);
// GaussianFactorGraph smoother = createSmoother(7, ordering).first; // GaussianFactorGraph smoother = createSmoother(7, ordering).first;
// //
// // Create the Bayes tree // // Create the Bayes tree
@ -289,9 +289,9 @@ TEST_UNSAFE( BayesTree, balanced_smoother_shortcuts )
// //
// // Check the clique marginal P(C3) // // Check the clique marginal P(C3)
// double sigmax2_alt = 1/1.45533; // THIS NEEDS TO BE CHECKED! // double sigmax2_alt = 1/1.45533; // THIS NEEDS TO BE CHECKED!
// GaussianBayesNet expected = simpleGaussian(ordering[kx(2)],zero(2),sigmax2_alt); // GaussianBayesNet expected = simpleGaussian(ordering[X(2)],zero(2),sigmax2_alt);
// push_front(expected,ordering[kx(1)], zero(2), eye(2)*sqrt(2), ordering[kx(2)], -eye(2)*sqrt(2)/2, ones(2)); // push_front(expected,ordering[X(1)], zero(2), eye(2)*sqrt(2), ordering[X(2)], -eye(2)*sqrt(2)/2, ones(2));
// GaussianISAM::sharedClique R = bayesTree.root(), C3 = bayesTree[ordering[kx(1)]]; // GaussianISAM::sharedClique R = bayesTree.root(), C3 = bayesTree[ordering[X(1)]];
// GaussianFactorGraph marginal = C3->marginal(R); // GaussianFactorGraph marginal = C3->marginal(R);
// GaussianVariableIndex varIndex(marginal); // GaussianVariableIndex varIndex(marginal);
// Permutation toFront(Permutation::PullToFront(C3->keys(), varIndex.size())); // Permutation toFront(Permutation::PullToFront(C3->keys(), varIndex.size()));
@ -309,7 +309,7 @@ TEST_UNSAFE( BayesTree, balanced_smoother_joint )
{ {
// Create smoother with 7 nodes // Create smoother with 7 nodes
Ordering ordering; Ordering ordering;
ordering += kx(1),kx(3),kx(5),kx(7),kx(2),kx(6),kx(4); ordering += X(1),X(3),X(5),X(7),X(2),X(6),X(4);
GaussianFactorGraph smoother = createSmoother(7, ordering).first; GaussianFactorGraph smoother = createSmoother(7, ordering).first;
// Create the Bayes tree, expected to look like: // Create the Bayes tree, expected to look like:
@ -328,41 +328,41 @@ TEST_UNSAFE( BayesTree, balanced_smoother_joint )
GaussianBayesNet expected1; GaussianBayesNet expected1;
// Why does the sign get flipped on the prior? // Why does the sign get flipped on the prior?
GaussianConditional::shared_ptr GaussianConditional::shared_ptr
parent1(new GaussianConditional(ordering[kx(7)], zero(2), -1*I/sigmax7, ones(2))); parent1(new GaussianConditional(ordering[X(7)], zero(2), -1*I/sigmax7, ones(2)));
expected1.push_front(parent1); expected1.push_front(parent1);
push_front(expected1,ordering[kx(1)], zero(2), I/sigmax7, ordering[kx(7)], A/sigmax7, sigma); push_front(expected1,ordering[X(1)], zero(2), I/sigmax7, ordering[X(7)], A/sigmax7, sigma);
GaussianBayesNet actual1 = *bayesTree.jointBayesNet(ordering[kx(1)],ordering[kx(7)]); GaussianBayesNet actual1 = *bayesTree.jointBayesNet(ordering[X(1)],ordering[X(7)]);
EXPECT(assert_equal(expected1,actual1,tol)); EXPECT(assert_equal(expected1,actual1,tol));
// // Check the joint density P(x7,x1) factored as P(x7|x1)P(x1) // // Check the joint density P(x7,x1) factored as P(x7|x1)P(x1)
// GaussianBayesNet expected2; // GaussianBayesNet expected2;
// GaussianConditional::shared_ptr // GaussianConditional::shared_ptr
// parent2(new GaussianConditional(ordering[kx(1)], zero(2), -1*I/sigmax1, ones(2))); // parent2(new GaussianConditional(ordering[X(1)], zero(2), -1*I/sigmax1, ones(2)));
// expected2.push_front(parent2); // expected2.push_front(parent2);
// push_front(expected2,ordering[kx(7)], zero(2), I/sigmax1, ordering[kx(1)], A/sigmax1, sigma); // push_front(expected2,ordering[X(7)], zero(2), I/sigmax1, ordering[X(1)], A/sigmax1, sigma);
// GaussianBayesNet actual2 = *bayesTree.jointBayesNet(ordering[kx(7)],ordering[kx(1)]); // GaussianBayesNet actual2 = *bayesTree.jointBayesNet(ordering[X(7)],ordering[X(1)]);
// EXPECT(assert_equal(expected2,actual2,tol)); // EXPECT(assert_equal(expected2,actual2,tol));
// Check the joint density P(x1,x4), i.e. with a root variable // Check the joint density P(x1,x4), i.e. with a root variable
GaussianBayesNet expected3; GaussianBayesNet expected3;
GaussianConditional::shared_ptr GaussianConditional::shared_ptr
parent3(new GaussianConditional(ordering[kx(4)], zero(2), I/sigmax4, ones(2))); parent3(new GaussianConditional(ordering[X(4)], zero(2), I/sigmax4, ones(2)));
expected3.push_front(parent3); expected3.push_front(parent3);
double sig14 = 0.784465; double sig14 = 0.784465;
Matrix A14 = -0.0769231*I; Matrix A14 = -0.0769231*I;
push_front(expected3,ordering[kx(1)], zero(2), I/sig14, ordering[kx(4)], A14/sig14, sigma); push_front(expected3,ordering[X(1)], zero(2), I/sig14, ordering[X(4)], A14/sig14, sigma);
GaussianBayesNet actual3 = *bayesTree.jointBayesNet(ordering[kx(1)],ordering[kx(4)]); GaussianBayesNet actual3 = *bayesTree.jointBayesNet(ordering[X(1)],ordering[X(4)]);
EXPECT(assert_equal(expected3,actual3,tol)); EXPECT(assert_equal(expected3,actual3,tol));
// // Check the joint density P(x4,x1), i.e. with a root variable, factored the other way // // Check the joint density P(x4,x1), i.e. with a root variable, factored the other way
// GaussianBayesNet expected4; // GaussianBayesNet expected4;
// GaussianConditional::shared_ptr // GaussianConditional::shared_ptr
// parent4(new GaussianConditional(ordering[kx(1)], zero(2), -1.0*I/sigmax1, ones(2))); // parent4(new GaussianConditional(ordering[X(1)], zero(2), -1.0*I/sigmax1, ones(2)));
// expected4.push_front(parent4); // expected4.push_front(parent4);
// double sig41 = 0.668096; // double sig41 = 0.668096;
// Matrix A41 = -0.055794*I; // Matrix A41 = -0.055794*I;
// push_front(expected4,ordering[kx(4)], zero(2), I/sig41, ordering[kx(1)], A41/sig41, sigma); // push_front(expected4,ordering[X(4)], zero(2), I/sig41, ordering[X(1)], A41/sig41, sigma);
// GaussianBayesNet actual4 = *bayesTree.jointBayesNet(ordering[kx(4)],ordering[kx(1)]); // GaussianBayesNet actual4 = *bayesTree.jointBayesNet(ordering[X(4)],ordering[X(1)]);
// EXPECT(assert_equal(expected4,actual4,tol)); // EXPECT(assert_equal(expected4,actual4,tol));
} }

66
tests/testGaussianJunctionTreeB.cpp
View File

@ -42,8 +42,8 @@ using namespace std;
using namespace gtsam; using namespace gtsam;
using namespace example; using namespace example;
Key kx(size_t i) { return Symbol('x',i); } using symbol_shorthand::X;
Key kl(size_t i) { return Symbol('l',i); } using symbol_shorthand::L;
/* ************************************************************************* * /* ************************************************************************* *
Bayes tree for smoother with "nested dissection" ordering: Bayes tree for smoother with "nested dissection" ordering:
@ -55,20 +55,20 @@ Key kl(size_t i) { return Symbol('l',i); }
TEST( GaussianJunctionTree, constructor2 ) TEST( GaussianJunctionTree, constructor2 )
{ {
// create a graph // create a graph
Ordering ordering; ordering += kx(1),kx(3),kx(5),kx(7),kx(2),kx(6),kx(4); Ordering ordering; ordering += X(1),X(3),X(5),X(7),X(2),X(6),X(4);
GaussianFactorGraph fg = createSmoother(7, ordering).first; GaussianFactorGraph fg = createSmoother(7, ordering).first;
// create an ordering // create an ordering
GaussianJunctionTree actual(fg); GaussianJunctionTree actual(fg);
vector<Index> frontal1; frontal1 += ordering[kx(5)], ordering[kx(6)], ordering[kx(4)]; vector<Index> frontal1; frontal1 += ordering[X(5)], ordering[X(6)], ordering[X(4)];
vector<Index> frontal2; frontal2 += ordering[kx(3)], ordering[kx(2)]; vector<Index> frontal2; frontal2 += ordering[X(3)], ordering[X(2)];
vector<Index> frontal3; frontal3 += ordering[kx(1)]; vector<Index> frontal3; frontal3 += ordering[X(1)];
vector<Index> frontal4; frontal4 += ordering[kx(7)]; vector<Index> frontal4; frontal4 += ordering[X(7)];
vector<Index> sep1; vector<Index> sep1;
vector<Index> sep2; sep2 += ordering[kx(4)]; vector<Index> sep2; sep2 += ordering[X(4)];
vector<Index> sep3; sep3 += ordering[kx(2)]; vector<Index> sep3; sep3 += ordering[X(2)];
vector<Index> sep4; sep4 += ordering[kx(6)]; vector<Index> sep4; sep4 += ordering[X(6)];
EXPECT(assert_equal(frontal1, actual.root()->frontal)); EXPECT(assert_equal(frontal1, actual.root()->frontal));
EXPECT(assert_equal(sep1, actual.root()->separator)); EXPECT(assert_equal(sep1, actual.root()->separator));
LONGS_EQUAL(5, actual.root()->size()); LONGS_EQUAL(5, actual.root()->size());
@ -103,7 +103,7 @@ TEST( GaussianJunctionTree, optimizeMultiFrontal )
VectorValues expected(vector<size_t>(7,2)); // expected solution VectorValues expected(vector<size_t>(7,2)); // expected solution
Vector v = Vector_(2, 0., 0.); Vector v = Vector_(2, 0., 0.);
for (int i=1; i<=7; i++) for (int i=1; i<=7; i++)
expected[ordering[Symbol('x',i)]] = v; expected[ordering[X(i)]] = v;
EXPECT(assert_equal(expected,actual)); EXPECT(assert_equal(expected,actual));
} }
@ -113,7 +113,7 @@ TEST( GaussianJunctionTree, optimizeMultiFrontal2)
// create a graph // create a graph
example::Graph nlfg = createNonlinearFactorGraph(); example::Graph nlfg = createNonlinearFactorGraph();
Values noisy = createNoisyValues(); Values noisy = createNoisyValues();
Ordering ordering; ordering += kx(1),kx(2),kl(1); Ordering ordering; ordering += X(1),X(2),L(1);
GaussianFactorGraph fg = *nlfg.linearize(noisy, ordering); GaussianFactorGraph fg = *nlfg.linearize(noisy, ordering);
// optimize the graph // optimize the graph
@ -136,39 +136,39 @@ TEST(GaussianJunctionTree, slamlike) {
size_t i = 0; size_t i = 0;
newfactors = planarSLAM::Graph(); newfactors = planarSLAM::Graph();
newfactors.addPrior(kx(0), Pose2(0.0, 0.0, 0.0), odoNoise); newfactors.addPrior(X(0), Pose2(0.0, 0.0, 0.0), odoNoise);
init.insert(kx(0), Pose2(0.01, 0.01, 0.01)); init.insert(X(0), Pose2(0.01, 0.01, 0.01));
fullgraph.push_back(newfactors); fullgraph.push_back(newfactors);
for( ; i<5; ++i) { for( ; i<5; ++i) {
newfactors = planarSLAM::Graph(); newfactors = planarSLAM::Graph();
newfactors.addOdometry(kx(i), kx(i+1), Pose2(1.0, 0.0, 0.0), odoNoise); newfactors.addOdometry(X(i), X(i+1), Pose2(1.0, 0.0, 0.0), odoNoise);
init.insert(kx(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); init.insert(X(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
fullgraph.push_back(newfactors); fullgraph.push_back(newfactors);
} }
newfactors = planarSLAM::Graph(); newfactors = planarSLAM::Graph();
newfactors.addOdometry(kx(i), kx(i+1), Pose2(1.0, 0.0, 0.0), odoNoise); newfactors.addOdometry(X(i), X(i+1), Pose2(1.0, 0.0, 0.0), odoNoise);
newfactors.addBearingRange(kx(i), kl(0), Rot2::fromAngle(M_PI/4.0), 5.0, brNoise); newfactors.addBearingRange(X(i), L(0), Rot2::fromAngle(M_PI/4.0), 5.0, brNoise);
newfactors.addBearingRange(kx(i), kl(1), Rot2::fromAngle(-M_PI/4.0), 5.0, brNoise); newfactors.addBearingRange(X(i), L(1), Rot2::fromAngle(-M_PI/4.0), 5.0, brNoise);
init.insert(kx(i+1), Pose2(1.01, 0.01, 0.01)); init.insert(X(i+1), Pose2(1.01, 0.01, 0.01));
init.insert(kl(0), Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0))); init.insert(L(0), Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0)));
init.insert(kl(1), Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0))); init.insert(L(1), Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0)));
fullgraph.push_back(newfactors); fullgraph.push_back(newfactors);
++ i; ++ i;
for( ; i<5; ++i) { for( ; i<5; ++i) {
newfactors = planarSLAM::Graph(); newfactors = planarSLAM::Graph();
newfactors.addOdometry(kx(i), kx(i+1), Pose2(1.0, 0.0, 0.0), odoNoise); newfactors.addOdometry(X(i), X(i+1), Pose2(1.0, 0.0, 0.0), odoNoise);
init.insert(kx(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); init.insert(X(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
fullgraph.push_back(newfactors); fullgraph.push_back(newfactors);
} }
newfactors = planarSLAM::Graph(); newfactors = planarSLAM::Graph();
newfactors.addOdometry(kx(i), kx(i+1), Pose2(1.0, 0.0, 0.0), odoNoise); newfactors.addOdometry(X(i), X(i+1), Pose2(1.0, 0.0, 0.0), odoNoise);
newfactors.addBearingRange(kx(i), kl(0), Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 4.5, brNoise); newfactors.addBearingRange(X(i), L(0), Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 4.5, brNoise);
newfactors.addBearingRange(kx(i), kl(1), Rot2::fromAngle(-M_PI/4.0 + M_PI/16.0), 4.5, brNoise); newfactors.addBearingRange(X(i), L(1), Rot2::fromAngle(-M_PI/4.0 + M_PI/16.0), 4.5, brNoise);
init.insert(kx(i+1), Pose2(6.9, 0.1, 0.01)); init.insert(X(i+1), Pose2(6.9, 0.1, 0.01));
fullgraph.push_back(newfactors); fullgraph.push_back(newfactors);
++ i; ++ i;
@ -194,15 +194,15 @@ TEST(GaussianJunctionTree, simpleMarginal) {
// Create a simple graph // Create a simple graph
pose2SLAM::Graph fg; pose2SLAM::Graph fg;
fg.addPrior(kx(0), Pose2(), sharedSigma(3, 10.0)); fg.addPrior(X(0), Pose2(), sharedSigma(3, 10.0));
fg.addOdometry(kx(0), kx(1), Pose2(1.0, 0.0, 0.0), sharedSigmas(Vector_(3, 10.0, 1.0, 1.0))); fg.addOdometry(X(0), X(1), Pose2(1.0, 0.0, 0.0), sharedSigmas(Vector_(3, 10.0, 1.0, 1.0)));
Values init; Values init;
init.insert(kx(0), Pose2()); init.insert(X(0), Pose2());
init.insert(kx(1), Pose2(1.0, 0.0, 0.0)); init.insert(X(1), Pose2(1.0, 0.0, 0.0));
Ordering ordering; Ordering ordering;
ordering += kx(1), kx(0); ordering += X(1), X(0);
GaussianFactorGraph gfg = *fg.linearize(init, ordering); GaussianFactorGraph gfg = *fg.linearize(init, ordering);

26
tests/testInferenceB.cpp
View File

@ -27,8 +27,8 @@ using namespace std;
using namespace gtsam; using namespace gtsam;
// Convenience for named keys // Convenience for named keys
Key kx(size_t i) { return Symbol('x',i); } using symbol_shorthand::X;
Key kl(size_t i) { return Symbol('l',i); } using symbol_shorthand::L;
/* ************************************************************************* */ /* ************************************************************************* */
// The tests below test the *generic* inference algorithms. Some of these have // The tests below test the *generic* inference algorithms. Some of these have
@ -57,23 +57,23 @@ TEST( inference, marginals2)
SharedDiagonal poseModel(sharedSigma(3, 0.1)); SharedDiagonal poseModel(sharedSigma(3, 0.1));
SharedDiagonal pointModel(sharedSigma(3, 0.1)); SharedDiagonal pointModel(sharedSigma(3, 0.1));
fg.addPrior(kx(0), Pose2(), poseModel); fg.addPrior(X(0), Pose2(), poseModel);
fg.addOdometry(kx(0), kx(1), Pose2(1.0,0.0,0.0), poseModel); fg.addOdometry(X(0), X(1), Pose2(1.0,0.0,0.0), poseModel);
fg.addOdometry(kx(1), kx(2), Pose2(1.0,0.0,0.0), poseModel); fg.addOdometry(X(1), X(2), Pose2(1.0,0.0,0.0), poseModel);
fg.addBearingRange(kx(0), kl(0), Rot2(), 1.0, pointModel); fg.addBearingRange(X(0), L(0), Rot2(), 1.0, pointModel);
fg.addBearingRange(kx(1), kl(0), Rot2(), 1.0, pointModel); fg.addBearingRange(X(1), L(0), Rot2(), 1.0, pointModel);
fg.addBearingRange(kx(2), kl(0), Rot2(), 1.0, pointModel); fg.addBearingRange(X(2), L(0), Rot2(), 1.0, pointModel);
Values init; Values init;
init.insert(kx(0), Pose2(0.0,0.0,0.0)); init.insert(X(0), Pose2(0.0,0.0,0.0));
init.insert(kx(1), Pose2(1.0,0.0,0.0)); init.insert(X(1), Pose2(1.0,0.0,0.0));
init.insert(kx(2), Pose2(2.0,0.0,0.0)); init.insert(X(2), Pose2(2.0,0.0,0.0));
init.insert(kl(0), Point2(1.0,1.0)); init.insert(L(0), Point2(1.0,1.0));
Ordering ordering(*fg.orderingCOLAMD(init)); Ordering ordering(*fg.orderingCOLAMD(init));
FactorGraph<GaussianFactor>::shared_ptr gfg(fg.linearize(init, ordering)); FactorGraph<GaussianFactor>::shared_ptr gfg(fg.linearize(init, ordering));
GaussianMultifrontalSolver solver(*gfg); GaussianMultifrontalSolver solver(*gfg);
solver.marginalFactor(ordering[kl(0)]); solver.marginalFactor(ordering[L(0)]);
} }
/* ************************************************************************* */ /* ************************************************************************* */

96
tests/testNonlinearFactor.cpp
View File

@ -39,8 +39,8 @@ using namespace gtsam;
using namespace example; using namespace example;
// Convenience for named keys // Convenience for named keys
Key kx(size_t i) { return Symbol('x',i); } using symbol_shorthand::X;
Key kl(size_t i) { return Symbol('l',i); } using symbol_shorthand::L;
typedef boost::shared_ptr<NonlinearFactor > shared_nlf; typedef boost::shared_ptr<NonlinearFactor > shared_nlf;
@ -51,11 +51,11 @@ TEST( NonlinearFactor, equals )
// create two nonlinear2 factors // create two nonlinear2 factors
Point2 z3(0.,-1.); Point2 z3(0.,-1.);
simulated2D::Measurement f0(z3, sigma, kx(1),kl(1)); simulated2D::Measurement f0(z3, sigma, X(1),L(1));
// measurement between x2 and l1 // measurement between x2 and l1
Point2 z4(-1.5, -1.); Point2 z4(-1.5, -1.);
simulated2D::Measurement f1(z4, sigma, kx(2),kl(1)); simulated2D::Measurement f1(z4, sigma, X(2),L(1));
CHECK(assert_equal(f0,f0)); CHECK(assert_equal(f0,f0));
CHECK(f0.equals(f0)); CHECK(f0.equals(f0));
@ -201,16 +201,16 @@ TEST( NonlinearFactor, linearize_constraint1 )
SharedDiagonal constraint = noiseModel::Constrained::MixedSigmas(sigmas); SharedDiagonal constraint = noiseModel::Constrained::MixedSigmas(sigmas);
Point2 mu(1., -1.); Point2 mu(1., -1.);
Graph::sharedFactor f0(new simulated2D::Prior(mu, constraint, kx(1))); Graph::sharedFactor f0(new simulated2D::Prior(mu, constraint, X(1)));
Values config; Values config;
config.insert(kx(1), Point2(1.0, 2.0)); config.insert(X(1), Point2(1.0, 2.0));
GaussianFactor::shared_ptr actual = f0->linearize(config, *config.orderingArbitrary()); GaussianFactor::shared_ptr actual = f0->linearize(config, *config.orderingArbitrary());
// create expected // create expected
Ordering ord(*config.orderingArbitrary()); Ordering ord(*config.orderingArbitrary());
Vector b = Vector_(2, 0., -3.); Vector b = Vector_(2, 0., -3.);
JacobianFactor expected(ord[kx(1)], Matrix_(2,2, 5.0, 0.0, 0.0, 1.0), b, constraint); JacobianFactor expected(ord[X(1)], Matrix_(2,2, 5.0, 0.0, 0.0, 1.0), b, constraint);
CHECK(assert_equal((const GaussianFactor&)expected, *actual)); CHECK(assert_equal((const GaussianFactor&)expected, *actual));
} }
@ -221,18 +221,18 @@ TEST( NonlinearFactor, linearize_constraint2 )
SharedDiagonal constraint = noiseModel::Constrained::MixedSigmas(sigmas); SharedDiagonal constraint = noiseModel::Constrained::MixedSigmas(sigmas);
Point2 z3(1.,-1.); Point2 z3(1.,-1.);
simulated2D::Measurement f0(z3, constraint, kx(1),kl(1)); simulated2D::Measurement f0(z3, constraint, X(1),L(1));
Values config; Values config;
config.insert(kx(1), Point2(1.0, 2.0)); config.insert(X(1), Point2(1.0, 2.0));
config.insert(kl(1), Point2(5.0, 4.0)); config.insert(L(1), Point2(5.0, 4.0));
GaussianFactor::shared_ptr actual = f0.linearize(config, *config.orderingArbitrary()); GaussianFactor::shared_ptr actual = f0.linearize(config, *config.orderingArbitrary());
// create expected // create expected
Ordering ord(*config.orderingArbitrary()); Ordering ord(*config.orderingArbitrary());
Matrix A = Matrix_(2,2, 5.0, 0.0, 0.0, 1.0); Matrix A = Matrix_(2,2, 5.0, 0.0, 0.0, 1.0);
Vector b = Vector_(2, -15., -3.); Vector b = Vector_(2, -15., -3.);
JacobianFactor expected(ord[kx(1)], -1*A, ord[kl(1)], A, b, constraint); JacobianFactor expected(ord[X(1)], -1*A, ord[L(1)], A, b, constraint);
CHECK(assert_equal((const GaussianFactor&)expected, *actual)); CHECK(assert_equal((const GaussianFactor&)expected, *actual));
} }
@ -240,7 +240,7 @@ TEST( NonlinearFactor, linearize_constraint2 )
class TestFactor4 : public NoiseModelFactor4<LieVector, LieVector, LieVector, LieVector> { class TestFactor4 : public NoiseModelFactor4<LieVector, LieVector, LieVector, LieVector> {
public: public:
typedef NoiseModelFactor4<LieVector, LieVector, LieVector, LieVector> Base; typedef NoiseModelFactor4<LieVector, LieVector, LieVector, LieVector> Base;
TestFactor4() : Base(sharedSigmas(Vector_(1, 2.0)), kx(1), kx(2), kx(3), kx(4)) {} TestFactor4() : Base(sharedSigmas(Vector_(1, 2.0)), X(1), X(2), X(3), X(4)) {}
virtual Vector virtual Vector
evaluateError(const LieVector& x1, const LieVector& x2, const LieVector& x3, const LieVector& x4, evaluateError(const LieVector& x1, const LieVector& x2, const LieVector& x3, const LieVector& x4,
@ -264,13 +264,13 @@ public:
TEST(NonlinearFactor, NoiseModelFactor4) { TEST(NonlinearFactor, NoiseModelFactor4) {
TestFactor4 tf; TestFactor4 tf;
Values tv; Values tv;
tv.insert(kx(1), LieVector(1, 1.0)); tv.insert(X(1), LieVector(1, 1.0));
tv.insert(kx(2), LieVector(1, 2.0)); tv.insert(X(2), LieVector(1, 2.0));
tv.insert(kx(3), LieVector(1, 3.0)); tv.insert(X(3), LieVector(1, 3.0));
tv.insert(kx(4), LieVector(1, 4.0)); tv.insert(X(4), LieVector(1, 4.0));
EXPECT(assert_equal(Vector_(1, 10.0), tf.unwhitenedError(tv))); EXPECT(assert_equal(Vector_(1, 10.0), tf.unwhitenedError(tv)));
DOUBLES_EQUAL(25.0/2.0, tf.error(tv), 1e-9); DOUBLES_EQUAL(25.0/2.0, tf.error(tv), 1e-9);
Ordering ordering; ordering += kx(1), kx(2), kx(3), kx(4); Ordering ordering; ordering += X(1), X(2), X(3), X(4);
JacobianFactor jf(*boost::dynamic_pointer_cast<JacobianFactor>(tf.linearize(tv, ordering))); JacobianFactor jf(*boost::dynamic_pointer_cast<JacobianFactor>(tf.linearize(tv, ordering)));
LONGS_EQUAL(jf.keys()[0], 0); LONGS_EQUAL(jf.keys()[0], 0);
LONGS_EQUAL(jf.keys()[1], 1); LONGS_EQUAL(jf.keys()[1], 1);
@ -287,7 +287,7 @@ TEST(NonlinearFactor, NoiseModelFactor4) {
class TestFactor5 : public NoiseModelFactor5<LieVector, LieVector, LieVector, LieVector, LieVector> { class TestFactor5 : public NoiseModelFactor5<LieVector, LieVector, LieVector, LieVector, LieVector> {
public: public:
typedef NoiseModelFactor5<LieVector, LieVector, LieVector, LieVector, LieVector> Base; typedef NoiseModelFactor5<LieVector, LieVector, LieVector, LieVector, LieVector> Base;
TestFactor5() : Base(sharedSigmas(Vector_(1, 2.0)), kx(1), kx(2), kx(3), kx(4), kx(5)) {} TestFactor5() : Base(sharedSigmas(Vector_(1, 2.0)), X(1), X(2), X(3), X(4), X(5)) {}
virtual Vector virtual Vector
evaluateError(const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, evaluateError(const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5,
@ -313,14 +313,14 @@ public:
TEST(NonlinearFactor, NoiseModelFactor5) { TEST(NonlinearFactor, NoiseModelFactor5) {
TestFactor5 tf; TestFactor5 tf;
Values tv; Values tv;
tv.insert(kx(1), LieVector(1, 1.0)); tv.insert(X(1), LieVector(1, 1.0));
tv.insert(kx(2), LieVector(1, 2.0)); tv.insert(X(2), LieVector(1, 2.0));
tv.insert(kx(3), LieVector(1, 3.0)); tv.insert(X(3), LieVector(1, 3.0));
tv.insert(kx(4), LieVector(1, 4.0)); tv.insert(X(4), LieVector(1, 4.0));
tv.insert(kx(5), LieVector(1, 5.0)); tv.insert(X(5), LieVector(1, 5.0));
EXPECT(assert_equal(Vector_(1, 15.0), tf.unwhitenedError(tv))); EXPECT(assert_equal(Vector_(1, 15.0), tf.unwhitenedError(tv)));
DOUBLES_EQUAL(56.25/2.0, tf.error(tv), 1e-9); DOUBLES_EQUAL(56.25/2.0, tf.error(tv), 1e-9);
Ordering ordering; ordering += kx(1), kx(2), kx(3), kx(4), kx(5); Ordering ordering; ordering += X(1), X(2), X(3), X(4), X(5);
JacobianFactor jf(*boost::dynamic_pointer_cast<JacobianFactor>(tf.linearize(tv, ordering))); JacobianFactor jf(*boost::dynamic_pointer_cast<JacobianFactor>(tf.linearize(tv, ordering)));
LONGS_EQUAL(jf.keys()[0], 0); LONGS_EQUAL(jf.keys()[0], 0);
LONGS_EQUAL(jf.keys()[1], 1); LONGS_EQUAL(jf.keys()[1], 1);
@ -339,7 +339,7 @@ TEST(NonlinearFactor, NoiseModelFactor5) {
class TestFactor6 : public NoiseModelFactor6<LieVector, LieVector, LieVector, LieVector, LieVector, LieVector> { class TestFactor6 : public NoiseModelFactor6<LieVector, LieVector, LieVector, LieVector, LieVector, LieVector> {
public: public:
typedef NoiseModelFactor6<LieVector, LieVector, LieVector, LieVector, LieVector, LieVector> Base; typedef NoiseModelFactor6<LieVector, LieVector, LieVector, LieVector, LieVector, LieVector> Base;
TestFactor6() : Base(sharedSigmas(Vector_(1, 2.0)), kx(1), kx(2), kx(3), kx(4), kx(5), kx(6)) {} TestFactor6() : Base(sharedSigmas(Vector_(1, 2.0)), X(1), X(2), X(3), X(4), X(5), X(6)) {}
virtual Vector virtual Vector
evaluateError(const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, evaluateError(const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6,
@ -367,15 +367,15 @@ public:
TEST(NonlinearFactor, NoiseModelFactor6) { TEST(NonlinearFactor, NoiseModelFactor6) {
TestFactor6 tf; TestFactor6 tf;
Values tv; Values tv;
tv.insert(kx(1), LieVector(1, 1.0)); tv.insert(X(1), LieVector(1, 1.0));
tv.insert(kx(2), LieVector(1, 2.0)); tv.insert(X(2), LieVector(1, 2.0));
tv.insert(kx(3), LieVector(1, 3.0)); tv.insert(X(3), LieVector(1, 3.0));
tv.insert(kx(4), LieVector(1, 4.0)); tv.insert(X(4), LieVector(1, 4.0));
tv.insert(kx(5), LieVector(1, 5.0)); tv.insert(X(5), LieVector(1, 5.0));
tv.insert(kx(6), LieVector(1, 6.0)); tv.insert(X(6), LieVector(1, 6.0));
EXPECT(assert_equal(Vector_(1, 21.0), tf.unwhitenedError(tv))); EXPECT(assert_equal(Vector_(1, 21.0), tf.unwhitenedError(tv)));
DOUBLES_EQUAL(110.25/2.0, tf.error(tv), 1e-9); DOUBLES_EQUAL(110.25/2.0, tf.error(tv), 1e-9);
Ordering ordering; ordering += kx(1), kx(2), kx(3), kx(4), kx(5), kx(6); Ordering ordering; ordering += X(1), X(2), X(3), X(4), X(5), X(6);
JacobianFactor jf(*boost::dynamic_pointer_cast<JacobianFactor>(tf.linearize(tv, ordering))); JacobianFactor jf(*boost::dynamic_pointer_cast<JacobianFactor>(tf.linearize(tv, ordering)));
LONGS_EQUAL(jf.keys()[0], 0); LONGS_EQUAL(jf.keys()[0], 0);
LONGS_EQUAL(jf.keys()[1], 1); LONGS_EQUAL(jf.keys()[1], 1);
@ -397,10 +397,10 @@ TEST(NonlinearFactor, NoiseModelFactor6) {
TEST( NonlinearFactor, clone_rekey ) TEST( NonlinearFactor, clone_rekey )
{ {
shared_nlf init(new TestFactor4()); shared_nlf init(new TestFactor4());
EXPECT_LONGS_EQUAL(kx(1), init->keys()[0]); EXPECT_LONGS_EQUAL(X(1), init->keys()[0]);
EXPECT_LONGS_EQUAL(kx(2), init->keys()[1]); EXPECT_LONGS_EQUAL(X(2), init->keys()[1]);
EXPECT_LONGS_EQUAL(kx(3), init->keys()[2]); EXPECT_LONGS_EQUAL(X(3), init->keys()[2]);
EXPECT_LONGS_EQUAL(kx(4), init->keys()[3]); EXPECT_LONGS_EQUAL(X(4), init->keys()[3]);
// Standard clone // Standard clone
shared_nlf actClone = init->clone(); shared_nlf actClone = init->clone();
@ -409,24 +409,24 @@ TEST( NonlinearFactor, clone_rekey )
// Re-key factor - clones with different keys // Re-key factor - clones with different keys
std::vector<Key> new_keys(4); std::vector<Key> new_keys(4);
new_keys[0] = kx(5); new_keys[0] = X(5);
new_keys[1] = kx(6); new_keys[1] = X(6);
new_keys[2] = kx(7); new_keys[2] = X(7);
new_keys[3] = kx(8); new_keys[3] = X(8);
shared_nlf actRekey = init->rekey(new_keys); shared_nlf actRekey = init->rekey(new_keys);
EXPECT(actRekey.get() != init.get()); // Ensure different pointers EXPECT(actRekey.get() != init.get()); // Ensure different pointers
// Ensure init is unchanged // Ensure init is unchanged
EXPECT_LONGS_EQUAL(kx(1), init->keys()[0]); EXPECT_LONGS_EQUAL(X(1), init->keys()[0]);
EXPECT_LONGS_EQUAL(kx(2), init->keys()[1]); EXPECT_LONGS_EQUAL(X(2), init->keys()[1]);
EXPECT_LONGS_EQUAL(kx(3), init->keys()[2]); EXPECT_LONGS_EQUAL(X(3), init->keys()[2]);
EXPECT_LONGS_EQUAL(kx(4), init->keys()[3]); EXPECT_LONGS_EQUAL(X(4), init->keys()[3]);
// Check new keys // Check new keys
EXPECT_LONGS_EQUAL(kx(5), actRekey->keys()[0]); EXPECT_LONGS_EQUAL(X(5), actRekey->keys()[0]);
EXPECT_LONGS_EQUAL(kx(6), actRekey->keys()[1]); EXPECT_LONGS_EQUAL(X(6), actRekey->keys()[1]);
EXPECT_LONGS_EQUAL(kx(7), actRekey->keys()[2]); EXPECT_LONGS_EQUAL(X(7), actRekey->keys()[2]);
EXPECT_LONGS_EQUAL(kx(8), actRekey->keys()[3]); EXPECT_LONGS_EQUAL(X(8), actRekey->keys()[3]);
} }
/* ************************************************************************* */ /* ************************************************************************* */

18
tests/testNonlinearFactorGraph.cpp
View File

@ -37,8 +37,8 @@ using namespace boost::assign;
using namespace gtsam; using namespace gtsam;
using namespace example; using namespace example;
Key kx(size_t i) { return Symbol('x',i); } using symbol_shorthand::X;
Key kl(size_t i) { return Symbol('l',i); } using symbol_shorthand::L;
/* ************************************************************************* */ /* ************************************************************************* */
TEST( Graph, equals ) TEST( Graph, equals )
@ -68,16 +68,16 @@ TEST( Graph, keys )
set<Key> actual = fg.keys(); set<Key> actual = fg.keys();
LONGS_EQUAL(3, actual.size()); LONGS_EQUAL(3, actual.size());
set<Key>::const_iterator it = actual.begin(); set<Key>::const_iterator it = actual.begin();
LONGS_EQUAL(kl(1), *(it++)); LONGS_EQUAL(L(1), *(it++));
LONGS_EQUAL(kx(1), *(it++)); LONGS_EQUAL(X(1), *(it++));
LONGS_EQUAL(kx(2), *(it++)); LONGS_EQUAL(X(2), *(it++));
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( Graph, GET_ORDERING) TEST( Graph, GET_ORDERING)
{ {
// Ordering expected; expected += "x1","l1","x2"; // For starting with x1,x2,l1 // Ordering expected; expected += "x1","l1","x2"; // For starting with x1,x2,l1
Ordering expected; expected += kl(1), kx(2), kx(1); // For starting with l1,x1,x2 Ordering expected; expected += L(1), X(2), X(1); // For starting with l1,x1,x2
Graph nlfg = createNonlinearFactorGraph(); Graph nlfg = createNonlinearFactorGraph();
SymbolicFactorGraph::shared_ptr symbolic; SymbolicFactorGraph::shared_ptr symbolic;
Ordering::shared_ptr ordering; Ordering::shared_ptr ordering;
@ -123,7 +123,7 @@ TEST( Graph, rekey )
{ {
Graph init = createNonlinearFactorGraph(); Graph init = createNonlinearFactorGraph();
map<Key,Key> rekey_mapping; map<Key,Key> rekey_mapping;
rekey_mapping.insert(make_pair(kl(1), kl(4))); rekey_mapping.insert(make_pair(L(1), L(4)));
Graph actRekey = init.rekey(rekey_mapping); Graph actRekey = init.rekey(rekey_mapping);
// ensure deep clone // ensure deep clone
@ -139,8 +139,8 @@ TEST( Graph, rekey )
// updated measurements // updated measurements
Point2 z3(0, -1), z4(-1.5, -1.); Point2 z3(0, -1), z4(-1.5, -1.);
SharedDiagonal sigma0_2 = noiseModel::Isotropic::Sigma(2,0.2); SharedDiagonal sigma0_2 = noiseModel::Isotropic::Sigma(2,0.2);
expRekey.add(simulated2D::Measurement(z3, sigma0_2, kx(1), kl(4))); expRekey.add(simulated2D::Measurement(z3, sigma0_2, X(1), L(4)));
expRekey.add(simulated2D::Measurement(z4, sigma0_2, kx(2), kl(4))); expRekey.add(simulated2D::Measurement(z4, sigma0_2, X(2), L(4)));
EXPECT(assert_equal(expRekey, actRekey)); EXPECT(assert_equal(expRekey, actRekey));
} }

42
tests/testNonlinearOptimizer.cpp
View File

@ -39,8 +39,8 @@ using namespace gtsam;
const double tol = 1e-5; const double tol = 1e-5;
Key kx(size_t i) { return Symbol('x',i); } using symbol_shorthand::X;
Key kl(size_t i) { return Symbol('l',i); } using symbol_shorthand::L;
/* ************************************************************************* */ /* ************************************************************************* */
TEST( NonlinearOptimizer, iterateLM ) TEST( NonlinearOptimizer, iterateLM )
@ -51,7 +51,7 @@ TEST( NonlinearOptimizer, iterateLM )
// config far from minimum // config far from minimum
Point2 x0(3,0); Point2 x0(3,0);
Values config; Values config;
config.insert(kx(1), x0); config.insert(X(1), x0);
// normal iterate // normal iterate
GaussNewtonParams gnParams; GaussNewtonParams gnParams;
@ -75,18 +75,18 @@ TEST( NonlinearOptimizer, optimize )
// test error at minimum // test error at minimum
Point2 xstar(0,0); Point2 xstar(0,0);
Values cstar; Values cstar;
cstar.insert(kx(1), xstar); cstar.insert(X(1), xstar);
DOUBLES_EQUAL(0.0,fg.error(cstar),0.0); DOUBLES_EQUAL(0.0,fg.error(cstar),0.0);
// test error at initial = [(1-cos(3))^2 + (sin(3))^2]*50 = // test error at initial = [(1-cos(3))^2 + (sin(3))^2]*50 =
Point2 x0(3,3); Point2 x0(3,3);
Values c0; Values c0;
c0.insert(kx(1), x0); c0.insert(X(1), x0);
DOUBLES_EQUAL(199.0,fg.error(c0),1e-3); DOUBLES_EQUAL(199.0,fg.error(c0),1e-3);
// optimize parameters // optimize parameters
Ordering ord; Ordering ord;
ord.push_back(kx(1)); ord.push_back(X(1));
// Gauss-Newton // Gauss-Newton
GaussNewtonParams gnParams; GaussNewtonParams gnParams;
@ -114,7 +114,7 @@ TEST( NonlinearOptimizer, SimpleLMOptimizer )
Point2 x0(3,3); Point2 x0(3,3);
Values c0; Values c0;
c0.insert(kx(1), x0); c0.insert(X(1), x0);
Values actual = LevenbergMarquardtOptimizer(fg, c0).optimize(); Values actual = LevenbergMarquardtOptimizer(fg, c0).optimize();
DOUBLES_EQUAL(0,fg.error(actual),tol); DOUBLES_EQUAL(0,fg.error(actual),tol);
@ -127,7 +127,7 @@ TEST( NonlinearOptimizer, SimpleGNOptimizer )
Point2 x0(3,3); Point2 x0(3,3);
Values c0; Values c0;
c0.insert(kx(1), x0); c0.insert(X(1), x0);
Values actual = GaussNewtonOptimizer(fg, c0).optimize(); Values actual = GaussNewtonOptimizer(fg, c0).optimize();
DOUBLES_EQUAL(0,fg.error(actual),tol); DOUBLES_EQUAL(0,fg.error(actual),tol);
@ -140,7 +140,7 @@ TEST( NonlinearOptimizer, SimpleDLOptimizer )
Point2 x0(3,3); Point2 x0(3,3);
Values c0; Values c0;
c0.insert(kx(1), x0); c0.insert(X(1), x0);
Values actual = DoglegOptimizer(fg, c0).optimize(); Values actual = DoglegOptimizer(fg, c0).optimize();
DOUBLES_EQUAL(0,fg.error(actual),tol); DOUBLES_EQUAL(0,fg.error(actual),tol);
@ -158,7 +158,7 @@ TEST( NonlinearOptimizer, optimization_method )
Point2 x0(3,3); Point2 x0(3,3);
Values c0; Values c0;
c0.insert(kx(1), x0); c0.insert(X(1), x0);
Values actualMFQR = LevenbergMarquardtOptimizer(fg, c0, paramsQR).optimize(); Values actualMFQR = LevenbergMarquardtOptimizer(fg, c0, paramsQR).optimize();
DOUBLES_EQUAL(0,fg.error(actualMFQR),tol); DOUBLES_EQUAL(0,fg.error(actualMFQR),tol);
@ -171,23 +171,23 @@ TEST( NonlinearOptimizer, optimization_method )
TEST( NonlinearOptimizer, Factorization ) TEST( NonlinearOptimizer, Factorization )
{ {
Values config; Values config;
config.insert(kx(1), Pose2(0.,0.,0.)); config.insert(X(1), Pose2(0.,0.,0.));
config.insert(kx(2), Pose2(1.5,0.,0.)); config.insert(X(2), Pose2(1.5,0.,0.));
pose2SLAM::Graph graph; pose2SLAM::Graph graph;
graph.addPrior(kx(1), Pose2(0.,0.,0.), noiseModel::Isotropic::Sigma(3, 1e-10)); graph.addPrior(X(1), Pose2(0.,0.,0.), noiseModel::Isotropic::Sigma(3, 1e-10));
graph.addOdometry(kx(1),kx(2), Pose2(1.,0.,0.), noiseModel::Isotropic::Sigma(3, 1)); graph.addOdometry(X(1),X(2), Pose2(1.,0.,0.), noiseModel::Isotropic::Sigma(3, 1));
Ordering ordering; Ordering ordering;
ordering.push_back(kx(1)); ordering.push_back(X(1));
ordering.push_back(kx(2)); ordering.push_back(X(2));
LevenbergMarquardtOptimizer optimizer(graph, config, ordering); LevenbergMarquardtOptimizer optimizer(graph, config, ordering);
optimizer.iterate(); optimizer.iterate();
Values expected; Values expected;
expected.insert(kx(1), Pose2(0.,0.,0.)); expected.insert(X(1), Pose2(0.,0.,0.));
expected.insert(kx(2), Pose2(1.,0.,0.)); expected.insert(X(2), Pose2(1.,0.,0.));
CHECK(assert_equal(expected, optimizer.values(), 1e-5)); CHECK(assert_equal(expected, optimizer.values(), 1e-5));
} }
@ -202,18 +202,18 @@ TEST(NonlinearOptimizer, NullFactor) {
// test error at minimum // test error at minimum
Point2 xstar(0,0); Point2 xstar(0,0);
Values cstar; Values cstar;
cstar.insert(kx(1), xstar); cstar.insert(X(1), xstar);
DOUBLES_EQUAL(0.0,fg.error(cstar),0.0); DOUBLES_EQUAL(0.0,fg.error(cstar),0.0);
// test error at initial = [(1-cos(3))^2 + (sin(3))^2]*50 = // test error at initial = [(1-cos(3))^2 + (sin(3))^2]*50 =
Point2 x0(3,3); Point2 x0(3,3);
Values c0; Values c0;
c0.insert(kx(1), x0); c0.insert(X(1), x0);
DOUBLES_EQUAL(199.0,fg.error(c0),1e-3); DOUBLES_EQUAL(199.0,fg.error(c0),1e-3);
// optimize parameters // optimize parameters
Ordering ord; Ordering ord;
ord.push_back(kx(1)); ord.push_back(X(1));
// Gauss-Newton // Gauss-Newton
Values actual1 = GaussNewtonOptimizer(fg, c0, ord).optimize(); Values actual1 = GaussNewtonOptimizer(fg, c0, ord).optimize();

12
tests/testSymbolicBayesNetB.cpp
View File

@ -32,18 +32,18 @@ using namespace std;
using namespace gtsam; using namespace gtsam;
using namespace example; using namespace example;
Key kx(size_t i) { return Symbol('x',i); } using symbol_shorthand::X;
Key kl(size_t i) { return Symbol('l',i); } using symbol_shorthand::L;
/* ************************************************************************* */ /* ************************************************************************* */
TEST( SymbolicBayesNet, constructor ) TEST( SymbolicBayesNet, constructor )
{ {
Ordering o; o += kx(2),kl(1),kx(1); Ordering o; o += X(2),L(1),X(1);
// Create manually // Create manually
IndexConditional::shared_ptr IndexConditional::shared_ptr
x2(new IndexConditional(o[kx(2)],o[kl(1)], o[kx(1)])), x2(new IndexConditional(o[X(2)],o[L(1)], o[X(1)])),
l1(new IndexConditional(o[kl(1)],o[kx(1)])), l1(new IndexConditional(o[L(1)],o[X(1)])),
x1(new IndexConditional(o[kx(1)])); x1(new IndexConditional(o[X(1)]));
BayesNet<IndexConditional> expected; BayesNet<IndexConditional> expected;
expected.push_back(x2); expected.push_back(x2);
expected.push_back(l1); expected.push_back(l1);

38
tests/testSymbolicFactorGraphB.cpp
View File

@ -30,19 +30,19 @@ using namespace boost::assign;
using namespace std; using namespace std;
using namespace gtsam; using namespace gtsam;
Key kx(size_t i) { return Symbol('x',i); } using symbol_shorthand::X;
Key kl(size_t i) { return Symbol('l',i); } using symbol_shorthand::L;
/* ************************************************************************* */ /* ************************************************************************* */
TEST( SymbolicFactorGraph, symbolicFactorGraph ) TEST( SymbolicFactorGraph, symbolicFactorGraph )
{ {
Ordering o; o += kx(1),kl(1),kx(2); Ordering o; o += X(1),L(1),X(2);
// construct expected symbolic graph // construct expected symbolic graph
SymbolicFactorGraph expected; SymbolicFactorGraph expected;
expected.push_factor(o[kx(1)]); expected.push_factor(o[X(1)]);
expected.push_factor(o[kx(1)],o[kx(2)]); expected.push_factor(o[X(1)],o[X(2)]);
expected.push_factor(o[kx(1)],o[kl(1)]); expected.push_factor(o[X(1)],o[L(1)]);
expected.push_factor(o[kx(2)],o[kl(1)]); expected.push_factor(o[X(2)],o[L(1)]);
// construct it from the factor graph // construct it from the factor graph
GaussianFactorGraph factorGraph = example::createGaussianFactorGraph(o); GaussianFactorGraph factorGraph = example::createGaussianFactorGraph(o);
@ -59,7 +59,7 @@ TEST( SymbolicFactorGraph, symbolicFactorGraph )
// SymbolicFactorGraph actual(factorGraph); // SymbolicFactorGraph actual(factorGraph);
// SymbolicFactor::shared_ptr f1 = actual[0]; // SymbolicFactor::shared_ptr f1 = actual[0];
// SymbolicFactor::shared_ptr f3 = actual[2]; // SymbolicFactor::shared_ptr f3 = actual[2];
// actual.findAndRemoveFactors(kx(2)); // actual.findAndRemoveFactors(X(2));
// //
// // construct expected graph after find_factors_and_remove // // construct expected graph after find_factors_and_remove
// SymbolicFactorGraph expected; // SymbolicFactorGraph expected;
@ -79,13 +79,13 @@ TEST( SymbolicFactorGraph, symbolicFactorGraph )
// SymbolicFactorGraph fg(factorGraph); // SymbolicFactorGraph fg(factorGraph);
// //
// // ask for all factor indices connected to x1 // // ask for all factor indices connected to x1
// list<size_t> x1_factors = fg.factors(kx(1)); // list<size_t> x1_factors = fg.factors(X(1));
// int x1_indices[] = { 0, 1, 2 }; // int x1_indices[] = { 0, 1, 2 };
// list<size_t> x1_expected(x1_indices, x1_indices + 3); // list<size_t> x1_expected(x1_indices, x1_indices + 3);
// CHECK(x1_factors==x1_expected); // CHECK(x1_factors==x1_expected);
// //
// // ask for all factor indices connected to x2 // // ask for all factor indices connected to x2
// list<size_t> x2_factors = fg.factors(kx(2)); // list<size_t> x2_factors = fg.factors(X(2));
// int x2_indices[] = { 1, 3 }; // int x2_indices[] = { 1, 3 };
// list<size_t> x2_expected(x2_indices, x2_indices + 2); // list<size_t> x2_expected(x2_indices, x2_indices + 2);
// CHECK(x2_factors==x2_expected); // CHECK(x2_factors==x2_expected);
@ -99,26 +99,26 @@ TEST( SymbolicFactorGraph, symbolicFactorGraph )
// SymbolicFactorGraph fg(factorGraph); // SymbolicFactorGraph fg(factorGraph);
// //
// // combine all factors connected to x1 // // combine all factors connected to x1
// SymbolicFactor::shared_ptr actual = removeAndCombineFactors(fg,kx(1)); // SymbolicFactor::shared_ptr actual = removeAndCombineFactors(fg,X(1));
// //
// // check result // // check result
// SymbolicFactor expected(kl(1),kx(1),kx(2)); // SymbolicFactor expected(L(1),X(1),X(2));
// CHECK(assert_equal(expected,*actual)); // CHECK(assert_equal(expected,*actual));
//} //}
///* ************************************************************************* */ ///* ************************************************************************* */
//TEST( SymbolicFactorGraph, eliminateOne ) //TEST( SymbolicFactorGraph, eliminateOne )
//{ //{
// Ordering o; o += kx(1),kl(1),kx(2); // Ordering o; o += X(1),L(1),X(2);
// // create a test graph // // create a test graph
// GaussianFactorGraph factorGraph = example::createGaussianFactorGraph(o); // GaussianFactorGraph factorGraph = example::createGaussianFactorGraph(o);
// SymbolicFactorGraph fg(factorGraph); // SymbolicFactorGraph fg(factorGraph);
// //
// // eliminate // // eliminate
// IndexConditional::shared_ptr actual = GaussianSequentialSolver::EliminateUntil(fg, o[kx(1)]+1); // IndexConditional::shared_ptr actual = GaussianSequentialSolver::EliminateUntil(fg, o[X(1)]+1);
// //
// // create expected symbolic IndexConditional // // create expected symbolic IndexConditional
// IndexConditional expected(o[kx(1)],o[kl(1)],o[kx(2)]); // IndexConditional expected(o[X(1)],o[L(1)],o[X(2)]);
// //
// CHECK(assert_equal(expected,*actual)); // CHECK(assert_equal(expected,*actual));
//} //}
@ -126,12 +126,12 @@ TEST( SymbolicFactorGraph, symbolicFactorGraph )
/* ************************************************************************* */ /* ************************************************************************* */
TEST( SymbolicFactorGraph, eliminate ) TEST( SymbolicFactorGraph, eliminate )
{ {
Ordering o; o += kx(2),kl(1),kx(1); Ordering o; o += X(2),L(1),X(1);
// create expected Chordal bayes Net // create expected Chordal bayes Net
IndexConditional::shared_ptr x2(new IndexConditional(o[kx(2)], o[kl(1)], o[kx(1)])); IndexConditional::shared_ptr x2(new IndexConditional(o[X(2)], o[L(1)], o[X(1)]));
IndexConditional::shared_ptr l1(new IndexConditional(o[kl(1)], o[kx(1)])); IndexConditional::shared_ptr l1(new IndexConditional(o[L(1)], o[X(1)]));
IndexConditional::shared_ptr x1(new IndexConditional(o[kx(1)])); IndexConditional::shared_ptr x1(new IndexConditional(o[X(1)]));
SymbolicBayesNet expected; SymbolicBayesNet expected;
expected.push_back(x2); expected.push_back(x2);