addOdometry now works
							parent
							
								
									c987ab397c
								
							
						
					
					
						commit
						d11d674c01
					
				|  | @ -8,6 +8,8 @@ | |||
| #include "FactorGraph-inl.h" | ||||
| #include "NonlinearOptimizer-inl.h" | ||||
| #include "UrbanGraph.h" | ||||
| #include "UrbanMeasurement.h" | ||||
| #include "UrbanOdometry.h" | ||||
| 
 | ||||
| namespace gtsam { | ||||
| 
 | ||||
|  | @ -43,7 +45,12 @@ namespace gtsam { | |||
| 	/* ************************************************************************* */ | ||||
| 	void UrbanGraph::addOdometry(double dx, double yaw, double sigmadx, | ||||
| 			double sigmayaw, int p) { | ||||
| 		// TODO
 | ||||
| 		Vector z = Vector_(dx,0,0,yaw,0,0); | ||||
| 		Matrix cov = eye(6); | ||||
| 		cov(1,1)=sigmadx*sigmadx; | ||||
| 		cov(4,4)=sigmadx*sigmadx; | ||||
| 		sharedFactor factor(new UrbanOdometry(symbol('x',p),symbol('x',p+1),z,cov)); | ||||
| 		push_back(factor); | ||||
| 	} | ||||
| 
 | ||||
| 	/* ************************************************************************* */ | ||||
|  |  | |||
|  | @ -8,7 +8,7 @@ | |||
| #pragma once | ||||
| 
 | ||||
| #include "NonlinearFactorGraph.h" | ||||
| #include "UrbanMeasurement.h" | ||||
| #include "UrbanFactor.h" | ||||
| 
 | ||||
| namespace gtsam { | ||||
| 
 | ||||
|  |  | |||
|  | @ -0,0 +1,86 @@ | |||
| /**
 | ||||
|  * @file    UrbanOdometry.cpp | ||||
|  * @brief   A non-linear factor specialized to the Visual SLAM problem | ||||
|  * @author  Frank Dellaert | ||||
|  * @author  Viorela Ila | ||||
|  */ | ||||
| 
 | ||||
| #include "UrbanOdometry.h" | ||||
| 
 | ||||
| using namespace std; | ||||
| namespace gtsam { | ||||
| 
 | ||||
| /* ************************************************************************* *
 | ||||
| UrbanOdometry::UrbanOdometry() { | ||||
| 	/// Arbitrary values
 | ||||
| 	robotPoseNumber_ = 1; | ||||
| 	robotPoseName_ = symbol('x',robotPoseNumber_); | ||||
| 	keys_.push_back(robotPoseName_); | ||||
| } | ||||
| /* ************************************************************************* *
 | ||||
| UrbanOdometry::UrbanOdometry(const Point2& z, double sigma, int cn, int ln) | ||||
| : NonlinearFactor<Pose3Config>(z.vector(), sigma) | ||||
|   { | ||||
| 	robotPoseNumber_ = cn; | ||||
| 	landmarkNumber_    = ln; | ||||
| 	robotPoseName_ = symbol('x',robotPoseNumber_); | ||||
| 	landmarkName_    = symbol('l',landmarkNumber_); | ||||
| 	keys_.push_back(robotPoseName_); | ||||
| 	keys_.push_back(landmarkName_); | ||||
|   } | ||||
| 
 | ||||
| /* ************************************************************************* *
 | ||||
| void UrbanOdometry::print(const std::string& s) const { | ||||
|   printf("%s %s %s\n", s.c_str(), robotPoseName_.c_str(), landmarkName_.c_str()); | ||||
|   gtsam::print(this->z_, s+".z"); | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* *
 | ||||
| bool UrbanOdometry::equals(const UrbanOdometry& p, double tol) const { | ||||
|   if (&p == NULL) return false; | ||||
|   if (robotPoseNumber_ != p.robotPoseNumber_ || landmarkNumber_ != p.landmarkNumber_) return false; | ||||
|   if (!equal_with_abs_tol(this->z_,p.z_,tol)) return false; | ||||
|   return true; | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* *
 | ||||
| // TODO Implement transformPoint2_from
 | ||||
| // the difference here that we have a 2d point in a 3D coordinate
 | ||||
| Vector UrbanOdometry::predict(const Pose3Config& c) const { | ||||
|   Pose3 pose = c.cameraPose(robotPoseNumber_); | ||||
|   Point2 landmark = c.landmarkPoint(landmarkNumber_); | ||||
|   return transformPoint2_from(SimpleCamera(*K_,pose), landmark).vector(); | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* *
 | ||||
| Vector UrbanOdometry::error_vector(const Pose3Config& c) const { | ||||
|   // Right-hand-side b = (z - h(x))/sigma
 | ||||
|   Point2 h = predict(c); | ||||
|   return (this->z_ - h); | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* *
 | ||||
| GaussianFactor::shared_ptr UrbanOdometry::linearize(const Pose3Config& c) const | ||||
| { | ||||
|   // get arguments from config
 | ||||
|   Pose3 pose = c.cameraPose(robotPoseNumber_); | ||||
|   Point3 landmark = c.landmarkPoint(landmarkNumber_); | ||||
| 
 | ||||
|   SimpleCamera camera(*K_,pose); | ||||
| 
 | ||||
|   // Jacobians
 | ||||
|   Matrix Dh1 = Dproject_pose(camera, landmark); | ||||
|   Matrix Dh2 = Dproject_point(camera, landmark); | ||||
| 
 | ||||
|   // Right-hand-side b = (z - h(x))
 | ||||
|   Vector h = project(camera, landmark).vector(); | ||||
|   Vector b = this->z_ - h; | ||||
| 
 | ||||
|   // Make new linearfactor, divide by sigma
 | ||||
|   GaussianFactor::shared_ptr | ||||
|     p(new GaussianFactor(robotPoseName_, Dh1, landmarkName_, Dh2, b, this->sigma_)); | ||||
|   return p; | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| } // namespace gtsam
 | ||||
|  | @ -0,0 +1,79 @@ | |||
| /**
 | ||||
|  * @file    UrbanOdometry.h | ||||
|  * @brief   A Nonlinear Factor, specialized for Urban application | ||||
|  * @author  Frank Dellaert | ||||
|  * @author  Viorela Ila | ||||
|  */ | ||||
| 
 | ||||
| #pragma once | ||||
| 
 | ||||
| /*
 | ||||
| #include <map> | ||||
| #include "NonlinearFactor.h" | ||||
| #include "GaussianFactor.h" | ||||
| #include "VectorConfig.h" | ||||
| #include "Pose3.h" | ||||
| */ | ||||
| #include "UrbanFactor.h" | ||||
| 
 | ||||
| namespace gtsam { | ||||
| 
 | ||||
| 	class UrbanOdometry: public UrbanFactor { | ||||
| 	private: | ||||
| 		std::string key1_, key2_; /** The keys of the two poses, order matters */ | ||||
| 		Matrix square_root_inverse_covariance_; /** sqrt(inv(measurement_covariance)) */ | ||||
| 
 | ||||
| 	public: | ||||
| 
 | ||||
| 		typedef boost::shared_ptr<UrbanOdometry> shared_ptr; // shorthand for a smart pointer to a factor
 | ||||
| 
 | ||||
| 		UrbanOdometry(const std::string& key1, const std::string& key2, | ||||
| 				const Vector& measured, const Matrix& measurement_covariance) : | ||||
| 					UrbanFactor(measured, 1.0), | ||||
| 			key1_(key1), key2_(key2) { | ||||
| 			square_root_inverse_covariance_ = inverse_square_root( | ||||
| 					measurement_covariance); | ||||
| 		} | ||||
| 
 | ||||
| 		/** implement functions needed for Testable */ | ||||
| 		void print(const std::string& name) const { | ||||
| 			std::cout << name << std::endl; | ||||
| 			std::cout << "Factor " << std::endl; | ||||
| 			std::cout << "key1 " << key1_ << std::endl; | ||||
| 			std::cout << "key2 " << key2_ << std::endl; | ||||
| 			gtsam::print(z_,"measured "); | ||||
| 			gtsam::print(square_root_inverse_covariance_, "MeasurementCovariance"); | ||||
| 		} | ||||
| 
 | ||||
| 		bool equals(const NonlinearFactor<UrbanConfig>& expected, double tol) const { | ||||
| 			return false; | ||||
| 		} | ||||
| 
 | ||||
| 		/** implement functions needed to derive from Factor */ | ||||
| 		Vector error_vector(const UrbanConfig& config) const { | ||||
| 			return zero(6); | ||||
| 		} | ||||
| 
 | ||||
| 		std::list<std::string> keys() const { | ||||
| 			std::list<std::string> l; | ||||
| 			return l; | ||||
| 		} | ||||
| 
 | ||||
| 		std::size_t size() const { | ||||
| 			return 2; | ||||
| 		} | ||||
| 
 | ||||
| 		/** linearize */ | ||||
| 		boost::shared_ptr<GaussianFactor> linearize(const UrbanConfig& config) const { | ||||
| 			Vector b = zero(6); | ||||
| 			Matrix H1 = zeros(6,6); | ||||
| 			Matrix H2 = zeros(6,6); | ||||
| 			boost::shared_ptr<GaussianFactor> linearized(new GaussianFactor(key1_, | ||||
| 					square_root_inverse_covariance_ * H1, key2_, | ||||
| 					square_root_inverse_covariance_ * H2, b, 1.0)); | ||||
| 			return linearized; | ||||
| 		} | ||||
| 	}; | ||||
| 
 | ||||
| } /// namespace gtsam
 | ||||
| 
 | ||||
|  | @ -50,7 +50,7 @@ TEST( UrbanGraph, addMeasurement) | |||
| 	// Check adding a measurement
 | ||||
|   UrbanGraph g; | ||||
|   double sigma = 0.2;// 20 cm
 | ||||
|   g.addMeasurement(4, 2, sigma, 1, 1); | ||||
|   g.addMeasurement(4, 2, sigma, 1, 1); // ground truth would be (5,2)
 | ||||
|   LONGS_EQUAL(1,g.size()); | ||||
| 
 | ||||
| 	// Create a config
 | ||||
|  | @ -63,10 +63,30 @@ TEST( UrbanGraph, addMeasurement) | |||
|   DOUBLES_EQUAL(expected, g.error(config), 1e-9); | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| TEST( UrbanGraph, addOdometry) | ||||
| { | ||||
| 	// Check adding a measurement
 | ||||
|   UrbanGraph g; | ||||
|   double sigmadx = 0.01; // 1 cm
 | ||||
|   double sigmayaw = M_PI/180.0;// 1 degree
 | ||||
|   g.addOdometry(2, 0, sigmadx,sigmayaw, 1); // 1m forward, 0 yaw
 | ||||
|   LONGS_EQUAL(1,g.size()); | ||||
| 
 | ||||
| 	// Create a config
 | ||||
| 	UrbanConfig config; | ||||
| 	config.addRobotPose(1,robotPose); | ||||
| 	config.addLandmark(1,landmark); | ||||
| 
 | ||||
|   // Check error
 | ||||
| 	double expected = 0.5/0.01/0.01; | ||||
|   // TODO DOUBLES_EQUAL(expected, g.error(config), 1e-9);
 | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* *
 | ||||
|  * the measurements are relative to the robot pose so | ||||
|  * they are in robot coordinate frame different from the global (above) | ||||
|  * | ||||
|  */ | ||||
| UrbanGraph testGraph() { | ||||
| 
 | ||||
|   double sigma = 0.2;// 20 cm
 | ||||
|  | @ -88,7 +108,7 @@ UrbanGraph testGraph() { | |||
|   return g; | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* *
 | ||||
| /* ************************************************************************* */ | ||||
| TEST( UrbanGraph, optimizeLM) | ||||
| { | ||||
|   // build a graph
 | ||||
|  | @ -103,23 +123,30 @@ TEST( UrbanGraph, optimizeLM) | |||
|   initialEstimate->addLandmark(3, landmark3); | ||||
|   initialEstimate->addLandmark(4, landmark4); | ||||
| 
 | ||||
|   // Check error vector
 | ||||
|   Vector expected = zero(16); | ||||
| 	// TODO CHECK(assert_equal(expected, graph.error_vector(*initialEstimate)));
 | ||||
| 
 | ||||
|   // Check error = zero with this config
 | ||||
| 	//DOUBLES_EQUAL(55, graph.error(*initialEstimate), 1e-9);
 | ||||
| 
 | ||||
|   // Create an ordering of the variables
 | ||||
|   Ordering ordering; | ||||
|   ordering += "l1","l2","l3","l4","x1","x2"; // boost assign syntax
 | ||||
| 
 | ||||
|   // Create an optimizer and check its error
 | ||||
|   // We expect the initial to be zero because config is the ground truth
 | ||||
|   //Optimizer optimizer(graph, ordering, initialEstimate, 1e-5);
 | ||||
|   //DOUBLES_EQUAL(0, optimizer.error(), 1e-9);
 | ||||
|   Optimizer optimizer(graph, ordering, initialEstimate, 1e-5); | ||||
|   // TODO DOUBLES_EQUAL(0, optimizer.error(), 1e-9);
 | ||||
| 
 | ||||
|   // Iterate once, and the config should not have changed because we started
 | ||||
|   // with the ground truth
 | ||||
| 
 | ||||
|   // Optimizer afterOneIteration = optimizer.iterate();
 | ||||
|   // DOUBLES_EQUAL(0, optimizer.error(), 1e-9);
 | ||||
|   // TODO DOUBLES_EQUAL(0, optimizer.error(), 1e-9);
 | ||||
| 
 | ||||
|   // check if correct
 | ||||
|   // CHECK(assert_equal(*initialEstimate,*(afterOneIteration.config())));
 | ||||
|   // TODO CHECK(assert_equal(*initialEstimate,*(afterOneIteration.config())));
 | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
|  |  | |||
		Loading…
	
		Reference in New Issue