C++ Values类代码示例

CJValueP
CJBindFunction::
exec(CJavaScript *js, const Values &values)
{
  Values callValues;

  callValues.push_back(thisValue_);

  for (uint i = 1; i < values.size(); ++i)
    callValues.push_back(values[i]);

  for (uint i = 1; i < values_.size(); ++i)
    callValues.push_back(values_[i]);

  CJValueP res;

  if (thisValue_->isObject()) {
    js->pushThis(CJValue::cast<CJObj>(thisValue_));

    res = function_->exec(js, callValues);

    js->popThis();
  }
  else
    res = function_->exec(js, callValues);

  return res;
}

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

void GuiMedia::replaceMedia(string previousMedia, string type)
{
    auto scene = dynamic_pointer_cast<Scene>(_root.lock());

    // We get the list of all objects linked to previousMedia
    auto targetObjects = list<weak_ptr<BaseObject>>();
    for (auto& objIt : scene->_objects)
    {
        auto& object = objIt.second;
        if (!object->getSavable())
            continue;
        auto linkedObjects = object->getLinkedObjects();
        for (auto& linked : linkedObjects)
            if (linked->getName() == previousMedia)
                targetObjects.push_back(object);
    }

    Values msg;
    msg.push_back(previousMedia);
    msg.push_back(_mediaTypes[type]);
    for (const auto& weakObject : targetObjects)
    {
        if (weakObject.expired())
            continue;
        auto object = weakObject.lock();
        msg.push_back(object->getName());
    }

    setGlobal("replaceObject", msg);
}

/* ************************************************************************* */
TEST( Graph, composePoses )
{
  NonlinearFactorGraph graph;
  SharedNoiseModel cov = noiseModel::Unit::Create(3);
  Pose2 p1(1.0, 2.0, 0.3), p2(4.0, 5.0, 0.6), p3(7.0, 8.0, 0.9), p4(2.0, 2.0, 2.9);
  Pose2 p12=p1.between(p2), p23=p2.between(p3), p43=p4.between(p3);
  graph += BetweenFactor<Pose2>(1,2, p12, cov);
  graph += BetweenFactor<Pose2>(2,3, p23, cov);
  graph += BetweenFactor<Pose2>(4,3, p43, cov);

  PredecessorMap<Key> tree;
  tree.insert(1,2);
  tree.insert(2,2);
  tree.insert(3,2);
  tree.insert(4,3);

  Pose2 rootPose = p2;

  boost::shared_ptr<Values> actual = composePoses<NonlinearFactorGraph, BetweenFactor<Pose2>, Pose2, Key> (graph, tree, rootPose);

  Values expected;
  expected.insert(1, p1);
  expected.insert(2, p2);
  expected.insert(3, p3);
  expected.insert(4, p4);

  LONGS_EQUAL(4, (long)actual->size());
  CHECK(assert_equal(expected, *actual));
}

TEST(CTest, test_lpush)
{
    Values vVal;
    vVal.push_back(toString(time(NULL)));
    int64_t count = 0;
    EXPECT_EQ(true, redisClient.lpush("list_test", vVal, count));
}

/* ************************************************************************* */
TEST( testNonlinearEqualityConstraint, unary_simple_optimization ) {
	// create a single-node graph with a soft and hard constraint to
	// ensure that the hard constraint overrides the soft constraint
	Point2 truth_pt(1.0, 2.0);
  Symbol key('x',1);
	double mu = 10.0;
	eq2D::UnaryEqualityConstraint::shared_ptr constraint(
			new eq2D::UnaryEqualityConstraint(truth_pt, key, mu));

	Point2 badPt(100.0, -200.0);
	simulated2D::Prior::shared_ptr factor(
			new simulated2D::Prior(badPt, soft_model, key));

	NonlinearFactorGraph graph;
	graph.push_back(constraint);
	graph.push_back(factor);

	Values initValues;
	initValues.insert(key, badPt);

	// verify error values
	EXPECT(constraint->active(initValues));

	Values expected;
	expected.insert(key, truth_pt);
	EXPECT(constraint->active(expected));
	EXPECT_DOUBLES_EQUAL(0.0, constraint->error(expected), tol);

	Values actual = LevenbergMarquardtOptimizer(graph, initValues).optimize();
	EXPECT(assert_equal(expected, actual, tol));
}

