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

remove support for embedded lagrangian part to constraint's jacobian matrices. It's very hacky!

release/4.3a0
thduynguyen 2014-04-29 16:20:32 -04:00
parent db93f4137c
commit fc63f540db
3 changed files with 17 additions and 25 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

25
gtsam/linear/NoiseModel.cpp
View File

@ -334,9 +334,9 @@ void Constrained::print(const std::string& name) const {
/* ************************************************************************* */ /* ************************************************************************* */
Vector Constrained::whiten(const Vector& v) const { Vector Constrained::whiten(const Vector& v) const {
// If sigmas[i] is not 0 then divide v[i] by sigmas[i], as usually done in // If sigmas[i] > 0 then divide v[i] by sigmas[i], as usually done in
// other normal Gaussian noise model. Otherwise, sigmas[i] = 0 indicating // other normal Gaussian noise model. Otherwise, sigmas[i] <= 0 indicating
// a hard constraint, we don't do anything. // a constraint (ineq and eq), we don't do anything.
const Vector& a = v; const Vector& a = v;
const Vector& b = sigmas_; const Vector& b = sigmas_;
// Now allows for whiten augmented vector with a new additional part coming // Now allows for whiten augmented vector with a new additional part coming
@ -344,7 +344,7 @@ Vector Constrained::whiten(const Vector& v) const {
Vector c = a; Vector c = a;
for( DenseIndex i = 0; i < b.size(); i++ ) { for( DenseIndex i = 0; i < b.size(); i++ ) {
const double& ai = a(i), &bi = b(i); const double& ai = a(i), &bi = b(i);
if (bi!=0) c(i) = ai/bi; if (bi>0) c(i) = ai/bi;
} }
return c; return c;
} }
@ -355,6 +355,7 @@ double Constrained::distance(const Vector& v) const {
// TODO Find a better way of doing these checks // TODO Find a better way of doing these checks
for (size_t i=0; i<dim_; ++i) // add mu weights on constrained variables 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 if (!std::isfinite(1./sigmas_[i])) // whiten makes constrained variables zero
// TODO: how to deal with ineq constraint (sigmas_[i] < 0) ???
w[i] = v[i] * sqrt(mu_[i]); // TODO: may want to store sqrt rather than rebuild w[i] = v[i] * sqrt(mu_[i]); // TODO: may want to store sqrt rather than rebuild
return w.dot(w); return w.dot(w);
} }
@ -381,7 +382,7 @@ void Constrained::WhitenInPlace(Matrix& H) const {
// vector_scale_inplace(v, A, true); // vector_scale_inplace(v, A, true);
for (DenseIndex i=0; i<(DenseIndex)dim_; ++i) { for (DenseIndex i=0; i<(DenseIndex)dim_; ++i) {
const double& invsigma = invsigmas_(i); const double& invsigma = invsigmas_(i);
if (std::isfinite(1./sigmas_(i))) if (std::isfinite(1./sigmas_(i)) && sigmas_(i)>0)
H.row(i) *= invsigma; H.row(i) *= invsigma;
} }
} }
@ -394,19 +395,17 @@ void Constrained::WhitenInPlace(Eigen::Block<Matrix> H) const {
// indicating a hard constraint, we leave H's row i in place. // indicating a hard constraint, we leave H's row i in place.
const Vector& _invsigmas = invsigmas(); const Vector& _invsigmas = invsigmas();
for(DenseIndex row = 0; row < _invsigmas.size(); ++row) for(DenseIndex row = 0; row < _invsigmas.size(); ++row)
if(isfinite(_invsigmas(row))) if(isfinite(_invsigmas(row)) && _invsigmas(row)>0 )
H.row(row) *= _invsigmas(row); H.row(row) *= _invsigmas(row);
} }
/* ************************************************************************* */ /* ************************************************************************* */
Constrained::shared_ptr Constrained::unit(size_t augmentedDim) const { Constrained::shared_ptr Constrained::unit() const {
Vector sigmas = ones(dim()+augmentedDim); Vector sigmas = ones(dim());
for (size_t i=0; i<dim(); ++i) for (size_t i=0; i<dim(); ++i)
if (this->sigmas_(i) == 0.0) if (this->sigmas_(i) <= 0.0)
sigmas(i) = 0.0; sigmas(i) = sigmas_(i);
Vector augmentedMu = zero(dim()+augmentedDim); return MixedSigmas(mu_, sigmas);
subInsert(augmentedMu, mu_, 0);
return MixedSigmas(augmentedMu, sigmas);
} }
/* ************************************************************************* */ /* ************************************************************************* */

3
gtsam/linear/NoiseModel.h
View File

@ -479,9 +479,8 @@ namespace gtsam {
/** /**
* Returns a Unit version of a constrained noisemodel in which * Returns a Unit version of a constrained noisemodel in which
* constrained sigmas remain constrained and the rest are unit scaled * constrained sigmas remain constrained and the rest are unit scaled
* Now support augmented part from the Lagrange multiplier.
*/ */
shared_ptr unit(size_t augmentedDim = 0) const; shared_ptr unit() const;
private: private:
/** Serialization function */ /** Serialization function */

12
gtsam/nonlinear/NonlinearFactor.h
View File

@ -325,16 +325,10 @@ public:
{ {
noiseModel::Constrained::shared_ptr constrained = noiseModel::Constrained::shared_ptr constrained =
boost::dynamic_pointer_cast<noiseModel::Constrained>(this->noiseModel_); boost::dynamic_pointer_cast<noiseModel::Constrained>(this->noiseModel_);
if(constrained) { if(constrained)
size_t augmentedDim = terms[0].second.rows() - constrained->dim(); return GaussianFactor::shared_ptr(new JacobianFactor(terms, b, constrained->unit()));
Vector augmentedZero = zero(augmentedDim);
Vector augmentedb = concatVectors(2, &b, &augmentedZero);
return GaussianFactor::shared_ptr(new JacobianFactor(terms, augmentedb, constrained->unit(augmentedDim)));
} }
else
return GaussianFactor::shared_ptr(new JacobianFactor(terms, b));
}
else
return GaussianFactor::shared_ptr(new JacobianFactor(terms, b)); return GaussianFactor::shared_ptr(new JacobianFactor(terms, b));
} }