Moved DummyFactor to gtsam_unstable from MastSLAM - allows for adding sufficient connectivity for solvers to operate
							parent
							
								
									7fcd06bb4f
								
							
						
					
					
						commit
						f272a07e29
					
				|  | @ -199,4 +199,9 @@ virtual class RelativeElevationFactor: gtsam::NonlinearFactor { | |||
| 	void print(string s) const; | ||||
| }; | ||||
| 
 | ||||
| #include <gtsam_unstable/slam/DummyFactor.h> | ||||
| virtual class DummyFactor : gtsam::NonlinearFactor { | ||||
| 	DummyFactor(size_t key1, size_t dim1, size_t key2, size_t dim2); | ||||
| }; | ||||
| 
 | ||||
| } //\namespace gtsam
 | ||||
|  |  | |||
|  | @ -0,0 +1,64 @@ | |||
| /**
 | ||||
|  * @file DummyFactor.cpp | ||||
|  * | ||||
|  * @date Sep 10, 2012 | ||||
|  * @author Alex Cunningham | ||||
|  */ | ||||
| 
 | ||||
| #include <gtsam_unstable/slam/DummyFactor.h> | ||||
| 
 | ||||
| #include <boost/foreach.hpp> | ||||
| 
 | ||||
| namespace gtsam { | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| DummyFactor::DummyFactor(const Key& key1, size_t dim1, const Key& key2, size_t dim2) | ||||
| : NonlinearFactor(key1, key2) | ||||
| { | ||||
| 	dims_.push_back(dim1); | ||||
| 	dims_.push_back(dim2); | ||||
| 	if (dim1 > dim2) | ||||
| 		rowDim_ = dim1; | ||||
| 	else | ||||
| 		rowDim_ = dim2; | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| void DummyFactor::print(const std::string& s, const KeyFormatter& keyFormatter) const { | ||||
| 	std::cout << s << "  DummyFactor dim = " << rowDim_ << ", keys = { "; | ||||
| 	BOOST_FOREACH(Key key, this->keys()) { std::cout << keyFormatter(key) << " "; } | ||||
| 	std::cout << "}" << std::endl; | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| bool DummyFactor::equals(const NonlinearFactor& f, double tol) const { | ||||
|   const DummyFactor* e = dynamic_cast<const DummyFactor*>(&f); | ||||
|   return e && Base::equals(f, tol) && dims_ == e->dims_ && rowDim_ == e->rowDim_; | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| boost::shared_ptr<GaussianFactor> | ||||
| DummyFactor::linearize(const Values& c, const Ordering& ordering) const { | ||||
| 	// Only linearize if the factor is active
 | ||||
| 	if (!this->active(c)) | ||||
| 		return boost::shared_ptr<JacobianFactor>(); | ||||
| 
 | ||||
| 	 // Fill in terms with zero matrices
 | ||||
|   std::vector<std::pair<Index, Matrix> > terms(this->size()); | ||||
|   for(size_t j=0; j<this->size(); ++j) { | ||||
|     terms[j].first = ordering[this->keys()[j]]; | ||||
|     terms[j].second = zeros(rowDim_, dims_[j]); | ||||
|   } | ||||
| 
 | ||||
|   noiseModel::Diagonal::shared_ptr model = noiseModel::Unit::Create(rowDim_); | ||||
|   return GaussianFactor::shared_ptr( | ||||
|   		new JacobianFactor(terms, zero(rowDim_), model)); | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| 
 | ||||
| } // \namespace gtsam
 | ||||
| 
 | ||||
| 
 | ||||
| 
 | ||||
| 
 | ||||
|  | @ -0,0 +1,76 @@ | |||
| /**
 | ||||
|  * @file DummyFactor.h | ||||
|  * | ||||
|  * @brief A simple factor that can be used to trick gtsam solvers into believing a graph is connected. | ||||
|  *  | ||||
|  * @date Sep 10, 2012 | ||||
|  * @author Alex Cunningham | ||||
|  */ | ||||
| 
 | ||||
| #pragma once | ||||
| 
 | ||||
| #include <gtsam/nonlinear/NonlinearFactor.h> | ||||
| 
 | ||||
| namespace gtsam { | ||||
| 
 | ||||
| class DummyFactor : public NonlinearFactor { | ||||
| protected: | ||||
| 
 | ||||
| 	// Store the dimensions of the variables and the dimension of the full system
 | ||||
| 	std::vector<size_t> dims_; | ||||
| 	size_t rowDim_; ///< choose dimension for the rows
 | ||||
| 
 | ||||
| public: | ||||
| 
 | ||||
| 	/** Default constructor: don't use directly */ | ||||
| 	DummyFactor() : rowDim_(1) { } | ||||
| 
 | ||||
| 	/** standard binary constructor */ | ||||
| 	DummyFactor(const Key& key1, size_t dim1, const Key& key2, size_t dim2); | ||||
| 
 | ||||
| 	virtual ~DummyFactor() {} | ||||
| 
 | ||||
| 	// testable
 | ||||
| 
 | ||||
|   /** print */ | ||||
|   virtual void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; | ||||
| 
 | ||||
|   /** Check if two factors are equal */ | ||||
|   virtual bool equals(const NonlinearFactor& f, double tol = 1e-9) const; | ||||
| 
 | ||||
|   // access
 | ||||
| 
 | ||||
|   const std::vector<size_t>& dims() const { return dims_; } | ||||
| 
 | ||||
|   // factor interface
 | ||||
| 
 | ||||
|   /**
 | ||||
|    * Calculate the error of the factor - zero for dummy factors | ||||
|    */ | ||||
|   virtual double error(const Values& c) const { return 0.0; } | ||||
| 
 | ||||
|   /** get the dimension of the factor (number of rows on linearization) */ | ||||
|   virtual size_t dim() const { return rowDim_; } | ||||
| 
 | ||||
|   /** linearize to a GaussianFactor */ | ||||
|   virtual boost::shared_ptr<GaussianFactor> | ||||
|   linearize(const Values& c, const Ordering& ordering) const; | ||||
| 
 | ||||
|   /**
 | ||||
|    * Creates a shared_ptr clone of the factor - needs to be specialized to allow | ||||
|    * for subclasses | ||||
|    * | ||||
|    * By default, throws exception if subclass does not implement the function. | ||||
|    */ | ||||
|   virtual NonlinearFactor::shared_ptr clone() const { | ||||
| 	  return boost::static_pointer_cast<NonlinearFactor>( | ||||
| 	      NonlinearFactor::shared_ptr(new DummyFactor(*this))); | ||||
|   } | ||||
| 
 | ||||
| }; | ||||
| 
 | ||||
| } // \namespace gtsam
 | ||||
| 
 | ||||
| 
 | ||||
| 
 | ||||
| 
 | ||||
|  | @ -0,0 +1,58 @@ | |||
| /**
 | ||||
|  * @file testDummyFactor.cpp | ||||
|  * | ||||
|  * @brief A simple dummy nonlinear factor that can be used to enforce connectivity | ||||
|  * without adding any real information | ||||
|  * | ||||
|  * @date Sep 10, 2012 | ||||
|  * @author Alex Cunningham | ||||
|  */ | ||||
| 
 | ||||
| #include <CppUnitLite/TestHarness.h> | ||||
| 
 | ||||
| #include <gtsam/geometry/Point3.h> | ||||
| 
 | ||||
| #include <gtsam_unstable/slam/DummyFactor.h> | ||||
| 
 | ||||
| using namespace gtsam; | ||||
| 
 | ||||
| const double tol = 1e-9; | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| TEST( testDummyFactor, basics ) { | ||||
| 
 | ||||
| 	gtsam::Key key1 = 7, key2 = 9; | ||||
| 	size_t dim1 = 3, dim2 = 3; | ||||
| 	DummyFactor dummyfactor(key1, dim1, key2, dim2); | ||||
| 
 | ||||
| 	// verify contents
 | ||||
| 	LONGS_EQUAL(2, dummyfactor.size()); | ||||
| 	EXPECT_LONGS_EQUAL(key1, dummyfactor.keys()[0]); | ||||
| 	EXPECT_LONGS_EQUAL(key2, dummyfactor.keys()[1]); | ||||
| 
 | ||||
| 	LONGS_EQUAL(2, dummyfactor.dims().size()); | ||||
| 	EXPECT_LONGS_EQUAL(dim1, dummyfactor.dims()[0]); | ||||
| 	EXPECT_LONGS_EQUAL(dim2, dummyfactor.dims()[1]); | ||||
| 
 | ||||
| 	Values c; | ||||
| 	c.insert(key1, Point3(1.0, 2.0, 3.0)); | ||||
| 	c.insert(key2, Point3(4.0, 5.0, 6.0)); | ||||
| 
 | ||||
| 	// errors - all zeros
 | ||||
| 	DOUBLES_EQUAL(0.0, dummyfactor.error(c), tol); | ||||
| 
 | ||||
| 	Ordering ordering; | ||||
| 	ordering += key1, key2; | ||||
| 
 | ||||
| 	// linearization: all zeros
 | ||||
| 	GaussianFactor::shared_ptr actLinearization = dummyfactor.linearize(c, ordering); | ||||
| 	CHECK(actLinearization); | ||||
| 	noiseModel::Diagonal::shared_ptr model3 = noiseModel::Unit::Create(3); | ||||
| 	GaussianFactor::shared_ptr expLinearization(new JacobianFactor( | ||||
| 			ordering[key1], zeros(3,3), ordering[key2], zeros(3,3), zero(3), model3)); | ||||
| 	EXPECT(assert_equal(*expLinearization, *actLinearization, tol)); | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| int main() { TestResult tr; return TestRegistry::runAllTests(tr); } | ||||
| /* ************************************************************************* */ | ||||
		Loading…
	
		Reference in New Issue