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

Add a robust noise model. Change SharedGaussian to SharedNoiseModel

release/4.3a0
Yong-Dian Jian 2011-08-26 21:41:01 +00:00
parent b39970ad9a
commit fd07d61a2e
44 changed files with 395 additions and 116 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
examples/CameraResectioning.cpp
View File

@ -29,7 +29,7 @@ class ResectioningFactor: public NonlinearFactor1<PoseValues, PoseKey> {
Point2 p_; // 2D measurement of the 3D point
public:
ResectioningFactor(const SharedGaussian& model, const PoseKey& key,
ResectioningFactor(const SharedNoiseModel& model, const PoseKey& key,
const shared_ptrK& calib, const Point2& p, const Point3& P) :
Base(model, key), K_(calib), P_(P), p_(p) {
}

2
examples/Pose2SLAMwSPCG_advanced.cpp
View File

@ -43,7 +43,7 @@ Values result;
int main(void) {
/* generate synthetic data */
const SharedGaussian sigma(noiseModel::Unit::Create(0.1));
const SharedNoiseModel sigma(noiseModel::Unit::Create(0.1));
Key x1(1), x2(2), x3(3), x4(4), x5(5), x6(6), x7(7), x8(8), x9(9);
graph = boost::make_shared<Graph>() ;

2
examples/Pose2SLAMwSPCG_easy.cpp
View File

@ -34,7 +34,7 @@ Values result;
int main(void) {
/* generate synthetic data */
const SharedGaussian sigma(noiseModel::Unit::Create(0.1));
const SharedNoiseModel sigma(noiseModel::Unit::Create(0.1));
Key x1(1), x2(2), x3(3), x4(4), x5(5), x6(6), x7(7), x8(8), x9(9);
// create a 3 by 3 grid

6
examples/vSLAMexample/vISAMexample.cpp
View File

@ -55,8 +55,8 @@ std::vector<Pose3> g_poses; // prior camera poses at each frame
std::vector<std::vector<Feature2D> > g_measurements; // feature sets detected at each frame
// Noise models
SharedGaussian measurementSigma(noiseModel::Isotropic::Sigma(2, 5.0f));
SharedGaussian poseSigma(noiseModel::Unit::Create(1));
SharedNoiseModel measurementSigma(noiseModel::Isotropic::Sigma(2, 5.0f));
SharedNoiseModel poseSigma(noiseModel::Unit::Create(1));
/* ************************************************************************* */
@ -82,7 +82,7 @@ void readAllDataISAM() {
* Setup newFactors and initialValues for each new pose and set of measurements at each frame.
*/
void createNewFactors(shared_ptr<Graph>& newFactors, boost::shared_ptr<Values>& initialValues,
int pose_id, Pose3& pose, std::vector<Feature2D>& measurements, SharedGaussian measurementSigma, shared_ptrK calib) {
int pose_id, Pose3& pose, std::vector<Feature2D>& measurements, SharedNoiseModel measurementSigma, shared_ptrK calib) {
// Create a graph of newFactors with new measurements
newFactors = shared_ptr<Graph> (new Graph());

4
examples/vSLAMexample/vSFMexample.cpp
View File

@ -52,7 +52,7 @@ map<int, Pose3> g_poses; // map: <camera_id, pose>
std::vector<Feature2D> g_measurements; // Feature2D: {camera_id, landmark_id, 2d feature_position}
// Noise models
SharedGaussian measurementSigma(noiseModel::Isotropic::Sigma(2, 5.0f));
SharedNoiseModel measurementSigma(noiseModel::Isotropic::Sigma(2, 5.0f));
/* ************************************************************************* */
/**
@ -79,7 +79,7 @@ void readAllData() {
* by adding and linking 2D features (measurements) detected in each captured image
* with their corresponding landmarks.
*/
Graph setupGraph(std::vector<Feature2D>& measurements, SharedGaussian measurementSigma, shared_ptrK calib) {
Graph setupGraph(std::vector<Feature2D>& measurements, SharedNoiseModel measurementSigma, shared_ptrK calib) {
Graph g;

134
gtsam/linear/NoiseModel.cpp
View File

@ -164,6 +164,24 @@ SharedDiagonal Gaussian::Cholesky(Matrix& Ab, size_t nFrontals) const {
return Unit::Create(maxrank);
}
void Gaussian::WhitenSystem(Matrix& A, Vector& b) const {
WhitenInPlace(A);
whitenInPlace(b);
}
void Gaussian::WhitenSystem(Matrix& A1, Matrix& A2, Vector& b) const {
WhitenInPlace(A1);
WhitenInPlace(A2);
whitenInPlace(b);
}
void Gaussian::WhitenSystem(Matrix& A1, Matrix& A2, Matrix& A3, Vector& b) const{
WhitenInPlace(A1);
WhitenInPlace(A2);
WhitenInPlace(A3);
whitenInPlace(b);
}
/* ************************************************************************* */
// Diagonal
/* ************************************************************************* */
@ -414,6 +432,122 @@ void Unit::print(const std::string& name) const {
cout << name << ": unit (" << dim_ << ") " << endl;
}
/* ************************************************************************* */
// M-Estimator
/* ************************************************************************* */
namespace MEstimator {
Vector Base::weight(const Vector &error) const {
const size_t n = error.rows();
Vector w(n);
for ( size_t i = 0 ; i < n ; ++i )
w(i) = weight(error(i));
return w;
}
void Base::reweight(Matrix &A, Vector &error) const {
const Vector W = weight(error);
vector_scale_inplace(W,A);
error = emul(W, error);
}
void Base::reweight(Matrix &A1, Matrix &A2, Vector &error) const {
const Vector W = weight(error);
vector_scale_inplace(W,A1);
vector_scale_inplace(W,A2);
error = emul(W, error);
}
void Base::reweight(Matrix &A1, Matrix &A2, Matrix &A3, Vector &error) const {
const Vector W = weight(error);
vector_scale_inplace(W,A1);
vector_scale_inplace(W,A2);
vector_scale_inplace(W,A3);
error = emul(W, error);
}
Fair::Fair(const double c): c_(c) {
if ( c_ <= 0 ) {
cout << "MEstimator Fair takes only positive double in constructor. forced to 1.0" << endl;
c_ = 1.0;
}
}
double Fair::weight(const double &error) const
{ return 1.0 / (1.0 + fabs(error)/c_); }
void Fair::print(const std::string &s) const
{ cout << s << ": fair (" << c_ << ")" << endl; }
bool Fair::equals(const Base &expected, const double tol) const {
const Fair* p = dynamic_cast<const Fair*> (&expected);
if (p == NULL) return false;
return fabs(c_ - p->c_ ) < tol;
}
Fair::shared_ptr Fair::Create(const double c)
{ return shared_ptr(new Fair(c)); }
Huber::Huber(const double k): k_(k) {
if ( k_ <= 0 ) {
cout << "MEstimator Huber takes only positive double in constructor. forced to 1.0" << endl;
k_ = 1.0;
}
}
double Huber::weight(const double &error) const
{ return (error < k_) ? (1.0) : (k_ / fabs(error)); }
void Huber::print(const std::string &s) const
{ cout << s << ": huber (" << k_ << ")" << endl; }
bool Huber::equals(const Base &expected, const double tol) const {
const Huber* p = dynamic_cast<const Huber*> (&expected);
if (p == NULL) return false;
return fabs(k_ - p->k_ ) < tol;
}
Huber::shared_ptr Huber::Create(const double c)
{ return shared_ptr(new Huber(c)); }
}
/* ************************************************************************* */
// Robust
/* ************************************************************************* */
void Robust::print(const std::string& name) const {
robust_->print(name);
noise_->print(name);
}
bool Robust::equals(const Base& expected, double tol) const {
const Robust* p = dynamic_cast<const Robust*> (&expected);
if (p == NULL) return false;
return noise_->equals(*p->noise_,tol) && robust_->equals(*p->robust_,tol);
}
void Robust::WhitenSystem(Matrix& A, Vector& b) const {
noise_->WhitenSystem(A,b);
robust_->reweight(A,b);
}
void Robust::WhitenSystem(Matrix& A1, Matrix& A2, Vector& b) const {
noise_->WhitenSystem(A1,A2,b);
robust_->reweight(A1,A2,b);
}
void Robust::WhitenSystem(Matrix& A1, Matrix& A2, Matrix& A3, Vector& b) const{
noise_->WhitenSystem(A1,A2,A3,b);
robust_->reweight(A1,A2,A3,b);
}
Robust::shared_ptr Robust::Create(
const RobustModel::shared_ptr &robust, const NoiseModel::shared_ptr noise){
return shared_ptr(new Robust(robust,noise));
}
/* ************************************************************************* */
}

102
gtsam/linear/NoiseModel.h
View File

@ -43,6 +43,9 @@ namespace gtsam {
*/
class Base : public Testable<Base> {
public:
typedef boost::shared_ptr<Base> shared_ptr;
protected:
size_t dim_;
@ -58,6 +61,10 @@ namespace gtsam {
*/
inline size_t dim() const { return dim_;}
virtual void print(const std::string& name) const = 0;
virtual bool equals(const Base& expected, double tol=1e-9) const = 0;
/**
* Whiten an error vector.
*/
@ -68,6 +75,15 @@ namespace gtsam {
*/
virtual Vector unwhiten(const Vector& v) const = 0;
/**
* Mahalanobis distance v'*R'*R*v = <R*v,R*v>
*/
virtual double Mahalanobis(const Vector& v) const = 0;
virtual void WhitenSystem(Matrix& A, Vector& b) const = 0;
virtual void WhitenSystem(Matrix& A1, Matrix& A2, Vector& b) const = 0;
virtual void WhitenSystem(Matrix& A1, Matrix& A2, Matrix& A3, Vector& b) const = 0;
/** in-place whiten, override if can be done more efficiently */
virtual void whitenInPlace(Vector& v) const {
v = whiten(v);
@ -166,10 +182,9 @@ namespace gtsam {
/**
* Whiten a system, in place as well
*/
inline virtual void WhitenSystem(Matrix& A, Vector& b) const {
WhitenInPlace(A);
whitenInPlace(b);
}
virtual void WhitenSystem(Matrix& A, Vector& b) const ;
virtual void WhitenSystem(Matrix& A1, Matrix& A2, Vector& b) const ;
virtual void WhitenSystem(Matrix& A1, Matrix& A2, Matrix& A3, Vector& b) const;
/**
* Apply appropriately weighted QR factorization to the system [A b]
@ -488,6 +503,85 @@ namespace gtsam {
}
};
namespace MEstimator {
class Base {
public:
typedef boost::shared_ptr<Base> shared_ptr;
Base() {}
virtual ~Base() {}
virtual double weight(const double &error) const = 0;
virtual void print(const std::string &s) const = 0;
virtual bool equals(const Base& expected, const double tol=1e-8) const = 0;
Vector weight(const Vector &error) const;
void reweight(Matrix &A, Vector &error) const;
void reweight(Matrix &A1, Matrix &A2, Vector &error) const;
void reweight(Matrix &A1, Matrix &A2, Matrix &A3, Vector &error) const;
};
class Fair : public Base {
public:
typedef boost::shared_ptr<Fair> shared_ptr;
protected:
double c_;
public:
Fair(const double c);
virtual ~Fair() {}
virtual double weight(const double &error) const ;
virtual void print(const std::string &s) const ;
virtual bool equals(const Base& expected, const double tol=1e-8) const ;
static shared_ptr Create(const double c) ;
};
class Huber : public Base {
public:
typedef boost::shared_ptr<Huber> shared_ptr;
protected:
double k_;
public:
Huber(const double k);
virtual ~Huber() {}
virtual double weight(const double &error) const ;
virtual void print(const std::string &s) const ;
virtual bool equals(const Base& expected, const double tol=1e-8) const ;
static shared_ptr Create(const double k) ;
};
}
class Robust : public Base {
public:
typedef boost::shared_ptr<Robust> shared_ptr;
protected:
typedef MEstimator::Base RobustModel;
typedef noiseModel::Base NoiseModel;
const RobustModel::shared_ptr robust_;
const NoiseModel::shared_ptr noise_;
public:
Robust(const RobustModel::shared_ptr robust, const NoiseModel::shared_ptr noise)
: Base(noise->dim()), robust_(robust), noise_(noise) {}
virtual ~Robust() {}
virtual void print(const std::string& name) const;
virtual bool equals(const Base& expected, double tol=1e-9) const;
inline virtual Vector whiten(const Vector& v) const
{ return noise_->whiten(v); }
inline virtual Vector unwhiten(const Vector& v) const
{ return noise_->unwhiten(v); }
inline virtual double Mahalanobis(const Vector& v) const
{ return noise_->Mahalanobis(v); }
virtual void WhitenSystem(Matrix& A, Vector& b) const ;
virtual void WhitenSystem(Matrix& A1, Matrix& A2, Vector& b) const ;
virtual void WhitenSystem(Matrix& A1, Matrix& A2, Matrix& A3, Vector& b) const;
static shared_ptr Create(
const RobustModel::shared_ptr &robust, const NoiseModel::shared_ptr noise);
private:
Robust();
};
} // namespace noiseModel
} // namespace gtsam

1
gtsam/linear/SharedGaussian.h
View File

@ -19,6 +19,7 @@
#pragma once
#include <gtsam/linear/NoiseModel.h>
#include <gtsam/linear/SharedNoiseModel.h>
namespace gtsam { // note, deliberately not in noiseModel namespace

56
gtsam/linear/SharedNoiseModel.h Normal file
View File

@ -0,0 +1,56 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
#pragma once
#include <gtsam/linear/NoiseModel.h>
namespace gtsam { // note, deliberately not in noiseModel namespace
struct SharedNoiseModel: public noiseModel::Base::shared_ptr {
typedef noiseModel::Base::shared_ptr Base;
SharedNoiseModel() {}
SharedNoiseModel(const noiseModel::Robust::shared_ptr& p): Base(p) {}
SharedNoiseModel(const noiseModel::Gaussian::shared_ptr& p): Base(p) {}
SharedNoiseModel(const noiseModel::Diagonal::shared_ptr& p): Base(p) {}
SharedNoiseModel(const noiseModel::Constrained::shared_ptr& p): Base(p) {}
SharedNoiseModel(const noiseModel::Isotropic::shared_ptr& p): Base(p) {}
SharedNoiseModel(const noiseModel::Unit::shared_ptr& p): Base(p) {}
#ifdef GTSAM_MAGIC_GAUSSIAN
SharedNoiseModel(const Matrix& covariance) :
Base(boost::static_pointer_cast<noiseModel::Base>(
noiseModel::Gaussian::Covariance(covariance))) {}
SharedNoiseModel(const Vector& sigmas) :
Base(boost::static_pointer_cast<noiseModel::Base>(
noiseModel::Diagonal::Sigmas(sigmas))) {}
#endif
// Define GTSAM_DANGEROUS_GAUSSIAN to have access to bug-prone fixed dimension Gaussians
// Not intended for human use, only for backwards compatibility of old unit tests
#ifdef GTSAM_DANGEROUS_GAUSSIAN
SharedNoiseModel(const double& s) :
Base(boost::static_pointer_cast<noiseModel::Base>(
noiseModel::Isotropic::Sigma(GTSAM_DANGEROUS_GAUSSIAN, s))) {}
#endif
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & boost::serialization::make_nvp("SharedNoiseModel",
boost::serialization::base_object<Base>(*this));
}
};
}

10
gtsam/linear/tests/testNoiseModel.cpp
View File

@ -26,7 +26,7 @@ using namespace boost::assign;
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/TestableAssertions.h>
#include <gtsam/linear/NoiseModel.h>
#include <gtsam/linear/SharedGaussian.h>
#include <gtsam/linear/SharedNoiseModel.h>
#include <gtsam/linear/SharedDiagonal.h>
using namespace std;
@ -238,8 +238,8 @@ TEST(NoiseModel, QRNan )
TEST(NoiseModel, SmartCovariance )
{
bool smart = true;
SharedGaussian expected = Unit::Create(3);
SharedGaussian actual = Gaussian::Covariance(eye(3), smart);
SharedNoiseModel expected = Unit::Create(3);
SharedNoiseModel actual = Gaussian::Covariance(eye(3), smart);
EXPECT(assert_equal(*expected,*actual));
}
@ -247,8 +247,8 @@ TEST(NoiseModel, SmartCovariance )
TEST(NoiseModel, ScalarOrVector )
{
bool smart = true;
SharedGaussian expected = Unit::Create(3);
SharedGaussian actual = Gaussian::Covariance(eye(3), smart);
SharedNoiseModel expected = Unit::Create(3);
SharedNoiseModel actual = Gaussian::Covariance(eye(3), smart);
EXPECT(assert_equal(*expected,*actual));
}

32
gtsam/nonlinear/NonlinearFactor.h
View File

@ -27,7 +27,7 @@
#include <gtsam/inference/Factor-inl.h>
#include <gtsam/inference/IndexFactor.h>
#include <gtsam/linear/SharedGaussian.h>
#include <gtsam/linear/SharedNoiseModel.h>
#include <gtsam/linear/JacobianFactor.h>
#include <gtsam/nonlinear/Ordering.h>
@ -49,7 +49,7 @@ namespace gtsam {
typedef NonlinearFactor<VALUES> This;
SharedGaussian noiseModel_; /** Noise model */
SharedNoiseModel noiseModel_; /** Noise model */
public:
@ -65,7 +65,7 @@ namespace gtsam {
* Constructor
* @param noiseModel shared pointer to a noise model
*/
NonlinearFactor(const SharedGaussian& noiseModel) :
NonlinearFactor(const SharedNoiseModel& noiseModel) :
noiseModel_(noiseModel) {
}
@ -74,7 +74,7 @@ namespace gtsam {
* @param z measurement
* @param key by which to look up X value in Values
*/
NonlinearFactor(const SharedGaussian& noiseModel, const Symbol& key1) :
NonlinearFactor(const SharedNoiseModel& noiseModel, const Symbol& key1) :
Factor<Symbol>(key1), noiseModel_(noiseModel) {
}
@ -83,7 +83,7 @@ namespace gtsam {
* @param j1 key of the first variable
* @param j2 key of the second variable
*/
NonlinearFactor(const SharedGaussian& noiseModel, const Symbol& j1, const Symbol& j2) :
NonlinearFactor(const SharedNoiseModel& noiseModel, const Symbol& j1, const Symbol& j2) :
Factor<Symbol>(j1,j2), noiseModel_(noiseModel) {
}
@ -91,7 +91,7 @@ namespace gtsam {
* Constructor - arbitrary number of keys
* @param keys is the set of Symbols in the factor
*/
NonlinearFactor(const SharedGaussian& noiseModel, const std::set<Symbol>& keys) :
NonlinearFactor(const SharedNoiseModel& noiseModel, const std::set<Symbol>& keys) :
Factor<Symbol>(keys), noiseModel_(noiseModel) {
}
@ -120,7 +120,7 @@ namespace gtsam {
}
/** access to the noise model */
SharedGaussian get_noiseModel() const {
SharedNoiseModel get_noiseModel() const {
return noiseModel_;
}
@ -197,7 +197,7 @@ namespace gtsam {
* @param z measurement
* @param key by which to look up X value in Values
*/
NonlinearFactor1(const SharedGaussian& noiseModel, const KEY& key1) :
NonlinearFactor1(const SharedNoiseModel& noiseModel, const KEY& key1) :
Base(noiseModel,key1), key_(key1) {
}
@ -235,8 +235,7 @@ namespace gtsam {
boost::shared_dynamic_cast<noiseModel::Constrained>(this->noiseModel_);
if (constrained.get() != NULL)
return GaussianFactor::shared_ptr(new JacobianFactor(var, A, b, constrained));
this->noiseModel_->WhitenInPlace(A);
this->noiseModel_->whitenInPlace(b);
this->noiseModel_->WhitenSystem(A,b);
return GaussianFactor::shared_ptr(new JacobianFactor(var, A, b,
noiseModel::Unit::Create(b.size())));
}
@ -304,7 +303,7 @@ namespace gtsam {
* @param j1 key of the first variable
* @param j2 key of the second variable
*/
NonlinearFactor2(const SharedGaussian& noiseModel, KEY1 j1, KEY2 j2) :
NonlinearFactor2(const SharedNoiseModel& noiseModel, KEY1 j1, KEY2 j2) :
Base(noiseModel,j1,j2), key1_(j1), key2_(j2) {
}
@ -349,9 +348,7 @@ namespace gtsam {
return JacobianFactor::shared_ptr(new JacobianFactor(var1, A1, var2,
A2, b, constrained));
}
this->noiseModel_->WhitenInPlace(A1);
this->noiseModel_->WhitenInPlace(A2);
this->noiseModel_->whitenInPlace(b);
this->noiseModel_->WhitenSystem(A1,A2,b);
return GaussianFactor::shared_ptr(new JacobianFactor(var1, A1, var2,
A2, b, noiseModel::Unit::Create(b.size())));
}
@ -435,7 +432,7 @@ namespace gtsam {
* @param j2 key of the second variable
* @param j3 key of the third variable
*/
NonlinearFactor3(const SharedGaussian& noiseModel, KEY1 j1, KEY2 j2, KEY3 j3) :
NonlinearFactor3(const SharedNoiseModel& noiseModel, KEY1 j1, KEY2 j2, KEY3 j3) :
Base(noiseModel), key1_(j1), key2_(j2), key3_(j3) {
this->keys_.reserve(3);
this->keys_.push_back(key1_);
@ -487,10 +484,7 @@ namespace gtsam {
return GaussianFactor::shared_ptr(
new JacobianFactor(var1, A1, var2, A2, var3, A3, b, constrained));
}
this->noiseModel_->WhitenInPlace(A1);
this->noiseModel_->WhitenInPlace(A2);
this->noiseModel_->WhitenInPlace(A3);
this->noiseModel_->whitenInPlace(b);
this->noiseModel_->WhitenSystem(A1,A2,A3,b);
return GaussianFactor::shared_ptr(
new JacobianFactor(var1, A1, var2, A2, var3, A3, b, noiseModel::Unit::Create(b.size())));
}

2
gtsam/slam/BearingFactor.h
View File

@ -40,7 +40,7 @@ namespace gtsam {
/** primary constructor */
BearingFactor(const POSEKEY& i, const POINTKEY& j, const Rot2& z,
const SharedGaussian& model) :
const SharedNoiseModel& model) :
Base(model, i, j), z_(z) {
}

2
gtsam/slam/BearingRangeFactor.h
View File

@ -42,7 +42,7 @@ namespace gtsam {
BearingRangeFactor() {} /* Default constructor */
BearingRangeFactor(const POSEKEY& i, const POINTKEY& j, const Rot2& bearing, const double range,
const SharedGaussian& model) :
const SharedNoiseModel& model) :
Base(model, i, j), bearing_(bearing), range_(range) {
}

2
gtsam/slam/BetweenFactor.h
View File

@ -48,7 +48,7 @@ namespace gtsam {
/** Constructor */
BetweenFactor(const KEY1& key1, const KEY1& key2, const T& measured,
const SharedGaussian& model) :
const SharedNoiseModel& model) :
Base(model, key1, key2), measured_(measured) {
}

2
gtsam/slam/GeneralSFMFactor.h
View File

@ -55,7 +55,7 @@ namespace gtsam {
GeneralSFMFactor():z_(0.0,0.0) {}
GeneralSFMFactor(const Point2 & p):z_(p) {}
GeneralSFMFactor(double x, double y):z_(x,y) {}
GeneralSFMFactor(const Point2& z, const SharedGaussian& model, const CamK& i, const LmK& j) : Base(model, i, j), z_(z) {}
GeneralSFMFactor(const Point2& z, const SharedNoiseModel& model, const CamK& i, const LmK& j) : Base(model, i, j), z_(z) {}
virtual ~GeneralSFMFactor() {}

8
gtsam/slam/PartialPriorFactor.h
View File

@ -60,7 +60,7 @@ namespace gtsam {
* computation in the constructor. This should only be used by subclasses
* Sets the size of the mask with all values off
*/
PartialPriorFactor(const KEY& key, const SharedGaussian& model)
PartialPriorFactor(const KEY& key, const SharedNoiseModel& model)
: Base(model, key), mask_(model->dim(), false) {}
public:
@ -72,7 +72,7 @@ namespace gtsam {
/** Full Constructor: requires mask and vector - not for typical use */
PartialPriorFactor(const KEY& key, const std::vector<bool>& mask,
const Vector& prior, const SharedGaussian& model) :
const Vector& prior, const SharedNoiseModel& model) :
Base(model, key), prior_(prior), mask_(mask) {
assert(mask_.size() == T::Dim()); // NOTE: assumes constant size variable
assert(nrTrue() == model->dim());
@ -80,7 +80,7 @@ namespace gtsam {
}
/** Single Element Constructor: acts on a single parameter specified by idx */
PartialPriorFactor(const KEY& key, size_t idx, double prior, const SharedGaussian& model) :
PartialPriorFactor(const KEY& key, size_t idx, double prior, const SharedNoiseModel& model) :
Base(model, key), prior_(Vector_(1, prior)), mask_(T::Dim(), false) {
assert(model->dim() == 1);
mask_[idx] = true;
@ -89,7 +89,7 @@ namespace gtsam {
/** Indices Constructor: specify the mask with a set of indices */
PartialPriorFactor(const KEY& key, const std::vector<size_t>& mask, const Vector& prior,
const SharedGaussian& model) :
const SharedNoiseModel& model) :
Base(model, key), prior_(prior), mask_(T::Dim(), false) {
assert((size_t)prior_.size() == mask.size());
assert(model->dim() == (size_t) prior.size());

2
gtsam/slam/PriorFactor.h
View File

@ -51,7 +51,7 @@ namespace gtsam {
/** Constructor */
PriorFactor(const KEY& key, const T& prior,
const SharedGaussian& model) :
const SharedNoiseModel& model) :
Base(model, key), prior_(prior) {
}

2
gtsam/slam/ProjectionFactor.h
View File

@ -59,7 +59,7 @@ namespace gtsam {
* @param K the constant calibration
*/
GenericProjectionFactor(const Point2& z,
const SharedGaussian& model, POSK j_pose,
const SharedNoiseModel& model, POSK j_pose,
LMK j_landmark, const shared_ptrK& K) :
Base(model, j_pose, j_landmark), z_(z), K_(K) {
}

2
gtsam/slam/RangeFactor.h
View File

@ -38,7 +38,7 @@ namespace gtsam {
RangeFactor() {} /* Default constructor */
RangeFactor(const POSEKEY& i, const POINTKEY& j, double z,
const SharedGaussian& model) :
const SharedNoiseModel& model) :
Base(model, i, j), z_(z) {
}

4
gtsam/slam/Simulated3D.h
View File

@ -62,7 +62,7 @@ namespace simulated3D {
Point3 z_;
PointPrior3D(const Point3& z,
const SharedGaussian& model, const PoseKey& j) :
const SharedNoiseModel& model, const PoseKey& j) :
NonlinearFactor1<Values, PoseKey> (model, j), z_(z) {
}
@ -78,7 +78,7 @@ namespace simulated3D {
Point3 z_;
Simulated3DMeasurement(const Point3& z,
const SharedGaussian& model, PoseKey& j1, PointKey j2) :
const SharedNoiseModel& model, PoseKey& j1, PointKey j2) :
NonlinearFactor2<Values, PoseKey, PointKey> (
model, j1, j2), z_(z) {
}

2
gtsam/slam/StereoFactor.h
View File

@ -51,7 +51,7 @@ public:
* @param landmarkNumber is the index of the landmark
* @param K the constant calibration
*/
GenericStereoFactor(const StereoPoint2& z, const SharedGaussian& model, KEY1 j_pose,
GenericStereoFactor(const StereoPoint2& z, const SharedNoiseModel& model, KEY1 j_pose,
KEY2 j_landmark, const shared_ptrKStereo& K) :
Base(model, j_pose, j_landmark), z_(z), K_(K) {
}

2
gtsam/slam/dataset.cpp
View File

@ -124,7 +124,7 @@ pair<sharedPose2Graph, sharedPose2Values> load2D(const string& filename,
Pose2 l1Xl2(x, y, yaw);
// SharedGaussian noise = noiseModel::Gaussian::Covariance(m, smart);
// SharedNoiseModel noise = noiseModel::Gaussian::Covariance(m, smart);
if (!model) {
Vector variances = Vector_(3,m(0,0),m(1,1),m(2,2));
model = noiseModel::Diagonal::Variances(variances, smart);

10
gtsam/slam/planarSLAM.cpp
View File

@ -35,7 +35,7 @@ namespace gtsam {
NonlinearFactorGraph<Values>(graph) {}
void Graph::addPrior(const PoseKey& i, const Pose2& p,
const SharedGaussian& model) {
const SharedNoiseModel& model) {
sharedFactor factor(new Prior(i, p, model));
push_back(factor);
}
@ -46,25 +46,25 @@ namespace gtsam {
}
void Graph::addOdometry(const PoseKey& i, const PoseKey& j, const Pose2& z,
const SharedGaussian& model) {
const SharedNoiseModel& model) {
sharedFactor factor(new Odometry(i, j, z, model));
push_back(factor);
}
void Graph::addBearing(const PoseKey& i, const PointKey& j, const Rot2& z,
const SharedGaussian& model) {
const SharedNoiseModel& model) {
sharedFactor factor(new Bearing(i, j, z, model));
push_back(factor);
}
void Graph::addRange(const PoseKey& i, const PointKey& j, double z,
const SharedGaussian& model) {
const SharedNoiseModel& model) {
sharedFactor factor(new Range(i, j, z, model));
push_back(factor);
}
void Graph::addBearingRange(const PoseKey& i, const PointKey& j, const Rot2& z1,
double z2, const SharedGaussian& model) {
double z2, const SharedNoiseModel& model) {
sharedFactor factor(new BearingRange(i, j, z1, z2, model));
push_back(factor);
}

10
gtsam/slam/planarSLAM.h
View File

@ -50,16 +50,16 @@ namespace gtsam {
struct Graph: public NonlinearFactorGraph<Values> {
Graph(){}
Graph(const NonlinearFactorGraph<Values>& graph);
void addPrior(const PoseKey& i, const Pose2& p, const SharedGaussian& model);
void addPrior(const PoseKey& i, const Pose2& p, const SharedNoiseModel& model);
void addPoseConstraint(const PoseKey& i, const Pose2& p);
void addOdometry(const PoseKey& i, const PoseKey& j, const Pose2& z,
const SharedGaussian& model);
const SharedNoiseModel& model);
void addBearing(const PoseKey& i, const PointKey& j, const Rot2& z,
const SharedGaussian& model);
const SharedNoiseModel& model);
void addRange(const PoseKey& i, const PointKey& j, double z,
const SharedGaussian& model);
const SharedNoiseModel& model);
void addBearingRange(const PoseKey& i, const PointKey& j,
const Rot2& z1, double z2, const SharedGaussian& model);
const Rot2& z1, double z2, const SharedNoiseModel& model);
};
// Optimizer

4
gtsam/slam/pose2SLAM.cpp
View File

@ -42,13 +42,13 @@ namespace gtsam {
/* ************************************************************************* */
void Graph::addPrior(const Key& i, const Pose2& p,
const SharedGaussian& model) {
const SharedNoiseModel& model) {
sharedFactor factor(new Prior(i, p, model));
push_back(factor);
}
void Graph::addConstraint(const Key& i, const Key& j, const Pose2& z,
const SharedGaussian& model) {
const SharedNoiseModel& model) {
sharedFactor factor(new Constraint(i, j, z, model));
push_back(factor);
}

4
gtsam/slam/pose2SLAM.h
View File

@ -56,8 +56,8 @@ namespace gtsam {
struct Graph: public NonlinearFactorGraph<Values> {
typedef BetweenFactor<Values, Key> Constraint;
typedef Pose2 Pose;
void addPrior(const Key& i, const Pose2& p, const SharedGaussian& model);
void addConstraint(const Key& i, const Key& j, const Pose2& z, const SharedGaussian& model);
void addPrior(const Key& i, const Pose2& p, const SharedNoiseModel& model);
void addConstraint(const Key& i, const Key& j, const Pose2& z, const SharedNoiseModel& model);
void addHardConstraint(const Key& i, const Pose2& p);
};

4
gtsam/slam/pose3SLAM.cpp
View File

@ -52,13 +52,13 @@ namespace gtsam {
/* ************************************************************************* */
void Graph::addPrior(const Key& i, const Pose3& p,
const SharedGaussian& model) {
const SharedNoiseModel& model) {
sharedFactor factor(new Prior(i, p, model));
push_back(factor);
}
void Graph::addConstraint(const Key& i, const Key& j, const Pose3& z,
const SharedGaussian& model) {
const SharedNoiseModel& model) {
sharedFactor factor(new Constraint(i, j, z, model));
push_back(factor);
}

4
gtsam/slam/pose3SLAM.h
View File

@ -52,9 +52,9 @@ namespace gtsam {
// Graph
struct Graph: public NonlinearFactorGraph<Values> {
void addPrior(const Key& i, const Pose3& p,
const SharedGaussian& model);
const SharedNoiseModel& model);
void addConstraint(const Key& i, const Key& j, const Pose3& z,
const SharedGaussian& model);
const SharedNoiseModel& model);
void addHardConstraint(const Key& i, const Pose3& p);
};

6
gtsam/slam/simulated2D.h
View File

@ -105,7 +105,7 @@ namespace gtsam {
typedef typename KEY::Value Pose;
Pose z_;
GenericPrior(const Pose& z, const SharedGaussian& model, const KEY& key) :
GenericPrior(const Pose& z, const SharedNoiseModel& model, const KEY& key) :
NonlinearFactor1<CFG, KEY> (model, key), z_(z) {
}
@ -136,7 +136,7 @@ namespace gtsam {
typedef typename KEY::Value Pose;
Pose z_;
GenericOdometry(const Pose& z, const SharedGaussian& model,
GenericOdometry(const Pose& z, const SharedNoiseModel& model,
const KEY& i1, const KEY& i2) :
NonlinearFactor2<CFG, KEY, KEY> (model, i1, i2), z_(z) {
}
@ -170,7 +170,7 @@ namespace gtsam {
Point z_;
GenericMeasurement(const Point& z, const SharedGaussian& model,
GenericMeasurement(const Point& z, const SharedNoiseModel& model,
const XKEY& i, const LKEY& j) :
NonlinearFactor2<CFG, XKEY, LKEY> (model, i, j), z_(z) {
}

4
gtsam/slam/simulated2DOriented.h
View File

@ -63,7 +63,7 @@ namespace gtsam {
Pose2 z_;
GenericPosePrior(const Pose2& z, const SharedGaussian& model, const Key& key) :
GenericPosePrior(const Pose2& z, const SharedNoiseModel& model, const Key& key) :
NonlinearFactor1<CFG, Key> (model, key), z_(z) {
}
@ -81,7 +81,7 @@ namespace gtsam {
struct GenericOdometry: public NonlinearFactor2<CFG, KEY, KEY> {
Pose2 z_;
GenericOdometry(const Pose2& z, const SharedGaussian& model,
GenericOdometry(const Pose2& z, const SharedNoiseModel& model,
const KEY& i1, const KEY& i2) :
NonlinearFactor2<CFG, KEY, KEY> (model, i1, i2), z_(z) {
}

2
gtsam/slam/smallExample.cpp
View File

@ -201,7 +201,7 @@ namespace example {
Point2 z_;
UnaryFactor(const Point2& z, const SharedGaussian& model,
UnaryFactor(const Point2& z, const SharedNoiseModel& model,
const simulated2D::PoseKey& key) :
gtsam::NonlinearFactor1<Values, simulated2D::PoseKey>(model, key), z_(z) {
}

8
gtsam/slam/tests/testGeneralSFMFactor.cpp
View File

@ -42,7 +42,7 @@ typedef NonlinearEquality<Values, PointKey> Point3Constraint;
class Graph: public NonlinearFactorGraph<Values> {
public:
void addMeasurement(const CameraKey& i, const PointKey& j, const Point2& z, const SharedGaussian& model) {
void addMeasurement(const CameraKey& i, const PointKey& j, const Point2& z, const SharedNoiseModel& model) {
push_back(boost::make_shared<Projection>(z, model, i, j));
}
@ -75,7 +75,7 @@ double getGaussian()
typedef NonlinearOptimizer<Graph,Values> Optimizer;
const SharedGaussian sigma1(noiseModel::Unit::Create(1));
const SharedNoiseModel sigma1(noiseModel::Unit::Create(1));
/* ************************************************************************* */
TEST( GeneralSFMFactor, equals )
@ -83,7 +83,7 @@ TEST( GeneralSFMFactor, equals )
// Create two identical factors and make sure they're equal
Vector z = Vector_(2,323.,240.);
const int cameraFrameNumber=1, landmarkNumber=1;
const SharedGaussian sigma(noiseModel::Unit::Create(1));
const SharedNoiseModel sigma(noiseModel::Unit::Create(1));
boost::shared_ptr<Projection>
factor1(new Projection(z, sigma, cameraFrameNumber, landmarkNumber));
@ -97,7 +97,7 @@ TEST( GeneralSFMFactor, equals )
TEST( GeneralSFMFactor, error ) {
Point2 z(3.,0.);
const int cameraFrameNumber=1, landmarkNumber=1;
const SharedGaussian sigma(noiseModel::Unit::Create(1));
const SharedNoiseModel sigma(noiseModel::Unit::Create(1));
boost::shared_ptr<Projection>
factor(new Projection(z, sigma, cameraFrameNumber, landmarkNumber));
// For the following configuration, the factor predicts 320,240

8
gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp
View File

@ -41,7 +41,7 @@ typedef NonlinearEquality<Values, PointKey> Point3Constraint;
class Graph: public NonlinearFactorGraph<Values> {
public:
void addMeasurement(const CameraKey& i, const PointKey& j, const Point2& z, const SharedGaussian& model) {
void addMeasurement(const CameraKey& i, const PointKey& j, const Point2& z, const SharedNoiseModel& model) {
push_back(boost::make_shared<Projection>(z, model, i, j));
}
@ -74,7 +74,7 @@ double getGaussian()
typedef NonlinearOptimizer<Graph,Values> Optimizer;
const SharedGaussian sigma1(noiseModel::Unit::Create(1));
const SharedNoiseModel sigma1(noiseModel::Unit::Create(1));
/* ************************************************************************* */
TEST( GeneralSFMFactor, equals )
@ -82,7 +82,7 @@ TEST( GeneralSFMFactor, equals )
// Create two identical factors and make sure they're equal
Vector z = Vector_(2,323.,240.);
const int cameraFrameNumber=1, landmarkNumber=1;
const SharedGaussian sigma(noiseModel::Unit::Create(1));
const SharedNoiseModel sigma(noiseModel::Unit::Create(1));
boost::shared_ptr<Projection>
factor1(new Projection(z, sigma, cameraFrameNumber, landmarkNumber));
@ -96,7 +96,7 @@ TEST( GeneralSFMFactor, equals )
TEST( GeneralSFMFactor, error ) {
Point2 z(3.,0.);
const int cameraFrameNumber=1, landmarkNumber=1;
const SharedGaussian sigma(noiseModel::Unit::Create(1));
const SharedNoiseModel sigma(noiseModel::Unit::Create(1));
boost::shared_ptr<Projection>
factor(new Projection(z, sigma, cameraFrameNumber, landmarkNumber));
// For the following configuration, the factor predicts 320,240

2
gtsam/slam/tests/testPlanarSLAM.cpp
View File

@ -28,7 +28,7 @@ using namespace gtsam;
static Pose2 x1, x2(1, 1, 0), x3(1, 1, M_PI_4);
static Point2 l1(1, 0), l2(1, 1), l3(2, 2), l4(1, 3);
SharedGaussian
SharedNoiseModel
sigma(noiseModel::Isotropic::Sigma(1,0.1)),
sigma2(noiseModel::Isotropic::Sigma(2,0.1)),
I3(noiseModel::Unit::Create(3));

2
gtsam/slam/tests/testPose2SLAM.cpp
View File

@ -331,7 +331,7 @@ TEST( Pose2Values, expmap )
}
// Common measurement covariance
static SharedGaussian sigmas = sharedSigmas(Vector_(3,sx,sy,st));
static SharedNoiseModel sigmas = sharedSigmas(Vector_(3,sx,sy,st));
/* ************************************************************************* */
// Very simple test establishing Ax-b \approx z-h(x)

2
gtsam/slam/tests/testPose3SLAM.cpp
View File

@ -132,7 +132,7 @@ TEST( Pose3Factor, error )
Pose3 z(Rot3::rodriguez(0.2,0.2,0.3),Point3(0,1.1,0));;
// Create factor
SharedGaussian I6(noiseModel::Unit::Create(6));
SharedNoiseModel I6(noiseModel::Unit::Create(6));
Pose3Factor factor(1,2, z, I6);
// Create config

2
gtsam/slam/tests/testProjectionFactor.cpp
View File

@ -37,7 +37,7 @@ static double fov = 60; // degrees
static size_t w=640,h=480;
static Cal3_S2 K(fov,w,h);
static SharedGaussian sigma(noiseModel::Unit::Create(1));
static SharedNoiseModel sigma(noiseModel::Unit::Create(1));
static shared_ptrK sK(new Cal3_S2(K));
// make cameras

2
gtsam/slam/tests/testStereoFactor.cpp
View File

@ -45,7 +45,7 @@ StereoCamera stereoCam(Pose3(), K);
// point X Y Z in meters
Point3 p(0, 0, 5);
static SharedGaussian sigma(noiseModel::Unit::Create(1));
static SharedNoiseModel sigma(noiseModel::Unit::Create(1));
/* ************************************************************************* */
TEST( StereoFactor, singlePoint)

2
gtsam/slam/tests/testVSLAM.cpp
View File

@ -35,7 +35,7 @@ using namespace gtsam;
using namespace gtsam::visualSLAM;
using namespace boost;
static SharedGaussian sigma(noiseModel::Unit::Create(1));
static SharedNoiseModel sigma(noiseModel::Unit::Create(1));
/* ************************************************************************* */
Point3 landmark1(-1.0,-1.0, 0.0);

6
gtsam/slam/visualSLAM.h
View File

@ -79,7 +79,7 @@ namespace gtsam {
* @param j index of camera
* @param p to which pose to constrain it to
*/
void addMeasurement(const Point2& z, const SharedGaussian& model,
void addMeasurement(const Point2& z, const SharedNoiseModel& model,
PoseKey i, PointKey j, const shared_ptrK& K) {
boost::shared_ptr<ProjectionFactor> factor(new ProjectionFactor(z, model, i, j, K));
push_back(factor);
@ -111,7 +111,7 @@ namespace gtsam {
* @param p to which pose to constrain it to
* @param model uncertainty model of this prior
*/
void addPosePrior(int j, const Pose3& p = Pose3(), const SharedGaussian& model = noiseModel::Unit::Create(1)) {
void addPosePrior(int j, const Pose3& p = Pose3(), const SharedNoiseModel& model = noiseModel::Unit::Create(1)) {
boost::shared_ptr<PosePrior> factor(new PosePrior(j, p, model));
push_back(factor);
}
@ -121,7 +121,7 @@ namespace gtsam {
* @param j index of landmark
* @param model uncertainty model of this prior
*/
void addPointPrior(int j, const Point3& p = Point3(), const SharedGaussian& model = noiseModel::Unit::Create(1)) {
void addPointPrior(int j, const Point3& p = Point3(), const SharedNoiseModel& model = noiseModel::Unit::Create(1)) {
boost::shared_ptr<PointPrior> factor(new PointPrior(j, p, model));
push_back(factor);
}

2
tests/testNonlinearEqualityConstraint.cpp
View File

@ -221,7 +221,7 @@ TEST (testNonlinearEqualityConstraint, two_pose ) {
graph->add(eq2D::UnaryEqualityConstraint(pt_x2, x2));
Point2 z1(0.0, 5.0);
SharedGaussian sigma(noiseModel::Isotropic::Sigma(2, 0.1));
SharedNoiseModel sigma(noiseModel::Isotropic::Sigma(2, 0.1));
graph->add(simulated2D::Measurement(z1, sigma, x1,l1));
Point2 z2(-4.0, 0.0);

2
tests/testNonlinearFactor.cpp
View File

@ -43,7 +43,7 @@ typedef boost::shared_ptr<NonlinearFactor<VectorValues> > shared_nlf;
/* ************************************************************************* */
TEST( NonlinearFactor, equals )
{
SharedGaussian sigma(noiseModel::Isotropic::Sigma(2,1.0));
SharedNoiseModel sigma(noiseModel::Isotropic::Sigma(2,1.0));
// create two nonlinear2 factors
Point2 z3(0.,-1.);

2
tests/testPose2SLAMwSPCG.cpp
View File

@ -18,7 +18,7 @@ const double tol = 1e-5;
TEST(testPose2SLAMwSPCG, example1) {
/* generate synthetic data */
const SharedGaussian sigma(noiseModel::Unit::Create(0.1));
const SharedNoiseModel sigma(noiseModel::Unit::Create(0.1));
Key x1(1), x2(2), x3(3), x4(4), x5(5), x6(6), x7(7), x8(8), x9(9);
// create a 3 by 3 grid

42
tests/testSerialization.cpp
View File

@ -255,7 +255,7 @@ BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Gaussian, "gtsam_noiseModel_Gaussian"
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Unit, "gtsam_noiseModel_Unit");
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Isotropic, "gtsam_noiseModel_Isotropic");
BOOST_CLASS_EXPORT_GUID(gtsam::SharedGaussian, "gtsam_SharedGaussian");
BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel");
BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal");
/* ************************************************************************* */
@ -286,25 +286,25 @@ TEST (Serialization, noiseModels) {
}
/* ************************************************************************* */
TEST (Serialization, SharedGaussian_noiseModels) {
SharedGaussian diag3_sg = diag3;
EXPECT(equalsDereferenced<SharedGaussian>(diag3_sg));
EXPECT(equalsDereferencedXML<SharedGaussian>(diag3_sg));
TEST (Serialization, SharedNoiseModel_noiseModels) {
SharedNoiseModel diag3_sg = diag3;
EXPECT(equalsDereferenced<SharedNoiseModel>(diag3_sg));
EXPECT(equalsDereferencedXML<SharedNoiseModel>(diag3_sg));
EXPECT(equalsDereferenced<SharedGaussian>(diag3));
EXPECT(equalsDereferencedXML<SharedGaussian>(diag3));
EXPECT(equalsDereferenced<SharedNoiseModel>(diag3));
EXPECT(equalsDereferencedXML<SharedNoiseModel>(diag3));
EXPECT(equalsDereferenced<SharedGaussian>(iso3));
EXPECT(equalsDereferencedXML<SharedGaussian>(iso3));
EXPECT(equalsDereferenced<SharedNoiseModel>(iso3));
EXPECT(equalsDereferencedXML<SharedNoiseModel>(iso3));
EXPECT(equalsDereferenced<SharedGaussian>(gaussian3));
EXPECT(equalsDereferencedXML<SharedGaussian>(gaussian3));
EXPECT(equalsDereferenced<SharedNoiseModel>(gaussian3));
EXPECT(equalsDereferencedXML<SharedNoiseModel>(gaussian3));
EXPECT(equalsDereferenced<SharedGaussian>(unit3));
EXPECT(equalsDereferencedXML<SharedGaussian>(unit3));
EXPECT(equalsDereferenced<SharedNoiseModel>(unit3));
EXPECT(equalsDereferencedXML<SharedNoiseModel>(unit3));
EXPECT(equalsDereferenced<SharedGaussian>(constrained3));
EXPECT(equalsDereferencedXML<SharedGaussian>(constrained3));
EXPECT(equalsDereferenced<SharedNoiseModel>(constrained3));
EXPECT(equalsDereferencedXML<SharedNoiseModel>(constrained3));
}
/* ************************************************************************* */
@ -484,9 +484,9 @@ TEST (Serialization, planar_system) {
values.insert(PointKey(3), Point2(1.0, 2.0));
values.insert(PoseKey(4), Pose2(1.0, 2.0, 0.3));
SharedGaussian model1 = noiseModel::Isotropic::Sigma(1, 0.3);
SharedGaussian model2 = noiseModel::Isotropic::Sigma(2, 0.3);
SharedGaussian model3 = noiseModel::Isotropic::Sigma(3, 0.3);
SharedNoiseModel model1 = noiseModel::Isotropic::Sigma(1, 0.3);
SharedNoiseModel model2 = noiseModel::Isotropic::Sigma(2, 0.3);
SharedNoiseModel model3 = noiseModel::Isotropic::Sigma(3, 0.3);
Prior prior(PoseKey(3), Pose2(0.1,-0.3, 0.2), model1);
Bearing bearing(PoseKey(3), PointKey(5), Rot2::fromDegrees(0.5), model1);
@ -547,9 +547,9 @@ TEST (Serialization, visual_system) {
Point3 pt1(1.0, 2.0, 3.0), pt2(4.0, 5.0, 6.0);
values.insert(x1, pose1);
values.insert(l1, pt1);
SharedGaussian model2 = noiseModel::Isotropic::Sigma(2, 0.3);
SharedGaussian model3 = noiseModel::Isotropic::Sigma(3, 0.3);
SharedGaussian model6 = noiseModel::Isotropic::Sigma(6, 0.3);
SharedNoiseModel model2 = noiseModel::Isotropic::Sigma(2, 0.3);
SharedNoiseModel model3 = noiseModel::Isotropic::Sigma(3, 0.3);
SharedNoiseModel model6 = noiseModel::Isotropic::Sigma(6, 0.3);
boost::shared_ptr<Cal3_S2> K(new Cal3_S2(cal1));
Graph graph;