Merge branch 'feature/ordering' of bitbucket.org:gtborg/gtsam into feature/ordering
commit
c92b7cca8c
|
@ -55,10 +55,10 @@ int main(int argc, char** argv) {
|
|||
LevenbergMarquardtParams params;
|
||||
// In order to specify the ordering type, we need to se the NonlinearOptimizerParameter "orderingType"
|
||||
// By default this parameter is set to OrderingType::COLAMD
|
||||
params.orderingType = OrderingType::METIS;
|
||||
params.orderingType = Ordering::METIS;
|
||||
LevenbergMarquardtOptimizer optimizer(graph, initial, params);
|
||||
Values result = optimizer.optimize();
|
||||
result.print("Final Result:\n");
|
||||
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
|
|
|
@ -589,7 +589,7 @@ void runStats()
|
|||
{
|
||||
cout << "Gathering statistics..." << endl;
|
||||
GaussianFactorGraph linear = *datasetMeasurements.linearize(initial);
|
||||
GaussianJunctionTree jt(GaussianEliminationTree(linear, Ordering::COLAMD(linear)));
|
||||
GaussianJunctionTree jt(GaussianEliminationTree(linear, Ordering::colamd(linear)));
|
||||
treeTraversal::ForestStatistics statistics = treeTraversal::GatherStatistics(jt);
|
||||
|
||||
ofstream file;
|
||||
|
|
|
@ -54,10 +54,10 @@ namespace gtsam {
|
|||
// If no Ordering provided, compute one and call this function again. We are guaranteed to
|
||||
// have a VariableIndex already here because we computed one if needed in the previous 'else'
|
||||
// block.
|
||||
if (orderingType == OrderingType::METIS)
|
||||
return eliminateSequential(Ordering::METIS(asDerived()), function, variableIndex, orderingType);
|
||||
if (orderingType == Ordering::METIS)
|
||||
return eliminateSequential(Ordering::metis(asDerived()), function, variableIndex, orderingType);
|
||||
else
|
||||
return eliminateSequential(Ordering::COLAMD(*variableIndex), function, variableIndex, orderingType);
|
||||
return eliminateSequential(Ordering::colamd(*variableIndex), function, variableIndex, orderingType);
|
||||
}
|
||||
}
|
||||
|
||||
|
@ -92,10 +92,10 @@ namespace gtsam {
|
|||
// If no Ordering provided, compute one and call this function again. We are guaranteed to
|
||||
// have a VariableIndex already here because we computed one if needed in the previous 'else'
|
||||
// block.
|
||||
if (orderingType == OrderingType::METIS)
|
||||
return eliminateMultifrontal(Ordering::METIS(asDerived()), function, variableIndex, orderingType);
|
||||
if (orderingType == Ordering::METIS)
|
||||
return eliminateMultifrontal(Ordering::metis(asDerived()), function, variableIndex, orderingType);
|
||||
else
|
||||
return eliminateMultifrontal(Ordering::COLAMD(*variableIndex), function, variableIndex, orderingType);
|
||||
return eliminateMultifrontal(Ordering::colamd(*variableIndex), function, variableIndex, orderingType);
|
||||
}
|
||||
}
|
||||
|
||||
|
@ -125,7 +125,7 @@ namespace gtsam {
|
|||
if(variableIndex) {
|
||||
gttic(eliminatePartialSequential);
|
||||
// Compute full ordering
|
||||
Ordering fullOrdering = Ordering::COLAMDConstrainedFirst(*variableIndex, variables);
|
||||
Ordering fullOrdering = Ordering::colamdConstrainedFirst(*variableIndex, variables);
|
||||
|
||||
// Split off the part of the ordering for the variables being eliminated
|
||||
Ordering ordering(fullOrdering.begin(), fullOrdering.begin() + variables.size());
|
||||
|
@ -163,7 +163,7 @@ namespace gtsam {
|
|||
if(variableIndex) {
|
||||
gttic(eliminatePartialMultifrontal);
|
||||
// Compute full ordering
|
||||
Ordering fullOrdering = Ordering::COLAMDConstrainedFirst(*variableIndex, variables);
|
||||
Ordering fullOrdering = Ordering::colamdConstrainedFirst(*variableIndex, variables);
|
||||
|
||||
// Split off the part of the ordering for the variables being eliminated
|
||||
Ordering ordering(fullOrdering.begin(), fullOrdering.begin() + variables.size());
|
||||
|
@ -216,7 +216,7 @@ namespace gtsam {
|
|||
boost::get<const Ordering&>(&variables) : boost::get<const std::vector<Key>&>(&variables);
|
||||
|
||||
Ordering totalOrdering =
|
||||
Ordering::COLAMDConstrainedLast(*variableIndex, *variablesOrOrdering, unmarginalizedAreOrdered);
|
||||
Ordering::colamdConstrainedLast(*variableIndex, *variablesOrOrdering, unmarginalizedAreOrdered);
|
||||
|
||||
// Split up ordering
|
||||
const size_t nVars = variablesOrOrdering->size();
|
||||
|
@ -275,7 +275,7 @@ namespace gtsam {
|
|||
boost::get<const Ordering&>(&variables) : boost::get<const std::vector<Key>&>(&variables);
|
||||
|
||||
Ordering totalOrdering =
|
||||
Ordering::COLAMDConstrainedLast(*variableIndex, *variablesOrOrdering, unmarginalizedAreOrdered);
|
||||
Ordering::colamdConstrainedLast(*variableIndex, *variablesOrOrdering, unmarginalizedAreOrdered);
|
||||
|
||||
// Split up ordering
|
||||
const size_t nVars = variablesOrOrdering->size();
|
||||
|
@ -301,7 +301,7 @@ namespace gtsam {
|
|||
if(variableIndex)
|
||||
{
|
||||
// Compute a total ordering for all variables
|
||||
Ordering totalOrdering = Ordering::COLAMDConstrainedLast(*variableIndex, variables);
|
||||
Ordering totalOrdering = Ordering::colamdConstrainedLast(*variableIndex, variables);
|
||||
|
||||
// Split out the part for the marginalized variables
|
||||
Ordering marginalizationOrdering(totalOrdering.begin(), totalOrdering.end() - variables.size());
|
||||
|
|
|
@ -95,7 +95,7 @@ namespace gtsam {
|
|||
typedef boost::optional<const VariableIndex&> OptionalVariableIndex;
|
||||
|
||||
/// Typedef for an optional ordering type
|
||||
typedef boost::optional<OrderingType> OptionalOrderingType;
|
||||
typedef boost::optional<Ordering::OrderingType> OptionalOrderingType;
|
||||
|
||||
/** Do sequential elimination of all variables to produce a Bayes net. If an ordering is not
|
||||
* provided, the ordering provided by COLAMD will be used.
|
||||
|
|
|
@ -46,7 +46,7 @@ namespace gtsam {
|
|||
const VariableIndex varIndex(factors);
|
||||
const FastSet<Key> newFactorKeys = newFactors.keys();
|
||||
const Ordering constrainedOrdering =
|
||||
Ordering::COLAMDConstrainedLast(varIndex, std::vector<Key>(newFactorKeys.begin(), newFactorKeys.end()));
|
||||
Ordering::colamdConstrainedLast(varIndex, std::vector<Key>(newFactorKeys.begin(), newFactorKeys.end()));
|
||||
Base bayesTree = *factors.eliminateMultifrontal(constrainedOrdering, function, varIndex);
|
||||
this->roots_.insert(this->roots_.end(), bayesTree.roots().begin(), bayesTree.roots().end());
|
||||
this->nodes_.insert(bayesTree.nodes().begin(), bayesTree.nodes().end());
|
||||
|
|
|
@ -39,15 +39,15 @@ namespace gtsam {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Ordering Ordering::COLAMD(const VariableIndex& variableIndex)
|
||||
Ordering Ordering::colamd(const VariableIndex& variableIndex)
|
||||
{
|
||||
// Call constrained version with all groups set to zero
|
||||
vector<int> dummy_groups(variableIndex.size(), 0);
|
||||
return Ordering::COLAMDConstrained(variableIndex, dummy_groups);
|
||||
return Ordering::colamdConstrained(variableIndex, dummy_groups);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Ordering Ordering::COLAMDConstrained(
|
||||
Ordering Ordering::colamdConstrained(
|
||||
const VariableIndex& variableIndex, std::vector<int>& cmember)
|
||||
{
|
||||
gttic(Ordering_COLAMDConstrained);
|
||||
|
@ -114,7 +114,7 @@ namespace gtsam {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Ordering Ordering::COLAMDConstrainedLast(
|
||||
Ordering Ordering::colamdConstrainedLast(
|
||||
const VariableIndex& variableIndex, const std::vector<Key>& constrainLast, bool forceOrder)
|
||||
{
|
||||
gttic(Ordering_COLAMDConstrainedLast);
|
||||
|
@ -137,11 +137,11 @@ namespace gtsam {
|
|||
++ group;
|
||||
}
|
||||
|
||||
return Ordering::COLAMDConstrained(variableIndex, cmember);
|
||||
return Ordering::colamdConstrained(variableIndex, cmember);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Ordering Ordering::COLAMDConstrainedFirst(
|
||||
Ordering Ordering::colamdConstrainedFirst(
|
||||
const VariableIndex& variableIndex, const std::vector<Key>& constrainFirst, bool forceOrder)
|
||||
{
|
||||
gttic(Ordering_COLAMDConstrainedFirst);
|
||||
|
@ -171,11 +171,11 @@ namespace gtsam {
|
|||
if(c == none)
|
||||
c = group;
|
||||
|
||||
return Ordering::COLAMDConstrained(variableIndex, cmember);
|
||||
return Ordering::colamdConstrained(variableIndex, cmember);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Ordering Ordering::COLAMDConstrained(const VariableIndex& variableIndex,
|
||||
Ordering Ordering::colamdConstrained(const VariableIndex& variableIndex,
|
||||
const FastMap<Key, int>& groups)
|
||||
{
|
||||
gttic(Ordering_COLAMDConstrained);
|
||||
|
@ -195,12 +195,12 @@ namespace gtsam {
|
|||
cmember[keyIndices.at(p.first)] = p.second;
|
||||
}
|
||||
|
||||
return Ordering::COLAMDConstrained(variableIndex, cmember);
|
||||
return Ordering::colamdConstrained(variableIndex, cmember);
|
||||
}
|
||||
|
||||
|
||||
/* ************************************************************************* */
|
||||
Ordering Ordering::METIS(const MetisIndex& met)
|
||||
Ordering Ordering::metis(const MetisIndex& met)
|
||||
{
|
||||
gttic(Ordering_METIS);
|
||||
|
||||
|
|
|
@ -30,15 +30,17 @@
|
|||
|
||||
namespace gtsam {
|
||||
|
||||
enum OrderingType {
|
||||
COLAMD, METIS, CUSTOM
|
||||
};
|
||||
|
||||
class Ordering : public std::vector<Key> {
|
||||
protected:
|
||||
typedef std::vector<Key> Base;
|
||||
|
||||
public:
|
||||
|
||||
/// Type of ordering to use
|
||||
enum OrderingType {
|
||||
COLAMD, METIS, CUSTOM
|
||||
};
|
||||
|
||||
typedef Ordering This; ///< Typedef to this class
|
||||
typedef boost::shared_ptr<This> shared_ptr; ///< shared_ptr to this class
|
||||
|
||||
|
@ -69,11 +71,11 @@ namespace gtsam {
|
|||
/// performance). This internally builds a VariableIndex so if you already have a VariableIndex,
|
||||
/// it is faster to use COLAMD(const VariableIndex&)
|
||||
template<class FACTOR>
|
||||
static Ordering COLAMD(const FactorGraph<FACTOR>& graph) {
|
||||
return COLAMD(VariableIndex(graph)); }
|
||||
static Ordering colamd(const FactorGraph<FACTOR>& graph) {
|
||||
return colamd(VariableIndex(graph)); }
|
||||
|
||||
/// Compute a fill-reducing ordering using COLAMD from a VariableIndex.
|
||||
static GTSAM_EXPORT Ordering COLAMD(const VariableIndex& variableIndex);
|
||||
static GTSAM_EXPORT Ordering colamd(const VariableIndex& variableIndex);
|
||||
|
||||
/// Compute a fill-reducing ordering using constrained COLAMD from a factor graph (see details
|
||||
/// for note on performance). This internally builds a VariableIndex so if you already have a
|
||||
|
@ -84,9 +86,9 @@ namespace gtsam {
|
|||
/// constrainLast. If \c forceOrder is false, the variables in \c constrainLast will be
|
||||
/// ordered after all the others, but will be rearranged by CCOLAMD to reduce fill-in as well.
|
||||
template<class FACTOR>
|
||||
static Ordering COLAMDConstrainedLast(const FactorGraph<FACTOR>& graph,
|
||||
static Ordering colamdConstrainedLast(const FactorGraph<FACTOR>& graph,
|
||||
const std::vector<Key>& constrainLast, bool forceOrder = false) {
|
||||
return COLAMDConstrainedLast(VariableIndex(graph), constrainLast, forceOrder); }
|
||||
return colamdConstrainedLast(VariableIndex(graph), constrainLast, forceOrder); }
|
||||
|
||||
/// Compute a fill-reducing ordering using constrained COLAMD from a VariableIndex. This
|
||||
/// function constrains the variables in \c constrainLast to the end of the ordering, and orders
|
||||
|
@ -94,7 +96,7 @@ namespace gtsam {
|
|||
/// variables in \c constrainLast will be ordered in the same order specified in the vector<Key>
|
||||
/// \c constrainLast. If \c forceOrder is false, the variables in \c constrainLast will be
|
||||
/// ordered after all the others, but will be rearranged by CCOLAMD to reduce fill-in as well.
|
||||
static GTSAM_EXPORT Ordering COLAMDConstrainedLast(const VariableIndex& variableIndex,
|
||||
static GTSAM_EXPORT Ordering colamdConstrainedLast(const VariableIndex& variableIndex,
|
||||
const std::vector<Key>& constrainLast, bool forceOrder = false);
|
||||
|
||||
/// Compute a fill-reducing ordering using constrained COLAMD from a factor graph (see details
|
||||
|
@ -106,9 +108,9 @@ namespace gtsam {
|
|||
/// constrainLast. If \c forceOrder is false, the variables in \c constrainFirst will be
|
||||
/// ordered after all the others, but will be rearranged by CCOLAMD to reduce fill-in as well.
|
||||
template<class FACTOR>
|
||||
static Ordering COLAMDConstrainedFirst(const FactorGraph<FACTOR>& graph,
|
||||
static Ordering colamdConstrainedFirst(const FactorGraph<FACTOR>& graph,
|
||||
const std::vector<Key>& constrainFirst, bool forceOrder = false) {
|
||||
return COLAMDConstrainedFirst(VariableIndex(graph), constrainFirst, forceOrder); }
|
||||
return colamdConstrainedFirst(VariableIndex(graph), constrainFirst, forceOrder); }
|
||||
|
||||
/// Compute a fill-reducing ordering using constrained COLAMD from a VariableIndex. This
|
||||
/// function constrains the variables in \c constrainFirst to the front of the ordering, and
|
||||
|
@ -117,7 +119,7 @@ namespace gtsam {
|
|||
/// vector<Key> \c constrainFirst. If \c forceOrder is false, the variables in \c
|
||||
/// constrainFirst will be ordered after all the others, but will be rearranged by CCOLAMD to
|
||||
/// reduce fill-in as well.
|
||||
static GTSAM_EXPORT Ordering COLAMDConstrainedFirst(const VariableIndex& variableIndex,
|
||||
static GTSAM_EXPORT Ordering colamdConstrainedFirst(const VariableIndex& variableIndex,
|
||||
const std::vector<Key>& constrainFirst, bool forceOrder = false);
|
||||
|
||||
/// Compute a fill-reducing ordering using constrained COLAMD from a factor graph (see details
|
||||
|
@ -130,9 +132,9 @@ namespace gtsam {
|
|||
/// function simply fills the \c cmember argument to CCOLAMD with the supplied indices, see the
|
||||
/// CCOLAMD documentation for more information.
|
||||
template<class FACTOR>
|
||||
static Ordering COLAMDConstrained(const FactorGraph<FACTOR>& graph,
|
||||
static Ordering colamdConstrained(const FactorGraph<FACTOR>& graph,
|
||||
const FastMap<Key, int>& groups) {
|
||||
return COLAMDConstrained(VariableIndex(graph), groups); }
|
||||
return colamdConstrained(VariableIndex(graph), groups); }
|
||||
|
||||
/// Compute a fill-reducing ordering using constrained COLAMD from a VariableIndex. In this
|
||||
/// function, a group for each variable should be specified in \c groups, and each group of
|
||||
|
@ -141,7 +143,7 @@ namespace gtsam {
|
|||
/// appear in \c groups in arbitrary order. Any variables not present in \c groups will be
|
||||
/// assigned to group 0. This function simply fills the \c cmember argument to CCOLAMD with the
|
||||
/// supplied indices, see the CCOLAMD documentation for more information.
|
||||
static GTSAM_EXPORT Ordering COLAMDConstrained(const VariableIndex& variableIndex,
|
||||
static GTSAM_EXPORT Ordering colamdConstrained(const VariableIndex& variableIndex,
|
||||
const FastMap<Key, int>& groups);
|
||||
|
||||
/// Return a natural Ordering. Typically used by iterative solvers
|
||||
|
@ -158,12 +160,12 @@ namespace gtsam {
|
|||
static GTSAM_EXPORT void CSRFormat(std::vector<int>& xadj, std::vector<int>& adj, const FactorGraph<FACTOR>& graph);
|
||||
|
||||
/// Compute an ordering determined by METIS from a VariableIndex
|
||||
static GTSAM_EXPORT Ordering METIS(const MetisIndex& met);
|
||||
static GTSAM_EXPORT Ordering metis(const MetisIndex& met);
|
||||
|
||||
template<class FACTOR>
|
||||
static Ordering METIS(const FactorGraph<FACTOR>& graph)
|
||||
static Ordering metis(const FactorGraph<FACTOR>& graph)
|
||||
{
|
||||
return METIS(MetisIndex(graph));
|
||||
return metis(MetisIndex(graph));
|
||||
}
|
||||
|
||||
/// @}
|
||||
|
@ -178,7 +180,7 @@ namespace gtsam {
|
|||
|
||||
private:
|
||||
/// Internal COLAMD function
|
||||
static GTSAM_EXPORT Ordering COLAMDConstrained(
|
||||
static GTSAM_EXPORT Ordering colamdConstrained(
|
||||
const VariableIndex& variableIndex, std::vector<int>& cmember);
|
||||
|
||||
|
||||
|
|
|
@ -22,7 +22,7 @@
|
|||
#include <gtsam/base/TestableAssertions.h>
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
|
||||
#include <boost/assign/list_of.hpp>
|
||||
#include <boost/assign/std.hpp>
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
@ -40,17 +40,17 @@ TEST(Ordering, constrained_ordering) {
|
|||
sfg.push_factor(4,5);
|
||||
|
||||
// unconstrained version
|
||||
Ordering actUnconstrained = Ordering::COLAMD(sfg);
|
||||
Ordering actUnconstrained = Ordering::colamd(sfg);
|
||||
Ordering expUnconstrained = Ordering(list_of(0)(1)(2)(3)(4)(5));
|
||||
EXPECT(assert_equal(expUnconstrained, actUnconstrained));
|
||||
|
||||
// constrained version - push one set to the end
|
||||
Ordering actConstrained = Ordering::COLAMDConstrainedLast(sfg, list_of(2)(4));
|
||||
Ordering actConstrained = Ordering::colamdConstrainedLast(sfg, list_of(2)(4));
|
||||
Ordering expConstrained = Ordering(list_of(0)(1)(5)(3)(4)(2));
|
||||
EXPECT(assert_equal(expConstrained, actConstrained));
|
||||
|
||||
// constrained version - push one set to the start
|
||||
Ordering actConstrained2 = Ordering::COLAMDConstrainedFirst(sfg, list_of(2)(4));
|
||||
Ordering actConstrained2 = Ordering::colamdConstrainedFirst(sfg, list_of(2)(4));
|
||||
Ordering expConstrained2 = Ordering(list_of(2)(4)(0)(1)(3)(5));
|
||||
EXPECT(assert_equal(expConstrained2, actConstrained2));
|
||||
}
|
||||
|
@ -74,7 +74,7 @@ TEST(Ordering, grouped_constrained_ordering) {
|
|||
constraints[4] = 1;
|
||||
constraints[5] = 2;
|
||||
|
||||
Ordering actConstrained = Ordering::COLAMDConstrained(sfg, constraints);
|
||||
Ordering actConstrained = Ordering::colamdConstrained(sfg, constraints);
|
||||
Ordering expConstrained = list_of(0)(1)(3)(2)(4)(5);
|
||||
EXPECT(assert_equal(expConstrained, actConstrained));
|
||||
}
|
||||
|
@ -109,17 +109,18 @@ TEST(Ordering, csr_format) {
|
|||
|
||||
MetisIndex mi(sfg);
|
||||
|
||||
vector<int> xadjExpected{ 0, 2, 5, 8, 11, 13, 16, 20, 24, 28, 31, 33, 36, 39, 42, 44};
|
||||
vector<int> adjExpected{ 1, 5, 0, 2, 6, 1, 3, 7, 2, 4, 8, 3, 9, 0, 6, 10, 1, 5, 7, 11,
|
||||
vector<int> xadjExpected, adjExpected;
|
||||
xadjExpected += 0, 2, 5, 8, 11, 13, 16, 20, 24, 28, 31, 33, 36, 39, 42, 44;
|
||||
adjExpected += 1, 5, 0, 2, 6, 1, 3, 7, 2, 4, 8, 3, 9, 0, 6, 10, 1, 5, 7, 11,
|
||||
2, 6, 8, 12, 3, 7, 9, 13, 4, 8, 14, 5, 11, 6, 10, 12, 7, 11,
|
||||
13, 8, 12, 14, 9, 13 };
|
||||
13, 8, 12, 14, 9, 13 ;
|
||||
|
||||
EXPECT(xadjExpected == mi.xadj());
|
||||
EXPECT(adjExpected.size() == mi.adj().size());
|
||||
EXPECT(adjExpected == mi.adj());
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(Ordering, csr_format_2) {
|
||||
SymbolicFactorGraph sfg;
|
||||
|
||||
|
@ -132,16 +133,17 @@ TEST(Ordering, csr_format_2) {
|
|||
|
||||
MetisIndex mi(sfg);
|
||||
|
||||
vector<int> xadjExpected { 0, 1, 4, 6, 8, 10 };
|
||||
vector<int> adjExpected { 1, 0, 2, 4, 1, 3, 2, 4, 1, 3 };
|
||||
vector<int> xadjExpected, adjExpected;
|
||||
xadjExpected += 0, 1, 4, 6, 8, 10;
|
||||
adjExpected += 1, 0, 2, 4, 1, 3, 2, 4, 1, 3;
|
||||
|
||||
EXPECT(xadjExpected == mi.xadj());
|
||||
EXPECT(adjExpected.size() == mi.adj().size());
|
||||
EXPECT(adjExpected == mi.adj());
|
||||
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(Ordering, csr_format_3) {
|
||||
SymbolicFactorGraph sfg;
|
||||
|
||||
|
@ -154,40 +156,43 @@ TEST(Ordering, csr_format_3) {
|
|||
|
||||
MetisIndex mi(sfg);
|
||||
|
||||
vector<int> xadjExpected{ 0, 1, 4, 6, 8, 10 };
|
||||
vector<int> adjExpected{ 1, 0, 2, 4, 1, 3, 2, 4, 1, 3 };
|
||||
size_t minKey = mi.minKey();
|
||||
vector<int> xadjExpected, adjExpected;
|
||||
xadjExpected += 0, 1, 4, 6, 8, 10;
|
||||
adjExpected += 1, 0, 2, 4, 1, 3, 2, 4, 1, 3;
|
||||
size_t minKey = mi.minKey();
|
||||
|
||||
vector<int> adjAcutal = mi.adj();
|
||||
vector<int> adjAcutal = mi.adj();
|
||||
|
||||
// Normalize, subtract the smallest key
|
||||
std::transform(adjAcutal.begin(), adjAcutal.end(), adjAcutal.begin(), std::bind2nd(std::minus<size_t>(), minKey));
|
||||
// Normalize, subtract the smallest key
|
||||
std::transform(adjAcutal.begin(), adjAcutal.end(), adjAcutal.begin(),
|
||||
std::bind2nd(std::minus<size_t>(), minKey));
|
||||
|
||||
EXPECT(xadjExpected == mi.xadj());
|
||||
EXPECT(adjExpected.size() == mi.adj().size());
|
||||
EXPECT(adjExpected == adjAcutal);
|
||||
EXPECT(adjExpected == adjAcutal);
|
||||
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(Ordering, metis) {
|
||||
|
||||
|
||||
SymbolicFactorGraph sfg;
|
||||
|
||||
sfg.push_factor(0);
|
||||
sfg.push_factor(0, 1);
|
||||
sfg.push_factor(1, 2);
|
||||
sfg.push_factor(1, 2);
|
||||
|
||||
MetisIndex mi(sfg);
|
||||
|
||||
vector<int> xadjExpected{ 0, 1, 3, 4 };
|
||||
vector<int> adjExpected { 1, 0, 2, 1 };
|
||||
vector<int> xadjExpected, adjExpected;
|
||||
xadjExpected += 0, 1, 3, 4;
|
||||
adjExpected += 1, 0, 2, 1;
|
||||
|
||||
EXPECT(xadjExpected == mi.xadj());
|
||||
EXPECT(adjExpected.size() == mi.adj().size());
|
||||
EXPECT(adjExpected == mi.adj());
|
||||
|
||||
Ordering metis = Ordering::METIS(sfg);
|
||||
Ordering metis = Ordering::metis(sfg);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
|
||||
|
|
|
@ -96,7 +96,7 @@ void DoglegOptimizer::iterate(void) {
|
|||
/* ************************************************************************* */
|
||||
DoglegParams DoglegOptimizer::ensureHasOrdering(DoglegParams params, const NonlinearFactorGraph& graph) const {
|
||||
if(!params.ordering)
|
||||
params.ordering = Ordering::COLAMD(graph);
|
||||
params.ordering = Ordering::colamd(graph);
|
||||
return params;
|
||||
}
|
||||
|
||||
|
|
|
@ -49,7 +49,7 @@ GaussNewtonParams GaussNewtonOptimizer::ensureHasOrdering(
|
|||
GaussNewtonParams params, const NonlinearFactorGraph& graph) const
|
||||
{
|
||||
if(!params.ordering)
|
||||
params.ordering = Ordering::COLAMD(graph);
|
||||
params.ordering = Ordering::colamd(graph);
|
||||
return params;
|
||||
}
|
||||
|
||||
|
|
|
@ -341,7 +341,7 @@ boost::shared_ptr<FastSet<Key> > ISAM2::recalculate(const FastSet<Key>& markedKe
|
|||
Ordering order;
|
||||
if(constrainKeys)
|
||||
{
|
||||
order = Ordering::COLAMDConstrained(variableIndex_, *constrainKeys);
|
||||
order = Ordering::colamdConstrained(variableIndex_, *constrainKeys);
|
||||
}
|
||||
else
|
||||
{
|
||||
|
@ -351,11 +351,11 @@ boost::shared_ptr<FastSet<Key> > ISAM2::recalculate(const FastSet<Key>& markedKe
|
|||
FastMap<Key, int> constraintGroups;
|
||||
BOOST_FOREACH(Key var, observedKeys)
|
||||
constraintGroups[var] = 1;
|
||||
order = Ordering::COLAMDConstrained(variableIndex_, constraintGroups);
|
||||
order = Ordering::colamdConstrained(variableIndex_, constraintGroups);
|
||||
}
|
||||
else
|
||||
{
|
||||
order = Ordering::COLAMD(variableIndex_);
|
||||
order = Ordering::colamd(variableIndex_);
|
||||
}
|
||||
}
|
||||
gttoc(ordering);
|
||||
|
@ -481,7 +481,7 @@ boost::shared_ptr<FastSet<Key> > ISAM2::recalculate(const FastSet<Key>& markedKe
|
|||
|
||||
// Generate ordering
|
||||
gttic(Ordering);
|
||||
Ordering ordering = Ordering::COLAMDConstrained(affectedFactorsVarIndex, constraintGroups);
|
||||
Ordering ordering = Ordering::colamdConstrained(affectedFactorsVarIndex, constraintGroups);
|
||||
gttoc(Ordering);
|
||||
|
||||
ISAM2BayesTree::shared_ptr bayesTree = ISAM2JunctionTree(GaussianEliminationTree(
|
||||
|
|
|
@ -341,10 +341,10 @@ void LevenbergMarquardtOptimizer::iterate() {
|
|||
LevenbergMarquardtParams LevenbergMarquardtOptimizer::ensureHasOrdering(
|
||||
LevenbergMarquardtParams params, const NonlinearFactorGraph& graph) const {
|
||||
if (!params.ordering){
|
||||
if (params.orderingType = OrderingType::METIS)
|
||||
params.ordering = Ordering::METIS(graph);
|
||||
if (params.orderingType == Ordering::METIS)
|
||||
params.ordering = Ordering::metis(graph);
|
||||
else
|
||||
params.ordering = Ordering::COLAMD(graph);
|
||||
params.ordering = Ordering::colamd(graph);
|
||||
}
|
||||
return params;
|
||||
}
|
||||
|
|
|
@ -282,13 +282,13 @@ FastSet<Key> NonlinearFactorGraph::keys() const {
|
|||
/* ************************************************************************* */
|
||||
Ordering NonlinearFactorGraph::orderingCOLAMD() const
|
||||
{
|
||||
return Ordering::COLAMD(*this);
|
||||
return Ordering::colamd(*this);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Ordering NonlinearFactorGraph::orderingCOLAMDConstrained(const FastMap<Key, int>& constraints) const
|
||||
{
|
||||
return Ordering::COLAMDConstrained(*this, constraints);
|
||||
return Ordering::colamdConstrained(*this, constraints);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
@ -109,10 +109,10 @@ void NonlinearOptimizerParams::print(const std::string& str) const {
|
|||
}
|
||||
|
||||
switch (orderingType){
|
||||
case OrderingType::COLAMD:
|
||||
case Ordering::COLAMD:
|
||||
std::cout << " ordering: COLAMD\n";
|
||||
break;
|
||||
case OrderingType::METIS:
|
||||
case Ordering::METIS:
|
||||
std::cout << " ordering: METIS\n";
|
||||
break;
|
||||
default:
|
||||
|
@ -165,11 +165,11 @@ NonlinearOptimizerParams::LinearSolverType NonlinearOptimizerParams::linearSolve
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
std::string NonlinearOptimizerParams::orderingTypeTranslator(OrderingType type) const{
|
||||
std::string NonlinearOptimizerParams::orderingTypeTranslator(Ordering::OrderingType type) const{
|
||||
switch (type) {
|
||||
case OrderingType::METIS:
|
||||
case Ordering::METIS:
|
||||
return "METIS";
|
||||
case OrderingType::COLAMD:
|
||||
case Ordering::COLAMD:
|
||||
return "COLAMD";
|
||||
default:
|
||||
if (ordering)
|
||||
|
@ -181,11 +181,11 @@ std::string NonlinearOptimizerParams::orderingTypeTranslator(OrderingType type)
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
OrderingType NonlinearOptimizerParams::orderingTypeTranslator(const std::string& type) const{
|
||||
Ordering::OrderingType NonlinearOptimizerParams::orderingTypeTranslator(const std::string& type) const{
|
||||
if (type == "METIS")
|
||||
return OrderingType::METIS;
|
||||
return Ordering::METIS;
|
||||
if (type == "COLAMD")
|
||||
return OrderingType::COLAMD;
|
||||
return Ordering::COLAMD;
|
||||
throw std::invalid_argument(
|
||||
"Invalid ordering type: You must provide an ordering for a custom ordering type. See setOrdering");
|
||||
}
|
||||
|
|
|
@ -43,12 +43,12 @@ public:
|
|||
double absoluteErrorTol; ///< The maximum absolute error decrease to stop iterating (default 1e-5)
|
||||
double errorTol; ///< The maximum total error to stop iterating (default 0.0)
|
||||
Verbosity verbosity; ///< The printing verbosity during optimization (default SILENT)
|
||||
OrderingType orderingType; ///< The method of ordering use during variable elimination (default COLAMD)
|
||||
Ordering::OrderingType orderingType; ///< The method of ordering use during variable elimination (default COLAMD)
|
||||
|
||||
NonlinearOptimizerParams() :
|
||||
maxIterations(100), relativeErrorTol(1e-5), absoluteErrorTol(1e-5), errorTol(
|
||||
0.0), verbosity(SILENT), linearSolverType(MULTIFRONTAL_CHOLESKY), orderingType(COLAMD) {
|
||||
}
|
||||
0.0), verbosity(SILENT), orderingType(Ordering::COLAMD),
|
||||
linearSolverType(MULTIFRONTAL_CHOLESKY) {}
|
||||
|
||||
virtual ~NonlinearOptimizerParams() {
|
||||
}
|
||||
|
@ -154,7 +154,7 @@ public:
|
|||
|
||||
void setOrdering(const Ordering& ordering) {
|
||||
this->ordering = ordering;
|
||||
this->orderingType = OrderingType::CUSTOM;
|
||||
this->orderingType = Ordering::CUSTOM;
|
||||
}
|
||||
|
||||
std::string getOrderingType() const {
|
||||
|
@ -171,9 +171,9 @@ private:
|
|||
|
||||
LinearSolverType linearSolverTranslator(const std::string& linearSolverType) const;
|
||||
|
||||
std::string orderingTypeTranslator(OrderingType type) const;
|
||||
std::string orderingTypeTranslator(Ordering::OrderingType type) const;
|
||||
|
||||
OrderingType orderingTypeTranslator(const std::string& type) const;
|
||||
Ordering::OrderingType orderingTypeTranslator(const std::string& type) const;
|
||||
|
||||
};
|
||||
|
||||
|
|
|
@ -191,7 +191,7 @@ void BatchFixedLagSmoother::reorder(const std::set<Key>& marginalizeKeys) {
|
|||
}
|
||||
|
||||
// COLAMD groups will be used to place marginalize keys in Group 0, and everything else in Group 1
|
||||
ordering_ = Ordering::COLAMDConstrainedFirst(factors_, std::vector<Key>(marginalizeKeys.begin(), marginalizeKeys.end()));
|
||||
ordering_ = Ordering::colamdConstrainedFirst(factors_, std::vector<Key>(marginalizeKeys.begin(), marginalizeKeys.end()));
|
||||
|
||||
if(debug) {
|
||||
ordering_.print("New Ordering: ");
|
||||
|
|
|
@ -362,9 +362,9 @@ void ConcurrentBatchFilter::reorder(const boost::optional<FastList<Key> >& keysT
|
|||
|
||||
// COLAMD groups will be used to place marginalize keys in Group 0, and everything else in Group 1
|
||||
if(keysToMove && keysToMove->size() > 0) {
|
||||
ordering_ = Ordering::COLAMDConstrainedFirst(factors_, std::vector<Key>(keysToMove->begin(), keysToMove->end()));
|
||||
ordering_ = Ordering::colamdConstrainedFirst(factors_, std::vector<Key>(keysToMove->begin(), keysToMove->end()));
|
||||
}else{
|
||||
ordering_ = Ordering::COLAMD(factors_);
|
||||
ordering_ = Ordering::colamd(factors_);
|
||||
}
|
||||
|
||||
}
|
||||
|
|
|
@ -231,7 +231,7 @@ void ConcurrentBatchSmoother::reorder() {
|
|||
variableIndex_ = VariableIndex(factors_);
|
||||
|
||||
FastList<Key> separatorKeys = separatorValues_.keys();
|
||||
ordering_ = Ordering::COLAMDConstrainedLast(variableIndex_, std::vector<Key>(separatorKeys.begin(), separatorKeys.end()));
|
||||
ordering_ = Ordering::colamdConstrainedLast(variableIndex_, std::vector<Key>(separatorKeys.begin(), separatorKeys.end()));
|
||||
|
||||
}
|
||||
|
||||
|
|
|
@ -79,14 +79,14 @@ TEST( NonlinearFactorGraph, GET_ORDERING)
|
|||
{
|
||||
Ordering expected; expected += L(1), X(2), X(1); // For starting with l1,x1,x2
|
||||
NonlinearFactorGraph nlfg = createNonlinearFactorGraph();
|
||||
Ordering actual = Ordering::COLAMD(nlfg);
|
||||
Ordering actual = Ordering::colamd(nlfg);
|
||||
EXPECT(assert_equal(expected,actual));
|
||||
|
||||
// Constrained ordering - put x2 at the end
|
||||
Ordering expectedConstrained; expectedConstrained += L(1), X(1), X(2);
|
||||
FastMap<Key, int> constraints;
|
||||
constraints[X(2)] = 1;
|
||||
Ordering actualConstrained = Ordering::COLAMDConstrained(nlfg, constraints);
|
||||
Ordering actualConstrained = Ordering::colamdConstrained(nlfg, constraints);
|
||||
EXPECT(assert_equal(expectedConstrained, actualConstrained));
|
||||
}
|
||||
|
||||
|
|
Loading…
Reference in New Issue