support cloning (deep copy) for ISAM2
							parent
							
								
									58ace25e33
								
							
						
					
					
						commit
						e036dd460e
					
				|  | @ -81,6 +81,20 @@ namespace gtsam { | |||
| 
 | ||||
| 			Clique(const sharedConditional& conditional); | ||||
| 
 | ||||
| 			void cloneToBayesTree(BayesTree& newTree, shared_ptr parent_clique = shared_ptr()) const { | ||||
| 			  sharedConditional newConditional = sharedConditional(new CONDITIONAL(*conditional_)); | ||||
| 			  sharedClique newClique = newTree.addClique(newConditional, parent_clique); | ||||
| 			  if (cachedFactor_) | ||||
| 			    newClique->cachedFactor_ = cachedFactor_->clone(); | ||||
| 			  else newClique->cachedFactor_ = typename FactorType::shared_ptr(); | ||||
| 			  if (!parent_clique) { | ||||
| 			    newTree.root_ = newClique; | ||||
| 			  } | ||||
| 			  BOOST_FOREACH(const shared_ptr& childClique, children_) { | ||||
| 			    childClique->cloneToBayesTree(newTree, newClique); | ||||
| 			  } | ||||
| 			} | ||||
| 
 | ||||
| 			/** print this node */ | ||||
| 			void print(const std::string& s = "") const; | ||||
| 
 | ||||
|  | @ -264,6 +278,11 @@ namespace gtsam { | |||
| 		/** check equality */ | ||||
| 		bool equals(const BayesTree<CONDITIONAL>& other, double tol = 1e-9) const; | ||||
| 
 | ||||
| 		/** deep copy from another tree */ | ||||
| 		void cloneTo(shared_ptr& newTree) const { | ||||
| 		  root_->cloneToBayesTree(*newTree); | ||||
| 		} | ||||
| 
 | ||||
| 		/**
 | ||||
| 		 * Find parent clique of a conditional.  It will look at all parents and | ||||
| 		 * return the one with the lowest index in the ordering. | ||||
|  |  | |||
|  | @ -174,6 +174,13 @@ public: | |||
|   /** Access the container through the permutation (const version) */ | ||||
|   const value_type& operator[](size_t index) const { return container_[permutation_[index]]; } | ||||
| 
 | ||||
| 	/** Assignment operator for cloning in ISAM2 */ | ||||
|   Permuted<CONTAINER> operator=(const Permuted<CONTAINER>& other) { | ||||
|     permutation_ = other.permutation_; | ||||
|     container_ = other.container_; | ||||
|     return *this; | ||||
|   } | ||||
| 
 | ||||
|   /** Permute this view by applying a permutation to the underlying permutation */ | ||||
|   void permute(const Permutation& permutation) { assert(permutation.size() == this->size()); permutation_ = *permutation_.permute(permutation); } | ||||
| 
 | ||||
|  |  | |||
|  | @ -88,6 +88,9 @@ namespace gtsam { | |||
|     /** Return the dimension of the variable pointed to by the given key iterator */ | ||||
|     virtual size_t getDim(const_iterator variable) const = 0; | ||||
| 
 | ||||
|     /** Clone a factor (make a deep copy) */ | ||||
|     virtual GaussianFactor::shared_ptr clone() const = 0; | ||||
| 
 | ||||
|     /**
 | ||||
|      * Permutes the GaussianFactor, but for efficiency requires the permutation | ||||
|      * to already be inverted.  This acts just as a change-of-name for each | ||||
|  |  | |||
|  | @ -191,6 +191,12 @@ namespace gtsam { | |||
|     /** Destructor */ | ||||
| 		virtual ~HessianFactor() {} | ||||
| 
 | ||||
| 		/** Clone this JacobianFactor */ | ||||
| 		virtual GaussianFactor::shared_ptr clone() const { | ||||
| 		  return boost::static_pointer_cast<GaussianFactor>( | ||||
| 		      shared_ptr(new HessianFactor(*this))); | ||||
| 		} | ||||
| 
 | ||||
|     /** Print the factor for debugging and testing (implementing Testable) */ | ||||
|     virtual void print(const std::string& s = "") const; | ||||
| 
 | ||||
|  |  | |||
|  | @ -136,6 +136,12 @@ namespace gtsam { | |||
| 
 | ||||
|     virtual ~JacobianFactor() {} | ||||
| 
 | ||||
|     /** Clone this JacobianFactor */ | ||||
|     virtual GaussianFactor::shared_ptr clone() const { | ||||
|       return boost::static_pointer_cast<GaussianFactor>( | ||||
|           shared_ptr(new JacobianFactor(*this))); | ||||
|     } | ||||
| 
 | ||||
|     // Implementing Testable interface
 | ||||
|     virtual void print(const std::string& s = "") const; | ||||
|     virtual bool equals(const GaussianFactor& lf, double tol = 1e-9) const; | ||||
|  |  | |||
|  | @ -27,12 +27,18 @@ namespace gtsam { | |||
|  */ | ||||
| template <class VALUES, class GRAPH = NonlinearFactorGraph<VALUES> > | ||||
| class GaussianISAM2 : public ISAM2<GaussianConditional, VALUES, GRAPH> { | ||||
|   typedef ISAM2<GaussianConditional, VALUES, GRAPH> Base; | ||||
| public: | ||||
|   /** Create an empty ISAM2 instance */ | ||||
|   GaussianISAM2(const ISAM2Params& params) : ISAM2<GaussianConditional, VALUES, GRAPH>(params) {} | ||||
| 
 | ||||
|   /** Create an empty ISAM2 instance using the default set of parameters (see ISAM2Params) */ | ||||
|   GaussianISAM2() : ISAM2<GaussianConditional, VALUES, GRAPH>() {} | ||||
| 
 | ||||
|   void cloneTo(boost::shared_ptr<GaussianISAM2>& newGaussianISAM2) const { | ||||
|     boost::shared_ptr<Base> isam2 = boost::static_pointer_cast<Base>(newGaussianISAM2); | ||||
|     Base::cloneTo(isam2); | ||||
|   } | ||||
| }; | ||||
| 
 | ||||
| // optimize the BayesTree, starting from the root
 | ||||
|  |  | |||
|  | @ -167,6 +167,27 @@ public: | |||
|   /** Create an empty ISAM2 instance using the default set of parameters (see ISAM2Params) */ | ||||
|   ISAM2(); | ||||
| 
 | ||||
|   void cloneTo(boost::shared_ptr<ISAM2>& newISAM2) const { | ||||
|     boost::shared_ptr<Base> bayesTree = boost::static_pointer_cast<Base>(newISAM2); | ||||
|     Base::cloneTo(bayesTree); | ||||
|     newISAM2->theta_ = theta_; | ||||
|     newISAM2->variableIndex_ = variableIndex_; | ||||
|     newISAM2->deltaUnpermuted_ = deltaUnpermuted_; | ||||
|     newISAM2->delta_ = delta_; | ||||
|     newISAM2->nonlinearFactors_ = nonlinearFactors_; | ||||
|     newISAM2->ordering_ = ordering_; | ||||
|     newISAM2->params_ = params_; | ||||
| #ifndef NDEBUG | ||||
|     newISAM2->lastRelinVariables_ = lastRelinVariables_; | ||||
| #endif | ||||
|     newISAM2->lastAffectedVariableCount = lastAffectedVariableCount; | ||||
|     newISAM2->lastAffectedFactorCount = lastAffectedFactorCount; | ||||
|     newISAM2->lastAffectedCliqueCount = lastAffectedCliqueCount; | ||||
|     newISAM2->lastAffectedMarkedCount = lastAffectedMarkedCount; | ||||
|     newISAM2->lastBacksubVariableCount = lastBacksubVariableCount; | ||||
|     newISAM2->lastNnzTop = lastNnzTop; | ||||
|   } | ||||
| 
 | ||||
|   typedef typename BayesTree<CONDITIONAL>::sharedClique sharedClique; ///< Shared pointer to a clique
 | ||||
|   typedef typename BayesTree<CONDITIONAL>::Cliques Cliques; ///< List of Clique typedef from base class
 | ||||
| 
 | ||||
|  | @ -235,6 +256,8 @@ public: | |||
|   size_t lastBacksubVariableCount; | ||||
|   size_t lastNnzTop; | ||||
| 
 | ||||
|   ISAM2Params params() const { return params_; } | ||||
| 
 | ||||
|   //@}
 | ||||
| 
 | ||||
| private: | ||||
|  |  | |||
|  | @ -292,6 +292,110 @@ TEST(ISAM2, slamlike_solution) | |||
|   EXPECT(isam_check(fullgraph, fullinit, isam)); | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| TEST_UNSAFE(ISAM2, clone) { | ||||
| 
 | ||||
|   // Pose and landmark key types from planarSLAM
 | ||||
|   typedef planarSLAM::PoseKey PoseKey; | ||||
|   typedef planarSLAM::PointKey PointKey; | ||||
| 
 | ||||
|   // Set up parameters
 | ||||
|   SharedDiagonal odoNoise = sharedSigmas(Vector_(3, 0.1, 0.1, M_PI/100.0)); | ||||
|   SharedDiagonal brNoise = sharedSigmas(Vector_(2, M_PI/100.0, 0.1)); | ||||
| 
 | ||||
|   // These variables will be reused and accumulate factors and values
 | ||||
|   GaussianISAM2<planarSLAM::Values> isam(ISAM2Params(0.001, 0.0, 0, false, true)); | ||||
|   planarSLAM::Values fullinit; | ||||
|   planarSLAM::Graph fullgraph; | ||||
| 
 | ||||
|   // i keeps track of the time step
 | ||||
|   size_t i = 0; | ||||
| 
 | ||||
|   // Add a prior at time 0 and update isam
 | ||||
|   { | ||||
|     planarSLAM::Graph newfactors; | ||||
|     newfactors.addPrior(0, Pose2(0.0, 0.0, 0.0), odoNoise); | ||||
|     fullgraph.push_back(newfactors); | ||||
| 
 | ||||
|     planarSLAM::Values init; | ||||
|     init.insert(PoseKey(0), Pose2(0.01, 0.01, 0.01)); | ||||
|     fullinit.insert(PoseKey(0), Pose2(0.01, 0.01, 0.01)); | ||||
| 
 | ||||
|     isam.update(newfactors, init); | ||||
|   } | ||||
| 
 | ||||
|   EXPECT(isam_check(fullgraph, fullinit, isam)); | ||||
| 
 | ||||
|   // Add odometry from time 0 to time 5
 | ||||
|   for( ; i<5; ++i) { | ||||
|     planarSLAM::Graph newfactors; | ||||
|     newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); | ||||
|     fullgraph.push_back(newfactors); | ||||
| 
 | ||||
|     planarSLAM::Values init; | ||||
|     init.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); | ||||
|     fullinit.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); | ||||
| 
 | ||||
|     isam.update(newfactors, init); | ||||
|   } | ||||
| 
 | ||||
|   // Add odometry from time 5 to 6 and landmark measurement at time 5
 | ||||
|   { | ||||
|     planarSLAM::Graph newfactors; | ||||
|     newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); | ||||
|     newfactors.addBearingRange(i, 0, Rot2::fromAngle(M_PI/4.0), 5.0, brNoise); | ||||
|     newfactors.addBearingRange(i, 1, Rot2::fromAngle(-M_PI/4.0), 5.0, brNoise); | ||||
|     fullgraph.push_back(newfactors); | ||||
| 
 | ||||
|     planarSLAM::Values init; | ||||
|     init.insert(PoseKey(i+1), Pose2(1.01, 0.01, 0.01)); | ||||
|     init.insert(PointKey(0), Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0))); | ||||
|     init.insert(PointKey(1), Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0))); | ||||
|     fullinit.insert(PoseKey(i+1), Pose2(1.01, 0.01, 0.01)); | ||||
|     fullinit.insert(PointKey(0), Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0))); | ||||
|     fullinit.insert(PointKey(1), Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0))); | ||||
| 
 | ||||
