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
**/
#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<Pose3, Point3> {
class BiasedGPSFactor: public NoiseModelFactor2<Pose3, Point3> {
private:
typedef GPSFactor This;
typedef BiasedGPSFactor This;
typedef NoiseModelFactor2<Pose3, Point3> 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<GPSFactor> shared_ptr;
typedef typename boost::shared_ptr<BiasedGPSFactor> 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<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(measured_);
}
}; // \class GPSFactor
}; // \class BiasedGPSFactor
} /// 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
* @author Luca Carlone
* @date July 30, 2014
@ -8,7 +8,7 @@
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/slam/GPSFactor.h>
#include <gtsam/slam/BiasedGPSFactor.h>
#include <CppUnitLite/TestHarness.h>
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<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);
EXPECT(assert_equal(numericalH1,actualH1, 1E-5));
Matrix numericalH2 = numericalDerivative22(
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);
EXPECT(assert_equal(numericalH2,actualH2, 1E-5));
}