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

12
examples/LocalizationExample.cpp
View File

@ -35,7 +35,7 @@ using namespace gtsam;
*/
int main(int argc, char** argv) {
// create the graph (defined in pose2SLAM.h, derived from NonlinearFactorGraph)
// create the graph (defined in pose2SLAM.h, derived from NonlinearFactorGraph)
pose2SLAM::Graph graph;
// add a Gaussian prior on pose x_1
@ -55,7 +55,7 @@ int main(int argc, char** argv) {
// create (deliberatly inaccurate) initial estimate
pose2SLAM::Values initialEstimate;
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.print("\nInitial estimate:\n ");
@ -64,11 +64,11 @@ int main(int argc, char** argv) {
result.print("\nFinal result:\n ");
// Query the marginals
Marginals marginals = graph.marginals(result);
cout.precision(2);
cout << "\nP1:\n" << marginals.marginalCovariance(1) << endl;
cout << "\nP2:\n" << marginals.marginalCovariance(2) << endl;
cout << "\nP3:\n" << marginals.marginalCovariance(3) << endl;
Marginals marginals = graph.marginals(result);
cout << "\nP1:\n" << marginals.marginalCovariance(1) << endl;
cout << "\nP2:\n" << marginals.marginalCovariance(2) << endl;
cout << "\nP3:\n" << marginals.marginalCovariance(3) << endl;
return 0;
}

14
examples/PlanarSLAMSelfContained_advanced.cpp
View File

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

30
examples/Pose2SLAMExample_advanced.cpp
View File

@ -35,26 +35,22 @@ int main(int argc, char** argv) {
pose2SLAM::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
Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin
SharedDiagonal priorNoise(Vector_(3, 0.3, 0.3, 0.1)); // 30cm std on x,y, 0.1 rad on theta
graph.addPrior(1, priorMean, priorNoise); // 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.addOdometry(1, 2, odom_measurement, odom_model);
graph.addOdometry(2, 3, odom_measurement, odom_model);
SharedDiagonal odometryNoise(Vector_(3, 0.2, 0.2, 0.1)); // 20cm std on x,y, 0.1 rad on theta
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);
graph.addOdometry(2, 3, odometry, odometryNoise);
graph.print("full graph");
/* 3. Create the data structure to hold the initial estimate to the solution
* initialize to noisy points */
/* 3. Create the data structure to hold the initial estimate to the solution
* initialize to noisy points */
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(2, Pose2(2.3, 0.1, -0.2));
initial.insertPose(3, Pose2(4.1, 0.1, 0.1));
initial.print("initial estimate");
@ -67,15 +63,15 @@ int main(int argc, char** argv) {
params.absoluteErrorTol = 1e-15;
params.relativeErrorTol = 1e-15;
params.ordering = ordering;
LevenbergMarquardtOptimizer optimizer(graph, initial, params);
LevenbergMarquardtOptimizer optimizer(graph, initial, params);
pose2SLAM::Values result = optimizer.optimize();
result.print("final result");
/* Get covariances */
Marginals marginals(graph, result, Marginals::CHOLESKY);
Matrix covariance1 = marginals.marginalCovariance(1);
Matrix covariance2 = marginals.marginalCovariance(2);
Matrix covariance1 = marginals.marginalCovariance(1);
Matrix covariance2 = marginals.marginalCovariance(2);
print(covariance1, "Covariance1");
print(covariance2, "Covariance2");

6
examples/matlab/LocalizationExample.m
View File

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

28
examples/matlab/Pose2SLAMExample_advanced.m
View File

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

23
examples/matlab/Pose2SLAMExample_easy.m
View File

@ -18,24 +18,21 @@
% - We have full odometry for measurements
% - 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
graph = pose2SLAMGraph;
%% Add prior
% gaussian for prior
prior_model = gtsamSharedNoiseModel_Sigmas([0.3; 0.3; 0.1]);
prior_measurement = gtsamPose2(0.0, 0.0, 0.0); % prior at origin
graph.addPrior(x1, prior_measurement, prior_model); % add directly to graph
priorNoise = gtsamSharedNoiseModel_Sigmas([0.3; 0.3; 0.1]);
priorMean = gtsamPose2(0.0, 0.0, 0.0); % prior at origin
graph.addPrior(1, priorMean, priorNoise); % add directly to graph
%% Add odometry
% general noisemodel for odometry
odom_model = 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)
graph.addOdometry(x1, x2, odom_measurement, odom_model);
graph.addOdometry(x2, x3, odom_measurement, odom_model);
odometryNoise = gtsamSharedNoiseModel_Sigmas([0.2; 0.2; 0.1]);
odometry = gtsamPose2(2.0, 0.0, 0.0); % create a measurement for both factors (the same in this case)
graph.addOdometry(1, 2, odometry, odometryNoise);
graph.addOdometry(2, 3, odometry, odometryNoise);
%% Add measurements
% general noisemodel for measurements
@ -46,9 +43,9 @@ graph.print('full graph');
%% Initialize to noisy points
initialEstimate = pose2SLAMValues;
initialEstimate.insertPose(x1, gtsamPose2(0.5, 0.0, 0.2));
initialEstimate.insertPose(x2, gtsamPose2(2.3, 0.1,-0.2));
initialEstimate.insertPose(x3, gtsamPose2(4.1, 0.1, 0.1));
initialEstimate.insertPose(1, gtsamPose2(0.5, 0.0, 0.2));
initialEstimate.insertPose(2, gtsamPose2(2.3, 0.1,-0.2));
initialEstimate.insertPose(3, gtsamPose2(4.1, 0.1, 0.1));
initialEstimate.print('initial estimate');

14
tests/testMarginals.cpp
View File

@ -51,17 +51,17 @@ TEST(Marginals, planarSLAMmarginals) {
/* 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.add(PriorFactor<Pose2>(x1, prior_measurement, prior_model)); // add the factor to the graph
SharedDiagonal priorNoise = noiseModel::Diagonal::Sigmas(Vector_(3, 0.3, 0.3, 0.1));
Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin
graph.add(PriorFactor<Pose2>(x1, priorMean, priorNoise)); // add the factor to the graph
/* add odometry */
// general noisemodel for odometry
SharedDiagonal odom_model = 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)
SharedDiagonal odometryNoise = 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)
// create between factors to represent odometry
graph.add(BetweenFactor<Pose2>(x1, x2, odom_measurement, odom_model));
graph.add(BetweenFactor<Pose2>(x2, x3, odom_measurement, odom_model));
graph.add(BetweenFactor<Pose2>(x1, x2, odometry, odometryNoise));
graph.add(BetweenFactor<Pose2>(x2, x3, odometry, odometryNoise));
/* add measurements */
// general noisemodel for measurements