diff --git a/gtsam/slam/GPSFactor.h b/gtsam/slam/BiasedGPSFactor.h similarity index 87% rename from gtsam/slam/GPSFactor.h rename to gtsam/slam/BiasedGPSFactor.h index 9add83be0..fc525f52a 100644 --- a/gtsam/slam/GPSFactor.h +++ b/gtsam/slam/BiasedGPSFactor.h @@ -10,7 +10,7 @@ * -------------------------------------------------------------------------- */ /** - * @file GPSFactor.h + * @file BiasedGPSFactor.h * @author Luca Carlone **/ #pragma once @@ -27,11 +27,11 @@ namespace gtsam { * common-mode errors and that can be partially corrected if other sensors are used * @addtogroup SLAM */ - class GPSFactor: public NoiseModelFactor2 { + class BiasedGPSFactor: public NoiseModelFactor2 { private: - typedef GPSFactor This; + typedef BiasedGPSFactor This; typedef NoiseModelFactor2 Base; Point3 measured_; /** The measurement */ @@ -39,24 +39,24 @@ namespace gtsam { public: // shorthand for a smart pointer to a factor - typedef typename boost::shared_ptr shared_ptr; + typedef typename boost::shared_ptr shared_ptr; /** default constructor - only use for serialization */ - GPSFactor() {} + BiasedGPSFactor() {} /** Constructor */ - GPSFactor(Key posekey, Key biaskey, const Point3 measured, + BiasedGPSFactor(Key posekey, Key biaskey, const Point3 measured, const SharedNoiseModel& model) : Base(model, posekey, biaskey), measured_(measured) { } - virtual ~GPSFactor() {} + virtual ~BiasedGPSFactor() {} /** implement functions needed for Testable */ /** print */ virtual void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { - std::cout << s << "GPSFactor(" + std::cout << s << "BiasedGPSFactor(" << keyFormatter(this->key1()) << "," << keyFormatter(this->key2()) << ")\n"; measured_.print(" measured: "); @@ -100,6 +100,6 @@ namespace gtsam { boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(measured_); } - }; // \class GPSFactor + }; // \class BiasedGPSFactor } /// namespace gtsam diff --git a/gtsam/slam/tests/testGPSFactor.cpp b/gtsam/slam/tests/testBiasedGPSFactor.cpp similarity index 78% rename from gtsam/slam/tests/testGPSFactor.cpp rename to gtsam/slam/tests/testBiasedGPSFactor.cpp index f501183b3..d907852c5 100644 --- a/gtsam/slam/tests/testGPSFactor.cpp +++ b/gtsam/slam/tests/testBiasedGPSFactor.cpp @@ -1,5 +1,5 @@ /** - * @file testGPSFactor.cpp + * @file testBiasedGPSFactor.cpp * @brief * @author Luca Carlone * @date July 30, 2014 @@ -8,7 +8,7 @@ #include #include #include -#include +#include #include using namespace gtsam; @@ -19,7 +19,7 @@ using namespace gtsam::noiseModel; using symbol_shorthand::X; using symbol_shorthand::B; -TEST(GPSFactor, errorNoiseless) { +TEST(BiasedGPSFactor, errorNoiseless) { Rot3 R = Rot3::rodriguez(0.1, 0.2, 0.3); Point3 t(1.0, 0.5, 0.2); @@ -28,13 +28,13 @@ TEST(GPSFactor, errorNoiseless) { Point3 noise(0.0,0.0,0.0); Point3 measured = t + noise; - GPSFactor factor(X(1), B(1), measured, Isotropic::Sigma(3, 0.05)); + BiasedGPSFactor factor(X(1), B(1), measured, Isotropic::Sigma(3, 0.05)); Vector expectedError = (Vector(3) << 0.0, 0.0, 0.0 ); Vector actualError = factor.evaluateError(pose,bias); EXPECT(assert_equal(expectedError,actualError, 1E-5)); } -TEST(GPSFactor, errorNoisy) { +TEST(BiasedGPSFactor, errorNoisy) { Rot3 R = Rot3::rodriguez(0.1, 0.2, 0.3); Point3 t(1.0, 0.5, 0.2); @@ -43,13 +43,13 @@ TEST(GPSFactor, errorNoisy) { Point3 noise(1.0,2.0,3.0); Point3 measured = t - noise; - GPSFactor factor(X(1), B(1), measured, Isotropic::Sigma(3, 0.05)); + BiasedGPSFactor factor(X(1), B(1), measured, Isotropic::Sigma(3, 0.05)); Vector expectedError = (Vector(3) << 1.0, 2.0, 3.0 ); Vector actualError = factor.evaluateError(pose,bias); EXPECT(assert_equal(expectedError,actualError, 1E-5)); } -TEST(GPSFactor, jacobian) { +TEST(BiasedGPSFactor, jacobian) { Rot3 R = Rot3::rodriguez(0.1, 0.2, 0.3); Point3 t(1.0, 0.5, 0.2); @@ -59,20 +59,20 @@ TEST(GPSFactor, jacobian) { Point3 noise(0.0,0.0,0.0); Point3 measured = t + noise; - GPSFactor factor(X(1), B(1), measured, Isotropic::Sigma(3, 0.05)); + BiasedGPSFactor factor(X(1), B(1), measured, Isotropic::Sigma(3, 0.05)); Matrix actualH1, actualH2; factor.evaluateError(pose,bias, actualH1, actualH2); Matrix numericalH1 = numericalDerivative21( boost::function(boost::bind( - &GPSFactor::evaluateError, factor, _1, _2, boost::none, + &BiasedGPSFactor::evaluateError, factor, _1, _2, boost::none, boost::none)), pose, bias, 1e-5); EXPECT(assert_equal(numericalH1,actualH1, 1E-5)); Matrix numericalH2 = numericalDerivative22( boost::function(boost::bind( - &GPSFactor::evaluateError, factor, _1, _2, boost::none, + &BiasedGPSFactor::evaluateError, factor, _1, _2, boost::none, boost::none)), pose, bias, 1e-5); EXPECT(assert_equal(numericalH2,actualH2, 1E-5)); }