diff --git a/gtsam/slam/StereoFactor.h b/gtsam/slam/StereoFactor.h index 5be783aaf..94af6e10e 100644 --- a/gtsam/slam/StereoFactor.h +++ b/gtsam/slam/StereoFactor.h @@ -18,6 +18,7 @@ #pragma once +#include #include namespace gtsam { diff --git a/gtsam/slam/tests/testStereoFactor.cpp b/gtsam/slam/tests/testStereoFactor.cpp index b38dae990..f6a861354 100644 --- a/gtsam/slam/tests/testStereoFactor.cpp +++ b/gtsam/slam/tests/testStereoFactor.cpp @@ -15,13 +15,16 @@ * @author Chris Beall */ -#include #include -#include +#include #include +#include +#include #include #include +#include #include +#include #include @@ -35,8 +38,7 @@ static Pose3 camera1(Matrix_(3,3, ), Point3(0,0,6.25)); -static Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(1500, 1500, 0, 320, 240, 0.5)); -static StereoCamera stereoCam(Pose3(), K); +static boost::shared_ptr K(new Cal3_S2Stereo(625, 625, 0, 320, 240, 0.5)); // point X Y Z in meters static Point3 p(0, 0, 5); @@ -46,20 +48,16 @@ static SharedNoiseModel sigma(noiseModel::Unit::Create(1)); using symbol_shorthand::X; using symbol_shorthand::L; -typedef GenericStereoFactor MyStereoFactor; - /* ************************************************************************* */ TEST( StereoFactor, singlePoint) { - //Cal3_S2 K(625, 625, 0, 320, 240, 0.5); - boost::shared_ptr K(new Cal3_S2Stereo(625, 625, 0, 320, 240, 0.5)); NonlinearFactorGraph graph; - graph.add(NonlinearEquality(X(1),camera1)); + graph.add(NonlinearEquality(X(1), camera1)); - StereoPoint2 z14(320,320.0-50, 240); + StereoPoint2 z14(320, 320.0-50, 240); // arguments: measurement, sigma, cam#, measurement #, K, baseline (m) - graph.add(MyStereoFactor(z14,sigma, X(1), L(1), K)); + graph.add(GenericStereoFactor(z14, sigma, X(1), L(1), K)); // Create a configuration corresponding to the ground truth Values values;