Merge branch 'feature/BAD_custom_chart' of https://bitbucket.org/gtborg/gtsam into feature/BAD_custom_chart
						commit
						5b44ddc3e5
					
				|  | @ -2777,6 +2777,14 @@ | |||
| 				<useDefaultCommand>true</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
| 			</target> | ||||
| 			<target name="testCustomChartExpression.run" path="build/gtsam_unstable/nonlinear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments>-j4</buildArguments> | ||||
| 				<buildTarget>testCustomChartExpression.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> | ||||
|  |  | |||
|  | @ -524,71 +524,5 @@ inline Matrix numericalHessian323(double (*f)(const X1&, const X2&, const X3&), | |||
|       delta); | ||||
| } | ||||
| 
 | ||||
| // The benefit of this method is that it does not need to know what types are involved
 | ||||
| // to evaluate the factor. If all the machinery of gtsam is working correctly, we should
 | ||||
| // get the correct numerical derivatives out the other side.
 | ||||
| template<typename FactorType> | ||||
| JacobianFactor computeNumericalDerivativeJacobianFactor(const FactorType& factor, | ||||
|                                                      const Values& values, | ||||
|                                                      double fd_step) { | ||||
|   Eigen::VectorXd e = factor.unwhitenedError(values); | ||||
|   const size_t rows = e.size(); | ||||
| 
 | ||||
|   std::map<Key, Matrix> jacobians; | ||||
|   typename FactorType::const_iterator key_it = factor.begin(); | ||||
|   VectorValues dX = values.zeroVectors(); | ||||
|   for(; key_it != factor.end(); ++key_it) { | ||||
|     size_t key = *key_it; | ||||
|     // Compute central differences using the values struct.
 | ||||
|     const size_t cols = dX.dim(key); | ||||
|     Matrix J = Matrix::Zero(rows, cols); | ||||
|     for(size_t col = 0; col < cols; ++col) { | ||||
|       Eigen::VectorXd dx = Eigen::VectorXd::Zero(cols); | ||||
|       dx[col] = fd_step; | ||||
|       dX[key] = dx; | ||||
|       Values eval_values = values.retract(dX); | ||||
|       Eigen::VectorXd left = factor.unwhitenedError(eval_values); | ||||
|       dx[col] = -fd_step; | ||||
|       dX[key] = dx; | ||||
|       eval_values = values.retract(dX); | ||||
|       Eigen::VectorXd right = factor.unwhitenedError(eval_values); | ||||
|       J.col(col) = (left - right) * (1.0/(2.0 * fd_step)); | ||||
|     } | ||||
|     jacobians[key] = J; | ||||
|   } | ||||
| 
 | ||||
|   // Next step...return JacobianFactor
 | ||||
|   return JacobianFactor(jacobians, -e); | ||||
| } | ||||
| 
 | ||||
| template<typename FactorType> | ||||
| void testFactorJacobians(TestResult& result_, | ||||
|                          const std::string& name_, | ||||
|                          const FactorType& f, | ||||
|                          const gtsam::Values& values, | ||||
|                          double fd_step, | ||||
|                          double tolerance) { | ||||
|   // Check linearization
 | ||||
|   JacobianFactor expected = computeNumericalDerivativeJacobianFactor(f, values, fd_step); | ||||
|   boost::shared_ptr<GaussianFactor> gf = f.linearize(values); | ||||
|   boost::shared_ptr<JacobianFactor> jf = //
 | ||||
|       boost::dynamic_pointer_cast<JacobianFactor>(gf); | ||||
| 
 | ||||
|   typedef std::pair<Eigen::MatrixXd, Eigen::VectorXd> Jacobian; | ||||
|   Jacobian evalJ = jf->jacobianUnweighted(); | ||||
|   Jacobian estJ = expected.jacobianUnweighted(); | ||||
|   EXPECT(assert_equal(evalJ.first, estJ.first, tolerance)); | ||||
|   EXPECT(assert_equal(evalJ.second, Eigen::VectorXd::Zero(evalJ.second.size()), tolerance)); | ||||
|   EXPECT(assert_equal(estJ.second, Eigen::VectorXd::Zero(evalJ.second.size()), tolerance)); | ||||
| } | ||||
| 
 | ||||
| } // namespace gtsam
 | ||||
| 
 | ||||
| /// \brief Check the Jacobians produced by a factor against finite differences.
 | ||||
| /// \param factor The factor to test.
 | ||||
| /// \param values Values filled in for testing the Jacobians.
 | ||||
| /// \param numerical_derivative_step The step to use when computing the numerical derivative Jacobians
 | ||||
| /// \param tolerance The numerical tolerance to use when comparing Jacobians.
 | ||||
| #define EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, numerical_derivative_step, tolerance) \ | ||||
|     { gtsam::testFactorJacobians(result_, name_, factor, values, numerical_derivative_step, tolerance); } | ||||
| 
 | ||||
|  |  | |||
|  | @ -30,19 +30,88 @@ | |||
| 
 | ||||
