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

Using symbol_shorthand instead of redundant kx, kl functions

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

14
examples/vSLAMexample/vISAMexample.cpp
View File

@ -32,6 +32,10 @@ using boost::shared_ptr;
using namespace std;
using namespace 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<visualSLAM::Graph>& 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<Values> (new Values());
initialValues->insert(Symbol('x',pose_id), pose);
initialValues->insert(X(pose_id), pose);
for (size_t i = 0; i < measurements.size(); i++) {
initialValues->insert(Symbol('l',measurements[i].m_idLandmark), g_landmarks[measurements[i].m_idLandmark]);
initialValues->insert(L(measurements[i].m_idLandmark), g_landmarks[measurements[i].m_idLandmark]);
}
}

14
examples/vSLAMexample/vSFMexample.cpp
View File

@ -30,6 +30,10 @@ using boost::shared_ptr;
using namespace std;
using namespace 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<Feature2D>& 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<int, Point3> landmarks, std::map<int, Pose3> poses) {
// Initialize landmarks 3D positions.
for (map<int, Point3>::iterator lmit = landmarks.begin(); lmit != landmarks.end(); lmit++)
initValues.insert(Symbol('l',lmit->first), lmit->second);
initValues.insert(L(lmit->first), lmit->second);
// Initialize camera poses.
for (map<int, Pose3>::iterator poseit = poses.begin(); poseit != poses.end(); poseit++)
initValues.insert(Symbol('x',poseit->first), poseit->second);
initValues.insert(X(poseit->first), poseit->second);
return initValues;
}
@ -137,7 +141,7 @@ int main(int argc, char* argv[]) {
// Add prior factor for all poses in the graph
map<int, Pose3>::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;

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

@ -32,6 +32,10 @@ using namespace gtsam;
using namespace std;
static double inf = std::numeric_limits<double>::infinity();
// Convenience for named keys
using symbol_shorthand::X;
using symbol_shorthand::L;
const Symbol key1('v',1), key2('v',2), key3('v',3), key4('v',4);
/* ************************************************************************* */
@ -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;

58
gtsam/slam/smallExample.cpp
View File

@ -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<const Graph> 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<const Values> sharedNoisyValues() {
boost::shared_ptr<Values> 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<size_t>(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<size_t>(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<FactorGraph<JacobianFactor> >();
}
@ -221,7 +221,7 @@ namespace example {
Vector z = Vector_(2, 1.0, 0.0);
double sigma = 0.1;
boost::shared_ptr<smallOptimize::UnaryFactor> factor(
new smallOptimize::UnaryFactor(z, noiseModel::Isotropic::Sigma(2,sigma), kx(1)));
new smallOptimize::UnaryFactor(z, noiseModel::Isotropic::Sigma(2,sigma), X(1)));
fg->push_back(factor);
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);
}
/* ************************************************************************* */

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

@ -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<Cal3_S2> GeneralCamera;
typedef GeneralSFMFactor<GeneralCamera, Point3> Projection;
typedef NonlinearEquality<GeneralCamera> CameraConstraint;
@ -46,16 +50,16 @@ typedef NonlinearEquality<Point3> Point3Constraint;
class Graph: public NonlinearFactorGraph {
public:
void addMeasurement(int i, int j, const Point2& z, const SharedNoiseModel& model) {
push_back(boost::make_shared<Projection>(z, model, Symbol('x',i), Symbol('l',j)));
push_back(boost::make_shared<Projection>(z, model, X(i), L(j)));
}
void addCameraConstraint(int j, const GeneralCamera& p) {
boost::shared_ptr<CameraConstraint> factor(new CameraConstraint(Symbol('x',j), p));
boost::shared_ptr<CameraConstraint> factor(new CameraConstraint(X(j), p));
push_back(factor);
}
void addPoint3Constraint(int j, const Point3& p) {
boost::shared_ptr<Point3Constraint> factor(new Point3Constraint(Symbol('l',j), p));
boost::shared_ptr<Point3Constraint> factor(new Point3Constraint(L(j), p));
push_back(factor);
}
@ -98,14 +102,14 @@ TEST( GeneralSFMFactor, equals )
TEST( GeneralSFMFactor, error ) {
Point2 z(3.,0.);
const SharedNoiseModel sigma(noiseModel::Unit::Create(1));
boost::shared_ptr<Projection> factor(new Projection(z, sigma, Symbol('x',1), Symbol('l',1)));
boost::shared_ptr<Projection> factor(new Projection(z, sigma, X(1), L(1)));
// For the following configuration, the factor predicts 320,240
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<Point3> genPoint3() {
const double z = 5;
vector<Point3> 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<Point3> 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<GeneralCamera> genCameraDefaultCalibration() {
@ -145,10 +149,10 @@ vector<GeneralCamera> genCameraVariableCalibration() {
return X ;
}
shared_ptr<Ordering> getOrdering(const vector<GeneralCamera>& X, const vector<Point3>& L) {
shared_ptr<Ordering> getOrdering(const vector<GeneralCamera>& cameras, const vector<Point3>& landmarks) {
shared_ptr<Ordering> ordering(new Ordering);
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<Ordering> getOrdering(const vector<GeneralCamera>& X, const vector<Po
/* ************************************************************************* */
TEST( GeneralSFMFactor, optimize_defaultK ) {
vector<Point3> L = genPoint3();
vector<GeneralCamera> X = genCameraDefaultCalibration();
vector<Point3> landmarks = genPoint3();
vector<GeneralCamera> 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<Point3> L = genPoint3();
vector<GeneralCamera> X = genCameraVariableCalibration();
vector<Point3> landmarks = genPoint3();
vector<GeneralCamera> 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<Point3> L = genPoint3();
vector<GeneralCamera> X = genCameraVariableCalibration();
vector<Point3> landmarks = genPoint3();
vector<GeneralCamera> 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<Point3> L = genPoint3();
vector<GeneralCamera> X = genCameraVariableCalibration();
vector<Point3> landmarks = genPoint3();
vector<GeneralCamera> 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<Point3> L = genPoint3();
vector<GeneralCamera> X = genCameraVariableCalibration();
vector<Point3> landmarks = genPoint3();
vector<GeneralCamera> 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<GeneralCamera,GeneralCamera>(Symbol('x',0), Symbol('x',1), 2.0, sharedSigma(1, 10.0)));
graph.add(RangeFactor<GeneralCamera,GeneralCamera>(X(0), X(1), 2.0, sharedSigma(1, 10.0)));
const double reproj_error = 1e-5 ;
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<GeneralCamera, Pose3>(Symbol('x',0), Symbol('x',1), 2.0, sharedSigma(1, 1.0)));
graph.add(PriorFactor<Pose3>(Symbol('x',1), Pose3(Rot3(), Point3(1.0, 0.0, 0.0)), sharedSigma(6, 1.0)));
graph.add(RangeFactor<GeneralCamera, Pose3>(X(0), X(1), 2.0, sharedSigma(1, 1.0)));
graph.add(PriorFactor<Pose3>(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<CalibratedCamera>(Symbol('x',0), CalibratedCamera(), sharedSigma(6, 1.0)));
graph.add(RangeFactor<CalibratedCamera, Pose3>(Symbol('x',0), Symbol('x',1), 2.0, sharedSigma(1, 1.0)));
graph.add(PriorFactor<Pose3>(Symbol('x',1), Pose3(Rot3(), Point3(1.0, 0.0, 0.0)), sharedSigma(6, 1.0)));
graph.add(PriorFactor<CalibratedCamera>(X(0), CalibratedCamera(), sharedSigma(6, 1.0)));
graph.add(RangeFactor<CalibratedCamera, Pose3>(X(0), X(1), 2.0, sharedSigma(1, 1.0)));
graph.add(PriorFactor<Pose3>(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;

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

@ -38,6 +38,10 @@
using namespace std;
using namespace gtsam;
// Convenience for named keys
using symbol_shorthand::X;
using symbol_shorthand::L;
typedef PinholeCamera<Cal3Bundler> GeneralCamera;
typedef GeneralSFMFactor<GeneralCamera, Point3> Projection;
typedef NonlinearEquality<GeneralCamera> CameraConstraint;
@ -47,16 +51,16 @@ typedef NonlinearEquality<Point3> 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<Projection>(z, model, Symbol('x',i), Symbol('l',j)));
push_back(boost::make_shared<Projection>(z, model, X(i), L(j)));
}
void addCameraConstraint(int j, const GeneralCamera& p) {
boost::shared_ptr<CameraConstraint> factor(new CameraConstraint(Symbol('x', j), p));
boost::shared_ptr<CameraConstraint> factor(new CameraConstraint(X(j), p));
push_back(factor);
}
void addPoint3Constraint(int j, const Point3& p) {
boost::shared_ptr<Point3Constraint> factor(new Point3Constraint(Symbol('l', j), p));
boost::shared_ptr<Point3Constraint> factor(new Point3Constraint(L(j), p));
push_back(factor);
}
@ -100,14 +104,14 @@ TEST( GeneralSFMFactor, error ) {
Point2 z(3.,0.);
const SharedNoiseModel sigma(noiseModel::Unit::Create(1));
boost::shared_ptr<Projection>
factor(new Projection(z, sigma, Symbol('x',1), Symbol('l',1)));
factor(new Projection(z, sigma, X(1), L(1)));
// For the following configuration, the factor predicts 320,240
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<Point3> genPoint3() {
const double z = 5;
vector<Point3> 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<Point3> 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<GeneralCamera> genCameraDefaultCalibration() {
vector<GeneralCamera> 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<GeneralCamera> 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<GeneralCamera> genCameraVariableCalibration() {
const Cal3Bundler K(500,1e-3,1e-3);
vector<GeneralCamera> 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<GeneralCamera> 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<Ordering> getOrdering(const std::vector<GeneralCamera>& X, const std::vector<Point3>& L) {
boost::shared_ptr<Ordering> getOrdering(const std::vector<GeneralCamera>& cameras, const std::vector<Point3>& landmarks) {
boost::shared_ptr<Ordering> ordering(new Ordering);
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<Point3> L = genPoint3();
vector<GeneralCamera> X = genCameraDefaultCalibration();
vector<Point3> landmarks = genPoint3();
vector<GeneralCamera> 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<Point3> L = genPoint3();
vector<GeneralCamera> X = genCameraVariableCalibration();
vector<Point3> landmarks = genPoint3();
vector<GeneralCamera> 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<Point3> L = genPoint3();
vector<GeneralCamera> X = genCameraVariableCalibration();
vector<Point3> landmarks = genPoint3();
vector<GeneralCamera> 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<Point3> L = genPoint3();
vector<GeneralCamera> X = genCameraVariableCalibration();
vector<Point3> landmarks = genPoint3();
vector<GeneralCamera> 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<Point3> L = genPoint3();
vector<GeneralCamera> X = genCameraVariableCalibration();
vector<Point3> landmarks = genPoint3();
vector<GeneralCamera> 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<GeneralCamera,GeneralCamera>(Symbol('x',0), Symbol('x',1), 2.0, sharedSigma(1, 10.0)));
graph.add(RangeFactor<GeneralCamera,GeneralCamera>(X(0), X(1), 2.0, sharedSigma(1, 10.0)));
const double reproj_error = 1e-5 ;
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);

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

@ -37,8 +37,8 @@ static SharedNoiseModel sigma(noiseModel::Unit::Create(1));
static shared_ptrK sK(new Cal3_S2(K));
// 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<visualSLAM::ProjectionFactor>
factor(new visualSLAM::ProjectionFactor(z, sigma, kx(i), kl(j), sK));
factor(new visualSLAM::ProjectionFactor(z, sigma, X(i), L(j), sK));
// For the following values structure, the factor predicts 320,240
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<JacobianFactor>(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<visualSLAM::ProjectionFactor>
factor1(new visualSLAM::ProjectionFactor(z, sigma, kx(i), kl(j), sK));
factor1(new visualSLAM::ProjectionFactor(z, sigma, X(i), L(j), sK));
boost::shared_ptr<visualSLAM::ProjectionFactor>
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));
}

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

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

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

@ -29,8 +29,8 @@ using namespace gtsam;
#include <CppUnitLite/TestHarness.h>
// 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<GaussianFactor> lf = factor.linearize(config, *config.orderingArbitrary());
}

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

@ -43,8 +43,8 @@ Point3 p(0, 0, 5);
static SharedNoiseModel sigma(noiseModel::Unit::Create(1));
// 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<Cal3_S2Stereo> 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);

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

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

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

@ -15,24 +15,28 @@
* @author Alex Cunningham
**/
#include <iostream>
#include <CppUnitLite/TestHarness.h>
#include <gtsam_unstable/slam/simulated3D.h>
#include <gtsam/nonlinear/Symbol.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/numericalDerivative.h>
#include <CppUnitLite/TestHarness.h>
#include <iostream>
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));
}

8
tests/testDoglegOptimizer.cpp
View File

@ -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<Values> config(new Values);
config->insert(Symbol('x',1), x0);
config->insert(X(1), x0);
// ordering
shared_ptr<Ordering> 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) {

8
tests/testExtendedKalmanFilter.cpp
View File

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

6
tests/testGaussianFactor.cpp
View File

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

236
tests/testGaussianFactorGraphB.cpp
View File

@ -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<JacobianFactor> fg = createGaussianFactorGraph(ordering);
// VectorValues cfg = createZeroDelta(ordering);
//
@ -74,10 +74,10 @@ TEST( GaussianFactorGraph, equals ) {
//{
// GaussianFactorGraph fg = createGaussianFactorGraph();
//
// set<Symbol> separator = fg.find_separator(kx(2));
// set<Symbol> separator = fg.find_separator(X(2));
// set<Symbol> expected;
// expected.insert(kx(1));
// expected.insert(kl(1));
// expected.insert(X(1));
// expected.insert(L(1));
//
// EXPECT(separator.size()==expected.size());
// set<Symbol>::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<pair<Symbol, Matrix> > 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<pair<Symbol, Matrix> > 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<size_t>(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<IndexFactor> 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<Symbol> interested; interested += kl(1),kx(1);
// set<Symbol> 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<int> v) {
// GaussianFactorGraph fg = createGaussianFactorGraph();
//
// // ask for all factor indices connected to x1
// list<size_t> x1_factors = fg.factors(kx(1));
// list<size_t> x1_factors = fg.factors(X(1));
// size_t x1_indices[] = { 0, 1, 2 };
// list<size_t> x1_expected(x1_indices, x1_indices + 3);
// EXPECT(x1_factors==x1_expected);
//
// // ask for all factor indices connected to x2
// list<size_t> x2_factors = fg.factors(kx(2));
// list<size_t> x2_factors = fg.factors(X(2));
// size_t x2_indices[] = { 1, 3 };
// list<size_t> x2_expected(x2_indices, x2_indices + 2);
// EXPECT(x2_factors==x2_expected);
@ -613,7 +613,7 @@ void print(vector<int> v) {
// GaussianFactor::shared_ptr f2 = fg[2];
//
// // call the function
// vector<GaussianFactor::shared_ptr> factors = fg.findAndRemoveFactors(kx(1));
// vector<GaussianFactor::shared_ptr> 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<JacobianFactor> 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<string, string> tree = g.findMinimumSpanningTree<string, GaussianFactor>();
// 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<string> 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<string, GaussianFactor>(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<Index> x3var; x3var.push_back(ordering[kx(3)]);
vector<Index> x1var; x1var.push_back(ordering[kx(1)]);
vector<Index> x3var; x3var.push_back(ordering[X(3)]);
vector<Index> 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<GaussianFactor> fgc2 = createSimpleConstraintGraph() ;
EXPECT(hasConstraints(fgc2));
Ordering ordering; ordering += kx(1), kx(2), kl(1);
Ordering ordering; ordering += X(1), X(2), L(1);
FactorGraph<GaussianFactor> fg = createGaussianFactorGraph(ordering);
EXPECT(!hasConstraints(fg));
}

94
tests/testGaussianISAM.cpp
View File

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

66
tests/testGaussianJunctionTreeB.cpp
View File

@ -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<Index> frontal1; frontal1 += ordering[kx(5)], ordering[kx(6)], ordering[kx(4)];
vector<Index> frontal2; frontal2 += ordering[kx(3)], ordering[kx(2)];
vector<Index> frontal3; frontal3 += ordering[kx(1)];
vector<Index> frontal4; frontal4 += ordering[kx(7)];
vector<Index> frontal1; frontal1 += ordering[X(5)], ordering[X(6)], ordering[X(4)];
vector<Index> frontal2; frontal2 += ordering[X(3)], ordering[X(2)];
vector<Index> frontal3; frontal3 += ordering[X(1)];
vector<Index> frontal4; frontal4 += ordering[X(7)];
vector<Index> sep1;
vector<Index> sep2; sep2 += ordering[kx(4)];
vector<Index> sep3; sep3 += ordering[kx(2)];
vector<Index> sep4; sep4 += ordering[kx(6)];
vector<Index> sep2; sep2 += ordering[X(4)];
vector<Index> sep3; sep3 += ordering[X(2)];
vector<Index> 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<size_t>(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);

26
tests/testInferenceB.cpp
View File

@ -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<GaussianFactor>::shared_ptr gfg(fg.linearize(init, ordering));
GaussianMultifrontalSolver solver(*gfg);
solver.marginalFactor(ordering[kl(0)]);
solver.marginalFactor(ordering[L(0)]);
}
/* ************************************************************************* */

96
tests/testNonlinearFactor.cpp
View File

@ -39,8 +39,8 @@ using namespace gtsam;
using namespace example;
// 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<NonlinearFactor > 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<LieVector, LieVector, LieVector, LieVector> {
public:
typedef NoiseModelFactor4<LieVector, LieVector, LieVector, LieVector> Base;
TestFactor4() : Base(sharedSigmas(Vector_(1, 2.0)), kx(1), kx(2), kx(3), kx(4)) {}
TestFactor4() : Base(sharedSigmas(Vector_(1, 2.0)), X(1), X(2), X(3), X(4)) {}
virtual Vector
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<JacobianFactor>(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<LieVector, LieVector, LieVector, LieVector, LieVector> {
public:
typedef NoiseModelFactor5<LieVector, LieVector, LieVector, LieVector, LieVector> Base;
TestFactor5() : Base(sharedSigmas(Vector_(1, 2.0)), kx(1), kx(2), kx(3), kx(4), kx(5)) {}
TestFactor5() : Base(sharedSigmas(Vector_(1, 2.0)), X(1), X(2), X(3), X(4), X(5)) {}
virtual Vector
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<JacobianFactor>(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<LieVector, LieVector, LieVector, LieVector, LieVector, LieVector> {
public:
typedef NoiseModelFactor6<LieVector, LieVector, LieVector, LieVector, LieVector, LieVector> Base;
TestFactor6() : Base(sharedSigmas(Vector_(1, 2.0)), kx(1), kx(2), kx(3), kx(4), kx(5), kx(6)) {}
TestFactor6() : Base(sharedSigmas(Vector_(1, 2.0)), X(1), X(2), X(3), X(4), X(5), X(6)) {}
virtual Vector
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<JacobianFactor>(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<Key> 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]);
}
/* ************************************************************************* */

18
tests/testNonlinearFactorGraph.cpp
View File

@ -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<Key> actual = fg.keys();
LONGS_EQUAL(3, actual.size());
set<Key>::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<Key,Key> rekey_mapping;
rekey_mapping.insert(make_pair(kl(1), kl(4)));
rekey_mapping.insert(make_pair(L(1), L(4)));
Graph actRekey = init.rekey(rekey_mapping);
// 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));
}

42
tests/testNonlinearOptimizer.cpp
View File

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

12
tests/testSymbolicBayesNetB.cpp
View File

@ -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<IndexConditional> expected;
expected.push_back(x2);
expected.push_back(l1);

38
tests/testSymbolicFactorGraphB.cpp
View File

@ -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<size_t> x1_factors = fg.factors(kx(1));
// list<size_t> x1_factors = fg.factors(X(1));
// int x1_indices[] = { 0, 1, 2 };
// list<size_t> x1_expected(x1_indices, x1_indices + 3);
// CHECK(x1_factors==x1_expected);
//
// // ask for all factor indices connected to x2
// list<size_t> x2_factors = fg.factors(kx(2));
// list<size_t> x2_factors = fg.factors(X(2));
// int x2_indices[] = { 1, 3 };
// list<size_t> 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);