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

make SharedGaussian inherit SharedNoiseModel

release/4.3a0
Yong-Dian Jian 2011-08-27 01:44:49 +00:00
parent ee6569db29
commit af9b523b2d
4 changed files with 30 additions and 43 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
gtsam/base/Vector.cpp
View File

@ -229,19 +229,19 @@ bool assert_equal(const ConstSubVector& expected, const ConstSubVector& actual,
/* ************************************************************************* */
bool linear_dependent(const Vector& vec1, const Vector& vec2, double tol) {
if (vec1.size()!=vec2.size()) return false;
boost::optional<double> scale;
bool flag = false; double scale = 1.0;
size_t m = vec1.size();
for(size_t i=0; i<m; i++) {
if((fabs(vec1[i])>tol&&fabs(vec2[i])<tol) || (fabs(vec1[i])<tol&&fabs(vec2[i])>tol))
return false;
if(vec1[i] == 0 && vec2[i] == 0)
continue;
if (!scale)
if(vec1[i] == 0 && vec2[i] == 0) continue;
if (!flag) {
scale = vec1[i] / vec2[i];
else if (fabs(vec1[i] - vec2[i] * (*scale)) > tol)
return false;
flag = true ;
}
else if (fabs(vec1[i] - vec2[i]*scale) > tol) return false;
}
return scale.is_initialized();
return flag;
}
/* ************************************************************************* */

47
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
@ -26,44 +27,32 @@ namespace gtsam { // note, deliberately not in noiseModel namespace
* A useful convenience class to refer to a shared Gaussian model
* Also needed to make noise models in matlab
*/
struct SharedGaussian: public noiseModel::Gaussian::shared_ptr {
SharedGaussian() {
}
// TODO: better way ?
SharedGaussian(const noiseModel::Gaussian::shared_ptr& p) :
noiseModel::Gaussian::shared_ptr(p) {
}
SharedGaussian(const noiseModel::Diagonal::shared_ptr& p) :
noiseModel::Gaussian::shared_ptr(p) {
}
SharedGaussian(const noiseModel::Constrained::shared_ptr& p) :
noiseModel::Gaussian::shared_ptr(p) {
}
SharedGaussian(const noiseModel::Isotropic::shared_ptr& p) :
noiseModel::Gaussian::shared_ptr(p) {
}
SharedGaussian(const noiseModel::Unit::shared_ptr& p) :
noiseModel::Gaussian::shared_ptr(p) {
}
struct SharedGaussian: public SharedNoiseModel {
typedef SharedNoiseModel Base;
SharedGaussian() {}
SharedGaussian(const noiseModel::Gaussian::shared_ptr& p) : Base(p) {}
SharedGaussian(const noiseModel::Diagonal::shared_ptr& p) : Base(p) {}
SharedGaussian(const noiseModel::Constrained::shared_ptr& p) : Base(p) {}
SharedGaussian(const noiseModel::Isotropic::shared_ptr& p) : Base(p) {}
SharedGaussian(const noiseModel::Unit::shared_ptr& p) : Base(p) {}
// Define GTSAM_MAGIC_GAUSSIAN to have access to slightly
// dangerous and non-shared (inefficient, wasteful) noise models.
// Intended to be used only in tests (if you must) and the MATLAB wrapper
#ifdef GTSAM_MAGIC_GAUSSIAN
SharedGaussian(const Matrix& covariance) :
noiseModel::Gaussian::shared_ptr(noiseModel::Gaussian::Covariance(covariance)) {
}
SharedGaussian(const Vector& sigmas) :
noiseModel::Gaussian::shared_ptr(noiseModel::Diagonal::Sigmas(sigmas)) {
}
SharedGaussian(const Matrix& covariance)
: Base(noiseModel::Gaussian::Covariance(covariance)) {}
SharedGaussian(const Vector& sigmas)
: 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
SharedGaussian(const double& s) :
noiseModel::Gaussian::shared_ptr(noiseModel::Isotropic::Sigma(
GTSAM_DANGEROUS_GAUSSIAN, s)) {
}
Base(noiseModel::Isotropic::Sigma(GTSAM_DANGEROUS_GAUSSIAN, s)) {}
#endif
/** Serialization function */
@ -71,7 +60,7 @@ namespace gtsam { // note, deliberately not in noiseModel namespace
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & boost::serialization::make_nvp("SharedGaussian",
boost::serialization::base_object<noiseModel::Gaussian::shared_ptr >(*this));
boost::serialization::base_object<noiseModel::Gaussian::shared_ptr >(*this));
}
};
}

2
gtsam/linear/SharedNoiseModel.h
View File

@ -12,7 +12,6 @@
#pragma once
#include <gtsam/linear/NoiseModel.h>
#include <gtsam/linear/SharedGaussian.h>
namespace gtsam { // note, deliberately not in noiseModel namespace
@ -21,7 +20,6 @@ namespace gtsam { // note, deliberately not in noiseModel namespace
typedef noiseModel::Base::shared_ptr Base;
SharedNoiseModel() {}
SharedNoiseModel(const SharedGaussian &p): Base(p) {}
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) {}

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