/* ************************************************************************** */
TEST(GaussianPriorWorkspacePoseArm, optimization) {
noiseModel::Gaussian::shared_ptr cost_model = noiseModel::Isotropic::Sigma(6, 0.1);
Vector a = (Vector(2) << 1, 1).finished();
Vector alpha = (Vector(2) << 0, 0).finished();
Vector d = (Vector(2) << 0, 0).finished();
ArmModel arm = ArmModel(Arm(2, a, alpha, d), BodySphereVector());
Pose3 des = Pose3(Rot3(), Point3(2, 0, 0));
Key qkey = Symbol('x', 0);
Vector q = (Vector(2) << 0, 0).finished();
Vector qinit = (Vector(2) << M_PI/2, M_PI/2).finished();
NonlinearFactorGraph graph;
graph.add(GaussianPriorWorkspacePoseArm(qkey, arm, 1, des, cost_model));
Values init_values;
init_values.insert(qkey, qinit);
LevenbergMarquardtParams parameters;
parameters.setVerbosity("ERROR");
parameters.setAbsoluteErrorTol(1e-12);
LevenbergMarquardtOptimizer optimizer(graph, init_values, parameters);
optimizer.optimize();
Values results = optimizer.values();
EXPECT_DOUBLES_EQUAL(0, graph.error(results), 1e-3);
EXPECT(assert_equal(q, results.at<Vector>(qkey), 1e-3));
}
/* ************************************************************************** */
TEST(GoalFactorArm, optimization_1) {
// use optimization to solve inverse kinematics
noiseModel::Gaussian::shared_ptr cost_model = noiseModel::Isotropic::Sigma(3, 0.1);
// 2 link simple example
Vector a = (Vector(2) << 1, 1).finished();
Vector alpha = (Vector(2) << 0, 0).finished();
Vector d = (Vector(2) << 0, 0).finished();
Arm arm(2, a, alpha, d);
Point3 goal(1.414213562373095, 1.414213562373095, 0);
Key qkey = Symbol('x', 0);
Vector q = (Vector(2) << M_PI/4.0, 0).finished();
Vector qinit = (Vector(2) << 0, 0).finished();
NonlinearFactorGraph graph;
graph.add(GoalFactorArm(qkey, cost_model, arm, goal));
Values init_values;
init_values.insert(qkey, qinit);
LevenbergMarquardtParams parameters;
parameters.setVerbosity("ERROR");
parameters.setAbsoluteErrorTol(1e-12);
LevenbergMarquardtOptimizer optimizer(graph, init_values, parameters);
optimizer.optimize();
Values results = optimizer.values();
EXPECT_DOUBLES_EQUAL(0, graph.error(results), 1e-3);
EXPECT(assert_equal(q, results.at<Vector>(qkey), 1e-3));
}
/* ************************************************************************* */
int main (int argc, char* argv[]) {
// Find default file, but if an argument is given, try loading a file
string filename = findExampleDataFile("dubrovnik-3-7-pre");
if (argc>1) filename = string(argv[1]);
// Load the SfM data from file
SfM_data mydata;
assert(readBAL(filename, mydata));
cout << boost::format("read %1% tracks on %2% cameras\n") % mydata.number_tracks() % mydata.number_cameras();
// Create a factor graph
NonlinearFactorGraph graph;
// We share *one* noiseModel between all projection factors
noiseModel::Isotropic::shared_ptr noise =
noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v
// Add measurements to the factor graph
size_t j = 0;
BOOST_FOREACH(const SfM_Track& track, mydata.tracks) {
BOOST_FOREACH(const SfM_Measurement& m, track.measurements) {
size_t i = m.first;
Point2 uv = m.second;
graph.push_back(MyFactor(uv, noise, C(i), P(j))); // note use of shorthand symbols C and P
}
j += 1;
}
// Add a prior on pose x1. This indirectly specifies where the origin is.
// and a prior on the position of the first landmark to fix the scale
graph.push_back(PriorFactor<SfM_Camera>(C(0), mydata.cameras[0], noiseModel::Isotropic::Sigma(9, 0.1)));
graph.push_back(PriorFactor<Point3> (P(0), mydata.tracks[0].p, noiseModel::Isotropic::Sigma(3, 0.1)));
// Create initial estimate
Values initial;
size_t i = 0; j = 0;
BOOST_FOREACH(const SfM_Camera& camera, mydata.cameras) initial.insert(C(i++), camera);
BOOST_FOREACH(const SfM_Track& track, mydata.tracks) initial.insert(P(j++), track.p);
/* Optimize the graph and print results */
Values result;
try {
LevenbergMarquardtParams params;
params.setVerbosity("ERROR");
LevenbergMarquardtOptimizer lm(graph, initial, params);
result = lm.optimize();
} catch (exception& e) {
cout << e.what();
}
cout << "final error: " << graph.error(result) << endl;
return 0;
}
