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.
Frank Dellaert
parent
5ef0400e06
commit
0eed38c7a0
|
|
@ -58,14 +58,14 @@ Graph testGraph() {
|
|||
|
||||
shared_ptrK sK(new Cal3_S2(625, 625, 0, 0, 0));
|
||||
Graph g;
|
||||
g.add(ProjectionFactor(z11, sigma, 1, 1, sK));
|
||||
g.add(ProjectionFactor(z12, sigma, 1, 2, sK));
|
||||
g.add(ProjectionFactor(z13, sigma, 1, 3, sK));
|
||||
g.add(ProjectionFactor(z14, sigma, 1, 4, sK));
|
||||
g.add(ProjectionFactor(z21, sigma, 2, 1, sK));
|
||||
g.add(ProjectionFactor(z22, sigma, 2, 2, sK));
|
||||
g.add(ProjectionFactor(z23, sigma, 2, 3, sK));
|
||||
g.add(ProjectionFactor(z24, sigma, 2, 4, sK));
|
||||
g.addMeasurement(z11, sigma, 1, 1, sK);
|
||||
g.addMeasurement(z12, sigma, 1, 2, sK);
|
||||
g.addMeasurement(z13, sigma, 1, 3, sK);
|
||||
g.addMeasurement(z14, sigma, 1, 4, sK);
|
||||
g.addMeasurement(z21, sigma, 2, 1, sK);
|
||||
g.addMeasurement(z22, sigma, 2, 2, sK);
|
||||
g.addMeasurement(z23, sigma, 2, 3, sK);
|
||||
g.addMeasurement(z24, sigma, 2, 4, sK);
|
||||
return g;
|
||||
}
|
||||
|
||||
|
|
@ -75,9 +75,9 @@ TEST( Graph, optimizeLM)
|
|||
// build a graph
|
||||
shared_ptr<Graph> graph(new Graph(testGraph()));
|
||||
// add 3 landmark constraints
|
||||
graph->add(PointConstraint(1, landmark1));
|
||||
graph->add(PointConstraint(2, landmark2));
|
||||
graph->add(PointConstraint(3, landmark3));
|
||||
graph->addPointConstraint(1, landmark1);
|
||||
graph->addPointConstraint(2, landmark2);
|
||||
graph->addPointConstraint(3, landmark3);
|
||||
|
||||
// Create an initial configuration corresponding to the ground truth
|
||||
boost::shared_ptr<Config> initialEstimate(new Config);
|
||||
|
|
@ -120,8 +120,8 @@ TEST( Graph, optimizeLM2)
|
|||
// build a graph
|
||||
shared_ptr<Graph> graph(new Graph(testGraph()));
|
||||
// add 2 camera constraints
|
||||
graph->add(PoseConstraint(1, camera1));
|
||||
graph->add(PoseConstraint(2, camera2));
|
||||
graph->addPoseConstraint(1, camera1);
|
||||
graph->addPoseConstraint(2, camera2);
|
||||
|
||||
// Create an initial configuration corresponding to the ground truth
|
||||
boost::shared_ptr<Config> initialEstimate(new Config);
|
||||
|
|
@ -165,8 +165,8 @@ TEST( Graph, CHECK_ORDERING)
|
|||
// build a graph
|
||||
shared_ptr<Graph> graph(new Graph(testGraph()));
|
||||
// add 2 camera constraints
|
||||
graph->add(PoseConstraint(1, camera1));
|
||||
graph->add(PoseConstraint(2, camera2));
|
||||
graph->addPoseConstraint(1, camera1);
|
||||
graph->addPoseConstraint(2, camera2);
|
||||
|
||||
// Create an initial configuration corresponding to the ground truth
|
||||
boost::shared_ptr<Config> initialEstimate(new Config);
|
||||
|
|
|
|||
|
|
@ -11,53 +11,8 @@
|
|||
#include "NonlinearFactorGraph-inl.h"
|
||||
|
||||
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);
|
||||
// }
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -25,7 +25,6 @@ namespace gtsam { namespace visualSLAM {
|
|||
typedef TypedSymbol<Pose3,'x'> PoseKey;
|
||||
typedef TypedSymbol<Point3,'l'> PointKey;
|
||||
typedef PairConfig<PoseKey, Pose3, PointKey, Point3> Config;
|
||||
typedef NonlinearFactorGraph<Config> Graph;
|
||||
typedef NonlinearEquality<Config, PoseKey, Pose3> PoseConstraint;
|
||||
typedef NonlinearEquality<Config, PointKey, Point3> PointConstraint;
|
||||
|
||||
|
|
@ -116,52 +115,58 @@ namespace gtsam { namespace visualSLAM {
|
|||
// Typedef for general use
|
||||
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() {
|
||||
}
|
||||
|
||||
// /**
|
||||
// * Non-linear factor graph for visual SLAM
|
||||
// */
|
||||
// class VSLAMGraph : public NonlinearFactorGraph<VSLAMConfig>{
|
||||
//
|
||||
// public:
|
||||
//
|
||||
// /** default constructor is empty graph */
|
||||
// VSLAMGraph() {}
|
||||
//
|
||||
// /**
|
||||
// * print out graph
|
||||
// */
|
||||
// void print(const std::string& s = "") const {
|
||||
// NonlinearFactorGraph<VSLAMConfig>::print(s);
|
||||
// }
|
||||
//
|
||||
// /**
|
||||
// * equals
|
||||
// */
|
||||
// bool equals(const VSLAMGraph& p, double tol=1e-9) const {
|
||||
// return NonlinearFactorGraph<VSLAMConfig>::equals(p, tol);
|
||||
// }
|
||||
//
|
||||
// /**
|
||||
// * Add a constraint on a landmark (for now, *must* be satisfied in any Config)
|
||||
// * @param j index of landmark
|
||||
// * @param p to which point to constrain it to
|
||||
// */
|
||||
// void addLandmarkConstraint(int j, const Point3& p = Point3());
|
||||
//
|
||||
// /**
|
||||
// * Add a constraint on a camera (for now, *must* be satisfied in any Config)
|
||||
// * @param j index of camera
|
||||
// * @param p to which pose to constrain it to
|
||||
// */
|
||||
// void addCameraConstraint(int j, const Pose3& p = Pose3());
|
||||
//
|
||||
// private:
|
||||
// /** Serialization function */
|
||||
// friend class boost::serialization::access;
|
||||
// template<class Archive>
|
||||
// void serialize(Archive & ar, const unsigned int version) {}
|
||||
// };
|
||||
/** print out graph */
|
||||
void print(const std::string& s = "") const {
|
||||
NonlinearFactorGraph<Config>::print(s);
|
||||
}
|
||||
|
||||
/** equals */
|
||||
bool equals(const Graph& p, double tol = 1e-9) const {
|
||||
return NonlinearFactorGraph<Config>::equals(p, tol);
|
||||
}
|
||||
|
||||
/**
|
||||
* Add a measurement
|
||||
* @param j index of camera
|
||||
* @param p to which pose to constrain it to
|
||||
*/
|
||||
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));
|
||||
push_back(factor);
|
||||
}
|
||||
|
||||
/**
|
||||
* 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
|
||||
*/
|
||||
void addPoseConstraint(int j, const Pose3& p = Pose3()) {
|
||||
boost::shared_ptr<PoseConstraint> factor(new PoseConstraint(j, p));
|
||||
push_back(factor);
|
||||
}
|
||||
|
||||
/**
|
||||
* Add a constraint on a point (for now, *must* be satisfied in any Config)
|
||||
* @param j index of landmark
|
||||
* @param p to which point to constrain it to
|
||||
*/
|
||||
void addPointConstraint(int j, const Point3& p = Point3()) {
|
||||
boost::shared_ptr<PointConstraint> factor(new PointConstraint(j, p));
|
||||
push_back(factor);
|
||||
}
|
||||
|
||||
}; // Graph
|
||||
|
||||
} } // namespaces
|
||||
|
|
|
|||
Loading…
Reference in New Issue