12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Merge branch 'feature/BAD_custom_chart' of https://bitbucket.org/gtborg/gtsam into feature/BAD_custom_chart

release/4.3a0
Paul Furgale 2014-11-25 06:43:45 +01:00
parent 14b582d268 4790bade85
commit 5b44ddc3e5
3 changed files with 86 additions and 75 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
.cproject
View File

@ -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>

66
gtsam/base/numericalDerivative.h
View File

@ -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); }

87
gtsam_unstable/nonlinear/expressionTesting.h
View File

@ -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); }