finished wrapping visualSLAM namespace
							parent
							
								
									877e9d4045
								
							
						
					
					
						commit
						ae78b89c6f
					
				|  | @ -78,7 +78,7 @@ void readAllDataISAM() { | |||
| /**
 | ||||
|  * Setup newFactors and initialValues for each new pose and set of measurements at each frame. | ||||
|  */ | ||||
| void createNewFactors(shared_ptr<visualSLAM::Graph>& newFactors, boost::shared_ptr<Values>& initialValues, | ||||
| void createNewFactors(shared_ptr<visualSLAM::Graph>& newFactors, boost::shared_ptr<visualSLAM::Values>& initialValues, | ||||
|     int pose_id, const Pose3& pose, const std::vector<Feature2D>& measurements, SharedNoiseModel measurementSigma, shared_ptrK calib) { | ||||
| 
 | ||||
|   // Create a graph of newFactors with new measurements
 | ||||
|  | @ -99,7 +99,7 @@ void createNewFactors(shared_ptr<visualSLAM::Graph>& newFactors, boost::shared_p | |||
|   } | ||||
| 
 | ||||
|   // Create initial values for all nodes in the newFactors
 | ||||
|   initialValues = shared_ptr<Values> (new Values()); | ||||
|   initialValues = shared_ptr<visualSLAM::Values> (new visualSLAM::Values()); | ||||
|   initialValues->insert(X(pose_id), pose); | ||||
|   for (size_t i = 0; i < measurements.size(); i++) { | ||||
|     initialValues->insert(L(measurements[i].m_idLandmark), g_landmarks[measurements[i].m_idLandmark]); | ||||
|  |  | |||
							
								
								
									
										74
									
								
								gtsam.h
								
								
								
								
							
							
						
						
									
										74
									
								
								gtsam.h
								
								
								
								
							|  | @ -81,6 +81,18 @@ class Point2 { | |||
| 	gtsam::Point2 retract(Vector v); | ||||
| }; | ||||
| 
 | ||||
| class StereoPoint2 { | ||||
|   StereoPoint2(); | ||||
|   StereoPoint2(double uL, double uR, double v); | ||||
|   static gtsam::StereoPoint2 Expmap(Vector v); | ||||
|   static Vector Logmap(const gtsam::StereoPoint2& p); | ||||
|   void print(string s) const; | ||||
|   Vector localCoordinates(const gtsam::StereoPoint2& p); | ||||
|   gtsam::StereoPoint2 compose(const gtsam::StereoPoint2& p2); | ||||
|   gtsam::StereoPoint2 between(const gtsam::StereoPoint2& p2); | ||||
|   gtsam::StereoPoint2 retract(Vector v); | ||||
| }; | ||||
| 
 | ||||
| class Point3 { | ||||
| 	Point3(); | ||||
| 	Point3(double x, double y, double z); | ||||
|  | @ -207,6 +219,20 @@ class Pose3 { | |||
| 	gtsam::Rot3 rotation() const; | ||||
| }; | ||||
| 
 | ||||
| class Cal3_S2 { | ||||
|   Cal3_S2(); | ||||
|   Cal3_S2(double fx, double fy, double s, double u0, double v0); | ||||
| 
 | ||||
|   void print(string s) const; | ||||
| }; | ||||
| 
 | ||||
| class Cal3_S2Stereo { | ||||
|   Cal3_S2Stereo(); | ||||
|   Cal3_S2Stereo(double fx, double fy, double s, double u0, double v0, double b); | ||||
| 
 | ||||
|   void print(string s) const; | ||||
| }; | ||||
| 
 | ||||
| class CalibratedCamera { | ||||
| 
 | ||||
| 	CalibratedCamera(); | ||||
|  | @ -635,3 +661,51 @@ class Graph { | |||
| // TODO: add factors, etc.
 | ||||
| 
 | ||||
| }///\namespace simulated2DOriented
 | ||||
| 
 | ||||
| //*************************************************************************
 | ||||
| // VisualSLAM
 | ||||
| //*************************************************************************
 | ||||
| 
 | ||||
| #include <gtsam/slam/visualSLAM.h> | ||||
| namespace visualSLAM { | ||||
| 
 | ||||
| class Values { | ||||
|   Values(); | ||||
|   void insertPose(size_t key, const gtsam::Pose3& pose); | ||||
|   void insertPoint(size_t key, const gtsam::Point3& pose); | ||||
|   size_t size() const; | ||||
|   void print(string s) const; | ||||
|   gtsam::Pose3 pose(size_t i); | ||||
|   gtsam::Point3 point(size_t j); | ||||
| }; | ||||
| 
 | ||||
| class Graph { | ||||
|   Graph(); | ||||
| 
 | ||||
|   void print(string s) const; | ||||
| 
 | ||||
|   double error(const pose2SLAM::Values& values) const; | ||||
|   gtsam::Ordering* orderingCOLAMD(const pose2SLAM::Values& values) const; | ||||
|   gtsam::GaussianFactorGraph* linearize(const pose2SLAM::Values& values, | ||||
|       const gtsam::Ordering& ordering) const; | ||||
| 
 | ||||
|   // Measurements
 | ||||
|   void addMeasurement(const gtsam::Point2& measured, const gtsam::SharedNoiseModel& model, | ||||
|       size_t poseKey, size_t pointKey, const gtsam::Cal3_S2* K); | ||||
|   void addStereoMeasurement(const gtsam::StereoPoint2& measured, const gtsam::SharedNoiseModel& model, | ||||
|       size_t poseKey, size_t pointKey, const gtsam::Cal3_S2Stereo* K); | ||||
| 
 | ||||
|   // Constraints
 | ||||
|   void addPoseConstraint(size_t poseKey, const gtsam::Pose3& p); | ||||
|   void addPointConstraint(size_t pointKey, const gtsam::Point3& p); | ||||
| 
 | ||||
|   // Priors
 | ||||
|   void addPosePrior(size_t poseKey, const gtsam::Pose3& p, const gtsam::SharedNoiseModel& model); | ||||
|   void addPointPrior(size_t pointKey, const gtsam::Point3& p, const gtsam::SharedNoiseModel& model); | ||||
|   void addRangeFactor(size_t poseKey, size_t pointKey, double range, const gtsam::SharedNoiseModel& model); | ||||
| 
 | ||||
|   visualSLAM::Values optimize(const visualSLAM::Values& initialEstimate) const; | ||||
|   gtsam::Marginals marginals(const visualSLAM::Values& solution) const; | ||||
| }; | ||||
| 
 | ||||
| }///\namespace visualSLAM
 | ||||
|  |  | |||
|  | @ -22,14 +22,14 @@ namespace visualSLAM { | |||
| 
 | ||||
|   /* ************************************************************************* */ | ||||
|   void Graph::addMeasurement(const Point2& measured, const SharedNoiseModel& model, | ||||
|        Key poseKey, Key pointKey, const shared_ptrK& K) { | ||||
|        Key poseKey, Key pointKey, const shared_ptrK K) { | ||||
|     boost::shared_ptr<ProjectionFactor> factor(new ProjectionFactor(measured, model, poseKey, pointKey, K)); | ||||
|     push_back(factor); | ||||
|   } | ||||
| 
 | ||||
|   /* ************************************************************************* */ | ||||
|   void Graph::addStereoMeasurement(const StereoPoint2& measured, const SharedNoiseModel& model, | ||||
|        Key poseKey, Key pointKey, const shared_ptrKStereo& K) { | ||||
|        Key poseKey, Key pointKey, const shared_ptrKStereo K) { | ||||
|     boost::shared_ptr<StereoFactor> factor(new StereoFactor(measured, model, poseKey, pointKey, K)); | ||||
|     push_back(factor); | ||||
|   } | ||||
|  |  | |||
|  | @ -26,6 +26,7 @@ | |||
| #include <gtsam/nonlinear/NonlinearFactorGraph.h> | ||||
| #include <gtsam/nonlinear/NonlinearEquality.h> | ||||
| #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h> | ||||
| #include <gtsam/nonlinear/Marginals.h> | ||||
| #include <gtsam/geometry/SimpleCamera.h> | ||||
| 
 | ||||
| 
 | ||||
|  | @ -62,9 +63,15 @@ namespace visualSLAM { | |||
|     /// insert a pose
 | ||||
|     void insertPose(Key i, const Pose3& pose) { insert(i, pose); } | ||||
| 
 | ||||
|     /// insert a point
 | ||||
|     void insertPoint(Key j, const Point3& point) { insert(j, point); } | ||||
| 
 | ||||
|     /// get a pose
 | ||||
|     Pose3 pose(Key i) const { return at<Pose3>(i); } | ||||
| 
 | ||||
|     /// get a point
 | ||||
|     Point3 point(Key j) const { return at<Point3>(j); } | ||||
| 
 | ||||
|   }; | ||||
| 
 | ||||
|   /**
 | ||||
|  | @ -99,7 +106,7 @@ namespace visualSLAM { | |||
|      *  @param K shared pointer to calibration object | ||||
|      */ | ||||
|     void addMeasurement(const Point2& measured, const SharedNoiseModel& model, | ||||
|         Key poseKey, Key pointKey, const shared_ptrK& K); | ||||
|         Key poseKey, Key pointKey, const shared_ptrK K); | ||||
| 
 | ||||
|     /**
 | ||||
|      *  Add a stereo factor measurement | ||||
|  | @ -110,7 +117,7 @@ namespace visualSLAM { | |||
|      *  @param K shared pointer to stereo calibration object | ||||
|      */ | ||||
|     void addStereoMeasurement(const StereoPoint2& measured, const SharedNoiseModel& model, | ||||
|         Key poseKey, Key pointKey, const shared_ptrKStereo& K); | ||||
|         Key poseKey, Key pointKey, const shared_ptrKStereo K); | ||||
| 
 | ||||
|     /**
 | ||||
|      *  Add a constraint on a pose (for now, *must* be satisfied in any Values) | ||||
|  | @ -162,6 +169,11 @@ namespace visualSLAM { | |||
|       return LevenbergMarquardtOptimizer(*this, initialEstimate).optimize(); | ||||
|     } | ||||
| 
 | ||||
|     /// Return a Marginals object
 | ||||
|     Marginals marginals(const Values& solution) const { | ||||
|       return Marginals(*this,solution); | ||||
|     } | ||||
| 
 | ||||
|   }; // Graph
 | ||||
| 
 | ||||
| } // namespaces
 | ||||
|  |  | |||
		Loading…
	
		Reference in New Issue