Merged in feature/RevertConstraintNoiseHack (pull request #21)
commit
b4763ea83a
|
@ -543,8 +543,7 @@ Matrix collect(size_t nrMatrices, ...)
|
||||||
void vector_scale_inplace(const Vector& v, Matrix& A, bool inf_mask) {
|
void vector_scale_inplace(const Vector& v, Matrix& A, bool inf_mask) {
|
||||||
const DenseIndex m = A.rows();
|
const DenseIndex m = A.rows();
|
||||||
if (inf_mask) {
|
if (inf_mask) {
|
||||||
// only scale the first v.size() rows of A to support augmented Matrix
|
for (DenseIndex i=0; i<m; ++i) {
|
||||||
for (DenseIndex i=0; i<v.size(); ++i) {
|
|
||||||
const double& vi = v(i);
|
const double& vi = v(i);
|
||||||
if (std::isfinite(vi))
|
if (std::isfinite(vi))
|
||||||
A.row(i) *= vi;
|
A.row(i) *= vi;
|
||||||
|
|
|
@ -398,7 +398,6 @@ GTSAM_EXPORT Matrix collect(size_t nrMatrices, ...);
|
||||||
* Arguments (Matrix, Vector) scales the columns,
|
* Arguments (Matrix, Vector) scales the columns,
|
||||||
* (Vector, Matrix) scales the rows
|
* (Vector, Matrix) scales the rows
|
||||||
* @param inf_mask when true, will not scale with a NaN or inf value.
|
* @param inf_mask when true, will not scale with a NaN or inf value.
|
||||||
* The inplace version also allows v.size()<A.rows() and only scales the first v.size() rows of A.
|
|
||||||
*/
|
*/
|
||||||
GTSAM_EXPORT void vector_scale_inplace(const Vector& v, Matrix& A, bool inf_mask = false); // row
|
GTSAM_EXPORT void vector_scale_inplace(const Vector& v, Matrix& A, bool inf_mask = false); // row
|
||||||
GTSAM_EXPORT Matrix vector_scale(const Vector& v, const Matrix& A, bool inf_mask = false); // row
|
GTSAM_EXPORT Matrix vector_scale(const Vector& v, const Matrix& A, bool inf_mask = false); // row
|
||||||
|
|
|
@ -339,12 +339,12 @@ Vector Constrained::whiten(const Vector& v) const {
|
||||||
// a hard constraint, we don't do anything.
|
// a hard constraint, 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
|
size_t n = a.size();
|
||||||
// from the Lagrange multiplier. So a.size() >= b.size()
|
assert (b.size()==a.size());
|
||||||
Vector c = a;
|
Vector c(n);
|
||||||
for( DenseIndex i = 0; i < b.size(); i++ ) {
|
for( size_t i = 0; i < n; i++ ) {
|
||||||
const double& ai = a(i), &bi = b(i);
|
const double& ai = a(i), &bi = b(i);
|
||||||
if (bi!=0) c(i) = ai/bi;
|
c(i) = (bi==0.0) ? ai : ai/bi; // NOTE: not ediv_()
|
||||||
}
|
}
|
||||||
return c;
|
return c;
|
||||||
}
|
}
|
||||||
|
@ -362,11 +362,7 @@ double Constrained::distance(const Vector& v) const {
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
Matrix Constrained::Whiten(const Matrix& H) const {
|
Matrix Constrained::Whiten(const Matrix& H) const {
|
||||||
// selective scaling
|
// selective scaling
|
||||||
// Now allow augmented Matrix with a new additional part coming
|
return vector_scale(invsigmas(), H, true);
|
||||||
// from the Lagrange multiplier.
|
|
||||||
Matrix M(H.block(0, 0, dim(), H.cols()));
|
|
||||||
Constrained::WhitenInPlace(M);
|
|
||||||
return M;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
|
@ -375,8 +371,6 @@ void Constrained::WhitenInPlace(Matrix& H) const {
|
||||||
// Scale row i of H by sigmas[i], basically multiplying H with diag(sigmas)
|
// Scale row i of H by sigmas[i], basically multiplying H with diag(sigmas)
|
||||||
// Set inf_mask flag is true so that if invsigmas[i] is inf, i.e. sigmas[i] = 0,
|
// Set inf_mask flag is true so that if invsigmas[i] is inf, i.e. sigmas[i] = 0,
|
||||||
// indicating a hard constraint, we leave H's row i in place.
|
// 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.
|
|
||||||
// Inlined: vector_scale_inplace(invsigmas(), H, true);
|
// Inlined: vector_scale_inplace(invsigmas(), H, true);
|
||||||
// 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) {
|
||||||
|
@ -399,14 +393,12 @@ void Constrained::WhitenInPlace(Eigen::Block<Matrix> H) const {
|
||||||
}
|
}
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
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) = 0.0;
|
||||||
Vector augmentedMu = zero(dim()+augmentedDim);
|
return MixedSigmas(mu_, sigmas);
|
||||||
subInsert(augmentedMu, mu_, 0);
|
|
||||||
return MixedSigmas(augmentedMu, sigmas);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
|
|
|
@ -481,9 +481,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 */
|
||||||
|
@ -732,7 +731,7 @@ namespace gtsam {
|
||||||
};
|
};
|
||||||
|
|
||||||
/// Cauchy implements the "Cauchy" robust error model (Lee2013IROS). Contributed by:
|
/// Cauchy implements the "Cauchy" robust error model (Lee2013IROS). Contributed by:
|
||||||
/// Dipl.-Inform. Jan Oberl<EFBFBD>nder (M.Sc.), FZI Research Center for
|
/// Dipl.-Inform. Jan Oberlaender (M.Sc.), FZI Research Center for
|
||||||
/// Information Technology, Karlsruhe, Germany.
|
/// Information Technology, Karlsruhe, Germany.
|
||||||
/// oberlaender@fzi.de
|
/// oberlaender@fzi.de
|
||||||
/// Thanks Jan!
|
/// Thanks Jan!
|
||||||
|
|
|
@ -319,18 +319,13 @@ public:
|
||||||
terms[j].second.swap(A[j]);
|
terms[j].second.swap(A[j]);
|
||||||
}
|
}
|
||||||
|
|
||||||
// TODO pass unwhitened + noise model to Gaussian factor
|
|
||||||
// For now, only linearized constrained factors have noise model at linear level!!!
|
|
||||||
if(noiseModel_)
|
if(noiseModel_)
|
||||||
{
|
{
|
||||||
|
// TODO pass unwhitened + noise model to Gaussian factor
|
||||||
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
|
else
|
||||||
return GaussianFactor::shared_ptr(new JacobianFactor(terms, b));
|
return GaussianFactor::shared_ptr(new JacobianFactor(terms, b));
|
||||||
}
|
}
|
||||||
|
|
Loading…
Reference in New Issue