commit
e43aca3333
|
@ -83,7 +83,7 @@ vector<RangeTriple> readTriples() {
|
||||||
|
|
||||||
while (is) {
|
while (is) {
|
||||||
double t, sender, range;
|
double t, sender, range;
|
||||||
size_t receiver;
|
size_t receiver;
|
||||||
is >> t >> sender >> receiver >> range;
|
is >> t >> sender >> receiver >> range;
|
||||||
triples.push_back(RangeTriple(t, receiver, range));
|
triples.push_back(RangeTriple(t, receiver, range));
|
||||||
}
|
}
|
||||||
|
|
|
@ -189,7 +189,7 @@ void print(const Matrix& A, const string &s, ostream& stream) {
|
||||||
0, // flags
|
0, // flags
|
||||||
" ", // coeffSeparator
|
" ", // coeffSeparator
|
||||||
";\n", // rowSeparator
|
";\n", // rowSeparator
|
||||||
" \t", // rowPrefix
|
" \t", // rowPrefix
|
||||||
"", // rowSuffix
|
"", // rowSuffix
|
||||||
"[\n", // matPrefix
|
"[\n", // matPrefix
|
||||||
"\n ]" // matSuffix
|
"\n ]" // matSuffix
|
||||||
|
|
|
@ -28,8 +28,8 @@ namespace gtsam {
|
||||||
template<class FACTORGRAPH>
|
template<class FACTORGRAPH>
|
||||||
boost::shared_ptr<typename EliminateableFactorGraph<FACTORGRAPH>::BayesNetType>
|
boost::shared_ptr<typename EliminateableFactorGraph<FACTORGRAPH>::BayesNetType>
|
||||||
EliminateableFactorGraph<FACTORGRAPH>::eliminateSequential(
|
EliminateableFactorGraph<FACTORGRAPH>::eliminateSequential(
|
||||||
OptionalOrdering ordering, const Eliminate& function,
|
OptionalOrdering ordering, const Eliminate& function,
|
||||||
OptionalVariableIndex variableIndex, OptionalOrderingType orderingType) const
|
OptionalVariableIndex variableIndex, OptionalOrderingType orderingType) const
|
||||||
{
|
{
|
||||||
if(ordering && variableIndex) {
|
if(ordering && variableIndex) {
|
||||||
gttic(eliminateSequential);
|
gttic(eliminateSequential);
|
||||||
|
@ -65,8 +65,8 @@ namespace gtsam {
|
||||||
template<class FACTORGRAPH>
|
template<class FACTORGRAPH>
|
||||||
boost::shared_ptr<typename EliminateableFactorGraph<FACTORGRAPH>::BayesTreeType>
|
boost::shared_ptr<typename EliminateableFactorGraph<FACTORGRAPH>::BayesTreeType>
|
||||||
EliminateableFactorGraph<FACTORGRAPH>::eliminateMultifrontal(
|
EliminateableFactorGraph<FACTORGRAPH>::eliminateMultifrontal(
|
||||||
OptionalOrdering ordering, const Eliminate& function,
|
OptionalOrdering ordering, const Eliminate& function,
|
||||||
OptionalVariableIndex variableIndex, OptionalOrderingType orderingType) const
|
OptionalVariableIndex variableIndex, OptionalOrderingType orderingType) const
|
||||||
{
|
{
|
||||||
if(ordering && variableIndex) {
|
if(ordering && variableIndex) {
|
||||||
gttic(eliminateMultifrontal);
|
gttic(eliminateMultifrontal);
|
||||||
|
@ -86,16 +86,16 @@ namespace gtsam {
|
||||||
// If no VariableIndex provided, compute one and call this function again IMPORTANT: we check
|
// 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
|
// for no variable index first so that it's always computed if we need to call COLAMD because
|
||||||
// no Ordering is provided.
|
// no Ordering is provided.
|
||||||
return eliminateMultifrontal(ordering, function, VariableIndex(asDerived()), orderingType);
|
return eliminateMultifrontal(ordering, function, VariableIndex(asDerived()), orderingType);
|
||||||
}
|
}
|
||||||
else /*if(!ordering)*/ {
|
else /*if(!ordering)*/ {
|
||||||
// If no Ordering provided, compute one and call this function again. We are guaranteed to
|
// 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'
|
// have a VariableIndex already here because we computed one if needed in the previous 'else'
|
||||||
// block.
|
// block.
|
||||||
if (orderingType == Ordering::METIS)
|
if (orderingType == Ordering::METIS)
|
||||||
return eliminateMultifrontal(Ordering::Metis(asDerived()), function, variableIndex, orderingType);
|
return eliminateMultifrontal(Ordering::Metis(asDerived()), function, variableIndex, orderingType);
|
||||||
else
|
else
|
||||||
return eliminateMultifrontal(Ordering::Colamd(*variableIndex), function, variableIndex, orderingType);
|
return eliminateMultifrontal(Ordering::Colamd(*variableIndex), function, variableIndex, orderingType);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -94,8 +94,8 @@ namespace gtsam {
|
||||||
/// Typedef for an optional variable index as an argument to elimination functions
|
/// Typedef for an optional variable index as an argument to elimination functions
|
||||||
typedef boost::optional<const VariableIndex&> OptionalVariableIndex;
|
typedef boost::optional<const VariableIndex&> OptionalVariableIndex;
|
||||||
|
|
||||||
/// Typedef for an optional ordering type
|
/// Typedef for an optional ordering type
|
||||||
typedef boost::optional<Ordering::OrderingType> OptionalOrderingType;
|
typedef boost::optional<Ordering::OrderingType> OptionalOrderingType;
|
||||||
|
|
||||||
/** Do sequential elimination of all variables to produce a Bayes net. If an ordering is not
|
/** 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.
|
* provided, the ordering provided by COLAMD will be used.
|
||||||
|
@ -104,10 +104,10 @@ namespace gtsam {
|
||||||
* \code
|
* \code
|
||||||
* boost::shared_ptr<GaussianBayesNet> result = graph.eliminateSequential(EliminateCholesky);
|
* boost::shared_ptr<GaussianBayesNet> result = graph.eliminateSequential(EliminateCholesky);
|
||||||
* \endcode
|
* \endcode
|
||||||
*
|
*
|
||||||
* <b> Example - METIS ordering for elimination
|
* <b> Example - METIS ordering for elimination
|
||||||
* \code
|
* \code
|
||||||
* boost::shared_ptr<GaussianBayesNet> result = graph.eliminateSequential(OrderingType::METIS);
|
* boost::shared_ptr<GaussianBayesNet> result = graph.eliminateSequential(OrderingType::METIS);
|
||||||
*
|
*
|
||||||
* <b> Example - Full QR elimination in specified order:
|
* <b> Example - Full QR elimination in specified order:
|
||||||
* \code
|
* \code
|
||||||
|
@ -125,7 +125,7 @@ namespace gtsam {
|
||||||
OptionalOrdering ordering = boost::none,
|
OptionalOrdering ordering = boost::none,
|
||||||
const Eliminate& function = EliminationTraitsType::DefaultEliminate,
|
const Eliminate& function = EliminationTraitsType::DefaultEliminate,
|
||||||
OptionalVariableIndex variableIndex = boost::none,
|
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
|
/** 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
|
* provided, the ordering will be computed using either COLAMD or METIS, dependeing on
|
||||||
|
@ -151,8 +151,8 @@ namespace gtsam {
|
||||||
boost::shared_ptr<BayesTreeType> eliminateMultifrontal(
|
boost::shared_ptr<BayesTreeType> eliminateMultifrontal(
|
||||||
OptionalOrdering ordering = boost::none,
|
OptionalOrdering ordering = boost::none,
|
||||||
const Eliminate& function = EliminationTraitsType::DefaultEliminate,
|
const Eliminate& function = EliminationTraitsType::DefaultEliminate,
|
||||||
OptionalVariableIndex variableIndex = boost::none,
|
OptionalVariableIndex variableIndex = boost::none,
|
||||||
OptionalOrderingType orderingType = boost::none) const;
|
OptionalOrderingType orderingType = boost::none) const;
|
||||||
|
|
||||||
/** Do sequential elimination of some variables, in \c ordering provided, to produce a Bayes net
|
/** 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$,
|
* and a remaining factor graph. This computes the factorization \f$ p(X) = p(A|B) p(B) \f$,
|
||||||
|
|
|
@ -349,11 +349,11 @@ void JacobianFactor::print(const string& s,
|
||||||
const KeyFormatter& formatter) const {
|
const KeyFormatter& formatter) const {
|
||||||
static const Eigen::IOFormat matlab(
|
static const Eigen::IOFormat matlab(
|
||||||
Eigen::StreamPrecision, // precision
|
Eigen::StreamPrecision, // precision
|
||||||
0, // flags
|
0, // flags
|
||||||
" ", // coeffSeparator
|
" ", // coeffSeparator
|
||||||
";\n", // rowSeparator
|
";\n", // rowSeparator
|
||||||
"\t", // rowPrefix
|
"\t", // rowPrefix
|
||||||
"", // rowSuffix
|
"", // rowSuffix
|
||||||
"[\n", // matPrefix
|
"[\n", // matPrefix
|
||||||
"\n ]" // matSuffix
|
"\n ]" // matSuffix
|
||||||
);
|
);
|
||||||
|
|
|
@ -110,8 +110,8 @@ VectorValues NonlinearOptimizer::solve(const GaussianFactorGraph &gfg,
|
||||||
delta = gfg.optimize(*params.ordering, params.getEliminationFunction());
|
delta = gfg.optimize(*params.ordering, params.getEliminationFunction());
|
||||||
} else if (params.isSequential()) {
|
} else if (params.isSequential()) {
|
||||||
// Sequential QR or Cholesky (decided by params.getEliminationFunction())
|
// Sequential QR or Cholesky (decided by params.getEliminationFunction())
|
||||||
delta = gfg.eliminateSequential(*params.ordering, params.getEliminationFunction(),
|
delta = gfg.eliminateSequential(*params.ordering,
|
||||||
boost::none, params.orderingType)->optimize();
|
params.getEliminationFunction(), boost::none, params.orderingType)->optimize();
|
||||||
} else if (params.isIterative()) {
|
} else if (params.isIterative()) {
|
||||||
|
|
||||||
// Conjugate Gradient -> needs params.iterativeParams
|
// Conjugate Gradient -> needs params.iterativeParams
|
||||||
|
|
|
@ -110,14 +110,14 @@ void NonlinearOptimizerParams::print(const std::string& str) const {
|
||||||
|
|
||||||
switch (orderingType){
|
switch (orderingType){
|
||||||
case Ordering::COLAMD:
|
case Ordering::COLAMD:
|
||||||
std::cout << " ordering: COLAMD\n";
|
std::cout << " ordering: COLAMD\n";
|
||||||
break;
|
break;
|
||||||
case Ordering::METIS:
|
case Ordering::METIS:
|
||||||
std::cout << " ordering: METIS\n";
|
std::cout << " ordering: METIS\n";
|
||||||
break;
|
break;
|
||||||
default:
|
default:
|
||||||
std::cout << " ordering: custom\n";
|
std::cout << " ordering: custom\n";
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
|
||||||
std::cout.flush();
|
std::cout.flush();
|
||||||
|
@ -165,29 +165,31 @@ NonlinearOptimizerParams::LinearSolverType NonlinearOptimizerParams::linearSolve
|
||||||
}
|
}
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
std::string NonlinearOptimizerParams::orderingTypeTranslator(Ordering::OrderingType type) const{
|
std::string NonlinearOptimizerParams::orderingTypeTranslator(
|
||||||
switch (type) {
|
Ordering::OrderingType type) const {
|
||||||
case Ordering::METIS:
|
switch (type) {
|
||||||
return "METIS";
|
case Ordering::METIS:
|
||||||
case Ordering::COLAMD:
|
return "METIS";
|
||||||
return "COLAMD";
|
case Ordering::COLAMD:
|
||||||
default:
|
return "COLAMD";
|
||||||
if (ordering)
|
default:
|
||||||
return "CUSTOM";
|
if (ordering)
|
||||||
else
|
return "CUSTOM";
|
||||||
throw std::invalid_argument(
|
else
|
||||||
"Invalid ordering type: You must provide an ordering for a custom ordering type. See setOrdering");
|
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{
|
Ordering::OrderingType NonlinearOptimizerParams::orderingTypeTranslator(
|
||||||
if (type == "METIS")
|
const std::string& type) const {
|
||||||
return Ordering::METIS;
|
if (type == "METIS")
|
||||||
if (type == "COLAMD")
|
return Ordering::METIS;
|
||||||
return Ordering::COLAMD;
|
if (type == "COLAMD")
|
||||||
throw std::invalid_argument(
|
return Ordering::COLAMD;
|
||||||
"Invalid ordering type: You must provide an ordering for a custom ordering type. See setOrdering");
|
throw std::invalid_argument(
|
||||||
|
"Invalid ordering type: You must provide an ordering for a custom ordering type. See setOrdering");
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
|
@ -154,16 +154,16 @@ public:
|
||||||
|
|
||||||
void setOrdering(const Ordering& ordering) {
|
void setOrdering(const Ordering& ordering) {
|
||||||
this->ordering = ordering;
|
this->ordering = ordering;
|
||||||
this->orderingType = Ordering::CUSTOM;
|
this->orderingType = Ordering::CUSTOM;
|
||||||
}
|
}
|
||||||
|
|
||||||
std::string getOrderingType() const {
|
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
|
// 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){
|
void setOrderingType(const std::string& ordering){
|
||||||
orderingType = orderingTypeTranslator(ordering);
|
orderingType = orderingTypeTranslator(ordering);
|
||||||
}
|
}
|
||||||
|
|
||||||
private:
|
private:
|
||||||
|
|
Loading…
Reference in New Issue