C++ Ordering::push_back方法代码示例

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

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

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