|     isam.update(newfactors, init); | ||||
|     ++ i; | ||||
|   } | ||||
| 
 | ||||
|   // Add odometry from time 6 to time 10
 | ||||
|   for( ; i<10; ++i) { | ||||
|     planarSLAM::Graph newfactors; | ||||
|     newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); | ||||
|     fullgraph.push_back(newfactors); | ||||
| 
 | ||||
|     planarSLAM::Values init; | ||||
|     init.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); | ||||
|     fullinit.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); | ||||
| 
 | ||||
|     isam.update(newfactors, init); | ||||
|   } | ||||
| 
 | ||||
|   // Add odometry from time 10 to 11 and landmark measurement at time 10
 | ||||
|   { | ||||
|     planarSLAM::Graph newfactors; | ||||
|     newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); | ||||
|     newfactors.addBearingRange(i, 0, Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 4.5, brNoise); | ||||
|     newfactors.addBearingRange(i, 1, Rot2::fromAngle(-M_PI/4.0 + M_PI/16.0), 4.5, brNoise); | ||||
|     fullgraph.push_back(newfactors); | ||||
| 
 | ||||
|     planarSLAM::Values init; | ||||
|     init.insert(PoseKey(i+1), Pose2(6.9, 0.1, 0.01)); | ||||
|     fullinit.insert(PoseKey(i+1), Pose2(6.9, 0.1, 0.01)); | ||||
| 
 | ||||
|     isam.update(newfactors, init); | ||||
|     ++ i; | ||||
|   } | ||||
| 
 | ||||
|   // CLONING...
 | ||||
|   boost::shared_ptr<GaussianISAM2<planarSLAM::Values> > isam2 | ||||
|       = boost::shared_ptr<GaussianISAM2<planarSLAM::Values> >(new GaussianISAM2<planarSLAM::Values>()); | ||||
|   isam.cloneTo(isam2); | ||||
| 
 | ||||
|   CHECK(assert_equal(isam, *isam2)); | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| int main() { TestResult tr; return TestRegistry::runAllTests(tr);} | ||||
| /* ************************************************************************* */ | ||||
|  |  | |||
		Loading…
	
		Reference in New Issue