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

Added 'optimize' methods to all the namespace graphs, and general clean-up/consistency between namespaces

release/4.3a0
Stephen Williams 2012-05-15 18:48:25 +00:00
parent 3dc3f93145
commit 0237328ab2
9 changed files with 157 additions and 117 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
gtsam/slam/planarSLAM.cpp
View File

@ -21,39 +21,48 @@
// Use planarSLAM namespace for specific SLAM instance
namespace planarSLAM {
/* ************************************************************************* */
Graph::Graph(const NonlinearFactorGraph& graph) :
NonlinearFactorGraph(graph) {}
/* ************************************************************************* */
void Graph::addPrior(Index i, const Pose2& p, const SharedNoiseModel& model) {
sharedFactor factor(new Prior(PoseKey(i), p, model));
push_back(factor);
}
/* ************************************************************************* */
void Graph::addPoseConstraint(Index i, const Pose2& p) {
sharedFactor factor(new Constraint(PoseKey(i), p));
push_back(factor);
}
/* ************************************************************************* */
void Graph::addOdometry(Index i, Index j, const Pose2& odometry, const SharedNoiseModel& model) {
sharedFactor factor(new Odometry(PoseKey(i), PoseKey(j), odometry, model));
push_back(factor);
}
/* ************************************************************************* */
void Graph::addBearing(Index i, Index j, const Rot2& z, const SharedNoiseModel& model) {
sharedFactor factor(new Bearing(PoseKey(i), PointKey(j), z, model));
push_back(factor);
}
/* ************************************************************************* */
void Graph::addRange(Index i, Index j, double z, const SharedNoiseModel& model) {
sharedFactor factor(new Range(PoseKey(i), PointKey(j), z, model));
push_back(factor);
}
/* ************************************************************************* */
void Graph::addBearingRange(Index i, Index j, const Rot2& z1,
double z2, const SharedNoiseModel& model) {
sharedFactor factor(new BearingRange(PoseKey(i), PointKey(j), z1, z2, model));
push_back(factor);
}
/* ************************************************************************* */
} // planarSLAM

1
gtsam/slam/planarSLAM.h
View File

@ -25,7 +25,6 @@
#include <gtsam/nonlinear/NonlinearEquality.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/NonlinearOptimizer.h>
#include <gtsam/geometry/Pose2.h>
// Use planarSLAM namespace for specific SLAM instance

6
gtsam/slam/pose2SLAM.cpp
View File

@ -16,8 +16,6 @@
**/
#include <gtsam/slam/pose2SLAM.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/NonlinearOptimizer.h>
// Use pose2SLAM namespace for specific SLAM instance
@ -39,17 +37,19 @@ namespace pose2SLAM {
push_back(factor);
}
/* ************************************************************************* */
void Graph::addPoseConstraint(Index i, const Pose2& p) {
sharedFactor factor(new HardConstraint(PoseKey(i), p));
push_back(factor);
}
/* ************************************************************************* */
void Graph::addOdometry(Index i, Index j, const Pose2& z,
const SharedNoiseModel& model) {
sharedFactor factor(new Odometry(PoseKey(i), PoseKey(j), z, model));
push_back(factor);
}
/* ************************************************************************* */
/* ************************************************************************* */
} // pose2SLAM

4
gtsam/slam/pose2SLAM.h
View File

