diff --git a/examples/vSLAMexample/vISAMexample.cpp b/examples/vSLAMexample/vISAMexample.cpp index d23ea9b10..16c250953 100644 --- a/examples/vSLAMexample/vISAMexample.cpp +++ b/examples/vSLAMexample/vISAMexample.cpp @@ -32,6 +32,10 @@ using boost::shared_ptr; using namespace std; using namespace gtsam; +// Convenience for named keys +using symbol_shorthand::X; +using symbol_shorthand::L; + /* ************************************************************************* */ #define CALIB_FILE "calib.txt" #define LANDMARKS_FILE "landmarks.txt" @@ -83,22 +87,22 @@ void createNewFactors(shared_ptr& newFactors, boost::shared_p newFactors->addMeasurement( measurements[i].m_p, measurementSigma, - Symbol('x',pose_id), - Symbol('l',measurements[i].m_idLandmark), + X(pose_id), + L(measurements[i].m_idLandmark), calib); } // ... we need priors on the new pose and all new landmarks newFactors->addPosePrior(pose_id, pose, poseSigma); 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 initialValues = shared_ptr (new Values()); - initialValues->insert(Symbol('x',pose_id), pose); + initialValues->insert(X(pose_id), pose); 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]); } } diff --git a/examples/vSLAMexample/vSFMexample.cpp b/examples/vSLAMexample/vSFMexample.cpp index be41ad831..66e9a475b 100644 --- a/examples/vSLAMexample/vSFMexample.cpp +++ b/examples/vSLAMexample/vSFMexample.cpp @@ -30,6 +30,10 @@ using boost::shared_ptr; using namespace std; using namespace gtsam; +// Convenience for named keys +using symbol_shorthand::X; +using symbol_shorthand::L; + /* ************************************************************************* */ #define CALIB_FILE "calib.txt" #define LANDMARKS_FILE "landmarks.txt" @@ -84,8 +88,8 @@ visualSLAM::Graph setupGraph(std::vector& measurements, SharedNoiseMo g.addMeasurement( measurements[i].m_p, measurementSigma, - Symbol('x',measurements[i].m_idCamera), - Symbol('l',measurements[i].m_idLandmark), + X(measurements[i].m_idCamera), + L(measurements[i].m_idLandmark), calib); } @@ -103,11 +107,11 @@ Values initialize(std::map landmarks, std::map poses) { // Initialize landmarks 3D positions. for (map::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. for (map::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; } @@ -137,7 +141,7 @@ int main(int argc, char* argv[]) { // Add prior factor for all poses in the graph map::iterator poseit = g_poses.begin(); 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 cout << "*******************************************************" << endl; diff --git a/gtsam/nonlinear/tests/testValues.cpp b/gtsam/nonlinear/tests/testValues.cpp index f5caa846b..3818e99a2 100644 --- a/gtsam/nonlinear/tests/testValues.cpp +++ b/gtsam/nonlinear/tests/testValues.cpp @@ -32,6 +32,10 @@ using namespace gtsam; using namespace std; static double inf = std::numeric_limits::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); /* ************************************************************************* */ @@ -333,9 +337,9 @@ TEST(Values, Symbol_filter) { Pose3 pose3(Pose2(0.3, 0.7, 0.9)); Values values; - values.insert(Symbol('x',0), pose0); + values.insert(X(0), pose0); values.insert(Symbol('y',1), pose1); - values.insert(Symbol('x',2), pose2); + values.insert(X(2), pose2); values.insert(Symbol('y',3), pose3); int i = 0; diff --git a/gtsam/slam/smallExample.cpp b/gtsam/slam/smallExample.cpp index b0e709a80..5b0ee90dd 100644 --- a/gtsam/slam/smallExample.cpp +++ b/gtsam/slam/smallExample.cpp @@ -47,8 +47,8 @@ namespace example { static const Index _x_=0, _y_=1, _z_=2; // Convenience for named keys - Key kx(size_t i) { return Symbol('x',i); } - Key kl(size_t i) { return Symbol('l',i); } + using symbol_shorthand::X; + using symbol_shorthand::L; /* ************************************************************************* */ boost::shared_ptr sharedNonlinearFactorGraph() { @@ -58,22 +58,22 @@ namespace example { // prior on x1 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); // odometry between x1 and x2 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); // measurement between x1 and l1 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); // measurement between x2 and l1 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); return nlfg; @@ -87,9 +87,9 @@ namespace example { /* ************************************************************************* */ Values createValues() { Values c; - c.insert(kx(1), Point2(0.0, 0.0)); - c.insert(kx(2), Point2(1.5, 0.0)); - c.insert(kl(1), Point2(0.0, -1.0)); + c.insert(X(1), Point2(0.0, 0.0)); + c.insert(X(2), Point2(1.5, 0.0)); + c.insert(L(1), Point2(0.0, -1.0)); return c; } @@ -105,9 +105,9 @@ namespace example { /* ************************************************************************* */ boost::shared_ptr sharedNoisyValues() { boost::shared_ptr c(new Values); - c->insert(kx(1), Point2(0.1, 0.1)); - c->insert(kx(2), Point2(1.4, 0.2)); - c->insert(kl(1), Point2(0.1, -1.1)); + c->insert(X(1), Point2(0.1, 0.1)); + c->insert(X(2), Point2(1.4, 0.2)); + c->insert(L(1), Point2(0.1, -1.1)); return c; } @@ -119,18 +119,18 @@ namespace example { /* ************************************************************************* */ VectorValues createCorrectDelta(const Ordering& ordering) { VectorValues c(vector(3,2)); - c[ordering[kl(1)]] = Vector_(2, -0.1, 0.1); - c[ordering[kx(1)]] = Vector_(2, -0.1, -0.1); - c[ordering[kx(2)]] = Vector_(2, 0.1, -0.2); + c[ordering[L(1)]] = Vector_(2, -0.1, 0.1); + c[ordering[X(1)]] = Vector_(2, -0.1, -0.1); + c[ordering[X(2)]] = Vector_(2, 0.1, -0.2); return c; } /* ************************************************************************* */ VectorValues createZeroDelta(const Ordering& ordering) { VectorValues c(vector(3,2)); - c[ordering[kl(1)]] = zero(2); - c[ordering[kx(1)]] = zero(2); - c[ordering[kx(2)]] = zero(2); + c[ordering[L(1)]] = zero(2); + c[ordering[X(1)]] = zero(2); + c[ordering[X(2)]] = zero(2); return c; } @@ -142,16 +142,16 @@ namespace example { SharedDiagonal unit2 = noiseModel::Unit::Create(2); // 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] - 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] - 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] - 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 >(); } @@ -221,7 +221,7 @@ namespace example { Vector z = Vector_(2, 1.0, 0.0); double sigma = 0.1; boost::shared_ptr 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); return fg; } @@ -239,23 +239,23 @@ namespace example { // prior on x1 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); - poses.insert(kx(1), x1); + poses.insert(X(1), x1); for (int t = 2; t <= T; t++) { // odometry between x_t and x_{t-1} 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); // measurement on x_t is like perfect GPS 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); // initial estimate - poses.insert(kx(t), xt); + poses.insert(X(t), xt); } return make_pair(nlfg, poses); @@ -416,7 +416,7 @@ namespace example { /* ************************************************************************* */ // Create key for simulated planar graph Symbol key(int x, int y) { - return kx(1000*x+y); + return X(1000*x+y); } /* ************************************************************************* */ diff --git a/gtsam/slam/tests/testGeneralSFMFactor.cpp b/gtsam/slam/tests/testGeneralSFMFactor.cpp index f9911ce7d..1b924aa55 100644 --- a/gtsam/slam/tests/testGeneralSFMFactor.cpp +++ b/gtsam/slam/tests/testGeneralSFMFactor.cpp @@ -38,6 +38,10 @@ using namespace boost; using namespace std; using namespace gtsam; +// Convenience for named keys +using symbol_shorthand::X; +using symbol_shorthand::L; + typedef PinholeCamera GeneralCamera; typedef GeneralSFMFactor Projection; typedef NonlinearEquality CameraConstraint; @@ -46,16 +50,16 @@ typedef NonlinearEquality Point3Constraint; class Graph: public NonlinearFactorGraph { public: void addMeasurement(int i, int j, const Point2& z, const SharedNoiseModel& model) { - push_back(boost::make_shared(z, model, Symbol('x',i), Symbol('l',j))); + push_back(boost::make_shared(z, model, X(i), L(j))); } void addCameraConstraint(int j, const GeneralCamera& p) { - boost::shared_ptr factor(new CameraConstraint(Symbol('x',j), p)); + boost::shared_ptr factor(new CameraConstraint(X(j), p)); push_back(factor); } void addPoint3Constraint(int j, const Point3& p) { - boost::shared_ptr factor(new Point3Constraint(Symbol('l',j), p)); + boost::shared_ptr factor(new Point3Constraint(L(j), p)); push_back(factor); } @@ -98,14 +102,14 @@ TEST( GeneralSFMFactor, equals ) TEST( GeneralSFMFactor, error ) { Point2 z(3.,0.); const SharedNoiseModel sigma(noiseModel::Unit::Create(1)); - boost::shared_ptr factor(new Projection(z, sigma, Symbol('x',1), Symbol('l',1))); + boost::shared_ptr factor(new Projection(z, sigma, X(1), L(1))); // For the following configuration, the factor predicts 320,240 Values values; Rot3 R; Point3 t1(0,0,-6); Pose3 x1(R,t1); - values.insert(Symbol('x',1), GeneralCamera(x1)); - Point3 l1; values.insert(Symbol('l',1), l1); + values.insert(X(1), GeneralCamera(x1)); + Point3 l1; values.insert(L(1), l1); EXPECT(assert_equal(Vector_(2, -3.0, 0.0), factor->unwhitenedError(values))); } @@ -114,20 +118,20 @@ static const double baseline = 5.0 ; /* ************************************************************************* */ vector genPoint3() { const double z = 5; - vector L ; - L.push_back(Point3 (-1.0,-1.0, z)); - L.push_back(Point3 (-1.0, 1.0, z)); - L.push_back(Point3 ( 1.0, 1.0, z)); - L.push_back(Point3 ( 1.0,-1.0, z)); - L.push_back(Point3 (-1.5,-1.5, 1.5*z)); - L.push_back(Point3 (-1.5, 1.5, 1.5*z)); - L.push_back(Point3 ( 1.5, 1.5, 1.5*z)); - L.push_back(Point3 ( 1.5,-1.5, 1.5*z)); - L.push_back(Point3 (-2.0,-2.0, 2*z)); - L.push_back(Point3 (-2.0, 2.0, 2*z)); - L.push_back(Point3 ( 2.0, 2.0, 2*z)); - L.push_back(Point3 ( 2.0,-2.0, 2*z)); - return L ; + vector landmarks ; + landmarks.push_back(Point3 (-1.0,-1.0, z)); + landmarks.push_back(Point3 (-1.0, 1.0, z)); + landmarks.push_back(Point3 ( 1.0, 1.0, z)); + landmarks.push_back(Point3 ( 1.0,-1.0, z)); + landmarks.push_back(Point3 (-1.5,-1.5, 1.5*z)); + landmarks.push_back(Point3 (-1.5, 1.5, 1.5*z)); + landmarks.push_back(Point3 ( 1.5, 1.5, 1.5*z)); + landmarks.push_back(Point3 ( 1.5,-1.5, 1.5*z)); + landmarks.push_back(Point3 (-2.0,-2.0, 2*z)); + landmarks.push_back(Point3 (-2.0, 2.0, 2*z)); + landmarks.push_back(Point3 ( 2.0, 2.0, 2*z)); + landmarks.push_back(Point3 ( 2.0,-2.0, 2*z)); + return landmarks ; } vector genCameraDefaultCalibration() { @@ -145,10 +149,10 @@ vector genCameraVariableCalibration() { return X ; } -shared_ptr getOrdering(const vector& X, const vector& L) { +shared_ptr getOrdering(const vector& cameras, const vector& landmarks) { shared_ptr ordering(new Ordering); - for ( size_t i = 0 ; i < L.size() ; ++i ) ordering->push_back(Symbol('l', i)) ; - for ( size_t i = 0 ; i < X.size() ; ++i ) ordering->push_back(Symbol('x', i)) ; + for ( size_t i = 0 ; i < landmarks.size() ; ++i ) ordering->push_back(L(i)) ; + for ( size_t i = 0 ; i < cameras.size() ; ++i ) ordering->push_back(X(i)) ; return ordering ; } @@ -156,37 +160,37 @@ shared_ptr getOrdering(const vector& X, const vector L = genPoint3(); - vector X = genCameraDefaultCalibration(); + vector landmarks = genPoint3(); + vector cameras = genCameraDefaultCalibration(); // add measurement with noise Graph graph; - for ( size_t j = 0 ; j < X.size() ; ++j) { - for ( size_t i = 0 ; i < L.size() ; ++i) { - Point2 pt = X[j].project(L[i]) ; + for ( size_t j = 0 ; j < cameras.size() ; ++j) { + for ( size_t i = 0 ; i < landmarks.size() ; ++i) { + Point2 pt = cameras[j].project(landmarks[i]) ; graph.addMeasurement(j, i, pt, sigma1); } } - const size_t nMeasurements = X.size()*L.size() ; + const size_t nMeasurements = cameras.size()*landmarks.size() ; // add initial const double noise = baseline*0.1; Values values; - for ( size_t i = 0 ; i < X.size() ; ++i ) - values.insert(Symbol('x',i), X[i]) ; + for ( size_t i = 0 ; i < cameras.size() ; ++i ) + values.insert(X(i), cameras[i]) ; - for ( size_t i = 0 ; i < L.size() ; ++i ) { - Point3 pt(L[i].x()+noise*getGaussian(), - L[i].y()+noise*getGaussian(), - L[i].z()+noise*getGaussian()); - values.insert(Symbol('l',i), pt) ; + for ( size_t i = 0 ; i < landmarks.size() ; ++i ) { + Point3 pt(landmarks[i].x()+noise*getGaussian(), + landmarks[i].y()+noise*getGaussian(), + landmarks[i].z()+noise*getGaussian()); + values.insert(L(i), pt) ; } - graph.addCameraConstraint(0, X[0]); + graph.addCameraConstraint(0, cameras[0]); // Create an ordering of the variables - Ordering ordering = *getOrdering(X,L); + Ordering ordering = *getOrdering(cameras,landmarks); LevenbergMarquardtOptimizer optimizer(graph, values, ordering); Values final = optimizer.optimize(); EXPECT(optimizer.error() < 0.5 * 1e-5 * nMeasurements); @@ -194,42 +198,42 @@ TEST( GeneralSFMFactor, optimize_defaultK ) { /* ************************************************************************* */ TEST( GeneralSFMFactor, optimize_varK_SingleMeasurementError ) { - vector L = genPoint3(); - vector X = genCameraVariableCalibration(); + vector landmarks = genPoint3(); + vector cameras = genCameraVariableCalibration(); // add measurement with noise Graph graph; - for ( size_t j = 0 ; j < X.size() ; ++j) { - for ( size_t i = 0 ; i < L.size() ; ++i) { - Point2 pt = X[j].project(L[i]) ; + for ( size_t j = 0 ; j < cameras.size() ; ++j) { + for ( size_t i = 0 ; i < landmarks.size() ; ++i) { + Point2 pt = cameras[j].project(landmarks[i]) ; graph.addMeasurement(j, i, pt, sigma1); } } - const size_t nMeasurements = X.size()*L.size() ; + const size_t nMeasurements = cameras.size()*landmarks.size() ; // add initial const double noise = baseline*0.1; Values values; - for ( size_t i = 0 ; i < X.size() ; ++i ) - values.insert(Symbol('x',i), X[i]) ; + for ( size_t i = 0 ; i < cameras.size() ; ++i ) + values.insert(X(i), cameras[i]) ; // 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 ) { - Point3 pt(L[i].x()+noise*getGaussian(), - L[i].y()+noise*getGaussian(), - L[i].z()+noise*getGaussian()); - values.insert(Symbol('l',i), pt) ; + Point3 pt(landmarks[i].x()+noise*getGaussian(), + landmarks[i].y()+noise*getGaussian(), + landmarks[i].z()+noise*getGaussian()); + values.insert(L(i), pt) ; } 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; - Ordering ordering = *getOrdering(X,L); + Ordering ordering = *getOrdering(cameras,landmarks); LevenbergMarquardtOptimizer optimizer(graph, values, ordering); Values final = optimizer.optimize(); EXPECT(optimizer.error() < 0.5 * reproj_error * nMeasurements); @@ -238,39 +242,39 @@ TEST( GeneralSFMFactor, optimize_varK_SingleMeasurementError ) { /* ************************************************************************* */ TEST( GeneralSFMFactor, optimize_varK_FixCameras ) { - vector L = genPoint3(); - vector X = genCameraVariableCalibration(); + vector landmarks = genPoint3(); + vector cameras = genCameraVariableCalibration(); // add measurement with noise const double noise = baseline*0.1; Graph graph; - for ( size_t j = 0 ; j < X.size() ; ++j) { - for ( size_t i = 0 ; i < L.size() ; ++i) { - Point2 pt = X[j].project(L[i]) ; + for ( size_t j = 0 ; j < cameras.size() ; ++j) { + for ( size_t i = 0 ; i < landmarks.size() ; ++i) { + Point2 pt = cameras[j].project(landmarks[i]) ; graph.addMeasurement(j, i, pt, sigma1); } } - const size_t nMeasurements = L.size()*X.size(); + const size_t nMeasurements = landmarks.size()*cameras.size(); Values values; - for ( size_t i = 0 ; i < X.size() ; ++i ) - values.insert(Symbol('x',i), X[i]) ; + for ( size_t i = 0 ; i < cameras.size() ; ++i ) + values.insert(X(i), cameras[i]) ; - for ( size_t i = 0 ; i < L.size() ; ++i ) { - Point3 pt(L[i].x()+noise*getGaussian(), - L[i].y()+noise*getGaussian(), - L[i].z()+noise*getGaussian()); - //Point3 pt(L[i].x(), L[i].y(), L[i].z()); - values.insert(Symbol('l',i), pt) ; + for ( size_t i = 0 ; i < landmarks.size() ; ++i ) { + Point3 pt(landmarks[i].x()+noise*getGaussian(), + landmarks[i].y()+noise*getGaussian(), + landmarks[i].z()+noise*getGaussian()); + //Point3 pt(landmarks[i].x(), landmarks[i].y(), landmarks[i].z()); + values.insert(L(i), pt) ; } - for ( size_t i = 0 ; i < X.size() ; ++i ) - graph.addCameraConstraint(i, X[i]); + for ( size_t i = 0 ; i < cameras.size() ; ++i ) + graph.addCameraConstraint(i, cameras[i]); const double reproj_error = 1e-5 ; - Ordering ordering = *getOrdering(X,L); + Ordering ordering = *getOrdering(cameras,landmarks); LevenbergMarquardtOptimizer optimizer(graph, values, ordering); Values final = optimizer.optimize(); EXPECT(optimizer.error() < 0.5 * reproj_error * nMeasurements); @@ -279,29 +283,29 @@ TEST( GeneralSFMFactor, optimize_varK_FixCameras ) { /* ************************************************************************* */ TEST( GeneralSFMFactor, optimize_varK_FixLandmarks ) { - vector L = genPoint3(); - vector X = genCameraVariableCalibration(); + vector landmarks = genPoint3(); + vector cameras = genCameraVariableCalibration(); // add measurement with noise Graph graph; - for ( size_t j = 0 ; j < X.size() ; ++j) { - for ( size_t i = 0 ; i < L.size() ; ++i) { - Point2 pt = X[j].project(L[i]) ; + for ( size_t j = 0 ; j < cameras.size() ; ++j) { + for ( size_t i = 0 ; i < landmarks.size() ; ++i) { + Point2 pt = cameras[j].project(landmarks[i]) ; graph.addMeasurement(j, i, pt, sigma1); } } - const size_t nMeasurements = L.size()*X.size(); + const size_t nMeasurements = landmarks.size()*cameras.size(); Values values; - for ( size_t i = 0 ; i < X.size() ; ++i ) { + for ( size_t i = 0 ; i < cameras.size() ; ++i ) { const double rot_noise = 1e-5, trans_noise = 1e-3, focal_noise = 1, skew_noise = 1e-5; if ( i == 0 ) { - values.insert(Symbol('x',i), X[i]) ; + values.insert(X(i), cameras[i]) ; } else { @@ -312,22 +316,22 @@ TEST( GeneralSFMFactor, optimize_varK_FixLandmarks ) { skew_noise, // s 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 ) { - values.insert(Symbol('l',i), L[i]) ; + for ( size_t i = 0 ; i < landmarks.size() ; ++i ) { + values.insert(L(i), landmarks[i]) ; } - // fix X0 and all landmarks, allow only the X[1] to move - graph.addCameraConstraint(0, X[0]); - for ( size_t i = 0 ; i < L.size() ; ++i ) - graph.addPoint3Constraint(i, L[i]); + // fix X0 and all landmarks, allow only the cameras[1] to move + graph.addCameraConstraint(0, cameras[0]); + for ( size_t i = 0 ; i < landmarks.size() ; ++i ) + graph.addPoint3Constraint(i, landmarks[i]); const double reproj_error = 1e-5 ; - Ordering ordering = *getOrdering(X,L); + Ordering ordering = *getOrdering(cameras,landmarks); LevenbergMarquardtOptimizer optimizer(graph, values, ordering); Values final = optimizer.optimize(); EXPECT(optimizer.error() < 0.5 * reproj_error * nMeasurements); @@ -335,43 +339,43 @@ TEST( GeneralSFMFactor, optimize_varK_FixLandmarks ) { /* ************************************************************************* */ TEST( GeneralSFMFactor, optimize_varK_BA ) { - vector L = genPoint3(); - vector X = genCameraVariableCalibration(); + vector landmarks = genPoint3(); + vector cameras = genCameraVariableCalibration(); // add measurement with noise Graph graph; - for ( size_t j = 0 ; j < X.size() ; ++j) { - for ( size_t i = 0 ; i < L.size() ; ++i) { - Point2 pt = X[j].project(L[i]) ; + for ( size_t j = 0 ; j < cameras.size() ; ++j) { + for ( size_t i = 0 ; i < landmarks.size() ; ++i) { + Point2 pt = cameras[j].project(landmarks[i]) ; graph.addMeasurement(j, i, pt, sigma1); } } - const size_t nMeasurements = X.size()*L.size() ; + const size_t nMeasurements = cameras.size()*landmarks.size() ; // add initial const double noise = baseline*0.1; Values values; - for ( size_t i = 0 ; i < X.size() ; ++i ) - values.insert(Symbol('x',i), X[i]) ; + for ( size_t i = 0 ; i < cameras.size() ; ++i ) + values.insert(X(i), cameras[i]) ; // add noise only to the first landmark - for ( size_t i = 0 ; i < L.size() ; ++i ) { - Point3 pt(L[i].x()+noise*getGaussian(), - L[i].y()+noise*getGaussian(), - L[i].z()+noise*getGaussian()); - values.insert(Symbol('l',i), pt) ; + for ( size_t i = 0 ; i < landmarks.size() ; ++i ) { + Point3 pt(landmarks[i].x()+noise*getGaussian(), + landmarks[i].y()+noise*getGaussian(), + landmarks[i].z()+noise*getGaussian()); + values.insert(L(i), pt) ; } // 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 - graph.add(RangeFactor(Symbol('x',0), Symbol('x',1), 2.0, sharedSigma(1, 10.0))); + graph.add(RangeFactor(X(0), X(1), 2.0, sharedSigma(1, 10.0))); const double reproj_error = 1e-5 ; - Ordering ordering = *getOrdering(X,L); + Ordering ordering = *getOrdering(cameras,landmarks); LevenbergMarquardtOptimizer optimizer(graph, values, ordering); Values final = optimizer.optimize(); EXPECT(optimizer.error() < 0.5 * reproj_error * nMeasurements); @@ -382,17 +386,17 @@ TEST(GeneralSFMFactor, GeneralCameraPoseRange) { // Tests range factor between a GeneralCamera and a Pose3 Graph graph; graph.addCameraConstraint(0, GeneralCamera()); - graph.add(RangeFactor(Symbol('x',0), Symbol('x',1), 2.0, sharedSigma(1, 1.0))); - graph.add(PriorFactor(Symbol('x',1), Pose3(Rot3(), Point3(1.0, 0.0, 0.0)), sharedSigma(6, 1.0))); + graph.add(RangeFactor(X(0), X(1), 2.0, sharedSigma(1, 1.0))); + graph.add(PriorFactor(X(1), Pose3(Rot3(), Point3(1.0, 0.0, 0.0)), sharedSigma(6, 1.0))); Values init; - init.insert(Symbol('x',0), GeneralCamera()); - init.insert(Symbol('x',1), Pose3(Rot3(), Point3(1.0,1.0,1.0))); + init.insert(X(0), GeneralCamera()); + 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 Values expected; - expected.insert(Symbol('x',0), GeneralCamera()); - expected.insert(Symbol('x',1), Pose3(Rot3(), Point3(1.5,0.0,0.0))); + expected.insert(X(0), GeneralCamera()); + expected.insert(X(1), Pose3(Rot3(), Point3(1.5,0.0,0.0))); LevenbergMarquardtParams params; params.absoluteErrorTol = 1e-9; @@ -406,17 +410,17 @@ TEST(GeneralSFMFactor, GeneralCameraPoseRange) { TEST(GeneralSFMFactor, CalibratedCameraPoseRange) { // Tests range factor between a CalibratedCamera and a Pose3 NonlinearFactorGraph graph; - graph.add(PriorFactor(Symbol('x',0), CalibratedCamera(), sharedSigma(6, 1.0))); - graph.add(RangeFactor(Symbol('x',0), Symbol('x',1), 2.0, sharedSigma(1, 1.0))); - graph.add(PriorFactor(Symbol('x',1), Pose3(Rot3(), Point3(1.0, 0.0, 0.0)), sharedSigma(6, 1.0))); + graph.add(PriorFactor(X(0), CalibratedCamera(), sharedSigma(6, 1.0))); + graph.add(RangeFactor(X(0), X(1), 2.0, sharedSigma(1, 1.0))); + graph.add(PriorFactor(X(1), Pose3(Rot3(), Point3(1.0, 0.0, 0.0)), sharedSigma(6, 1.0))); Values init; - init.insert(Symbol('x',0), CalibratedCamera()); - init.insert(Symbol('x',1), Pose3(Rot3(), Point3(1.0,1.0,1.0))); + init.insert(X(0), CalibratedCamera()); + init.insert(X(1), Pose3(Rot3(), Point3(1.0,1.0,1.0))); Values expected; - expected.insert(Symbol('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(0), CalibratedCamera(Pose3(Rot3(), Point3(-0.333333333333, 0, 0)))); + expected.insert(X(1), Pose3(Rot3(), Point3(1.333333333333, 0, 0))); LevenbergMarquardtParams params; params.absoluteErrorTol = 1e-9; diff --git a/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp b/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp index 7698acae4..142c27e7e 100644 --- a/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp +++ b/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp @@ -38,6 +38,10 @@ using namespace std; using namespace gtsam; +// Convenience for named keys +using symbol_shorthand::X; +using symbol_shorthand::L; + typedef PinholeCamera GeneralCamera; typedef GeneralSFMFactor Projection; typedef NonlinearEquality CameraConstraint; @@ -47,16 +51,16 @@ typedef NonlinearEquality Point3Constraint; class Graph: public NonlinearFactorGraph { public: void addMeasurement(const int& i, const int& j, const Point2& z, const SharedNoiseModel& model) { - push_back(boost::make_shared(z, model, Symbol('x',i), Symbol('l',j))); + push_back(boost::make_shared(z, model, X(i), L(j))); } void addCameraConstraint(int j, const GeneralCamera& p) { - boost::shared_ptr factor(new CameraConstraint(Symbol('x', j), p)); + boost::shared_ptr factor(new CameraConstraint(X(j), p)); push_back(factor); } void addPoint3Constraint(int j, const Point3& p) { - boost::shared_ptr factor(new Point3Constraint(Symbol('l', j), p)); + boost::shared_ptr factor(new Point3Constraint(L(j), p)); push_back(factor); } @@ -100,14 +104,14 @@ TEST( GeneralSFMFactor, error ) { Point2 z(3.,0.); const SharedNoiseModel sigma(noiseModel::Unit::Create(1)); boost::shared_ptr - 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 Values values; Rot3 R; Point3 t1(0,0,-6); Pose3 x1(R,t1); - values.insert(Symbol('x',1), GeneralCamera(x1)); - Point3 l1; values.insert(Symbol('l',1), l1); + values.insert(X(1), GeneralCamera(x1)); + Point3 l1; values.insert(L(1), l1); EXPECT(assert_equal(Vector_(2, -3.0, 0.0), factor->unwhitenedError(values))); } @@ -117,78 +121,78 @@ static const double baseline = 5.0 ; /* ************************************************************************* */ vector genPoint3() { const double z = 5; - vector L ; - L.push_back(Point3 (-1.0,-1.0, z)); - L.push_back(Point3 (-1.0, 1.0, z)); - L.push_back(Point3 ( 1.0, 1.0, z)); - L.push_back(Point3 ( 1.0,-1.0, z)); - L.push_back(Point3 (-1.5,-1.5, 1.5*z)); - L.push_back(Point3 (-1.5, 1.5, 1.5*z)); - L.push_back(Point3 ( 1.5, 1.5, 1.5*z)); - L.push_back(Point3 ( 1.5,-1.5, 1.5*z)); - L.push_back(Point3 (-2.0,-2.0, 2*z)); - L.push_back(Point3 (-2.0, 2.0, 2*z)); - L.push_back(Point3 ( 2.0, 2.0, 2*z)); - L.push_back(Point3 ( 2.0,-2.0, 2*z)); - return L ; + vector landmarks ; + landmarks.push_back(Point3 (-1.0,-1.0, z)); + landmarks.push_back(Point3 (-1.0, 1.0, z)); + landmarks.push_back(Point3 ( 1.0, 1.0, z)); + landmarks.push_back(Point3 ( 1.0,-1.0, z)); + landmarks.push_back(Point3 (-1.5,-1.5, 1.5*z)); + landmarks.push_back(Point3 (-1.5, 1.5, 1.5*z)); + landmarks.push_back(Point3 ( 1.5, 1.5, 1.5*z)); + landmarks.push_back(Point3 ( 1.5,-1.5, 1.5*z)); + landmarks.push_back(Point3 (-2.0,-2.0, 2*z)); + landmarks.push_back(Point3 (-2.0, 2.0, 2*z)); + landmarks.push_back(Point3 ( 2.0, 2.0, 2*z)); + landmarks.push_back(Point3 ( 2.0,-2.0, 2*z)); + return landmarks ; } vector genCameraDefaultCalibration() { - vector X ; - X.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)))); - return X ; + vector cameras ; + cameras.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 cameras ; } vector genCameraVariableCalibration() { const Cal3Bundler K(500,1e-3,1e-3); - vector X ; - X.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)); - return X ; + vector cameras ; + cameras.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 cameras ; } -boost::shared_ptr getOrdering(const std::vector& X, const std::vector& L) { +boost::shared_ptr getOrdering(const std::vector& cameras, const std::vector& landmarks) { boost::shared_ptr ordering(new Ordering); - for ( size_t i = 0 ; i < L.size() ; ++i ) ordering->push_back(Symbol('l', i)) ; - for ( size_t i = 0 ; i < X.size() ; ++i ) ordering->push_back(Symbol('x', i)) ; + for ( size_t i = 0 ; i < landmarks.size() ; ++i ) ordering->push_back(L(i)) ; + for ( size_t i = 0 ; i < cameras.size() ; ++i ) ordering->push_back(X(i)) ; return ordering ; } /* ************************************************************************* */ TEST( GeneralSFMFactor, optimize_defaultK ) { - vector L = genPoint3(); - vector X = genCameraDefaultCalibration(); + vector landmarks = genPoint3(); + vector cameras = genCameraDefaultCalibration(); // add measurement with noise Graph graph; - for ( size_t j = 0 ; j < X.size() ; ++j) { - for ( size_t i = 0 ; i < L.size() ; ++i) { - Point2 pt = X[j].project(L[i]) ; + for ( size_t j = 0 ; j < cameras.size() ; ++j) { + for ( size_t i = 0 ; i < landmarks.size() ; ++i) { + Point2 pt = cameras[j].project(landmarks[i]) ; graph.addMeasurement(j, i, pt, sigma1); } } - const size_t nMeasurements = X.size()*L.size() ; + const size_t nMeasurements = cameras.size()*landmarks.size() ; // add initial const double noise = baseline*0.1; Values values; - for ( size_t i = 0 ; i < X.size() ; ++i ) - values.insert(Symbol('x',i), X[i]) ; + for ( size_t i = 0 ; i < cameras.size() ; ++i ) + values.insert(X(i), cameras[i]) ; - for ( size_t i = 0 ; i < L.size() ; ++i ) { - Point3 pt(L[i].x()+noise*getGaussian(), - L[i].y()+noise*getGaussian(), - L[i].z()+noise*getGaussian()); - values.insert(Symbol('l',i), pt) ; + for ( size_t i = 0 ; i < landmarks.size() ; ++i ) { + Point3 pt(landmarks[i].x()+noise*getGaussian(), + landmarks[i].y()+noise*getGaussian(), + landmarks[i].z()+noise*getGaussian()); + values.insert(L(i), pt) ; } - graph.addCameraConstraint(0, X[0]); + graph.addCameraConstraint(0, cameras[0]); // Create an ordering of the variables - Ordering ordering = *getOrdering(X,L); + Ordering ordering = *getOrdering(cameras,landmarks); LevenbergMarquardtOptimizer optimizer(graph, values, ordering); Values final = optimizer.optimize(); EXPECT(optimizer.error() < 0.5 * 1e-5 * nMeasurements); @@ -196,42 +200,42 @@ TEST( GeneralSFMFactor, optimize_defaultK ) { /* ************************************************************************* */ TEST( GeneralSFMFactor, optimize_varK_SingleMeasurementError ) { - vector L = genPoint3(); - vector X = genCameraVariableCalibration(); + vector landmarks = genPoint3(); + vector cameras = genCameraVariableCalibration(); // add measurement with noise Graph graph; - for ( size_t j = 0 ; j < X.size() ; ++j) { - for ( size_t i = 0 ; i < L.size() ; ++i) { - Point2 pt = X[j].project(L[i]) ; + for ( size_t j = 0 ; j < cameras.size() ; ++j) { + for ( size_t i = 0 ; i < landmarks.size() ; ++i) { + Point2 pt = cameras[j].project(landmarks[i]) ; graph.addMeasurement(j, i, pt, sigma1); } } - const size_t nMeasurements = X.size()*L.size() ; + const size_t nMeasurements = cameras.size()*landmarks.size() ; // add initial const double noise = baseline*0.1; Values values; - for ( size_t i = 0 ; i < X.size() ; ++i ) - values.insert(Symbol('x',i), X[i]) ; + for ( size_t i = 0 ; i < cameras.size() ; ++i ) + values.insert(X(i), cameras[i]) ; // 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 ) { - Point3 pt(L[i].x()+noise*getGaussian(), - L[i].y()+noise*getGaussian(), - L[i].z()+noise*getGaussian()); - values.insert(Symbol('l',i), pt) ; + Point3 pt(landmarks[i].x()+noise*getGaussian(), + landmarks[i].y()+noise*getGaussian(), + landmarks[i].z()+noise*getGaussian()); + values.insert(L(i), pt) ; } 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; - Ordering ordering = *getOrdering(X,L); + Ordering ordering = *getOrdering(cameras,landmarks); LevenbergMarquardtOptimizer optimizer(graph, values, ordering); Values final = optimizer.optimize(); EXPECT(optimizer.error() < 0.5 * reproj_error * nMeasurements); @@ -240,39 +244,39 @@ TEST( GeneralSFMFactor, optimize_varK_SingleMeasurementError ) { /* ************************************************************************* */ TEST( GeneralSFMFactor, optimize_varK_FixCameras ) { - vector L = genPoint3(); - vector X = genCameraVariableCalibration(); + vector landmarks = genPoint3(); + vector cameras = genCameraVariableCalibration(); // add measurement with noise const double noise = baseline*0.1; Graph graph; - for ( size_t j = 0 ; j < X.size() ; ++j) { - for ( size_t i = 0 ; i < L.size() ; ++i) { - Point2 pt = X[j].project(L[i]) ; + for ( size_t j = 0 ; j < cameras.size() ; ++j) { + for ( size_t i = 0 ; i < landmarks.size() ; ++i) { + Point2 pt = cameras[j].project(landmarks[i]) ; graph.addMeasurement(j, i, pt, sigma1); } } - const size_t nMeasurements = L.size()*X.size(); + const size_t nMeasurements = landmarks.size()*cameras.size(); Values values; - for ( size_t i = 0 ; i < X.size() ; ++i ) - values.insert(Symbol('x',i), X[i]) ; + for ( size_t i = 0 ; i < cameras.size() ; ++i ) + values.insert(X(i), cameras[i]) ; - for ( size_t i = 0 ; i < L.size() ; ++i ) { - Point3 pt(L[i].x()+noise*getGaussian(), - L[i].y()+noise*getGaussian(), - L[i].z()+noise*getGaussian()); - //Point3 pt(L[i].x(), L[i].y(), L[i].z()); - values.insert(Symbol('l',i), pt) ; + for ( size_t i = 0 ; i < landmarks.size() ; ++i ) { + Point3 pt(landmarks[i].x()+noise*getGaussian(), + landmarks[i].y()+noise*getGaussian(), + landmarks[i].z()+noise*getGaussian()); + //Point3 pt(landmarks[i].x(), landmarks[i].y(), landmarks[i].z()); + values.insert(L(i), pt) ; } - for ( size_t i = 0 ; i < X.size() ; ++i ) - graph.addCameraConstraint(i, X[i]); + for ( size_t i = 0 ; i < cameras.size() ; ++i ) + graph.addCameraConstraint(i, cameras[i]); const double reproj_error = 1e-5 ; - Ordering ordering = *getOrdering(X,L); + Ordering ordering = *getOrdering(cameras,landmarks); LevenbergMarquardtOptimizer optimizer(graph, values, ordering); Values final = optimizer.optimize(); EXPECT(optimizer.error() < 0.5 * reproj_error * nMeasurements); @@ -281,27 +285,27 @@ TEST( GeneralSFMFactor, optimize_varK_FixCameras ) { /* ************************************************************************* */ TEST( GeneralSFMFactor, optimize_varK_FixLandmarks ) { - vector L = genPoint3(); - vector X = genCameraVariableCalibration(); + vector landmarks = genPoint3(); + vector cameras = genCameraVariableCalibration(); // add measurement with noise Graph graph; - for ( size_t j = 0 ; j < X.size() ; ++j) { - for ( size_t i = 0 ; i < L.size() ; ++i) { - Point2 pt = X[j].project(L[i]) ; + for ( size_t j = 0 ; j < cameras.size() ; ++j) { + for ( size_t i = 0 ; i < landmarks.size() ; ++i) { + Point2 pt = cameras[j].project(landmarks[i]) ; graph.addMeasurement(j, i, pt, sigma1); } } - const size_t nMeasurements = L.size()*X.size(); + const size_t nMeasurements = landmarks.size()*cameras.size(); Values values; - for ( size_t i = 0 ; i < X.size() ; ++i ) { + for ( size_t i = 0 ; i < cameras.size() ; ++i ) { const double rot_noise = 1e-5, trans_noise = 1e-3, focal_noise = 1, distort_noise = 1e-3; if ( i == 0 ) { - values.insert(Symbol('x',i), X[i]) ; + values.insert(X(i), cameras[i]) ; } else { @@ -310,22 +314,22 @@ TEST( GeneralSFMFactor, optimize_varK_FixLandmarks ) { trans_noise, trans_noise, trans_noise, // translation 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 ) { - values.insert(Symbol('l',i), L[i]) ; + for ( size_t i = 0 ; i < landmarks.size() ; ++i ) { + values.insert(L(i), landmarks[i]) ; } - // fix X0 and all landmarks, allow only the X[1] to move - graph.addCameraConstraint(0, X[0]); - for ( size_t i = 0 ; i < L.size() ; ++i ) - graph.addPoint3Constraint(i, L[i]); + // fix X0 and all landmarks, allow only the cameras[1] to move + graph.addCameraConstraint(0, cameras[0]); + for ( size_t i = 0 ; i < landmarks.size() ; ++i ) + graph.addPoint3Constraint(i, landmarks[i]); const double reproj_error = 1e-5 ; - Ordering ordering = *getOrdering(X,L); + Ordering ordering = *getOrdering(cameras,landmarks); LevenbergMarquardtOptimizer optimizer(graph, values, ordering); Values final = optimizer.optimize(); EXPECT(optimizer.error() < 0.5 * reproj_error * nMeasurements); @@ -333,43 +337,43 @@ TEST( GeneralSFMFactor, optimize_varK_FixLandmarks ) { /* ************************************************************************* */ TEST( GeneralSFMFactor, optimize_varK_BA ) { - vector L = genPoint3(); - vector X = genCameraVariableCalibration(); + vector landmarks = genPoint3(); + vector cameras = genCameraVariableCalibration(); // add measurement with noise Graph graph; - for ( size_t j = 0 ; j < X.size() ; ++j) { - for ( size_t i = 0 ; i < L.size() ; ++i) { - Point2 pt = X[j].project(L[i]) ; + for ( size_t j = 0 ; j < cameras.size() ; ++j) { + for ( size_t i = 0 ; i < landmarks.size() ; ++i) { + Point2 pt = cameras[j].project(landmarks[i]) ; graph.addMeasurement(j, i, pt, sigma1); } } - const size_t nMeasurements = X.size()*L.size() ; + const size_t nMeasurements = cameras.size()*landmarks.size() ; // add initial const double noise = baseline*0.1; Values values; - for ( size_t i = 0 ; i < X.size() ; ++i ) - values.insert(Symbol('x',i), X[i]) ; + for ( size_t i = 0 ; i < cameras.size() ; ++i ) + values.insert(X(i), cameras[i]) ; // add noise only to the first landmark - for ( size_t i = 0 ; i < L.size() ; ++i ) { - Point3 pt(L[i].x()+noise*getGaussian(), - L[i].y()+noise*getGaussian(), - L[i].z()+noise*getGaussian()); - values.insert(Symbol('l',i), pt) ; + for ( size_t i = 0 ; i < landmarks.size() ; ++i ) { + Point3 pt(landmarks[i].x()+noise*getGaussian(), + landmarks[i].y()+noise*getGaussian(), + landmarks[i].z()+noise*getGaussian()); + values.insert(L(i), pt) ; } // 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 - graph.add(RangeFactor(Symbol('x',0), Symbol('x',1), 2.0, sharedSigma(1, 10.0))); + graph.add(RangeFactor(X(0), X(1), 2.0, sharedSigma(1, 10.0))); const double reproj_error = 1e-5 ; - Ordering ordering = *getOrdering(X,L); + Ordering ordering = *getOrdering(cameras,landmarks); LevenbergMarquardtOptimizer optimizer(graph, values, ordering); Values final = optimizer.optimize(); EXPECT(optimizer.error() < 0.5 * reproj_error * nMeasurements); diff --git a/gtsam/slam/tests/testProjectionFactor.cpp b/gtsam/slam/tests/testProjectionFactor.cpp index 2ec97f301..0cd357c70 100644 --- a/gtsam/slam/tests/testProjectionFactor.cpp +++ b/gtsam/slam/tests/testProjectionFactor.cpp @@ -37,8 +37,8 @@ static SharedNoiseModel sigma(noiseModel::Unit::Create(1)); static shared_ptrK sK(new Cal3_S2(K)); // Convenience for named keys -Key kx(size_t i) { return Symbol('x',i); } -Key kl(size_t i) { return Symbol('l',i); } +using symbol_shorthand::X; +using symbol_shorthand::L; /* ************************************************************************* */ TEST( ProjectionFactor, error ) @@ -47,12 +47,12 @@ TEST( ProjectionFactor, error ) Point2 z(323.,240.); int i=1, j=1; boost::shared_ptr - 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 Values config; - Rot3 R;Point3 t1(0,0,-6); Pose3 x1(R,t1); config.insert(kx(1), x1); - Point3 l1; config.insert(kl(1), l1); + Rot3 R;Point3 t1(0,0,-6); Pose3 x1(R,t1); config.insert(X(1), x1); + Point3 l1; config.insert(L(1), l1); // Point should project to Point2(320.,240.) 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); // 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 Al1 = Matrix_(2, 3, 92.376, 0., 0., 0., 92.376, 0.); Vector b = Vector_(2,3.,0.); 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 = boost::dynamic_pointer_cast(factor->linearize(config, ordering)); CHECK(assert_equal(expected,*actual,1e-3)); @@ -80,11 +80,11 @@ TEST( ProjectionFactor, error ) // expmap on a config Values expected_config; - Point3 t2(1,1,-5); Pose3 x2(R,t2); expected_config.insert(kx(1), x2); - Point3 l2(1,2,3); expected_config.insert(kl(1), l2); + Point3 t2(1,1,-5); Pose3 x2(R,t2); expected_config.insert(X(1), x2); + Point3 l2(1,2,3); expected_config.insert(L(1), l2); VectorValues delta(expected_config.dims(ordering)); - delta[ordering[kx(1)]] = Vector_(6, 0.,0.,0., 1.,1.,1.); - delta[ordering[kl(1)]] = Vector_(3, 1.,2.,3.); + delta[ordering[X(1)]] = Vector_(6, 0.,0.,0., 1.,1.,1.); + delta[ordering[L(1)]] = Vector_(3, 1.,2.,3.); Values actual_config = config.retract(delta, ordering); CHECK(assert_equal(expected_config,actual_config,1e-9)); } @@ -96,10 +96,10 @@ TEST( ProjectionFactor, equals ) Vector z = Vector_(2,323.,240.); int i=1, j=1; boost::shared_ptr - 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 - 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)); } diff --git a/gtsam/slam/tests/testSerializationSLAM.cpp b/gtsam/slam/tests/testSerializationSLAM.cpp index 23093a28e..51d37db74 100644 --- a/gtsam/slam/tests/testSerializationSLAM.cpp +++ b/gtsam/slam/tests/testSerializationSLAM.cpp @@ -29,6 +29,10 @@ using namespace std; using namespace gtsam; using namespace gtsam::serializationTestHelpers; +// Convenience for named keys +using symbol_shorthand::X; +using symbol_shorthand::L; + /* Create GUIDs for factors */ /* ************************************************************************* */ BOOST_CLASS_EXPORT_GUID(gtsam::JacobianFactor, "gtsam::JacobianFactor"); @@ -49,7 +53,7 @@ BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal"); TEST (Serialization, smallExample_linear) { 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); EXPECT(equalsObj(ordering)); EXPECT(equalsXML(ordering)); diff --git a/gtsam/slam/tests/testSimulated2DOriented.cpp b/gtsam/slam/tests/testSimulated2DOriented.cpp index afca72402..d017512ce 100644 --- a/gtsam/slam/tests/testSimulated2DOriented.cpp +++ b/gtsam/slam/tests/testSimulated2DOriented.cpp @@ -29,8 +29,8 @@ using namespace gtsam; #include // Convenience for named keys -Key kx(size_t i) { return Symbol('x',i); } -Key kl(size_t i) { return Symbol('l',i); } +using symbol_shorthand::X; +using symbol_shorthand::L; /* ************************************************************************* */ TEST( simulated2DOriented, Dprior ) @@ -59,11 +59,11 @@ TEST( simulated2DOriented, constructor ) { Pose2 measurement(0.2, 0.3, 0.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; - config.insert(kx(1), Pose2(1., 0., 0.2)); - config.insert(kx(2), Pose2(2., 0., 0.1)); + config.insert(X(1), Pose2(1., 0., 0.2)); + config.insert(X(2), Pose2(2., 0., 0.1)); boost::shared_ptr lf = factor.linearize(config, *config.orderingArbitrary()); } diff --git a/gtsam/slam/tests/testStereoFactor.cpp b/gtsam/slam/tests/testStereoFactor.cpp index 9a8a27054..35ef081bc 100644 --- a/gtsam/slam/tests/testStereoFactor.cpp +++ b/gtsam/slam/tests/testStereoFactor.cpp @@ -43,8 +43,8 @@ Point3 p(0, 0, 5); static SharedNoiseModel sigma(noiseModel::Unit::Create(1)); // Convenience for named keys -Key kx(size_t i) { return Symbol('x',i); } -Key kl(size_t i) { return Symbol('l',i); } +using symbol_shorthand::X; +using symbol_shorthand::L; /* ************************************************************************* */ TEST( StereoFactor, singlePoint) @@ -53,18 +53,18 @@ TEST( StereoFactor, singlePoint) boost::shared_ptr K(new Cal3_S2Stereo(625, 625, 0, 320, 240, 0.5)); NonlinearFactorGraph graph; - graph.add(visualSLAM::PoseConstraint(kx(1),camera1)); + graph.add(visualSLAM::PoseConstraint(X(1),camera1)); StereoPoint2 z14(320,320.0-50, 240); // 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 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); - values.insert(kl(1), l1); // add point at origin; + values.insert(L(1), l1); // add point at origin; GaussNewtonOptimizer optimizer(graph, values); diff --git a/gtsam/slam/tests/testVisualSLAM.cpp b/gtsam/slam/tests/testVisualSLAM.cpp index dc9b64f4d..f09d5117a 100644 --- a/gtsam/slam/tests/testVisualSLAM.cpp +++ b/gtsam/slam/tests/testVisualSLAM.cpp @@ -34,8 +34,8 @@ using namespace gtsam; static SharedNoiseModel sigma(noiseModel::Unit::Create(1)); // Convenience for named keys -Key kx(size_t i) { return Symbol('x',i); } -Key kl(size_t i) { return Symbol('l',i); } +using symbol_shorthand::X; +using symbol_shorthand::L; static Point3 landmark1(-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)); visualSLAM::Graph g; - g.addMeasurement(z11, sigma, kx(1), kl(1), sK); - g.addMeasurement(z12, sigma, kx(1), kl(2), sK); - g.addMeasurement(z13, sigma, kx(1), kl(3), sK); - g.addMeasurement(z14, sigma, kx(1), kl(4), sK); - g.addMeasurement(z21, sigma, kx(2), kl(1), sK); - g.addMeasurement(z22, sigma, kx(2), kl(2), sK); - g.addMeasurement(z23, sigma, kx(2), kl(3), sK); - g.addMeasurement(z24, sigma, kx(2), kl(4), sK); + g.addMeasurement(z11, sigma, X(1), L(1), sK); + g.addMeasurement(z12, sigma, X(1), L(2), sK); + g.addMeasurement(z13, sigma, X(1), L(3), sK); + g.addMeasurement(z14, sigma, X(1), L(4), sK); + g.addMeasurement(z21, sigma, X(2), L(1), sK); + g.addMeasurement(z22, sigma, X(2), L(2), sK); + g.addMeasurement(z23, sigma, X(2), L(3), sK); + g.addMeasurement(z24, sigma, X(2), L(4), sK); return g; } @@ -86,22 +86,22 @@ TEST( Graph, optimizeLM) // build a graph visualSLAM::Graph graph(testGraph()); // add 3 landmark constraints - graph.addPointConstraint(kl(1), landmark1); - graph.addPointConstraint(kl(2), landmark2); - graph.addPointConstraint(kl(3), landmark3); + graph.addPointConstraint(L(1), landmark1); + graph.addPointConstraint(L(2), landmark2); + graph.addPointConstraint(L(3), landmark3); // Create an initial values structure corresponding to the ground truth Values initialEstimate; - initialEstimate.insert(kx(1), camera1); - initialEstimate.insert(kx(2), camera2); - initialEstimate.insert(kl(1), landmark1); - initialEstimate.insert(kl(2), landmark2); - initialEstimate.insert(kl(3), landmark3); - initialEstimate.insert(kl(4), landmark4); + initialEstimate.insert(X(1), camera1); + initialEstimate.insert(X(2), camera2); + initialEstimate.insert(L(1), landmark1); + initialEstimate.insert(L(2), landmark2); + initialEstimate.insert(L(3), landmark3); + initialEstimate.insert(L(4), landmark4); // Create an ordering of the variables 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 // We expect the initial to be zero because config is the ground truth @@ -124,21 +124,21 @@ TEST( Graph, optimizeLM2) // build a graph visualSLAM::Graph graph(testGraph()); // add 2 camera constraints - graph.addPoseConstraint(kx(1), camera1); - graph.addPoseConstraint(kx(2), camera2); + graph.addPoseConstraint(X(1), camera1); + graph.addPoseConstraint(X(2), camera2); // Create an initial values structure corresponding to the ground truth Values initialEstimate; - initialEstimate.insert(kx(1), camera1); - initialEstimate.insert(kx(2), camera2); - initialEstimate.insert(kl(1), landmark1); - initialEstimate.insert(kl(2), landmark2); - initialEstimate.insert(kl(3), landmark3); - initialEstimate.insert(kl(4), landmark4); + initialEstimate.insert(X(1), camera1); + initialEstimate.insert(X(2), camera2); + initialEstimate.insert(L(1), landmark1); + initialEstimate.insert(L(2), landmark2); + initialEstimate.insert(L(3), landmark3); + initialEstimate.insert(L(4), landmark4); // Create an ordering of the variables 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 // We expect the initial to be zero because config is the ground truth @@ -161,17 +161,17 @@ TEST( Graph, CHECK_ORDERING) // build a graph visualSLAM::Graph graph = testGraph(); // add 2 camera constraints - graph.addPoseConstraint(kx(1), camera1); - graph.addPoseConstraint(kx(2), camera2); + graph.addPoseConstraint(X(1), camera1); + graph.addPoseConstraint(X(2), camera2); // Create an initial values structure corresponding to the ground truth Values initialEstimate; - initialEstimate.insert(kx(1), camera1); - initialEstimate.insert(kx(2), camera2); - initialEstimate.insert(kl(1), landmark1); - initialEstimate.insert(kl(2), landmark2); - initialEstimate.insert(kl(3), landmark3); - initialEstimate.insert(kl(4), landmark4); + initialEstimate.insert(X(1), camera1); + initialEstimate.insert(X(2), camera2); + initialEstimate.insert(L(1), landmark1); + initialEstimate.insert(L(2), landmark2); + initialEstimate.insert(L(3), landmark3); + initialEstimate.insert(L(4), landmark4); // Create an optimizer and check its error // 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 // the size of the config, it only updates existing variables Values init; - init.insert(kx(1), Pose3()); - init.insert(kl(1), Point3(1.0, 2.0, 3.0)); + init.insert(X(1), Pose3()); + init.insert(L(1), Point3(1.0, 2.0, 3.0)); Values expected; - expected.insert(kx(1), Pose3(Rot3(), Point3(0.1, 0.1, 0.1))); - expected.insert(kl(1), Point3(1.1, 2.1, 3.1)); + expected.insert(X(1), Pose3(Rot3(), Point3(0.1, 0.1, 0.1))); + expected.insert(L(1), Point3(1.1, 2.1, 3.1)); Ordering largeOrdering; Values largeValues = init; - largeValues.insert(kx(2), Pose3()); - largeOrdering += kx(1),kl(1),kx(2); + largeValues.insert(X(2), Pose3()); + largeOrdering += X(1),L(1),X(2); 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[kl(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(1)]] = Vector_(6, 0.0, 0.0, 0.0, 0.1, 0.1, 0.1); + delta[largeOrdering[L(1)]] = Vector_(3, 0.1, 0.1, 0.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); CHECK(assert_equal(expected,actual)); diff --git a/gtsam_unstable/slam/tests/testSimulated3D.cpp b/gtsam_unstable/slam/tests/testSimulated3D.cpp index d7633f473..7283544a1 100644 --- a/gtsam_unstable/slam/tests/testSimulated3D.cpp +++ b/gtsam_unstable/slam/tests/testSimulated3D.cpp @@ -15,24 +15,28 @@ * @author Alex Cunningham **/ -#include -#include - #include #include #include #include #include +#include + +#include + using namespace gtsam; -using namespace simulated3D; + +// Convenience for named keys +using symbol_shorthand::X; +using symbol_shorthand::L; /* ************************************************************************* */ TEST( simulated3D, Values ) { Values actual; - actual.insert(Symbol('l',1),Point3(1,1,1)); - actual.insert(Symbol('x',2),Point3(2,2,2)); + actual.insert(L(1),Point3(1,1,1)); + actual.insert(X(2),Point3(2,2,2)); EXPECT(assert_equal(actual,actual,1e-9)); } diff --git a/tests/testDoglegOptimizer.cpp b/tests/testDoglegOptimizer.cpp index f175ca9f3..e9d1e2f05 100644 --- a/tests/testDoglegOptimizer.cpp +++ b/tests/testDoglegOptimizer.cpp @@ -35,6 +35,10 @@ using namespace std; using namespace gtsam; using boost::shared_ptr; +// Convenience for named keys +using symbol_shorthand::X; +using symbol_shorthand::L; + /* ************************************************************************* */ double computeError(const GaussianBayesNet& gbn, const LieVector& values) { @@ -389,11 +393,11 @@ TEST(DoglegOptimizer, Iterate) { // config far from minimum Point2 x0(3,0); boost::shared_ptr config(new Values); - config->insert(Symbol('x',1), x0); + config->insert(X(1), x0); // ordering shared_ptr ord(new Ordering()); - ord->push_back(Symbol('x',1)); + ord->push_back(X(1)); double Delta = 1.0; for(size_t it=0; it<10; ++it) { diff --git a/tests/testExtendedKalmanFilter.cpp b/tests/testExtendedKalmanFilter.cpp index 9071d9c63..440119e66 100644 --- a/tests/testExtendedKalmanFilter.cpp +++ b/tests/testExtendedKalmanFilter.cpp @@ -25,6 +25,10 @@ using namespace gtsam; +// Convenience for named keys +using symbol_shorthand::X; +using symbol_shorthand::L; + /* ************************************************************************* */ TEST( ExtendedKalmanFilter, linear ) { @@ -414,11 +418,11 @@ TEST( ExtendedKalmanFilter, nonlinear ) { Point2 x_predict, x_update; for(unsigned int i = 0; i < 10; ++i){ // Create motion factor - NonlinearMotionModel motionFactor(Symbol('x',i), Symbol('x',i+1)); + NonlinearMotionModel motionFactor(X(i), X(i+1)); x_predict = ekf.predict(motionFactor); // 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); EXPECT(assert_equal(expected_predict[i],x_predict, 1e-6)); diff --git a/tests/testGaussianFactor.cpp b/tests/testGaussianFactor.cpp index f380ad4e2..cb91bc88c 100644 --- a/tests/testGaussianFactor.cpp +++ b/tests/testGaussianFactor.cpp @@ -36,11 +36,15 @@ using namespace boost::assign; using namespace std; using namespace gtsam; +// Convenience for named keys +using symbol_shorthand::X; +using symbol_shorthand::L; + static SharedDiagonal sigma0_1 = sharedSigma(2,0.1), sigma_02 = sharedSigma(2,0.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 ) diff --git a/tests/testGaussianFactorGraphB.cpp b/tests/testGaussianFactorGraphB.cpp index 6eb673018..616c96ab6 100644 --- a/tests/testGaussianFactorGraphB.cpp +++ b/tests/testGaussianFactorGraphB.cpp @@ -42,13 +42,13 @@ using namespace example; double tol=1e-5; -Key kx(size_t i) { return Symbol('x',i); } -Key kl(size_t i) { return Symbol('l',i); } +using symbol_shorthand::X; +using symbol_shorthand::L; /* ************************************************************************* */ 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 fg2 = createGaussianFactorGraph(ordering); EXPECT(fg.equals(fg2)); @@ -56,7 +56,7 @@ TEST( GaussianFactorGraph, equals ) { /* ************************************************************************* */ //TEST( GaussianFactorGraph, error ) { -// Ordering ordering; ordering += kx(1),kx(2),kl(1); +// Ordering ordering; ordering += X(1),X(2),L(1); // FactorGraph fg = createGaussianFactorGraph(ordering); // VectorValues cfg = createZeroDelta(ordering); // @@ -74,10 +74,10 @@ TEST( GaussianFactorGraph, equals ) { //{ // GaussianFactorGraph fg = createGaussianFactorGraph(); // -// set separator = fg.find_separator(kx(2)); +// set separator = fg.find_separator(X(2)); // set expected; -// expected.insert(kx(1)); -// expected.insert(kl(1)); +// expected.insert(X(1)); +// expected.insert(L(1)); // // EXPECT(separator.size()==expected.size()); // set::iterator it1 = separator.begin(), it2 = expected.begin(); @@ -92,7 +92,7 @@ TEST( GaussianFactorGraph, equals ) { // GaussianFactorGraph fg = createGaussianFactorGraph(); // // // combine all factors -// GaussianFactor::shared_ptr actual = removeAndCombineFactors(fg,kx(1)); +// GaussianFactor::shared_ptr actual = removeAndCombineFactors(fg,X(1)); // // // the expected linear factor // Matrix Al1 = Matrix_(6,2, @@ -132,11 +132,11 @@ TEST( GaussianFactorGraph, equals ) { // b(5) = 1; // // vector > meas; -// meas.push_back(make_pair(kl(1), Al1)); -// meas.push_back(make_pair(kx(1), Ax1)); -// meas.push_back(make_pair(kx(2), Ax2)); +// meas.push_back(make_pair(L(1), Al1)); +// meas.push_back(make_pair(X(1), Ax1)); +// meas.push_back(make_pair(X(2), Ax2)); // 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 // EXPECT(assert_equal(expected,*actual)); @@ -149,7 +149,7 @@ TEST( GaussianFactorGraph, equals ) { // GaussianFactorGraph fg = createGaussianFactorGraph(); // // // combine all factors -// GaussianFactor::shared_ptr actual = removeAndCombineFactors(fg,kx(2)); +// GaussianFactor::shared_ptr actual = removeAndCombineFactors(fg,X(2)); // // // the expected linear factor // Matrix Al1 = Matrix_(4,2, @@ -184,9 +184,9 @@ TEST( GaussianFactorGraph, equals ) { // b(3) = 1.5; // // vector > meas; -// meas.push_back(make_pair(kl(1), Al1)); -// meas.push_back(make_pair(kx(1), Ax1)); -// meas.push_back(make_pair(kx(2), Ax2)); +// meas.push_back(make_pair(L(1), Al1)); +// meas.push_back(make_pair(X(1), Ax1)); +// meas.push_back(make_pair(X(2), Ax2)); // GaussianFactor expected(meas, b, ones(4)); // // // check if the two factors are the same @@ -196,7 +196,7 @@ TEST( GaussianFactorGraph, equals ) { /* ************************************************************************* */ 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::FactorizationResult result = inference::eliminateOne(fg, 0, EliminateQR); @@ -204,7 +204,7 @@ TEST( GaussianFactorGraph, eliminateOne_x1 ) // create expected Conditional Gaussian 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); - 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)); } @@ -212,7 +212,7 @@ TEST( GaussianFactorGraph, eliminateOne_x1 ) /* ************************************************************************* */ 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); GaussianConditional::shared_ptr actual = inference::eliminateOne(fg, 0, EliminateQR).first; @@ -220,7 +220,7 @@ TEST( GaussianFactorGraph, eliminateOne_x2 ) double sig = 0.0894427; 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); - 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)); } @@ -228,7 +228,7 @@ TEST( GaussianFactorGraph, eliminateOne_x2 ) /* ************************************************************************* */ 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); GaussianConditional::shared_ptr actual = inference::eliminateOne(fg, 0, EliminateQR).first; @@ -236,7 +236,7 @@ TEST( GaussianFactorGraph, eliminateOne_l1 ) double sig = sqrt(2)/10.; 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); - 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)); } @@ -244,16 +244,16 @@ TEST( GaussianFactorGraph, eliminateOne_l1 ) /* ************************************************************************* */ 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::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; GaussianFactorGraph remaining = result.second; // create expected Conditional Gaussian 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); - 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 JacobianFactor expectedFactor(1, Matrix_(4,2, @@ -275,15 +275,15 @@ TEST( GaussianFactorGraph, eliminateOne_x1_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); - 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 double sig = 0.0894427; 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); - 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)); } @@ -291,15 +291,15 @@ TEST( GaussianFactorGraph, eliminateOne_x2_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); - 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 double sig = sqrt(2)/10.; 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); - 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)); } @@ -311,18 +311,18 @@ TEST( GaussianFactorGraph, eliminateAll ) Matrix I = eye(2); Ordering ordering; - ordering += kx(2),kl(1),kx(1); + ordering += X(2),L(1),X(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; 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; 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 GaussianFactorGraph fg1 = createGaussianFactorGraph(ordering); @@ -340,20 +340,20 @@ TEST( GaussianFactorGraph, eliminateAll ) // Matrix I = eye(2); // // 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; // 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; // 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 // GaussianFactorGraph fg1 = createGaussianFactorGraph(); // Ordering ordering; -// ordering += kx(2),kl(1),kx(1); +// ordering += X(2),L(1),X(1); // GaussianBayesNet actual = fg1.eliminate(ordering, false); // EXPECT(assert_equal(expected,actual,tol)); //} @@ -361,16 +361,16 @@ TEST( GaussianFactorGraph, eliminateAll ) ///* ************************************************************************* */ //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 actual = fg.add_priors(3, vector(3,2)); // GaussianFactorGraph expected = createGaussianFactorGraph(ordering); // Matrix A = eye(2); // Vector b = zero(2); // 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[kx(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[L(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[X(2)],A,b,sigma))); // EXPECT(assert_equal(expected,actual)); //} @@ -378,7 +378,7 @@ TEST( GaussianFactorGraph, eliminateAll ) TEST( GaussianFactorGraph, copying ) { // 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); // Copy the graph ! @@ -399,7 +399,7 @@ TEST( GaussianFactorGraph, copying ) //{ // // render with a given ordering // Ordering ord; -// ord += kx(2),kl(1),kx(1); +// ord += X(2),L(1),X(1); // // // Create a graph // GaussianFactorGraph fg = createGaussianFactorGraph(ordering); @@ -438,7 +438,7 @@ TEST( GaussianFactorGraph, copying ) TEST( GaussianFactorGraph, CONSTRUCTOR_GaussianBayesNet ) { Ordering ord; - ord += kx(2),kl(1),kx(1); + ord += X(2),L(1),X(1); GaussianFactorGraph fg = createGaussianFactorGraph(ord); // render with a given ordering @@ -458,20 +458,20 @@ TEST( GaussianFactorGraph, CONSTRUCTOR_GaussianBayesNet ) /* ************************************************************************* */ TEST( GaussianFactorGraph, getOrdering) { - Ordering original; original += kl(1),kx(1),kx(2); + Ordering original; original += L(1),X(1),X(2); FactorGraph symbolic(createGaussianFactorGraph(original)); Permutation perm(*inference::PermutationCOLAMD(VariableIndex(symbolic))); 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)); } // SL-FIX TEST( GaussianFactorGraph, getOrdering2) //{ // Ordering expected; -// expected += kl(1),kx(1); +// expected += L(1),X(1); // GaussianFactorGraph fg = createGaussianFactorGraph(); -// set interested; interested += kl(1),kx(1); +// set interested; interested += L(1),X(1); // Ordering actual = fg.getOrdering(interested); // EXPECT(assert_equal(expected,actual)); //} @@ -480,7 +480,7 @@ TEST( GaussianFactorGraph, getOrdering) TEST( GaussianFactorGraph, optimize_Cholesky ) { // create an ordering - Ordering ord; ord += kx(2),kl(1),kx(1); + Ordering ord; ord += X(2),L(1),X(1); // create a graph GaussianFactorGraph fg = createGaussianFactorGraph(ord); @@ -498,7 +498,7 @@ TEST( GaussianFactorGraph, optimize_Cholesky ) TEST( GaussianFactorGraph, optimize_QR ) { // create an ordering - Ordering ord; ord += kx(2),kl(1),kx(1); + Ordering ord; ord += X(2),L(1),X(1); // create a graph GaussianFactorGraph fg = createGaussianFactorGraph(ord); @@ -516,7 +516,7 @@ TEST( GaussianFactorGraph, optimize_QR ) // SL-FIX TEST( GaussianFactorGraph, optimizeMultiFrontlas ) //{ // // create an ordering -// Ordering ord; ord += kx(2),kl(1),kx(1); +// Ordering ord; ord += X(2),L(1),X(1); // // // create a graph // GaussianFactorGraph fg = createGaussianFactorGraph(ord); @@ -534,7 +534,7 @@ TEST( GaussianFactorGraph, optimize_QR ) TEST( GaussianFactorGraph, combine) { // 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 GaussianFactorGraph fg1 = createGaussianFactorGraph(ord); @@ -556,7 +556,7 @@ TEST( GaussianFactorGraph, combine) TEST( GaussianFactorGraph, combine2) { // 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 GaussianFactorGraph fg1 = createGaussianFactorGraph(ord); @@ -589,13 +589,13 @@ void print(vector v) { // GaussianFactorGraph fg = createGaussianFactorGraph(); // // // ask for all factor indices connected to x1 -// list x1_factors = fg.factors(kx(1)); +// list x1_factors = fg.factors(X(1)); // size_t x1_indices[] = { 0, 1, 2 }; // list x1_expected(x1_indices, x1_indices + 3); // EXPECT(x1_factors==x1_expected); // // // ask for all factor indices connected to x2 -// list x2_factors = fg.factors(kx(2)); +// list x2_factors = fg.factors(X(2)); // size_t x2_indices[] = { 1, 3 }; // list x2_expected(x2_indices, x2_indices + 2); // EXPECT(x2_factors==x2_expected); @@ -613,7 +613,7 @@ void print(vector v) { // GaussianFactor::shared_ptr f2 = fg[2]; // // // call the function -// vector factors = fg.findAndRemoveFactors(kx(1)); +// vector factors = fg.findAndRemoveFactors(X(1)); // // // Check the factors // EXPECT(f0==factors[0]); @@ -638,7 +638,7 @@ TEST(GaussianFactorGraph, createSmoother) //{ // GaussianFactorGraph fg = createGaussianFactorGraph(); // 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(); // EXPECT(expected==actual); //} @@ -648,7 +648,7 @@ TEST(GaussianFactorGraph, createSmoother) //{ // GaussianFactorGraph fg = createGaussianFactorGraph(); // Ordering expected; -// expected += kl(1),kx(1),kx(2); +// expected += L(1),X(1),X(2); // EXPECT(assert_equal(expected,fg.keys())); //} @@ -656,16 +656,16 @@ TEST(GaussianFactorGraph, createSmoother) // SL-NEEDED? TEST( GaussianFactorGraph, involves ) //{ // GaussianFactorGraph fg = createGaussianFactorGraph(); -// EXPECT(fg.involves(kl(1))); -// EXPECT(fg.involves(kx(1))); -// EXPECT(fg.involves(kx(2))); -// EXPECT(!fg.involves(kx(3))); +// EXPECT(fg.involves(L(1))); +// EXPECT(fg.involves(X(1))); +// EXPECT(fg.involves(X(2))); +// EXPECT(!fg.involves(X(3))); //} /* ************************************************************************* */ double error(const VectorValues& x) { // 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); return fg.error(x); @@ -679,11 +679,11 @@ double error(const VectorValues& x) { // // Construct expected gradient // 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] -// expected.insert(kl(1),Vector_(2, 5.0,-12.5)); -// expected.insert(kx(1),Vector_(2, 30.0, 5.0)); -// expected.insert(kx(2),Vector_(2,-25.0, 17.5)); +// expected.insert(L(1),Vector_(2, 5.0,-12.5)); +// expected.insert(X(1),Vector_(2, 30.0, 5.0)); +// expected.insert(X(2),Vector_(2,-25.0, 17.5)); // // // Check the gradient at delta=0 // VectorValues zero = createZeroDelta(); @@ -696,7 +696,7 @@ double error(const VectorValues& x) { // // // Check the gradient at the solution (should be zero) // Ordering ord; -// ord += kx(2),kl(1),kx(1); +// ord += X(2),L(1),X(1); // GaussianFactorGraph fg2 = createGaussianFactorGraph(); // VectorValues solution = fg2.optimize(ord); // destructive // VectorValues actual2 = fg.gradient(solution); @@ -707,7 +707,7 @@ double error(const VectorValues& x) { TEST( GaussianFactorGraph, multiplication ) { // create an ordering - Ordering ord; ord += kx(2),kl(1),kx(1); + Ordering ord; ord += X(2),L(1),X(1); FactorGraph A = createGaussianFactorGraph(ord); VectorValues x = createCorrectDelta(ord); @@ -724,7 +724,7 @@ TEST( GaussianFactorGraph, multiplication ) // SL-NEEDED? TEST( GaussianFactorGraph, transposeMultiplication ) //{ // // 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); // Errors e; @@ -734,9 +734,9 @@ TEST( GaussianFactorGraph, multiplication ) // e += Vector_(2,-7.5,-5.0); // // VectorValues expected = createZeroDelta(ord), actual = A ^ e; -// expected[ord[kl(1)]] = Vector_(2, -37.5,-50.0); -// expected[ord[kx(1)]] = Vector_(2,-150.0, 25.0); -// expected[ord[kx(2)]] = Vector_(2, 187.5, 25.0); +// expected[ord[L(1)]] = Vector_(2, -37.5,-50.0); +// expected[ord[X(1)]] = Vector_(2,-150.0, 25.0); +// expected[ord[X(2)]] = Vector_(2, 187.5, 25.0); // EXPECT(assert_equal(expected,actual)); //} @@ -744,7 +744,7 @@ TEST( GaussianFactorGraph, multiplication ) // SL-NEEDED? TEST( GaussianFactorGraph, rhs ) //{ // // 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); // Errors expected = createZeroDelta(ord), actual = Ab.rhs(); @@ -760,21 +760,21 @@ TEST( GaussianFactorGraph, multiplication ) TEST( GaussianFactorGraph, elimination ) { Ordering ord; - ord += kx(1), kx(2); + ord += X(1), X(2); // Create Gaussian Factor Graph GaussianFactorGraph fg; Matrix Ap = eye(1), An = eye(1) * -1; Vector b = Vector_(1, 0.0); SharedDiagonal sigma = sharedSigma(1,2.0); - fg.add(ord[kx(1)], An, ord[kx(2)], Ap, b, sigma); - fg.add(ord[kx(1)], Ap, b, sigma); - fg.add(ord[kx(2)], Ap, b, sigma); + fg.add(ord[X(1)], An, ord[X(2)], Ap, b, sigma); + fg.add(ord[X(1)], Ap, b, sigma); + fg.add(ord[X(2)], Ap, b, sigma); // Eliminate GaussianBayesNet bayesNet = *GaussianSequentialSolver(fg).eliminate(); // 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 Matrix R;Vector d; @@ -877,18 +877,18 @@ SharedDiagonal model = sharedSigma(2,1); // GaussianFactorGraph g; // Matrix I = eye(2); // Vector b = Vector_(0, 0, 0); -// g.add(kx(1), I, kx(2), I, b, model); -// g.add(kx(1), I, kx(3), I, b, model); -// g.add(kx(1), I, kx(4), I, b, model); -// g.add(kx(2), I, kx(3), I, b, model); -// g.add(kx(2), I, kx(4), I, b, model); -// g.add(kx(3), I, kx(4), I, b, model); +// g.add(X(1), I, X(2), I, b, model); +// g.add(X(1), I, X(3), I, b, model); +// g.add(X(1), I, X(4), I, b, model); +// g.add(X(2), I, X(3), I, b, model); +// g.add(X(2), I, X(4), I, b, model); +// g.add(X(3), I, X(4), I, b, model); // // map tree = g.findMinimumSpanningTree(); -// EXPECT(tree[kx(1)].compare(kx(1))==0); -// EXPECT(tree[kx(2)].compare(kx(1))==0); -// EXPECT(tree[kx(3)].compare(kx(1))==0); -// EXPECT(tree[kx(4)].compare(kx(1))==0); +// EXPECT(tree[X(1)].compare(X(1))==0); +// EXPECT(tree[X(2)].compare(X(1))==0); +// EXPECT(tree[X(3)].compare(X(1))==0); +// EXPECT(tree[X(4)].compare(X(1))==0); //} ///* ************************************************************************* */ @@ -897,17 +897,17 @@ SharedDiagonal model = sharedSigma(2,1); // GaussianFactorGraph g; // Matrix I = eye(2); // Vector b = Vector_(0, 0, 0); -// g.add(kx(1), I, kx(2), I, b, model); -// g.add(kx(1), I, kx(3), I, b, model); -// g.add(kx(1), I, kx(4), I, b, model); -// g.add(kx(2), I, kx(3), I, b, model); -// g.add(kx(2), I, kx(4), I, b, model); +// g.add(X(1), I, X(2), I, b, model); +// g.add(X(1), I, X(3), I, b, model); +// g.add(X(1), I, X(4), I, b, model); +// g.add(X(2), I, X(3), I, b, model); +// g.add(X(2), I, X(4), I, b, model); // // PredecessorMap tree; -// tree[kx(1)] = kx(1); -// tree[kx(2)] = kx(1); -// tree[kx(3)] = kx(1); -// tree[kx(4)] = kx(1); +// tree[X(1)] = X(1); +// tree[X(2)] = X(1); +// tree[X(3)] = X(1); +// tree[X(4)] = X(1); // // GaussianFactorGraph Ab1, Ab2; // g.split(tree, Ab1, Ab2); @@ -918,17 +918,17 @@ SharedDiagonal model = sharedSigma(2,1); /* ************************************************************************* */ 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)); 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( - 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( - 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( - 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; actual.push_back(f1); @@ -964,7 +964,7 @@ TEST(GaussianFactorGraph, replace) // // // combine in a factor // 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); // // // the expected augmented matrix @@ -992,26 +992,26 @@ TEST(GaussianFactorGraph, replace) // typedef GaussianFactorGraph::sharedFactor Factor; // SharedDiagonal model(Vector_(1, 0.5)); // GaussianFactorGraph fg; -// Factor factor1(new JacobianFactor(kx(1), Matrix_(1,1,1.), kx(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 factor3(new JacobianFactor(kx(3), Matrix_(1,1,1.), kx(3), 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(X(2), Matrix_(1,1,1.), X(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(factor2); // 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_expected; -// GaussianBayesNet::sharedConditional conditional1(new GaussianConditional(kx(2), Vector_(1, 2.), Matrix_(1, 1, 2.), -// kx(1), Matrix_(1, 1, 1.), kx(3), Matrix_(1, 1, 1.), Vector_(1, 1.))); -// GaussianBayesNet::sharedConditional conditional2(new GaussianConditional(kx(1), Vector_(1, 0.), Matrix_(1, 1, -1.), -// kx(3), Matrix_(1, 1, 1.), Vector_(1, 1.))); +// GaussianBayesNet::sharedConditional conditional1(new GaussianConditional(X(2), Vector_(1, 2.), Matrix_(1, 1, 2.), +// X(1), Matrix_(1, 1, 1.), X(3), Matrix_(1, 1, 1.), Vector_(1, 1.))); +// GaussianBayesNet::sharedConditional conditional2(new GaussianConditional(X(1), Vector_(1, 0.), Matrix_(1, 1, -1.), +// X(3), Matrix_(1, 1, 1.), Vector_(1, 1.))); // bn_expected.push_back(conditional1); // bn_expected.push_back(conditional2); // 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; // fg_expected.push_back(factor_expected); // EXPECT(assert_equal(fg_expected, fg)); @@ -1027,8 +1027,8 @@ TEST(GaussianFactorGraph, createSmoother2) LONGS_EQUAL(5,fg2.size()); // eliminate - vector x3var; x3var.push_back(ordering[kx(3)]); - vector x1var; x1var.push_back(ordering[kx(1)]); + vector x3var; x3var.push_back(ordering[X(3)]); + vector x1var; x1var.push_back(ordering[X(1)]); GaussianBayesNet p_x3 = *GaussianSequentialSolver( *GaussianSequentialSolver(fg2).jointFactorGraph(x3var)).eliminate(); GaussianBayesNet p_x1 = *GaussianSequentialSolver( @@ -1045,7 +1045,7 @@ TEST(GaussianFactorGraph, hasConstraints) FactorGraph fgc2 = createSimpleConstraintGraph() ; EXPECT(hasConstraints(fgc2)); - Ordering ordering; ordering += kx(1), kx(2), kl(1); + Ordering ordering; ordering += X(1), X(2), L(1); FactorGraph fg = createGaussianFactorGraph(ordering); EXPECT(!hasConstraints(fg)); } diff --git a/tests/testGaussianISAM.cpp b/tests/testGaussianISAM.cpp index d4946a625..03a747196 100644 --- a/tests/testGaussianISAM.cpp +++ b/tests/testGaussianISAM.cpp @@ -35,8 +35,8 @@ using namespace std; using namespace gtsam; using namespace example; -Key kx(size_t i) { return Symbol('x',i); } -Key kl(size_t i) { return Symbol('l',i); } +using symbol_shorthand::X; +using symbol_shorthand::L; /* ************************************************************************* */ // Some numbers that should be consistent among all smoother tests @@ -50,7 +50,7 @@ const double tol = 1e-4; TEST_UNSAFE( ISAM, iSAM_smoother ) { 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 GaussianFactorGraph smoother = createSmoother(7, ordering).first; @@ -83,7 +83,7 @@ TEST_UNSAFE( ISAM, iSAM_smoother ) // GaussianFactorGraph smoother = createSmoother(7); // // // 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; // for (int i=0;i<7;i++) factors1.push_back(smoother[i]); // GaussianISAM actual(*inference::Eliminate(factors1)); @@ -130,7 +130,7 @@ TEST_UNSAFE( BayesTree, linear_smoother_shortcuts ) EXPECT(assert_equal(empty,actual1,tol)); // 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); EXPECT(assert_equal(empty,actual2,tol)); @@ -138,8 +138,8 @@ TEST_UNSAFE( BayesTree, linear_smoother_shortcuts ) double sigma3 = 0.61808; Matrix A56 = Matrix_(2,2,-0.382022,0.,0.,-0.382022); GaussianBayesNet expected3; - push_front(expected3,ordering[kx(5)], zero(2), eye(2)/sigma3, ordering[kx(6)], A56/sigma3, ones(2)); - GaussianISAM::sharedClique C3 = isamTree[ordering[kx(4)]]; + push_front(expected3,ordering[X(5)], zero(2), eye(2)/sigma3, ordering[X(6)], A56/sigma3, ones(2)); + GaussianISAM::sharedClique C3 = isamTree[ordering[X(4)]]; GaussianBayesNet actual3 = GaussianISAM::shortcut(C3,R); EXPECT(assert_equal(expected3,actual3,tol)); @@ -147,8 +147,8 @@ TEST_UNSAFE( BayesTree, linear_smoother_shortcuts ) double sigma4 = 0.661968; Matrix A46 = Matrix_(2,2,-0.146067,0.,0.,-0.146067); GaussianBayesNet expected4; - push_front(expected4, ordering[kx(4)], zero(2), eye(2)/sigma4, ordering[kx(6)], A46/sigma4, ones(2)); - GaussianISAM::sharedClique C4 = isamTree[ordering[kx(3)]]; + push_front(expected4, ordering[X(4)], zero(2), eye(2)/sigma4, ordering[X(6)], A46/sigma4, ones(2)); + GaussianISAM::sharedClique C4 = isamTree[ordering[X(3)]]; GaussianBayesNet actual4 = GaussianISAM::shortcut(C4,R); EXPECT(assert_equal(expected4,actual4,tol)); } @@ -176,7 +176,7 @@ TEST_UNSAFE( BayesTree, balanced_smoother_marginals ) { // Create smoother with 7 nodes 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; // Create the Bayes tree @@ -193,48 +193,48 @@ TEST_UNSAFE( BayesTree, balanced_smoother_marginals ) double tol=1e-5; // Check marginal on x1 - GaussianBayesNet expected1 = simpleGaussian(ordering[kx(1)], zero(2), sigmax1); - GaussianBayesNet actual1 = *bayesTree.marginalBayesNet(ordering[kx(1)]); + GaussianBayesNet expected1 = simpleGaussian(ordering[X(1)], zero(2), sigmax1); + GaussianBayesNet actual1 = *bayesTree.marginalBayesNet(ordering[X(1)]); Matrix expectedCovarianceX1 = eye(2,2) * (sigmax1 * sigmax1); Matrix actualCovarianceX1; - actualCovarianceX1 = bayesTree.marginalCovariance(ordering[kx(1)]); + actualCovarianceX1 = bayesTree.marginalCovariance(ordering[X(1)]); EXPECT(assert_equal(expectedCovarianceX1, actualCovarianceX1, tol)); EXPECT(assert_equal(expected1,actual1,tol)); // Check marginal on x2 double sigx2 = 0.68712938; // FIXME: this should be corrected analytically - GaussianBayesNet expected2 = simpleGaussian(ordering[kx(2)], zero(2), sigx2); - GaussianBayesNet actual2 = *bayesTree.marginalBayesNet(ordering[kx(2)]); + GaussianBayesNet expected2 = simpleGaussian(ordering[X(2)], zero(2), sigx2); + GaussianBayesNet actual2 = *bayesTree.marginalBayesNet(ordering[X(2)]); Matrix expectedCovarianceX2 = eye(2,2) * (sigx2 * sigx2); Matrix actualCovarianceX2; - actualCovarianceX2 = bayesTree.marginalCovariance(ordering[kx(2)]); + actualCovarianceX2 = bayesTree.marginalCovariance(ordering[X(2)]); EXPECT(assert_equal(expectedCovarianceX2, actualCovarianceX2, tol)); EXPECT(assert_equal(expected2,actual2,tol)); // Check marginal on x3 - GaussianBayesNet expected3 = simpleGaussian(ordering[kx(3)], zero(2), sigmax3); - GaussianBayesNet actual3 = *bayesTree.marginalBayesNet(ordering[kx(3)]); + GaussianBayesNet expected3 = simpleGaussian(ordering[X(3)], zero(2), sigmax3); + GaussianBayesNet actual3 = *bayesTree.marginalBayesNet(ordering[X(3)]); Matrix expectedCovarianceX3 = eye(2,2) * (sigmax3 * sigmax3); Matrix actualCovarianceX3; - actualCovarianceX3 = bayesTree.marginalCovariance(ordering[kx(3)]); + actualCovarianceX3 = bayesTree.marginalCovariance(ordering[X(3)]); EXPECT(assert_equal(expectedCovarianceX3, actualCovarianceX3, tol)); EXPECT(assert_equal(expected3,actual3,tol)); // Check marginal on x4 - GaussianBayesNet expected4 = simpleGaussian(ordering[kx(4)], zero(2), sigmax4); - GaussianBayesNet actual4 = *bayesTree.marginalBayesNet(ordering[kx(4)]); + GaussianBayesNet expected4 = simpleGaussian(ordering[X(4)], zero(2), sigmax4); + GaussianBayesNet actual4 = *bayesTree.marginalBayesNet(ordering[X(4)]); Matrix expectedCovarianceX4 = eye(2,2) * (sigmax4 * sigmax4); Matrix actualCovarianceX4; - actualCovarianceX4 = bayesTree.marginalCovariance(ordering[kx(4)]); + actualCovarianceX4 = bayesTree.marginalCovariance(ordering[X(4)]); EXPECT(assert_equal(expectedCovarianceX4, actualCovarianceX4, tol)); EXPECT(assert_equal(expected4,actual4,tol)); // Check marginal on x7 (should be equal to x1) - GaussianBayesNet expected7 = simpleGaussian(ordering[kx(7)], zero(2), sigmax7); - GaussianBayesNet actual7 = *bayesTree.marginalBayesNet(ordering[kx(7)]); + GaussianBayesNet expected7 = simpleGaussian(ordering[X(7)], zero(2), sigmax7); + GaussianBayesNet actual7 = *bayesTree.marginalBayesNet(ordering[X(7)]); Matrix expectedCovarianceX7 = eye(2,2) * (sigmax7 * sigmax7); Matrix actualCovarianceX7; - actualCovarianceX7 = bayesTree.marginalCovariance(ordering[kx(7)]); + actualCovarianceX7 = bayesTree.marginalCovariance(ordering[X(7)]); EXPECT(assert_equal(expectedCovarianceX7, actualCovarianceX7, tol)); EXPECT(assert_equal(expected7,actual7,tol)); } @@ -244,7 +244,7 @@ TEST_UNSAFE( BayesTree, balanced_smoother_shortcuts ) { // Create smoother with 7 nodes 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; // Create the Bayes tree @@ -258,19 +258,19 @@ TEST_UNSAFE( BayesTree, balanced_smoother_shortcuts ) EXPECT(assert_equal(empty,actual1,tol)); // 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); EXPECT(assert_equal(empty,actual2,tol)); // Check the conditional P(C3|Root), which should be equal to P(x2|x4) /** 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. */ -// 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: "); // 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); // EXPECT(assert_equal(expected3,actual3,tol)); } @@ -280,7 +280,7 @@ TEST_UNSAFE( BayesTree, balanced_smoother_shortcuts ) //{ // // Create smoother with 7 nodes // 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; // // // Create the Bayes tree @@ -289,9 +289,9 @@ TEST_UNSAFE( BayesTree, balanced_smoother_shortcuts ) // // // Check the clique marginal P(C3) // double sigmax2_alt = 1/1.45533; // THIS NEEDS TO BE CHECKED! -// GaussianBayesNet expected = simpleGaussian(ordering[kx(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)); -// GaussianISAM::sharedClique R = bayesTree.root(), C3 = bayesTree[ordering[kx(1)]]; +// GaussianBayesNet expected = simpleGaussian(ordering[X(2)],zero(2),sigmax2_alt); +// 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[X(1)]]; // GaussianFactorGraph marginal = C3->marginal(R); // GaussianVariableIndex varIndex(marginal); // Permutation toFront(Permutation::PullToFront(C3->keys(), varIndex.size())); @@ -309,7 +309,7 @@ TEST_UNSAFE( BayesTree, balanced_smoother_joint ) { // Create smoother with 7 nodes 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; // Create the Bayes tree, expected to look like: @@ -328,41 +328,41 @@ TEST_UNSAFE( BayesTree, balanced_smoother_joint ) GaussianBayesNet expected1; // Why does the sign get flipped on the prior? 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); - push_front(expected1,ordering[kx(1)], zero(2), I/sigmax7, ordering[kx(7)], A/sigmax7, sigma); - GaussianBayesNet actual1 = *bayesTree.jointBayesNet(ordering[kx(1)],ordering[kx(7)]); + push_front(expected1,ordering[X(1)], zero(2), I/sigmax7, ordering[X(7)], A/sigmax7, sigma); + GaussianBayesNet actual1 = *bayesTree.jointBayesNet(ordering[X(1)],ordering[X(7)]); EXPECT(assert_equal(expected1,actual1,tol)); // // Check the joint density P(x7,x1) factored as P(x7|x1)P(x1) // GaussianBayesNet expected2; // 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); -// push_front(expected2,ordering[kx(7)], zero(2), I/sigmax1, ordering[kx(1)], A/sigmax1, sigma); -// GaussianBayesNet actual2 = *bayesTree.jointBayesNet(ordering[kx(7)],ordering[kx(1)]); +// push_front(expected2,ordering[X(7)], zero(2), I/sigmax1, ordering[X(1)], A/sigmax1, sigma); +// GaussianBayesNet actual2 = *bayesTree.jointBayesNet(ordering[X(7)],ordering[X(1)]); // EXPECT(assert_equal(expected2,actual2,tol)); // Check the joint density P(x1,x4), i.e. with a root variable GaussianBayesNet expected3; 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); double sig14 = 0.784465; Matrix A14 = -0.0769231*I; - push_front(expected3,ordering[kx(1)], zero(2), I/sig14, ordering[kx(4)], A14/sig14, sigma); - GaussianBayesNet actual3 = *bayesTree.jointBayesNet(ordering[kx(1)],ordering[kx(4)]); + push_front(expected3,ordering[X(1)], zero(2), I/sig14, ordering[X(4)], A14/sig14, sigma); + GaussianBayesNet actual3 = *bayesTree.jointBayesNet(ordering[X(1)],ordering[X(4)]); EXPECT(assert_equal(expected3,actual3,tol)); // // Check the joint density P(x4,x1), i.e. with a root variable, factored the other way // GaussianBayesNet expected4; // 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); // double sig41 = 0.668096; // Matrix A41 = -0.055794*I; -// push_front(expected4,ordering[kx(4)], zero(2), I/sig41, ordering[kx(1)], A41/sig41, sigma); -// GaussianBayesNet actual4 = *bayesTree.jointBayesNet(ordering[kx(4)],ordering[kx(1)]); +// push_front(expected4,ordering[X(4)], zero(2), I/sig41, ordering[X(1)], A41/sig41, sigma); +// GaussianBayesNet actual4 = *bayesTree.jointBayesNet(ordering[X(4)],ordering[X(1)]); // EXPECT(assert_equal(expected4,actual4,tol)); } diff --git a/tests/testGaussianJunctionTreeB.cpp b/tests/testGaussianJunctionTreeB.cpp index 7d73ef83b..03e2d27d3 100644 --- a/tests/testGaussianJunctionTreeB.cpp +++ b/tests/testGaussianJunctionTreeB.cpp @@ -42,8 +42,8 @@ using namespace std; using namespace gtsam; using namespace example; -Key kx(size_t i) { return Symbol('x',i); } -Key kl(size_t i) { return Symbol('l',i); } +using symbol_shorthand::X; +using symbol_shorthand::L; /* ************************************************************************* * Bayes tree for smoother with "nested dissection" ordering: @@ -55,20 +55,20 @@ Key kl(size_t i) { return Symbol('l',i); } TEST( GaussianJunctionTree, constructor2 ) { // 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; // create an ordering GaussianJunctionTree actual(fg); - vector frontal1; frontal1 += ordering[kx(5)], ordering[kx(6)], ordering[kx(4)]; - vector frontal2; frontal2 += ordering[kx(3)], ordering[kx(2)]; - vector frontal3; frontal3 += ordering[kx(1)]; - vector frontal4; frontal4 += ordering[kx(7)]; + vector frontal1; frontal1 += ordering[X(5)], ordering[X(6)], ordering[X(4)]; + vector frontal2; frontal2 += ordering[X(3)], ordering[X(2)]; + vector frontal3; frontal3 += ordering[X(1)]; + vector frontal4; frontal4 += ordering[X(7)]; vector sep1; - vector sep2; sep2 += ordering[kx(4)]; - vector sep3; sep3 += ordering[kx(2)]; - vector sep4; sep4 += ordering[kx(6)]; + vector sep2; sep2 += ordering[X(4)]; + vector sep3; sep3 += ordering[X(2)]; + vector sep4; sep4 += ordering[X(6)]; EXPECT(assert_equal(frontal1, actual.root()->frontal)); EXPECT(assert_equal(sep1, actual.root()->separator)); LONGS_EQUAL(5, actual.root()->size()); @@ -103,7 +103,7 @@ TEST( GaussianJunctionTree, optimizeMultiFrontal ) VectorValues expected(vector(7,2)); // expected solution Vector v = Vector_(2, 0., 0.); for (int i=1; i<=7; i++) - expected[ordering[Symbol('x',i)]] = v; + expected[ordering[X(i)]] = v; EXPECT(assert_equal(expected,actual)); } @@ -113,7 +113,7 @@ TEST( GaussianJunctionTree, optimizeMultiFrontal2) // create a graph example::Graph nlfg = createNonlinearFactorGraph(); 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); // optimize the graph @@ -136,39 +136,39 @@ TEST(GaussianJunctionTree, slamlike) { size_t i = 0; newfactors = planarSLAM::Graph(); - newfactors.addPrior(kx(0), Pose2(0.0, 0.0, 0.0), odoNoise); - init.insert(kx(0), Pose2(0.01, 0.01, 0.01)); + newfactors.addPrior(X(0), Pose2(0.0, 0.0, 0.0), odoNoise); + init.insert(X(0), Pose2(0.01, 0.01, 0.01)); fullgraph.push_back(newfactors); for( ; i<5; ++i) { newfactors = planarSLAM::Graph(); - newfactors.addOdometry(kx(i), kx(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)); + newfactors.addOdometry(X(i), X(i+1), Pose2(1.0, 0.0, 0.0), odoNoise); + init.insert(X(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); fullgraph.push_back(newfactors); } newfactors = planarSLAM::Graph(); - newfactors.addOdometry(kx(i), kx(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(kx(i), kl(1), Rot2::fromAngle(-M_PI/4.0), 5.0, brNoise); - init.insert(kx(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(kl(1), Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0))); + newfactors.addOdometry(X(i), X(i+1), Pose2(1.0, 0.0, 0.0), odoNoise); + newfactors.addBearingRange(X(i), L(0), 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(X(i+1), Pose2(1.01, 0.01, 0.01)); + init.insert(L(0), 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); ++ i; for( ; i<5; ++i) { newfactors = planarSLAM::Graph(); - newfactors.addOdometry(kx(i), kx(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)); + newfactors.addOdometry(X(i), X(i+1), Pose2(1.0, 0.0, 0.0), odoNoise); + init.insert(X(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); fullgraph.push_back(newfactors); } newfactors = planarSLAM::Graph(); - newfactors.addOdometry(kx(i), kx(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(kx(i), kl(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)); + newfactors.addOdometry(X(i), X(i+1), Pose2(1.0, 0.0, 0.0), odoNoise); + newfactors.addBearingRange(X(i), L(0), 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(X(i+1), Pose2(6.9, 0.1, 0.01)); fullgraph.push_back(newfactors); ++ i; @@ -194,15 +194,15 @@ TEST(GaussianJunctionTree, simpleMarginal) { // Create a simple graph pose2SLAM::Graph fg; - fg.addPrior(kx(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.addPrior(X(0), Pose2(), sharedSigma(3, 10.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; - init.insert(kx(0), Pose2()); - init.insert(kx(1), Pose2(1.0, 0.0, 0.0)); + init.insert(X(0), Pose2()); + init.insert(X(1), Pose2(1.0, 0.0, 0.0)); Ordering ordering; - ordering += kx(1), kx(0); + ordering += X(1), X(0); GaussianFactorGraph gfg = *fg.linearize(init, ordering); diff --git a/tests/testInferenceB.cpp b/tests/testInferenceB.cpp index e023fe7e3..262d1801c 100644 --- a/tests/testInferenceB.cpp +++ b/tests/testInferenceB.cpp @@ -27,8 +27,8 @@ using namespace std; using namespace gtsam; // Convenience for named keys -Key kx(size_t i) { return Symbol('x',i); } -Key kl(size_t i) { return Symbol('l',i); } +using symbol_shorthand::X; +using symbol_shorthand::L; /* ************************************************************************* */ // 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 pointModel(sharedSigma(3, 0.1)); - fg.addPrior(kx(0), Pose2(), poseModel); - fg.addOdometry(kx(0), kx(1), Pose2(1.0,0.0,0.0), poseModel); - fg.addOdometry(kx(1), kx(2), Pose2(1.0,0.0,0.0), poseModel); - fg.addBearingRange(kx(0), kl(0), Rot2(), 1.0, pointModel); - fg.addBearingRange(kx(1), kl(0), Rot2(), 1.0, pointModel); - fg.addBearingRange(kx(2), kl(0), Rot2(), 1.0, pointModel); + fg.addPrior(X(0), Pose2(), poseModel); + fg.addOdometry(X(0), X(1), Pose2(1.0,0.0,0.0), poseModel); + fg.addOdometry(X(1), X(2), Pose2(1.0,0.0,0.0), poseModel); + fg.addBearingRange(X(0), L(0), Rot2(), 1.0, pointModel); + fg.addBearingRange(X(1), L(0), Rot2(), 1.0, pointModel); + fg.addBearingRange(X(2), L(0), Rot2(), 1.0, pointModel); Values init; - init.insert(kx(0), Pose2(0.0,0.0,0.0)); - init.insert(kx(1), Pose2(1.0,0.0,0.0)); - init.insert(kx(2), Pose2(2.0,0.0,0.0)); - init.insert(kl(0), Point2(1.0,1.0)); + init.insert(X(0), Pose2(0.0,0.0,0.0)); + init.insert(X(1), Pose2(1.0,0.0,0.0)); + init.insert(X(2), Pose2(2.0,0.0,0.0)); + init.insert(L(0), Point2(1.0,1.0)); Ordering ordering(*fg.orderingCOLAMD(init)); FactorGraph::shared_ptr gfg(fg.linearize(init, ordering)); GaussianMultifrontalSolver solver(*gfg); - solver.marginalFactor(ordering[kl(0)]); + solver.marginalFactor(ordering[L(0)]); } /* ************************************************************************* */ diff --git a/tests/testNonlinearFactor.cpp b/tests/testNonlinearFactor.cpp index eac14d480..e203bcf94 100644 --- a/tests/testNonlinearFactor.cpp +++ b/tests/testNonlinearFactor.cpp @@ -39,8 +39,8 @@ using namespace gtsam; using namespace example; // Convenience for named keys -Key kx(size_t i) { return Symbol('x',i); } -Key kl(size_t i) { return Symbol('l',i); } +using symbol_shorthand::X; +using symbol_shorthand::L; typedef boost::shared_ptr shared_nlf; @@ -51,11 +51,11 @@ TEST( NonlinearFactor, equals ) // create two nonlinear2 factors 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 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(f0.equals(f0)); @@ -201,16 +201,16 @@ TEST( NonlinearFactor, linearize_constraint1 ) SharedDiagonal constraint = noiseModel::Constrained::MixedSigmas(sigmas); 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; - 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()); // create expected Ordering ord(*config.orderingArbitrary()); 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)); } @@ -221,18 +221,18 @@ TEST( NonlinearFactor, linearize_constraint2 ) SharedDiagonal constraint = noiseModel::Constrained::MixedSigmas(sigmas); Point2 z3(1.,-1.); - simulated2D::Measurement f0(z3, constraint, kx(1),kl(1)); + simulated2D::Measurement f0(z3, constraint, X(1),L(1)); Values config; - config.insert(kx(1), Point2(1.0, 2.0)); - config.insert(kl(1), Point2(5.0, 4.0)); + config.insert(X(1), Point2(1.0, 2.0)); + config.insert(L(1), Point2(5.0, 4.0)); GaussianFactor::shared_ptr actual = f0.linearize(config, *config.orderingArbitrary()); // create expected Ordering ord(*config.orderingArbitrary()); Matrix A = Matrix_(2,2, 5.0, 0.0, 0.0, 1.0); 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)); } @@ -240,7 +240,7 @@ TEST( NonlinearFactor, linearize_constraint2 ) class TestFactor4 : public NoiseModelFactor4 { public: typedef NoiseModelFactor4 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 evaluateError(const LieVector& x1, const LieVector& x2, const LieVector& x3, const LieVector& x4, @@ -264,13 +264,13 @@ public: TEST(NonlinearFactor, NoiseModelFactor4) { TestFactor4 tf; Values tv; - tv.insert(kx(1), LieVector(1, 1.0)); - tv.insert(kx(2), LieVector(1, 2.0)); - tv.insert(kx(3), LieVector(1, 3.0)); - tv.insert(kx(4), LieVector(1, 4.0)); + tv.insert(X(1), LieVector(1, 1.0)); + tv.insert(X(2), LieVector(1, 2.0)); + tv.insert(X(3), LieVector(1, 3.0)); + tv.insert(X(4), LieVector(1, 4.0)); EXPECT(assert_equal(Vector_(1, 10.0), tf.unwhitenedError(tv))); 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(tf.linearize(tv, ordering))); LONGS_EQUAL(jf.keys()[0], 0); LONGS_EQUAL(jf.keys()[1], 1); @@ -287,7 +287,7 @@ TEST(NonlinearFactor, NoiseModelFactor4) { class TestFactor5 : public NoiseModelFactor5 { public: typedef NoiseModelFactor5 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 evaluateError(const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, @@ -313,14 +313,14 @@ public: TEST(NonlinearFactor, NoiseModelFactor5) { TestFactor5 tf; Values tv; - tv.insert(kx(1), LieVector(1, 1.0)); - tv.insert(kx(2), LieVector(1, 2.0)); - tv.insert(kx(3), LieVector(1, 3.0)); - tv.insert(kx(4), LieVector(1, 4.0)); - tv.insert(kx(5), LieVector(1, 5.0)); + tv.insert(X(1), LieVector(1, 1.0)); + tv.insert(X(2), LieVector(1, 2.0)); + tv.insert(X(3), LieVector(1, 3.0)); + tv.insert(X(4), LieVector(1, 4.0)); + tv.insert(X(5), LieVector(1, 5.0)); EXPECT(assert_equal(Vector_(1, 15.0), tf.unwhitenedError(tv))); 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(tf.linearize(tv, ordering))); LONGS_EQUAL(jf.keys()[0], 0); LONGS_EQUAL(jf.keys()[1], 1); @@ -339,7 +339,7 @@ TEST(NonlinearFactor, NoiseModelFactor5) { class TestFactor6 : public NoiseModelFactor6 { public: typedef NoiseModelFactor6 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 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) { TestFactor6 tf; Values tv; - tv.insert(kx(1), LieVector(1, 1.0)); - tv.insert(kx(2), LieVector(1, 2.0)); - tv.insert(kx(3), LieVector(1, 3.0)); - tv.insert(kx(4), LieVector(1, 4.0)); - tv.insert(kx(5), LieVector(1, 5.0)); - tv.insert(kx(6), LieVector(1, 6.0)); + tv.insert(X(1), LieVector(1, 1.0)); + tv.insert(X(2), LieVector(1, 2.0)); + tv.insert(X(3), LieVector(1, 3.0)); + tv.insert(X(4), LieVector(1, 4.0)); + tv.insert(X(5), LieVector(1, 5.0)); + tv.insert(X(6), LieVector(1, 6.0)); EXPECT(assert_equal(Vector_(1, 21.0), tf.unwhitenedError(tv))); 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(tf.linearize(tv, ordering))); LONGS_EQUAL(jf.keys()[0], 0); LONGS_EQUAL(jf.keys()[1], 1); @@ -397,10 +397,10 @@ TEST(NonlinearFactor, NoiseModelFactor6) { TEST( NonlinearFactor, clone_rekey ) { shared_nlf init(new TestFactor4()); - EXPECT_LONGS_EQUAL(kx(1), init->keys()[0]); - EXPECT_LONGS_EQUAL(kx(2), init->keys()[1]); - EXPECT_LONGS_EQUAL(kx(3), init->keys()[2]); - EXPECT_LONGS_EQUAL(kx(4), init->keys()[3]); + EXPECT_LONGS_EQUAL(X(1), init->keys()[0]); + EXPECT_LONGS_EQUAL(X(2), init->keys()[1]); + EXPECT_LONGS_EQUAL(X(3), init->keys()[2]); + EXPECT_LONGS_EQUAL(X(4), init->keys()[3]); // Standard clone shared_nlf actClone = init->clone(); @@ -409,24 +409,24 @@ TEST( NonlinearFactor, clone_rekey ) // Re-key factor - clones with different keys std::vector new_keys(4); - new_keys[0] = kx(5); - new_keys[1] = kx(6); - new_keys[2] = kx(7); - new_keys[3] = kx(8); + new_keys[0] = X(5); + new_keys[1] = X(6); + new_keys[2] = X(7); + new_keys[3] = X(8); shared_nlf actRekey = init->rekey(new_keys); EXPECT(actRekey.get() != init.get()); // Ensure different pointers // Ensure init is unchanged - EXPECT_LONGS_EQUAL(kx(1), init->keys()[0]); - EXPECT_LONGS_EQUAL(kx(2), init->keys()[1]); - EXPECT_LONGS_EQUAL(kx(3), init->keys()[2]); - EXPECT_LONGS_EQUAL(kx(4), init->keys()[3]); + EXPECT_LONGS_EQUAL(X(1), init->keys()[0]); + EXPECT_LONGS_EQUAL(X(2), init->keys()[1]); + EXPECT_LONGS_EQUAL(X(3), init->keys()[2]); + EXPECT_LONGS_EQUAL(X(4), init->keys()[3]); // Check new keys - EXPECT_LONGS_EQUAL(kx(5), actRekey->keys()[0]); - EXPECT_LONGS_EQUAL(kx(6), actRekey->keys()[1]); - EXPECT_LONGS_EQUAL(kx(7), actRekey->keys()[2]); - EXPECT_LONGS_EQUAL(kx(8), actRekey->keys()[3]); + EXPECT_LONGS_EQUAL(X(5), actRekey->keys()[0]); + EXPECT_LONGS_EQUAL(X(6), actRekey->keys()[1]); + EXPECT_LONGS_EQUAL(X(7), actRekey->keys()[2]); + EXPECT_LONGS_EQUAL(X(8), actRekey->keys()[3]); } /* ************************************************************************* */ diff --git a/tests/testNonlinearFactorGraph.cpp b/tests/testNonlinearFactorGraph.cpp index 59ee0977e..d1d240a67 100644 --- a/tests/testNonlinearFactorGraph.cpp +++ b/tests/testNonlinearFactorGraph.cpp @@ -37,8 +37,8 @@ using namespace boost::assign; using namespace gtsam; using namespace example; -Key kx(size_t i) { return Symbol('x',i); } -Key kl(size_t i) { return Symbol('l',i); } +using symbol_shorthand::X; +using symbol_shorthand::L; /* ************************************************************************* */ TEST( Graph, equals ) @@ -68,16 +68,16 @@ TEST( Graph, keys ) set actual = fg.keys(); LONGS_EQUAL(3, actual.size()); set::const_iterator it = actual.begin(); - LONGS_EQUAL(kl(1), *(it++)); - LONGS_EQUAL(kx(1), *(it++)); - LONGS_EQUAL(kx(2), *(it++)); + LONGS_EQUAL(L(1), *(it++)); + LONGS_EQUAL(X(1), *(it++)); + LONGS_EQUAL(X(2), *(it++)); } /* ************************************************************************* */ TEST( Graph, GET_ORDERING) { // 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(); SymbolicFactorGraph::shared_ptr symbolic; Ordering::shared_ptr ordering; @@ -123,7 +123,7 @@ TEST( Graph, rekey ) { Graph init = createNonlinearFactorGraph(); map 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); // ensure deep clone @@ -139,8 +139,8 @@ TEST( Graph, rekey ) // updated measurements Point2 z3(0, -1), z4(-1.5, -1.); SharedDiagonal sigma0_2 = noiseModel::Isotropic::Sigma(2,0.2); - expRekey.add(simulated2D::Measurement(z3, sigma0_2, kx(1), kl(4))); - expRekey.add(simulated2D::Measurement(z4, sigma0_2, kx(2), kl(4))); + expRekey.add(simulated2D::Measurement(z3, sigma0_2, X(1), L(4))); + expRekey.add(simulated2D::Measurement(z4, sigma0_2, X(2), L(4))); EXPECT(assert_equal(expRekey, actRekey)); } diff --git a/tests/testNonlinearOptimizer.cpp b/tests/testNonlinearOptimizer.cpp index a59584c96..7cb8bb045 100644 --- a/tests/testNonlinearOptimizer.cpp +++ b/tests/testNonlinearOptimizer.cpp @@ -39,8 +39,8 @@ using namespace gtsam; const double tol = 1e-5; -Key kx(size_t i) { return Symbol('x',i); } -Key kl(size_t i) { return Symbol('l',i); } +using symbol_shorthand::X; +using symbol_shorthand::L; /* ************************************************************************* */ TEST( NonlinearOptimizer, iterateLM ) @@ -51,7 +51,7 @@ TEST( NonlinearOptimizer, iterateLM ) // config far from minimum Point2 x0(3,0); Values config; - config.insert(kx(1), x0); + config.insert(X(1), x0); // normal iterate GaussNewtonParams gnParams; @@ -75,18 +75,18 @@ TEST( NonlinearOptimizer, optimize ) // test error at minimum Point2 xstar(0,0); Values cstar; - cstar.insert(kx(1), xstar); + cstar.insert(X(1), xstar); DOUBLES_EQUAL(0.0,fg.error(cstar),0.0); // test error at initial = [(1-cos(3))^2 + (sin(3))^2]*50 = Point2 x0(3,3); Values c0; - c0.insert(kx(1), x0); + c0.insert(X(1), x0); DOUBLES_EQUAL(199.0,fg.error(c0),1e-3); // optimize parameters Ordering ord; - ord.push_back(kx(1)); + ord.push_back(X(1)); // Gauss-Newton GaussNewtonParams gnParams; @@ -114,7 +114,7 @@ TEST( NonlinearOptimizer, SimpleLMOptimizer ) Point2 x0(3,3); Values c0; - c0.insert(kx(1), x0); + c0.insert(X(1), x0); Values actual = LevenbergMarquardtOptimizer(fg, c0).optimize(); DOUBLES_EQUAL(0,fg.error(actual),tol); @@ -127,7 +127,7 @@ TEST( NonlinearOptimizer, SimpleGNOptimizer ) Point2 x0(3,3); Values c0; - c0.insert(kx(1), x0); + c0.insert(X(1), x0); Values actual = GaussNewtonOptimizer(fg, c0).optimize(); DOUBLES_EQUAL(0,fg.error(actual),tol); @@ -140,7 +140,7 @@ TEST( NonlinearOptimizer, SimpleDLOptimizer ) Point2 x0(3,3); Values c0; - c0.insert(kx(1), x0); + c0.insert(X(1), x0); Values actual = DoglegOptimizer(fg, c0).optimize(); DOUBLES_EQUAL(0,fg.error(actual),tol); @@ -158,7 +158,7 @@ TEST( NonlinearOptimizer, optimization_method ) Point2 x0(3,3); Values c0; - c0.insert(kx(1), x0); + c0.insert(X(1), x0); Values actualMFQR = LevenbergMarquardtOptimizer(fg, c0, paramsQR).optimize(); DOUBLES_EQUAL(0,fg.error(actualMFQR),tol); @@ -171,23 +171,23 @@ TEST( NonlinearOptimizer, optimization_method ) TEST( NonlinearOptimizer, Factorization ) { Values config; - config.insert(kx(1), Pose2(0.,0.,0.)); - config.insert(kx(2), Pose2(1.5,0.,0.)); + config.insert(X(1), Pose2(0.,0.,0.)); + config.insert(X(2), Pose2(1.5,0.,0.)); pose2SLAM::Graph graph; - graph.addPrior(kx(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.addPrior(X(1), Pose2(0.,0.,0.), noiseModel::Isotropic::Sigma(3, 1e-10)); + graph.addOdometry(X(1),X(2), Pose2(1.,0.,0.), noiseModel::Isotropic::Sigma(3, 1)); Ordering ordering; - ordering.push_back(kx(1)); - ordering.push_back(kx(2)); + ordering.push_back(X(1)); + ordering.push_back(X(2)); LevenbergMarquardtOptimizer optimizer(graph, config, ordering); optimizer.iterate(); Values expected; - expected.insert(kx(1), Pose2(0.,0.,0.)); - expected.insert(kx(2), Pose2(1.,0.,0.)); + expected.insert(X(1), Pose2(0.,0.,0.)); + expected.insert(X(2), Pose2(1.,0.,0.)); CHECK(assert_equal(expected, optimizer.values(), 1e-5)); } @@ -202,18 +202,18 @@ TEST(NonlinearOptimizer, NullFactor) { // test error at minimum Point2 xstar(0,0); Values cstar; - cstar.insert(kx(1), xstar); + cstar.insert(X(1), xstar); DOUBLES_EQUAL(0.0,fg.error(cstar),0.0); // test error at initial = [(1-cos(3))^2 + (sin(3))^2]*50 = Point2 x0(3,3); Values c0; - c0.insert(kx(1), x0); + c0.insert(X(1), x0); DOUBLES_EQUAL(199.0,fg.error(c0),1e-3); // optimize parameters Ordering ord; - ord.push_back(kx(1)); + ord.push_back(X(1)); // Gauss-Newton Values actual1 = GaussNewtonOptimizer(fg, c0, ord).optimize(); diff --git a/tests/testSymbolicBayesNetB.cpp b/tests/testSymbolicBayesNetB.cpp index 356f4bb34..4f0d04595 100644 --- a/tests/testSymbolicBayesNetB.cpp +++ b/tests/testSymbolicBayesNetB.cpp @@ -32,18 +32,18 @@ using namespace std; using namespace gtsam; using namespace example; -Key kx(size_t i) { return Symbol('x',i); } -Key kl(size_t i) { return Symbol('l',i); } +using symbol_shorthand::X; +using symbol_shorthand::L; /* ************************************************************************* */ TEST( SymbolicBayesNet, constructor ) { - Ordering o; o += kx(2),kl(1),kx(1); + Ordering o; o += X(2),L(1),X(1); // Create manually IndexConditional::shared_ptr - x2(new IndexConditional(o[kx(2)],o[kl(1)], o[kx(1)])), - l1(new IndexConditional(o[kl(1)],o[kx(1)])), - x1(new IndexConditional(o[kx(1)])); + x2(new IndexConditional(o[X(2)],o[L(1)], o[X(1)])), + l1(new IndexConditional(o[L(1)],o[X(1)])), + x1(new IndexConditional(o[X(1)])); BayesNet expected; expected.push_back(x2); expected.push_back(l1); diff --git a/tests/testSymbolicFactorGraphB.cpp b/tests/testSymbolicFactorGraphB.cpp index cb64ddb4a..8fde094ed 100644 --- a/tests/testSymbolicFactorGraphB.cpp +++ b/tests/testSymbolicFactorGraphB.cpp @@ -30,19 +30,19 @@ using namespace boost::assign; using namespace std; using namespace gtsam; -Key kx(size_t i) { return Symbol('x',i); } -Key kl(size_t i) { return Symbol('l',i); } +using symbol_shorthand::X; +using symbol_shorthand::L; /* ************************************************************************* */ 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 SymbolicFactorGraph expected; - expected.push_factor(o[kx(1)]); - expected.push_factor(o[kx(1)],o[kx(2)]); - expected.push_factor(o[kx(1)],o[kl(1)]); - expected.push_factor(o[kx(2)],o[kl(1)]); + expected.push_factor(o[X(1)]); + expected.push_factor(o[X(1)],o[X(2)]); + expected.push_factor(o[X(1)],o[L(1)]); + expected.push_factor(o[X(2)],o[L(1)]); // construct it from the factor graph GaussianFactorGraph factorGraph = example::createGaussianFactorGraph(o); @@ -59,7 +59,7 @@ TEST( SymbolicFactorGraph, symbolicFactorGraph ) // SymbolicFactorGraph actual(factorGraph); // SymbolicFactor::shared_ptr f1 = actual[0]; // SymbolicFactor::shared_ptr f3 = actual[2]; -// actual.findAndRemoveFactors(kx(2)); +// actual.findAndRemoveFactors(X(2)); // // // construct expected graph after find_factors_and_remove // SymbolicFactorGraph expected; @@ -79,13 +79,13 @@ TEST( SymbolicFactorGraph, symbolicFactorGraph ) // SymbolicFactorGraph fg(factorGraph); // // // ask for all factor indices connected to x1 -// list x1_factors = fg.factors(kx(1)); +// list x1_factors = fg.factors(X(1)); // int x1_indices[] = { 0, 1, 2 }; // list x1_expected(x1_indices, x1_indices + 3); // CHECK(x1_factors==x1_expected); // // // ask for all factor indices connected to x2 -// list x2_factors = fg.factors(kx(2)); +// list x2_factors = fg.factors(X(2)); // int x2_indices[] = { 1, 3 }; // list x2_expected(x2_indices, x2_indices + 2); // CHECK(x2_factors==x2_expected); @@ -99,26 +99,26 @@ TEST( SymbolicFactorGraph, symbolicFactorGraph ) // SymbolicFactorGraph fg(factorGraph); // // // combine all factors connected to x1 -// SymbolicFactor::shared_ptr actual = removeAndCombineFactors(fg,kx(1)); +// SymbolicFactor::shared_ptr actual = removeAndCombineFactors(fg,X(1)); // // // check result -// SymbolicFactor expected(kl(1),kx(1),kx(2)); +// SymbolicFactor expected(L(1),X(1),X(2)); // CHECK(assert_equal(expected,*actual)); //} ///* ************************************************************************* */ //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 // GaussianFactorGraph factorGraph = example::createGaussianFactorGraph(o); // SymbolicFactorGraph fg(factorGraph); // // // 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 -// 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)); //} @@ -126,12 +126,12 @@ TEST( SymbolicFactorGraph, symbolicFactorGraph ) /* ************************************************************************* */ 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 - IndexConditional::shared_ptr x2(new IndexConditional(o[kx(2)], o[kl(1)], o[kx(1)])); - IndexConditional::shared_ptr l1(new IndexConditional(o[kl(1)], o[kx(1)])); - IndexConditional::shared_ptr x1(new IndexConditional(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[L(1)], o[X(1)])); + IndexConditional::shared_ptr x1(new IndexConditional(o[X(1)])); SymbolicBayesNet expected; expected.push_back(x2);