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

renamed GPS factor

release/4.3a0
Luca 2014-09-10 15:30:06 -04:00
parent 92b113d173
commit db411214ff
2 changed files with 19 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
gtsam/slam/GPSFactor.h → gtsam/slam/BiasedGPSFactor.h
View File

@ -10,7 +10,7 @@
* -------------------------------------------------------------------------- */ * -------------------------------------------------------------------------- */
/** /**
* @file GPSFactor.h * @file BiasedGPSFactor.h
* @author Luca Carlone * @author Luca Carlone
**/ **/
#pragma once #pragma once
@ -27,11 +27,11 @@ namespace gtsam {
* common-mode errors and that can be partially corrected if other sensors are used * common-mode errors and that can be partially corrected if other sensors are used
* @addtogroup SLAM * @addtogroup SLAM
*/ */
class GPSFactor: public NoiseModelFactor2<Pose3, Point3> { class BiasedGPSFactor: public NoiseModelFactor2<Pose3, Point3> {
private: private:
typedef GPSFactor This; typedef BiasedGPSFactor This;
typedef NoiseModelFactor2<Pose3, Point3> Base; typedef NoiseModelFactor2<Pose3, Point3> Base;
Point3 measured_; /** The measurement */ Point3 measured_; /** The measurement */
@ -39,24 +39,24 @@ namespace gtsam {
public: public:
// shorthand for a smart pointer to a factor // shorthand for a smart pointer to a factor
typedef typename boost::shared_ptr<GPSFactor> shared_ptr; typedef typename boost::shared_ptr<BiasedGPSFactor> shared_ptr;
/** default constructor - only use for serialization */ /** default constructor - only use for serialization */
GPSFactor() {} BiasedGPSFactor() {}
/** Constructor */ /** Constructor */
GPSFactor(Key posekey, Key biaskey, const Point3 measured, BiasedGPSFactor(Key posekey, Key biaskey, const Point3 measured,
const SharedNoiseModel& model) : const SharedNoiseModel& model) :
Base(model, posekey, biaskey), measured_(measured) { Base(model, posekey, biaskey), measured_(measured) {
} }
virtual ~GPSFactor() {} virtual ~BiasedGPSFactor() {}
/** implement functions needed for Testable */ /** implement functions needed for Testable */
/** print */ /** print */
virtual void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { 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->key1()) << ","
<< keyFormatter(this->key2()) << ")\n"; << keyFormatter(this->key2()) << ")\n";
measured_.print(" measured: "); measured_.print(" measured: ");
@ -100,6 +100,6 @@ namespace gtsam {
boost::serialization::base_object<Base>(*this)); boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(measured_); ar & BOOST_SERIALIZATION_NVP(measured_);
} }
}; // \class GPSFactor }; // \class BiasedGPSFactor
} /// namespace gtsam } /// namespace gtsam

20
gtsam/slam/tests/testGPSFactor.cpp → gtsam/slam/tests/testBiasedGPSFactor.cpp
View File

