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

Fixed issue with serialization by actively changing inf/nan values to zero in constrained noisemodel. Updated WhitenInPlace() and distance(). Apparently this caused a bunch of other unit tests to fail, though.

release/4.3a0
Alex Cunningham 2014-01-04 20:01:05 -05:00
parent 54c1ec0dcb
commit dfb3e60171
4 changed files with 65 additions and 31 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

48
gtsam/linear/NoiseModel.cpp
View File

@ -166,9 +166,11 @@ void Gaussian::WhitenSystem(Matrix& A1, Matrix& A2, Matrix& A3, Vector& b) const
// Diagonal
/* ************************************************************************* */
Diagonal::Diagonal() :
Gaussian(1), sigmas_(ones(1)), invsigmas_(ones(1)), precisions_(ones(1)) {
Gaussian(1)//, sigmas_(ones(1)), invsigmas_(ones(1)), precisions_(ones(1))
{
}
/* ************************************************************************* */
Diagonal::Diagonal(const Vector& sigmas) :
Gaussian(sigmas.size()), sigmas_(sigmas), invsigmas_(reciprocal(sigmas)), precisions_(
emul(invsigmas_, invsigmas_)) {
@ -229,6 +231,31 @@ void Diagonal::WhitenInPlace(Eigen::Block<Matrix> H) const {
/* ************************************************************************* */
// Constrained
/* ************************************************************************* */
/* ************************************************************************* */
Constrained::Constrained(const Vector& sigmas)
: Diagonal(sigmas), mu_(repeat(sigmas.size(), 1000.0)) {
for (int i=0; i<sigmas.size(); ++i) {
if (!std::isfinite(1./sigmas(i))) {
precisions_(i) = 0.0; // Set to finite value
invsigmas_(i) = 0.0;
}
}
}
/* ************************************************************************* */
Constrained::Constrained(const Vector& mu, const Vector& sigmas)
: Diagonal(sigmas), mu_(mu) {
assert(sigmas.size() == mu.size());
for (int i=0; i<sigmas.size(); ++i) {
if (!std::isfinite(1./sigmas(i))) {
precisions_(i) = 0.0; // Set to finite value
invsigmas_(i) = 0.0;
}
}
}
/* ************************************************************************* */
Constrained::shared_ptr Constrained::MixedSigmas(const Vector& mu, const Vector& sigmas, bool smart) {
// FIXME: can't return a diagonal shared_ptr due to conversion
@ -269,12 +296,9 @@ Vector Constrained::whiten(const Vector& v) const {
double Constrained::distance(const Vector& v) const {
Vector w = Diagonal::whiten(v); // get noisemodel for constrained elements
// TODO Find a better way of doing these checks
for (size_t i=0; i<dim_; ++i) { // add mu weights on constrained variables
if (std::isinf(w[i])) // whiten makes constrained variables infinite
for (size_t i=0; i<dim_; ++i) // add mu weights on constrained variables
if (!std::isfinite(1./sigmas_[i])) // whiten makes constrained variables zero
w[i] = v[i] * sqrt(mu_[i]); // TODO: may want to store sqrt rather than rebuild
if (std::isnan(w[i])) // ensure no other invalid values make it through
w[i] = v[i];
}
return w.dot(w);
}
@ -283,7 +307,9 @@ Matrix Constrained::Whiten(const Matrix& H) const {
// selective scaling
// Now allow augmented Matrix with a new additional part coming
// from the Lagrange multiplier.
return vector_scale(invsigmas(), H.block(0, 0, dim(), H.cols()), true);
Matrix M(H.block(0, 0, dim(), H.cols()));
Constrained::WhitenInPlace(M);
return M;
}
/* ************************************************************************* */
@ -294,7 +320,13 @@ void Constrained::WhitenInPlace(Matrix& H) const {
// indicating a hard constraint, we leave H's row i in place.
// Now allow augmented Matrix with a new additional part coming
// from the Lagrange multiplier.
vector_scale_inplace(invsigmas(), H, true);
// Inlined: vector_scale_inplace(invsigmas(), H, true);
// vector_scale_inplace(v, A, true);
for (DenseIndex i=0; i<(DenseIndex)dim_; ++i) {
const double& invsigma = invsigmas_(i);
if (std::isfinite(1./sigmas_(i)))
H.row(i) *= invsigma;
}
}
/* ************************************************************************* */

31
gtsam/linear/NoiseModel.h
View File

@ -258,10 +258,10 @@ namespace gtsam {
Vector sigmas_, invsigmas_, precisions_;
protected:
/** protected constructor takes sigmas */
/** protected constructor - no initializations */
Diagonal();
/** constructor to allow for disabling initializaion of invsigmas */
/** constructor to allow for disabling initialization of invsigmas */
Diagonal(const Vector& sigmas);
public:
@ -272,7 +272,7 @@ namespace gtsam {
/**
* A diagonal noise model created by specifying a Vector of sigmas, i.e.
* standard devations, the diagonal of the square root covariance matrix.
* standard deviations, the diagonal of the square root covariance matrix.
*/
static shared_ptr Sigmas(const Vector& sigmas, bool smart = true);
@ -353,19 +353,22 @@ namespace gtsam {
protected:
// Sigmas are contained in the base class
Vector mu_; ///< Penalty function weight - needs to be large enough to dominate soft constraints
// Penalty function parameters
Vector mu_;
/**
* protected constructor takes sigmas.
* prevents any inf values
* from appearing in invsigmas or precisions.
* mu set to large default value (1000.0)
*/
Constrained(const Vector& sigmas = zero(1));
/** protected constructor takes sigmas */
// Keeps only sigmas and calculates invsigmas when necessary
Constrained(const Vector& sigmas = zero(1)) :
Diagonal(sigmas), mu_(repeat(sigmas.size(), 1000.0)) {}
// Keeps only sigmas and calculates invsigmas when necessary
// allows for specifying mu
Constrained(const Vector& mu, const Vector& sigmas) :
Diagonal(sigmas), mu_(mu) {}
/**
* Constructor that prevents any inf values
* from appearing in invsigmas or precisions.
* Allows for specifying mu.
*/
Constrained(const Vector& mu, const Vector& sigmas);
public:

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

@ -161,9 +161,8 @@ TEST(NoiseModel, ConstrainedConstructors )
// TODO: why should this be a thousand ??? Dummy variable?
EXPECT(assert_equal(gtsam::repeat(d, 1000.0), actual->mu()));
EXPECT(assert_equal(gtsam::repeat(d, 0), actual->sigmas()));
double Inf = numeric_limits<double>::infinity();
EXPECT(assert_equal(gtsam::repeat(d, Inf), actual->invsigmas()));
EXPECT(assert_equal(gtsam::repeat(d, Inf), actual->precisions()));
EXPECT(assert_equal(gtsam::repeat(d, 0), actual->invsigmas())); // Actually zero as dummy value
EXPECT(assert_equal(gtsam::repeat(d, 0), actual->precisions())); // Actually zero as dummy value
actual = Constrained::All(d, m);
EXPECT(assert_equal(gtsam::repeat(d, m), actual->mu()));

12
gtsam/linear/tests/testSerializationLinear.cpp
View File

@ -75,8 +75,8 @@ TEST (Serialization, noiseModels) {
EXPECT(equalsDereferencedBinary<noiseModel::Unit::shared_ptr>(unit3));
EXPECT(equalsDereferencedBinary<noiseModel::Constrained::shared_ptr>(constrained3));
EXPECT(equalsDereferenced<noiseModel::Constrained::shared_ptr>(constrained3)); // FAILS: "Exception: input stream error" - there are inf's that cannot be deserialized
EXPECT(equalsDereferencedXML<noiseModel::Constrained::shared_ptr>(constrained3)); // FAILS: "Exception: input stream error"
EXPECT(equalsDereferenced<noiseModel::Constrained::shared_ptr>(constrained3));
EXPECT(equalsDereferencedXML<noiseModel::Constrained::shared_ptr>(constrained3));
}
/* ************************************************************************* */
@ -103,8 +103,8 @@ TEST (Serialization, SharedNoiseModel_noiseModels) {
EXPECT(equalsDereferencedBinary<SharedNoiseModel>(unit3));
EXPECT(equalsDereferencedBinary<SharedNoiseModel>(constrained3));
EXPECT(equalsDereferenced<SharedNoiseModel>(constrained3)); // FAILS: "Exception: input stream error"
EXPECT(equalsDereferencedXML<SharedNoiseModel>(constrained3)); // FAILS: "Exception: input stream error"
EXPECT(equalsDereferenced<SharedNoiseModel>(constrained3));
EXPECT(equalsDereferencedXML<SharedNoiseModel>(constrained3));
}
/* ************************************************************************* */
@ -122,8 +122,8 @@ TEST (Serialization, SharedDiagonal_noiseModels) {
EXPECT(equalsDereferencedBinary<SharedDiagonal>(unit3));
EXPECT(equalsDereferencedBinary<SharedDiagonal>(constrained3));
EXPECT(equalsDereferenced<SharedDiagonal>(constrained3)); // FAILS: "Exception: input stream error"
EXPECT(equalsDereferencedXML<SharedDiagonal>(constrained3)); // FAILS: "Exception: input stream error"
EXPECT(equalsDereferenced<SharedDiagonal>(constrained3));
EXPECT(equalsDereferencedXML<SharedDiagonal>(constrained3));
}
/* Create GUIDs for factors */