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

Synchronized mixed case naming

release/4.3a0
Frank Dellaert 2012-06-03 05:25:05 +00:00
parent 608155851a
commit f924d01bc3
7 changed files with 57 additions and 70 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
examples/LocalizationExample.cpp
View File

@ -55,7 +55,7 @@ int main(int argc, char** argv) {
// create (deliberatly inaccurate) initial estimate // create (deliberatly inaccurate) initial estimate
pose2SLAM::Values initialEstimate; pose2SLAM::Values initialEstimate;
initialEstimate.insertPose(1, Pose2(0.5, 0.0, 0.2)); initialEstimate.insertPose(1, Pose2(0.5, 0.0, 0.2));
initialEstimate.insertPose(2, Pose2(2.3, 0.1,-0.2)); initialEstimate.insertPose(2, Pose2(2.3, 0.1, -0.2));
initialEstimate.insertPose(3, Pose2(4.1, 0.1, 0.1)); initialEstimate.insertPose(3, Pose2(4.1, 0.1, 0.1));
initialEstimate.print("\nInitial estimate:\n "); initialEstimate.print("\nInitial estimate:\n ");
@ -64,8 +64,8 @@ int main(int argc, char** argv) {
result.print("\nFinal result:\n "); result.print("\nFinal result:\n ");
// Query the marginals // Query the marginals
Marginals marginals = graph.marginals(result);
cout.precision(2); cout.precision(2);
Marginals marginals = graph.marginals(result);
cout << "\nP1:\n" << marginals.marginalCovariance(1) << endl; cout << "\nP1:\n" << marginals.marginalCovariance(1) << endl;
cout << "\nP2:\n" << marginals.marginalCovariance(2) << endl; cout << "\nP2:\n" << marginals.marginalCovariance(2) << endl;
cout << "\nP3:\n" << marginals.marginalCovariance(3) << endl; cout << "\nP3:\n" << marginals.marginalCovariance(3) << endl;

14
examples/PlanarSLAMSelfContained_advanced.cpp
View File

@ -60,18 +60,18 @@ int main(int argc, char** argv) {
/* add prior */ /* add prior */
// gaussian for prior // gaussian for prior
SharedDiagonal prior_model = noiseModel::Diagonal::Sigmas(Vector_(3, 0.3, 0.3, 0.1)); SharedDiagonal priorNoise = noiseModel::Diagonal::Sigmas(Vector_(3, 0.3, 0.3, 0.1));
Pose2 prior_measurement(0.0, 0.0, 0.0); // prior at origin Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin
PriorFactor<Pose2> posePrior(i1, prior_measurement, prior_model); // create the factor PriorFactor<Pose2> posePrior(i1, priorMean, priorNoise); // create the factor
graph.add(posePrior); // add the factor to the graph graph.add(posePrior); // add the factor to the graph
/* add odometry */ /* add odometry */
// general noisemodel for odometry // general noisemodel for odometry
SharedDiagonal odom_model = noiseModel::Diagonal::Sigmas(Vector_(3, 0.2, 0.2, 0.1)); SharedDiagonal odometryNoise = noiseModel::Diagonal::Sigmas(Vector_(3, 0.2, 0.2, 0.1));
Pose2 odom_measurement(2.0, 0.0, 0.0); // create a measurement for both factors (the same in this case) Pose2 odometry(2.0, 0.0, 0.0); // create a measurement for both factors (the same in this case)
// create between factors to represent odometry // create between factors to represent odometry
BetweenFactor<Pose2> odom12(i1, i2, odom_measurement, odom_model); BetweenFactor<Pose2> odom12(i1, i2, odometry, odometryNoise);
BetweenFactor<Pose2> odom23(i2, i3, odom_measurement, odom_model); BetweenFactor<Pose2> odom23(i2, i3, odometry, odometryNoise);
graph.add(odom12); // add both to graph graph.add(odom12); // add both to graph
graph.add(odom23); graph.add(odom23);

20
examples/Pose2SLAMExample_advanced.cpp
View File

@ -35,26 +35,22 @@ int main(int argc, char** argv) {
pose2SLAM::Graph graph; pose2SLAM::Graph graph;
/* 2.a add prior */ /* 2.a add prior */
// gaussian for prior Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin
SharedDiagonal prior_model = noiseModel::Diagonal::Sigmas(Vector_(3, 0.3, 0.3, 0.1)); SharedDiagonal priorNoise(Vector_(3, 0.3, 0.3, 0.1)); // 30cm std on x,y, 0.1 rad on theta
Pose2 prior_measurement(0.0, 0.0, 0.0); // prior at origin graph.addPrior(1, priorMean, priorNoise); // add directly to graph
graph.addPrior(1, prior_measurement, prior_model); // add directly to graph
/* 2.b add odometry */ /* 2.b add odometry */
// general noisemodel for odometry SharedDiagonal odometryNoise(Vector_(3, 0.2, 0.2, 0.1)); // 20cm std on x,y, 0.1 rad on theta
SharedDiagonal odom_model = noiseModel::Diagonal::Sigmas(Vector_(3, 0.2, 0.2, 0.1)); Pose2 odometry(2.0, 0.0, 0.0); // create a measurement for both factors (the same in this case)
graph.addOdometry(1, 2, odometry, odometryNoise);
/* Pose2 measurements take (x,y,theta), where theta is taken from the positive x-axis*/ graph.addOdometry(2, 3, odometry, odometryNoise);
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.print("full graph");
/* 3. Create the data structure to hold the initial estimate to the solution /* 3. Create the data structure to hold the initial estimate to the solution
* initialize to noisy points */ * initialize to noisy points */
pose2SLAM::Values initial; pose2SLAM::Values initial;
initial.insertPose(1, Pose2(0.5, 0.0, 0.2)); initial.insertPose(1, Pose2(0.5, 0.0, 0.2));
initial.insertPose(2, Pose2(2.3, 0.1,-0.2)); initial.insertPose(2, Pose2(2.3, 0.1, -0.2));
initial.insertPose(3, Pose2(4.1, 0.1, 0.1)); initial.insertPose(3, Pose2(4.1, 0.1, 0.1));
initial.print("initial estimate"); initial.print("initial estimate");

6
examples/matlab/LocalizationExample.m
View File

@ -45,9 +45,9 @@ result.print(sprintf('\nFinal result:\n '));
%% Query the marginals %% Query the marginals
marginals = graph.marginals(result); marginals = graph.marginals(result);
P{1}=marginals.marginalCovariance(1) P{1}=marginals.marginalCovariance(1);
P{2}=marginals.marginalCovariance(2) P{2}=marginals.marginalCovariance(2);
P{3}=marginals.marginalCovariance(3) P{3}=marginals.marginalCovariance(3);
%% Plot Trajectory %% Plot Trajectory
figure(1) figure(1)

28
examples/matlab/Pose2SLAMExample_advanced.m
View File

@ -20,24 +20,21 @@
% - We have full odometry for measurements % - We have full odometry for measurements
% - The robot is on a grid, moving 2 meters each step % - The robot is on a grid, moving 2 meters each step
%% Create keys for variables
x1 = 1; x2 = 2; x3 = 3;
%% Create graph container and add factors to it %% Create graph container and add factors to it
graph = pose2SLAMGraph; graph = pose2SLAMGraph;
%% Add prior %% Add prior
% gaussian for prior % gaussian for prior
prior_model = gtsamSharedNoiseModel_Sigmas([0.3; 0.3; 0.1]); priorNoise = gtsamSharedNoiseModel_Sigmas([0.3; 0.3; 0.1]);
prior_measurement = gtsamPose2(0.0, 0.0, 0.0); % prior at origin priorMean = gtsamPose2(0.0, 0.0, 0.0); % prior at origin
graph.addPrior(x1, prior_measurement, prior_model); % add directly to graph graph.addPrior(1, priorMean, priorNoise); % add directly to graph
%% Add odometry %% Add odometry
% general noisemodel for odometry % general noisemodel for odometry
odom_model = gtsamSharedNoiseModel_Sigmas([0.2; 0.2; 0.1]); odometryNoise = gtsamSharedNoiseModel_Sigmas([0.2; 0.2; 0.1]);
odom_measurement = gtsamPose2(2.0, 0.0, 0.0); % create a measurement for both factors (the same in this case) odometry = gtsamPose2(2.0, 0.0, 0.0); % create a measurement for both factors (the same in this case)
graph.addOdometry(x1, x2, odom_measurement, odom_model); graph.addOdometry(1, 2, odometry, odometryNoise);
graph.addOdometry(x2, x3, odom_measurement, odom_model); graph.addOdometry(2, 3, odometry, odometryNoise);
%% Add measurements %% Add measurements
% general noisemodel for measurements % general noisemodel for measurements
@ -48,9 +45,9 @@ graph.print('full graph');
%% Initialize to noisy points %% Initialize to noisy points
initialEstimate = pose2SLAMValues; initialEstimate = pose2SLAMValues;
initialEstimate.insertPose(x1, gtsamPose2(0.5, 0.0, 0.2)); initialEstimate.insertPose(1, gtsamPose2(0.5, 0.0, 0.2));
initialEstimate.insertPose(x2, gtsamPose2(2.3, 0.1,-0.2)); initialEstimate.insertPose(2, gtsamPose2(2.3, 0.1,-0.2));
initialEstimate.insertPose(x3, gtsamPose2(4.1, 0.1, 0.1)); initialEstimate.insertPose(3, gtsamPose2(4.1, 0.1, 0.1));
initialEstimate.print('initial estimate'); initialEstimate.print('initial estimate');
@ -62,10 +59,7 @@ initialEstimate.print('initial estimate');
%result = pose2SLAMOptimizer(graph,initialEstimate,ord,params); %result = pose2SLAMOptimizer(graph,initialEstimate,ord,params);
%result.print('final result'); %result.print('final result');
% % %% Optimize using Levenberg-Marquardt optimization with an ordering from colamd
% % disp('\\\');
% %
% % %% Optimize using Levenberg-Marquardt optimization with an ordering from colamd
result = graph.optimize(initialEstimate); result = graph.optimize(initialEstimate);
result.print('final result'); result.print('final result');

23
examples/matlab/Pose2SLAMExample_easy.m
View File

@ -18,24 +18,21 @@
% - We have full odometry for measurements % - We have full odometry for measurements
% - The robot is on a grid, moving 2 meters each step % - The robot is on a grid, moving 2 meters each step
%% Create keys for variables
x1 = 1; x2 = 2; x3 = 3;
%% Create graph container and add factors to it %% Create graph container and add factors to it
graph = pose2SLAMGraph; graph = pose2SLAMGraph;
%% Add prior %% Add prior
% gaussian for prior % gaussian for prior
prior_model = gtsamSharedNoiseModel_Sigmas([0.3; 0.3; 0.1]); priorNoise = gtsamSharedNoiseModel_Sigmas([0.3; 0.3; 0.1]);
prior_measurement = gtsamPose2(0.0, 0.0, 0.0); % prior at origin priorMean = gtsamPose2(0.0, 0.0, 0.0); % prior at origin
graph.addPrior(x1, prior_measurement, prior_model); % add directly to graph graph.addPrior(1, priorMean, priorNoise); % add directly to graph
%% Add odometry %% Add odometry
% general noisemodel for odometry % general noisemodel for odometry
odom_model = gtsamSharedNoiseModel_Sigmas([0.2; 0.2; 0.1]); odometryNoise = gtsamSharedNoiseModel_Sigmas([0.2; 0.2; 0.1]);
odom_measurement = gtsamPose2(2.0, 0.0, 0.0); % create a measurement for both factors (the same in this case) odometry = gtsamPose2(2.0, 0.0, 0.0); % create a measurement for both factors (the same in this case)
graph.addOdometry(x1, x2, odom_measurement, odom_model); graph.addOdometry(1, 2, odometry, odometryNoise);
graph.addOdometry(x2, x3, odom_measurement, odom_model); graph.addOdometry(2, 3, odometry, odometryNoise);
%% Add measurements %% Add measurements
% general noisemodel for measurements % general noisemodel for measurements
@ -46,9 +43,9 @@ graph.print('full graph');
%% Initialize to noisy points %% Initialize to noisy points
initialEstimate = pose2SLAMValues; initialEstimate = pose2SLAMValues;
initialEstimate.insertPose(x1, gtsamPose2(0.5, 0.0, 0.2)); initialEstimate.insertPose(1, gtsamPose2(0.5, 0.0, 0.2));
initialEstimate.insertPose(x2, gtsamPose2(2.3, 0.1,-0.2)); initialEstimate.insertPose(2, gtsamPose2(2.3, 0.1,-0.2));
initialEstimate.insertPose(x3, gtsamPose2(4.1, 0.1, 0.1)); initialEstimate.insertPose(3, gtsamPose2(4.1, 0.1, 0.1));
initialEstimate.print('initial estimate'); initialEstimate.print('initial estimate');

14
tests/testMarginals.cpp
View File

@ -51,17 +51,17 @@ TEST(Marginals, planarSLAMmarginals) {
/* add prior */ /* add prior */
// gaussian for prior // gaussian for prior
SharedDiagonal prior_model = noiseModel::Diagonal::Sigmas(Vector_(3, 0.3, 0.3, 0.1)); SharedDiagonal priorNoise = noiseModel::Diagonal::Sigmas(Vector_(3, 0.3, 0.3, 0.1));
Pose2 prior_measurement(0.0, 0.0, 0.0); // prior at origin Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin
graph.add(PriorFactor<Pose2>(x1, prior_measurement, prior_model)); // add the factor to the graph graph.add(PriorFactor<Pose2>(x1, priorMean, priorNoise)); // add the factor to the graph
/* add odometry */ /* add odometry */
// general noisemodel for odometry // general noisemodel for odometry
SharedDiagonal odom_model = noiseModel::Diagonal::Sigmas(Vector_(3, 0.2, 0.2, 0.1)); SharedDiagonal odometryNoise = noiseModel::Diagonal::Sigmas(Vector_(3, 0.2, 0.2, 0.1));
Pose2 odom_measurement(2.0, 0.0, 0.0); // create a measurement for both factors (the same in this case) Pose2 odometry(2.0, 0.0, 0.0); // create a measurement for both factors (the same in this case)
// create between factors to represent odometry // create between factors to represent odometry
graph.add(BetweenFactor<Pose2>(x1, x2, odom_measurement, odom_model)); graph.add(BetweenFactor<Pose2>(x1, x2, odometry, odometryNoise));
graph.add(BetweenFactor<Pose2>(x2, x3, odom_measurement, odom_model)); graph.add(BetweenFactor<Pose2>(x2, x3, odometry, odometryNoise));
/* add measurements */ /* add measurements */
// general noisemodel for measurements // general noisemodel for measurements