//Nullary Method
TEST(Expression, NullaryMethod) {

  // Create expression
  Expression<Point3> p(67);
  Expression<double> norm(p, &Point3::norm);

  // Create Values
  Values values;
  values.insert(67, Point3(3, 4, 5));

  // Check dims as map
  std::map<Key, int> map;
  norm.dims(map);
  LONGS_EQUAL(1, map.size());

  // Get value and Jacobians
  std::vector<Matrix> H(1);
  double actual = norm.value(values, H);

  // Check all
  EXPECT(actual == sqrt(50));
  Matrix expected(1, 3);
  expected << 3.0 / sqrt(50.0), 4.0 / sqrt(50.0), 5.0 / sqrt(50.0);
  EXPECT(assert_equal(expected, H[0]));
}

void LIRGenerator::do_NewMultiArray(NewMultiArray* x) {
  // make sure registers are spilled
  spill_values_on_stack(x->state());
  if (x->state_before() != NULL) {
    spill_values_on_stack(x->state_before());
  }
  Values* dims = x->dims();
  int i = dims->length();
  LIRItemList* items = new LIRItemList();
  while (i-- > 0) {
    LIRItem* size = new LIRItem(dims->at(i), this);
    items->at_put(i, size);
    assert(!size->is_register() || x->state_before() == NULL, "shouldn't be since it was spilled above");
  }

  // need to get the info before, as the items may become invalid through item_free

  CodeEmitInfo* patching_info = NULL;
  if (!x->klass()->is_loaded() || PatchALot) {
    patching_info = state_for(x, x->state_before());
  }
  i = dims->length();
  while (i-- > 0) {
    LIRItem* size = items->at(i);
    size->load_item();

    emit()->store_stack_parameter(size->result(), i);
  }
  RInfo reg = set_with_result_register(x)->rinfo();

  CodeEmitInfo* info = state_for(x, x->state());
  emit()->new_multi_array(reg, x->klass(), x->rank(), norinfo, info, patching_info);
}

CJValueP
CJArrayFunction::
exec(CJavaScript *js, const Values &values)
{
  if (values.size() <= 1)
    return js->createArrayValue();

  if (values.size() == 2) {
    CJValueP protoValue = values[1];

    if (CJArrayType::canCreateArrayFromValue(protoValue))
      return CJArrayType::createArrayFromValue(js, protoValue);

    long n = values[1]->toInteger().getValue(0);

    return js->createArrayValue(n);
  }

  CJArrayP array = js->createArrayValue();

  for (uint i = 1; i < values.size(); ++i)
    array->addValue(values[i]);

  return array;
}

/* ************************************************************************* */
TEST(DoglegOptimizer, Iterate) {
  // really non-linear factor graph
  NonlinearFactorGraph fg = example::createReallyNonlinearFactorGraph();

  // config far from minimum
  Point2 x0(3,0);
  Values config;
  config.insert(X(1), x0);

  double Delta = 1.0;
  for(size_t it=0; it<10; ++it) {
    GaussianBayesNet gbn = *fg.linearize(config)->eliminateSequential();
    // Iterate assumes that linear error = nonlinear error at the linearization point, and this should be true
    double nonlinearError = fg.error(config);
    double linearError = GaussianFactorGraph(gbn).error(config.zeroVectors());
    DOUBLES_EQUAL(nonlinearError, linearError, 1e-5);
//    cout << "it " << it << ", Delta = " << Delta << ", error = " << fg->error(*config) << endl;
    VectorValues dx_u = gbn.optimizeGradientSearch();
    VectorValues dx_n = gbn.optimize();
    DoglegOptimizerImpl::IterationResult result = DoglegOptimizerImpl::Iterate(Delta, DoglegOptimizerImpl::SEARCH_EACH_ITERATION, dx_u, dx_n, gbn, fg, config, fg.error(config));
    Delta = result.Delta;
    EXPECT(result.f_error < fg.error(config)); // Check that error decreases
    Values newConfig(config.retract(result.dx_d));
    config = newConfig;
    DOUBLES_EQUAL(fg.error(config), result.f_error, 1e-5); // Check that error is correctly filled in
  }
}

int main (int argc, char** argv) {

  // Read File, create graph and initial estimate
  // we are in build/examples, data is in examples/Data
  NonlinearFactorGraph::shared_ptr graph;
  Values::shared_ptr initial;
  SharedDiagonal model = noiseModel::Diagonal::Sigmas((Vector(3) << 0.05, 0.05, 5.0 * M_PI / 180.0));
  string graph_file = findExampleDataFile("w100.graph");
  boost::tie(graph, initial) = load2D(graph_file, model);
  initial->print("Initial estimate:\n");

  // Add a Gaussian prior on first poses
  Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin
  SharedDiagonal priorNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.01, 0.01, 0.01));
  graph->push_back(PriorFactor<Pose2>(0, priorMean, priorNoise));

  // Single Step Optimization using Levenberg-Marquardt
  Values result = LevenbergMarquardtOptimizer(*graph, *initial).optimize();
  result.print("\nFinal result:\n");

  // Plot the covariance of the last pose
  Marginals marginals(*graph, result);
  cout.precision(2);
  cout << "\nP3:\n" << marginals.marginalCovariance(99) << endl;

  return 0;
}

