diff --git a/cpp/testVSLAMGraph.cpp b/cpp/testVSLAMGraph.cpp index 1756f4465..f2c634146 100644 --- a/cpp/testVSLAMGraph.cpp +++ b/cpp/testVSLAMGraph.cpp @@ -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(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 initialEstimate(new Config); @@ -120,8 +120,8 @@ TEST( Graph, optimizeLM2) // build a graph shared_ptr 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 initialEstimate(new Config); @@ -165,8 +165,8 @@ TEST( Graph, CHECK_ORDERING) // build a graph shared_ptr 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 initialEstimate(new Config); diff --git a/cpp/visualSLAM.cpp b/cpp/visualSLAM.cpp index 93791b4ba..8e4992279 100644 --- a/cpp/visualSLAM.cpp +++ b/cpp/visualSLAM.cpp @@ -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 NLE; - // boost::shared_ptr 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 NLE; - // boost::shared_ptr factor(new NLE(j,p)); - // push_back(factor); - // } - } } diff --git a/cpp/visualSLAM.h b/cpp/visualSLAM.h index 7bb0369ff..2395bc47a 100644 --- a/cpp/visualSLAM.h +++ b/cpp/visualSLAM.h @@ -25,7 +25,6 @@ namespace gtsam { namespace visualSLAM { typedef TypedSymbol PoseKey; typedef TypedSymbol PointKey; typedef PairConfig Config; - typedef NonlinearFactorGraph Graph; typedef NonlinearEquality PoseConstraint; typedef NonlinearEquality PointConstraint; @@ -116,52 +115,58 @@ namespace gtsam { namespace visualSLAM { // Typedef for general use typedef GenericProjectionFactor ProjectionFactor; + /** + * Non-linear factor graph for vanilla visual SLAM + */ + class Graph: public NonlinearFactorGraph { + public: + /** default constructor is empty graph */ + Graph() { + } - // /** - // * Non-linear factor graph for visual SLAM - // */ - // class VSLAMGraph : public NonlinearFactorGraph{ - // - // public: - // - // /** default constructor is empty graph */ - // VSLAMGraph() {} - // - // /** - // * print out graph - // */ - // void print(const std::string& s = "") const { - // NonlinearFactorGraph::print(s); - // } - // - // /** - // * equals - // */ - // bool equals(const VSLAMGraph& p, double tol=1e-9) const { - // return NonlinearFactorGraph::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 - // void serialize(Archive & ar, const unsigned int version) {} - // }; + /** print out graph */ + void print(const std::string& s = "") const { + NonlinearFactorGraph::print(s); + } + + /** equals */ + bool equals(const Graph& p, double tol = 1e-9) const { + return NonlinearFactorGraph::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 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 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 factor(new PointConstraint(j, p)); + push_back(factor); + } + + }; // Graph } } // namespaces