Removed Permuted views to simplify a lot of code - all objects that need to be permuted now have their data rearranged instead of using the "Permuted" indirection class.
Richard Roberts
parent
5431d74573
commit
57ca7d77db
|
|
@ -26,7 +26,7 @@
|
|||
|
||||
namespace gtsam {
|
||||
|
||||
/**
|
||||
/**
|
||||
* A permutation reorders variables, for example to reduce fill-in during
|
||||
* elimination. To save computation, the permutation can be applied to
|
||||
* the necessary data structures only once, then multiple computations
|
||||
|
|
@ -151,8 +151,8 @@ public:
|
|||
*/
|
||||
Permutation::shared_ptr inverse() const;
|
||||
|
||||
const_iterator begin() const { return rangeIndices_.begin(); } ///<TODO: comment
|
||||
const_iterator end() const { return rangeIndices_.end(); } ///<TODO: comment
|
||||
const_iterator begin() const { return rangeIndices_.begin(); } ///< Iterate through the indices
|
||||
const_iterator end() const { return rangeIndices_.end(); } ///< Iterate through the indices
|
||||
|
||||
|
||||
/// @}
|
||||
|
|
@ -167,8 +167,8 @@ public:
|
|||
*/
|
||||
Permutation::shared_ptr partialPermutation(const Permutation& selector, const Permutation& partialPermutation) const;
|
||||
|
||||
iterator begin() { return rangeIndices_.begin(); } ///<TODO: comment
|
||||
iterator end() { return rangeIndices_.end(); } ///<TODO: comment
|
||||
iterator begin() { return rangeIndices_.begin(); } ///< Iterate through the indices
|
||||
iterator end() { return rangeIndices_.end(); } ///< Iterate through the indices
|
||||
|
||||
protected:
|
||||
void check(Index variable) const { assert(variable < rangeIndices_.size()); }
|
||||
|
|
@ -176,77 +176,4 @@ protected:
|
|||
/// @}
|
||||
};
|
||||
|
||||
|
||||
/**
|
||||
* Syntactic sugar for accessing another container through a permutation.
|
||||
* Allows the syntax:
|
||||
* Permuted<Container> permuted(permutation, container);
|
||||
* permuted[index1];
|
||||
* permuted[index2];
|
||||
* which is equivalent to:
|
||||
* container[permutation[index1]];
|
||||
* container[permutation[index2]];
|
||||
* but more concise.
|
||||
*/
|
||||
template<typename CONTAINER>
|
||||
class Permuted {
|
||||
Permutation permutation_;
|
||||
CONTAINER& container_;
|
||||
public:
|
||||
typedef typename CONTAINER::iterator::value_type value_type;
|
||||
|
||||
/** Construct as a permuted view on the Container. The permutation is copied
|
||||
* but only a reference to the container is stored.
|
||||
*/
|
||||
Permuted(const Permutation& permutation, CONTAINER& container) : permutation_(permutation), container_(container) {}
|
||||
|
||||
/** Construct as a view on the Container with an identity permutation. Only
|
||||
* a reference to the container is stored.
|
||||
*/
|
||||
Permuted(CONTAINER& container) : permutation_(Permutation::Identity(container.size())), container_(container) {}
|
||||
|
||||
/** Print */
|
||||
void print(const std::string& str = "") const {
|
||||
std::cout << str;
|
||||
permutation_.print(" permutation: ");
|
||||
container_.print(" container: ");
|
||||
}
|
||||
|
||||
/** Access the container through the permutation */
|
||||
value_type& operator[](size_t index) { return container_[permutation_[index]]; }
|
||||
|
||||
/** Access the container through the permutation (const version) */
|
||||
const value_type& operator[](size_t index) const { return container_[permutation_[index]]; }
|
||||
|
||||
/** Assignment operator for cloning in ISAM2 */
|
||||
Permuted<CONTAINER> operator=(const Permuted<CONTAINER>& other) {
|
||||
permutation_ = other.permutation_;
|
||||
container_ = other.container_;
|
||||
return *this;
|
||||
}
|
||||
|
||||
/** Permute this view by applying a permutation to the underlying permutation */
|
||||
void permute(const Permutation& permutation) { assert(permutation.size() == this->size()); permutation_ = *permutation_.permute(permutation); }
|
||||
|
||||
/** Access the underlying container */
|
||||
CONTAINER* operator->() { return &container_; }
|
||||
|
||||
/** Access the underlying container (const version) */
|
||||
const CONTAINER* operator->() const { return &container_; }
|
||||
|
||||
/** Size of the underlying container */
|
||||
size_t size() const { return container_.size(); }
|
||||
|
||||
/** Access to the underlying container */
|
||||
CONTAINER& container() { return container_; }
|
||||
|
||||
/** Access to the underlying container (const version) */
|
||||
const CONTAINER& container() const { return container_; }
|
||||
|
||||
/** Access the underlying permutation */
|
||||
Permutation& permutation() { return permutation_; }
|
||||
const Permutation& permutation() const { return permutation_; }
|
||||
};
|
||||
|
||||
|
||||
}
|
||||
|
|
|
|||
|
|
@ -18,37 +18,12 @@
|
|||
#include <iostream>
|
||||
|
||||
#include <gtsam/inference/VariableIndex.h>
|
||||
#include <gtsam/inference/Permutation.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
using namespace std;
|
||||
|
||||
/* ************************************************************************* */
|
||||
VariableIndex::VariableIndex(const VariableIndex& other) :
|
||||
index_(indexUnpermuted_) {
|
||||
*this = other;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
VariableIndex& VariableIndex::operator=(const VariableIndex& rhs) {
|
||||
index_ = rhs.index_;
|
||||
nFactors_ = rhs.nFactors_;
|
||||
nEntries_ = rhs.nEntries_;
|
||||
return *this;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void VariableIndex::permute(const Permutation& permutation) {
|
||||
#ifndef NDEBUG
|
||||
// Assert that the permutation does not leave behind any non-empty variables,
|
||||
// otherwise the nFactors and nEntries counts would be incorrect.
|
||||
for(Index j=0; j<this->index_.size(); ++j)
|
||||
if(find(permutation.begin(), permutation.end(), j) == permutation.end())
|
||||
assert(this->operator[](j).empty());
|
||||
#endif
|
||||
index_.permute(permutation);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
bool VariableIndex::equals(const VariableIndex& other, double tol) const {
|
||||
if(this->nEntries_ == other.nEntries_ && this->nFactors_ == other.nFactors_) {
|
||||
|
|
@ -66,17 +41,13 @@ bool VariableIndex::equals(const VariableIndex& other, double tol) const {
|
|||
|
||||
/* ************************************************************************* */
|
||||
void VariableIndex::print(const string& str) const {
|
||||
cout << str << "\n";
|
||||
cout << str;
|
||||
cout << "nEntries = " << nEntries() << ", nFactors = " << nFactors() << "\n";
|
||||
Index var = 0;
|
||||
BOOST_FOREACH(const Factors& variable, index_.container()) {
|
||||
Permutation::const_iterator rvar = find(index_.permutation().begin(), index_.permutation().end(), var);
|
||||
assert(rvar != index_.permutation().end());
|
||||
cout << "var " << (rvar-index_.permutation().begin()) << ":";
|
||||
BOOST_FOREACH(const size_t factor, variable)
|
||||
for(Index var = 0; var < size(); ++var) {
|
||||
cout << "var " << var << ":";
|
||||
BOOST_FOREACH(const size_t factor, index_[var])
|
||||
cout << " " << factor;
|
||||
cout << "\n";
|
||||
++ var;
|
||||
}
|
||||
cout << flush;
|
||||
}
|
||||
|
|
@ -85,7 +56,7 @@ void VariableIndex::print(const string& str) const {
|
|||
void VariableIndex::outputMetisFormat(ostream& os) const {
|
||||
os << size() << " " << nFactors() << "\n";
|
||||
// run over variables, which will be hyper-edges.
|
||||
BOOST_FOREACH(const Factors& variable, index_.container()) {
|
||||
BOOST_FOREACH(const Factors& variable, index_) {
|
||||
// every variable is a hyper-edge covering its factors
|
||||
BOOST_FOREACH(const size_t factor, variable)
|
||||
os << (factor+1) << " "; // base 1
|
||||
|
|
@ -94,4 +65,15 @@ void VariableIndex::outputMetisFormat(ostream& os) const {
|
|||
os << flush;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void VariableIndex::permuteInPlace(const Permutation& permutation) {
|
||||
// Create new index and move references to data into it in permuted order
|
||||
vector<VariableIndex::Factors> newIndex(this->size());
|
||||
for(Index i = 0; i < newIndex.size(); ++i)
|
||||
newIndex[i].swap(this->index_[permutation[i]]);
|
||||
|
||||
// Move reference to entire index into the VariableIndex
|
||||
index_.swap(newIndex);
|
||||
}
|
||||
|
||||
}
|
||||
|
|
|
|||
|
|
@ -22,10 +22,12 @@
|
|||
#include <boost/foreach.hpp>
|
||||
|
||||
#include <gtsam/base/FastList.h>
|
||||
#include <gtsam/inference/Permutation.h>
|
||||
#include <gtsam/base/types.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
class Permutation;
|
||||
|
||||
/**
|
||||
* The VariableIndex class computes and stores the block column structure of a
|
||||
* factor graph. The factor graph stores a collection of factors, each of
|
||||
|
|
@ -44,8 +46,7 @@ public:
|
|||
typedef Factors::const_iterator Factor_const_iterator;
|
||||
|
||||
protected:
|
||||
std::vector<Factors> indexUnpermuted_;
|
||||
Permuted<std::vector<Factors> > index_; // Permuted view of indexUnpermuted.
|
||||
std::vector<Factors> index_;
|
||||
size_t nFactors_; // Number of factors in the original factor graph.
|
||||
size_t nEntries_; // Sum of involved variable counts of each factor.
|
||||
|
||||
|
|
@ -55,7 +56,7 @@ public:
|
|||
/// @{
|
||||
|
||||
/** Default constructor, creates an empty VariableIndex */
|
||||
VariableIndex() : index_(indexUnpermuted_), nFactors_(0), nEntries_(0) {}
|
||||
VariableIndex() : nFactors_(0), nEntries_(0) {}
|
||||
|
||||
/**
|
||||
* Create a VariableIndex that computes and stores the block column structure
|
||||
|
|
@ -70,16 +71,6 @@ public:
|
|||
*/
|
||||
template<class FactorGraph> VariableIndex(const FactorGraph& factorGraph);
|
||||
|
||||
/**
|
||||
* Copy constructor
|
||||
*/
|
||||
VariableIndex(const VariableIndex& other);
|
||||
|
||||
/**
|
||||
* Assignment operator
|
||||
*/
|
||||
VariableIndex& operator=(const VariableIndex& rhs);
|
||||
|
||||
/// @}
|
||||
/// @name Standard Interface
|
||||
/// @{
|
||||
|
|
@ -120,9 +111,6 @@ public:
|
|||
/// @name Advanced Interface
|
||||
/// @{
|
||||
|
||||
/** Access a list of factors by variable */
|
||||
Factors& operator[](Index variable) { checkVar(variable); return index_[variable]; }
|
||||
|
||||
/**
|
||||
* Augment the variable index with new factors. This can be used when
|
||||
* solving problems incrementally.
|
||||
|
|
@ -137,11 +125,8 @@ public:
|
|||
*/
|
||||
template<typename CONTAINER, class FactorGraph> void remove(const CONTAINER& indices, const FactorGraph& factors);
|
||||
|
||||
/**
|
||||
* Apply a variable permutation. Does not rearrange data, just permutes
|
||||
* future lookups by variable.
|
||||
*/
|
||||
void permute(const Permutation& permutation);
|
||||
/// Permute the variables in the VariableIndex according to the given permutation
|
||||
void permuteInPlace(const Permutation& permutation);
|
||||
|
||||
protected:
|
||||
Factor_iterator factorsBegin(Index variable) { checkVar(variable); return index_[variable].begin(); } ///<TODO: comment
|
||||
|
|
@ -150,13 +135,13 @@ protected:
|
|||
Factor_const_iterator factorsBegin(Index variable) const { checkVar(variable); return index_[variable].begin(); } ///<TODO: comment
|
||||
Factor_const_iterator factorsEnd(Index variable) const { checkVar(variable); return index_[variable].end(); } ///<TODO: comment
|
||||
|
||||
///TODO: comment
|
||||
VariableIndex(size_t nVars) : indexUnpermuted_(nVars), index_(indexUnpermuted_), nFactors_(0), nEntries_(0) {}
|
||||
/// Internal constructor to allocate a VariableIndex of the requested size
|
||||
VariableIndex(size_t nVars) : index_(nVars), nFactors_(0), nEntries_(0) {}
|
||||
|
||||
///TODO: comment
|
||||
/// Internal check of the validity of a variable
|
||||
void checkVar(Index variable) const { assert(variable < index_.size()); }
|
||||
|
||||
///TODO: comment
|
||||
/// Internal function to populate the variable index from a factor graph
|
||||
template<class FactorGraph> void fill(const FactorGraph& factorGraph);
|
||||
|
||||
/// @}
|
||||
|
|
@ -183,7 +168,7 @@ void VariableIndex::fill(const FactorGraph& factorGraph) {
|
|||
/* ************************************************************************* */
|
||||
template<class FactorGraph>
|
||||
VariableIndex::VariableIndex(const FactorGraph& factorGraph) :
|
||||
index_(indexUnpermuted_), nFactors_(0), nEntries_(0) {
|
||||
nFactors_(0), nEntries_(0) {
|
||||
|
||||
// If the factor graph is empty, return an empty index because inside this
|
||||
// if block we assume at least one factor.
|
||||
|
|
@ -200,8 +185,7 @@ VariableIndex::VariableIndex(const FactorGraph& factorGraph) :
|
|||
}
|
||||
|
||||
// Allocate array
|
||||
index_.container().resize(maxVar+1);
|
||||
index_.permutation() = Permutation::Identity(maxVar+1);
|
||||
index_.resize(maxVar+1);
|
||||
|
||||
fill(factorGraph);
|
||||
}
|
||||
|
|
@ -210,7 +194,7 @@ VariableIndex::VariableIndex(const FactorGraph& factorGraph) :
|
|||
/* ************************************************************************* */
|
||||
template<class FactorGraph>
|
||||
VariableIndex::VariableIndex(const FactorGraph& factorGraph, Index nVariables) :
|
||||
indexUnpermuted_(nVariables), index_(indexUnpermuted_), nFactors_(0), nEntries_(0) {
|
||||
index_(nVariables), nFactors_(0), nEntries_(0) {
|
||||
fill(factorGraph);
|
||||
}
|
||||
|
||||
|
|
@ -233,10 +217,7 @@ void VariableIndex::augment(const FactorGraph& factors) {
|
|||
|
||||
// Allocate index
|
||||
Index originalSize = index_.size();
|
||||
index_.container().resize(std::max(index_.size(), maxVar+1));
|
||||
index_.permutation().resize(index_.container().size());
|
||||
for(Index var=originalSize; var<index_.permutation().size(); ++var)
|
||||
index_.permutation()[var] = var;
|
||||
index_.resize(std::max(index_.size(), maxVar+1));
|
||||
|
||||
// Augment index mapping from variable id to factor index
|
||||
size_t orignFactors = nFactors_;
|
||||
|
|
|
|||
|
|
@ -183,33 +183,17 @@ JacobianFactor::shared_ptr GaussianConditional::toFactor() const {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class VALUES>
|
||||
inline static void doSolveInPlace(const GaussianConditional& conditional, VALUES& x) {
|
||||
|
||||
// Helper function to solve-in-place on a VectorValues or Permuted<VectorValues>,
|
||||
// called by GaussianConditional::solveInPlace(VectorValues&) and by
|
||||
// GaussianConditional::solveInPlace(Permuted<VectorValues>&).
|
||||
|
||||
void GaussianConditional::solveInPlace(VectorValues& x) const {
|
||||
static const bool debug = false;
|
||||
if(debug) conditional.print("Solving conditional in place");
|
||||
Vector xS = internal::extractVectorValuesSlices(x, conditional.beginParents(), conditional.endParents());
|
||||
xS = conditional.get_d() - conditional.get_S() * xS;
|
||||
Vector soln = conditional.get_R().triangularView<Eigen::Upper>().solve(xS);
|
||||
if(debug) this->print("Solving conditional in place");
|
||||
Vector xS = internal::extractVectorValuesSlices(x, this->beginParents(), this->endParents());
|
||||
xS = this->get_d() - this->get_S() * xS;
|
||||
Vector soln = this->get_R().triangularView<Eigen::Upper>().solve(xS);
|
||||
if(debug) {
|
||||
gtsam::print(Matrix(conditional.get_R()), "Calling backSubstituteUpper on ");
|
||||
gtsam::print(Matrix(this->get_R()), "Calling backSubstituteUpper on ");
|
||||
gtsam::print(soln, "full back-substitution solution: ");
|
||||
}
|
||||
internal::writeVectorValuesSlices(soln, x, conditional.beginFrontals(), conditional.endFrontals());
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void GaussianConditional::solveInPlace(VectorValues& x) const {
|
||||
doSolveInPlace(*this, x); // Call helper version above
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void GaussianConditional::solveInPlace(Permuted<VectorValues>& x) const {
|
||||
doSolveInPlace(*this, x); // Call helper version above
|
||||
internal::writeVectorValuesSlices(soln, x, this->beginFrontals(), this->endFrontals());
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -196,23 +196,6 @@ public:
|
|||
*/
|
||||
void solveInPlace(VectorValues& x) const;
|
||||
|
||||
/**
|
||||
* Solves a conditional Gaussian and writes the solution into the entries of
|
||||
* \c x for each frontal variable of the conditional (version for permuted
|
||||
* VectorValues). The parents are assumed to have already been solved in
|
||||
* and their values are read from \c x. This function works for multiple
|
||||
* frontal variables.
|
||||
*
|
||||
* Given the Gaussian conditional with log likelihood \f$ |R x_f - (d - S x_s)|^2,
|
||||
* where \f$ f \f$ are the frontal variables and \f$ s \f$ are the separator
|
||||
* variables of this conditional, this solve function computes
|
||||
* \f$ x_f = R^{-1} (d - S x_s) \f$ using back-substitution.
|
||||
*
|
||||
* @param x VectorValues structure with solved parents \f$ x_s \f$, and into which the
|
||||
* solution \f$ x_f \f$ will be written.
|
||||
*/
|
||||
void solveInPlace(Permuted<VectorValues>& x) const;
|
||||
|
||||
// functions for transpose backsubstitution
|
||||
|
||||
/**
|
||||
|
|
|
|||
|
|
@ -17,10 +17,12 @@
|
|||
*/
|
||||
|
||||
#include <gtsam/base/FastVector.h>
|
||||
#include <gtsam/inference/Permutation.h>
|
||||
#include <gtsam/linear/VectorValues.h>
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/* ************************************************************************* */
|
||||
VectorValues::VectorValues(const VectorValues& other) {
|
||||
|
|
@ -166,20 +168,24 @@ void VectorValues::operator+=(const VectorValues& c) {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
VectorValues& VectorValues::operator=(const Permuted<VectorValues>& rhs) {
|
||||
if(this->size() != rhs.size())
|
||||
throw std::invalid_argument("VectorValues assignment from Permuted<VectorValues> requires pre-allocation, see documentation.");
|
||||
for(size_t j=0; j<this->size(); ++j) {
|
||||
if(exists(j)) {
|
||||
SubVector& l(this->at(j));
|
||||
const SubVector& r(rhs[j]);
|
||||
if(l.rows() != r.rows())
|
||||
throw std::invalid_argument("VectorValues assignment from Permuted<VectorValues> requires pre-allocation, see documentation.");
|
||||
l = r;
|
||||
} else {
|
||||
if(rhs.container().exists(rhs.permutation()[j]))
|
||||
throw std::invalid_argument("VectorValues assignment from Permuted<VectorValues> requires pre-allocation, see documentation.");
|
||||
VectorValues VectorValues::permute(const Permutation& permutation) const {
|
||||
// Create result and allocate space
|
||||
VectorValues lhs;
|
||||
lhs.values_.resize(this->dim());
|
||||
lhs.maps_.reserve(this->size());
|
||||
|
||||
// Copy values from this VectorValues to the permuted VectorValues
|
||||
size_t lhsPos = 0;
|
||||
for(size_t i = 0; i < this->size(); ++i) {
|
||||
// Map the next LHS subvector to the next slice of the LHS vector
|
||||
lhs.maps_.push_back(SubVector(lhs.values_, lhsPos, this->at(permutation[i]).size()));
|
||||
// Copy the data from the RHS subvector to the LHS subvector
|
||||
lhs.maps_[i] = this->at(permutation[i]);
|
||||
// Increment lhs position
|
||||
lhsPos += lhs.maps_[i].size();
|
||||
}
|
||||
}
|
||||
return *this;
|
||||
|
||||
return lhs;
|
||||
}
|
||||
|
||||
}
|
||||
|
|
@ -19,7 +19,6 @@
|
|||
|
||||
#include <gtsam/base/Vector.h>
|
||||
#include <gtsam/base/types.h>
|
||||
#include <gtsam/inference/Permutation.h>
|
||||
|
||||
#include <boost/lexical_cast.hpp>
|
||||
#include <boost/foreach.hpp>
|
||||
|
|
@ -29,6 +28,9 @@
|
|||
|
||||
namespace gtsam {
|
||||
|
||||
// Forward declarations
|
||||
class Permutation;
|
||||
|
||||
/**
|
||||
* This class represents a collection of vector-valued variables associated
|
||||
* each with a unique integer index. It is typically used to store the variables
|
||||
|
|
@ -288,10 +290,11 @@ namespace gtsam {
|
|||
*/
|
||||
void operator+=(const VectorValues& c);
|
||||
|
||||
/** Assignment operator from Permuted<VectorValues>, requires the dimensions
|
||||
* of the assignee to already be properly pre-allocated.
|
||||
/**
|
||||
* Permute the entries of this VectorValues, returns a new VectorValues as
|
||||
* the result.
|
||||
*/
|
||||
VectorValues& operator=(const Permuted<VectorValues>& rhs);
|
||||
VectorValues permute(const Permutation& permutation) const;
|
||||
|
||||
/// @}
|
||||
|
||||
|
|
|
|||
|
|
@ -267,55 +267,6 @@ TEST( GaussianConditional, solve_multifrontal )
|
|||
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( GaussianConditional, solve_multifrontal_permuted )
|
||||
{
|
||||
// create full system, 3 variables, 2 frontals, all 2 dim
|
||||
Matrix full_matrix = Matrix_(4, 7,
|
||||
1.0, 0.0, 2.0, 0.0, 3.0, 0.0, 0.1,
|
||||
0.0, 1.0, 0.0, 2.0, 0.0, 3.0, 0.2,
|
||||
0.0, 0.0, 3.0, 0.0, 4.0, 0.0, 0.3,
|
||||
0.0, 0.0, 0.0, 3.0, 0.0, 4.0, 0.4);
|
||||
|
||||
// 3 variables, all dim=2
|
||||
vector<size_t> dims; dims += 2, 2, 2, 1;
|
||||
GaussianConditional::rsd_type matrices(full_matrix, dims.begin(), dims.end());
|
||||
Vector sigmas = ones(4);
|
||||
vector<size_t> cgdims; cgdims += _x_, _x1_, _l1_;
|
||||
GaussianConditional cg(cgdims.begin(), cgdims.end(), 2, matrices, sigmas);
|
||||
|
||||
EXPECT(assert_equal(Vector_(4, 0.1, 0.2, 0.3, 0.4), cg.get_d()));
|
||||
|
||||
// partial solution
|
||||
Vector sl1 = Vector_(2, 9.0, 10.0);
|
||||
|
||||
// elimination order; _x_, _x1_, _l1_
|
||||
VectorValues actualUnpermuted(vector<size_t>(3, 2));
|
||||
Permutation permutation(3);
|
||||
permutation[0] = 2;
|
||||
permutation[1] = 0;
|
||||
permutation[2] = 1;
|
||||
Permuted<VectorValues> actual(permutation, actualUnpermuted);
|
||||
actual[_x_] = Vector_(2, 0.1, 0.2); // rhs
|
||||
actual[_x1_] = Vector_(2, 0.3, 0.4); // rhs
|
||||
actual[_l1_] = sl1; // parent
|
||||
|
||||
VectorValues expectedUnpermuted(vector<size_t>(3, 2));
|
||||
Permuted<VectorValues> expected(permutation, expectedUnpermuted);
|
||||
expected[_x_] = Vector_(2, -3.1,-3.4);
|
||||
expected[_x1_] = Vector_(2, -11.9,-13.2);
|
||||
expected[_l1_] = sl1;
|
||||
|
||||
// verify indices/size
|
||||
EXPECT_LONGS_EQUAL(3, cg.size());
|
||||
EXPECT_LONGS_EQUAL(4, cg.dim());
|
||||
|
||||
// solve and verify
|
||||
cg.solveInPlace(actual);
|
||||
EXPECT(assert_equal(expected.container(), actual.container(), tol));
|
||||
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( GaussianConditional, solveTranspose ) {
|
||||
static const Index _y_=1;
|
||||
|
|
|
|||
|
|
@ -421,52 +421,31 @@ TEST(VectorValues, hasSameStructure) {
|
|||
EXPECT(!v1.hasSameStructure(VectorValues()));
|
||||
}
|
||||
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(VectorValues, permuted_combined) {
|
||||
Vector v1 = Vector_(3, 1.0,2.0,3.0);
|
||||
Vector v2 = Vector_(2, 4.0,5.0);
|
||||
Vector v3 = Vector_(4, 6.0,7.0,8.0,9.0);
|
||||
TEST(VectorValues, permute) {
|
||||
|
||||
vector<size_t> dims(3); dims[0]=3; dims[1]=2; dims[2]=4;
|
||||
VectorValues combined(dims);
|
||||
combined[0] = v1;
|
||||
combined[1] = v2;
|
||||
combined[2] = v3;
|
||||
VectorValues original;
|
||||
original.insert(0, Vector_(1, 1.0));
|
||||
original.insert(1, Vector_(2, 2.0, 3.0));
|
||||
original.insert(2, Vector_(2, 4.0, 5.0));
|
||||
original.insert(3, Vector_(2, 6.0, 7.0));
|
||||
|
||||
Permutation perm1(3);
|
||||
perm1[0] = 1;
|
||||
perm1[1] = 2;
|
||||
perm1[2] = 0;
|
||||
VectorValues expected;
|
||||
expected.insert(0, Vector_(2, 4.0, 5.0)); // from 2
|
||||
expected.insert(1, Vector_(1, 1.0)); // from 0
|
||||
expected.insert(2, Vector_(2, 6.0, 7.0)); // from 3
|
||||
expected.insert(3, Vector_(2, 2.0, 3.0)); // from 1
|
||||
|
||||
Permutation perm2(3);
|
||||
perm2[0] = 1;
|
||||
perm2[1] = 2;
|
||||
perm2[2] = 0;
|
||||
Permutation permutation(4);
|
||||
permutation[0] = 2;
|
||||
permutation[1] = 0;
|
||||
permutation[2] = 3;
|
||||
permutation[3] = 1;
|
||||
|
||||
Permuted<VectorValues> permuted1(combined);
|
||||
CHECK(assert_equal(v1, permuted1[0]))
|
||||
CHECK(assert_equal(v2, permuted1[1]))
|
||||
CHECK(assert_equal(v3, permuted1[2]))
|
||||
VectorValues actual = original.permute(permutation);
|
||||
|
||||
permuted1.permute(perm1);
|
||||
CHECK(assert_equal(v1, permuted1[2]))
|
||||
CHECK(assert_equal(v2, permuted1[0]))
|
||||
CHECK(assert_equal(v3, permuted1[1]))
|
||||
|
||||
permuted1.permute(perm2);
|
||||
CHECK(assert_equal(v1, permuted1[1]))
|
||||
CHECK(assert_equal(v2, permuted1[2]))
|
||||
CHECK(assert_equal(v3, permuted1[0]))
|
||||
|
||||
Permuted<VectorValues> permuted2(perm1, combined);
|
||||
CHECK(assert_equal(v1, permuted2[2]))
|
||||
CHECK(assert_equal(v2, permuted2[0]))
|
||||
CHECK(assert_equal(v3, permuted2[1]))
|
||||
|
||||
permuted2.permute(perm2);
|
||||
CHECK(assert_equal(v1, permuted2[1]))
|
||||
CHECK(assert_equal(v2, permuted2[2]))
|
||||
CHECK(assert_equal(v3, permuted2[0]))
|
||||
EXPECT(assert_equal(expected, actual));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -29,8 +29,8 @@ namespace gtsam {
|
|||
|
||||
/* ************************************************************************* */
|
||||
void ISAM2::Impl::AddVariables(
|
||||
const Values& newTheta, Values& theta, Permuted<VectorValues>& delta,
|
||||
Permuted<VectorValues>& deltaNewton, Permuted<VectorValues>& deltaGradSearch, vector<bool>& replacedKeys,
|
||||
const Values& newTheta, Values& theta, VectorValues& delta,
|
||||
VectorValues& deltaNewton, VectorValues& deltaGradSearch, vector<bool>& replacedKeys,
|
||||
Ordering& ordering, Base::Nodes& nodes, const KeyFormatter& keyFormatter) {
|
||||
const bool debug = ISDEBUG("ISAM2 AddVariables");
|
||||
|
||||
|
|
@ -40,28 +40,21 @@ void ISAM2::Impl::AddVariables(
|
|||
std::vector<Index> dims(newTheta.dims(*newTheta.orderingArbitrary()));
|
||||
if(debug) cout << "New variables have total dimensionality " << accumulate(dims.begin(), dims.end(), 0) << endl;
|
||||
const size_t newDim = accumulate(dims.begin(), dims.end(), 0);
|
||||
const size_t originalDim = delta->dim();
|
||||
const size_t originalnVars = delta->size();
|
||||
delta.container().append(dims);
|
||||
delta.container().vector().segment(originalDim, newDim).operator=(Vector::Zero(newDim));
|
||||
delta.permutation().resize(originalnVars + newTheta.size());
|
||||
deltaNewton.container().append(dims);
|
||||
deltaNewton.container().vector().segment(originalDim, newDim).operator=(Vector::Zero(newDim));
|
||||
deltaNewton.permutation().resize(originalnVars + newTheta.size());
|
||||
deltaGradSearch.container().append(dims);
|
||||
deltaGradSearch.container().vector().segment(originalDim, newDim).operator=(Vector::Zero(newDim));
|
||||
deltaGradSearch.permutation().resize(originalnVars + newTheta.size());
|
||||
const size_t originalDim = delta.dim();
|
||||
const size_t originalnVars = delta.size();
|
||||
delta.append(dims);
|
||||
delta.vector().segment(originalDim, newDim).operator=(Vector::Zero(newDim));
|
||||
deltaNewton.append(dims);
|
||||
deltaNewton.vector().segment(originalDim, newDim).operator=(Vector::Zero(newDim));
|
||||
deltaGradSearch.append(dims);
|
||||
deltaGradSearch.vector().segment(originalDim, newDim).operator=(Vector::Zero(newDim));
|
||||
{
|
||||
Index nextVar = originalnVars;
|
||||
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, newTheta) {
|
||||
delta.permutation()[nextVar] = nextVar;
|
||||
deltaNewton.permutation()[nextVar] = nextVar;
|
||||
deltaGradSearch.permutation()[nextVar] = nextVar;
|
||||
ordering.insert(key_value.key, nextVar);
|
||||
if(debug) cout << "Adding variable " << keyFormatter(key_value.key) << " with order " << nextVar << endl;
|
||||
++ nextVar;
|
||||
}
|
||||
assert(delta.permutation().size() == delta.container().size());
|
||||
assert(ordering.nVars() == delta.size());
|
||||
assert(ordering.size() == delta.size());
|
||||
}
|
||||
|
|
@ -82,7 +75,7 @@ FastSet<Index> ISAM2::Impl::IndicesFromFactors(const Ordering& ordering, const N
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
FastSet<Index> ISAM2::Impl::CheckRelinearizationFull(const Permuted<VectorValues>& delta, const Ordering& ordering,
|
||||
FastSet<Index> ISAM2::Impl::CheckRelinearizationFull(const VectorValues& delta, const Ordering& ordering,
|
||||
const ISAM2Params::RelinearizationThreshold& relinearizeThreshold, const KeyFormatter& keyFormatter) {
|
||||
FastSet<Index> relinKeys;
|
||||
|
||||
|
|
@ -110,7 +103,7 @@ FastSet<Index> ISAM2::Impl::CheckRelinearizationFull(const Permuted<VectorValues
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void CheckRelinearizationRecursiveDouble(FastSet<Index>& relinKeys, double threshold, const Permuted<VectorValues>& delta, const ISAM2Clique::shared_ptr& clique) {
|
||||
void CheckRelinearizationRecursiveDouble(FastSet<Index>& relinKeys, double threshold, const VectorValues& delta, const ISAM2Clique::shared_ptr& clique) {
|
||||
|
||||
// Check the current clique for relinearization
|
||||
bool relinearize = false;
|
||||
|
|
@ -131,7 +124,7 @@ void CheckRelinearizationRecursiveDouble(FastSet<Index>& relinKeys, double thres
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void CheckRelinearizationRecursiveMap(FastSet<Index>& relinKeys, const FastMap<char,Vector>& thresholds, const Permuted<VectorValues>& delta, const Ordering::InvertedMap& decoder, const ISAM2Clique::shared_ptr& clique) {
|
||||
void CheckRelinearizationRecursiveMap(FastSet<Index>& relinKeys, const FastMap<char,Vector>& thresholds, const VectorValues& delta, const Ordering::InvertedMap& decoder, const ISAM2Clique::shared_ptr& clique) {
|
||||
|
||||
// Check the current clique for relinearization
|
||||
bool relinearize = false;
|
||||
|
|
@ -163,7 +156,7 @@ void CheckRelinearizationRecursiveMap(FastSet<Index>& relinKeys, const FastMap<c
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
FastSet<Index> ISAM2::Impl::CheckRelinearizationPartial(const ISAM2Clique::shared_ptr& root, const Permuted<VectorValues>& delta, const Ordering& ordering,
|
||||
FastSet<Index> ISAM2::Impl::CheckRelinearizationPartial(const ISAM2Clique::shared_ptr& root, const VectorValues& delta, const Ordering& ordering,
|
||||
const ISAM2Params::RelinearizationThreshold& relinearizeThreshold, const KeyFormatter& keyFormatter) {
|
||||
|
||||
FastSet<Index> relinKeys;
|
||||
|
|
@ -201,8 +194,8 @@ void ISAM2::Impl::FindAll(ISAM2Clique::shared_ptr clique, FastSet<Index>& keys,
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void ISAM2::Impl::ExpmapMasked(Values& values, const Permuted<VectorValues>& delta, const Ordering& ordering,
|
||||
const vector<bool>& mask, boost::optional<Permuted<VectorValues>&> invalidateIfDebug, const KeyFormatter& keyFormatter) {
|
||||
void ISAM2::Impl::ExpmapMasked(Values& values, const VectorValues& delta, const Ordering& ordering,
|
||||
const vector<bool>& mask, boost::optional<VectorValues&> invalidateIfDebug, const KeyFormatter& keyFormatter) {
|
||||
// If debugging, invalidate if requested, otherwise do not invalidate.
|
||||
// Invalidating means setting expmapped entries to Inf, to trigger assertions
|
||||
// if we try to re-use them.
|
||||
|
|
@ -304,7 +297,7 @@ ISAM2::Impl::PartialSolve(GaussianFactorGraph& factors,
|
|||
toc(4,"ccolamd permutations");
|
||||
|
||||
tic(5,"permute affected variable index");
|
||||
affectedFactorsIndex.permute(*affectedColamd);
|
||||
affectedFactorsIndex.permuteInPlace(*affectedColamd);
|
||||
toc(5,"permute affected variable index");
|
||||
|
||||
tic(6,"permute affected factors");
|
||||
|
|
@ -354,25 +347,13 @@ inline static void optimizeInPlace(const boost::shared_ptr<ISAM2Clique>& clique,
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
size_t ISAM2::Impl::UpdateDelta(const boost::shared_ptr<ISAM2Clique>& root, std::vector<bool>& replacedKeys, Permuted<VectorValues>& delta, double wildfireThreshold) {
|
||||
size_t ISAM2::Impl::UpdateDelta(const boost::shared_ptr<ISAM2Clique>& root, std::vector<bool>& replacedKeys, VectorValues& delta, double wildfireThreshold) {
|
||||
|
||||
size_t lastBacksubVariableCount;
|
||||
|
||||
if (wildfireThreshold <= 0.0) {
|
||||
// Threshold is zero or less, so do a full recalculation
|
||||
// Collect dimensions and allocate new VectorValues
|
||||
vector<size_t> dims(delta.size());
|
||||
for(size_t j=0; j<delta.size(); ++j)
|
||||
dims[j] = delta->dim(j);
|
||||
VectorValues newDelta(dims);
|
||||
|
||||
// Optimize full solution delta
|
||||
internal::optimizeInPlace(root, newDelta);
|
||||
|
||||
// Copy solution into delta
|
||||
delta.permutation() = Permutation::Identity(delta.size());
|
||||
delta.container() = newDelta;
|
||||
|
||||
internal::optimizeInPlace(root, delta);
|
||||
lastBacksubVariableCount = delta.size();
|
||||
|
||||
} else {
|
||||
|
|
@ -380,8 +361,8 @@ size_t ISAM2::Impl::UpdateDelta(const boost::shared_ptr<ISAM2Clique>& root, std:
|
|||
lastBacksubVariableCount = optimizeWildfire(root, wildfireThreshold, replacedKeys, delta); // modifies delta_
|
||||
|
||||
#ifndef NDEBUG
|
||||
for(size_t j=0; j<delta.container().size(); ++j)
|
||||
assert(delta.container()[j].unaryExpr(ptr_fun(isfinite<double>)).all());
|
||||
for(size_t j=0; j<delta.size(); ++j)
|
||||
assert(delta[j].unaryExpr(ptr_fun(isfinite<double>)).all());
|
||||
#endif
|
||||
}
|
||||
|
||||
|
|
@ -394,7 +375,7 @@ size_t ISAM2::Impl::UpdateDelta(const boost::shared_ptr<ISAM2Clique>& root, std:
|
|||
/* ************************************************************************* */
|
||||
namespace internal {
|
||||
void updateDoglegDeltas(const boost::shared_ptr<ISAM2Clique>& clique, std::vector<bool>& replacedKeys,
|
||||
const VectorValues& grad, Permuted<VectorValues>& deltaNewton, Permuted<VectorValues>& RgProd, size_t& varsUpdated) {
|
||||
const VectorValues& grad, VectorValues& deltaNewton, VectorValues& RgProd, size_t& varsUpdated) {
|
||||
|
||||
// Check if any frontal or separator keys were recalculated, if so, we need
|
||||
// update deltas and recurse to children, but if not, we do not need to
|
||||
|
|
@ -433,7 +414,7 @@ void updateDoglegDeltas(const boost::shared_ptr<ISAM2Clique>& clique, std::vecto
|
|||
|
||||
/* ************************************************************************* */
|
||||
size_t ISAM2::Impl::UpdateDoglegDeltas(const ISAM2& isam, double wildfireThreshold, std::vector<bool>& replacedKeys,
|
||||
Permuted<VectorValues>& deltaNewton, Permuted<VectorValues>& RgProd) {
|
||||
VectorValues& deltaNewton, VectorValues& RgProd) {
|
||||
|
||||
// Get gradient
|
||||
VectorValues grad = *allocateVectorValues(isam);
|
||||
|
|
|
|||
|
|
@ -46,8 +46,8 @@ struct ISAM2::Impl {
|
|||
* @param nodes Current BayesTree::Nodes index to be augmented with slots for new variables
|
||||
* @param keyFormatter Formatter for printing nonlinear keys during debugging
|
||||
*/
|
||||
static void AddVariables(const Values& newTheta, Values& theta, Permuted<VectorValues>& delta,
|
||||
Permuted<VectorValues>& deltaNewton, Permuted<VectorValues>& deltaGradSearch, std::vector<bool>& replacedKeys,
|
||||
static void AddVariables(const Values& newTheta, Values& theta, VectorValues& delta,
|
||||
VectorValues& deltaNewton, VectorValues& deltaGradSearch, std::vector<bool>& replacedKeys,
|
||||
Ordering& ordering, Base::Nodes& nodes, const KeyFormatter& keyFormatter = DefaultKeyFormatter);
|
||||
|
||||
/**
|
||||
|
|
@ -68,7 +68,7 @@ struct ISAM2::Impl {
|
|||
* @return The set of variable indices in delta whose magnitude is greater than or
|
||||
* equal to relinearizeThreshold
|
||||
*/
|
||||
static FastSet<Index> CheckRelinearizationFull(const Permuted<VectorValues>& delta, const Ordering& ordering,
|
||||
static FastSet<Index> CheckRelinearizationFull(const VectorValues& delta, const Ordering& ordering,
|
||||
const ISAM2Params::RelinearizationThreshold& relinearizeThreshold, const KeyFormatter& keyFormatter = DefaultKeyFormatter);
|
||||
|
||||
/**
|
||||
|
|
@ -82,7 +82,7 @@ struct ISAM2::Impl {
|
|||
* @return The set of variable indices in delta whose magnitude is greater than or
|
||||
* equal to relinearizeThreshold
|
||||
*/
|
||||
static FastSet<Index> CheckRelinearizationPartial(const ISAM2Clique::shared_ptr& root, const Permuted<VectorValues>& delta, const Ordering& ordering,
|
||||
static FastSet<Index> CheckRelinearizationPartial(const ISAM2Clique::shared_ptr& root, const VectorValues& delta, const Ordering& ordering,
|
||||
const ISAM2Params::RelinearizationThreshold& relinearizeThreshold, const KeyFormatter& keyFormatter = DefaultKeyFormatter);
|
||||
|
||||
/**
|
||||
|
|
@ -115,9 +115,9 @@ struct ISAM2::Impl {
|
|||
* recalculate its delta.
|
||||
* @param keyFormatter Formatter for printing nonlinear keys during debugging
|
||||
*/
|
||||
static void ExpmapMasked(Values& values, const Permuted<VectorValues>& delta,
|
||||
static void ExpmapMasked(Values& values, const VectorValues& delta,
|
||||
const Ordering& ordering, const std::vector<bool>& mask,
|
||||
boost::optional<Permuted<VectorValues>&> invalidateIfDebug = boost::optional<Permuted<VectorValues>&>(),
|
||||
boost::optional<VectorValues&> invalidateIfDebug = boost::none,
|
||||
const KeyFormatter& keyFormatter = DefaultKeyFormatter);
|
||||
|
||||
/**
|
||||
|
|
@ -137,10 +137,10 @@ struct ISAM2::Impl {
|
|||
static PartialSolveResult PartialSolve(GaussianFactorGraph& factors, const FastSet<Index>& keys,
|
||||
const ReorderingMode& reorderingMode, bool useQR);
|
||||
|
||||
static size_t UpdateDelta(const boost::shared_ptr<ISAM2Clique>& root, std::vector<bool>& replacedKeys, Permuted<VectorValues>& delta, double wildfireThreshold);
|
||||
static size_t UpdateDelta(const boost::shared_ptr<ISAM2Clique>& root, std::vector<bool>& replacedKeys, VectorValues& delta, double wildfireThreshold);
|
||||
|
||||
static size_t UpdateDoglegDeltas(const ISAM2& isam, double wildfireThreshold, std::vector<bool>& replacedKeys,
|
||||
Permuted<VectorValues>& deltaNewton, Permuted<VectorValues>& RgProd);
|
||||
VectorValues& deltaNewton, VectorValues& RgProd);
|
||||
|
||||
};
|
||||
|
||||
|
|
|
|||
|
|
@ -37,7 +37,7 @@ VALUE ISAM2::calculateEstimate(Key key) const {
|
|||
namespace internal {
|
||||
template<class CLIQUE>
|
||||
void optimizeWildfire(const boost::shared_ptr<CLIQUE>& clique, double threshold,
|
||||
std::vector<bool>& changed, const std::vector<bool>& replaced, Permuted<VectorValues>& delta, int& count) {
|
||||
std::vector<bool>& changed, const std::vector<bool>& replaced, VectorValues& delta, int& count) {
|
||||
// if none of the variables in this clique (frontal and separator!) changed
|
||||
// significantly, then by the running intersection property, none of the
|
||||
// cliques in the children need to be processed
|
||||
|
|
@ -114,7 +114,7 @@ void optimizeWildfire(const boost::shared_ptr<CLIQUE>& clique, double threshold,
|
|||
|
||||
/* ************************************************************************* */
|
||||
template<class CLIQUE>
|
||||
int optimizeWildfire(const boost::shared_ptr<CLIQUE>& root, double threshold, const std::vector<bool>& keys, Permuted<VectorValues>& delta) {
|
||||
int optimizeWildfire(const boost::shared_ptr<CLIQUE>& root, double threshold, const std::vector<bool>& keys, VectorValues& delta) {
|
||||
std::vector<bool> changed(keys.size(), false);
|
||||
int count = 0;
|
||||
// starting from the root, call optimize on each conditional
|
||||
|
|
|
|||
|
|
@ -41,7 +41,6 @@ static const double batchThreshold = 0.65;
|
|||
|
||||
/* ************************************************************************* */
|
||||
ISAM2::ISAM2(const ISAM2Params& params):
|
||||
delta_(deltaUnpermuted_), deltaNewton_(deltaNewtonUnpermuted_), RgProd_(RgProdUnpermuted_),
|
||||
deltaDoglegUptodate_(true), deltaUptodate_(true), params_(params) {
|
||||
if(params_.optimizationParams.type() == typeid(ISAM2DoglegParams))
|
||||
doglegDelta_ = boost::get<ISAM2DoglegParams>(params_.optimizationParams).initialDelta;
|
||||
|
|
@ -49,15 +48,13 @@ ISAM2::ISAM2(const ISAM2Params& params):
|
|||
|
||||
/* ************************************************************************* */
|
||||
ISAM2::ISAM2():
|
||||
delta_(deltaUnpermuted_), deltaNewton_(deltaNewtonUnpermuted_), RgProd_(RgProdUnpermuted_),
|
||||
deltaDoglegUptodate_(true), deltaUptodate_(true) {
|
||||
if(params_.optimizationParams.type() == typeid(ISAM2DoglegParams))
|
||||
doglegDelta_ = boost::get<ISAM2DoglegParams>(params_.optimizationParams).initialDelta;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
ISAM2::ISAM2(const ISAM2& other):
|
||||
delta_(deltaUnpermuted_), deltaNewton_(deltaNewtonUnpermuted_), RgProd_(RgProdUnpermuted_) {
|
||||
ISAM2::ISAM2(const ISAM2& other) {
|
||||
*this = other;
|
||||
}
|
||||
|
||||
|
|
@ -308,12 +305,12 @@ boost::shared_ptr<FastSet<Index> > ISAM2::recalculate(
|
|||
|
||||
// Reorder
|
||||
tic(2,"permute global variable index");
|
||||
variableIndex_.permute(*colamd);
|
||||
variableIndex_.permuteInPlace(*colamd);
|
||||
toc(2,"permute global variable index");
|
||||
tic(3,"permute delta");
|
||||
delta_.permute(*colamd);
|
||||
deltaNewton_.permute(*colamd);
|
||||
RgProd_.permute(*colamd);
|
||||
delta_ = delta_.permute(*colamd);
|
||||
deltaNewton_ = deltaNewton_.permute(*colamd);
|
||||
RgProd_ = RgProd_.permute(*colamd);
|
||||
toc(3,"permute delta");
|
||||
tic(4,"permute ordering");
|
||||
ordering_.permuteWithInverse(*colamdInverse);
|
||||
|
|
@ -429,12 +426,12 @@ boost::shared_ptr<FastSet<Index> > ISAM2::recalculate(
|
|||
// re-eliminate. The reordered variables are also mentioned in the
|
||||
// orphans and the leftover cached factors.
|
||||
tic(3,"permute global variable index");
|
||||
variableIndex_.permute(partialSolveResult.fullReordering);
|
||||
variableIndex_.permuteInPlace(partialSolveResult.fullReordering);
|
||||
toc(3,"permute global variable index");
|
||||
tic(4,"permute delta");
|
||||
delta_.permute(partialSolveResult.fullReordering);
|
||||
deltaNewton_.permute(partialSolveResult.fullReordering);
|
||||
RgProd_.permute(partialSolveResult.fullReordering);
|
||||
delta_ = delta_.permute(partialSolveResult.fullReordering);
|
||||
deltaNewton_ = deltaNewton_.permute(partialSolveResult.fullReordering);
|
||||
RgProd_ = RgProd_.permute(partialSolveResult.fullReordering);
|
||||
toc(4,"permute delta");
|
||||
tic(5,"permute ordering");
|
||||
ordering_.permuteWithInverse(partialSolveResult.fullReorderingInverse);
|
||||
|
|
@ -723,8 +720,7 @@ void ISAM2::updateDelta(bool forceFullSolve) const {
|
|||
tic(2, "Copy dx_d");
|
||||
// Update Delta and linear step
|
||||
doglegDelta_ = doglegResult.Delta;
|
||||
delta_.permutation() = Permutation::Identity(delta_.size()); // Dogleg solves for the full delta so there is no permutation
|
||||
delta_.container() = doglegResult.dx_d; // Copy the VectorValues containing with the linear solution
|
||||
delta_ = doglegResult.dx_d; // Copy the VectorValues containing with the linear solution
|
||||
toc(2, "Copy dx_d");
|
||||
}
|
||||
|
||||
|
|
@ -739,7 +735,7 @@ Values ISAM2::calculateEstimate() const {
|
|||
Values ret(theta_);
|
||||
toc(1, "Copy Values");
|
||||
tic(2, "getDelta");
|
||||
const Permuted<VectorValues>& delta(getDelta());
|
||||
const VectorValues& delta(getDelta());
|
||||
toc(2, "getDelta");
|
||||
tic(3, "Expmap");
|
||||
vector<bool> mask(ordering_.nVars(), true);
|
||||
|
|
@ -756,7 +752,7 @@ Values ISAM2::calculateBestEstimate() const {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
const Permuted<VectorValues>& ISAM2::getDelta() const {
|
||||
const VectorValues& ISAM2::getDelta() const {
|
||||
if(!deltaUptodate_)
|
||||
updateDelta();
|
||||
return delta_;
|
||||
|
|
@ -829,7 +825,7 @@ void optimizeGradientSearchInPlace(const ISAM2& isam, VectorValues& grad) {
|
|||
|
||||
tic(3, "Compute minimizing step size");
|
||||
// Compute minimizing step size
|
||||
double RgNormSq = isam.RgProd_.container().vector().squaredNorm();
|
||||
double RgNormSq = isam.RgProd_.vector().squaredNorm();
|
||||
double step = -gradientSqNorm / RgNormSq;
|
||||
toc(3, "Compute minimizing step size");
|
||||
|
||||
|
|
|
|||
|
|
@ -347,26 +347,16 @@ protected:
|
|||
/** VariableIndex lets us look up factors by involved variable and keeps track of dimensions */
|
||||
VariableIndex variableIndex_;
|
||||
|
||||
/** The linear delta from the last linear solution, an update to the estimate in theta */
|
||||
VectorValues deltaUnpermuted_;
|
||||
|
||||
/** The permutation through which the deltaUnpermuted_ is
|
||||
* referenced.
|
||||
*
|
||||
* Permuting Vector entries would be slow, so for performance we
|
||||
* instead maintain this permutation through which we access the linear delta
|
||||
* indirectly
|
||||
/** The linear delta from the last linear solution, an update to the estimate in theta
|
||||
*
|
||||
* This is \c mutable because it is a "cached" variable - it is not updated
|
||||
* until either requested with getDelta() or calculateEstimate(), or needed
|
||||
* during update() to evaluate whether to relinearize variables.
|
||||
*/
|
||||
mutable Permuted<VectorValues> delta_;
|
||||
mutable VectorValues delta_;
|
||||
|
||||
VectorValues deltaNewtonUnpermuted_;
|
||||
mutable Permuted<VectorValues> deltaNewton_;
|
||||
VectorValues RgProdUnpermuted_;
|
||||
mutable Permuted<VectorValues> RgProd_;
|
||||
mutable VectorValues deltaNewton_;
|
||||
mutable VectorValues RgProd_;
|
||||
mutable bool deltaDoglegUptodate_;
|
||||
|
||||
/** Indicates whether the current delta is up-to-date, only used
|
||||
|
|
@ -497,7 +487,7 @@ public:
|
|||
Values calculateBestEstimate() const;
|
||||
|
||||
/** Access the current delta, computed during the last call to update */
|
||||
const Permuted<VectorValues>& getDelta() const;
|
||||
const VectorValues& getDelta() const;
|
||||
|
||||
/** Access the set of nonlinear factors */
|
||||
const NonlinearFactorGraph& getFactorsUnsafe() const { return nonlinearFactors_; }
|
||||
|
|
@ -555,7 +545,7 @@ void optimizeInPlace(const ISAM2& isam, VectorValues& delta);
|
|||
/// @return The number of variables that were solved for
|
||||
template<class CLIQUE>
|
||||
int optimizeWildfire(const boost::shared_ptr<CLIQUE>& root,
|
||||
double threshold, const std::vector<bool>& replaced, Permuted<VectorValues>& delta);
|
||||
double threshold, const std::vector<bool>& replaced, VectorValues& delta);
|
||||
|
||||
/**
|
||||
* Optimize along the gradient direction, with a closed-form computation to
|
||||
|
|
|
|||
|
|
@ -139,112 +139,69 @@ TEST_UNSAFE(ISAM2, AddVariables) {
|
|||
|
||||
// Create initial state
|
||||
Values theta;
|
||||
theta.insert((0), Pose2(.1, .2, .3));
|
||||
theta.insert(0, Pose2(.1, .2, .3));
|
||||
theta.insert(100, Point2(.4, .5));
|
||||
Values newTheta;
|
||||
newTheta.insert((1), Pose2(.6, .7, .8));
|
||||
newTheta.insert(1, Pose2(.6, .7, .8));
|
||||
|
||||
VectorValues deltaUnpermuted;
|
||||
deltaUnpermuted.insert(0, Vector_(3, .1, .2, .3));
|
||||
deltaUnpermuted.insert(1, Vector_(2, .4, .5));
|
||||
VectorValues delta;
|
||||
delta.insert(0, Vector_(3, .1, .2, .3));
|
||||
delta.insert(1, Vector_(2, .4, .5));
|
||||
|
||||
Permutation permutation(2);
|
||||
permutation[0] = 1;
|
||||
permutation[1] = 0;
|
||||
VectorValues deltaNewton;
|
||||
deltaNewton.insert(0, Vector_(3, .1, .2, .3));
|
||||
deltaNewton.insert(1, Vector_(2, .4, .5));
|
||||
|
||||
Permuted<VectorValues> delta(permutation, deltaUnpermuted);
|
||||
|
||||
VectorValues deltaNewtonUnpermuted;
|
||||
deltaNewtonUnpermuted.insert(0, Vector_(3, .1, .2, .3));
|
||||
deltaNewtonUnpermuted.insert(1, Vector_(2, .4, .5));
|
||||
|
||||
Permutation permutationNewton(2);
|
||||
permutationNewton[0] = 1;
|
||||
permutationNewton[1] = 0;
|
||||
|
||||
Permuted<VectorValues> deltaNewton(permutationNewton, deltaNewtonUnpermuted);
|
||||
|
||||
VectorValues deltaRgUnpermuted;
|
||||
deltaRgUnpermuted.insert(0, Vector_(3, .1, .2, .3));
|
||||
deltaRgUnpermuted.insert(1, Vector_(2, .4, .5));
|
||||
|
||||
Permutation permutationRg(2);
|
||||
permutationRg[0] = 1;
|
||||
permutationRg[1] = 0;
|
||||
|
||||
Permuted<VectorValues> deltaRg(permutationRg, deltaRgUnpermuted);
|
||||
VectorValues deltaRg;
|
||||
deltaRg.insert(0, Vector_(3, .1, .2, .3));
|
||||
deltaRg.insert(1, Vector_(2, .4, .5));
|
||||
|
||||
vector<bool> replacedKeys(2, false);
|
||||
|
||||
Ordering ordering; ordering += 100, (0);
|
||||
Ordering ordering; ordering += 100, 0;
|
||||
|
||||
ISAM2::Nodes nodes(2);
|
||||
|
||||
// Verify initial state
|
||||
LONGS_EQUAL(0, ordering[100]);
|
||||
LONGS_EQUAL(1, ordering[(0)]);
|
||||
EXPECT(assert_equal(deltaUnpermuted[1], delta[ordering[100]]));
|
||||
EXPECT(assert_equal(deltaUnpermuted[0], delta[ordering[(0)]]));
|
||||
LONGS_EQUAL(1, ordering[0]);
|
||||
EXPECT(assert_equal(delta[0], delta[ordering[100]]));
|
||||
EXPECT(assert_equal(delta[1], delta[ordering[0]]));
|
||||
|
||||
// Create expected state
|
||||
Values thetaExpected;
|
||||
thetaExpected.insert((0), Pose2(.1, .2, .3));
|
||||
thetaExpected.insert(0, Pose2(.1, .2, .3));
|
||||
thetaExpected.insert(100, Point2(.4, .5));
|
||||
thetaExpected.insert((1), Pose2(.6, .7, .8));
|
||||
thetaExpected.insert(1, Pose2(.6, .7, .8));
|
||||
|
||||
VectorValues deltaUnpermutedExpected;
|
||||
deltaUnpermutedExpected.insert(0, Vector_(3, .1, .2, .3));
|
||||
deltaUnpermutedExpected.insert(1, Vector_(2, .4, .5));
|
||||
deltaUnpermutedExpected.insert(2, Vector_(3, 0.0, 0.0, 0.0));
|
||||
VectorValues deltaExpected;
|
||||
deltaExpected.insert(0, Vector_(3, .1, .2, .3));
|
||||
deltaExpected.insert(1, Vector_(2, .4, .5));
|
||||
deltaExpected.insert(2, Vector_(3, 0.0, 0.0, 0.0));
|
||||
|
||||
Permutation permutationExpected(3);
|
||||
permutationExpected[0] = 1;
|
||||
permutationExpected[1] = 0;
|
||||
permutationExpected[2] = 2;
|
||||
VectorValues deltaNewtonExpected;
|
||||
deltaNewtonExpected.insert(0, Vector_(3, .1, .2, .3));
|
||||
deltaNewtonExpected.insert(1, Vector_(2, .4, .5));
|
||||
deltaNewtonExpected.insert(2, Vector_(3, 0.0, 0.0, 0.0));
|
||||
|
||||
Permuted<VectorValues> deltaExpected(permutationExpected, deltaUnpermutedExpected);
|
||||
|
||||
VectorValues deltaNewtonUnpermutedExpected;
|
||||
deltaNewtonUnpermutedExpected.insert(0, Vector_(3, .1, .2, .3));
|
||||
deltaNewtonUnpermutedExpected.insert(1, Vector_(2, .4, .5));
|
||||
deltaNewtonUnpermutedExpected.insert(2, Vector_(3, 0.0, 0.0, 0.0));
|
||||
|
||||
Permutation permutationNewtonExpected(3);
|
||||
permutationNewtonExpected[0] = 1;
|
||||
permutationNewtonExpected[1] = 0;
|
||||
permutationNewtonExpected[2] = 2;
|
||||
|
||||
Permuted<VectorValues> deltaNewtonExpected(permutationNewtonExpected, deltaNewtonUnpermutedExpected);
|
||||
|
||||
VectorValues deltaRgUnpermutedExpected;
|
||||
deltaRgUnpermutedExpected.insert(0, Vector_(3, .1, .2, .3));
|
||||
deltaRgUnpermutedExpected.insert(1, Vector_(2, .4, .5));
|
||||
deltaRgUnpermutedExpected.insert(2, Vector_(3, 0.0, 0.0, 0.0));
|
||||
|
||||
Permutation permutationRgExpected(3);
|
||||
permutationRgExpected[0] = 1;
|
||||
permutationRgExpected[1] = 0;
|
||||
permutationRgExpected[2] = 2;
|
||||
|
||||
Permuted<VectorValues> deltaRgExpected(permutationRgExpected, deltaRgUnpermutedExpected);
|
||||
VectorValues deltaRgExpected;
|
||||
deltaRgExpected.insert(0, Vector_(3, .1, .2, .3));
|
||||
deltaRgExpected.insert(1, Vector_(2, .4, .5));
|
||||
deltaRgExpected.insert(2, Vector_(3, 0.0, 0.0, 0.0));
|
||||
|
||||
vector<bool> replacedKeysExpected(3, false);
|
||||
|
||||
Ordering orderingExpected; orderingExpected += 100, (0), (1);
|
||||
Ordering orderingExpected; orderingExpected += 100, 0, 1;
|
||||
|
||||
ISAM2::Nodes nodesExpected(
|
||||
3, ISAM2::sharedClique());
|
||||
ISAM2::Nodes nodesExpected(3, ISAM2::sharedClique());
|
||||
|
||||
// Expand initial state
|
||||
ISAM2::Impl::AddVariables(newTheta, theta, delta, deltaNewton, deltaRg, replacedKeys, ordering, nodes);
|
||||
|
||||
EXPECT(assert_equal(thetaExpected, theta));
|
||||
EXPECT(assert_equal(deltaUnpermutedExpected, deltaUnpermuted));
|
||||
EXPECT(assert_equal(deltaExpected.permutation(), delta.permutation()));
|
||||
EXPECT(assert_equal(deltaNewtonUnpermutedExpected, deltaNewtonUnpermuted));
|
||||
EXPECT(assert_equal(deltaNewtonExpected.permutation(), deltaNewton.permutation()));
|
||||
EXPECT(assert_equal(deltaRgUnpermutedExpected, deltaRgUnpermuted));
|
||||
EXPECT(assert_equal(deltaRgExpected.permutation(), deltaRg.permutation()));
|
||||
EXPECT(assert_equal(deltaExpected, delta));
|
||||
EXPECT(assert_equal(deltaNewtonExpected, deltaNewton));
|
||||
EXPECT(assert_equal(deltaRgExpected, deltaRg));
|
||||
EXPECT(assert_container_equality(replacedKeysExpected, replacedKeys));
|
||||
EXPECT(assert_equal(orderingExpected, ordering));
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue