separate the examples into easy/advanced style, remove shared pointer in easy examples, unify the convention
Yong-Dian Jian
parent
c7248a2b13
commit
adc8921f3b
|
|
@ -11,11 +11,13 @@ sources =
|
|||
check_PROGRAMS =
|
||||
|
||||
# Examples
|
||||
noinst_PROGRAMS = SimpleRotation # Optimizes a single nonlinear rotation variable
|
||||
noinst_PROGRAMS += PlanarSLAMExample # Solves SLAM example from tutorial by using planarSLAM
|
||||
noinst_PROGRAMS += Pose2SLAMExample # Solves SLAM example from tutorial by using planarSLAM
|
||||
noinst_PROGRAMS += PlanarSLAMSelfContained # Solves SLAM example from tutorial with all typedefs in the script
|
||||
noinst_PROGRAMS += Pose2SLAMwSPCG # Solves a simple SLAM example with SPCG solver
|
||||
noinst_PROGRAMS = SimpleRotation # Optimizes a single nonlinear rotation variable
|
||||
noinst_PROGRAMS += PlanarSLAMExample_easy # Solves SLAM example from tutorial by using planarSLAM
|
||||
noinst_PROGRAMS += PlanarSLAMSelfContained_advanced # Solves SLAM example from tutorial with all typedefs in the script
|
||||
noinst_PROGRAMS += Pose2SLAMExample_easy # Solves SLAM example from tutorial by using Pose2SLAM and easy optimization interface
|
||||
noinst_PROGRAMS += Pose2SLAMExample_advanced # Solves SLAM example from tutorial by using Pose2SLAM and advanced optimization interface
|
||||
noinst_PROGRAMS += Pose2SLAMwSPCG_easy # Solves a simple Pose2 SLAM example with advanced SPCG solver
|
||||
noinst_PROGRAMS += Pose2SLAMwSPCG_advanced # Solves a simple Pose2 SLAM example with easy SPCG solver
|
||||
SUBDIRS = vSLAMexample # does not compile....
|
||||
#----------------------------------------------------------------------------------------------------
|
||||
# rules to build local programs
|
||||
|
|
|
|||
|
|
@ -74,7 +74,7 @@ int main(int argc, char** argv) {
|
|||
graph.addBearingRange(x2, l1, bearing21, range21, meas_model);
|
||||
graph.addBearingRange(x3, l2, bearing32, range32, meas_model);
|
||||
|
||||
graph.print("Full Graph");
|
||||
graph.print("full graph");
|
||||
|
||||
// initialize to noisy points
|
||||
Values initialEstimate;
|
||||
|
|
@ -84,11 +84,11 @@ int main(int argc, char** argv) {
|
|||
initialEstimate.insert(l1, Point2(1.8, 2.1));
|
||||
initialEstimate.insert(l2, Point2(4.1, 1.8));
|
||||
|
||||
initialEstimate.print("Initial Estimate");
|
||||
initialEstimate.print("initial estimate");
|
||||
|
||||
// optimize using Levenberg-Marquardt optimization with an ordering from colamd
|
||||
Values result = optimize<Graph, Values>(graph, initialEstimate);
|
||||
result.print("Final Result");
|
||||
result.print("final result");
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
|
@ -110,19 +110,19 @@ int main(int argc, char** argv) {
|
|||
graph.print("Full Graph");
|
||||
|
||||
// initialize to noisy points
|
||||
Values initialEstimate;
|
||||
initialEstimate.insert(x1, Pose2(0.5, 0.0, 0.2));
|
||||
initialEstimate.insert(x2, Pose2(2.3, 0.1,-0.2));
|
||||
initialEstimate.insert(x3, Pose2(4.1, 0.1, 0.1));
|
||||
initialEstimate.insert(l1, Point2(1.8, 2.1));
|
||||
initialEstimate.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));
|
||||
|
||||
initialEstimate.print("Initial Estimate");
|
||||
initial.print("initial estimate");
|
||||
|
||||
// optimize using Levenburg-Marquadt optimization with an ordering from colamd
|
||||
Optimizer::shared_values result = Optimizer::optimizeLM(graph, initialEstimate);
|
||||
Optimizer::shared_values result = Optimizer::optimizeLM(graph, initial);
|
||||
|
||||
result->print("Final Result");
|
||||
result->print("final result");
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
|
@ -34,7 +34,6 @@ int main(int argc, char** argv) {
|
|||
Key x1(1), x2(2), x3(3);
|
||||
|
||||
/* 1. create graph container and add factors to it */
|
||||
//Graph graph;
|
||||
shared_ptr<Graph> graph(new Graph);
|
||||
|
||||
/* 2.a add prior */
|
||||
|
|
@ -51,39 +50,29 @@ int main(int argc, char** argv) {
|
|||
Pose2 odom_measurement(2.0, 0.0, 0.0); // create a measurement for both factors (the same in this case)
|
||||
graph->addConstraint(x1, x2, odom_measurement, odom_model);
|
||||
graph->addConstraint(x2, x3, odom_measurement, odom_model);
|
||||
|
||||
graph->print("Full Graph");
|
||||
graph->print("full graph");
|
||||
|
||||
/* 3. Create the data structure to hold the initial estinmate to the solution
|
||||
* initialize to noisy points */
|
||||
shared_ptr<Values> initialEstimate(new Values);
|
||||
initialEstimate->insert(x1, Pose2(0.5, 0.0, 0.2));
|
||||
initialEstimate->insert(x2, Pose2(2.3, 0.1,-0.2));
|
||||
initialEstimate->insert(x3, Pose2(4.1, 0.1, 0.1));
|
||||
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->print("initial estimate");
|
||||
|
||||
initialEstimate->print("Initial Estimate");
|
||||
|
||||
/* There are several ways to solve the graph. */
|
||||
|
||||
/* 4.1 Single Step:
|
||||
* optimize using Levenberg-Marquardt optimization with an ordering from colamd */
|
||||
Values result = optimize<Graph, Values>(*graph, *initialEstimate);
|
||||
result.print("Final Result");
|
||||
|
||||
/* 4.2.1 Alternatively, you can go through the process step by step
|
||||
* Choose an ordering */
|
||||
Ordering::shared_ptr ordering = graph->orderingCOLAMD(*initialEstimate);
|
||||
Ordering::shared_ptr ordering = graph->orderingCOLAMD(*initial);
|
||||
|
||||
/* 4.2.2 set up solver and optimize */
|
||||
/* 4.2.2 set up solver and optimize */
|
||||
Optimizer::shared_solver solver(new Optimizer::solver(ordering));
|
||||
Optimizer optimizer(graph, initial, solver);
|
||||
Optimizer::Parameters::verbosityLevel verbosity = pose2SLAM::Optimizer::Parameters::SILENT;
|
||||
Optimizer optimizer_result = optimizer.levenbergMarquardt(1e-15, 1e-15, verbosity);
|
||||
|
||||
Optimizer optimizer(graph, initialEstimate, solver);
|
||||
Optimizer::Parameters::verbosityLevel verbosity = pose2SLAM::Optimizer::Parameters::SILENT;
|
||||
Optimizer optimizer0 = optimizer.levenbergMarquardt(1e-15, 1e-15, verbosity);
|
||||
|
||||
Values result2 = *optimizer0.config();
|
||||
result2.print("Final Result 2");
|
||||
|
||||
Values result = *optimizer_result.config();
|
||||
result.print("final result");
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
|
@ -0,0 +1,59 @@
|
|||
/*
|
||||
* Pose2SLAMExample_easy.cpp
|
||||
*
|
||||
* Created on: Oct 21, 2010
|
||||
* Author: ydjian
|
||||
*/
|
||||
|
||||
#include <cmath>
|
||||
#include <iostream>
|
||||
#include <boost/shared_ptr.hpp>
|
||||
|
||||
// pull in the Pose2 SLAM domain with all typedefs and helper functions defined
|
||||
#include <gtsam/slam/pose2SLAM.h>
|
||||
#include <gtsam/nonlinear/NonlinearOptimization-inl.h>
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
using namespace gtsam::pose2SLAM;
|
||||
|
||||
int main(int argc, char** argv) {
|
||||
// create keys for robot positions
|
||||
Key x1(1), x2(2), x3(3);
|
||||
|
||||
/* 1. create graph container and add factors to it */
|
||||
//Graph 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(x1, prior_measurement, prior_model); // add directly to graph
|
||||
|
||||
/* 2.b add odometry */
|
||||
// general noisemodel for odometry
|
||||
SharedDiagonal odom_model = noiseModel::Diagonal::Sigmas(Vector_(3, 0.2, 0.2, 0.1));
|
||||
|
||||
/* 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.addConstraint(x1, x2, odom_measurement, odom_model);
|
||||
graph.addConstraint(x2, x3, odom_measurement, odom_model);
|
||||
graph.print("full graph");
|
||||
|
||||
/* 3. Create the data structure to hold the initial estinmate to the solution
|
||||
* initialize to noisy points */
|
||||
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.print("initial estimate");
|
||||
|
||||
/* 4 Single Step Optimization
|
||||
* optimize using Levenberg-Marquardt optimization with an ordering from colamd */
|
||||
Values result = optimize<Graph, Values>(graph, initial);
|
||||
result.print("final result");
|
||||
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
|
@ -1,111 +0,0 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* Atlanta, Georgia 30332-0415
|
||||
* All Rights Reserved
|
||||
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||
|
||||
* See LICENSE for the license information
|
||||
|
||||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/*
|
||||
* Pose2SLAMwSPCG.cpp
|
||||
*
|
||||
* Created on: Oct 18, 2010
|
||||
* Author: Yong-Dian Jian
|
||||
*
|
||||
* Demonstrate how to use SPCG solver to solve Pose2 SLAM problem
|
||||
*/
|
||||
|
||||
#include <boost/shared_ptr.hpp>
|
||||
|
||||
#include <gtsam/inference/graph-inl.h>
|
||||
#include <gtsam/inference/FactorGraph-inl.h>
|
||||
#include <gtsam/linear/SubgraphSolver-inl.h>
|
||||
#include <gtsam/nonlinear/NonlinearOptimizer-inl.h>
|
||||
#include <gtsam/nonlinear/NonlinearOptimization-inl.h>
|
||||
#include <gtsam/slam/pose2SLAM.h>
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
using namespace pose2SLAM;
|
||||
|
||||
typedef boost::shared_ptr<Graph> sharedGraph;
|
||||
typedef boost::shared_ptr<Values> sharedValue;
|
||||
|
||||
|
||||
typedef NonlinearOptimizer<Graph, Values, SubgraphPreconditioner, SubgraphSolver<Graph,Values> > SPCGOptimizer;
|
||||
|
||||
sharedGraph G;
|
||||
Values initial;
|
||||
Values result;
|
||||
|
||||
void generateData() ;
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main(void) {
|
||||
|
||||
const bool bNewInterface = true ;
|
||||
const SharedGaussian sigma(noiseModel::Unit::Create(0.1));
|
||||
|
||||
// generate measurement and initial configuration
|
||||
generateData() ;
|
||||
|
||||
if ( bNewInterface == true ) {
|
||||
result = optimizeSPCG(*G, initial, NonlinearOptimizationParameters());
|
||||
}
|
||||
else {
|
||||
cout << "Initialize .... " << endl;
|
||||
SPCGOptimizer::shared_solver solver(new SPCGOptimizer::solver(*G, initial)) ;
|
||||
sharedValue SV(new Values(initial)) ;
|
||||
SPCGOptimizer optimizer(G, SV, solver) ;
|
||||
cout << "before optimization, sum of error is " << optimizer.error() << endl;
|
||||
cout << "Optimize .... " << endl;
|
||||
NonlinearOptimizationParameters parameter;
|
||||
SPCGOptimizer optimizer2 = optimizer.levenbergMarquardt(parameter);
|
||||
cout << "after optimization, sum of error is " << optimizer2.error() << endl;
|
||||
result = *optimizer2.config() ;
|
||||
}
|
||||
|
||||
result.print("result") ;
|
||||
|
||||
return 0 ;
|
||||
}
|
||||
|
||||
void generateData() {
|
||||
|
||||
// noise model
|
||||
const SharedGaussian sigma(noiseModel::Unit::Create(0.1));
|
||||
|
||||
Key x1(1), x2(2), x3(3), x4(4), x5(5), x6(6), x7(7), x8(8), x9(9);
|
||||
|
||||
// create a 3 by 3 grid
|
||||
// x3 x6 x9
|
||||
// x2 x5 x8
|
||||
// x1 x4 x7
|
||||
G = sharedGraph(new Graph) ;
|
||||
G->addConstraint(x1,x2,Pose2(0,2,0),sigma) ;
|
||||
G->addConstraint(x2,x3,Pose2(0,2,0),sigma) ;
|
||||
G->addConstraint(x4,x5,Pose2(0,2,0),sigma) ;
|
||||
G->addConstraint(x5,x6,Pose2(0,2,0),sigma) ;
|
||||
G->addConstraint(x7,x8,Pose2(0,2,0),sigma) ;
|
||||
G->addConstraint(x8,x9,Pose2(0,2,0),sigma) ;
|
||||
G->addConstraint(x1,x4,Pose2(2,0,0),sigma) ;
|
||||
G->addConstraint(x4,x7,Pose2(2,0,0),sigma) ;
|
||||
G->addConstraint(x2,x5,Pose2(2,0,0),sigma) ;
|
||||
G->addConstraint(x5,x8,Pose2(2,0,0),sigma) ;
|
||||
G->addConstraint(x3,x6,Pose2(2,0,0),sigma) ;
|
||||
G->addConstraint(x6,x9,Pose2(2,0,0),sigma) ;
|
||||
G->addPrior(x1, Pose2(0,0,0), sigma) ;
|
||||
|
||||
initial.insert(x1, Pose2( 0, 0, 0));
|
||||
initial.insert(x2, Pose2( 0, 2.1, 0.01));
|
||||
initial.insert(x3, Pose2( 0, 3.9,-0.01));
|
||||
initial.insert(x4, Pose2(2.1,-0.1, 0));
|
||||
initial.insert(x5, Pose2(1.9, 2.1, 0.02));
|
||||
initial.insert(x6, Pose2(2.0, 3.9,-0.02));
|
||||
initial.insert(x7, Pose2(4.0, 0.1, 0.03 ));
|
||||
initial.insert(x8, Pose2(3.9, 2.1, 0.01));
|
||||
initial.insert(x9, Pose2(4.1, 3.9,-0.01));
|
||||
}
|
||||
|
|
@ -0,0 +1,98 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* Atlanta, Georgia 30332-0415
|
||||
* All Rights Reserved
|
||||
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||
|
||||
* See LICENSE for the license information
|
||||
|
||||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/*
|
||||
* Solve a simple 3 by 3 grid of Pose2 SLAM problem by using advanced SPCG interface
|
||||
*/
|
||||
|
||||
#include <boost/shared_ptr.hpp>
|
||||
|
||||
#include <gtsam/inference/graph-inl.h>
|
||||
#include <gtsam/inference/FactorGraph-inl.h>
|
||||
#include <gtsam/linear/SubgraphSolver-inl.h>
|
||||
#include <gtsam/nonlinear/NonlinearOptimizer-inl.h>
|
||||
#include <gtsam/slam/pose2SLAM.h>
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
using namespace pose2SLAM;
|
||||
|
||||
typedef boost::shared_ptr<Graph> sharedGraph;
|
||||
typedef boost::shared_ptr<Values> sharedValue;
|
||||
typedef NonlinearOptimizer<Graph, Values, SubgraphPreconditioner, SubgraphSolver<Graph,Values> > SPCGOptimizer;
|
||||
|
||||
sharedGraph graph;
|
||||
sharedValue initial;
|
||||
Values result;
|
||||
|
||||
void generateData() ;
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main(void) {
|
||||
|
||||
const SharedGaussian sigma(noiseModel::Unit::Create(0.1));
|
||||
generateData() ;
|
||||
|
||||
graph->print("full graph") ;
|
||||
initial->print("initial estimate") ;
|
||||
|
||||
SPCGOptimizer::shared_solver solver(new SPCGOptimizer::solver(*graph, *initial)) ;
|
||||
SPCGOptimizer optimizer(graph, initial, solver) ;
|
||||
|
||||
cout << "before optimization, sum of error is " << optimizer.error() << endl;
|
||||
NonlinearOptimizationParameters parameter;
|
||||
SPCGOptimizer optimizer2 = optimizer.levenbergMarquardt(parameter);
|
||||
cout << "after optimization, sum of error is " << optimizer2.error() << endl;
|
||||
|
||||
result = *optimizer2.config() ;
|
||||
result.print("final result") ;
|
||||
|
||||
return 0 ;
|
||||
}
|
||||
|
||||
void generateData() {
|
||||
|
||||
// noise model
|
||||
const SharedGaussian sigma(noiseModel::Unit::Create(0.1));
|
||||
|
||||
Key x1(1), x2(2), x3(3), x4(4), x5(5), x6(6), x7(7), x8(8), x9(9);
|
||||
|
||||
graph = boost::make_shared<Graph>() ;
|
||||
initial = boost::make_shared<Values>() ;
|
||||
|
||||
// create a 3 by 3 grid
|
||||
// x3 x6 x9
|
||||
// x2 x5 x8
|
||||
// x1 x4 x7
|
||||
graph->addConstraint(x1,x2,Pose2(0,2,0),sigma) ;
|
||||
graph->addConstraint(x2,x3,Pose2(0,2,0),sigma) ;
|
||||
graph->addConstraint(x4,x5,Pose2(0,2,0),sigma) ;
|
||||
graph->addConstraint(x5,x6,Pose2(0,2,0),sigma) ;
|
||||
graph->addConstraint(x7,x8,Pose2(0,2,0),sigma) ;
|
||||
graph->addConstraint(x8,x9,Pose2(0,2,0),sigma) ;
|
||||
graph->addConstraint(x1,x4,Pose2(2,0,0),sigma) ;
|
||||
graph->addConstraint(x4,x7,Pose2(2,0,0),sigma) ;
|
||||
graph->addConstraint(x2,x5,Pose2(2,0,0),sigma) ;
|
||||
graph->addConstraint(x5,x8,Pose2(2,0,0),sigma) ;
|
||||
graph->addConstraint(x3,x6,Pose2(2,0,0),sigma) ;
|
||||
graph->addConstraint(x6,x9,Pose2(2,0,0),sigma) ;
|
||||
graph->addPrior(x1, Pose2(0,0,0), sigma) ;
|
||||
|
||||
initial->insert(x1, Pose2( 0, 0, 0));
|
||||
initial->insert(x2, Pose2( 0, 2.1, 0.01));
|
||||
initial->insert(x3, Pose2( 0, 3.9,-0.01));
|
||||
initial->insert(x4, Pose2(2.1,-0.1, 0));
|
||||
initial->insert(x5, Pose2(1.9, 2.1, 0.02));
|
||||
initial->insert(x6, Pose2(2.0, 3.9,-0.02));
|
||||
initial->insert(x7, Pose2(4.0, 0.1, 0.03 ));
|
||||
initial->insert(x8, Pose2(3.9, 2.1, 0.01));
|
||||
initial->insert(x9, Pose2(4.1, 3.9,-0.01));
|
||||
}
|
||||
|
|
@ -0,0 +1,79 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* Atlanta, Georgia 30332-0415
|
||||
* All Rights Reserved
|
||||
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||
|
||||
* See LICENSE for the license information
|
||||
|
||||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/*
|
||||
* Solve a simple 3 by 3 grid of Pose2 SLAM problem by using easy SPCG interface
|
||||
*/
|
||||
|
||||
|
||||
#include <boost/shared_ptr.hpp>
|
||||
|
||||
#include <gtsam/inference/graph-inl.h>
|
||||
#include <gtsam/inference/FactorGraph-inl.h>
|
||||
#include <gtsam/nonlinear/NonlinearOptimization-inl.h>
|
||||
#include <gtsam/slam/pose2SLAM.h>
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
using namespace pose2SLAM;
|
||||
|
||||
Graph graph;
|
||||
Values initial;
|
||||
Values result;
|
||||
|
||||
void generateData() ;
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main(void) {
|
||||
|
||||
generateData() ;
|
||||
graph.print("full graph") ;
|
||||
initial.print("initial estimate");
|
||||
result = optimizeSPCG(graph, initial);
|
||||
result.print("final result") ;
|
||||
return 0 ;
|
||||
}
|
||||
|
||||
void generateData() {
|
||||
|
||||
// noise model
|
||||
const SharedGaussian sigma(noiseModel::Unit::Create(0.1));
|
||||
|
||||
Key x1(1), x2(2), x3(3), x4(4), x5(5), x6(6), x7(7), x8(8), x9(9);
|
||||
|
||||
// create a 3 by 3 grid
|
||||
// x3 x6 x9
|
||||
// x2 x5 x8
|
||||
// x1 x4 x7
|
||||
graph.addConstraint(x1,x2,Pose2(0,2,0),sigma) ;
|
||||
graph.addConstraint(x2,x3,Pose2(0,2,0),sigma) ;
|
||||
graph.addConstraint(x4,x5,Pose2(0,2,0),sigma) ;
|
||||
graph.addConstraint(x5,x6,Pose2(0,2,0),sigma) ;
|
||||
graph.addConstraint(x7,x8,Pose2(0,2,0),sigma) ;
|
||||
graph.addConstraint(x8,x9,Pose2(0,2,0),sigma) ;
|
||||
graph.addConstraint(x1,x4,Pose2(2,0,0),sigma) ;
|
||||
graph.addConstraint(x4,x7,Pose2(2,0,0),sigma) ;
|
||||
graph.addConstraint(x2,x5,Pose2(2,0,0),sigma) ;
|
||||
graph.addConstraint(x5,x8,Pose2(2,0,0),sigma) ;
|
||||
graph.addConstraint(x3,x6,Pose2(2,0,0),sigma) ;
|
||||
graph.addConstraint(x6,x9,Pose2(2,0,0),sigma) ;
|
||||
graph.addPrior(x1, Pose2(0,0,0), sigma) ;
|
||||
|
||||
initial.insert(x1, Pose2( 0, 0, 0));
|
||||
initial.insert(x2, Pose2( 0, 2.1, 0.01));
|
||||
initial.insert(x3, Pose2( 0, 3.9,-0.01));
|
||||
initial.insert(x4, Pose2(2.1,-0.1, 0));
|
||||
initial.insert(x5, Pose2(1.9, 2.1, 0.02));
|
||||
initial.insert(x6, Pose2(2.0, 3.9,-0.02));
|
||||
initial.insert(x7, Pose2(4.0, 0.1, 0.03 ));
|
||||
initial.insert(x8, Pose2(3.9, 2.1, 0.01));
|
||||
initial.insert(x9, Pose2(4.1, 3.9,-0.01));
|
||||
}
|
||||
|
|
@ -54,7 +54,7 @@ int main() {
|
|||
|
||||
// Create a factor
|
||||
Rot2 prior1 = Rot2::fromAngle(30 * degree);
|
||||
prior1.print("Goal Angle");
|
||||
prior1.print("goal angle");
|
||||
SharedDiagonal model1 = noiseModel::Isotropic::Sigma(1, 1 * degree);
|
||||
Key key1(1);
|
||||
PriorFactor<Values, Key> factor1(key1, prior1, model1);
|
||||
|
|
@ -62,17 +62,15 @@ int main() {
|
|||
// Create a factor graph
|
||||
Graph graph;
|
||||
graph.add(factor1);
|
||||
graph.print("full graph") ;
|
||||
|
||||
// and an initial estimate
|
||||
Values initialEstimate;
|
||||
initialEstimate.insert(key1, Rot2::fromAngle(20 * degree));
|
||||
initialEstimate.print("Initialization");
|
||||
Values initial;
|
||||
initial.insert(key1, Rot2::fromAngle(20 * degree));
|
||||
initial.print("initial estimate");
|
||||
|
||||
// create an ordering
|
||||
Optimizer::Parameters parameters;
|
||||
parameters.verbosity_ = Optimizer::Parameters::LAMBDA;
|
||||
Values result = optimize<Graph, Values>(graph, initialEstimate, parameters);
|
||||
result.print("Final config");
|
||||
Values result = optimize<Graph, Values>(graph, initial);
|
||||
result.print("final result");
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue