12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Updated examples and namespaces for the new NonlinearOptimizer interface

release/4.3a0
Stephen Williams 2012-05-14 20:25:20 +00:00
parent 75bd1689df
commit 7f0881f2e4
8 changed files with 54 additions and 52 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
examples/CameraResectioning.cpp
View File

@ -95,7 +95,7 @@ int main(int argc, char* argv[]) {
0.,0.,-1.), Point3(0.,0.,2.0)));
/* 4. Optimize the graph using Levenberg-Marquardt*/
Values result = *LevenbergMarquardtOptimizer(graph, initial).optimized();
Values result = LevenbergMarquardtOptimizer(graph, initial).optimize();
result.print("Final result: ");
return 0;

2
examples/PlanarSLAMExample_easy.cpp
View File

@ -83,7 +83,7 @@ int main(int argc, char** argv) {
initialEstimate.print("initial estimate");
// optimize using Levenberg-Marquardt optimization with an ordering from colamd
planarSLAM::Values result = *LevenbergMarquardtOptimizer(graph, initialEstimate).optimized();
planarSLAM::Values result = LevenbergMarquardtOptimizer(graph, initialEstimate).optimize();
result.print("final result");
return 0;

42
examples/PlanarSLAMSelfContained_advanced.cpp
View File

@ -57,14 +57,14 @@ int main(int argc, char** argv) {
Symbol l1('l',1), l2('l',2);
// create graph container and add factors to it
NonlinearFactorGraph::shared_ptr graph(new NonlinearFactorGraph);
NonlinearFactorGraph graph;
/* add prior */
// gaussian for prior
SharedDiagonal prior_model = noiseModel::Diagonal::Sigmas(Vector_(3, 0.3, 0.3, 0.1));
Pose2 prior_measurement(0.0, 0.0, 0.0); // prior at origin
PriorFactor<Pose2> posePrior(x1, prior_measurement, prior_model); // create the factor
graph->add(posePrior); // add the factor to the graph
graph.add(posePrior); // add the factor to the graph
/* add odometry */
// general noisemodel for odometry
@ -73,8 +73,8 @@ int main(int argc, char** argv) {
// create between factors to represent odometry
BetweenFactor<Pose2> odom12(x1, x2, odom_measurement, odom_model);
BetweenFactor<Pose2> odom23(x2, x3, odom_measurement, odom_model);
graph->add(odom12); // add both to graph
graph->add(odom23);
graph.add(odom12); // add both to graph
graph.add(odom23);
/* add measurements */
// general noisemodel for measurements
@ -94,39 +94,39 @@ int main(int argc, char** argv) {
BearingRangeFactor<Pose2, Point2> meas32(x3, l2, bearing32, range32, meas_model);
// add the factors
graph->add(meas11);
graph->add(meas21);
graph->add(meas32);
graph.add(meas11);
graph.add(meas21);
graph.add(meas32);
graph->print("Full Graph");
graph.print("Full Graph");
// initialize to noisy points
boost::shared_ptr<Values> initial(new Values);
initial->insert(x1, Pose2(0.5, 0.0, 0.2));
initial->insert(x2, Pose2(2.3, 0.1,-0.2));
initial->insert(x3, Pose2(4.1, 0.1, 0.1));
initial->insert(l1, Point2(1.8, 2.1));
initial->insert(l2, Point2(4.1, 1.8));
Values initial;
initial.insert(x1, Pose2(0.5, 0.0, 0.2));
initial.insert(x2, Pose2(2.3, 0.1,-0.2));
initial.insert(x3, Pose2(4.1, 0.1, 0.1));
initial.insert(l1, Point2(1.8, 2.1));
initial.insert(l2, Point2(4.1, 1.8));
initial->print("initial estimate");
initial.print("initial estimate");
// optimize using Levenberg-Marquardt optimization with an ordering from colamd
// first using sequential elimination
LevenbergMarquardtParams lmParams;
lmParams.elimination = LevenbergMarquardtParams::SEQUENTIAL;
Values::const_shared_ptr resultSequential = LevenbergMarquardtOptimizer(graph, initial, lmParams).optimized();
resultSequential->print("final result (solved with a sequential solver)");
Values resultSequential = LevenbergMarquardtOptimizer(graph, initial, lmParams).optimize();
resultSequential.print("final result (solved with a sequential solver)");
// then using multifrontal, advanced interface
// Note that we keep the original optimizer object so we can use the COLAMD
// ordering it computes.
LevenbergMarquardtOptimizer optimizer(graph, initial);
Values::const_shared_ptr resultMultifrontal = optimizer.optimized();
resultMultifrontal->print("final result (solved with a multifrontal solver)");
Values resultMultifrontal = optimizer.optimize();
resultMultifrontal.print("final result (solved with a multifrontal solver)");
const Ordering& ordering = *optimizer.ordering();
GaussianMultifrontalSolver linearSolver(*graph->linearize(*resultMultifrontal, ordering));
const Ordering& ordering = *optimizer.params().ordering;
GaussianMultifrontalSolver linearSolver(*graph.linearize(resultMultifrontal, ordering));
// Print marginals covariances for all variables
print(linearSolver.marginalCovariance(ordering[x1]), "x1 covariance");

44
examples/Pose2SLAMExample_advanced.cpp
View File

@ -34,13 +34,13 @@ using namespace pose2SLAM;
int main(int argc, char** argv) {
/* 1. create graph container and add factors to it */
shared_ptr<Graph> graph(new Graph);
Graph graph;
/* 2.a add prior */
// gaussian for prior
SharedDiagonal prior_model = noiseModel::Diagonal::Sigmas(Vector_(3, 0.3, 0.3, 0.1));
Pose2 prior_measurement(0.0, 0.0, 0.0); // prior at origin
graph->addPrior(1, prior_measurement, prior_model); // add directly to graph
graph.addPrior(1, prior_measurement, prior_model); // add directly to graph
/* 2.b add odometry */
// general noisemodel for odometry
@ -48,36 +48,38 @@ int main(int argc, char** argv) {
/* Pose2 measurements take (x,y,theta), where theta is taken from the positive x-axis*/
Pose2 odom_measurement(2.0, 0.0, 0.0); // create a measurement for both factors (the same in this case)
graph->addOdometry(1, 2, odom_measurement, odom_model);
graph->addOdometry(2, 3, odom_measurement, odom_model);
graph->print("full graph");
graph.addOdometry(1, 2, odom_measurement, odom_model);
graph.addOdometry(2, 3, odom_measurement, odom_model);
graph.print("full graph");
/* 3. Create the data structure to hold the initial estimate to the solution
* initialize to noisy points */
shared_ptr<pose2SLAM::Values> initial(new pose2SLAM::Values);
initial->insertPose(1, Pose2(0.5, 0.0, 0.2));
initial->insertPose(2, Pose2(2.3, 0.1,-0.2));
initial->insertPose(3, Pose2(4.1, 0.1, 0.1));
initial->print("initial estimate");
pose2SLAM::Values initial;
initial.insertPose(1, Pose2(0.5, 0.0, 0.2));
initial.insertPose(2, Pose2(2.3, 0.1,-0.2));
initial.insertPose(3, Pose2(4.1, 0.1, 0.1));
initial.print("initial estimate");
/* 4.2.1 Alternatively, you can go through the process step by step
* Choose an ordering */
Ordering::shared_ptr ordering = graph->orderingCOLAMD(*initial);
Ordering ordering = *graph.orderingCOLAMD(initial);
/* 4.2.2 set up solver and optimize */
NonlinearOptimizationParameters::shared_ptr params = NonlinearOptimizationParameters::newDecreaseThresholds(1e-15, 1e-15);
Optimizer optimizer(graph, initial, ordering, params);
Optimizer optimizer_result = optimizer.levenbergMarquardt();
LevenbergMarquardtParams params;
params.absoluteErrorTol = 1e-15;
params.relativeErrorTol = 1e-15;
params.ordering = ordering;
LevenbergMarquardtOptimizer optimizer(graph, initial, params);
pose2SLAM::Values result = *optimizer_result.values();
pose2SLAM::Values result = optimizer.optimize();
result.print("final result");
/* Get covariances */
Matrix covariance1 = optimizer_result.marginalCovariance(PoseKey(1));
Matrix covariance2 = optimizer_result.marginalCovariance(PoseKey(2));
print(covariance1, "Covariance1");
print(covariance2, "Covariance2");
// /* Get covariances */
// Matrix covariance1 = optimizer_result.marginalCovariance(PoseKey(1));
// Matrix covariance2 = optimizer_result.marginalCovariance(PoseKey(2));
//
// print(covariance1, "Covariance1");
// print(covariance2, "Covariance2");
return 0;
}

2
examples/Pose2SLAMExample_easy.cpp
View File

@ -61,7 +61,7 @@ int main(int argc, char** argv) {
/* 4 Single Step Optimization
* optimize using Levenberg-Marquardt optimization with an ordering from colamd */
pose2SLAM::Values result = *LevenbergMarquardtOptimizer(graph, initial).optimized();
pose2SLAM::Values result = graph.optimize(initial);
result.print("final result");

2
examples/SimpleRotation.cpp
View File

@ -105,7 +105,7 @@ int main() {
* initial estimate. This will yield a new RotValues structure
* with the final state of the optimization.
*/
Values result = *LevenbergMarquardtOptimizer(graph, initial).optimized();
Values result = LevenbergMarquardtOptimizer(graph, initial).optimize();
result.print("final result");
return 0;

10
examples/vSLAMexample/vSFMexample.cpp
View File

@ -129,23 +129,23 @@ int main(int argc, char* argv[]) {
readAllData();
// Create a graph using the 2D measurements (features) and the calibration data
boost::shared_ptr<Graph> graph(new Graph(setupGraph(g_measurements, measurementSigma, g_calib)));
Graph graph(setupGraph(g_measurements, measurementSigma, g_calib));
// Create an initial Values structure using groundtruth values as the initial estimates
boost::shared_ptr<Values> initialEstimates(new Values(initialize(g_landmarks, g_poses)));
Values initialEstimates(initialize(g_landmarks, g_poses));
cout << "*******************************************************" << endl;
initialEstimates->print("INITIAL ESTIMATES: ");
initialEstimates.print("INITIAL ESTIMATES: ");
// Add prior factor for all poses in the graph
map<int, Pose3>::iterator poseit = g_poses.begin();
for (; poseit != g_poses.end(); poseit++)
graph->addPosePrior(poseit->first, poseit->second, noiseModel::Unit::Create(1));
graph.addPosePrior(poseit->first, poseit->second, noiseModel::Unit::Create(1));
// Optimize the graph
cout << "*******************************************************" << endl;
LevenbergMarquardtParams params;
params.lmVerbosity = LevenbergMarquardtParams::DAMPED;
visualSLAM::Values result = *LevenbergMarquardtOptimizer(graph, initialEstimates, params).optimized();
visualSLAM::Values result = LevenbergMarquardtOptimizer(graph, initialEstimates, params).optimize();
// Print final results
cout << "*******************************************************" << endl;

2
gtsam/slam/planarSLAM.h
View File

@ -110,7 +110,7 @@ namespace planarSLAM {
/// Optimize
Values optimize(const Values& initialEstimate) {
return LevenbergMarquardtOptimizer(*this).optimized(initialEstimate);
return LevenbergMarquardtOptimizer(*this, initialEstimate).optimize();
}
};