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

Updated several codes.

release/4.3a0
yao 2016-06-15 13:18:23 -04:00
parent da480844b9
commit 7c8e1f80ce
4 changed files with 9 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
doc/Code/LocalizationExample2.cpp
View File

@ -1,6 +1,6 @@
// add unary measurement factors, like GPS, on all three poses
noiseModel::Diagonal::shared_ptr unaryNoise =
unaryNoise::Diagonal::Sigmas((Vector(2)<< 0.1, 0.1)); // 10cm std on x,y
unaryNoise::Diagonal::Sigmas(Vector2(0.1, 0.1)); // 10cm std on x,y
graph.add(boost::make_shared<UnaryFactor>(1, 0.0, 0.0, unaryNoise));
graph.add(boost::make_shared<UnaryFactor>(2, 2.0, 0.0, unaryNoise));
graph.add(boost::make_shared<UnaryFactor>(3, 4.0, 0.0, unaryNoise));

4
doc/Code/LocalizationFactor.cpp
View File

@ -8,7 +8,7 @@ public:
Vector evaluateError(const Pose2& q,
boost::optional<Matrix&> H = boost::none) const
{
if (H) (*H) = (Matrix(2,3)<< 1.0,0.0,0.0, 0.0,1.0,0.0);
return (Vector(2) << q.x() - mx_, q.y() - my_);
if (H) (*H) = (Matrix(2,3)<< 1.0,0.0,0.0, 0.0,1.0,0.0).finished();
return (Vector(2) << q.x() - mx_, q.y() - my_).finished();
}
};

4
doc/Code/OdometryExample.cpp
View File

@ -4,12 +4,12 @@ NonlinearFactorGraph graph;
// Add a Gaussian prior on pose x_1
Pose2 priorMean(0.0, 0.0, 0.0);
noiseModel::Diagonal::shared_ptr priorNoise =
noiseModel::Diagonal::Sigmas((Vector(3)<< 0.3, 0.3, 0.1));
noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
graph.add(PriorFactor<Pose2>(1, priorMean, priorNoise));
// Add two odometry factors
Pose2 odometry(2.0, 0.0, 0.0);
noiseModel::Diagonal::shared_ptr odometryNoise =
noiseModel::Diagonal::Sigmas((Vector(3)<< 0.2, 0.2, 0.1));
noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
graph.add(BetweenFactor<Pose2>(1, 2, odometry, odometryNoise));
graph.add(BetweenFactor<Pose2>(2, 3, odometry, odometryNoise));

8
doc/Code/Pose2SLAMExample.cpp
View File

@ -1,16 +1,16 @@
NonlinearFactorGraph graph;
noiseModel::Diagonal::shared_ptr priorNoise =
noiseModel::Diagonal::Sigmas((Vector(3)<< 0.3, 0.3, 0.1));
graph.add(PriorFactor<Pose2>(1, Pose2(0,0,0), priorNoise));
noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
graph.add(PriorFactor<Pose2>(1, Pose2(0, 0, 0), priorNoise));
// Add odometry factors
noiseModel::Diagonal::shared_ptr model =
noiseModel::Diagonal::Sigmas((Vector(3)<< 0.2, 0.2, 0.1));
noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
graph.add(BetweenFactor<Pose2>(1, 2, Pose2(2, 0, 0 ), model));
graph.add(BetweenFactor<Pose2>(2, 3, Pose2(2, 0, M_PI_2), model));
graph.add(BetweenFactor<Pose2>(3, 4, Pose2(2, 0, M_PI_2), model));
graph.add(BetweenFactor<Pose2>(4, 5, Pose2(2, 0, M_PI_2), model));
// Add pose constraint
// Add the loop closure constraint
graph.add(BetweenFactor<Pose2>(5, 2, Pose2(2, 0, M_PI_2), model));