simulated2D now allows for custom variable types (as long as they are still Point2)
							parent
							
								
									9ffd075225
								
							
						
					
					
						commit
						dbe01bd0c8
					
				|  | @ -57,14 +57,14 @@ namespace gtsam { | |||
| 		template<class Cfg = Config, class Key = PoseKey> | ||||
| 		struct GenericPrior: public NonlinearFactor1<Cfg, Key> { | ||||
| 			typedef boost::shared_ptr<GenericPrior<Cfg, Key> > shared_ptr; | ||||
| 			typedef typename PoseKey::Value_t Point; | ||||
| 			Point z_; | ||||
| 
 | ||||
| 			Point2 z_; | ||||
| 
 | ||||
| 			GenericPrior(const Point2& z, const SharedGaussian& model, const Key& key) : | ||||
| 			GenericPrior(const Point& z, const SharedGaussian& model, const Key& key) : | ||||
| 				NonlinearFactor1<Cfg, Key> (model, key), z_(z) { | ||||
| 			} | ||||
| 
 | ||||
| 			Vector evaluateError(const Point2& x, boost::optional<Matrix&> H = | ||||
| 			Vector evaluateError(const Point& x, boost::optional<Matrix&> H = | ||||
| 					boost::none) const { | ||||
| 				return (prior(x, H) - z_).vector(); | ||||
| 			} | ||||
|  | @ -77,14 +77,15 @@ namespace gtsam { | |||
| 		template<class Cfg = Config, class Key = PoseKey> | ||||
| 		struct GenericOdometry: public NonlinearFactor2<Cfg, Key, Key> { | ||||
| 			typedef boost::shared_ptr<GenericOdometry<Cfg, Key> > shared_ptr; | ||||
| 			Point2 z_; | ||||
| 			typedef typename PoseKey::Value_t Pose; | ||||
| 			Pose z_; | ||||
| 
 | ||||
| 			GenericOdometry(const Point2& z, const SharedGaussian& model, | ||||
| 			GenericOdometry(const Pose& z, const SharedGaussian& model, | ||||
| 					const Key& i1, const Key& i2) : | ||||
| 				NonlinearFactor2<Cfg, Key, Key> (model, i1, i2), z_(z) { | ||||
| 			} | ||||
| 
 | ||||
| 			Vector evaluateError(const Point2& x1, const Point2& x2, boost::optional< | ||||
| 			Vector evaluateError(const Pose& x1, const Pose& x2, boost::optional< | ||||
| 					Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 = boost::none) const { | ||||
| 				return (odo(x1, x2, H1, H2) - z_).vector(); | ||||
| 			} | ||||
|  | @ -98,14 +99,17 @@ namespace gtsam { | |||
| 		class GenericMeasurement: public NonlinearFactor2<Cfg, XKey, LKey> { | ||||
| 		public: | ||||
| 			typedef boost::shared_ptr<GenericMeasurement<Cfg, XKey, LKey> > shared_ptr; | ||||
| 			Point2 z_; | ||||
| 			typedef typename PoseKey::Value_t Pose; | ||||
| 			typedef typename PointKey::Value_t Point; | ||||
| 
 | ||||
| 			GenericMeasurement(const Point2& z, const SharedGaussian& model, | ||||
| 			Point z_; | ||||
| 
 | ||||
| 			GenericMeasurement(const Point& z, const SharedGaussian& model, | ||||
| 					const XKey& i, const LKey& j) : | ||||
| 				NonlinearFactor2<Cfg, XKey, LKey> (model, i, j), z_(z) { | ||||
| 			} | ||||
| 
 | ||||
| 			Vector evaluateError(const Point2& x1, const Point2& x2, boost::optional< | ||||
| 			Vector evaluateError(const Pose& x1, const Point& x2, boost::optional< | ||||
| 					Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 = boost::none) const { | ||||
| 				return (mea(x1, x2, H1, H2) - z_).vector(); | ||||
| 			} | ||||
|  |  | |||
		Loading…
	
		Reference in New Issue