Chris Beall 2016-02-09 20:00:38 -05:00
parent 738cc66a8e
commit cc0b7cfdc1
8 changed files with 56 additions and 54 deletions

View File

@ -83,7 +83,7 @@ vector<RangeTriple> readTriples() {
while (is) {
double t, sender, range;
size_t receiver;
size_t receiver;
is >> t >> sender >> receiver >> range;
triples.push_back(RangeTriple(t, receiver, range));
}

View File

@ -189,7 +189,7 @@ void print(const Matrix& A, const string &s, ostream& stream) {
0, // flags
" ", // coeffSeparator
";\n", // rowSeparator
" \t", // rowPrefix
" \t", // rowPrefix
"", // rowSuffix
"[\n", // matPrefix
"\n ]" // matSuffix

View File

@ -28,8 +28,8 @@ namespace gtsam {
template<class FACTORGRAPH>
boost::shared_ptr<typename EliminateableFactorGraph<FACTORGRAPH>::BayesNetType>
EliminateableFactorGraph<FACTORGRAPH>::eliminateSequential(
OptionalOrdering ordering, const Eliminate& function,
OptionalVariableIndex variableIndex, OptionalOrderingType orderingType) const
OptionalOrdering ordering, const Eliminate& function,
OptionalVariableIndex variableIndex, OptionalOrderingType orderingType) const
{
if(ordering && variableIndex) {
gttic(eliminateSequential);
@ -65,8 +65,8 @@ namespace gtsam {
template<class FACTORGRAPH>
boost::shared_ptr<typename EliminateableFactorGraph<FACTORGRAPH>::BayesTreeType>
EliminateableFactorGraph<FACTORGRAPH>::eliminateMultifrontal(
OptionalOrdering ordering, const Eliminate& function,
OptionalVariableIndex variableIndex, OptionalOrderingType orderingType) const
OptionalOrdering ordering, const Eliminate& function,
OptionalVariableIndex variableIndex, OptionalOrderingType orderingType) const
{
if(ordering && variableIndex) {
gttic(eliminateMultifrontal);
@ -86,16 +86,16 @@ namespace gtsam {
// If no VariableIndex provided, compute one and call this function again IMPORTANT: we check
// for no variable index first so that it's always computed if we need to call COLAMD because
// no Ordering is provided.
return eliminateMultifrontal(ordering, function, VariableIndex(asDerived()), orderingType);
return eliminateMultifrontal(ordering, function, VariableIndex(asDerived()), orderingType);
}
else /*if(!ordering)*/ {
// 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 == Ordering::METIS)
return eliminateMultifrontal(Ordering::Metis(asDerived()), function, variableIndex, orderingType);
else
return eliminateMultifrontal(Ordering::Colamd(*variableIndex), function, variableIndex, orderingType);
if (orderingType == Ordering::METIS)
return eliminateMultifrontal(Ordering::Metis(asDerived()), function, variableIndex, orderingType);
else
return eliminateMultifrontal(Ordering::Colamd(*variableIndex), function, variableIndex, orderingType);
}
}

View File

@ -94,8 +94,8 @@ namespace gtsam {
/// Typedef for an optional variable index as an argument to elimination functions
typedef boost::optional<const VariableIndex&> OptionalVariableIndex;
/// Typedef for an optional ordering type
typedef boost::optional<Ordering::OrderingType> OptionalOrderingType;
/// Typedef for an optional ordering type
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.
@ -104,10 +104,10 @@ namespace gtsam {
* \code
* boost::shared_ptr<GaussianBayesNet> result = graph.eliminateSequential(EliminateCholesky);
* \endcode
*
* <b> Example - METIS ordering for elimination
* \code
* boost::shared_ptr<GaussianBayesNet> result = graph.eliminateSequential(OrderingType::METIS);
*
* <b> Example - METIS ordering for elimination
* \code
* boost::shared_ptr<GaussianBayesNet> result = graph.eliminateSequential(OrderingType::METIS);
*
* <b> Example - Full QR elimination in specified order:
* \code
@ -125,7 +125,7 @@ namespace gtsam {
OptionalOrdering ordering = boost::none,
const Eliminate& function = EliminationTraitsType::DefaultEliminate,
OptionalVariableIndex variableIndex = boost::none,
OptionalOrderingType orderingType = boost::none) const;
OptionalOrderingType orderingType = boost::none) const;
/** Do multifrontal elimination of all variables to produce a Bayes tree. If an ordering is not
* provided, the ordering will be computed using either COLAMD or METIS, dependeing on
@ -151,8 +151,8 @@ namespace gtsam {
boost::shared_ptr<BayesTreeType> eliminateMultifrontal(
OptionalOrdering ordering = boost::none,
const Eliminate& function = EliminationTraitsType::DefaultEliminate,
OptionalVariableIndex variableIndex = boost::none,
OptionalOrderingType orderingType = boost::none) const;
OptionalVariableIndex variableIndex = boost::none,
OptionalOrderingType orderingType = boost::none) const;
/** Do sequential elimination of some variables, in \c ordering provided, to produce a Bayes net
* and a remaining factor graph. This computes the factorization \f$ p(X) = p(A|B) p(B) \f$,

View File

@ -349,11 +349,11 @@ void JacobianFactor::print(const string& s,
const KeyFormatter& formatter) const {
static const Eigen::IOFormat matlab(
Eigen::StreamPrecision, // precision
0, // flags
0, // flags
" ", // coeffSeparator
";\n", // rowSeparator
"\t", // rowPrefix
"", // rowSuffix
"\t", // rowPrefix
"", // rowSuffix
"[\n", // matPrefix
"\n ]" // matSuffix
);

View File

@ -110,8 +110,8 @@ VectorValues NonlinearOptimizer::solve(const GaussianFactorGraph &gfg,
delta = gfg.optimize(*params.ordering, params.getEliminationFunction());
} else if (params.isSequential()) {
// Sequential QR or Cholesky (decided by params.getEliminationFunction())
delta = gfg.eliminateSequential(*params.ordering, params.getEliminationFunction(),
boost::none, params.orderingType)->optimize();
delta = gfg.eliminateSequential(*params.ordering,
params.getEliminationFunction(), boost::none, params.orderingType)->optimize();
} else if (params.isIterative()) {
// Conjugate Gradient -> needs params.iterativeParams

View File

@ -110,14 +110,14 @@ void NonlinearOptimizerParams::print(const std::string& str) const {
switch (orderingType){
case Ordering::COLAMD:
std::cout << " ordering: COLAMD\n";
break;
std::cout << " ordering: COLAMD\n";
break;
case Ordering::METIS:
std::cout << " ordering: METIS\n";
break;
std::cout << " ordering: METIS\n";
break;
default:
std::cout << " ordering: custom\n";
break;
std::cout << " ordering: custom\n";
break;
}
std::cout.flush();
@ -165,29 +165,31 @@ NonlinearOptimizerParams::LinearSolverType NonlinearOptimizerParams::linearSolve
}
/* ************************************************************************* */
std::string NonlinearOptimizerParams::orderingTypeTranslator(Ordering::OrderingType type) const{
switch (type) {
case Ordering::METIS:
return "METIS";
case Ordering::COLAMD:
return "COLAMD";
default:
if (ordering)
return "CUSTOM";
else
throw std::invalid_argument(
"Invalid ordering type: You must provide an ordering for a custom ordering type. See setOrdering");
}
std::string NonlinearOptimizerParams::orderingTypeTranslator(
Ordering::OrderingType type) const {
switch (type) {
case Ordering::METIS:
return "METIS";
case Ordering::COLAMD:
return "COLAMD";
default:
if (ordering)
return "CUSTOM";
else
throw std::invalid_argument(
"Invalid ordering type: You must provide an ordering for a custom ordering type. See setOrdering");
}
}
/* ************************************************************************* */
Ordering::OrderingType NonlinearOptimizerParams::orderingTypeTranslator(const std::string& type) const{
if (type == "METIS")
return Ordering::METIS;
if (type == "COLAMD")
return Ordering::COLAMD;
throw std::invalid_argument(
"Invalid ordering type: You must provide an ordering for a custom ordering type. See setOrdering");
Ordering::OrderingType NonlinearOptimizerParams::orderingTypeTranslator(
const std::string& type) const {
if (type == "METIS")
return Ordering::METIS;
if (type == "COLAMD")
return Ordering::COLAMD;
throw std::invalid_argument(
"Invalid ordering type: You must provide an ordering for a custom ordering type. See setOrdering");
}

View File

@ -154,16 +154,16 @@ public:
void setOrdering(const Ordering& ordering) {
this->ordering = ordering;
this->orderingType = Ordering::CUSTOM;
this->orderingType = Ordering::CUSTOM;
}
std::string getOrderingType() const {
return orderingTypeTranslator(orderingType);
return orderingTypeTranslator(orderingType);
}
// Note that if you want to use a custom ordering, you must set the ordering directly, this will switch to custom type
void setOrderingType(const std::string& ordering){
orderingType = orderingTypeTranslator(ordering);
orderingType = orderingTypeTranslator(ordering);
}
private: