Fixed doxygen warnings
							parent
							
								
									ecd5862a90
								
							
						
					
					
						commit
						8e39e6b656
					
				|  | @ -31,7 +31,7 @@ namespace gtsam { | |||
|  * convenience to avoid having lengthy types in the code.  Through timing, | ||||
|  * we've seen that the fast_pool_allocator can lead to speedups of several | ||||
|  * percent. | ||||
| 	 * @ingroup base | ||||
| 	 * @addtogroup base | ||||
|  */ | ||||
| template<typename VALUE> | ||||
| class FastList: public std::list<VALUE, boost::fast_pool_allocator<VALUE> > { | ||||
|  |  | |||
|  | @ -31,7 +31,7 @@ namespace gtsam { | |||
|  * convenience to avoid having lengthy types in the code.  Through timing, | ||||
|  * we've seen that the fast_pool_allocator can lead to speedups of several | ||||
|  * percent. | ||||
|  * @ingroup base | ||||
|  * @addtogroup base | ||||
|  */ | ||||
| template<typename KEY, typename VALUE> | ||||
| class FastMap : public std::map<KEY, VALUE, std::less<KEY>, boost::fast_pool_allocator<std::pair<const KEY, VALUE> > > { | ||||
|  |  | |||
|  | @ -42,7 +42,7 @@ struct FastSetTestableHelper; | |||
|  * fast_pool_allocator instead of the default STL allocator.  This is just a | ||||
|  * convenience to avoid having lengthy types in the code.  Through timing, | ||||
|  * we've seen that the fast_pool_allocator can lead to speedups of several %. | ||||
|  * @ingroup base | ||||
|  * @addtogroup base | ||||
|  */ | ||||
| template<typename VALUE, class ENABLE = void> | ||||
| class FastSet: public std::set<VALUE, std::less<VALUE>, boost::fast_pool_allocator<VALUE> > { | ||||
|  |  | |||
|  | @ -30,7 +30,7 @@ namespace gtsam { | |||
|  * pool_allocator instead of the default STL allocator.  This is just a | ||||
|  * convenience to avoid having lengthy types in the code.  Through timing, | ||||
|  * we've seen that the pool_allocator can lead to speedups of several % | ||||
|  * @ingroup base | ||||
|  * @addtogroup base | ||||
|  */ | ||||
| template<typename VALUE> | ||||
| class FastVector: public std::vector<VALUE, boost::pool_allocator<VALUE> > { | ||||
|  |  | |||
|  | @ -46,7 +46,7 @@ namespace gtsam { | |||
| 	 * tests and in generic algorithms. | ||||
| 	 * | ||||
| 	 * See macros for details on using this structure | ||||
| 	 * @ingroup base | ||||
| 	 * @addtogroup base | ||||
| 	 * @tparam T is the type this constrains to be testable - assumes print() and equals() | ||||
| 	 */ | ||||
| 	template <class T> | ||||
|  |  | |||
|  | @ -40,7 +40,7 @@ template<class MATRIX> class SymmetricBlockView; | |||
|  * all rows, rowEnd() should be set to the number of rows in the matrix (i.e. | ||||
|  * one after the last true row index). | ||||
|  * | ||||
|  * @ingroup base | ||||
|  * @addtogroup base | ||||
|  */ | ||||
| template<class MATRIX> | ||||
| class VerticalBlockView { | ||||
|  | @ -330,7 +330,7 @@ private: | |||
|  * change the apparent matrix view.  firstBlock() determines the block that | ||||
|  * appears to have index 0 for all operations (except re-setting firstBlock()). | ||||
|  * | ||||
|  * @ingroup base | ||||
|  * @addtogroup base | ||||
|  */ | ||||
| template<class MATRIX> | ||||
| class SymmetricBlockView { | ||||
|  |  | |||
|  | @ -14,7 +14,7 @@ | |||
|  * @brief    Typedefs for easier changing of types | ||||
|  * @author   Richard Roberts | ||||
|  * @date     Aug 21, 2010 | ||||
|  * @defgroup base | ||||
|  * @addtogroup base | ||||
|  */ | ||||
| 
 | ||||
| #pragma once | ||||
|  |  | |||
|  | @ -25,7 +25,7 @@ namespace gtsam { | |||
| 
 | ||||
| /**
 | ||||
|  * @brief Calibration used by Bundler | ||||
|  * @ingroup geometry | ||||
|  * @addtogroup geometry | ||||
|  * \nosubgrouping | ||||
|  */ | ||||
| class Cal3Bundler : public DerivedValue<Cal3Bundler> { | ||||
|  |  | |||
|  | @ -25,7 +25,7 @@ namespace gtsam { | |||
| 
 | ||||
| /**
 | ||||
|  * @brief Calibration of a camera with radial distortion | ||||
|  * @ingroup geometry | ||||
|  * @addtogroup geometry | ||||
|  * \nosubgrouping | ||||
|  */ | ||||
| class Cal3DS2 : public DerivedValue<Cal3DS2> { | ||||
|  |  | |||
|  | @ -16,7 +16,7 @@ | |||
|  */ | ||||
| 
 | ||||
| /**
 | ||||
|  * @defgroup geometry | ||||
|  * @addtogroup geometry | ||||
|  */ | ||||
| 
 | ||||
| #pragma once | ||||
|  | @ -28,7 +28,7 @@ namespace gtsam { | |||
| 
 | ||||
| 	/**
 | ||||
| 	 * @brief The most common 5DOF 3D->2D calibration | ||||
| 	 * @ingroup geometry | ||||
| 	 * @addtogroup geometry | ||||
| 	 * \nosubgrouping | ||||
| 	 */ | ||||
| 	class Cal3_S2 : public DerivedValue<Cal3_S2> { | ||||
|  |  | |||
|  | @ -23,7 +23,7 @@ namespace gtsam { | |||
| 
 | ||||
| 	/**
 | ||||
| 	 * @brief The most common 5DOF 3D->2D calibration, stereo version | ||||
| 	 * @ingroup geometry | ||||
| 	 * @addtogroup geometry | ||||
| 	 * \nosubgrouping | ||||
| 	 */ | ||||
| 	class Cal3_S2Stereo { | ||||
|  |  | |||
|  | @ -33,7 +33,7 @@ namespace gtsam { | |||
| 	 * A Calibrated camera class [R|-R't], calibration K=I. | ||||
| 	 * If calibration is known, it is more computationally efficient | ||||
| 	 * to calibrate the measurements rather than try to predict in pixels. | ||||
| 	 * @ingroup geometry | ||||
| 	 * @addtogroup geometry | ||||
| 	 * \nosubgrouping | ||||
| 	 */ | ||||
| 	class CalibratedCamera : public DerivedValue<CalibratedCamera> { | ||||
|  |  | |||
|  | @ -33,7 +33,7 @@ namespace gtsam { | |||
| 
 | ||||
|   /**
 | ||||
|    * A pinhole camera class that has a Pose3 and a Calibration. | ||||
|    * @ingroup geometry | ||||
|    * @addtogroup geometry | ||||
|    * \nosubgrouping | ||||
|    */ | ||||
|   template <typename Calibration> | ||||
|  |  | |||
|  | @ -29,7 +29,7 @@ namespace gtsam { | |||
|  * A 2D point | ||||
|  * Complies with the Testable Concept | ||||
|  * Functional, so no set functions: once created, a point is constant. | ||||
|  * @ingroup geometry | ||||
|  * @addtogroup geometry | ||||
|  * \nosubgrouping | ||||
|  */ | ||||
| class Point2 : public DerivedValue<Point2> { | ||||
|  |  | |||
|  | @ -32,7 +32,7 @@ namespace gtsam { | |||
| 
 | ||||
|   /**
 | ||||
|    * A 3D point | ||||
|    * @ingroup geometry | ||||
|    * @addtogroup geometry | ||||
|    * \nosubgrouping | ||||
|    */ | ||||
|   class Point3 : public DerivedValue<Point3> { | ||||
|  |  | |||
|  | @ -30,7 +30,7 @@ namespace gtsam { | |||
| 
 | ||||
| /**
 | ||||
|  * A 2D pose (Point2,Rot2) | ||||
|  * @ingroup geometry | ||||
|  * @addtogroup geometry | ||||
|  * \nosubgrouping | ||||
|  */ | ||||
| class Pose2 : public DerivedValue<Pose2> { | ||||
|  |  | |||
|  | @ -32,7 +32,7 @@ namespace gtsam { | |||
| 
 | ||||
|   /**
 | ||||
|    * A 3D pose (R,t) : (Rot3,Point3) | ||||
|    * @ingroup geometry | ||||
|    * @addtogroup geometry | ||||
|    * \nosubgrouping | ||||
|    */ | ||||
|   class Pose3 : public DerivedValue<Pose3> { | ||||
|  |  | |||
|  | @ -28,7 +28,7 @@ namespace gtsam { | |||
| 	/**
 | ||||
| 	 * Rotation matrix | ||||
| 	 * NOTE: the angle theta is in radians unless explicitly stated | ||||
| 	 * @ingroup geometry | ||||
| 	 * @addtogroup geometry | ||||
| 	 * \nosubgrouping | ||||
| 	 */ | ||||
| 	class Rot2 : public DerivedValue<Rot2> { | ||||
|  |  | |||
|  | @ -48,7 +48,7 @@ namespace gtsam { | |||
|    * @brief A 3D rotation represented as a rotation matrix if the preprocessor | ||||
|    * symbol GTSAM_DEFAULT_QUATERNIONS is not defined, or as a quaternion if it | ||||
|    * is defined. | ||||
|    * @ingroup geometry | ||||
|    * @addtogroup geometry | ||||
|    * \nosubgrouping | ||||
|    */ | ||||
|   class Rot3 : public DerivedValue<Rot3> { | ||||
|  |  | |||
|  | @ -28,7 +28,7 @@ namespace gtsam { | |||
| 
 | ||||
| /**
 | ||||
|  * A stereo camera class, parameterize by left camera pose and stereo calibration | ||||
|  * @ingroup geometry | ||||
|  * @addtogroup geometry | ||||
|  */ | ||||
| class StereoCamera { | ||||
| 
 | ||||
|  |  | |||
|  | @ -25,7 +25,7 @@ namespace gtsam { | |||
| 
 | ||||
| 	/**
 | ||||
| 	 * A 2D stereo point, v will be same for rectified images | ||||
| 	 * @ingroup geometry | ||||
| 	 * @addtogroup geometry | ||||
| 	 * \nosubgrouping | ||||
| 	 */ | ||||
| 	class StereoPoint2 : public DerivedValue<StereoPoint2> { | ||||
|  |  | |||
|  | @ -23,7 +23,7 @@ namespace tensors { | |||
| 
 | ||||
| 	/**
 | ||||
| 	 * A rank 1 tensor. Actually stores data. | ||||
| 	 * @ingroup tensors | ||||
| 	 * @addtogroup tensors | ||||
| 	 * \nosubgrouping | ||||
| 	 */ | ||||
| 	template<int N> | ||||
|  |  | |||
|  | @ -29,7 +29,7 @@ namespace tensors { | |||
| 	 * Templated class to provide a rank 1 tensor interface to a class. | ||||
| 	 * This class does not store any data but the result of an expression. | ||||
| 	 * It is associated with an index. | ||||
| 	 * @ingroup tensors | ||||
| 	 * @addtogroup tensors | ||||
| 	 * \nosubgrouping | ||||
| 	 */ | ||||
| 	template<class A, class I> class Tensor1Expression { | ||||
|  |  | |||
|  | @ -23,7 +23,7 @@ namespace tensors { | |||
| 
 | ||||
| /**
 | ||||
|  * Rank 2 Tensor | ||||
|  * @ingroup tensors | ||||
|  * @addtogroup tensors | ||||
|  * \nosubgrouping | ||||
|  */ | ||||
| template<int N1, int N2> | ||||
|  |  | |||
|  | @ -26,7 +26,7 @@ namespace tensors { | |||
| 
 | ||||
| 	/**
 | ||||
| 	 * Templated class to hold a rank 2 tensor expression. | ||||
| 	 * @ingroup tensors | ||||
| 	 * @addtogroup tensors | ||||
| 	 * \nosubgrouping | ||||
| 	 */ | ||||
| 	template<class A, class I, class J> class Tensor2Expression { | ||||
|  |  | |||
|  | @ -23,7 +23,7 @@ namespace tensors { | |||
| 
 | ||||
| 	/**
 | ||||
| 	 * Rank 3 Tensor | ||||
| 	 * @ingroup tensors | ||||
| 	 * @addtogroup tensors | ||||
| 	 * \nosubgrouping | ||||
| 	 */ | ||||
| 	template<int N1, int N2, int N3> | ||||
|  |  | |||
|  | @ -25,7 +25,7 @@ namespace tensors { | |||
| 
 | ||||
| 	/**
 | ||||
| 	 * templated class to interface to an object A as a rank 3 tensor | ||||
| 	 * @ingroup tensors | ||||
| 	 * @addtogroup tensors | ||||
| 	 * \nosubgrouping | ||||
| 	 */ | ||||
| 	template<class A, class I, class J, class K> class Tensor3Expression { | ||||
|  |  | |||
|  | @ -23,7 +23,7 @@ namespace tensors { | |||
| 
 | ||||
| 	/**
 | ||||
| 	 * Rank 4 Tensor | ||||
| 	 * @ingroup tensors | ||||
| 	 * @addtogroup tensors | ||||
| 	 * \nosubgrouping | ||||
| 	 */ | ||||
| 	template<int N1, int N2, int N3, int N4> | ||||
|  |  | |||
|  | @ -23,7 +23,7 @@ namespace tensors { | |||
| 
 | ||||
| 	/**
 | ||||
| 	 * Rank 5 Tensor | ||||
| 	 * @ingroup tensors | ||||
| 	 * @addtogroup tensors | ||||
| 	 * \nosubgrouping | ||||
| 	 */ | ||||
| 	template<int N1, int N2, int N3, int N4, int N5> | ||||
|  |  | |||
|  | @ -25,7 +25,7 @@ namespace tensors { | |||
| 
 | ||||
| 	/**
 | ||||
| 	 * templated class to interface to an object A as a rank 5 tensor | ||||
| 	 * @ingroup tensors | ||||
| 	 * @addtogroup tensors | ||||
| 	 * \nosubgrouping | ||||
| 	 */ | ||||
| 	template<class A, class I, class J, class K, class L, class M> class Tensor5Expression { | ||||
|  |  | |||
|  | @ -25,21 +25,21 @@ namespace gtsam { | |||
| 
 | ||||
| 	/**
 | ||||
| 	 * 2D Point in homogeneous coordinates | ||||
| 	 * @ingroup geometry | ||||
| 	 * @addtogroup geometry | ||||
| 	 */ | ||||
| 	typedef tensors::Tensor1<3> Point2h; | ||||
| 	Point2h point2h(double x, double y, double w); ///< create Point2h
 | ||||
| 
 | ||||
| 	/**
 | ||||
| 	 * 2D Line in homogeneous coordinates | ||||
| 	 * @ingroup geometry | ||||
| 	 * @addtogroup geometry | ||||
| 	 */ | ||||
| 	typedef tensors::Tensor1<3> Line2h; | ||||
| 	Line2h line2h(double a, double b, double c); ///< create Line2h
 | ||||
| 
 | ||||
| 	/**
 | ||||
| 	 * 2D (homegeneous) Point correspondence | ||||
| 	 * @ingroup geometry | ||||
| 	 * @addtogroup geometry | ||||
| 	 */ | ||||
| 	struct Correspondence { | ||||
| 		Point2h first;  ///< First point
 | ||||
|  | @ -63,19 +63,19 @@ namespace gtsam { | |||
| 
 | ||||
| 	/**
 | ||||
| 	 * 2D-2D Homography | ||||
| 	 * @ingroup geometry | ||||
| 	 * @addtogroup geometry | ||||
| 	 */ | ||||
| 	typedef tensors::Tensor2<3, 3> Homography2; | ||||
| 
 | ||||
| 	/**
 | ||||
| 	 * Fundamental Matrix | ||||
| 	 * @ingroup geometry | ||||
| 	 * @addtogroup geometry | ||||
| 	 */ | ||||
| 	typedef tensors::Tensor2<3, 3> FundamentalMatrix; | ||||
| 
 | ||||
| 	/**
 | ||||
| 	 * Triplet of (homogeneous) 2D points | ||||
| 	 * @ingroup geometry | ||||
| 	 * @addtogroup geometry | ||||
| 	 */ | ||||
| 	struct Triplet { | ||||
| 		Point2h first;  ///< First point
 | ||||
|  | @ -97,27 +97,27 @@ namespace gtsam { | |||
| 
 | ||||
| 	/**
 | ||||
| 	 * Trifocal Tensor | ||||
| 	 * @ingroup geometry | ||||
| 	 * @addtogroup geometry | ||||
| 	 */ | ||||
| 	typedef tensors::Tensor3<3, 3, 3> TrifocalTensor; | ||||
| 
 | ||||
| 	/**
 | ||||
| 	 * 3D Point in homogeneous coordinates | ||||
| 	 * @ingroup geometry | ||||
| 	 * @addtogroup geometry | ||||
| 	 */ | ||||
| 	typedef tensors::Tensor1<4> Point3h; | ||||
| 	Point3h point3h(double X, double Y, double Z, double W); ///< create Point3h
 | ||||
| 
 | ||||
| 	/**
 | ||||
| 	 * 3D Plane in homogeneous coordinates | ||||
| 	 * @ingroup geometry | ||||
| 	 * @addtogroup geometry | ||||
| 	 */ | ||||
| 	typedef tensors::Tensor1<4> Plane3h; | ||||
| 	Plane3h plane3h(double a, double b, double c, double d); ///< create Plane3h
 | ||||
| 
 | ||||
| 	/**
 | ||||
| 	 * 3D to 2D projective camera | ||||
| 	 * @ingroup geometry | ||||
| 	 * @addtogroup geometry | ||||
| 	 */ | ||||
| 	typedef tensors::Tensor2<3, 4> ProjectiveCamera; | ||||
| 
 | ||||
|  |  | |||
|  | @ -14,7 +14,7 @@ | |||
|  * @brief Tensor expression templates based on http://www.gps.caltech.edu/~walter/FTensor/FTensor.pdf
 | ||||
|  * @date Feb 10, 2010 | ||||
|  * @author Frank Dellaert | ||||
|  * @defgroup tensors | ||||
|  * @addtogroup tensors | ||||
|  */ | ||||
| 
 | ||||
| #pragma once | ||||
|  |  | |||
|  | @ -51,9 +51,9 @@ namespace gtsam { | |||
| 		return data; | ||||
| 	} | ||||
| 
 | ||||
|   /* ************************************************************************* */ | ||||
| 	template<class CONDITIONAL, class CLIQUE> | ||||
| 	void BayesTree<CONDITIONAL,CLIQUE>::getCliqueData(CliqueData& data, | ||||
| 			typename BayesTree<CONDITIONAL,CLIQUE>::sharedClique clique) const { | ||||
| 	void BayesTree<CONDITIONAL,CLIQUE>::getCliqueData(CliqueData& data, sharedClique clique) const { | ||||
| 		data.conditionalSizes.push_back((*clique)->nrFrontals()); | ||||
| 		data.separatorSizes.push_back((*clique)->nrParents()); | ||||
| 		BOOST_FOREACH(sharedClique c, clique->children_) { | ||||
|  | @ -64,7 +64,7 @@ namespace gtsam { | |||
| 	/* ************************************************************************* */ | ||||
| 	template<class CONDITIONAL, class CLIQUE> | ||||
| 	void BayesTree<CONDITIONAL,CLIQUE>::saveGraph(const std::string &s) const { | ||||
| 		if (!root_.get()) throw invalid_argument("the root of bayes tree has not been initialized!"); | ||||
| 		if (!root_.get()) throw invalid_argument("the root of Bayes tree has not been initialized!"); | ||||
| 		ofstream of(s.c_str()); | ||||
| 		of<< "digraph G{\n"; | ||||
| 		saveGraph(of, root_); | ||||
|  | @ -73,9 +73,7 @@ namespace gtsam { | |||
| 	} | ||||
| 
 | ||||
| 	template<class CONDITIONAL, class CLIQUE> | ||||
| 	void BayesTree<CONDITIONAL,CLIQUE>::saveGraph(ostream &s, | ||||
| 			typename BayesTree<CONDITIONAL,CLIQUE>::sharedClique clique, | ||||
| 			int parentnum) const { | ||||
| 	void BayesTree<CONDITIONAL,CLIQUE>::saveGraph(ostream &s, sharedClique clique, int parentnum) const { | ||||
| 		static int num = 0; | ||||
| 		bool first = true; | ||||
| 		std::stringstream out; | ||||
|  |  | |||
|  | @ -46,7 +46,7 @@ namespace gtsam { | |||
| 	 * @tparam CLIQUE The type of the clique data structure, defaults to BayesTreeClique, normally do not change this | ||||
| 	 * as it is only used when developing special versions of BayesTree, e.g. for ISAM2. | ||||
| 	 * | ||||
| 	 * \ingroup Multifrontal | ||||
| 	 * \addtogroup Multifrontal | ||||
| 	 * \nosubgrouping | ||||
| 	 */ | ||||
| 	template<class CONDITIONAL, class CLIQUE=BayesTreeClique<CONDITIONAL> > | ||||
|  |  | |||
|  | @ -36,15 +36,14 @@ namespace gtsam { | |||
| 	/* ************************************************************************* */ | ||||
| 	template<class F, class JT> | ||||
| 	GenericMultifrontalSolver<F, JT>::GenericMultifrontalSolver( | ||||
| 			const typename FactorGraph<F>::shared_ptr& graph, | ||||
| 			const sharedGraph& graph, | ||||
| 			const VariableIndex::shared_ptr& variableIndex) : | ||||
| 			structure_(variableIndex), junctionTree_(new JT(*graph, *structure_)) { | ||||
| 	} | ||||
| 
 | ||||
| 	/* ************************************************************************* */ | ||||
| 	template<class F, class JT> | ||||
| 	void GenericMultifrontalSolver<F, JT>::replaceFactors( | ||||
| 			const typename FactorGraph<F>::shared_ptr& graph) { | ||||
| 	void GenericMultifrontalSolver<F, JT>::replaceFactors(const sharedGraph& graph) { | ||||
| 		junctionTree_.reset(new JT(*graph, *structure_)); | ||||
| 	} | ||||
| 
 | ||||
|  |  | |||
|  | @ -41,7 +41,7 @@ namespace gtsam { | |||
| 	template<class FACTOR> | ||||
| 	GenericSequentialSolver<FACTOR>::GenericSequentialSolver( | ||||
| 			const sharedFactorGraph& factorGraph, | ||||
| 			const VariableIndex::shared_ptr& variableIndex) : | ||||
| 			const boost::shared_ptr<VariableIndex>& variableIndex) : | ||||
| 			factors_(factorGraph), structure_(variableIndex), | ||||
| 			eliminationTree_(EliminationTree<FACTOR>::Create(*factors_, *structure_)) { | ||||
| 	} | ||||
|  |  | |||
|  | @ -72,7 +72,7 @@ namespace gtsam { | |||
|   /* ************************************************************************* */ | ||||
|   template<class FG, class BTCLIQUE> | ||||
|   typename JunctionTree<FG,BTCLIQUE>::sharedClique JunctionTree<FG,BTCLIQUE>::distributeFactors( | ||||
|       const FG& fg, const typename SymbolicBayesTree::sharedClique& bayesClique) { | ||||
|       const FG& fg, const SymbolicBayesTree::sharedClique& bayesClique) { | ||||
| 
 | ||||
|     // Build "target" index.  This is an index for each variable of the factors
 | ||||
|     // that involve this variable as their *lowest-ordered* variable.  For each
 | ||||
|  |  | |||
|  | @ -47,7 +47,7 @@ namespace gtsam { | |||
| 	 * except that in the JunctionTree, at each node multiple variables are eliminated at a time. | ||||
| 	 * | ||||
| 	 * | ||||
| 	 * \ingroup Multifrontal | ||||
| 	 * \addtogroup Multifrontal | ||||
| 	 * \nosubgrouping | ||||
| 	 */ | ||||
| 	template<class FG, class BTCLIQUE=typename BayesTree<typename FG::FactorType::ConditionalType>::Clique> | ||||
|  |  | |||
|  | @ -35,27 +35,27 @@ void optimizeInPlace(const GaussianBayesTree& bayesTree, VectorValues& result) { | |||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| VectorValues optimizeGradientSearch(const GaussianBayesTree& Rd) { | ||||
| VectorValues optimizeGradientSearch(const GaussianBayesTree& bayesTree) { | ||||
|   tic(0, "Allocate VectorValues"); | ||||
|   VectorValues grad = *allocateVectorValues(Rd); | ||||
|   VectorValues grad = *allocateVectorValues(bayesTree); | ||||
|   toc(0, "Allocate VectorValues"); | ||||
| 
 | ||||
|   optimizeGradientSearchInPlace(Rd, grad); | ||||
|   optimizeGradientSearchInPlace(bayesTree, grad); | ||||
| 
 | ||||
|   return grad; | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| void optimizeGradientSearchInPlace(const GaussianBayesTree& Rd, VectorValues& grad) { | ||||
| void optimizeGradientSearchInPlace(const GaussianBayesTree& bayesTree, VectorValues& grad) { | ||||
|   tic(1, "Compute Gradient"); | ||||
|   // Compute gradient (call gradientAtZero function, which is defined for various linear systems)
 | ||||
|   gradientAtZero(Rd, grad); | ||||
|   gradientAtZero(bayesTree, grad); | ||||
|   double gradientSqNorm = grad.dot(grad); | ||||
|   toc(1, "Compute Gradient"); | ||||
| 
 | ||||
|   tic(2, "Compute R*g"); | ||||
|   // Compute R * g
 | ||||
|   FactorGraph<JacobianFactor> Rd_jfg(Rd); | ||||
|   FactorGraph<JacobianFactor> Rd_jfg(bayesTree); | ||||
|   Errors Rg = Rd_jfg * grad; | ||||
|   toc(2, "Compute R*g"); | ||||
| 
 | ||||
|  |  | |||
|  | @ -25,13 +25,14 @@ | |||
| 
 | ||||
| namespace gtsam { | ||||
| 
 | ||||
| /// A Bayes Tree representing a Gaussian density
 | ||||
| typedef BayesTree<GaussianConditional> GaussianBayesTree; | ||||
| 
 | ||||
| /// optimize the BayesTree, starting from the root
 | ||||
| VectorValues optimize(const GaussianBayesTree& bayesTree); | ||||
| 
 | ||||
| /// recursively optimize this conditional and all subtrees
 | ||||
| void optimizeInPlace(const GaussianBayesTree& clique, VectorValues& result); | ||||
| void optimizeInPlace(const GaussianBayesTree& bayesTree, VectorValues& result); | ||||
| 
 | ||||
| namespace internal { | ||||
| template<class BAYESTREE> | ||||
|  | @ -63,10 +64,10 @@ void optimizeInPlace(const typename BAYESTREE::sharedClique& clique, VectorValue | |||
|  * | ||||
|  * \f[ \delta x = \hat\alpha g = \frac{-g^T g}{(R g)^T(R g)} \f] | ||||
|  */ | ||||
| VectorValues optimizeGradientSearch(const GaussianBayesTree& bn); | ||||
| VectorValues optimizeGradientSearch(const GaussianBayesTree& bayesTree); | ||||
| 
 | ||||
| /** In-place version of optimizeGradientSearch requiring pre-allocated VectorValues \c x */ | ||||
| void optimizeGradientSearchInPlace(const GaussianBayesTree& bn, VectorValues& grad); | ||||
| void optimizeGradientSearchInPlace(const GaussianBayesTree& bayesTree, VectorValues& grad); | ||||
| 
 | ||||
| /**
 | ||||
|  * Compute the gradient of the energy function, | ||||
|  |  | |||
|  | @ -202,7 +202,7 @@ namespace gtsam { | |||
| 
 | ||||
|   /**
 | ||||
|    * Combine and eliminate several factors. | ||||
|    * \ingroup LinearSolving | ||||
|    * \addtogroup LinearSolving | ||||
|    */ | ||||
| 	JacobianFactor::shared_ptr CombineJacobians( | ||||
| 			const FactorGraph<JacobianFactor>& factors, | ||||
|  | @ -223,7 +223,7 @@ namespace gtsam { | |||
| 	 * @param nrFrontals Number of frontal variables to eliminate. | ||||
| 	 * @return The conditional and remaining factor | ||||
| 
 | ||||
|    * \ingroup LinearSolving | ||||
|    * \addtogroup LinearSolving | ||||
| 	 */ | ||||
| 	GaussianFactorGraph::EliminationResult EliminateJacobians(const FactorGraph< | ||||
| 			JacobianFactor>& factors, size_t nrFrontals = 1); | ||||
|  | @ -238,7 +238,7 @@ namespace gtsam { | |||
| 	 * @param nrFrontals Number of frontal variables to eliminate. | ||||
| 	 * @return The conditional and remaining factor | ||||
| 
 | ||||
|    * \ingroup LinearSolving | ||||
|    * \addtogroup LinearSolving | ||||
| 	 */ | ||||
|   GaussianFactorGraph::EliminationResult EliminateQR(const FactorGraph< | ||||
| 			GaussianFactor>& factors, size_t nrFrontals = 1); | ||||
|  | @ -260,7 +260,7 @@ namespace gtsam { | |||
|    * @param nrFrontals Number of frontal variables to eliminate. | ||||
|    * @return The conditional and remaining factor | ||||
| 
 | ||||
|    * \ingroup LinearSolving | ||||
|    * \addtogroup LinearSolving | ||||
|    */ | ||||
|   GaussianFactorGraph::EliminationResult EliminatePreferCholesky(const FactorGraph< | ||||
| 			GaussianFactor>& factors, size_t nrFrontals = 1); | ||||
|  | @ -281,7 +281,7 @@ namespace gtsam { | |||
|    * @param nrFrontals Number of frontal variables to eliminate. | ||||
|    * @return The conditional and remaining factor | ||||
| 
 | ||||
|    * \ingroup LinearSolving | ||||
|    * \addtogroup LinearSolving | ||||
|    */ | ||||
|   GaussianFactorGraph::EliminationResult EliminateCholesky(const FactorGraph< | ||||
| 			GaussianFactor>& factors, size_t nrFrontals = 1); | ||||
|  |  | |||
|  | @ -35,7 +35,7 @@ namespace gtsam { | |||
| 	 * create a BayesTree<GaussianConditional>. In both cases, you need to provide a basic | ||||
| 	 * GaussianFactorGraph::Eliminate function that will be used to | ||||
| 	 * | ||||
| 	 * \ingroup Multifrontal | ||||
| 	 * \addtogroup Multifrontal | ||||
| 	 */ | ||||
| 	class GaussianJunctionTree: public JunctionTree<GaussianFactorGraph> { | ||||
| 
 | ||||
|  |  | |||
|  | @ -28,11 +28,7 @@ | |||
| namespace gtsam { | ||||
| 
 | ||||
| /**
 | ||||
|  * @defgroup ISAM2 | ||||
|  */ | ||||
| 
 | ||||
| /**
 | ||||
|  * @ingroup ISAM2 | ||||
|  * @addtogroup ISAM2 | ||||
|  * Parameters for ISAM2 using Gauss-Newton optimization.  Either this class or | ||||
|  * ISAM2DoglegParams should be specified as the optimizationParams in | ||||
|  * ISAM2Params, which should in turn be passed to ISAM2(const ISAM2Params&). | ||||
|  | @ -47,7 +43,7 @@ struct ISAM2GaussNewtonParams { | |||
| }; | ||||
| 
 | ||||
| /**
 | ||||
|  * @ingroup ISAM2 | ||||
|  * @addtogroup ISAM2 | ||||
|  * Parameters for ISAM2 using Dogleg optimization.  Either this class or | ||||
|  * ISAM2GaussNewtonParams should be specified as the optimizationParams in | ||||
|  * ISAM2Params, which should in turn be passed to ISAM2(const ISAM2Params&). | ||||
|  | @ -69,7 +65,7 @@ struct ISAM2DoglegParams { | |||
| }; | ||||
| 
 | ||||
| /**
 | ||||
|  * @ingroup ISAM2 | ||||
|  * @addtogroup ISAM2 | ||||
|  * Parameters for the ISAM2 algorithm.  Default parameter values are listed below. | ||||
|  */ | ||||
| struct ISAM2Params { | ||||
|  | @ -147,7 +143,7 @@ struct ISAM2Params { | |||
| }; | ||||
| 
 | ||||
| /**
 | ||||
|  * @ingroup ISAM2 | ||||
|  * @addtogroup ISAM2 | ||||
|  * This struct is returned from ISAM2::update() and contains information about | ||||
|  * the update that is useful for determining whether the solution is | ||||
|  * converging, and about how much work was required for the update.  See member | ||||
|  | @ -315,7 +311,7 @@ private: | |||
| }; | ||||
| 
 | ||||
| /**
 | ||||
|  * @ingroup ISAM2 | ||||
|  * @addtogroup ISAM2 | ||||
|  * Implementation of the full ISAM2 algorithm for incremental nonlinear optimization. | ||||
|  * | ||||
|  * The typical cycle of using this class to create an instance by providing ISAM2Params | ||||
|  |  | |||
|  | @ -27,10 +27,6 @@ | |||
| #include <gtsam/nonlinear/Marginals.h> | ||||
| #include <gtsam/geometry/Pose2.h> | ||||
| 
 | ||||
| /**
 | ||||
|  * @defgroup SLAM | ||||
|  */ | ||||
| 
 | ||||
| // Use planarSLAM namespace for specific SLAM instance
 | ||||
| namespace planarSLAM { | ||||
| 
 | ||||
|  | @ -55,7 +51,7 @@ namespace planarSLAM { | |||
| 
 | ||||
|   /*
 | ||||
|    * Values class, inherited from Values, mainly used as a convenience for MATLAB wrapper | ||||
|    * @ingroup SLAM | ||||
|    * @addtogroup SLAM | ||||
|    */ | ||||
|   struct Values: public gtsam::Values { | ||||
| 
 | ||||
|  | @ -88,7 +84,7 @@ namespace planarSLAM { | |||
| 
 | ||||
|   /**
 | ||||
|    * Graph class, inherited from NonlinearFactorGraph, used as a convenience for MATLAB wrapper | ||||
|    * @ingroup SLAM | ||||
|    * @addtogroup SLAM | ||||
|    */ | ||||
|   struct Graph: public NonlinearFactorGraph { | ||||
| 
 | ||||
|  |  | |||
|  | @ -32,7 +32,7 @@ namespace pose2SLAM { | |||
| 
 | ||||
|   /*
 | ||||
|    * Values class, inherited from Values, mainly used as a convenience for MATLAB wrapper | ||||
|    * @ingroup SLAM | ||||
|    * @addtogroup SLAM | ||||
|    */ | ||||
|   struct Values: public gtsam::Values { | ||||
| 
 | ||||
|  | @ -82,7 +82,7 @@ namespace pose2SLAM { | |||
| 
 | ||||
|   /**
 | ||||
|    * Graph class, inherited from NonlinearFactorGraph, used as a convenience for MATLAB wrapper | ||||
|    * @ingroup SLAM | ||||
|    * @addtogroup SLAM | ||||
|    */ | ||||
|   struct Graph: public NonlinearFactorGraph { | ||||
| 
 | ||||
|  |  | |||
|  | @ -32,7 +32,7 @@ namespace pose3SLAM { | |||
| 
 | ||||
|   /*
 | ||||
|    * Values class, inherited from Values, mainly used as a convenience for MATLAB wrapper | ||||
|    * @ingroup SLAM | ||||
|    * @addtogroup SLAM | ||||
|    */ | ||||
|   struct Values: public gtsam::Values { | ||||
| 
 | ||||
|  | @ -77,7 +77,7 @@ namespace pose3SLAM { | |||
| 
 | ||||
|   /**
 | ||||
|    * Graph class, inherited from NonlinearFactorGraph, used as a convenience for MATLAB wrapper | ||||
|    * @ingroup SLAM | ||||
|    * @addtogroup SLAM | ||||
|    */ | ||||
|   struct Graph: public NonlinearFactorGraph { | ||||
| 
 | ||||
|  |  | |||
|  | @ -26,7 +26,7 @@ namespace gtsam { | |||
| 
 | ||||
| 	/**
 | ||||
| 	 * @brief Binary tree | ||||
| 	 * @ingroup base | ||||
| 	 * @addtogroup base | ||||
| 	 */ | ||||
| 	template<class KEY, class VALUE> | ||||
| 	class BTree { | ||||
|  |  | |||
|  | @ -34,7 +34,7 @@ namespace gtsam { | |||
| 	 * S = {S_1,S_2,...} of disjoint dynamic sets. Each set is identified by | ||||
| 	 * a representative, which is some member of the set. | ||||
| 	 * | ||||
| 	 * @ingroup base | ||||
| 	 * @addtogroup base | ||||
| 	 */ | ||||
| 	template <class KEY> | ||||
| 	class DSF : protected BTree<KEY, KEY> { | ||||
|  |  | |||
|  | @ -27,7 +27,7 @@ namespace gtsam { | |||
| 
 | ||||
| 	/**
 | ||||
| 	 * A fast implementation of disjoint set forests that uses vector as underly data structure. | ||||
| 	 * @ingroup base | ||||
| 	 * @addtogroup base | ||||
| 	 */ | ||||
| 	class DSFVector { | ||||
| 
 | ||||
|  |  | |||
		Loading…
	
		Reference in New Issue