/* ************************************************************************* */
TEST(NonlinearOptimizer, NullFactor) {
example::Graph fg = example::createReallyNonlinearFactorGraph();
// Add null factor
fg.push_back(example::Graph::sharedFactor());
// test error at minimum
Point2 xstar(0,0);
Values cstar;
cstar.insert(X(1), xstar);
DOUBLES_EQUAL(0.0,fg.error(cstar),0.0);
// test error at initial = [(1-cos(3))^2 + (sin(3))^2]*50 =
Point2 x0(3,3);
Values c0;
c0.insert(X(1), x0);
DOUBLES_EQUAL(199.0,fg.error(c0),1e-3);
// optimize parameters
Ordering ord;
ord.push_back(X(1));
// Gauss-Newton
Values actual1 = GaussNewtonOptimizer(fg, c0, ord).optimize();
DOUBLES_EQUAL(0,fg.error(actual1),tol);
// Levenberg-Marquardt
Values actual2 = LevenbergMarquardtOptimizer(fg, c0, ord).optimize();
DOUBLES_EQUAL(0,fg.error(actual2),tol);
// Dogleg
Values actual3 = DoglegOptimizer(fg, c0, ord).optimize();
DOUBLES_EQUAL(0,fg.error(actual3),tol);
}
/* ************************************************************************* */
TEST ( NonlinearEquality, allow_error_optimize ) {
Symbol key1('x',1);
Pose2 feasible1(1.0, 2.0, 3.0);
double error_gain = 500.0;
PoseNLE nle(key1, feasible1, error_gain);
// add to a graph
NonlinearFactorGraph graph;
graph.add(nle);
// initialize away from the ideal
Pose2 initPose(0.0, 2.0, 3.0);
Values init;
init.insert(key1, initPose);
// optimize
Ordering ordering;
ordering.push_back(key1);
Values result = LevenbergMarquardtOptimizer(graph, init, ordering).optimize();
// verify
Values expected;
expected.insert(key1, feasible1);
EXPECT(assert_equal(expected, result));
}
/* ************************************************************************* */
std::pair<DiscreteConditional::shared_ptr, DecisionTreeFactor::shared_ptr> //
EliminateDiscrete(const DiscreteFactorGraph& factors, const Ordering& frontalKeys) {
// PRODUCT: multiply all factors
gttic(product);
DecisionTreeFactor product;
BOOST_FOREACH(const DiscreteFactor::shared_ptr& factor, factors)
product = (*factor) * product;
gttoc(product);
// sum out frontals, this is the factor on the separator
gttic(sum);
DecisionTreeFactor::shared_ptr sum = product.sum(frontalKeys);
gttoc(sum);
// Ordering keys for the conditional so that frontalKeys are really in front
Ordering orderedKeys;
orderedKeys.insert(orderedKeys.end(), frontalKeys.begin(), frontalKeys.end());
orderedKeys.insert(orderedKeys.end(), sum->keys().begin(), sum->keys().end());
// now divide product/sum to get conditional
gttic(divide);
DiscreteConditional::shared_ptr cond(new DiscreteConditional(product, *sum, orderedKeys));
gttoc(divide);
return std::make_pair(cond, sum);
}
/* ************************************************************************* */
int main() {
gtsam::Key PoseKey1(11);
gtsam::Key PoseKey2(12);
gtsam::Key VelKey1(21);
gtsam::Key VelKey2(22);
gtsam::Key BiasKey1(31);
double measurement_dt(0.1);
Vector world_g(Vector3(0.0, 0.0, 9.81));
Vector world_rho(Vector3(0.0, -1.5724e-05, 0.0)); // NED system
gtsam::Vector ECEF_omega_earth(Vector3(0.0, 0.0, 7.292115e-5));
gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth);
SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1));
// Second test: zero angular motion, some acceleration - generated in matlab
Vector measurement_acc(Vector3(6.501390843381716, -6.763926150509185, -2.300389940090343));
Vector measurement_gyro(Vector3(0.1, 0.2, 0.3));
InertialNavFactor_GlobalVelocity<Pose3, Vector3, imuBias::ConstantBias> f(PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model);
Rot3 R1(0.487316618, 0.125253866, 0.86419557,
0.580273724, 0.693095498, -0.427669306,
-0.652537293, 0.709880342, 0.265075427);
Point3 t1(2.0,1.0,3.0);
Pose3 Pose1(R1, t1);
Vector3 Vel1 = Vector(Vector3(0.5,-0.5,0.4));
Rot3 R2(0.473618898, 0.119523052, 0.872582019,
0.609241153, 0.67099888, -0.422594037,
-0.636011287, 0.731761397, 0.244979388);
Point3 t2 = t1.compose( Point3(Vel1*measurement_dt) );
Pose3 Pose2(R2, t2);
Vector dv = measurement_dt * (R1.matrix() * measurement_acc + world_g);
Vector3 Vel2 = Vel1 + dv;
imuBias::ConstantBias Bias1;
Values values;
values.insert(PoseKey1, Pose1);
values.insert(PoseKey2, Pose2);
values.insert(VelKey1, Vel1);
values.insert(VelKey2, Vel2);
values.insert(BiasKey1, Bias1);
Ordering ordering;
ordering.push_back(PoseKey1);
ordering.push_back(VelKey1);
ordering.push_back(BiasKey1);
ordering.push_back(PoseKey2);
ordering.push_back(VelKey2);
GaussianFactorGraph graph;
gttic_(LinearizeTiming);
for(size_t i = 0; i < 100000; ++i) {
GaussianFactor::shared_ptr g = f.linearize(values);
graph.push_back(g);
}
gttoc_(LinearizeTiming);
tictoc_finishedIteration_();
tictoc_print_();
}
/* ************************************************************************* */
TEST ( NonlinearEquality, allow_error_optimize_with_factors ) {
// create a hard constraint
Symbol key1('x',1);
Pose2 feasible1(1.0, 2.0, 3.0);
// initialize away from the ideal
Values init;
Pose2 initPose(0.0, 2.0, 3.0);
init.insert(key1, initPose);
double error_gain = 500.0;
PoseNLE nle(key1, feasible1, error_gain);
// create a soft prior that conflicts
PosePrior prior(key1, initPose, noiseModel::Isotropic::Sigma(3, 0.1));
// add to a graph
NonlinearFactorGraph graph;
graph.add(nle);
graph.add(prior);
// optimize
Ordering ordering;
ordering.push_back(key1);
Values actual = LevenbergMarquardtOptimizer(graph, init, ordering).optimize();
// verify
Values expected;
expected.insert(key1, feasible1);
EXPECT(assert_equal(expected, actual));
}
/* ************************************************************************* */
JointMarginal Marginals::jointMarginalInformation(const std::vector<Key>& variables) const {
// If 2 variables, we can use the BayesTree::joint function, otherwise we
// have to use sequential elimination.
if(variables.size() == 1) {
Matrix info = marginalInformation(variables.front());
std::vector<size_t> dims;
dims.push_back(info.rows());
Ordering indices;
indices.insert(variables.front(), 0);
return JointMarginal(info, dims, indices);
} else {
// Obtain requested variables as ordered indices
vector<Index> indices(variables.size());
for(size_t i=0; i<variables.size(); ++i) { indices[i] = ordering_[variables[i]]; }
// Compute joint marginal factor graph.
GaussianFactorGraph jointFG;
if(variables.size() == 2) {
if(factorization_ == CHOLESKY)
jointFG = *bayesTree_.joint(indices[0], indices[1], EliminatePreferCholesky);
else if(factorization_ == QR)
jointFG = *bayesTree_.joint(indices[0], indices[1], EliminateQR);
} else {
if(factorization_ == CHOLESKY)
jointFG = *GaussianSequentialSolver(graph_, false).jointFactorGraph(indices);
else if(factorization_ == QR)
jointFG = *GaussianSequentialSolver(graph_, true).jointFactorGraph(indices);
}
// Build map from variable keys to position in factor graph variables,
// which are sorted in index order.
Ordering variableConversion;
{
// First build map from index to key
FastMap<Index,Key> usedIndices;
for(size_t i=0; i<variables.size(); ++i)
usedIndices.insert(make_pair(indices[i], variables[i]));
// Next run over indices in sorted order
size_t slot = 0;
typedef pair<Index,Key> Index_Key;
BOOST_FOREACH(const Index_Key& index_key, usedIndices) {
variableConversion.insert(index_key.second, slot);
++ slot;
}
}
// Get dimensions from factor graph
std::vector<size_t> dims(indices.size(), 0);
BOOST_FOREACH(Key key, variables) {
dims[variableConversion[key]] = values_.at(key).dim();
}
int KeyV1::woCompare(const KeyV1& right, const Ordering &order) const {
const unsigned char *l = _keyData;
const unsigned char *r = right._keyData;
if( (*l|*r) == IsBSON ) // only can do this if cNOTUSED maintained
return compareHybrid(right, order);
unsigned mask = 1;
while( 1 ) {
char lval = *l;
char rval = *r;
{
int x = compare(l, r); // updates l and r pointers
if( x ) {
if( order.descending(mask) )
x = -x;
return x;
}
}
{
int x = ((int)(lval & cHASMORE)) - ((int)(rval & cHASMORE));
if( x )
return x;
if( (lval & cHASMORE) == 0 )
break;
}
mask <<= 1;
}
return 0;
}
// pre signed dates & such
int oldCompare(const BSONObj& l,const BSONObj& r, const Ordering &o) {
BSONObjIterator i(l);
BSONObjIterator j(r);
unsigned mask = 1;
while ( 1 ) {
// so far, equal...
BSONElement l = i.next();
BSONElement r = j.next();
if ( l.eoo() )
return r.eoo() ? 0 : -1;
if ( r.eoo() )
return 1;
int x;
{
x = oldElemCompare(l, r);
if( o.descending(mask) )
x = -x;
}
if ( x != 0 )
return x;
mask <<= 1;
}
return -1;
}
int BSONObj::woCompare(const BSONObj& r, const Ordering &o, bool considerFieldName) const {
if ( isEmpty() )
return r.isEmpty() ? 0 : -1;
if ( r.isEmpty() )
return 1;
BSONObjIterator i(*this);
BSONObjIterator j(r);
unsigned mask = 1;
while ( 1 ) {
// so far, equal...
BSONElement l = i.next();
BSONElement r = j.next();
if ( l.eoo() )
return r.eoo() ? 0 : -1;
if ( r.eoo() )
return 1;
int x;
{
x = l.woCompare( r, considerFieldName );
if( o.descending(mask) )
x = -x;
}
if ( x != 0 )
return x;
mask <<= 1;
}
return -1;
}
/* ************************************************************************* */
TEST( NonlinearOptimizer, Factorization )
{
Values config;
config.insert(X(1), Pose2(0.,0.,0.));
config.insert(X(2), Pose2(1.5,0.,0.));
NonlinearFactorGraph graph;
graph.add(PriorFactor<Pose2>(X(1), Pose2(0.,0.,0.), noiseModel::Isotropic::Sigma(3, 1e-10)));
graph.add(BetweenFactor<Pose2>(X(1),X(2), Pose2(1.,0.,0.), noiseModel::Isotropic::Sigma(3, 1)));
Ordering ordering;
ordering.push_back(X(1));
ordering.push_back(X(2));
LevenbergMarquardtOptimizer optimizer(graph, config, ordering);
optimizer.iterate();
Values expected;
expected.insert(X(1), Pose2(0.,0.,0.));
expected.insert(X(2), Pose2(1.,0.,0.));
CHECK(assert_equal(expected, optimizer.values(), 1e-5));
}
/* ************************************************************************* */
TEST(NonlinearClusterTree, Clusters) {
NonlinearFactorGraph graph = planarSLAMGraph();
Values initial = planarSLAMValues();
// Build the clusters
// NOTE(frank): Order matters here as factors are removed!
VariableIndex variableIndex(graph);
typedef NonlinearClusterTree::NonlinearCluster Cluster;
auto marginalCluster = boost::shared_ptr<Cluster>(new Cluster(variableIndex, {x1}, &graph));
auto landmarkCluster = boost::shared_ptr<Cluster>(new Cluster(variableIndex, {l1, l2}, &graph));
auto rootCluster = boost::shared_ptr<Cluster>(new Cluster(variableIndex, {x2, x3}, &graph));
EXPECT_LONGS_EQUAL(3, marginalCluster->nrFactors());
EXPECT_LONGS_EQUAL(2, landmarkCluster->nrFactors());
EXPECT_LONGS_EQUAL(1, rootCluster->nrFactors());
EXPECT_LONGS_EQUAL(1, marginalCluster->nrFrontals());
EXPECT_LONGS_EQUAL(2, landmarkCluster->nrFrontals());
EXPECT_LONGS_EQUAL(2, rootCluster->nrFrontals());
// Test linearize
auto gfg = marginalCluster->linearize(initial);
EXPECT_LONGS_EQUAL(3, gfg->size());
// Calculate expected result of only evaluating the marginalCluster
Ordering ordering;
ordering.push_back(x1);
auto expected = gfg->eliminatePartialSequential(ordering);
auto expectedFactor = boost::dynamic_pointer_cast<HessianFactor>(expected.second->at(0));
if (!expectedFactor)
throw std::runtime_error("Expected HessianFactor");
// Linearize and eliminate the marginalCluster
auto actual = marginalCluster->linearizeAndEliminate(initial);
const GaussianBayesNet& bayesNet = actual.first;
const HessianFactor& factor = *actual.second;
EXPECT(assert_equal(*expected.first->at(0), *bayesNet.at(0), 1e-6));
EXPECT(assert_equal(*expectedFactor, factor, 1e-6));
}
/* ************************************************************************* */
TEST( NonlinearOptimizer, optimize )
{
example::Graph fg(example::createReallyNonlinearFactorGraph());
// test error at minimum
Point2 xstar(0,0);
Values cstar;
cstar.insert(X(1), xstar);
DOUBLES_EQUAL(0.0,fg.error(cstar),0.0);
// test error at initial = [(1-cos(3))^2 + (sin(3))^2]*50 =
Point2 x0(3,3);
Values c0;
c0.insert(X(1), x0);
DOUBLES_EQUAL(199.0,fg.error(c0),1e-3);
// optimize parameters
Ordering ord;
ord.push_back(X(1));
// Gauss-Newton
GaussNewtonParams gnParams;
gnParams.ordering = ord;
Values actual1 = GaussNewtonOptimizer(fg, c0, gnParams).optimize();
DOUBLES_EQUAL(0,fg.error(actual1),tol);
// Levenberg-Marquardt
LevenbergMarquardtParams lmParams;
lmParams.ordering = ord;
Values actual2 = LevenbergMarquardtOptimizer(fg, c0, lmParams).optimize();
DOUBLES_EQUAL(0,fg.error(actual2),tol);
// Dogleg
DoglegParams dlParams;
dlParams.ordering = ord;
Values actual3 = DoglegOptimizer(fg, c0, dlParams).optimize();
DOUBLES_EQUAL(0,fg.error(actual3),tol);
}
/* ************************************************************************* */
Ordering Ordering::Metis(const MetisIndex& met) {
#ifdef GTSAM_SUPPORT_NESTED_DISSECTION
gttic(Ordering_METIS);
vector<idx_t> xadj = met.xadj();
vector<idx_t> adj = met.adj();
vector<idx_t> perm, iperm;
idx_t size = met.nValues();
for (idx_t i = 0; i < size; i++) {
perm.push_back(0);
iperm.push_back(0);
}
int outputError;
outputError = METIS_NodeND(&size, &xadj[0], &adj[0], NULL, NULL, &perm[0],
&iperm[0]);
Ordering result;
if (outputError != METIS_OK) {
std::cout << "METIS failed during Nested Dissection ordering!\n";
return result;
}
result.resize(size);
for (size_t j = 0; j < (size_t) size; ++j) {
// We have to add the minKey value back to obtain the original key in the Values
result[j] = met.intToKey(perm[j]);
}
return result;
#else
throw runtime_error("GTSAM was built without support for Metis-based "
"nested dissection");
#endif
}
void testPermutation(const std::vector<BSONObj>& elementsOrig,
const std::vector<BSONObj>& orderings,
bool debug) {
// Since KeyStrings are compared using memcmp we can assume it provides a total ordering such
// that there won't be cases where (a < b && b < c && !(a < c)). This test still needs to ensure
// that it provides the *correct* total ordering.
for (size_t k = 0; k < orderings.size(); k++) {
BSONObj orderObj = orderings[k];
Ordering ordering = Ordering::make(orderObj);
if (debug)
log() << "ordering: " << orderObj;
std::vector<BSONObj> elements = elementsOrig;
std::stable_sort(elements.begin(), elements.end(), BSONObjCmp(orderObj));
for (size_t i = 0; i < elements.size(); i++) {
const BSONObj& o1 = elements[i];
if (debug)
log() << "\to1: " << o1;
ROUNDTRIP_ORDER(o1, ordering);
KeyString k1(o1, ordering);
KeyString l1(BSON("l" << o1.firstElement()), ordering); // kLess
KeyString g1(BSON("g" << o1.firstElement()), ordering); // kGreater
ASSERT_LT(l1, k1);
ASSERT_GT(g1, k1);
if (i + 1 < elements.size()) {
const BSONObj& o2 = elements[i + 1];
if (debug)
log() << "\t\t o2: " << o2;
KeyString k2(o2, ordering);
KeyString g2(BSON("g" << o2.firstElement()), ordering);
KeyString l2(BSON("l" << o2.firstElement()), ordering);
int bsonCmp = o1.woCompare(o2, ordering);
invariant(bsonCmp <= 0); // We should be sorted...
if (bsonCmp == 0) {
ASSERT_EQ(k1, k2);
} else {
ASSERT_LT(k1, k2);
}
// Test the query encodings using kLess and kGreater
int firstElementComp = o1.firstElement().woCompare(o2.firstElement());
if (ordering.descending(1))
firstElementComp = -firstElementComp;
invariant(firstElementComp <= 0);
if (firstElementComp == 0) {
// If they share a first element then l1/g1 should equal l2/g2 and l1 should be
// less than both and g1 should be greater than both.
ASSERT_EQ(l1, l2);
ASSERT_EQ(g1, g2);
ASSERT_LT(l1, k2);
ASSERT_GT(g1, k2);
} else {
// k1 is less than k2. Less(k2) and Greater(k1) should be between them.
ASSERT_LT(g1, k2);
ASSERT_GT(l2, k1);
}
}
}
}
}
void Plan::CalcEncoding() {
int n = mOrderings.size() + mSteps.size() + 1;
if(mA)
delete mA;
mA = new SparceMatrix(n, mSteps.size()*3 + n);
int row = 0; //The current equation
int k = 0; //The current slack variable
for(vector<Step*>::iterator it = mSteps.begin();
it != mSteps.end(); it++) {
Step* curStep = *it;
if(curStep->getAction()->LBDurration() ==
curStep->getAction()->UBDurration()) {
//If they are the same we don't need slack variables
//s_end - s_start = dur
mA->set(row, getStepCol(curStep, StepTime::End), mgr.addOne());
mA->set(row, getStepCol(curStep, StepTime::Start), -mgr.addOne());
(*mb)[row] = mgr.constant(curStep->getAction()->UBDurration());
row++;
} else {
//Other wise we do
//s_end - s_start - s_k = ub;
mA->set(row, getStepCol(curStep, StepTime::End), mgr.addOne());
mA->set(row, getStepCol(curStep, StepTime::Start), -mgr.addOne());
mA->set(row, getSlackVar(k), -mgr.addOne());
(*mb)[row] = mgr.constant(curStep->getAction()->UBDurration());
row++;
k++;
//s_start - s_end - s_k+1 = -lb;
mA->set(row, getStepCol(curStep, StepTime::Start), mgr.addOne());
mA->set(row, getStepCol(curStep, StepTime::End), -mgr.addOne());
mA->set(row, getSlackVar(k), -mgr.addOne());
(*mb)[row] = mgr.constant(curStep->getAction()->LBDurration());
row++;
k++;
}
}
for(vector<Ordering*>::iterator it = mOrderings.begin();
it != mOrderings.end(); it++) {
Ordering* curOrdering = *it;
//Post_time - Pre_time - s_k = epsilon
mA->set(row, getStepCol(curOrdering->Post(), curOrdering->PostTime()), mgr.addOne());
mA->set(row, getStepCol(curOrdering->Pre(), curOrdering->PreTime()), -mgr.addOne());
mA->set(row, getSlackVar(k), -mgr.addOne());
(*mb)[row] = mgr.constant(epsilon);
row++;
k++;
}
mA->set(row, getStepCol(FrameStep, StepTime::Start), -mgr.addOne());
mA->set(row, getStepCol(FrameStep, StepTime::End), mgr.addOne());
mA->set(row, getSlackVar(k), -mgr.addOne());
(*mb)[row] = mgr.constant(Deadline);
row++;
k++;
int index = 0;
mf.resize(k+mA->getNumCols());
for(int i = 0; i < k; i++) {
mf[index].index = getSlackVar(i);
mf[index].inverted = false;
index++;
}
for(int i = 0; i < mA->getNumCols(); i++) {
mf[i].index = i;
}
}
/* ************************************************************************* */
Ordering Ordering::COLAMDConstrained(
const VariableIndex& variableIndex, std::vector<int>& cmember)
{
gttic(Ordering_COLAMDConstrained);
gttic(Prepare);
size_t nEntries = variableIndex.nEntries(), nFactors = variableIndex.nFactors(), nVars = variableIndex.size();
// Convert to compressed column major format colamd wants it in (== MATLAB format!)
size_t Alen = ccolamd_recommended((int)nEntries, (int)nFactors, (int)nVars); /* colamd arg 3: size of the array A */
vector<int> A = vector<int>(Alen); /* colamd arg 4: row indices of A, of size Alen */
vector<int> p = vector<int>(nVars + 1); /* colamd arg 5: column pointers of A, of size n_col+1 */
// Fill in input data for COLAMD
p[0] = 0;
int count = 0;
vector<Key> keys(nVars); // Array to store the keys in the order we add them so we can retrieve them in permuted order
size_t index = 0;
BOOST_FOREACH(const VariableIndex::value_type key_factors, variableIndex) {
// Arrange factor indices into COLAMD format
const VariableIndex::Factors& column = key_factors.second;
size_t lastFactorId = numeric_limits<size_t>::max();
BOOST_FOREACH(size_t factorIndex, column) {
if(lastFactorId != numeric_limits<size_t>::max())
assert(factorIndex > lastFactorId);
A[count++] = (int)factorIndex; // copy sparse column
}
p[index+1] = count; // column j (base 1) goes from A[j-1] to A[j]-1
// Store key in array and increment index
keys[index] = key_factors.first;
++ index;
}
assert((size_t)count == variableIndex.nEntries());
//double* knobs = NULL; /* colamd arg 6: parameters (uses defaults if NULL) */
double knobs[CCOLAMD_KNOBS];
ccolamd_set_defaults(knobs);
knobs[CCOLAMD_DENSE_ROW]=-1;
knobs[CCOLAMD_DENSE_COL]=-1;
int stats[CCOLAMD_STATS]; /* colamd arg 7: colamd output statistics and error codes */
gttoc(Prepare);
// call colamd, result will be in p
/* returns (1) if successful, (0) otherwise*/
if(nVars > 0) {
gttic(ccolamd);
int rv = ccolamd((int)nFactors, (int)nVars, (int)Alen, &A[0], &p[0], knobs, stats, &cmember[0]);
if(rv != 1)
throw runtime_error((boost::format("ccolamd failed with return value %1%")%rv).str());
}
// ccolamd_report(stats);
gttic(Fill_Ordering);
// Convert elimination ordering in p to an ordering
Ordering result;
result.resize(nVars);
for(size_t j = 0; j < nVars; ++j)
result[j] = keys[p[j]];
gttoc(Fill_Ordering);
return result;
}
/* ************************************************************************* */
Ordering::Ordering(const Ordering& other) : order_(other.order_), orderingIndex_(other.size()) {
for(iterator item = order_.begin(); item != order_.end(); ++item)
orderingIndex_[item->second] = item;
}
