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

VisualSLAM Graph is now a class and has convenience functions. These function as documentation (autocompletion and the like) and I think we should also have this style in addition to the generic add.

release/4.3a0
Frank Dellaert 2010-03-05 15:48:01 +00:00
parent 5ef0400e06
commit 0eed38c7a0
3 changed files with 68 additions and 108 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

30
cpp/testVSLAMGraph.cpp
View File

@ -58,14 +58,14 @@ Graph testGraph() {
shared_ptrK sK(new Cal3_S2(625, 625, 0, 0, 0)); shared_ptrK sK(new Cal3_S2(625, 625, 0, 0, 0));
Graph g; Graph g;
g.add(ProjectionFactor(z11, sigma, 1, 1, sK)); g.addMeasurement(z11, sigma, 1, 1, sK);
g.add(ProjectionFactor(z12, sigma, 1, 2, sK)); g.addMeasurement(z12, sigma, 1, 2, sK);
g.add(ProjectionFactor(z13, sigma, 1, 3, sK)); g.addMeasurement(z13, sigma, 1, 3, sK);
g.add(ProjectionFactor(z14, sigma, 1, 4, sK)); g.addMeasurement(z14, sigma, 1, 4, sK);
g.add(ProjectionFactor(z21, sigma, 2, 1, sK)); g.addMeasurement(z21, sigma, 2, 1, sK);
g.add(ProjectionFactor(z22, sigma, 2, 2, sK)); g.addMeasurement(z22, sigma, 2, 2, sK);
g.add(ProjectionFactor(z23, sigma, 2, 3, sK)); g.addMeasurement(z23, sigma, 2, 3, sK);
g.add(ProjectionFactor(z24, sigma, 2, 4, sK)); g.addMeasurement(z24, sigma, 2, 4, sK);
return g; return g;
} }
@ -75,9 +75,9 @@ TEST( Graph, optimizeLM)
// build a graph // build a graph
shared_ptr<Graph> graph(new Graph(testGraph())); shared_ptr<Graph> graph(new Graph(testGraph()));
// add 3 landmark constraints // add 3 landmark constraints
graph->add(PointConstraint(1, landmark1)); graph->addPointConstraint(1, landmark1);
graph->add(PointConstraint(2, landmark2)); graph->addPointConstraint(2, landmark2);
graph->add(PointConstraint(3, landmark3)); graph->addPointConstraint(3, landmark3);
// Create an initial configuration corresponding to the ground truth // Create an initial configuration corresponding to the ground truth
boost::shared_ptr<Config> initialEstimate(new Config); boost::shared_ptr<Config> initialEstimate(new Config);
@ -120,8 +120,8 @@ TEST( Graph, optimizeLM2)
// build a graph // build a graph
shared_ptr<Graph> graph(new Graph(testGraph())); shared_ptr<Graph> graph(new Graph(testGraph()));
// add 2 camera constraints // add 2 camera constraints
graph->add(PoseConstraint(1, camera1)); graph->addPoseConstraint(1, camera1);
graph->add(PoseConstraint(2, camera2)); graph->addPoseConstraint(2, camera2);
// Create an initial configuration corresponding to the ground truth // Create an initial configuration corresponding to the ground truth
boost::shared_ptr<Config> initialEstimate(new Config); boost::shared_ptr<Config> initialEstimate(new Config);
@ -165,8 +165,8 @@ TEST( Graph, CHECK_ORDERING)
// build a graph // build a graph
shared_ptr<Graph> graph(new Graph(testGraph())); shared_ptr<Graph> graph(new Graph(testGraph()));
// add 2 camera constraints // add 2 camera constraints
graph->add(PoseConstraint(1, camera1)); graph->addPoseConstraint(1, camera1);
graph->add(PoseConstraint(2, camera2)); graph->addPoseConstraint(2, camera2);
// Create an initial configuration corresponding to the ground truth // Create an initial configuration corresponding to the ground truth
boost::shared_ptr<Config> initialEstimate(new Config); boost::shared_ptr<Config> initialEstimate(new Config);

51
cpp/visualSLAM.cpp
View File

