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