@ -1,5 +1,5 @@
/** /**
* @file testGPSFactor.cpp * @file testBiasedGPSFactor.cpp
* @brief * @brief
* @author Luca Carlone * @author Luca Carlone
* @date July 30, 2014 * @date July 30, 2014
@ -8,7 +8,7 @@
#include <gtsam/base/numericalDerivative.h> #include <gtsam/base/numericalDerivative.h>
#include <gtsam/geometry/Pose3.h> #include <gtsam/geometry/Pose3.h>
#include <gtsam/inference/Symbol.h> #include <gtsam/inference/Symbol.h>
#include <gtsam/slam/GPSFactor.h> #include <gtsam/slam/BiasedGPSFactor.h>
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
using namespace gtsam; using namespace gtsam;
@ -19,7 +19,7 @@ using namespace gtsam::noiseModel;
using symbol_shorthand::X; using symbol_shorthand::X;
using symbol_shorthand::B; using symbol_shorthand::B;
TEST(GPSFactor, errorNoiseless) { TEST(BiasedGPSFactor, errorNoiseless) {
Rot3 R = Rot3::rodriguez(0.1, 0.2, 0.3); Rot3 R = Rot3::rodriguez(0.1, 0.2, 0.3);
Point3 t(1.0, 0.5, 0.2); Point3 t(1.0, 0.5, 0.2);
@ -28,13 +28,13 @@ TEST(GPSFactor, errorNoiseless) {
Point3 noise(0.0,0.0,0.0); Point3 noise(0.0,0.0,0.0);
Point3 measured = t + noise; 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 expectedError = (Vector(3) << 0.0, 0.0, 0.0 );
Vector actualError = factor.evaluateError(pose,bias); Vector actualError = factor.evaluateError(pose,bias);
EXPECT(assert_equal(expectedError,actualError, 1E-5)); EXPECT(assert_equal(expectedError,actualError, 1E-5));
} }
TEST(GPSFactor, errorNoisy) { TEST(BiasedGPSFactor, errorNoisy) {
Rot3 R = Rot3::rodriguez(0.1, 0.2, 0.3); Rot3 R = Rot3::rodriguez(0.1, 0.2, 0.3);
Point3 t(1.0, 0.5, 0.2); Point3 t(1.0, 0.5, 0.2);
@ -43,13 +43,13 @@ TEST(GPSFactor, errorNoisy) {
Point3 noise(1.0,2.0,3.0); Point3 noise(1.0,2.0,3.0);
Point3 measured = t - noise; 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 expectedError = (Vector(3) << 1.0, 2.0, 3.0 );
Vector actualError = factor.evaluateError(pose,bias); Vector actualError = factor.evaluateError(pose,bias);
EXPECT(assert_equal(expectedError,actualError, 1E-5)); EXPECT(assert_equal(expectedError,actualError, 1E-5));
} }
TEST(GPSFactor, jacobian) { TEST(BiasedGPSFactor, jacobian) {
Rot3 R = Rot3::rodriguez(0.1, 0.2, 0.3); Rot3 R = Rot3::rodriguez(0.1, 0.2, 0.3);
Point3 t(1.0, 0.5, 0.2); Point3 t(1.0, 0.5, 0.2);
@ -59,20 +59,20 @@ TEST(GPSFactor, jacobian) {
Point3 noise(0.0,0.0,0.0); Point3 noise(0.0,0.0,0.0);
Point3 measured = t + noise; 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; Matrix actualH1, actualH2;
factor.evaluateError(pose,bias, actualH1, actualH2); factor.evaluateError(pose,bias, actualH1, actualH2);
Matrix numericalH1 = numericalDerivative21( Matrix numericalH1 = numericalDerivative21(
boost::function<Vector(const Pose3&, const Point3&)>(boost::bind( boost::function<Vector(const Pose3&, const Point3&)>(boost::bind(
&GPSFactor::evaluateError, factor, _1, _2, boost::none, &BiasedGPSFactor::evaluateError, factor, _1, _2, boost::none,
boost::none)), pose, bias, 1e-5); boost::none)), pose, bias, 1e-5);
EXPECT(assert_equal(numericalH1,actualH1, 1E-5)); EXPECT(assert_equal(numericalH1,actualH1, 1E-5));
Matrix numericalH2 = numericalDerivative22( Matrix numericalH2 = numericalDerivative22(
boost::function<Vector(const Pose3&, const Point3&)>(boost::bind( boost::function<Vector(const Pose3&, const Point3&)>(boost::bind(
&GPSFactor::evaluateError, factor, _1, _2, boost::none, &BiasedGPSFactor::evaluateError, factor, _1, _2, boost::none,
boost::none)), pose, bias, 1e-5); boost::none)), pose, bias, 1e-5);
EXPECT(assert_equal(numericalH2,actualH2, 1E-5)); EXPECT(assert_equal(numericalH2,actualH2, 1E-5));
} }