@ -25,8 +25,6 @@
#include <gtsam/nonlinear/NonlinearEquality.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/linear/GaussianSequentialSolver.h>
#include <gtsam/linear/GaussianMultifrontalSolver.h>
// Use pose2SLAM namespace for specific SLAM instance
namespace pose2SLAM {
@ -96,7 +94,7 @@ namespace pose2SLAM {
const SharedNoiseModel& model);
/// Optimize
Values optimize(const Values& initialEstimate) {
Values optimize(const Values& initialEstimate) const {
return LevenbergMarquardtOptimizer(*this, initialEstimate).optimize();
}
};

77
gtsam/slam/pose3SLAM.cpp
View File

@ -17,51 +17,48 @@
#include <gtsam/slam/pose3SLAM.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/NonlinearOptimizer.h>
// Use pose3SLAM namespace for specific SLAM instance
namespace gtsam {
namespace pose3SLAM {
namespace pose3SLAM {
/* ************************************************************************* */
Values circle(size_t n, double radius) {
Values x;
double theta = 0, dtheta = 2 * M_PI / n;
// We use aerospace/navlab convention, X forward, Y right, Z down
// First pose will be at (R,0,0)
// ^y ^ X
// | |
// z-->xZ--> Y (z pointing towards viewer, Z pointing away from viewer)
// Vehicle at p0 is looking towards y axis (X-axis points towards world y)
Rot3 gR0(Point3(0, 1, 0), Point3(1, 0, 0), Point3(0, 0, -1));
for (size_t i = 0; i < n; i++, theta += dtheta) {
Point3 gti(radius*cos(theta), radius*sin(theta), 0);
Rot3 _0Ri = Rot3::yaw(-theta); // negative yaw goes counterclockwise, with Z down !
Pose3 gTi(gR0 * _0Ri, gti);
x.insert(PoseKey(i), gTi);
}
return x;
}
/* ************************************************************************* */
Values circle(size_t n, double radius) {
Values x;
double theta = 0, dtheta = 2 * M_PI / n;
// We use aerospace/navlab convention, X forward, Y right, Z down
// First pose will be at (R,0,0)
// ^y ^ X
// | |
// z-->xZ--> Y (z pointing towards viewer, Z pointing away from viewer)
// Vehicle at p0 is looking towards y axis (X-axis points towards world y)
Rot3 gR0(Point3(0, 1, 0), Point3(1, 0, 0), Point3(0, 0, -1));
for (size_t i = 0; i < n; i++, theta += dtheta) {
Point3 gti(radius*cos(theta), radius*sin(theta), 0);
Rot3 _0Ri = Rot3::yaw(-theta); // negative yaw goes counterclockwise, with Z down !
Pose3 gTi(gR0 * _0Ri, gti);
x.insert(PoseKey(i), gTi);
}
return x;
}
/* ************************************************************************* */
void Graph::addPrior(Index i, const Pose3& p, const SharedNoiseModel& model) {
sharedFactor factor(new Prior(PoseKey(i), p, model));
push_back(factor);
}
/* ************************************************************************* */
void Graph::addPrior(Index i, const Pose3& p, const SharedNoiseModel& model) {
sharedFactor factor(new Prior(PoseKey(i), p, model));
push_back(factor);
}
/* ************************************************************************* */
void Graph::addConstraint(Index i, Index j, const Pose3& z, const SharedNoiseModel& model) {
sharedFactor factor(new Constraint(PoseKey(i), PoseKey(j), z, model));
push_back(factor);
}
void Graph::addConstraint(Index i, Index j, const Pose3& z, const SharedNoiseModel& model) {
sharedFactor factor(new Constraint(PoseKey(i), PoseKey(j), z, model));
push_back(factor);
}
/* ************************************************************************* */
void Graph::addHardConstraint(Index i, const Pose3& p) {
sharedFactor factor(new HardConstraint(PoseKey(i), p));
push_back(factor);
}
void Graph::addHardConstraint(Index i, const Pose3& p) {
sharedFactor factor(new HardConstraint(PoseKey(i), p));
push_back(factor);
}
/* ************************************************************************* */
/* ************************************************************************* */
} // pose3SLAM
} // gtsam
} // pose3SLAM

93
gtsam/slam/pose3SLAM.h
View File

@ -23,52 +23,57 @@
#include <gtsam/nonlinear/Symbol.h>
#include <gtsam/nonlinear/NonlinearEquality.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/NonlinearOptimizer.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
/// Use pose3SLAM namespace for specific SLAM instance
namespace pose3SLAM {
using namespace gtsam;
/// Convenience function for constructing a pose key
inline Symbol PoseKey(Index j) { return Symbol('x', j); }
/**
* Create a circle of n 3D poses tangent to circle of radius R, first pose at (R,0)
* @param n number of poses
* @param R radius of circle
* @return circle of n 3D poses
*/
Values circle(size_t n, double R);
/// A prior factor on Key with Pose3 data type.
typedef PriorFactor<Pose3> Prior;
/// A factor to put constraints between two factors.
typedef BetweenFactor<Pose3> Constraint;
/// A hard constraint would enforce that the given key would have the input value in the results.
typedef NonlinearEquality<Pose3> HardConstraint;
/// Graph
struct Graph: public NonlinearFactorGraph {
/// Adds a factor between keys of the same type
void addPrior(Index i, const Pose3& p, const SharedNoiseModel& model);
/// Creates a between factor between keys i and j with a noise model with Pos3 z in the graph
void addConstraint(Index i, Index j, const Pose3& z, const SharedNoiseModel& model);
/// Creates a hard constraint for key i with the given Pose3 p.
void addHardConstraint(Index i, const Pose3& p);
/// Optimize
Values optimize(const Values& initialEstimate) {
return LevenbergMarquardtOptimizer(*this, initialEstimate).optimize();
}
};
} // pose3SLAM
/**
* Backwards compatibility
*/
namespace gtsam {
/// Use pose3SLAM namespace for specific SLAM instance
namespace pose3SLAM {
/// Convenience function for constructing a pose key
inline Symbol PoseKey(Index j) { return Symbol('x', j); }
/**
* Create a circle of n 3D poses tangent to circle of radius R, first pose at (R,0)
* @param n number of poses
* @param R radius of circle
* @return circle of n 3D poses
*/
Values circle(size_t n, double R);
/// A prior factor on Key with Pose3 data type.
typedef PriorFactor<Pose3> Prior;
/// A factor to put constraints between two factors.
typedef BetweenFactor<Pose3> Constraint;
/// A hard constraint would enforce that the given key would have the input value in the results.
typedef NonlinearEquality<Pose3> HardConstraint;
/// Graph
struct Graph: public NonlinearFactorGraph {
/// Adds a factor between keys of the same type
void addPrior(Index i, const Pose3& p, const SharedNoiseModel& model);
/// Creates a between factor between keys i and j with a noise model with Pos3 z in the graph
void addConstraint(Index i, Index j, const Pose3& z, const SharedNoiseModel& model);
/// Creates a hard constraint for key i with the given Pose3 p.
void addHardConstraint(Index i, const Pose3& p);
};
} // pose3SLAM
/**
* Backwards compatibility
*/
typedef pose3SLAM::Prior Pose3Prior; ///< Typedef for Prior class for backwards compatibility
typedef pose3SLAM::Constraint Pose3Factor; ///< Typedef for Constraint class for backwards compatibility
typedef pose3SLAM::Graph Pose3Graph; ///< Typedef for Graph class for backwards compatibility
} // namespace gtsam
}

