Derivative of multiplying with inverse of matrix
parent
0c9910466e
commit
e6521703e6
|
@ -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_;
|
||||
|
||||
|
|
|
@ -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;
|
||||
|
|
Loading…
Reference in New Issue