/*******************************************************************************
 * Camera: f = 1, Image: 100x100, center: 50, 50.0
 * Pose (ground truth): (Xw, -Yw, -Zw, [0,0,2.0]')
 * Known landmarks:
 *    3D Points: (10,10,0) (-10,10,0) (-10,-10,0) (10,-10,0)
 * Perfect measurements:
 *    2D Point:  (55,45)   (45,45)    (45,55)     (55,55)
 *******************************************************************************/
int main(int argc, char* argv[]) {
  /* read camera intrinsic parameters */
  Cal3_S2::shared_ptr calib(new Cal3_S2(1, 1, 0, 50, 50));

  /* 1. create graph */
  NonlinearFactorGraph graph;

  /* 2. add factors to the graph */
  // add measurement factors
  SharedDiagonal measurementNoise = Diagonal::Sigmas((Vector(2) << 0.5, 0.5));
  boost::shared_ptr<ResectioningFactor> factor;
  graph.push_back(
      boost::make_shared<ResectioningFactor>(measurementNoise, X(1), calib,
          Point2(55, 45), Point3(10, 10, 0)));
  graph.push_back(
      boost::make_shared<ResectioningFactor>(measurementNoise, X(1), calib,
          Point2(45, 45), Point3(-10, 10, 0)));
  graph.push_back(
      boost::make_shared<ResectioningFactor>(measurementNoise, X(1), calib,
          Point2(45, 55), Point3(-10, -10, 0)));
  graph.push_back(
      boost::make_shared<ResectioningFactor>(measurementNoise, X(1), calib,
          Point2(55, 55), Point3(10, -10, 0)));

  /* 3. Create an initial estimate for the camera pose */
  Values initial;
  initial.insert(X(1),
      Pose3(Rot3(1, 0, 0, 0, -1, 0, 0, 0, -1), Point3(0, 0, 2)));

  /* 4. Optimize the graph using Levenberg-Marquardt*/
  Values result = LevenbergMarquardtOptimizer(graph, initial).optimize();
  result.print("Final result:\n");

  return 0;
}

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

int main(int argc, char* argv[]) {
  // parse options and read BAL file
  SfM_data db = preamble(argc, argv);

  // Build graph using conventional GeneralSFMFactor
  NonlinearFactorGraph graph;
  for (size_t j = 0; j < db.number_tracks(); j++) {
    BOOST_FOREACH (const SfM_Measurement& m, db.tracks[j].measurements) {
      size_t i = m.first;
      Point2 z = m.second;
      Pose3_ camTnav_(C(i));
      Cal3Bundler_ calibration_(K(i));
      Point3_ nav_point_(P(j));
      graph.addExpressionFactor(
          gNoiseModel, z,
          uncalibrate(calibration_,  // now using transform_from !!!:
                      project(transform_from(camTnav_, nav_point_))));
    }
  }

  Values initial;
  size_t i = 0, j = 0;
  BOOST_FOREACH (const SfM_Camera& camera, db.cameras) {
    initial.insert(C(i), camera.pose().inverse());  // inverse !!!
    initial.insert(K(i), camera.calibration());
    i += 1;
  }
  BOOST_FOREACH (const SfM_Track& track, db.tracks)
    initial.insert(P(j++), track.p);

  bool separateCalibration = true;
  return optimize(db, graph, initial, separateCalibration);
}

/* *************************************************************************/
TEST( SmartProjectionCameraFactor, noiselessBundler ) {

  using namespace bundler;
  Values values;
  values.insert(c1, level_camera);
  values.insert(c2, level_camera_right);

  SmartFactor::shared_ptr factor1(new SmartFactor(unit2));
  factor1->add(level_uv, c1);
  factor1->add(level_uv_right, c2);

  double actualError = factor1->error(values);

  double expectedError = 0.0;
  DOUBLES_EQUAL(expectedError, actualError, 1e-3);

  Point3 oldPoint; // this takes the point stored in the factor (we are not interested in this)
  if (factor1->point())
    oldPoint = *(factor1->point());

  Point3 expectedPoint;
  if (factor1->point(values))
    expectedPoint = *(factor1->point(values));

  EXPECT(assert_equal(expectedPoint, landmark1, 1e-3));
}