Merged in feature/expressions (pull request #45)
Pull request for debugging Expression<double> issuerelease/4.3a0
						commit
						ea02e577f5
					
				
							
								
								
									
										106
									
								
								.cproject
								
								
								
								
							
							
						
						
									
										106
									
								
								.cproject
								
								
								
								
							|  | @ -592,6 +592,7 @@ | |||
| 			</target> | ||||
| 			<target name="tests/testBayesTree.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments/> | ||||
| 				<buildTarget>tests/testBayesTree.run</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>false</useDefaultCommand> | ||||
|  | @ -599,6 +600,7 @@ | |||
| 			</target> | ||||
| 			<target name="testBinaryBayesNet.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments/> | ||||
| 				<buildTarget>testBinaryBayesNet.run</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>false</useDefaultCommand> | ||||
|  | @ -646,6 +648,7 @@ | |||
| 			</target> | ||||
| 			<target name="testSymbolicBayesNet.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments/> | ||||
| 				<buildTarget>testSymbolicBayesNet.run</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>false</useDefaultCommand> | ||||
|  | @ -653,6 +656,7 @@ | |||
| 			</target> | ||||
| 			<target name="tests/testSymbolicFactor.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments/> | ||||
| 				<buildTarget>tests/testSymbolicFactor.run</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>false</useDefaultCommand> | ||||
|  | @ -660,6 +664,7 @@ | |||
| 			</target> | ||||
| 			<target name="testSymbolicFactorGraph.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments/> | ||||
| 				<buildTarget>testSymbolicFactorGraph.run</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>false</useDefaultCommand> | ||||
|  | @ -675,6 +680,7 @@ | |||
| 			</target> | ||||
| 			<target name="tests/testBayesTree" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments/> | ||||
| 				<buildTarget>tests/testBayesTree</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>false</useDefaultCommand> | ||||
|  | @ -1122,6 +1128,7 @@ | |||
| 			</target> | ||||
| 			<target name="testErrors.run" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments/> | ||||
| 				<buildTarget>testErrors.run</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>false</useDefaultCommand> | ||||
|  | @ -1351,6 +1358,46 @@ | |||
| 				<useDefaultCommand>true</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
| 			</target> | ||||
| 			<target name="testBTree.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments>-j5</buildArguments> | ||||
| 				<buildTarget>testBTree.run</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>true</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
| 			</target> | ||||
| 			<target name="testDSF.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments>-j5</buildArguments> | ||||
| 				<buildTarget>testDSF.run</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>true</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
| 			</target> | ||||
| 			<target name="testDSFMap.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments>-j5</buildArguments> | ||||
| 				<buildTarget>testDSFMap.run</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>true</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
| 			</target> | ||||
| 			<target name="testDSFVector.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments>-j5</buildArguments> | ||||
| 				<buildTarget>testDSFVector.run</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>true</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
| 			</target> | ||||
| 			<target name="testFixedVector.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments>-j5</buildArguments> | ||||
| 				<buildTarget>testFixedVector.run</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>true</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
| 			</target> | ||||
| 			<target name="all" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments>-j2</buildArguments> | ||||
|  | @ -1433,7 +1480,6 @@ | |||
| 			</target> | ||||
| 			<target name="testSimulated2DOriented.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments/> | ||||
| 				<buildTarget>testSimulated2DOriented.run</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>false</useDefaultCommand> | ||||
|  | @ -1473,7 +1519,6 @@ | |||
| 			</target> | ||||
| 			<target name="testSimulated2D.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments/> | ||||
| 				<buildTarget>testSimulated2D.run</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>false</useDefaultCommand> | ||||
|  | @ -1481,7 +1526,6 @@ | |||
| 			</target> | ||||
| 			<target name="testSimulated3D.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments/> | ||||
| 				<buildTarget>testSimulated3D.run</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>false</useDefaultCommand> | ||||
|  | @ -1495,46 +1539,6 @@ | |||
| 				<useDefaultCommand>true</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
| 			</target> | ||||
| 			<target name="testBTree.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments>-j5</buildArguments> | ||||
| 				<buildTarget>testBTree.run</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>true</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
| 			</target> | ||||
| 			<target name="testDSF.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments>-j5</buildArguments> | ||||
| 				<buildTarget>testDSF.run</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>true</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
| 			</target> | ||||
| 			<target name="testDSFMap.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments>-j5</buildArguments> | ||||
| 				<buildTarget>testDSFMap.run</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>true</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
| 			</target> | ||||
| 			<target name="testDSFVector.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments>-j5</buildArguments> | ||||
| 				<buildTarget>testDSFVector.run</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>true</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
| 			</target> | ||||
| 			<target name="testFixedVector.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments>-j5</buildArguments> | ||||
| 				<buildTarget>testFixedVector.run</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>true</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
| 			</target> | ||||
| 			<target name="testEliminationTree.run" path="build/gtsam/inference/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments>-j5</buildArguments> | ||||
|  | @ -1792,6 +1796,7 @@ | |||
| 			</target> | ||||
| 			<target name="Generate DEB Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>cpack</buildCommand> | ||||
| 				<buildArguments/> | ||||
| 				<buildTarget>-G DEB</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>false</useDefaultCommand> | ||||
|  | @ -1799,6 +1804,7 @@ | |||
| 			</target> | ||||
| 			<target name="Generate RPM Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>cpack</buildCommand> | ||||
| 				<buildArguments/> | ||||
| 				<buildTarget>-G RPM</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>false</useDefaultCommand> | ||||
|  | @ -1806,6 +1812,7 @@ | |||
| 			</target> | ||||
| 			<target name="Generate TGZ Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>cpack</buildCommand> | ||||
| 				<buildArguments/> | ||||
| 				<buildTarget>-G TGZ</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>false</useDefaultCommand> | ||||
|  | @ -1813,6 +1820,7 @@ | |||
| 			</target> | ||||
| 			<target name="Generate TGZ Source Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>cpack</buildCommand> | ||||
| 				<buildArguments/> | ||||
| 				<buildTarget>--config CPackSourceConfig.cmake</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>false</useDefaultCommand> | ||||
|  | @ -2627,6 +2635,7 @@ | |||
| 			</target> | ||||
| 			<target name="testGraph.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments/> | ||||
| 				<buildTarget>testGraph.run</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>false</useDefaultCommand> | ||||
|  | @ -2634,6 +2643,7 @@ | |||
| 			</target> | ||||
| 			<target name="testJunctionTree.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments/> | ||||
| 				<buildTarget>testJunctionTree.run</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>false</useDefaultCommand> | ||||
|  | @ -2641,6 +2651,7 @@ | |||
| 			</target> | ||||
| 			<target name="testSymbolicBayesNetB.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments/> | ||||
| 				<buildTarget>testSymbolicBayesNetB.run</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>false</useDefaultCommand> | ||||
|  | @ -2758,6 +2769,14 @@ | |||
| 				<useDefaultCommand>true</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
| 			</target> | ||||
| 			<target name="testBasisDecompositions.run" path="build/gtsam_unstable/nonlinear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments>-j4</buildArguments> | ||||
| 				<buildTarget>testBasisDecompositions.run</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>true</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
| 			</target> | ||||
| 			<target name="testGaussianFactor.run" path="build/linear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments>-j2</buildArguments> | ||||
|  | @ -3168,7 +3187,6 @@ | |||
| 			</target> | ||||
| 			<target name="tests/testGaussianISAM2" path="build/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments/> | ||||
| 				<buildTarget>tests/testGaussianISAM2</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>false</useDefaultCommand> | ||||
|  |  | |||
|  | @ -235,7 +235,7 @@ struct DefaultChart<double> { | |||
|   static double retract(double origin, const vector& d) { | ||||
|     return origin + d[0]; | ||||
|   } | ||||
|   static const int getDimension(double) { | ||||
|   static int getDimension(double) { | ||||
|     return 1; | ||||
|   } | ||||
| }; | ||||
|  |  | |||
|  | @ -111,6 +111,18 @@ double Point3::norm(boost::optional<Matrix&> H) const { | |||
|   return r; | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| double Point3::norm(boost::optional<Eigen::Matrix<double,1,3>&> H) const { | ||||
|   double r = norm(); | ||||
|   if (H) { | ||||
|     if (fabs(r) > 1e-10) | ||||
|       *H << x_ / r, y_ / r, z_ / r; | ||||
|     else | ||||
|       *H << 1, 1, 1; // really infinity, why 1 ?
 | ||||
|   } | ||||
|   return r; | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| Point3 Point3::normalize(boost::optional<Matrix&> H) const { | ||||
|   Point3 normalized = *this / norm(); | ||||
|  |  | |||
|  | @ -186,6 +186,9 @@ namespace gtsam { | |||
|     /** Distance of the point from the origin, with Jacobian */ | ||||
|     double norm(boost::optional<Matrix&> H) const; | ||||
| 
 | ||||
|     /** Distance of the point from the origin, with Jacobian */ | ||||
|     double norm(boost::optional<Eigen::Matrix<double,1,3>&> H) const; | ||||
| 
 | ||||
|     /** normalize, with optional Jacobian */ | ||||
|     Point3 normalize(boost::optional<Matrix&> H = boost::none) const; | ||||
| 
 | ||||
|  |  | |||
|  | @ -46,7 +46,7 @@ namespace internal { | |||
|  * it just passes dense Eigen matrices through. | ||||
|  */ | ||||
| template<bool ConvertToDynamicRows> | ||||
| struct ConvertToDynamicRowsIf { | ||||
| struct ConvertToVirtualFunctionSupportedMatrixType { | ||||
|   template<typename Derived> | ||||
|   static Eigen::Matrix<double, Eigen::Dynamic, Derived::ColsAtCompileTime> convert( | ||||
|       const Eigen::MatrixBase<Derived> & x) { | ||||
|  | @ -55,7 +55,13 @@ struct ConvertToDynamicRowsIf { | |||
| }; | ||||
| 
 | ||||
| template<> | ||||
| struct ConvertToDynamicRowsIf<false> { | ||||
| struct ConvertToVirtualFunctionSupportedMatrixType<false> { | ||||
|   template<typename Derived> | ||||
|   static const Eigen::Matrix<double, Derived::RowsAtCompileTime, Derived::ColsAtCompileTime> convert( | ||||
|       const Eigen::MatrixBase<Derived> & x) { | ||||
|     return x; | ||||
|   } | ||||
|   // special treatment of matrices that don't need conversion
 | ||||
|   template<int Rows, int Cols> | ||||
|   static const Eigen::Matrix<double, Rows, Cols> & convert( | ||||
|       const Eigen::Matrix<double, Rows, Cols> & x) { | ||||
|  | @ -143,11 +149,11 @@ struct CallRecord: virtual private internal::ReverseADInterface< | |||
|     _startReverseAD(jacobians); | ||||
|   } | ||||
| 
 | ||||
|   template<int Rows> | ||||
|   inline void reverseAD(const Eigen::Matrix<double, Rows, Cols> & dFdT, | ||||
|   template<typename Derived> | ||||
|   inline void reverseAD(const Eigen::MatrixBase<Derived> & dFdT, | ||||
|       JacobianMap& jacobians) const { | ||||
|     _reverseAD( | ||||
|         internal::ConvertToDynamicRowsIf<(Rows > MaxVirtualStaticRows)>::convert( | ||||
|         internal::ConvertToVirtualFunctionSupportedMatrixType<(Derived::RowsAtCompileTime > MaxVirtualStaticRows)>::convert( | ||||
|             dFdT), jacobians); | ||||
|   } | ||||
| 
 | ||||
|  |  | |||
|  | @ -64,18 +64,36 @@ public: | |||
| }; | ||||
| 
 | ||||
| //-----------------------------------------------------------------------------
 | ||||
| /// Handle Leaf Case: reverseAD ends here, by writing a matrix into Jacobians
 | ||||
| template<int ROWS, int COLS> | ||||
| void handleLeafCase(const Eigen::Matrix<double, ROWS, COLS>& dTdA, | ||||
|     JacobianMap& jacobians, Key key) { | ||||
|   jacobians(key).block<ROWS, COLS>(0, 0) += dTdA; // block makes HUGE difference
 | ||||
| 
 | ||||
| namespace internal { | ||||
| 
 | ||||
| template <bool UseBlock, typename Derived> | ||||
| struct UseBlockIf { | ||||
|   static void addToJacobian(const Eigen::MatrixBase<Derived>& dTdA, | ||||
|                             JacobianMap& jacobians, Key key){ | ||||
|     // block makes HUGE difference
 | ||||
|     jacobians(key).block<Derived::RowsAtCompileTime, Derived::ColsAtCompileTime>(0, 0) += dTdA; | ||||
|   }; | ||||
| }; | ||||
| /// Handle Leaf Case for Dynamic Matrix type (slower)
 | ||||
| template <typename Derived> | ||||
| struct UseBlockIf<false, Derived> { | ||||
|   static void addToJacobian(const Eigen::MatrixBase<Derived>& dTdA, | ||||
|                             JacobianMap& jacobians, Key key) { | ||||
|     jacobians(key) += dTdA; | ||||
|   } | ||||
| }; | ||||
| } | ||||
| /// Handle Leaf Case for Dynamic ROWS Matrix type (slower)
 | ||||
| template<int COLS> | ||||
| inline void handleLeafCase( | ||||
|     const Eigen::Matrix<double, Eigen::Dynamic, COLS>& dTdA, | ||||
| 
 | ||||
| /// Handle Leaf Case: reverseAD ends here, by writing a matrix into Jacobians
 | ||||
| template<typename Derived> | ||||
| void handleLeafCase(const Eigen::MatrixBase<Derived>& dTdA, | ||||
|     JacobianMap& jacobians, Key key) { | ||||
|   jacobians(key) += dTdA; | ||||
|   internal::UseBlockIf< | ||||
|       Derived::RowsAtCompileTime != Eigen::Dynamic && | ||||
|       Derived::ColsAtCompileTime != Eigen::Dynamic, | ||||
|       Derived> | ||||
|     ::addToJacobian(dTdA, jacobians, key); | ||||
| } | ||||
| 
 | ||||
| //-----------------------------------------------------------------------------
 | ||||
|  | @ -166,9 +184,9 @@ public: | |||
|   void reverseAD(const Eigen::MatrixBase<DerivedMatrix> & dTdA, | ||||
|       JacobianMap& jacobians) const { | ||||
|     if (kind == Leaf) | ||||
|       handleLeafCase(dTdA.eval(), jacobians, content.key); | ||||
|       handleLeafCase(dTdA, jacobians, content.key); | ||||
|     else if (kind == Function) | ||||
|       content.ptr->reverseAD(dTdA.eval(), jacobians); | ||||
|       content.ptr->reverseAD(dTdA, jacobians); | ||||
|   } | ||||
| 
 | ||||
|   /// Define type so we can apply it as a meta-function
 | ||||
|  |  | |||
|  | @ -81,6 +81,8 @@ public: | |||
|    */ | ||||
|   virtual Vector unwhitenedError(const Values& x, | ||||
|       boost::optional<std::vector<Matrix>&> H = boost::none) const { | ||||
|     // TODO(PTF) Is this a place for custom charts?
 | ||||
|     DefaultChart<T> chart; | ||||
|     if (H) { | ||||
|       // H should be pre-allocated
 | ||||
|       assert(H->size()==size()); | ||||
|  | @ -95,18 +97,19 @@ public: | |||
|       T value = expression_.value(x, map); // <<< Reverse AD happens here !
 | ||||
| 
 | ||||
|       // Copy blocks into the vector of jacobians passed in
 | ||||
|       for (DenseIndex i = 0; i < size(); i++) | ||||
|       for (DenseIndex i = 0; i < static_cast<DenseIndex>(size()); i++) | ||||
|         H->at(i) = Ab(i); | ||||
| 
 | ||||
|       return measurement_.localCoordinates(value); | ||||
|       return chart.local(measurement_, value); | ||||
|     } else { | ||||
|       const T& value = expression_.value(x); | ||||
|       return measurement_.localCoordinates(value); | ||||
|       return chart.local(measurement_, value); | ||||
|     } | ||||
|   } | ||||
| 
 | ||||
|   virtual boost::shared_ptr<GaussianFactor> linearize(const Values& x) const { | ||||
| 
 | ||||
|     // TODO(PTF) Is this a place for custom charts?
 | ||||
|     DefaultChart<T> chart; | ||||
|     // Only linearize if the factor is active
 | ||||
|     if (!active(x)) | ||||
|       return boost::shared_ptr<JacobianFactor>(); | ||||
|  | @ -128,7 +131,7 @@ public: | |||
| 
 | ||||
|     // Evaluate error to get Jacobians and RHS vector b
 | ||||
|     T value = expression_.value(x, map); // <<< Reverse AD happens here !
 | ||||
|     Ab(size()).col(0) = -measurement_.localCoordinates(value); | ||||
|     Ab(size()).col(0) = -chart.local(measurement_, value); | ||||
| 
 | ||||
|     // Whiten the corresponding system, Ab already contains RHS
 | ||||
|     Vector dummy(Dim); | ||||
|  |  | |||
|  | @ -0,0 +1,26 @@ | |||
| /**
 | ||||
|  * @file expressions.h | ||||
|  * @brief Common expressions, both linear and non-linear | ||||
|  * @date Nov 23, 2014 | ||||
|  * @author Frank Dellaert | ||||
|  */ | ||||
| 
 | ||||
| #pragma once | ||||
| 
 | ||||
| #include <gtsam_unstable/nonlinear/Expression.h> | ||||
| #include <boost/bind.hpp> | ||||
| 
 | ||||
| namespace gtsam { | ||||
| 
 | ||||
| // Generics
 | ||||
| 
 | ||||
| template<class T> | ||||
| Expression<T> between(const Expression<T>& t1, const Expression<T>& t2) { | ||||
|   return Expression<T>(t1, &T::between, t2); | ||||
| } | ||||
| 
 | ||||
| typedef Expression<double> double_; | ||||
| typedef Expression<Vector3> Vector3_; | ||||
| 
 | ||||
| } // \namespace gtsam
 | ||||
| 
 | ||||
|  | @ -0,0 +1,119 @@ | |||
| /* ----------------------------------------------------------------------------
 | ||||
| 
 | ||||
|  * GTSAM Copyright 2010, Georgia Tech Research Corporation,  | ||||
|  * Atlanta, Georgia 30332-0415 | ||||
|  * All Rights Reserved | ||||
|  * Authors: Frank Dellaert, et al. (see THANKS for the full author list) | ||||
| 
 | ||||
|  * See LICENSE for the license information | ||||
| 
 | ||||
|  * -------------------------------------------------------------------------- */ | ||||
| 
 | ||||
| /**
 | ||||
|  * @file testBasisDecompositions.cpp | ||||
|  * @date November 23, 2014 | ||||
|  * @author Frank Dellaert | ||||
|  * @brief unit tests for Basis Decompositions w Expressions | ||||
|  */ | ||||
| 
 | ||||
| #include <gtsam_unstable/nonlinear/expressions.h> | ||||
| #include <gtsam_unstable/nonlinear/ExpressionFactor.h> | ||||
| #include <gtsam/linear/VectorValues.h> | ||||
| #include <gtsam/linear/GaussianFactorGraph.h> | ||||
| #include <gtsam/base/numericalDerivative.h> | ||||
| #include <gtsam/base/Testable.h> | ||||
| 
 | ||||
| #include <CppUnitLite/TestHarness.h> | ||||
| 
 | ||||
| #include <boost/assign/list_of.hpp> | ||||
| using boost::assign::list_of; | ||||
| 
 | ||||
| using namespace std; | ||||
| using namespace gtsam; | ||||
| 
 | ||||
| noiseModel::Diagonal::shared_ptr model = noiseModel::Unit::Create(1); | ||||
| 
 | ||||
| /// Fourier
 | ||||
| template<int N> | ||||
| class Fourier { | ||||
| 
 | ||||
| public: | ||||
| 
 | ||||
|   typedef Eigen::Matrix<double, N, 1> Coefficients; | ||||
|   typedef Eigen::Matrix<double, 1, N> Jacobian; | ||||
| 
 | ||||
| private: | ||||
| 
 | ||||
|   double x_; | ||||
|   Jacobian H_; | ||||
| 
 | ||||
| public: | ||||
| 
 | ||||
|   /// Constructor
 | ||||
|   Fourier(double x) : | ||||
|       x_(x) { | ||||
|     H_(0, 0) = 1; | ||||
|     for (size_t i = 1; i < N; i += 2) { | ||||
|       H_(0, i) = cos(i * x); | ||||
|       H_(0, i + 1) = sin(i * x); | ||||
|     } | ||||
|   } | ||||
| 
 | ||||
|   /// Given coefficients c, predict value for x
 | ||||
|   double operator()(const Coefficients& c, boost::optional<Jacobian&> H) { | ||||
|     if (H) | ||||
|       (*H) = H_; | ||||
|     return H_ * c; | ||||
|   } | ||||
| }; | ||||
| 
 | ||||
| //******************************************************************************
 | ||||
| TEST(BasisDecompositions, Fourier) { | ||||
|   Fourier<3> fx(0); | ||||
|   Eigen::Matrix<double, 1, 3> expectedH, actualH; | ||||
|   Vector3 c(1.5661, 1.2717, 1.2717); | ||||
|   expectedH = numericalDerivative11<double, Vector3>( | ||||
|       boost::bind(&Fourier<3>::operator(), fx, _1, boost::none), c); | ||||
|   EXPECT_DOUBLES_EQUAL(c[0]+c[1], fx(c,actualH), 1e-9); | ||||
|   EXPECT(assert_equal((Matrix)expectedH, actualH)); | ||||
| } | ||||
| 
 | ||||
| //******************************************************************************
 | ||||
| TEST(BasisDecompositions, FourierExpression) { | ||||
| 
 | ||||
|   // Create linear factor graph
 | ||||
|   GaussianFactorGraph g; | ||||
|   Key key(1); | ||||
|   Vector3_ c(key); | ||||
|   for (size_t i = 0; i < 16; i++) { | ||||
|     double x = i * M_PI / 8, y = exp(sin(x) + cos(x)); | ||||
| 
 | ||||
|     // Manual JacobianFactor
 | ||||
|     Matrix A(1, 3); | ||||
|     A << 1, cos(x), sin(x); | ||||
|     Vector b(1); | ||||
|     b << y; | ||||
|     JacobianFactor f1(key, A, b, model); | ||||
| 
 | ||||
|     // With ExpressionFactor
 | ||||
|     Expression<double> expression(Fourier<3>(x), c); | ||||
|     ExpressionFactor<double> f2(model, y, expression); | ||||
| 
 | ||||
|     g.add(f1); | ||||
|   } | ||||
| 
 | ||||
|   // Solve
 | ||||
|   VectorValues actual = g.optimize(); | ||||
| 
 | ||||
|   // Check
 | ||||
|   Vector3 expected(1.5661, 1.2717, 1.2717); | ||||
|   EXPECT(assert_equal((Vector) expected, actual.at(key),1e-4)); | ||||
| } | ||||
| 
 | ||||
| //******************************************************************************
 | ||||
| int main() { | ||||
|   TestResult tr; | ||||
|   return TestRegistry::runAllTests(tr); | ||||
| } | ||||
| //******************************************************************************
 | ||||
| 
 | ||||
|  | @ -73,27 +73,84 @@ TEST(Expression, Leaves) { | |||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| // Unary(Leaf)
 | ||||
| namespace unary { | ||||
| Point2 f0(const Point3& p, boost::optional<Matrix23&> H) { | ||||
|   return Point2(); | ||||
| } | ||||
| LieScalar f1(const Point3& p, boost::optional<Eigen::Matrix<double, 1, 3>&> H) { | ||||
|   return LieScalar(0.0); | ||||
| } | ||||
| double f2(const Point3& p, boost::optional<Eigen::Matrix<double, 1, 3>&> H) { | ||||
|   return 0.0; | ||||
| } | ||||
| Expression<Point3> p(1); | ||||
| set<Key> expected = list_of(1); | ||||
| } | ||||
| TEST(Expression, Unary0) { | ||||
|   using namespace unary; | ||||
|   Expression<Point2> e(f0, p); | ||||
|   EXPECT(expected == e.keys()); | ||||
| } | ||||
| TEST(Expression, Unary1) { | ||||
|   using namespace unary; | ||||
|   Expression<double> e(f1, p); | ||||
|   EXPECT(expected == e.keys()); | ||||
| } | ||||
| TEST(Expression, Unary2) { | ||||
|   using namespace unary; | ||||
|   Expression<double> e(f2, p); | ||||
|   EXPECT(expected == e.keys()); | ||||
| } | ||||
| /* ************************************************************************* */ | ||||
| //Nullary Method
 | ||||
| TEST(Expression, NullaryMethod) { | ||||
| 
 | ||||
| //TEST(Expression, NullaryMethod) {
 | ||||
| //  Expression<Point3> p(67);
 | ||||
| //  Expression<LieScalar> norm(p, &Point3::norm);
 | ||||
| //  Values values;
 | ||||
| //  values.insert(67,Point3(3,4,5));
 | ||||
| //  Augmented<LieScalar> a = norm.augmented(values);
 | ||||
| //  EXPECT(a.value() == sqrt(50));
 | ||||
| //  JacobianMap expected;
 | ||||
| //  expected[67] = (Matrix(1,3) << 3/sqrt(50),4/sqrt(50),5/sqrt(50));
 | ||||
| //  EXPECT(assert_equal(expected.at(67),a.jacobians().at(67)));
 | ||||
| //}
 | ||||
|   // Create expression
 | ||||
|   Expression<Point3> p(67); | ||||
|   Expression<double> norm(p, &Point3::norm); | ||||
| 
 | ||||
|   // Create Values
 | ||||
|   Values values; | ||||
|   values.insert(67, Point3(3, 4, 5)); | ||||
| 
 | ||||
|   // Pre-allocate JacobianMap
 | ||||
|   FastVector<Key> keys; | ||||
|   keys.push_back(67); | ||||
|   FastVector<int> dims; | ||||
|   dims.push_back(3); | ||||
|   VerticalBlockMatrix Ab(dims, 1); | ||||
|   JacobianMap map(keys, Ab); | ||||
| 
 | ||||
|   // Get value and Jacobian
 | ||||
|   double actual = norm.value(values, map); | ||||
| 
 | ||||
|   // Check all
 | ||||
|   EXPECT(actual == sqrt(50)); | ||||
|   Matrix expected(1, 3); | ||||
|   expected << 3.0 / sqrt(50.0), 4.0 / sqrt(50.0), 5.0 / sqrt(50.0); | ||||
|   EXPECT(assert_equal(expected,Ab(0))); | ||||
| } | ||||
| /* ************************************************************************* */ | ||||
| // Binary(Leaf,Leaf)
 | ||||
| namespace binary { | ||||
| // Create leaves
 | ||||
| double doubleF(const Pose3& pose, const Point3& point, | ||||
|     boost::optional<Eigen::Matrix<double, 1, 6>&> H1, | ||||
|     boost::optional<Eigen::Matrix<double, 1, 3>&> H2) { | ||||
|   return 0.0; | ||||
| } | ||||
| Expression<Pose3> x(1); | ||||
| Expression<Point3> p(2); | ||||
| Expression<Point3> p_cam(x, &Pose3::transform_to, p); | ||||
| } | ||||
| /* ************************************************************************* */ | ||||
| // Check that creating an expression to double compiles
 | ||||
| TEST(Expression, BinaryToDouble) { | ||||
|   using namespace binary; | ||||
|   Expression<double> p_cam(doubleF, x, p); | ||||
| } | ||||
| /* ************************************************************************* */ | ||||
| // keys
 | ||||
| TEST(Expression, BinaryKeys) { | ||||
|   set<Key> expected = list_of(1)(2); | ||||
|  |  | |||
|  | @ -22,8 +22,6 @@ | |||
| #include <gtsam/slam/GeneralSFMFactor.h> | ||||
| #include <gtsam/slam/ProjectionFactor.h> | ||||
| #include <gtsam/slam/PriorFactor.h> | ||||
| #include <gtsam/geometry/Pose3.h> | ||||
| #include <gtsam/geometry/Cal3_S2.h> | ||||
| #include <gtsam/base/Testable.h> | ||||
| 
 | ||||
| #include <CppUnitLite/TestHarness.h> | ||||
|  |  | |||
|  | @ -7,20 +7,12 @@ | |||
| 
 | ||||
| #pragma once | ||||
| 
 | ||||
| #include <gtsam_unstable/nonlinear/Expression.h> | ||||
| #include <gtsam_unstable/nonlinear/expressions.h> | ||||
| #include <gtsam/geometry/Cal3_S2.h> | ||||
| #include <gtsam/geometry/PinholeCamera.h> | ||||
| #include <boost/bind.hpp> | ||||
| 
 | ||||
| namespace gtsam { | ||||
| 
 | ||||
| // Generics
 | ||||
| 
 | ||||
| template<class T> | ||||
| Expression<T> between(const Expression<T>& t1, const Expression<T>& t2) { | ||||
|   return Expression<T>(t1, &T::between, t2); | ||||
| } | ||||
| 
 | ||||
| // 2D Geometry
 | ||||
| 
 | ||||
| typedef Expression<Point2> Point2_; | ||||
|  |  | |||
		Loading…
	
		Reference in New Issue