| namespace gtsam { | ||||
| 
 | ||||
| /**
 | ||||
|  * Linearize a nonlinear factor using numerical differentiation | ||||
|  * The benefit of this method is that it does not need to know what types are | ||||
|  * involved to evaluate the factor. If all the machinery of gtsam is working | ||||
|  * correctly, we should get the correct numerical derivatives out the other side. | ||||
|  */ | ||||
| JacobianFactor linearizeNumerically(const NoiseModelFactor& factor, | ||||
|     const Values& values, double delta = 1e-5) { | ||||
| 
 | ||||
|   // We will fill a map of Jacobians
 | ||||
|   std::map<Key, Matrix> jacobians; | ||||
| 
 | ||||
|   // Get size
 | ||||
|   Eigen::VectorXd e = factor.whitenedError(values); | ||||
|   const size_t rows = e.size(); | ||||
| 
 | ||||
|   // Loop over all variables
 | ||||
|   VectorValues dX = values.zeroVectors(); | ||||
|   BOOST_FOREACH(Key key, factor) { | ||||
|     // Compute central differences using the values struct.
 | ||||
|     const size_t cols = dX.dim(key); | ||||
|     Matrix J = Matrix::Zero(rows, cols); | ||||
|     for (size_t col = 0; col < cols; ++col) { | ||||
|       Eigen::VectorXd dx = Eigen::VectorXd::Zero(cols); | ||||
|       dx[col] = delta; | ||||
|       dX[key] = dx; | ||||
|       Values eval_values = values.retract(dX); | ||||
|       Eigen::VectorXd left = factor.whitenedError(eval_values); | ||||
|       dx[col] = -delta; | ||||
|       dX[key] = dx; | ||||
|       eval_values = values.retract(dX); | ||||
|       Eigen::VectorXd right = factor.whitenedError(eval_values); | ||||
|       J.col(col) = (left - right) * (1.0 / (2.0 * delta)); | ||||
|     } | ||||
|     jacobians[key] = J; | ||||
|   } | ||||
| 
 | ||||
|   // Next step...return JacobianFactor
 | ||||
|   return JacobianFactor(jacobians, -e); | ||||
| } | ||||
| 
 | ||||
| namespace internal { | ||||
| // CPPUnitLite-style test for linearization of a factor
 | ||||
| void testFactorJacobians(TestResult& result_, const std::string& name_, | ||||
|     const NoiseModelFactor& factor, const gtsam::Values& values, double delta, | ||||
|     double tolerance) { | ||||
| 
 | ||||
|   // Create expected value by numerical differentiation
 | ||||
|   JacobianFactor expected = linearizeNumerically(factor, values, delta); | ||||
| 
 | ||||
|   // Create actual value by linearize
 | ||||
|   boost::shared_ptr<JacobianFactor> actual = //
 | ||||
|       boost::dynamic_pointer_cast<JacobianFactor>(factor.linearize(values)); | ||||
| 
 | ||||
|   // Check cast result and then equality
 | ||||
|   CHECK(actual); | ||||
|   EXPECT( assert_equal(expected, *actual, tolerance)); | ||||
| } | ||||
| } | ||||
| 
 | ||||
| /// \brief Check the Jacobians produced by a factor against finite differences.
 | ||||
| /// \param factor The factor to test.
 | ||||
| /// \param values Values filled in for testing the Jacobians.
 | ||||
| /// \param numerical_derivative_step The step to use when computing the numerical derivative Jacobians
 | ||||
| /// \param tolerance The numerical tolerance to use when comparing Jacobians.
 | ||||
| #define EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, numerical_derivative_step, tolerance) \ | ||||
|     { gtsam::internal::testFactorJacobians(result_, name_, factor, values, numerical_derivative_step, tolerance); } | ||||
| 
 | ||||
| namespace internal { | ||||
| // CPPUnitLite-style test for linearization of an ExpressionFactor
 | ||||
| template<typename T> | ||||
| void testExpressionJacobians(TestResult& result_, | ||||
|                              const std::string& name_, | ||||
|                              const gtsam::Expression<T>& expression, | ||||
|                              const gtsam::Values& values, | ||||
|                              double nd_step, | ||||
|                              double tolerance) { | ||||
| void testExpressionJacobians(TestResult& result_, const std::string& name_, | ||||
|     const gtsam::Expression<T>& expression, const gtsam::Values& values, | ||||
|     double nd_step, double tolerance) { | ||||
|   // Create factor
 | ||||
|   size_t size = traits::dimension<T>::value; | ||||
|   ExpressionFactor<T> f(noiseModel::Unit::Create(size), expression.value(values), expression); | ||||
|   ExpressionFactor<T> f(noiseModel::Unit::Create(size), | ||||
|       expression.value(values), expression); | ||||
|   testFactorJacobians(result_, name_, f, values, nd_step, tolerance); | ||||
| } | ||||
| }  // namespace gtsam
 | ||||
| } | ||||
| } // namespace gtsam
 | ||||
| 
 | ||||
| /// \brief Check the Jacobians produced by an expression against finite differences.
 | ||||
| /// \param expression The expression to test.
 | ||||
|  | @ -50,4 +119,4 @@ void testExpressionJacobians(TestResult& result_, | |||
| /// \param numerical_derivative_step The step to use when computing the finite difference Jacobians
 | ||||
| /// \param tolerance The numerical tolerance to use when comparing Jacobians.
 | ||||
| #define EXPECT_CORRECT_EXPRESSION_JACOBIANS(expression, values, numerical_derivative_step, tolerance) \ | ||||
|     { gtsam::testExpressionJacobians(result_, name_, expression, values, numerical_derivative_step, tolerance); } | ||||
|     { gtsam::internal::testExpressionJacobians(result_, name_, expression, values, numerical_derivative_step, tolerance); } | ||||
|  |  | |||
		Loading…
	
		Reference in New Issue