@ -11,53 +11,8 @@
#include "NonlinearFactorGraph-inl.h" #include "NonlinearFactorGraph-inl.h"
namespace gtsam { namespace gtsam {
INSTANTIATE_PAIR_CONFIG(visualSLAM::PoseKey, Pose3, visualSLAM::PointKey, Point3)
INSTANTIATE_NONLINEAR_FACTOR_GRAPH(visualSLAM::Config)
INSTANTIATE_NONLINEAR_OPTIMIZER(visualSLAM::Graph, visualSLAM::Config)
INSTANTIATE_PAIR_CONFIG(visualSLAM::PoseKey, Pose3, visualSLAM::PointKey, Point3)
INSTANTIATE_NONLINEAR_FACTOR_GRAPH(visualSLAM::Config)
INSTANTIATE_NONLINEAR_OPTIMIZER(visualSLAM::Graph, visualSLAM::Config)
namespace visualSLAM {
// /* ************************************************************************* */
// void ProjectionFactor::print(const std::string& s) const {
// Base::print(s);
// z_.print(s + ".z");
// }
//
// /* ************************************************************************* */
// bool ProjectionFactor::equals(const ProjectionFactor& p, double tol) const {
// return Base::equals(p, tol) && z_.equals(p.z_, tol)
// && K_->equals(*p.K_, tol);
// }
// /* ************************************************************************* */
// bool compareLandmark(const std::string& key,
// const VSLAMConfig& feasible,
// const VSLAMConfig& input) {
// int j = atoi(key.substr(1, key.size() - 1).c_str());
// return feasible[VSLAMPointKey(j)].equals(input[VSLAMPointKey(j)]);
// }
//
// /* ************************************************************************* */
// void VSLAMGraph::addLandmarkConstraint(int j, const Point3& p) {
// typedef NonlinearEquality<VSLAMConfig,VSLAMPointKey,Point3> NLE;
// boost::shared_ptr<NLE> factor(new NLE(j, p));
// push_back(factor);
// }
//
// /* ************************************************************************* */
// bool compareCamera(const std::string& key,
// const VSLAMConfig& feasible,
// const VSLAMConfig& input) {
// int j = atoi(key.substr(1, key.size() - 1).c_str());
// return feasible[VSLAMPoseKey(j)].equals(input[VSLAMPoseKey(j)]);
// }
//
// /* ************************************************************************* */
// void VSLAMGraph::addCameraConstraint(int j, const Pose3& p) {
// typedef NonlinearEquality<VSLAMConfig,VSLAMPoseKey,Pose3> NLE;
// boost::shared_ptr<NLE> factor(new NLE(j,p));
// push_back(factor);
// }
}
} }

95
cpp/visualSLAM.h
View File

@ -25,7 +25,6 @@ namespace gtsam { namespace visualSLAM {
typedef TypedSymbol<Pose3,'x'> PoseKey; typedef TypedSymbol<Pose3,'x'> PoseKey;
typedef TypedSymbol<Point3,'l'> PointKey; typedef TypedSymbol<Point3,'l'> PointKey;
typedef PairConfig<PoseKey, Pose3, PointKey, Point3> Config; typedef PairConfig<PoseKey, Pose3, PointKey, Point3> Config;
typedef NonlinearFactorGraph<Config> Graph;
typedef NonlinearEquality<Config, PoseKey, Pose3> PoseConstraint; typedef NonlinearEquality<Config, PoseKey, Pose3> PoseConstraint;
typedef NonlinearEquality<Config, PointKey, Point3> PointConstraint; typedef NonlinearEquality<Config, PointKey, Point3> PointConstraint;
@ -116,52 +115,58 @@ namespace gtsam { namespace visualSLAM {
// Typedef for general use // Typedef for general use
typedef GenericProjectionFactor<Config, PointKey, PoseKey> ProjectionFactor; typedef GenericProjectionFactor<Config, PointKey, PoseKey> ProjectionFactor;
/**
* Non-linear factor graph for vanilla visual SLAM
*/
class Graph: public NonlinearFactorGraph<Config> {
public:
/** default constructor is empty graph */
Graph() {
}
// /** /** print out graph */
// * Non-linear factor graph for visual SLAM void print(const std::string& s = "") const {
// */ NonlinearFactorGraph<Config>::print(s);
// class VSLAMGraph : public NonlinearFactorGraph<VSLAMConfig>{ }
//
// public: /** equals */
// bool equals(const Graph& p, double tol = 1e-9) const {
// /** default constructor is empty graph */ return NonlinearFactorGraph<Config>::equals(p, tol);
// VSLAMGraph() {} }
//
// /** /**
// * print out graph * Add a measurement
// */ * @param j index of camera
// void print(const std::string& s = "") const { * @param p to which pose to constrain it to
// NonlinearFactorGraph<VSLAMConfig>::print(s); */
// } void addMeasurement(const Point2& z, const SharedGaussian& model,
// PoseKey i, PointKey j, const shared_ptrK& K) {
// /** boost::shared_ptr<ProjectionFactor> factor(new ProjectionFactor(z, model, i, j, K));
// * equals push_back(factor);
// */ }
// bool equals(const VSLAMGraph& p, double tol=1e-9) const {
// return NonlinearFactorGraph<VSLAMConfig>::equals(p, tol); /**
// } * Add a constraint on a pose (for now, *must* be satisfied in any Config)
// * @param j index of camera
// /** * @param p to which pose to constrain it to
// * Add a constraint on a landmark (for now, *must* be satisfied in any Config) */
// * @param j index of landmark void addPoseConstraint(int j, const Pose3& p = Pose3()) {
// * @param p to which point to constrain it to boost::shared_ptr<PoseConstraint> factor(new PoseConstraint(j, p));
// */ push_back(factor);
// void addLandmarkConstraint(int j, const Point3& p = Point3()); }
//
// /** /**
// * Add a constraint on a camera (for now, *must* be satisfied in any Config) * Add a constraint on a point (for now, *must* be satisfied in any Config)
// * @param j index of camera * @param j index of landmark
// * @param p to which pose to constrain it to * @param p to which point to constrain it to
// */ */
// void addCameraConstraint(int j, const Pose3& p = Pose3()); void addPointConstraint(int j, const Point3& p = Point3()) {
// boost::shared_ptr<PointConstraint> factor(new PointConstraint(j, p));
// private: push_back(factor);
// /** Serialization function */ }
// friend class boost::serialization::access;
// template<class Archive> }; // Graph
// void serialize(Archive & ar, const unsigned int version) {}
// };
} } // namespaces } } // namespaces