/* ************************************************************************* */ 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; }