C++ Ordering类代码示例

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

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

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