4
gtsam/slam/tests/testPose3SLAM.cpp
View File

@ -53,7 +53,7 @@ TEST(Pose3Graph, optimizeCircle) {
Pose3 gT0 = hexagon.at<Pose3>(PoseKey(0)), gT1 = hexagon.at<Pose3>(PoseKey(1));
// create a Pose graph with one equality constraint and one measurement
Pose3Graph fg;
pose3SLAM::Graph fg;
fg.addHardConstraint(0, gT0);
Pose3 _0T1 = gT0.between(gT1); // inv(gT0)*gT1
double theta = M_PI/3.0;
@ -128,7 +128,7 @@ TEST( Pose3Factor, error )
// Create factor
SharedNoiseModel I6(noiseModel::Unit::Create(6));
Pose3Factor factor(PoseKey(1), PoseKey(2), z, I6);
pose3SLAM::Constraint factor(PoseKey(1), PoseKey(2), z, I6);
// Create config
Values x;

40
gtsam/slam/visualSLAM.cpp
View File

@ -18,6 +18,44 @@
#include <gtsam/slam/visualSLAM.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
namespace gtsam {
namespace visualSLAM {
/* ************************************************************************* */
void Graph::addMeasurement(const Point2& measured, const SharedNoiseModel& model,
Index poseKey, Index pointKey, const shared_ptrK& K) {
boost::shared_ptr<ProjectionFactor> factor(new ProjectionFactor(measured, model, PoseKey(poseKey), PointKey(pointKey), K));
push_back(factor);
}
/* ************************************************************************* */
void Graph::addPoseConstraint(Index poseKey, const Pose3& p) {
boost::shared_ptr<PoseConstraint> factor(new PoseConstraint(PoseKey(poseKey), p));
push_back(factor);
}
/* ************************************************************************* */
void Graph::addPointConstraint(Index pointKey, const Point3& p) {
boost::shared_ptr<PointConstraint> factor(new PointConstraint(PointKey(pointKey), p));
push_back(factor);
}
/* ************************************************************************* */
void Graph::addPosePrior(Index poseKey, const Pose3& p, const SharedNoiseModel& model) {
boost::shared_ptr<PosePrior> factor(new PosePrior(PoseKey(poseKey), p, model));
push_back(factor);
}
/* ************************************************************************* */
void Graph::addPointPrior(Index pointKey, const Point3& p, const SharedNoiseModel& model) {
boost::shared_ptr<PointPrior> factor(new PointPrior(PointKey(pointKey), p, model));
push_back(factor);
}
/* ************************************************************************* */
void Graph::addRangeFactor(Index poseKey, Index pointKey, double range, const SharedNoiseModel& model) {
push_back(boost::shared_ptr<RangeFactor>(new RangeFactor(PoseKey(poseKey), PointKey(pointKey), range, model)));
}
/* ************************************************************************* */
}

40
gtsam/slam/visualSLAM.h
View File

@ -26,7 +26,7 @@
#include <gtsam/nonlinear/Symbol.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/NonlinearEquality.h>
#include <gtsam/nonlinear/NonlinearOptimizer.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/geometry/SimpleCamera.h>
@ -85,30 +85,21 @@ namespace visualSLAM {
* @param K shared pointer to calibration object
*/
void addMeasurement(const Point2& measured, const SharedNoiseModel& model,
Index poseKey, Index pointKey, const shared_ptrK& K) {
boost::shared_ptr<ProjectionFactor> factor(new ProjectionFactor(measured, model, PoseKey(poseKey), PointKey(pointKey), K));
push_back(factor);
}
Index poseKey, Index pointKey, const shared_ptrK& K);
/**
* Add a constraint on a pose (for now, *must* be satisfied in any Values)
* @param key variable key of the camera pose
* @param p to which pose to constrain it to
*/
void addPoseConstraint(Index poseKey, const Pose3& p = Pose3()) {
boost::shared_ptr<PoseConstraint> factor(new PoseConstraint(PoseKey(poseKey), p));
push_back(factor);
}
void addPoseConstraint(Index poseKey, const Pose3& p = Pose3());
/**
* Add a constraint on a point (for now, *must* be satisfied in any Values)
* @param key variable key of the landmark
* @param p point around which soft prior is defined
*/
void addPointConstraint(Index pointKey, const Point3& p = Point3()) {
boost::shared_ptr<PointConstraint> factor(new PointConstraint(PointKey(pointKey), p));
push_back(factor);
}
void addPointConstraint(Index pointKey, const Point3& p = Point3());
/**
* Add a prior on a pose
@ -116,10 +107,7 @@ namespace visualSLAM {
* @param p around which soft prior is defined
* @param model uncertainty model of this prior
*/
void addPosePrior(Index poseKey, const Pose3& p = Pose3(), const SharedNoiseModel& model = noiseModel::Unit::Create(6)) {
boost::shared_ptr<PosePrior> factor(new PosePrior(PoseKey(poseKey), p, model));
push_back(factor);
}
void addPosePrior(Index poseKey, const Pose3& p = Pose3(), const SharedNoiseModel& model = noiseModel::Unit::Create(6));
/**
* Add a prior on a landmark
@ -127,10 +115,7 @@ namespace visualSLAM {
* @param p to which point to constrain it to
* @param model uncertainty model of this prior
*/
void addPointPrior(Index pointKey, const Point3& p = Point3(), const SharedNoiseModel& model = noiseModel::Unit::Create(3)) {
boost::shared_ptr<PointPrior> factor(new PointPrior(PointKey(pointKey), p, model));
push_back(factor);
}
void addPointPrior(Index pointKey, const Point3& p = Point3(), const SharedNoiseModel& model = noiseModel::Unit::Create(3));
/**
* Add a range prior to a landmark
@ -139,8 +124,17 @@ namespace visualSLAM {
* @param range approximate range to landmark
* @param model uncertainty model of this prior
*/
void addRangeFactor(Index poseKey, Index pointKey, double range, const SharedNoiseModel& model = noiseModel::Unit::Create(1)) {
push_back(boost::shared_ptr<RangeFactor>(new RangeFactor(PoseKey(poseKey), PointKey(pointKey), range, model)));
void addRangeFactor(Index poseKey, Index pointKey, double range, const SharedNoiseModel& model = noiseModel::Unit::Create(1));
/**
* Optimize the graph
* @param initialEstimate initial estimate of all variables in the graph
* @param pointKey variable key of the landmark
* @param range approximate range to landmark
* @param model uncertainty model of this prior
*/
Values optimize(const Values& initialEstimate) {
return LevenbergMarquardtOptimizer(*this, initialEstimate).optimize();
}
}; // Graph