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 ( 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( 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)); }
/* ************************************************************************* */ 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)); }
CJValueP CJRegExpFunction:: exec(CJavaScript *js, const Values &values) { if (values.size() <= 1) return js->createRegExpValue(""); std::string s = values[1]->toString(); CJRegExpP regexp = js->createRegExpValue(s); if (values.size() > 2) { std::string flags = values[2]->toString(); for (auto &f : flags) { if (f == 'g') { regexp->setGlobalMatch(true); } else if (f == 'i') { regexp->setIgnoreCase(true); } else if (f == 'm') { } else if (f == 'u') { } else if (f == 'y') { } } } return regexp; }
void get_argument(void* env, int argposition, Values& values) { DATA_OBJECT arg; if (EnvArgTypeCheck(env, (char *)"clipsmm get_argument", argposition, MULTIFIELD, &arg) == 0) return; values.clear(); int end = EnvGetDOEnd(env, arg); void *mfp = EnvGetValue(env, arg); for (int i = EnvGetDOBegin(env, arg); i <= end; ++i) { switch (GetMFType(mfp, i)) { case SYMBOL: case STRING: case INSTANCE_NAME: values.push_back(Value(ValueToString(GetMFValue(mfp, i)))); break; case FLOAT: values.push_back(Value(ValueToDouble(GetMFValue(mfp, i)))); break; case INTEGER: values.push_back(Value(ValueToInteger(GetMFValue(mfp, i)))); break; default: continue; break; } } }
/* ************************************************************************* */ TEST( testLinearContainerFactor, generic_jacobian_factor ) { Matrix A1 = (Matrix(2, 2) << 2.74222, -0.0067457, 0.0, 2.63624); Matrix A2 = (Matrix(2, 2) << -0.0455167, -0.0443573, -0.0222154, -0.102489); Vector b = (Vector(2) << 0.0277052, -0.0533393); JacobianFactor expLinFactor(l1, A1, l2, A2, b, diag_model2); LinearContainerFactor actFactor(expLinFactor); EXPECT_LONGS_EQUAL(2, actFactor.size()); EXPECT(actFactor.isJacobian()); EXPECT(!actFactor.isHessian()); // check keys FastVector<Key> expKeys; expKeys += l1, l2; EXPECT(assert_container_equality(expKeys, actFactor.keys())); Values values; values.insert(l1, landmark1); values.insert(l2, landmark2); values.insert(x1, poseA1); values.insert(x2, poseA2); // Check reconstruction GaussianFactor::shared_ptr actLinearizationA = actFactor.linearize(values); EXPECT(assert_equal(*expLinFactor.clone(), *actLinearizationA, tol)); }
// Many Leaves TEST(Expression, Leaves) { Values values; Point3 somePoint(1, 2, 3); values.insert(Symbol('p', 10), somePoint); std::vector<Expression<Point3> > points = createUnknowns<Point3>(10, 'p', 1); EXPECT(assert_equal(somePoint, points.back().value(values))); }
Values keygen(Values & pub){ RNG::BBS(static_cast <MPI> (static_cast <unsigned int> (now()))); // seed just in case not seeded MPI x = 0; std::string test = "testing testing 123"; // a string to test the key with, just in case the key doesn't work for some reason unsigned int bits = bitsize(pub[1]) - 1; while (true){ // 0 < x < q while ((x == 0) || (pub[1] <= x)){ x = bintompi(RNG::BBS().rand(bits)); } // y = g^x mod p MPI y; y = powm(pub[2], x, pub[0]); // public key = p, q, g, y // private key = x pub.push_back(y); // check that this key works Values rs = sign(test, {x}, pub); // if it works, break if (verify(test, rs, pub)){ break; } pub.pop_back(); } return {x}; }
/* *************************************************************************/ TEST( SmartProjectionPoseFactor, noisy ) { using namespace vanillaPose; // Project two landmarks into two cameras Point2 pixelError(0.2, 0.2); Point2 level_uv = level_camera.project(landmark1) + pixelError; Point2 level_uv_right = level_camera_right.project(landmark1); Values values; values.insert(x1, cam1.pose()); Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 10, 0., -M_PI / 10), Point3(0.5, 0.1, 0.3)); values.insert(x2, pose_right.compose(noise_pose)); SmartFactor::shared_ptr factor(new SmartFactor(model, sharedK)); factor->add(level_uv, x1); factor->add(level_uv_right, x2); double actualError1 = factor->error(values); SmartFactor::shared_ptr factor2(new SmartFactor(model, sharedK)); vector<Point2> measurements; measurements.push_back(level_uv); measurements.push_back(level_uv_right); vector<Key> views; views.push_back(x1); views.push_back(x2); factor2->add(measurements, views); double actualError2 = factor2->error(values); DOUBLES_EQUAL(actualError1, actualError2, 1e-7); }
/* ************************************************************************* */ TEST( testDummyFactor, basics ) { gtsam::Key key1 = 7, key2 = 9; size_t dim1 = 3, dim2 = 3; DummyFactor dummyfactor(key1, dim1, key2, dim2); // verify contents LONGS_EQUAL(2, dummyfactor.size()); EXPECT_LONGS_EQUAL(key1, dummyfactor.keys()[0]); EXPECT_LONGS_EQUAL(key2, dummyfactor.keys()[1]); LONGS_EQUAL(2, dummyfactor.dims().size()); EXPECT_LONGS_EQUAL(dim1, dummyfactor.dims()[0]); EXPECT_LONGS_EQUAL(dim2, dummyfactor.dims()[1]); Values c; c.insert(key1, Point3(1.0, 2.0, 3.0)); c.insert(key2, Point3(4.0, 5.0, 6.0)); // errors - all zeros DOUBLES_EQUAL(0.0, dummyfactor.error(c), tol); // linearization: all zeros GaussianFactor::shared_ptr actLinearization = dummyfactor.linearize(c); CHECK(actLinearization); noiseModel::Diagonal::shared_ptr model3 = noiseModel::Unit::Create(3); GaussianFactor::shared_ptr expLinearization(new JacobianFactor( key1, Matrix::Zero(3,3), key2, Matrix::Zero(3,3), Z_3x1, model3)); EXPECT(assert_equal(*expLinearization, *actLinearization, tol)); }
/* ************************************************************************* */ TEST( StereoFactor, singlePoint) { NonlinearFactorGraph graph; graph.add(NonlinearEquality<Pose3>(X(1), camera1)); StereoPoint2 measurement(320, 320.0-50, 240); // arguments: measurement, sigma, cam#, measurement #, K, baseline (m) graph.add(GenericStereoFactor<Pose3, Point3>(measurement, model, X(1), L(1), K)); // Create a configuration corresponding to the ground truth Values values; values.insert(X(1), camera1); // add camera at z=6.25m looking towards origin Point3 l1(0, 0, 0); values.insert(L(1), l1); // add point at origin; GaussNewtonOptimizer optimizer(graph, values); // We expect the initial to be zero because config is the ground truth DOUBLES_EQUAL(0.0, optimizer.error(), 1e-9); // Iterate once, and the config should not have changed optimizer.iterate(); DOUBLES_EQUAL(0.0, optimizer.error(), 1e-9); // Complete solution optimizer.optimize(); DOUBLES_EQUAL(0.0, optimizer.error(), 1e-6); }
Values File::getFiles(const Name &name) { Values values; for (auto &f : functions) { if (!(f.name == "cc_library" || f.name == "cc_binary")) continue; auto i = std::find_if(f.parameters.begin(), f.parameters.end(), [](const auto &p) { return "name" == p.name; }); if (i == f.parameters.end() || i->values.empty() || prepare_project_name(*i->values.begin()) != name) continue; i = std::find_if(f.parameters.begin(), f.parameters.end(), [](const auto &p) { return "hdrs" == p.name; }); if (i != f.parameters.end()) { values.insert(i->values.begin(), i->values.end()); } i = std::find_if(f.parameters.begin(), f.parameters.end(), [](const auto &p) { return "srcs" == p.name; }); if (i != f.parameters.end()) { values.insert(i->values.begin(), i->values.end()); } } return values; }
Values fromAsebaVector(const std::vector<sint16>& values) { Values data; for (size_t i = 0; i < values.size(); ++i) data.push_back(values[i]); return data; }
/* ************************************************************************* */ TEST ( NonlinearEquality, allow_error_pose ) { Symbol key1('x',1); Pose2 feasible1(1.0, 2.0, 3.0); double error_gain = 500.0; PoseNLE nle(key1, feasible1, error_gain); // the unwhitened error should provide logmap to the feasible state Pose2 badPoint1(0.0, 2.0, 3.0); Vector actVec = nle.evaluateError(badPoint1); Vector expVec = Vector_(3, -0.989992, -0.14112, 0.0); EXPECT(assert_equal(expVec, actVec, 1e-5)); // the actual error should have a gain on it Values config; config.insert(key1, badPoint1); double actError = nle.error(config); DOUBLES_EQUAL(500.0, actError, 1e-9); // check linearization GaussianFactor::shared_ptr actLinFactor = nle.linearize(config, *config.orderingArbitrary()); Matrix A1 = eye(3,3); Vector b = expVec; SharedDiagonal model = noiseModel::Constrained::All(3); GaussianFactor::shared_ptr expLinFactor(new JacobianFactor(0, A1, b, model)); EXPECT(assert_equal(*expLinFactor, *actLinFactor, 1e-5)); }
/* ************************************************************************* */ TEST( LinearizedFactor, clone_jacobian ) { // Create a Between Factor from a Point3. This is actually a linear factor. Key x1(1); Key x2(2); Values values; values.insert(x1, Point3(-22.4, +8.5, +2.4)); values.insert(x2, Point3(-21.0, +5.0, +21.0)); Point3 measured(1.0, -2.5, 17.8); SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 0.1); BetweenFactor<Point3> betweenFactor(x1, x2, measured, model); // Create one factor that is a clone of the other and make sure they're equal JacobianFactor::shared_ptr jf = boost::static_pointer_cast<JacobianFactor>(betweenFactor.linearize(values)); LinearizedJacobianFactor jacobian1(jf, values); LinearizedJacobianFactor::shared_ptr jacobian2 = boost::static_pointer_cast<LinearizedJacobianFactor>(jacobian1.clone()); CHECK(assert_equal(jacobian1, *jacobian2)); JacobianFactor::shared_ptr jf1 = boost::static_pointer_cast<JacobianFactor>(jacobian1.linearize(values)); JacobianFactor::shared_ptr jf2 = boost::static_pointer_cast<JacobianFactor>(jacobian2->linearize(values)); CHECK(assert_equal(*jf1, *jf2)); Matrix information1 = jf1->augmentedInformation(); Matrix information2 = jf2->augmentedInformation(); CHECK(assert_equal(information1, information2)); }
int main(const int argc, const char *argv[]) { // Read graph from file string g2oFile; if (argc < 2) g2oFile = findExampleDataFile("noisyToyGraph.txt"); else g2oFile = argv[1]; NonlinearFactorGraph::shared_ptr graph; Values::shared_ptr initial; boost::tie(graph, initial) = readG2o(g2oFile); // Add prior on the pose having index (key) = 0 NonlinearFactorGraph graphWithPrior = *graph; noiseModel::Diagonal::shared_ptr priorModel = // noiseModel::Diagonal::Variances((Vector(3) << 1e-6, 1e-6, 1e-8)); graphWithPrior.add(PriorFactor<Pose2>(0, Pose2(), priorModel)); graphWithPrior.print(); std::cout << "Computing LAGO estimate" << std::endl; Values estimateLago = lago::initialize(graphWithPrior); std::cout << "done!" << std::endl; if (argc < 3) { estimateLago.print("estimateLago"); } else { const string outputFile = argv[2]; std::cout << "Writing results to file: " << outputFile << std::endl; writeG2o(*graph, estimateLago, outputFile); std::cout << "done! " << std::endl; } return 0; }