Derivative of multiplying with inverse of matrix

release/4.3a0
Frank Dellaert 2016-01-31 16:24:55 -08:00
parent 0c9910466e
commit e6521703e6
2 changed files with 57 additions and 3 deletions

View File

@ -12,18 +12,54 @@
namespace gtsam {
// Generics
// Generic between, assumes existence of traits<T>::Between
template<typename T>
Expression<T> between(const Expression<T>& t1, const Expression<T>& t2) {
return Expression<T>(traits<T>::Between, t1, t2);
}
// Generics
// Generic compose, assumes existence of traits<T>::Compose
template<typename T>
Expression<T> compose(const Expression<T>& t1, const Expression<T>& t2) {
return Expression<T>(traits<T>::Compose, t1, t2);
}
/**
* Functor that implements multiplication of a vector b with the inverse of a
* matrix A. The derivatives are inspired by Mike Giles' "An extended collection
* of matrix derivative results for forward and reverse mode algorithmic
* differentiation", at https://people.maths.ox.ac.uk/gilesm/files/NA-08-01.pdf
*
* Usage example:
* Expression<Vector3> expression = MultiplyWithInverse<3>()(Key(0), Key(1));
*/
template <int N>
struct MultiplyWithInverse {
typedef Eigen::Matrix<double, N, 1> VectorN;
typedef Eigen::Matrix<double, N, N> MatrixN;
/// A.inverse() * b, with optional derivatives
VectorN operator()(const MatrixN& A, const VectorN& b,
OptionalJacobian<N, N* N> H1 = boost::none,
OptionalJacobian<N, N> H2 = boost::none) const {
const MatrixN invA = A.inverse();
const VectorN c = invA * b;
// The derivative in A is just -[c[0]*invA c[1]*invA ... c[N-1]*invA]
if (H1)
for (size_t j = 0; j < N; j++)
H1->template middleCols<N>(N * j) = -c[j] * invA;
// The derivative in b is easy, as invA*b is just a linear map:
if (H2) *H2 = invA;
return c;
}
/// Create expression
Expression<VectorN> operator()(const Expression<MatrixN>& A_,
const Expression<VectorN>& b_) const {
return Expression<VectorN>(*this, A_, b_);
}
};
typedef Expression<double> double_;
typedef Expression<Vector3> Vector3_;

View File

@ -345,7 +345,6 @@ TEST(ExpressionFactor, tree) {
}
/* ************************************************************************* */
TEST(ExpressionFactor, Compose1) {
// Create expression
@ -600,6 +599,25 @@ TEST(Expression, testMultipleCompositions2) {
EXPECT_CORRECT_EXPRESSION_JACOBIANS(sum4_, values, fd_step, tolerance);
}
/* ************************************************************************* */
// Test multiplication with a matrix
TEST(ExpressionFactor, MultiplyWithInverse) {
// Create expression
auto model = noiseModel::Isotropic::Sigma(3, 1);
auto f_expr = MultiplyWithInverse<3>()(Key(0), Key(1));
// Check derivatives
Values values;
Matrix3 A = Vector3(1, 2, 3).asDiagonal();
A(0, 1) = 0.1;
A(0, 2) = 0.1;
const Vector3 b(0.1, 0.2, 0.3);
values.insert<Matrix3>(0, A);
values.insert<Vector3>(1, b);
ExpressionFactor<Vector3> factor(model, Vector3::Zero(), f_expr);
EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-5, 1e-5); // another way
}
/* ************************************************************************* */
int main() {
TestResult tr;