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.
							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