diff --git a/tests/testNonlinearEquality.cpp b/tests/testNonlinearEquality.cpp index 1efc40cad..2b580560f 100644 --- a/tests/testNonlinearEquality.cpp +++ b/tests/testNonlinearEquality.cpp @@ -17,12 +17,17 @@ #include #include -#include +#include #include #include #include #include +#include #include +#include +#include +#include +#include #include @@ -510,7 +515,7 @@ static Cal3_S2 K(fov,w,h); static boost::shared_ptr shK(new Cal3_S2(K)); // typedefs for visual SLAM example -typedef visualSLAM::Graph VGraph; +typedef NonlinearFactorGraph VGraph; // factors for visual slam typedef NonlinearEquality2 Point3Equality; @@ -537,13 +542,13 @@ TEST (testNonlinearEqualityConstraint, stereo_constrained ) { VGraph graph; // create equality constraints for poses - graph.addPoseConstraint(x1, camera1.pose()); - graph.addPoseConstraint(x2, camera2.pose()); + graph.add(NonlinearEquality(x1, camera1.pose())); + graph.add(NonlinearEquality(x2, camera2.pose())); // create factors SharedDiagonal vmodel = noiseModel::Unit::Create(3); - graph.addMeasurement(camera1.project(landmark), vmodel, x1, l1, shK); - graph.addMeasurement(camera2.project(landmark), vmodel, x2, l2, shK); + graph.add(GenericProjectionFactor(camera1.project(landmark), vmodel, x1, l1, shK)); + graph.add(GenericProjectionFactor(camera2.project(landmark), vmodel, x2, l2, shK)); // add equality constraint graph.add(Point3Equality(l1, l2));